41 files changed, 7213 insertions, 3303 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSAPIUnman.cs b/OpenSim/Region/Physics/BulletSPlugin/BSAPIUnman.cs
index 8c6e7d6..231f0f8 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSAPIUnman.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSAPIUnman.cs
@@ -75,11 +75,11 @@ private sealed class BulletBodyUnman : BulletBody
 private sealed class BulletShapeUnman : BulletShape
 {
     public IntPtr ptr;
-    public BulletShapeUnman(IntPtr xx, BSPhysicsShapeType typ) 
+    public BulletShapeUnman(IntPtr xx, BSPhysicsShapeType typ)
         : base()
     {
         ptr = xx;
-        type = typ;
+        shapeType = typ;
     }
     public override bool HasPhysicalShape
     {
@@ -91,7 +91,7 @@ private sealed class BulletShapeUnman : BulletShape
     }
     public override BulletShape Clone()
     {
-        return new BulletShapeUnman(ptr, type);
+        return new BulletShapeUnman(ptr, shapeType);
     }
     public override bool ReferenceSame(BulletShape other)
     {
@@ -166,7 +166,7 @@ public override BulletWorld Initialize(Vector3 maxPosition, ConfigurationParamet
 
     // If Debug logging level, enable logging from the unmanaged code
     m_DebugLogCallbackHandle = null;
-    if (BSScene.m_log.IsDebugEnabled || PhysicsScene.PhysicsLogging.Enabled)
+    if (BSScene.m_log.IsDebugEnabled && PhysicsScene.PhysicsLogging.Enabled)
     {
         BSScene.m_log.DebugFormat("{0}: Initialize: Setting debug callback for unmanaged code", BSScene.LogHeader);
         if (PhysicsScene.PhysicsLogging.Enabled)
@@ -202,7 +202,7 @@ private void BulletLoggerPhysLog(string msg)
 }
 
 public override int PhysicsStep(BulletWorld world, float timeStep, int maxSubSteps, float fixedTimeStep,
-                        out int updatedEntityCount, out int collidersCount)
+                                        out int updatedEntityCount, out int collidersCount)
 {
     BulletWorldUnman worldu = world as BulletWorldUnman;
     return BSAPICPP.PhysicsStep2(worldu.ptr, timeStep, maxSubSteps, fixedTimeStep, out updatedEntityCount, out collidersCount);
@@ -212,6 +212,19 @@ public override void Shutdown(BulletWorld world)
 {
     BulletWorldUnman worldu = world as BulletWorldUnman;
     BSAPICPP.Shutdown2(worldu.ptr);
+
+    if (m_paramsHandle.IsAllocated)
+    {
+        m_paramsHandle.Free();
+    }
+    if (m_collisionArrayPinnedHandle.IsAllocated)
+    {
+        m_collisionArrayPinnedHandle.Free();
+    }
+    if (m_updateArrayPinnedHandle.IsAllocated)
+    {
+        m_updateArrayPinnedHandle.Free();
+    }
 }
 
 public override bool PushUpdate(BulletBody obj)
@@ -242,19 +255,38 @@ public override BulletShape CreateHullShape(BulletWorld world, int hullCount, fl
 {
     BulletWorldUnman worldu = world as BulletWorldUnman;
     return new BulletShapeUnman(
-                    BSAPICPP.CreateHullShape2(worldu.ptr, hullCount, hulls), 
+                    BSAPICPP.CreateHullShape2(worldu.ptr, hullCount, hulls),
                     BSPhysicsShapeType.SHAPE_HULL);
 }
 
-public override BulletShape BuildHullShapeFromMesh(BulletWorld world, BulletShape meshShape)
+public override BulletShape BuildHullShapeFromMesh(BulletWorld world, BulletShape meshShape, HACDParams parms)
 {
     BulletWorldUnman worldu = world as BulletWorldUnman;
     BulletShapeUnman shapeu = meshShape as BulletShapeUnman;
     return new BulletShapeUnman(
-                    BSAPICPP.BuildHullShapeFromMesh2(worldu.ptr, shapeu.ptr),
+                    BSAPICPP.BuildHullShapeFromMesh2(worldu.ptr, shapeu.ptr, parms),
                     BSPhysicsShapeType.SHAPE_HULL);
 }
 
+public override BulletShape BuildConvexHullShapeFromMesh(BulletWorld world, BulletShape meshShape)
+{
+    BulletWorldUnman worldu = world as BulletWorldUnman;
+    BulletShapeUnman shapeu = meshShape as BulletShapeUnman;
+    return new BulletShapeUnman(
+                    BSAPICPP.BuildConvexHullShapeFromMesh2(worldu.ptr, shapeu.ptr),
+                    BSPhysicsShapeType.SHAPE_CONVEXHULL);
+}
+
+public override BulletShape CreateConvexHullShape(BulletWorld world,
+                int indicesCount, int[] indices,
+                int verticesCount, float[] vertices)
+{
+    BulletWorldUnman worldu = world as BulletWorldUnman;
+    return new BulletShapeUnman(
+                    BSAPICPP.CreateConvexHullShape2(worldu.ptr, indicesCount, indices, verticesCount, vertices),
+                    BSPhysicsShapeType.SHAPE_CONVEXHULL);
+}
+
 public override BulletShape BuildNativeShape(BulletWorld world, ShapeData shapeData)
 {
     BulletWorldUnman worldu = world as BulletWorldUnman;
@@ -273,7 +305,7 @@ public override void SetShapeCollisionMargin(BulletShape shape, float margin)
 {
     BulletShapeUnman shapeu = shape as BulletShapeUnman;
     if (shapeu != null && shapeu.HasPhysicalShape)
-        BSAPICPP.SetShapeCollisionMargin2(shapeu.ptr, margin);
+        BSAPICPP.SetShapeCollisionMargin(shapeu.ptr, margin);
 }
 
 public override BulletShape BuildCapsuleShape(BulletWorld world, float radius, float height, Vector3 scale)
@@ -327,6 +359,12 @@ public override void RemoveChildShapeFromCompoundShape(BulletShape shape, Bullet
     BSAPICPP.RemoveChildShapeFromCompoundShape2(shapeu.ptr, removeShapeu.ptr);
 }
 
+public override void UpdateChildTransform(BulletShape pShape, int childIndex, Vector3 pos, Quaternion rot, bool shouldRecalculateLocalAabb)
+{
+    BulletShapeUnman shapeu = pShape as BulletShapeUnman;
+    BSAPICPP.UpdateChildTransform2(shapeu.ptr, childIndex, pos, rot, shouldRecalculateLocalAabb);
+}
+
 public override void RecalculateCompoundShapeLocalAabb(BulletShape shape)
 {
     BulletShapeUnman shapeu = shape as BulletShapeUnman;
@@ -337,7 +375,7 @@ public override BulletShape DuplicateCollisionShape(BulletWorld world, BulletSha
 {
     BulletWorldUnman worldu = world as BulletWorldUnman;
     BulletShapeUnman srcShapeu = srcShape as BulletShapeUnman;
-    return new BulletShapeUnman(BSAPICPP.DuplicateCollisionShape2(worldu.ptr, srcShapeu.ptr, id), srcShape.type);
+    return new BulletShapeUnman(BSAPICPP.DuplicateCollisionShape2(worldu.ptr, srcShapeu.ptr, id), srcShape.shapeType);
 }
 
 public override bool DeleteCollisionShape(BulletWorld world, BulletShape shape)
@@ -419,6 +457,28 @@ public override BulletConstraint Create6DofConstraintToPoint(BulletWorld world,
                     joinPoint, useLinearReferenceFrameA, disableCollisionsBetweenLinkedBodies));
 }
 
+public override BulletConstraint Create6DofConstraintFixed(BulletWorld world, BulletBody obj1,
+                    Vector3 frameInBloc, Quaternion frameInBrot,
+                    bool useLinearReferenceFrameB, bool disableCollisionsBetweenLinkedBodies)
+{
+    BulletWorldUnman worldu = world as BulletWorldUnman;
+    BulletBodyUnman bodyu1 = obj1 as BulletBodyUnman;
+    return new BulletConstraintUnman(BSAPICPP.Create6DofConstraintFixed2(worldu.ptr, bodyu1.ptr,
+                    frameInBloc, frameInBrot, useLinearReferenceFrameB, disableCollisionsBetweenLinkedBodies));
+}
+
+public override BulletConstraint Create6DofSpringConstraint(BulletWorld world, BulletBody obj1, BulletBody obj2,
+                    Vector3 frame1loc, Quaternion frame1rot,
+                    Vector3 frame2loc, Quaternion frame2rot,
+                    bool useLinearReferenceFrameA, bool disableCollisionsBetweenLinkedBodies)
+{
+    BulletWorldUnman worldu = world as BulletWorldUnman;
+    BulletBodyUnman bodyu1 = obj1 as BulletBodyUnman;
+    BulletBodyUnman bodyu2 = obj2 as BulletBodyUnman;
+    return new BulletConstraintUnman(BSAPICPP.Create6DofSpringConstraint2(worldu.ptr, bodyu1.ptr, bodyu2.ptr, frame1loc, frame1rot,
+                    frame2loc, frame2rot, useLinearReferenceFrameA, disableCollisionsBetweenLinkedBodies));
+}
+
 public override BulletConstraint CreateHingeConstraint(BulletWorld world, BulletBody obj1, BulletBody obj2,
                     Vector3 pivotinA, Vector3 pivotinB,
                     Vector3 axisInA, Vector3 axisInB,
@@ -431,6 +491,52 @@ public override BulletConstraint CreateHingeConstraint(BulletWorld world, Bullet
                     pivotinA, pivotinB, axisInA, axisInB, useLinearReferenceFrameA, disableCollisionsBetweenLinkedBodies));
 }
 
+public override BulletConstraint CreateSliderConstraint(BulletWorld world, BulletBody obj1, BulletBody obj2,
+                    Vector3 frame1loc, Quaternion frame1rot,
+                    Vector3 frame2loc, Quaternion frame2rot,
+                    bool useLinearReferenceFrameA, bool disableCollisionsBetweenLinkedBodies)
+{
+    BulletWorldUnman worldu = world as BulletWorldUnman;
+    BulletBodyUnman bodyu1 = obj1 as BulletBodyUnman;
+    BulletBodyUnman bodyu2 = obj2 as BulletBodyUnman;
+    return new BulletConstraintUnman(BSAPICPP.CreateSliderConstraint2(worldu.ptr, bodyu1.ptr, bodyu2.ptr, frame1loc, frame1rot,
+                    frame2loc, frame2rot, useLinearReferenceFrameA, disableCollisionsBetweenLinkedBodies));
+}
+
+public override BulletConstraint CreateConeTwistConstraint(BulletWorld world, BulletBody obj1, BulletBody obj2,
+                    Vector3 frame1loc, Quaternion frame1rot,
+                    Vector3 frame2loc, Quaternion frame2rot,
+                    bool disableCollisionsBetweenLinkedBodies)
+{
+    BulletWorldUnman worldu = world as BulletWorldUnman;
+    BulletBodyUnman bodyu1 = obj1 as BulletBodyUnman;
+    BulletBodyUnman bodyu2 = obj2 as BulletBodyUnman;
+    return new BulletConstraintUnman(BSAPICPP.CreateConeTwistConstraint2(worldu.ptr, bodyu1.ptr, bodyu2.ptr, frame1loc, frame1rot,
+                                        frame2loc, frame2rot, disableCollisionsBetweenLinkedBodies));
+}
+
+public override BulletConstraint CreateGearConstraint(BulletWorld world, BulletBody obj1, BulletBody obj2,
+                    Vector3 axisInA, Vector3 axisInB,
+                    float ratio, bool disableCollisionsBetweenLinkedBodies)
+{
+    BulletWorldUnman worldu = world as BulletWorldUnman;
+    BulletBodyUnman bodyu1 = obj1 as BulletBodyUnman;
+    BulletBodyUnman bodyu2 = obj2 as BulletBodyUnman;
+    return new BulletConstraintUnman(BSAPICPP.CreateGearConstraint2(worldu.ptr, bodyu1.ptr, bodyu2.ptr, axisInA, axisInB,
+                                        ratio, disableCollisionsBetweenLinkedBodies));
+}
+
+public override BulletConstraint CreatePoint2PointConstraint(BulletWorld world, BulletBody obj1, BulletBody obj2,
+                    Vector3 pivotInA, Vector3 pivotInB,
+                    bool disableCollisionsBetweenLinkedBodies)
+{
+    BulletWorldUnman worldu = world as BulletWorldUnman;
+    BulletBodyUnman bodyu1 = obj1 as BulletBodyUnman;
+    BulletBodyUnman bodyu2 = obj2 as BulletBodyUnman;
+    return new BulletConstraintUnman(BSAPICPP.CreatePoint2PointConstraint2(worldu.ptr, bodyu1.ptr, bodyu2.ptr, pivotInA, pivotInB,
+                                        disableCollisionsBetweenLinkedBodies));
+}
+
 public override void SetConstraintEnable(BulletConstraint constrain, float numericTrueFalse)
 {
     BulletConstraintUnman constrainu = constrain as BulletConstraintUnman;
@@ -530,12 +636,12 @@ public override void SetForceUpdateAllAabbs(BulletWorld world, bool force)
 // btDynamicsWorld entries
 public override bool AddObjectToWorld(BulletWorld world, BulletBody obj)
 {
-    // Bullet resets several variables when an object is added to the world.
-    //   Gravity is reset to world default depending on the static/dynamic
-    //   type. Of course, the collision flags in the broadphase proxy are initialized to default.
     BulletWorldUnman worldu = world as BulletWorldUnman;
     BulletBodyUnman bodyu = obj as BulletBodyUnman;
 
+    // Bullet resets several variables when an object is added to the world.
+    //   Gravity is reset to world default depending on the static/dynamic
+    //   type. Of course, the collision flags in the broadphase proxy are initialized to default.
     Vector3 origGrav = BSAPICPP.GetGravity2(bodyu.ptr);
 
     bool ret = BSAPICPP.AddObjectToWorld2(worldu.ptr, bodyu.ptr);
@@ -921,6 +1027,7 @@ public override void SetCenterOfMassByPosRot(BulletBody obj, Vector3 pos, Quater
 }
 
 // Add a force to the object as if its mass is one.
+// Deep down in Bullet: m_totalForce += force*m_linearFactor;
 public override void ApplyCentralForce(BulletBody obj, Vector3 force)
 {
     BulletBodyUnman bodyu = obj as BulletBodyUnman;
@@ -964,6 +1071,7 @@ public override void SetSleepingThresholds(BulletBody obj, float lin_threshold,
     BSAPICPP.SetSleepingThresholds2(bodyu.ptr, lin_threshold, ang_threshold);
 }
 
+// Deep down in Bullet: m_totalTorque += torque*m_angularFactor;
 public override void ApplyTorque(BulletBody obj, Vector3 torque)
 {
     BulletBodyUnman bodyu = obj as BulletBodyUnman;
@@ -971,6 +1079,8 @@ public override void ApplyTorque(BulletBody obj, Vector3 torque)
 }
 
 // Apply force at the given point. Will add torque to the object.
+// Deep down in Bullet: applyCentralForce(force);
+//                              applyTorque(rel_pos.cross(force*m_linearFactor));
 public override void ApplyForce(BulletBody obj, Vector3 force, Vector3 pos)
 {
     BulletBodyUnman bodyu = obj as BulletBodyUnman;
@@ -978,6 +1088,7 @@ public override void ApplyForce(BulletBody obj, Vector3 force, Vector3 pos)
 }
 
 // Apply impulse to the object. Same as "ApplycentralForce" but force scaled by object's mass.
+// Deep down in Bullet: m_linearVelocity += impulse *m_linearFactor * m_inverseMass;
 public override void ApplyCentralImpulse(BulletBody obj, Vector3 imp)
 {
     BulletBodyUnman bodyu = obj as BulletBodyUnman;
@@ -985,6 +1096,7 @@ public override void ApplyCentralImpulse(BulletBody obj, Vector3 imp)
 }
 
 // Apply impulse to the object's torque. Force is scaled by object's mass.
+// Deep down in Bullet: m_angularVelocity += m_invInertiaTensorWorld * torque * m_angularFactor;
 public override void ApplyTorqueImpulse(BulletBody obj, Vector3 imp)
 {
     BulletBodyUnman bodyu = obj as BulletBodyUnman;
@@ -992,6 +1104,8 @@ public override void ApplyTorqueImpulse(BulletBody obj, Vector3 imp)
 }
 
 // Apply impulse at the point given. For is scaled by object's mass and effects both linear and angular forces.
+// Deep down in Bullet: applyCentralImpulse(impulse);
+//                              applyTorqueImpulse(rel_pos.cross(impulse*m_linearFactor));
 public override void ApplyImpulse(BulletBody obj, Vector3 imp, Vector3 pos)
 {
     BulletBodyUnman bodyu = obj as BulletBodyUnman;
@@ -1259,6 +1373,16 @@ public override void DumpPhysicsStatistics(BulletWorld world)
     BulletWorldUnman worldu = world as BulletWorldUnman;
     BSAPICPP.DumpPhysicsStatistics2(worldu.ptr);
 }
+public override void ResetBroadphasePool(BulletWorld world)
+{
+    BulletWorldUnman worldu = world as BulletWorldUnman;
+    BSAPICPP.ResetBroadphasePool(worldu.ptr);
+}
+public override void ResetConstraintSolver(BulletWorld world)
+{
+    BulletWorldUnman worldu = world as BulletWorldUnman;
+    BSAPICPP.ResetConstraintSolver(worldu.ptr);
+}
 
 // =====================================================================================
 // =====================================================================================
@@ -1306,7 +1430,15 @@ public static extern IntPtr CreateHullShape2(IntPtr world,
                 int hullCount, [MarshalAs(UnmanagedType.LPArray)] float[] hulls);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern IntPtr BuildHullShapeFromMesh2(IntPtr world, IntPtr meshShape);
+public static extern IntPtr BuildHullShapeFromMesh2(IntPtr world, IntPtr meshShape, HACDParams parms);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern IntPtr BuildConvexHullShapeFromMesh2(IntPtr world, IntPtr meshShape);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern IntPtr CreateConvexHullShape2(IntPtr world,
+                int indicesCount, [MarshalAs(UnmanagedType.LPArray)] int[] indices,
+                int verticesCount, [MarshalAs(UnmanagedType.LPArray)] float[] vertices );
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern IntPtr BuildNativeShape2(IntPtr world, ShapeData shapeData);
@@ -1315,7 +1447,7 @@ public static extern IntPtr BuildNativeShape2(IntPtr world, ShapeData shapeData)
 public static extern bool IsNativeShape2(IntPtr shape);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void SetShapeCollisionMargin2(IntPtr shape, float margin);
+public static extern void SetShapeCollisionMargin(IntPtr shape, float margin);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern IntPtr BuildCapsuleShape2(IntPtr world, float radius, float height, Vector3 scale);
@@ -1339,6 +1471,9 @@ public static extern IntPtr RemoveChildShapeFromCompoundShapeIndex2(IntPtr cShap
 public static extern void RemoveChildShapeFromCompoundShape2(IntPtr cShape, IntPtr removeShape);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void UpdateChildTransform2(IntPtr pShape, int childIndex, Vector3 pos, Quaternion rot, bool shouldRecalculateLocalAabb);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern void RecalculateCompoundShapeLocalAabb2(IntPtr cShape);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
@@ -1368,7 +1503,7 @@ public static extern void DestroyObject2(IntPtr sim, IntPtr obj);
 public static extern IntPtr CreateGroundPlaneShape2(uint id, float height, float collisionMargin);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern IntPtr CreateTerrainShape2(uint id, Vector3 size, float minHeight, float maxHeight, 
+public static extern IntPtr CreateTerrainShape2(uint id, Vector3 size, float minHeight, float maxHeight,
                                             [MarshalAs(UnmanagedType.LPArray)] float[] heightMap,
                                             float scaleFactor, float collisionMargin);
 
@@ -1386,12 +1521,46 @@ public static extern IntPtr Create6DofConstraintToPoint2(IntPtr world, IntPtr ob
                     bool useLinearReferenceFrameA, bool disableCollisionsBetweenLinkedBodies);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern IntPtr Create6DofConstraintFixed2(IntPtr world, IntPtr obj1,
+                    Vector3 frameInBloc, Quaternion frameInBrot,
+                    bool useLinearReferenceFrameB, bool disableCollisionsBetweenLinkedBodies);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern IntPtr Create6DofSpringConstraint2(IntPtr world, IntPtr obj1, IntPtr obj2,
+                    Vector3 frame1loc, Quaternion frame1rot,
+                    Vector3 frame2loc, Quaternion frame2rot,
+                    bool useLinearReferenceFrameA, bool disableCollisionsBetweenLinkedBodies);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern IntPtr CreateHingeConstraint2(IntPtr world, IntPtr obj1, IntPtr obj2,
                     Vector3 pivotinA, Vector3 pivotinB,
                     Vector3 axisInA, Vector3 axisInB,
                     bool useLinearReferenceFrameA, bool disableCollisionsBetweenLinkedBodies);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern IntPtr CreateSliderConstraint2(IntPtr world, IntPtr obj1, IntPtr obj2,
+                    Vector3 frameInAloc, Quaternion frameInArot,
+                    Vector3 frameInBloc, Quaternion frameInBrot,
+                    bool useLinearReferenceFrameA, bool disableCollisionsBetweenLinkedBodies);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern IntPtr CreateConeTwistConstraint2(IntPtr world, IntPtr obj1, IntPtr obj2,
+                    Vector3 frameInAloc, Quaternion frameInArot,
+                    Vector3 frameInBloc, Quaternion frameInBrot,
+                    bool disableCollisionsBetweenLinkedBodies);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern IntPtr CreateGearConstraint2(IntPtr world, IntPtr obj1, IntPtr obj2,
+                    Vector3 axisInA, Vector3 axisInB,
+                    float ratio, bool disableCollisionsBetweenLinkedBodies);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern IntPtr CreatePoint2PointConstraint2(IntPtr world, IntPtr obj1, IntPtr obj2,
+                    Vector3 pivotInA, Vector3 pivotInB,
+                    bool disableCollisionsBetweenLinkedBodies);
+
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern void SetConstraintEnable2(IntPtr constrain, float numericTrueFalse);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
@@ -1832,6 +2001,12 @@ public static extern void DumpAllInfo2(IntPtr sim);
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern void DumpPhysicsStatistics2(IntPtr sim);
 
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void ResetBroadphasePool(IntPtr sim);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void ResetConstraintSolver(IntPtr sim);
+
 }
 
 }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSAPIXNA.cs b/OpenSim/Region/Physics/BulletSPlugin/BSAPIXNA.cs
index 30a7bee..59780ae 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSAPIXNA.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSAPIXNA.cs
@@ -27,6 +27,7 @@
 using System;
 using System.Collections.Generic;
 using System.IO;
+using System.Runtime.InteropServices;
 using System.Text;
 
 using OpenSim.Framework;
@@ -80,11 +81,11 @@ private sealed class BulletBodyXNA : BulletBody
 private sealed class BulletShapeXNA : BulletShape
 {
     public CollisionShape shape;
-    public BulletShapeXNA(CollisionShape xx, BSPhysicsShapeType typ) 
+    public BulletShapeXNA(CollisionShape xx, BSPhysicsShapeType typ)
         : base()
     {
         shape = xx;
-        type = typ;
+        shapeType = typ;
     }
     public override bool HasPhysicalShape
     {
@@ -96,7 +97,7 @@ private sealed class BulletShapeXNA : BulletShape
     }
     public override BulletShape Clone()
     {
-        return new BulletShapeXNA(shape, type);
+        return new BulletShapeXNA(shape, shapeType);
     }
     public override bool ReferenceSame(BulletShape other)
     {
@@ -129,6 +130,14 @@ private sealed class BulletConstraintXNA : BulletConstraint
         get { return "XNAConstraint"; }
     }
 }
+    internal int m_maxCollisions;
+    internal CollisionDesc[] UpdatedCollisions;
+    internal int LastCollisionDesc = 0;
+    internal int m_maxUpdatesPerFrame;
+    internal int LastEntityProperty = 0;
+
+    internal EntityProperties[] UpdatedObjects;
+    internal Dictionary<uint, GhostObject> specialCollisionObjects;
 
     private static int m_collisionsThisFrame;
     private BSScene PhysicsScene { get; set; }
@@ -142,111 +151,127 @@ private sealed class BulletConstraintXNA : BulletConstraint
     }
 
     /// <summary>
-    /// 
+    ///
     /// </summary>
     /// <param name="p"></param>
     /// <param name="p_2"></param>
     public override bool RemoveObjectFromWorld(BulletWorld pWorld, BulletBody pBody)
     {
-        DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world;
+        DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
         RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
-        world.RemoveRigidBody(body);
+        CollisionObject collisionObject = ((BulletBodyXNA)pBody).body;
+        if (body != null)
+            world.RemoveRigidBody(body);
+        else if (collisionObject != null)
+            world.RemoveCollisionObject(collisionObject);
+        else
+            return false;
         return true;
     }
 
-    public override bool AddConstraintToWorld(BulletWorld world, BulletConstraint constrain, bool disableCollisionsBetweenLinkedObjects)
+    public override bool AddConstraintToWorld(BulletWorld pWorld, BulletConstraint pConstraint, bool pDisableCollisionsBetweenLinkedObjects)
     {
-        /* TODO */
-        return false;
+        DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
+        TypedConstraint constraint = (pConstraint as BulletConstraintXNA).constrain;
+        world.AddConstraint(constraint, pDisableCollisionsBetweenLinkedObjects);
+
+        return true;
+
     }
 
-    public override bool RemoveConstraintFromWorld(BulletWorld world, BulletConstraint constrain)
+    public override bool RemoveConstraintFromWorld(BulletWorld pWorld, BulletConstraint pConstraint)
     {
-        /* TODO */
-        return false;
+        DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
+        TypedConstraint constraint = (pConstraint as BulletConstraintXNA).constrain;
+        world.RemoveConstraint(constraint);
+        return true;
     }
 
-    public override void SetRestitution(BulletBody pBody, float pRestitution)
+    public override void SetRestitution(BulletBody pCollisionObject, float pRestitution)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
-        body.SetRestitution(pRestitution);
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).body;
+        collisionObject.SetRestitution(pRestitution);
     }
 
     public override int GetShapeType(BulletShape pShape)
     {
-        CollisionShape shape = ((BulletShapeXNA)pShape).shape;
+        CollisionShape shape = (pShape as BulletShapeXNA).shape;
         return (int)shape.GetShapeType();
     }
     public override void SetMargin(BulletShape pShape, float pMargin)
     {
-        CollisionShape shape = ((BulletShapeXNA)pShape).shape;
+        CollisionShape shape = (pShape as BulletShapeXNA).shape;
         shape.SetMargin(pMargin);
     }
 
     public override float GetMargin(BulletShape pShape)
     {
-        CollisionShape shape = ((BulletShapeXNA)pShape).shape;
+        CollisionShape shape = (pShape as BulletShapeXNA).shape;
         return shape.GetMargin();
     }
 
     public override void SetLocalScaling(BulletShape pShape, Vector3 pScale)
     {
-        CollisionShape shape = ((BulletShapeXNA)pShape).shape;
+        CollisionShape shape = (pShape as BulletShapeXNA).shape;
         IndexedVector3 vec = new IndexedVector3(pScale.X, pScale.Y, pScale.Z);
         shape.SetLocalScaling(ref vec);
 
     }
 
-    public override void SetContactProcessingThreshold(BulletBody pBody, float contactprocessingthreshold)
+    public override void SetContactProcessingThreshold(BulletBody pCollisionObject, float contactprocessingthreshold)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
-        body.SetContactProcessingThreshold(contactprocessingthreshold);
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
+        collisionObject.SetContactProcessingThreshold(contactprocessingthreshold);
     }
 
-    public override void SetCcdMotionThreshold(BulletBody pBody, float pccdMotionThreashold)
+    public override void SetCcdMotionThreshold(BulletBody pCollisionObject, float pccdMotionThreashold)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
-        body.SetCcdMotionThreshold(pccdMotionThreashold);
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).body;
+        collisionObject.SetCcdMotionThreshold(pccdMotionThreashold);
     }
 
-    public override void SetCcdSweptSphereRadius(BulletBody pBody, float pCcdSweptSphereRadius)
+    public override void SetCcdSweptSphereRadius(BulletBody pCollisionObject, float pCcdSweptSphereRadius)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
-        body.SetCcdSweptSphereRadius(pCcdSweptSphereRadius);
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).body;
+        collisionObject.SetCcdSweptSphereRadius(pCcdSweptSphereRadius);
     }
 
     public override void SetAngularFactorV(BulletBody pBody, Vector3 pAngularFactor)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
         body.SetAngularFactor(new IndexedVector3(pAngularFactor.X, pAngularFactor.Y, pAngularFactor.Z));
     }
 
-    public override CollisionFlags AddToCollisionFlags(BulletBody pBody, CollisionFlags pcollisionFlags)
+    public override CollisionFlags AddToCollisionFlags(BulletBody pCollisionObject, CollisionFlags pcollisionFlags)
     {
-        CollisionObject body = ((BulletBodyXNA)pBody).body;
-        CollisionFlags existingcollisionFlags = (CollisionFlags)(uint)body.GetCollisionFlags();
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).body;
+        CollisionFlags existingcollisionFlags = (CollisionFlags)(uint)collisionObject.GetCollisionFlags();
         existingcollisionFlags |= pcollisionFlags;
-        body.SetCollisionFlags((BulletXNA.BulletCollision.CollisionFlags)(uint)existingcollisionFlags);
+        collisionObject.SetCollisionFlags((BulletXNA.BulletCollision.CollisionFlags)(uint)existingcollisionFlags);
         return (CollisionFlags) (uint) existingcollisionFlags;
     }
 
     public override bool AddObjectToWorld(BulletWorld pWorld, BulletBody pBody)
     {
+        DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
+        CollisionObject cbody = (pBody as BulletBodyXNA).body;
+        RigidBody rbody = cbody as RigidBody;
+
         // Bullet resets several variables when an object is added to the world. In particular,
         //   BulletXNA resets position and rotation. Gravity is also reset depending on the static/dynamic
         //   type. Of course, the collision flags in the broadphase proxy are initialized to default.
-        DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world;
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
-
-        IndexedMatrix origPos = body.GetWorldTransform();
-        IndexedVector3 origGrav = body.GetGravity();
-
-        //if (!(body.GetCollisionShape().GetShapeType() == BroadphaseNativeTypes.STATIC_PLANE_PROXYTYPE && body.GetCollisionShape().GetShapeType() == BroadphaseNativeTypes.TERRAIN_SHAPE_PROXYTYPE))
-
-        world.AddRigidBody(body);
-
-        body.SetWorldTransform(origPos);
-        body.SetGravity(origGrav);
+        IndexedMatrix origPos = cbody.GetWorldTransform();
+        if (rbody != null)
+        {
+            IndexedVector3 origGrav = rbody.GetGravity();
+            world.AddRigidBody(rbody);
+            rbody.SetGravity(origGrav);
+        }
+        else
+        {
+            world.AddCollisionObject(cbody);
+        }
+        cbody.SetWorldTransform(origPos);
 
         pBody.ApplyCollisionMask(pWorld.physicsScene);
 
@@ -255,99 +280,110 @@ private sealed class BulletConstraintXNA : BulletConstraint
         return true;
     }
 
-    public override void ForceActivationState(BulletBody pBody, ActivationState pActivationState)
+    public override void ForceActivationState(BulletBody pCollisionObject, ActivationState pActivationState)
     {
-        CollisionObject body = ((BulletBodyXNA)pBody).body;
-        body.ForceActivationState((BulletXNA.BulletCollision.ActivationState)(uint)pActivationState);
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).body;
+        collisionObject.ForceActivationState((BulletXNA.BulletCollision.ActivationState)(uint)pActivationState);
     }
 
-    public override void UpdateSingleAabb(BulletWorld pWorld, BulletBody pBody)
+    public override void UpdateSingleAabb(BulletWorld pWorld, BulletBody pCollisionObject)
     {
-        DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world;
-        CollisionObject body = ((BulletBodyXNA)pBody).body;
-        world.UpdateSingleAabb(body);
+        DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).body;
+        world.UpdateSingleAabb(collisionObject);
     }
 
-    public override void UpdateAabbs(BulletWorld world) { /* TODO */ }
-    public override bool GetForceUpdateAllAabbs(BulletWorld world) { /* TODO */ return false;  }
-    public override void SetForceUpdateAllAabbs(BulletWorld world, bool force) { /* TODO */ }
+    public override void UpdateAabbs(BulletWorld pWorld) {
+        DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
+        world.UpdateAabbs();
+    }
+    public override bool GetForceUpdateAllAabbs(BulletWorld pWorld) {
+        DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
+        return world.GetForceUpdateAllAabbs();
 
-    public override bool SetCollisionGroupMask(BulletBody pBody, uint pGroup, uint pMask)
+    }
+    public override void SetForceUpdateAllAabbs(BulletWorld pWorld, bool pForce)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
-        body.GetBroadphaseHandle().m_collisionFilterGroup = (BulletXNA.BulletCollision.CollisionFilterGroups) pGroup;
-        body.GetBroadphaseHandle().m_collisionFilterGroup = (BulletXNA.BulletCollision.CollisionFilterGroups) pGroup;
-        if ((uint) body.GetBroadphaseHandle().m_collisionFilterGroup == 0)
+        DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
+        world.SetForceUpdateAllAabbs(pForce);
+    }
+
+    public override bool SetCollisionGroupMask(BulletBody pCollisionObject, uint pGroup, uint pMask)
+    {
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).body;
+        collisionObject.GetBroadphaseHandle().m_collisionFilterGroup = (BulletXNA.BulletCollision.CollisionFilterGroups) pGroup;
+        collisionObject.GetBroadphaseHandle().m_collisionFilterGroup = (BulletXNA.BulletCollision.CollisionFilterGroups) pGroup;
+        if ((uint) collisionObject.GetBroadphaseHandle().m_collisionFilterGroup == 0)
             return false;
         return true;
     }
 
-    public override void ClearAllForces(BulletBody pBody)
+    public override void ClearAllForces(BulletBody pCollisionObject)
     {
-        CollisionObject body = ((BulletBodyXNA)pBody).body;
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).body;
         IndexedVector3 zeroVector = new IndexedVector3(0, 0, 0);
-        body.SetInterpolationLinearVelocity(ref zeroVector);
-        body.SetInterpolationAngularVelocity(ref zeroVector);
-        IndexedMatrix bodytransform = body.GetWorldTransform();
+        collisionObject.SetInterpolationLinearVelocity(ref zeroVector);
+        collisionObject.SetInterpolationAngularVelocity(ref zeroVector);
+        IndexedMatrix bodytransform = collisionObject.GetWorldTransform();
 
-        body.SetInterpolationWorldTransform(ref bodytransform);
+        collisionObject.SetInterpolationWorldTransform(ref bodytransform);
 
-        if (body is RigidBody)
+        if (collisionObject is RigidBody)
         {
-            RigidBody rigidbody = body as RigidBody;
+            RigidBody rigidbody = collisionObject as RigidBody;
             rigidbody.SetLinearVelocity(zeroVector);
             rigidbody.SetAngularVelocity(zeroVector);
             rigidbody.ClearForces();
         }
     }
 
-    public override void SetInterpolationAngularVelocity(BulletBody pBody, Vector3 pVector3)
+    public override void SetInterpolationAngularVelocity(BulletBody pCollisionObject, Vector3 pVector3)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
         IndexedVector3 vec = new IndexedVector3(pVector3.X, pVector3.Y, pVector3.Z);
-        body.SetInterpolationAngularVelocity(ref vec);
+        collisionObject.SetInterpolationAngularVelocity(ref vec);
     }
 
     public override void SetAngularVelocity(BulletBody pBody, Vector3 pVector3)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
         IndexedVector3 vec = new IndexedVector3(pVector3.X, pVector3.Y, pVector3.Z);
         body.SetAngularVelocity(ref vec);
     }
     public override Vector3 GetTotalForce(BulletBody pBody)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
         IndexedVector3 iv3 = body.GetTotalForce();
         return new Vector3(iv3.X, iv3.Y, iv3.Z);
     }
     public override Vector3 GetTotalTorque(BulletBody pBody)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
         IndexedVector3 iv3 = body.GetTotalTorque();
         return new Vector3(iv3.X, iv3.Y, iv3.Z);
     }
     public override Vector3 GetInvInertiaDiagLocal(BulletBody pBody)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
         IndexedVector3 iv3 = body.GetInvInertiaDiagLocal();
         return new Vector3(iv3.X, iv3.Y, iv3.Z);
     }
     public override void SetInvInertiaDiagLocal(BulletBody pBody, Vector3 inert)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
         IndexedVector3 iv3 = new IndexedVector3(inert.X, inert.Y, inert.Z);
         body.SetInvInertiaDiagLocal(ref iv3);
     }
     public override void ApplyForce(BulletBody pBody, Vector3 force, Vector3 pos)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
         IndexedVector3 forceiv3 = new IndexedVector3(force.X, force.Y, force.Z);
         IndexedVector3 posiv3 = new IndexedVector3(pos.X, pos.Y, pos.Z);
         body.ApplyForce(ref forceiv3, ref posiv3);
     }
     public override void ApplyImpulse(BulletBody pBody, Vector3 imp, Vector3 pos)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
         IndexedVector3 impiv3 = new IndexedVector3(imp.X, imp.Y, imp.Z);
         IndexedVector3 posiv3 = new IndexedVector3(pos.X, pos.Y, pos.Z);
         body.ApplyImpulse(ref impiv3, ref posiv3);
@@ -355,32 +391,32 @@ private sealed class BulletConstraintXNA : BulletConstraint
 
     public override void ClearForces(BulletBody pBody)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
         body.ClearForces();
     }
 
-    public override void SetTranslation(BulletBody pBody, Vector3 _position, Quaternion _orientation)
+    public override void SetTranslation(BulletBody pCollisionObject, Vector3 _position, Quaternion _orientation)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).body;
         IndexedVector3 vposition = new IndexedVector3(_position.X, _position.Y, _position.Z);
         IndexedQuaternion vquaternion = new IndexedQuaternion(_orientation.X, _orientation.Y, _orientation.Z,
                                                               _orientation.W);
         IndexedMatrix mat = IndexedMatrix.CreateFromQuaternion(vquaternion);
         mat._origin = vposition;
-        body.SetWorldTransform(mat);
-        
+        collisionObject.SetWorldTransform(mat);
+
     }
 
-    public override Vector3 GetPosition(BulletBody pBody)
+    public override Vector3 GetPosition(BulletBody pCollisionObject)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
-        IndexedVector3 pos = body.GetInterpolationWorldTransform()._origin;
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
+        IndexedVector3 pos = collisionObject.GetInterpolationWorldTransform()._origin;
         return new Vector3(pos.X, pos.Y, pos.Z);
     }
 
     public override Vector3 CalculateLocalInertia(BulletShape pShape, float pphysMass)
     {
-        CollisionShape shape = ((BulletShapeXNA)pShape).shape;
+        CollisionShape shape = (pShape as BulletShapeXNA).shape;
         IndexedVector3 inertia = IndexedVector3.Zero;
         shape.CalculateLocalInertia(pphysMass, out inertia);
         return new Vector3(inertia.X, inertia.Y, inertia.Z);
@@ -388,81 +424,104 @@ private sealed class BulletConstraintXNA : BulletConstraint
 
     public override void SetMassProps(BulletBody pBody, float pphysMass, Vector3 plocalInertia)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
-        IndexedVector3 inertia = new IndexedVector3(plocalInertia.X, plocalInertia.Y, plocalInertia.Z);
-        body.SetMassProps(pphysMass, inertia);
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
+        if (body != null) // Can't set mass props on collision object.
+        {
+            IndexedVector3 inertia = new IndexedVector3(plocalInertia.X, plocalInertia.Y, plocalInertia.Z);
+            body.SetMassProps(pphysMass, inertia);
+        }
     }
 
 
     public override void SetObjectForce(BulletBody pBody, Vector3 _force)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
         IndexedVector3 force = new IndexedVector3(_force.X, _force.Y, _force.Z);
         body.SetTotalForce(ref force);
     }
 
-    public override void SetFriction(BulletBody pBody, float _currentFriction)
+    public override void SetFriction(BulletBody pCollisionObject, float _currentFriction)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
-        body.SetFriction(_currentFriction);
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).body;
+        collisionObject.SetFriction(_currentFriction);
     }
 
     public override void SetLinearVelocity(BulletBody pBody, Vector3 _velocity)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
         IndexedVector3 velocity = new IndexedVector3(_velocity.X, _velocity.Y, _velocity.Z);
         body.SetLinearVelocity(velocity);
     }
 
-    public override void Activate(BulletBody pBody, bool pforceactivation)
+    public override void Activate(BulletBody pCollisionObject, bool pforceactivation)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
-        body.Activate(pforceactivation);
-        
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
+        collisionObject.Activate(pforceactivation);
+
     }
 
-    public override Quaternion GetOrientation(BulletBody pBody)
+    public override Quaternion GetOrientation(BulletBody pCollisionObject)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
-        IndexedQuaternion mat = body.GetInterpolationWorldTransform().GetRotation();
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
+        IndexedQuaternion mat = collisionObject.GetInterpolationWorldTransform().GetRotation();
         return new Quaternion(mat.X, mat.Y, mat.Z, mat.W);
     }
 
-    public override CollisionFlags RemoveFromCollisionFlags(BulletBody pBody, CollisionFlags pcollisionFlags)
+    public override CollisionFlags RemoveFromCollisionFlags(BulletBody pCollisionObject, CollisionFlags pcollisionFlags)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
-        CollisionFlags existingcollisionFlags = (CollisionFlags)(uint)body.GetCollisionFlags();
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).body;
+        CollisionFlags existingcollisionFlags = (CollisionFlags)(uint)collisionObject.GetCollisionFlags();
         existingcollisionFlags &= ~pcollisionFlags;
-        body.SetCollisionFlags((BulletXNA.BulletCollision.CollisionFlags)(uint)existingcollisionFlags);
+        collisionObject.SetCollisionFlags((BulletXNA.BulletCollision.CollisionFlags)(uint)existingcollisionFlags);
         return (CollisionFlags)(uint)existingcollisionFlags;
     }
 
-    public override float GetCcdMotionThreshold(BulletBody obj) { /* TODO */ return 0f;  }
+    public override float GetCcdMotionThreshold(BulletBody pCollisionObject)
+    {
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
+        return collisionObject.GetCcdSquareMotionThreshold();
+    }
 
-    public override float GetCcdSweptSphereRadius(BulletBody obj) { /* TODO */ return 0f;  }
+    public override float GetCcdSweptSphereRadius(BulletBody pCollisionObject)
+    {
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
+        return collisionObject.GetCcdSweptSphereRadius();
+
+    }
 
-    public override IntPtr GetUserPointer(BulletBody obj) { /* TODO */ return IntPtr.Zero;  }
+    public override IntPtr GetUserPointer(BulletBody pCollisionObject)
+    {
+        CollisionObject shape = (pCollisionObject as BulletBodyXNA).body;
+        return (IntPtr)shape.GetUserPointer();
+    }
 
-    public override void SetUserPointer(BulletBody obj, IntPtr val) { /* TODO */ }
+    public override void SetUserPointer(BulletBody pCollisionObject, IntPtr val)
+    {
+        CollisionObject shape = (pCollisionObject as BulletBodyXNA).body;
+        shape.SetUserPointer(val);
+    }
 
     public override void SetGravity(BulletBody pBody, Vector3 pGravity)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
-        IndexedVector3 gravity = new IndexedVector3(pGravity.X, pGravity.Y, pGravity.Z);
-        body.SetGravity(gravity);
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
+        if (body != null) // Can't set collisionobject.set gravity
+        {
+            IndexedVector3 gravity = new IndexedVector3(pGravity.X, pGravity.Y, pGravity.Z);
+            body.SetGravity(gravity);
+        }
     }
 
     public override bool DestroyConstraint(BulletWorld pWorld, BulletConstraint pConstraint)
     {
-        DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world;
-        TypedConstraint constraint = ((BulletConstraintXNA)pConstraint).constrain;
+        DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
+        TypedConstraint constraint = (pConstraint as BulletConstraintXNA).constrain;
         world.RemoveConstraint(constraint);
         return true;
     }
 
     public override bool SetLinearLimits(BulletConstraint pConstraint, Vector3 low, Vector3 high)
     {
-        Generic6DofConstraint constraint = ((BulletConstraintXNA)pConstraint).constrain as Generic6DofConstraint;
+        Generic6DofConstraint constraint = (pConstraint as BulletConstraintXNA).constrain as Generic6DofConstraint;
         IndexedVector3 lowlimit = new IndexedVector3(low.X, low.Y, low.Z);
         IndexedVector3 highlimit = new IndexedVector3(high.X, high.Y, high.Z);
         constraint.SetLinearLowerLimit(lowlimit);
@@ -472,7 +531,7 @@ private sealed class BulletConstraintXNA : BulletConstraint
 
     public override bool SetAngularLimits(BulletConstraint pConstraint, Vector3 low, Vector3 high)
     {
-        Generic6DofConstraint constraint = ((BulletConstraintXNA)pConstraint).constrain as Generic6DofConstraint;
+        Generic6DofConstraint constraint = (pConstraint as BulletConstraintXNA).constrain as Generic6DofConstraint;
         IndexedVector3 lowlimit = new IndexedVector3(low.X, low.Y, low.Z);
         IndexedVector3 highlimit = new IndexedVector3(high.X, high.Y, high.Z);
         constraint.SetAngularLowerLimit(lowlimit);
@@ -482,31 +541,32 @@ private sealed class BulletConstraintXNA : BulletConstraint
 
     public override void SetConstraintNumSolverIterations(BulletConstraint pConstraint, float cnt)
     {
-        Generic6DofConstraint constraint = ((BulletConstraintXNA)pConstraint).constrain as Generic6DofConstraint;
+        Generic6DofConstraint constraint = (pConstraint as BulletConstraintXNA).constrain as Generic6DofConstraint;
         constraint.SetOverrideNumSolverIterations((int)cnt);
     }
 
     public override bool CalculateTransforms(BulletConstraint pConstraint)
     {
-        Generic6DofConstraint constraint = ((BulletConstraintXNA)pConstraint).constrain as Generic6DofConstraint;
+        Generic6DofConstraint constraint = (pConstraint as BulletConstraintXNA).constrain as Generic6DofConstraint;
         constraint.CalculateTransforms();
         return true;
     }
 
     public override void SetConstraintEnable(BulletConstraint pConstraint, float p_2)
     {
-        Generic6DofConstraint constraint = ((BulletConstraintXNA)pConstraint).constrain as Generic6DofConstraint;
+        Generic6DofConstraint constraint = (pConstraint as BulletConstraintXNA).constrain as Generic6DofConstraint;
         constraint.SetEnabled((p_2 == 0) ? false : true);
     }
 
 
-    //BulletSimAPI.Create6DofConstraint(m_world.ptr, m_body1.ptr, m_body2.ptr,frame1, frame1rot,frame2, frame2rot,useLinearReferenceFrameA, disableCollisionsBetweenLinkedBodies));
-    public override BulletConstraint Create6DofConstraint(BulletWorld pWorld, BulletBody pBody1, BulletBody pBody2, Vector3 pframe1, Quaternion pframe1rot, Vector3 pframe2, Quaternion pframe2rot, bool puseLinearReferenceFrameA, bool pdisableCollisionsBetweenLinkedBodies)
+    public override BulletConstraint Create6DofConstraint(BulletWorld pWorld, BulletBody pBody1, BulletBody pBody2,
+                                    Vector3 pframe1, Quaternion pframe1rot, Vector3 pframe2, Quaternion pframe2rot,
+                                    bool puseLinearReferenceFrameA, bool pdisableCollisionsBetweenLinkedBodies)
 
     {
-        DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world;
-        RigidBody body1 = ((BulletBodyXNA)pBody1).rigidBody;
-        RigidBody body2 = ((BulletBodyXNA)pBody2).rigidBody;
+        DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
+        RigidBody body1 = (pBody1 as BulletBodyXNA).rigidBody;
+        RigidBody body2 = (pBody2 as BulletBodyXNA).rigidBody;
         IndexedVector3 frame1v = new IndexedVector3(pframe1.X, pframe1.Y, pframe1.Z);
         IndexedQuaternion frame1rot = new IndexedQuaternion(pframe1rot.X, pframe1rot.Y, pframe1rot.Z, pframe1rot.W);
         IndexedMatrix frame1 = IndexedMatrix.CreateFromQuaternion(frame1rot);
@@ -525,9 +585,26 @@ private sealed class BulletConstraintXNA : BulletConstraint
         return new BulletConstraintXNA(consttr);
     }
 
-    
+    public override BulletConstraint Create6DofConstraintFixed(BulletWorld pWorld, BulletBody pBody1,
+                                        Vector3 pframe1, Quaternion pframe1rot,
+                                        bool pUseLinearReferenceFrameB, bool pdisableCollisionsBetweenLinkedBodies)
+    {
+        DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
+        RigidBody body1 = (pBody1 as BulletBodyXNA).rigidBody;
+        IndexedVector3 frame1v = new IndexedVector3(pframe1.X, pframe1.Y, pframe1.Z);
+        IndexedQuaternion frame1rot = new IndexedQuaternion(pframe1rot.X, pframe1rot.Y, pframe1rot.Z, pframe1rot.W);
+        IndexedMatrix frame1 = IndexedMatrix.CreateFromQuaternion(frame1rot);
+        frame1._origin = frame1v;
+
+        Generic6DofConstraint consttr = new Generic6DofConstraint(body1, ref frame1, pUseLinearReferenceFrameB);
+        consttr.CalculateTransforms();
+        world.AddConstraint(consttr,pdisableCollisionsBetweenLinkedBodies);
+
+        return new BulletConstraintXNA(consttr);
+    }
+
     /// <summary>
-    /// 
+    ///
     /// </summary>
     /// <param name="pWorld"></param>
     /// <param name="pBody1"></param>
@@ -538,9 +615,9 @@ private sealed class BulletConstraintXNA : BulletConstraint
     /// <returns></returns>
     public override BulletConstraint Create6DofConstraintToPoint(BulletWorld pWorld, BulletBody pBody1, BulletBody pBody2, Vector3 pjoinPoint, bool puseLinearReferenceFrameA, bool pdisableCollisionsBetweenLinkedBodies)
     {
-        DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world;
-        RigidBody body1 = ((BulletBodyXNA)pBody1).rigidBody;
-        RigidBody body2 = ((BulletBodyXNA)pBody2).rigidBody;
+        DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
+        RigidBody body1 = (pBody1 as BulletBodyXNA).rigidBody;
+        RigidBody body2 = (pBody2 as BulletBodyXNA).rigidBody;
         IndexedMatrix frame1 = new IndexedMatrix(IndexedBasisMatrix.Identity, new IndexedVector3(0, 0, 0));
         IndexedMatrix frame2 = new IndexedMatrix(IndexedBasisMatrix.Identity, new IndexedVector3(0, 0, 0));
 
@@ -559,7 +636,7 @@ private sealed class BulletConstraintXNA : BulletConstraint
     //SetFrames(m_constraint.ptr, frameA, frameArot, frameB, frameBrot);
     public override bool SetFrames(BulletConstraint pConstraint, Vector3 pframe1, Quaternion pframe1rot, Vector3 pframe2, Quaternion pframe2rot)
     {
-        Generic6DofConstraint constraint = ((BulletConstraintXNA)pConstraint).constrain as Generic6DofConstraint;
+        Generic6DofConstraint constraint = (pConstraint as BulletConstraintXNA).constrain as Generic6DofConstraint;
         IndexedVector3 frame1v = new IndexedVector3(pframe1.X, pframe1.Y, pframe1.Z);
         IndexedQuaternion frame1rot = new IndexedQuaternion(pframe1rot.X, pframe1rot.Y, pframe1rot.Z, pframe1rot.W);
         IndexedMatrix frame1 = IndexedMatrix.CreateFromQuaternion(frame1rot);
@@ -575,109 +652,110 @@ private sealed class BulletConstraintXNA : BulletConstraint
 
     public override Vector3 GetLinearVelocity(BulletBody pBody)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
         IndexedVector3 iv3 = body.GetLinearVelocity();
         return new Vector3(iv3.X, iv3.Y, iv3.Z);
     }
     public override Vector3 GetAngularVelocity(BulletBody pBody)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
         IndexedVector3 iv3 = body.GetAngularVelocity();
         return new Vector3(iv3.X, iv3.Y, iv3.Z);
     }
     public override Vector3 GetVelocityInLocalPoint(BulletBody pBody, Vector3 pos)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
         IndexedVector3 posiv3 = new IndexedVector3(pos.X, pos.Y, pos.Z);
         IndexedVector3 iv3 = body.GetVelocityInLocalPoint(ref posiv3);
         return new Vector3(iv3.X, iv3.Y, iv3.Z);
     }
-    public override void Translate(BulletBody pBody, Vector3 trans)
+    public override void Translate(BulletBody pCollisionObject, Vector3 trans)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
+        collisionObject.Translate(new IndexedVector3(trans.X,trans.Y,trans.Z));
     }
     public override void UpdateDeactivation(BulletBody pBody, float timeStep)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
         body.UpdateDeactivation(timeStep);
     }
 
     public override bool WantsSleeping(BulletBody pBody)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
         return body.WantsSleeping();
     }
 
     public override void SetAngularFactor(BulletBody pBody, float factor)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
         body.SetAngularFactor(factor);
     }
 
     public override Vector3 GetAngularFactor(BulletBody pBody)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
         IndexedVector3 iv3 =  body.GetAngularFactor();
         return new Vector3(iv3.X, iv3.Y, iv3.Z);
     }
 
-    public override bool IsInWorld(BulletWorld pWorld, BulletBody pBody)
+    public override bool IsInWorld(BulletWorld pWorld, BulletBody pCollisionObject)
     {
-        DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world;
-        CollisionObject body = ((BulletBodyXNA)pBody).body;
-        return world.IsInWorld(body);
+        DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).body;
+        return world.IsInWorld(collisionObject);
     }
 
-    public override void AddConstraintRef(BulletBody pBody, BulletConstraint pConstrain)
+    public override void AddConstraintRef(BulletBody pBody, BulletConstraint pConstraint)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
-        TypedConstraint constrain = ((BulletConstraintXNA)pConstrain).constrain;
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
+        TypedConstraint constrain = (pConstraint as BulletConstraintXNA).constrain;
         body.AddConstraintRef(constrain);
     }
 
-    public override void RemoveConstraintRef(BulletBody pBody, BulletConstraint pConstrain)
+    public override void RemoveConstraintRef(BulletBody pBody, BulletConstraint pConstraint)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
-        TypedConstraint constrain = ((BulletConstraintXNA)pConstrain).constrain;
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
+        TypedConstraint constrain = (pConstraint as BulletConstraintXNA).constrain;
         body.RemoveConstraintRef(constrain);
     }
 
     public override BulletConstraint GetConstraintRef(BulletBody pBody, int index)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
         return new BulletConstraintXNA(body.GetConstraintRef(index));
     }
 
     public override int GetNumConstraintRefs(BulletBody pBody)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
         return body.GetNumConstraintRefs();
     }
 
-    public override void SetInterpolationLinearVelocity(BulletBody pBody, Vector3 VehicleVelocity)
+    public override void SetInterpolationLinearVelocity(BulletBody pCollisionObject, Vector3 VehicleVelocity)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
         IndexedVector3 velocity = new IndexedVector3(VehicleVelocity.X, VehicleVelocity.Y, VehicleVelocity.Z);
-        body.SetInterpolationLinearVelocity(ref velocity);
+        collisionObject.SetInterpolationLinearVelocity(ref velocity);
     }
 
     public override bool UseFrameOffset(BulletConstraint pConstraint, float onOff)
     {
-        Generic6DofConstraint constraint = ((BulletConstraintXNA)pConstraint).constrain as Generic6DofConstraint;
+        Generic6DofConstraint constraint = (pConstraint as BulletConstraintXNA).constrain as Generic6DofConstraint;
         constraint.SetUseFrameOffset((onOff == 0) ? false : true);
         return true;
     }
     //SetBreakingImpulseThreshold(m_constraint.ptr, threshold);
     public override bool SetBreakingImpulseThreshold(BulletConstraint pConstraint, float threshold)
     {
-        Generic6DofConstraint constraint = ((BulletConstraintXNA)pConstraint).constrain as Generic6DofConstraint;
+        Generic6DofConstraint constraint = (pConstraint as BulletConstraintXNA).constrain as Generic6DofConstraint;
         constraint.SetBreakingImpulseThreshold(threshold);
         return true;
     }
     //BulletSimAPI.SetAngularDamping(Prim.PhysBody.ptr, angularDamping);
     public override void SetAngularDamping(BulletBody pBody, float angularDamping)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
         float lineardamping = body.GetLinearDamping();
         body.SetDamping(lineardamping, angularDamping);
 
@@ -685,163 +763,241 @@ private sealed class BulletConstraintXNA : BulletConstraint
 
     public override void UpdateInertiaTensor(BulletBody pBody)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
-        body.UpdateInertiaTensor();
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
+        if (body != null)  // can't update inertia tensor on CollisionObject
+            body.UpdateInertiaTensor();
     }
 
     public override void RecalculateCompoundShapeLocalAabb(BulletShape pCompoundShape)
     {
-        CompoundShape shape = ((BulletShapeXNA)pCompoundShape).shape as CompoundShape;
+        CompoundShape shape = (pCompoundShape as BulletShapeXNA).shape as CompoundShape;
         shape.RecalculateLocalAabb();
     }
 
     //BulletSimAPI.GetCollisionFlags(PhysBody.ptr)
-    public override CollisionFlags GetCollisionFlags(BulletBody pBody)
+    public override CollisionFlags GetCollisionFlags(BulletBody pCollisionObject)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
-        uint flags = (uint)body.GetCollisionFlags();
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
+        uint flags = (uint)collisionObject.GetCollisionFlags();
         return (CollisionFlags) flags;
     }
 
     public override void SetDamping(BulletBody pBody, float pLinear, float pAngular)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
         body.SetDamping(pLinear, pAngular);
     }
     //PhysBody.ptr, PhysicsScene.Params.deactivationTime);
-    public override void SetDeactivationTime(BulletBody pBody, float pDeactivationTime)
+    public override void SetDeactivationTime(BulletBody pCollisionObject, float pDeactivationTime)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
-        body.SetDeactivationTime(pDeactivationTime);
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
+        collisionObject.SetDeactivationTime(pDeactivationTime);
     }
     //SetSleepingThresholds(PhysBody.ptr, PhysicsScene.Params.linearSleepingThreshold, PhysicsScene.Params.angularSleepingThreshold);
     public override void SetSleepingThresholds(BulletBody pBody, float plinearSleepingThreshold, float pangularSleepingThreshold)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
         body.SetSleepingThresholds(plinearSleepingThreshold, pangularSleepingThreshold);
     }
 
-    public override CollisionObjectTypes GetBodyType(BulletBody pBody)
+    public override CollisionObjectTypes GetBodyType(BulletBody pCollisionObject)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
-        return (CollisionObjectTypes)(int) body.GetInternalType();
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).body;
+        return (CollisionObjectTypes)(int) collisionObject.GetInternalType();
     }
 
-    public override void ApplyGravity(BulletBody obj) { /* TODO */ }
+    public override void ApplyGravity(BulletBody pBody)
+    {
+
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
+        body.ApplyGravity();
+    }
 
-    public override Vector3 GetGravity(BulletBody obj) { /* TODO */ return Vector3.Zero;  }
+    public override Vector3 GetGravity(BulletBody pBody)
+    {
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
+        IndexedVector3 gravity = body.GetGravity();
+        return new Vector3(gravity.X, gravity.Y, gravity.Z);
+    }
 
-    public override void SetLinearDamping(BulletBody obj, float lin_damping) { /* TODO */ }
+    public override void SetLinearDamping(BulletBody pBody, float lin_damping)
+    {
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
+        float angularDamping = body.GetAngularDamping();
+        body.SetDamping(lin_damping, angularDamping);
+    }
 
-    public override float GetLinearDamping(BulletBody obj) { /* TODO */ return 0f;  }
+    public override float GetLinearDamping(BulletBody pBody)
+    {
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
+        return body.GetLinearDamping();
+    }
 
-    public override float GetAngularDamping(BulletBody obj) { /* TODO */ return 0f;  }
+    public override float GetAngularDamping(BulletBody pBody)
+    {
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
+        return body.GetAngularDamping();
+    }
 
-    public override float GetLinearSleepingThreshold(BulletBody obj) { /* TODO */ return 0f;  }
+    public override float GetLinearSleepingThreshold(BulletBody pBody)
+    {
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
+        return body.GetLinearSleepingThreshold();
+    }
 
-    public override void ApplyDamping(BulletBody obj, float timeStep) { /* TODO */ }
+    public override void ApplyDamping(BulletBody pBody, float timeStep)
+    {
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
+        body.ApplyDamping(timeStep);
+    }
 
-    public override Vector3 GetLinearFactor(BulletBody obj) { /* TODO */ return Vector3.Zero; }
+    public override Vector3 GetLinearFactor(BulletBody pBody)
+    {
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
+        IndexedVector3 linearFactor = body.GetLinearFactor();
+        return new Vector3(linearFactor.X, linearFactor.Y, linearFactor.Z);
+    }
 
-    public override void SetLinearFactor(BulletBody obj, Vector3 factor) { /* TODO */ }
+    public override void SetLinearFactor(BulletBody pBody, Vector3 factor)
+    {
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
+        body.SetLinearFactor(new IndexedVector3(factor.X, factor.Y, factor.Z));
+    }
 
-    public override void SetCenterOfMassByPosRot(BulletBody obj, Vector3 pos, Quaternion rot) { /* TODO */ }
+    public override void SetCenterOfMassByPosRot(BulletBody pBody, Vector3 pos, Quaternion rot)
+    {
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
+        IndexedQuaternion quat = new IndexedQuaternion(rot.X, rot.Y, rot.Z,rot.W);
+        IndexedMatrix mat = IndexedMatrix.CreateFromQuaternion(quat);
+        mat._origin = new IndexedVector3(pos.X, pos.Y, pos.Z);
+        body.SetCenterOfMassTransform( ref mat);
+        /* TODO: double check this */
+    }
 
     //BulletSimAPI.ApplyCentralForce(PhysBody.ptr, fSum);
     public override void ApplyCentralForce(BulletBody pBody, Vector3 pfSum)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
         IndexedVector3 fSum = new IndexedVector3(pfSum.X, pfSum.Y, pfSum.Z);
         body.ApplyCentralForce(ref fSum);
     }
     public override void ApplyCentralImpulse(BulletBody pBody, Vector3 pfSum)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
         IndexedVector3 fSum = new IndexedVector3(pfSum.X, pfSum.Y, pfSum.Z);
         body.ApplyCentralImpulse(ref fSum);
     }
     public override void ApplyTorque(BulletBody pBody, Vector3 pfSum)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
         IndexedVector3 fSum = new IndexedVector3(pfSum.X, pfSum.Y, pfSum.Z);
         body.ApplyTorque(ref fSum);
     }
     public override void ApplyTorqueImpulse(BulletBody pBody, Vector3 pfSum)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
         IndexedVector3 fSum = new IndexedVector3(pfSum.X, pfSum.Y, pfSum.Z);
         body.ApplyTorqueImpulse(ref fSum);
     }
 
-    public override void DumpRigidBody(BulletWorld p, BulletBody p_2)
-    {
-        //TODO:
-    }
-
-    public override void DumpCollisionShape(BulletWorld p, BulletShape p_2)
+    public override void DestroyObject(BulletWorld pWorld, BulletBody pBody)
     {
-        //TODO:
-    }
-    public override void DumpConstraint(BulletWorld world, BulletConstraint constrain)
-    {
-        //TODO:
-    }
-
-    public override void DumpActivationInfo(BulletWorld world)
-    {
-        //TODO:
-    }
+        DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
+        CollisionObject co = (pBody as BulletBodyXNA).rigidBody;
+        RigidBody bo = co as RigidBody;
+        if (bo == null)
+        {
 
-    public override void DumpAllInfo(BulletWorld world)
-    {
-        //TODO:
-    }
+            if (world.IsInWorld(co))
+            {
+                world.RemoveCollisionObject(co);
+            }
+        }
+        else
+        {
 
-    public override void DumpPhysicsStatistics(BulletWorld world)
-    {
-        //TODO:
-    }
+            if (world.IsInWorld(bo))
+            {
+                world.RemoveRigidBody(bo);
+            }
+        }
+        if (co != null)
+        {
+            if (co.GetUserPointer() != null)
+            {
+                uint localId = (uint) co.GetUserPointer();
+                if (specialCollisionObjects.ContainsKey(localId))
+                {
+                    specialCollisionObjects.Remove(localId);
+                }
+            }
+        }
 
-    public override void DestroyObject(BulletWorld p, BulletBody p_2)
-    {
-        //TODO:
     }
 
     public override void Shutdown(BulletWorld pWorld)
     {
-        DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world;
+        DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
         world.Cleanup();
     }
 
-    public override BulletShape DuplicateCollisionShape(BulletWorld sim, BulletShape srcShape, uint id)
+    public override BulletShape DuplicateCollisionShape(BulletWorld pWorld, BulletShape pShape, uint id)
     {
-        return null;
+        CollisionShape shape1 = (pShape as BulletShapeXNA).shape;
+
+        // TODO:  Turn this from a reference copy to a Value Copy.
+        BulletShapeXNA shape2 = new BulletShapeXNA(shape1, BSShapeTypeFromBroadPhaseNativeType(shape1.GetShapeType()));
+
+        return shape2;
     }
 
-    public override bool DeleteCollisionShape(BulletWorld p, BulletShape p_2)
+    public override bool DeleteCollisionShape(BulletWorld pWorld, BulletShape pShape)
     {
         //TODO:
         return false;
     }
     //(sim.ptr, shape.ptr, prim.LocalID, prim.RawPosition, prim.RawOrientation);
-               
+
     public override BulletBody CreateBodyFromShape(BulletWorld pWorld, BulletShape pShape, uint pLocalID, Vector3 pRawPosition, Quaternion pRawOrientation)
     {
-        CollisionWorld world = ((BulletWorldXNA)pWorld).world;
+        CollisionWorld world = (pWorld as BulletWorldXNA).world;
         IndexedMatrix mat =
             IndexedMatrix.CreateFromQuaternion(new IndexedQuaternion(pRawOrientation.X, pRawOrientation.Y,
                                                                      pRawOrientation.Z, pRawOrientation.W));
         mat._origin = new IndexedVector3(pRawPosition.X, pRawPosition.Y, pRawPosition.Z);
-        CollisionShape shape = ((BulletShapeXNA)pShape).shape;
+        CollisionShape shape = (pShape as BulletShapeXNA).shape;
         //UpdateSingleAabb(world, shape);
         // TODO: Feed Update array into null
-        RigidBody body = new RigidBody(0,new SimMotionState(world,pLocalID,mat,null),shape,IndexedVector3.Zero);
-        
+        SimMotionState motionState = new SimMotionState(this, pLocalID, mat, null);
+        RigidBody body = new RigidBody(0,motionState,shape,IndexedVector3.Zero);
+        RigidBodyConstructionInfo constructionInfo = new RigidBodyConstructionInfo(0, motionState, shape, IndexedVector3.Zero)
+                                                         {
+                                                             m_mass = 0
+                                                         };
+        /*
+            m_mass = mass;
+                        m_motionState =motionState;
+                        m_collisionShape = collisionShape;
+                        m_localInertia = localInertia;
+                        m_linearDamping = 0f;
+                        m_angularDamping = 0f;
+                        m_friction = 0.5f;
+                        m_restitution = 0f;
+                        m_linearSleepingThreshold = 0.8f;
+                        m_angularSleepingThreshold = 1f;
+                        m_additionalDamping = false;
+                        m_additionalDampingFactor = 0.005f;
+                        m_additionalLinearDampingThresholdSqr = 0.01f;
+                        m_additionalAngularDampingThresholdSqr = 0.01f;
+                        m_additionalAngularDampingFactor = 0.01f;
+            m_startWorldTransform = IndexedMatrix.Identity;
+        */
         body.SetUserPointer(pLocalID);
+
         return new BulletBodyXNA(pLocalID, body);
     }
 
-    
+
     public override BulletBody CreateBodyWithDefaultMotionState( BulletShape pShape, uint pLocalID, Vector3 pRawPosition, Quaternion pRawOrientation)
     {
 
@@ -850,7 +1006,7 @@ private sealed class BulletConstraintXNA : BulletConstraint
                                                                      pRawOrientation.Z, pRawOrientation.W));
         mat._origin = new IndexedVector3(pRawPosition.X, pRawPosition.Y, pRawPosition.Z);
 
-        CollisionShape shape = ((BulletShapeXNA)pShape).shape;
+        CollisionShape shape = (pShape as BulletShapeXNA).shape;
 
         // TODO: Feed Update array into null
         RigidBody body = new RigidBody(0, new DefaultMotionState( mat, IndexedMatrix.Identity), shape, IndexedVector3.Zero);
@@ -859,21 +1015,43 @@ private sealed class BulletConstraintXNA : BulletConstraint
         return new BulletBodyXNA(pLocalID, body);
     }
     //(m_mapInfo.terrainBody.ptr, CollisionFlags.CF_STATIC_OBJECT);
-    public override CollisionFlags SetCollisionFlags(BulletBody pBody, CollisionFlags collisionFlags)
+    public override CollisionFlags SetCollisionFlags(BulletBody pCollisionObject, CollisionFlags collisionFlags)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
-        body.SetCollisionFlags((BulletXNA.BulletCollision.CollisionFlags) (uint) collisionFlags);
-        return (CollisionFlags)body.GetCollisionFlags();
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
+        collisionObject.SetCollisionFlags((BulletXNA.BulletCollision.CollisionFlags) (uint) collisionFlags);
+        return (CollisionFlags)collisionObject.GetCollisionFlags();
     }
 
-    public override Vector3 GetAnisotripicFriction(BulletConstraint pconstrain) { /* TODO */ return Vector3.Zero; }
+    public override Vector3 GetAnisotripicFriction(BulletConstraint pconstrain)
+    {
+
+        /* TODO */
+        return Vector3.Zero;
+    }
     public override Vector3 SetAnisotripicFriction(BulletConstraint pconstrain, Vector3 frict) { /* TODO */ return Vector3.Zero; }
     public override bool HasAnisotripicFriction(BulletConstraint pconstrain) { /* TODO */ return false; }
     public override float GetContactProcessingThreshold(BulletBody pBody) { /* TODO */ return 0f; }
-    public override bool IsStaticObject(BulletBody pBody) { /* TODO */ return false; }
-    public override bool IsKinematicObject(BulletBody pBody) { /* TODO */ return false; }
-    public override bool IsStaticOrKinematicObject(BulletBody pBody) { /* TODO */ return false; }
-    public override bool HasContactResponse(BulletBody pBody) { /* TODO */ return false; }
+    public override bool IsStaticObject(BulletBody pCollisionObject)
+    {
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
+        return collisionObject.IsStaticObject();
+
+    }
+    public override bool IsKinematicObject(BulletBody pCollisionObject)
+    {
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
+        return collisionObject.IsKinematicObject();
+    }
+    public override bool IsStaticOrKinematicObject(BulletBody pCollisionObject)
+    {
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
+        return collisionObject.IsStaticOrKinematicObject();
+    }
+    public override bool HasContactResponse(BulletBody pCollisionObject)
+    {
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
+        return collisionObject.HasContactResponse();
+    }
     public override int GetActivationState(BulletBody pBody) { /* TODO */ return 0; }
     public override void SetActivationState(BulletBody pBody, int state) { /* TODO */ }
     public override float GetDeactivationTime(BulletBody pBody) { /* TODO */ return 0f; }
@@ -884,15 +1062,15 @@ private sealed class BulletConstraintXNA : BulletConstraint
     public override float GetHitFraction(BulletBody pBody) { /* TODO */ return 0f; }
 
     //(m_mapInfo.terrainBody.ptr, PhysicsScene.Params.terrainHitFraction);
-    public override void SetHitFraction(BulletBody pBody, float pHitFraction)
+    public override void SetHitFraction(BulletBody pCollisionObject, float pHitFraction)
     {
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
-        body.SetHitFraction(pHitFraction);
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
+        collisionObject.SetHitFraction(pHitFraction);
     }
     //BuildCapsuleShape(physicsScene.World.ptr, 1f, 1f, prim.Scale);
     public override BulletShape BuildCapsuleShape(BulletWorld pWorld, float pRadius, float pHeight, Vector3 pScale)
     {
-        DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world;
+        DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
         IndexedVector3 scale = new IndexedVector3(pScale.X, pScale.Y, pScale.Z);
         CapsuleShapeZ capsuleShapeZ = new CapsuleShapeZ(pRadius, pHeight);
         capsuleShapeZ.SetMargin(world.WorldSettings.Params.collisionMargin);
@@ -906,19 +1084,29 @@ private sealed class BulletConstraintXNA : BulletConstraint
                                             int maxUpdates, ref EntityProperties[] updateArray
                                             )
     {
+
+        UpdatedObjects = updateArray;
+        UpdatedCollisions = collisionArray;
         /* TODO */
-        return new BulletWorldXNA(1, null, null);
+        ConfigurationParameters[] configparms = new ConfigurationParameters[1];
+        configparms[0] = parms;
+        Vector3 worldExtent = new Vector3(Constants.RegionSize, Constants.RegionSize, Constants.RegionHeight);
+        m_maxCollisions = maxCollisions;
+        m_maxUpdatesPerFrame = maxUpdates;
+        specialCollisionObjects = new Dictionary<uint, GhostObject>();
+
+        return new BulletWorldXNA(1, PhysicsScene, BSAPIXNA.Initialize2(worldExtent, configparms, maxCollisions, ref collisionArray, maxUpdates, ref updateArray, null));
     }
 
-    private static object Initialize2(Vector3 worldExtent, 
+    private static DiscreteDynamicsWorld Initialize2(Vector3 worldExtent,
                         ConfigurationParameters[] o,
-                        int mMaxCollisionsPerFrame, ref List<BulletXNA.CollisionDesc> collisionArray,
-                        int mMaxUpdatesPerFrame, ref List<BulletXNA.EntityProperties> updateArray, 
+                        int mMaxCollisionsPerFrame, ref CollisionDesc[] collisionArray,
+                        int mMaxUpdatesPerFrame, ref EntityProperties[] updateArray,
                         object mDebugLogCallbackHandle)
     {
         CollisionWorld.WorldData.ParamData p = new CollisionWorld.WorldData.ParamData();
 
-        p.angularDamping = o[0].XangularDamping;
+        p.angularDamping = BSParam.AngularDamping;
         p.defaultFriction = o[0].defaultFriction;
         p.defaultFriction = o[0].defaultFriction;
         p.defaultDensity = o[0].defaultDensity;
@@ -926,33 +1114,33 @@ private sealed class BulletConstraintXNA : BulletConstraint
         p.collisionMargin = o[0].collisionMargin;
         p.gravity = o[0].gravity;
 
-        p.linearDamping = o[0].XlinearDamping;
-        p.angularDamping = o[0].XangularDamping;
-        p.deactivationTime = o[0].XdeactivationTime;
-        p.linearSleepingThreshold = o[0].XlinearSleepingThreshold;
-        p.angularSleepingThreshold = o[0].XangularSleepingThreshold;
-        p.ccdMotionThreshold = o[0].XccdMotionThreshold;
-        p.ccdSweptSphereRadius = o[0].XccdSweptSphereRadius;
-        p.contactProcessingThreshold = o[0].XcontactProcessingThreshold;
-
-        p.terrainImplementation = o[0].XterrainImplementation;
-        p.terrainFriction = o[0].XterrainFriction;
-
-        p.terrainHitFraction = o[0].XterrainHitFraction;
-        p.terrainRestitution = o[0].XterrainRestitution;
-        p.terrainCollisionMargin = o[0].XterrainCollisionMargin;
-
-        p.avatarFriction = o[0].XavatarFriction;
-        p.avatarStandingFriction = o[0].XavatarStandingFriction;
-        p.avatarDensity = o[0].XavatarDensity;
-        p.avatarRestitution = o[0].XavatarRestitution;
-        p.avatarCapsuleWidth = o[0].XavatarCapsuleWidth;
-        p.avatarCapsuleDepth = o[0].XavatarCapsuleDepth;
-        p.avatarCapsuleHeight = o[0].XavatarCapsuleHeight;
-        p.avatarContactProcessingThreshold = o[0].XavatarContactProcessingThreshold;
-       
-        p.vehicleAngularDamping = o[0].XvehicleAngularDamping;
-        
+        p.linearDamping = BSParam.LinearDamping;
+        p.angularDamping = BSParam.AngularDamping;
+        p.deactivationTime = BSParam.DeactivationTime;
+        p.linearSleepingThreshold = BSParam.LinearSleepingThreshold;
+        p.angularSleepingThreshold = BSParam.AngularSleepingThreshold;
+        p.ccdMotionThreshold = BSParam.CcdMotionThreshold;
+        p.ccdSweptSphereRadius = BSParam.CcdSweptSphereRadius;
+        p.contactProcessingThreshold = BSParam.ContactProcessingThreshold;
+
+        p.terrainImplementation = BSParam.TerrainImplementation;
+        p.terrainFriction = BSParam.TerrainFriction;
+
+        p.terrainHitFraction = BSParam.TerrainHitFraction;
+        p.terrainRestitution = BSParam.TerrainRestitution;
+        p.terrainCollisionMargin = BSParam.TerrainCollisionMargin;
+
+        p.avatarFriction = BSParam.AvatarFriction;
+        p.avatarStandingFriction = BSParam.AvatarStandingFriction;
+        p.avatarDensity = BSParam.AvatarDensity;
+        p.avatarRestitution = BSParam.AvatarRestitution;
+        p.avatarCapsuleWidth = BSParam.AvatarCapsuleWidth;
+        p.avatarCapsuleDepth = BSParam.AvatarCapsuleDepth;
+        p.avatarCapsuleHeight = BSParam.AvatarCapsuleHeight;
+        p.avatarContactProcessingThreshold = BSParam.AvatarContactProcessingThreshold;
+
+        p.vehicleAngularDamping = BSParam.VehicleAngularDamping;
+
         p.maxPersistantManifoldPoolSize = o[0].maxPersistantManifoldPoolSize;
         p.maxCollisionAlgorithmPoolSize = o[0].maxCollisionAlgorithmPoolSize;
         p.shouldDisableContactPoolDynamicAllocation = o[0].shouldDisableContactPoolDynamicAllocation;
@@ -962,17 +1150,17 @@ private sealed class BulletConstraintXNA : BulletConstraint
         p.shouldEnableFrictionCaching = o[0].shouldEnableFrictionCaching;
         p.numberOfSolverIterations = o[0].numberOfSolverIterations;
 
-        p.linksetImplementation = o[0].XlinksetImplementation;
-        p.linkConstraintUseFrameOffset = o[0].XlinkConstraintUseFrameOffset;
-        p.linkConstraintEnableTransMotor = o[0].XlinkConstraintEnableTransMotor;
-        p.linkConstraintTransMotorMaxVel = o[0].XlinkConstraintTransMotorMaxVel;
-        p.linkConstraintTransMotorMaxForce = o[0].XlinkConstraintTransMotorMaxForce;
-        p.linkConstraintERP = o[0].XlinkConstraintERP;
-        p.linkConstraintCFM = o[0].XlinkConstraintCFM;
-        p.linkConstraintSolverIterations = o[0].XlinkConstraintSolverIterations;
-        p.physicsLoggingFrames = o[0].XphysicsLoggingFrames;
+        p.linksetImplementation = BSParam.LinksetImplementation;
+        p.linkConstraintUseFrameOffset = BSParam.NumericBool(BSParam.LinkConstraintUseFrameOffset);
+        p.linkConstraintEnableTransMotor = BSParam.NumericBool(BSParam.LinkConstraintEnableTransMotor);
+        p.linkConstraintTransMotorMaxVel = BSParam.LinkConstraintTransMotorMaxVel;
+        p.linkConstraintTransMotorMaxForce = BSParam.LinkConstraintTransMotorMaxForce;
+        p.linkConstraintERP = BSParam.LinkConstraintERP;
+        p.linkConstraintCFM = BSParam.LinkConstraintCFM;
+        p.linkConstraintSolverIterations = BSParam.LinkConstraintSolverIterations;
+        p.physicsLoggingFrames = o[0].physicsLoggingFrames;
         DefaultCollisionConstructionInfo ccci = new DefaultCollisionConstructionInfo();
-        
+
         DefaultCollisionConfiguration cci = new DefaultCollisionConfiguration();
         CollisionDispatcher m_dispatcher = new CollisionDispatcher(cci);
 
@@ -993,8 +1181,10 @@ private sealed class BulletConstraintXNA : BulletConstraint
         SequentialImpulseConstraintSolver m_solver = new SequentialImpulseConstraintSolver();
 
         DiscreteDynamicsWorld world = new DiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_solver, cci);
-        world.UpdatedObjects = updateArray;
-        world.UpdatedCollisions = collisionArray;
+
+        world.LastCollisionDesc = 0;
+        world.LastEntityProperty = 0;
+
         world.WorldSettings.Params = p;
         world.SetForceUpdateAllAabbs(p.shouldForceUpdateAllAabbs != 0);
         world.GetSolverInfo().m_solverMode = SolverMode.SOLVER_USE_WARMSTARTING | SolverMode.SOLVER_SIMD;
@@ -1028,7 +1218,7 @@ private sealed class BulletConstraintXNA : BulletConstraint
         world.GetSolverInfo().m_restingContactRestitutionThreshold = 2;
         world.SetForceUpdateAllAabbs(true);
 
-
+        //BSParam.TerrainImplementation = 0;
         world.SetGravity(new IndexedVector3(0,0,p.gravity));
 
         return world;
@@ -1036,7 +1226,7 @@ private sealed class BulletConstraintXNA : BulletConstraint
     //m_constraint.ptr, ConstraintParams.BT_CONSTRAINT_STOP_CFM, cfm, ConstraintParamAxis.AXIS_ALL
     public override bool SetConstraintParam(BulletConstraint pConstraint, ConstraintParams paramIndex, float paramvalue, ConstraintParamAxis axis)
     {
-        Generic6DofConstraint constrain = ((BulletConstraintXNA)pConstraint).constrain as Generic6DofConstraint;
+        Generic6DofConstraint constrain = (pConstraint as BulletConstraintXNA).constrain as Generic6DofConstraint;
         if (axis == ConstraintParamAxis.AXIS_LINEAR_ALL || axis == ConstraintParamAxis.AXIS_ALL)
         {
             constrain.SetParam((BulletXNA.BulletDynamics.ConstraintParams) (int) paramIndex, paramvalue, 0);
@@ -1059,7 +1249,8 @@ private sealed class BulletConstraintXNA : BulletConstraint
     public override bool PushUpdate(BulletBody pCollisionObject)
     {
         bool ret = false;
-        RigidBody rb = ((BulletBodyXNA)pCollisionObject).rigidBody;
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
+        RigidBody rb = collisionObject as RigidBody;
         if (rb != null)
         {
             SimMotionState sms = rb.GetMotionState() as SimMotionState;
@@ -1072,62 +1263,62 @@ private sealed class BulletConstraintXNA : BulletConstraint
             }
         }
         return ret;
-        
+
     }
 
     public override float GetAngularMotionDisc(BulletShape pShape)
     {
-        CollisionShape shape = ((BulletShapeXNA)pShape).shape;
+        CollisionShape shape = (pShape as BulletShapeXNA).shape;
         return shape.GetAngularMotionDisc();
     }
     public override float GetContactBreakingThreshold(BulletShape pShape, float defaultFactor)
     {
-        CollisionShape shape = ((BulletShapeXNA)pShape).shape;
+        CollisionShape shape = (pShape as BulletShapeXNA).shape;
         return shape.GetContactBreakingThreshold(defaultFactor);
     }
     public override bool IsCompound(BulletShape pShape)
     {
-        CollisionShape shape = ((BulletShapeXNA)pShape).shape;
+        CollisionShape shape = (pShape as BulletShapeXNA).shape;
         return shape.IsCompound();
     }
     public override bool IsSoftBody(BulletShape pShape)
     {
-        CollisionShape shape = ((BulletShapeXNA)pShape).shape;
+        CollisionShape shape = (pShape as BulletShapeXNA).shape;
         return shape.IsSoftBody();
     }
     public override bool IsPolyhedral(BulletShape pShape)
     {
-        CollisionShape shape = ((BulletShapeXNA)pShape).shape;
+        CollisionShape shape = (pShape as BulletShapeXNA).shape;
         return shape.IsPolyhedral();
     }
     public override bool IsConvex2d(BulletShape pShape)
     {
-        CollisionShape shape = ((BulletShapeXNA)pShape).shape;
+        CollisionShape shape = (pShape as BulletShapeXNA).shape;
         return shape.IsConvex2d();
     }
     public override bool IsConvex(BulletShape pShape)
     {
-        CollisionShape shape = ((BulletShapeXNA)pShape).shape;
+        CollisionShape shape = (pShape as BulletShapeXNA).shape;
         return shape.IsConvex();
     }
     public override bool IsNonMoving(BulletShape pShape)
     {
-        CollisionShape shape = ((BulletShapeXNA)pShape).shape;
+        CollisionShape shape = (pShape as BulletShapeXNA).shape;
         return shape.IsNonMoving();
     }
     public override bool IsConcave(BulletShape pShape)
     {
-        CollisionShape shape = ((BulletShapeXNA)pShape).shape;
+        CollisionShape shape = (pShape as BulletShapeXNA).shape;
         return shape.IsConcave();
     }
     public override bool IsInfinite(BulletShape pShape)
     {
-        CollisionShape shape = ((BulletShapeXNA)pShape).shape;
+        CollisionShape shape = (pShape as BulletShapeXNA).shape;
         return shape.IsInfinite();
     }
     public override bool IsNativeShape(BulletShape pShape)
     {
-        CollisionShape shape = ((BulletShapeXNA)pShape).shape;
+        CollisionShape shape = (pShape as BulletShapeXNA).shape;
         bool ret;
         switch (shape.GetShapeType())
         {
@@ -1144,38 +1335,59 @@ private sealed class BulletConstraintXNA : BulletConstraint
         return ret;
     }
 
-    public override void SetShapeCollisionMargin(BulletShape shape, float margin) { /* TODO */ }
+    public override void SetShapeCollisionMargin(BulletShape pShape, float pMargin)
+    {
+        CollisionShape shape = (pShape as BulletShapeXNA).shape;
+        shape.SetMargin(pMargin);
+    }
 
     //sim.ptr, shape.ptr,prim.LocalID, prim.RawPosition, prim.RawOrientation
     public override BulletBody CreateGhostFromShape(BulletWorld pWorld, BulletShape pShape, uint pLocalID, Vector3 pRawPosition, Quaternion pRawOrientation)
     {
-        DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world;
+        DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
         IndexedMatrix bodyTransform = new IndexedMatrix();
         bodyTransform._origin = new IndexedVector3(pRawPosition.X, pRawPosition.Y, pRawPosition.Z);
         bodyTransform.SetRotation(new IndexedQuaternion(pRawOrientation.X,pRawOrientation.Y,pRawOrientation.Z,pRawOrientation.W));
         GhostObject gObj = new PairCachingGhostObject();
         gObj.SetWorldTransform(bodyTransform);
-        CollisionShape shape = ((BulletShapeXNA)pShape).shape;
+        CollisionShape shape = (pShape as BulletShapeXNA).shape;
         gObj.SetCollisionShape(shape);
         gObj.SetUserPointer(pLocalID);
+
+        if (specialCollisionObjects.ContainsKey(pLocalID))
+            specialCollisionObjects[pLocalID] = gObj;
+        else
+            specialCollisionObjects.Add(pLocalID, gObj);
+
         // TODO: Add to Special CollisionObjects!
         return new BulletBodyXNA(pLocalID, gObj);
     }
 
-    public override void SetCollisionShape(BulletWorld pWorld, BulletBody pObj, BulletShape pShape)
+    public override void SetCollisionShape(BulletWorld pWorld, BulletBody pCollisionObject, BulletShape pShape)
     {
-        DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world;
-        CollisionObject obj = ((BulletBodyXNA)pObj).body;
-        CollisionShape shape = ((BulletShapeXNA)pShape).shape;
-        obj.SetCollisionShape(shape);
-
+        DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).body;
+        if (pShape == null)
+        {
+            collisionObject.SetCollisionShape(new EmptyShape());
+        }
+        else
+        {
+            CollisionShape shape = (pShape as BulletShapeXNA).shape;
+            collisionObject.SetCollisionShape(shape);
+        }
+    }
+    public override BulletShape GetCollisionShape(BulletBody pCollisionObject)
+    {
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
+        CollisionShape shape = collisionObject.GetCollisionShape();
+        return new BulletShapeXNA(shape, BSShapeTypeFromBroadPhaseNativeType(shape.GetShapeType()));
     }
-    public override BulletShape GetCollisionShape(BulletBody obj) { /* TODO */ return null; }
 
     //(PhysicsScene.World.ptr, nativeShapeData)
     public override BulletShape BuildNativeShape(BulletWorld pWorld, ShapeData pShapeData)
     {
-        DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world;
+        DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
         CollisionShape shape = null;
         switch (pShapeData.Type)
         {
@@ -1210,15 +1422,15 @@ private sealed class BulletConstraintXNA : BulletConstraint
 
     public override int GetNumberOfCompoundChildren(BulletShape pCompoundShape)
     {
-        CompoundShape compoundshape = ((BulletShapeXNA)pCompoundShape).shape as CompoundShape;
+        CompoundShape compoundshape = (pCompoundShape as BulletShapeXNA).shape as CompoundShape;
         return compoundshape.GetNumChildShapes();
     }
     //LinksetRoot.PhysShape.ptr, newShape.ptr, displacementPos, displacementRot
     public override void AddChildShapeToCompoundShape(BulletShape pCShape, BulletShape paddShape, Vector3 displacementPos, Quaternion displacementRot)
     {
         IndexedMatrix relativeTransform = new IndexedMatrix();
-        CompoundShape compoundshape = ((BulletShapeXNA)pCShape).shape as CompoundShape;
-        CollisionShape addshape = ((BulletShapeXNA)paddShape).shape;
+        CompoundShape compoundshape = (pCShape as BulletShapeXNA).shape as CompoundShape;
+        CollisionShape addshape = (paddShape as BulletShapeXNA).shape;
 
         relativeTransform._origin = new IndexedVector3(displacementPos.X, displacementPos.Y, displacementPos.Z);
         relativeTransform.SetRotation(new IndexedQuaternion(displacementRot.X,displacementRot.Y,displacementRot.Z,displacementRot.W));
@@ -1228,15 +1440,155 @@ private sealed class BulletConstraintXNA : BulletConstraint
 
     public override BulletShape RemoveChildShapeFromCompoundShapeIndex(BulletShape pCShape, int pii)
     {
-        CompoundShape compoundshape = ((BulletShapeXNA)pCShape).shape as CompoundShape;
+        CompoundShape compoundshape = (pCShape as BulletShapeXNA).shape as CompoundShape;
         CollisionShape ret = null;
         ret = compoundshape.GetChildShape(pii);
         compoundshape.RemoveChildShapeByIndex(pii);
-        return new BulletShapeXNA(ret, BSPhysicsShapeType.SHAPE_UNKNOWN);
+        return new BulletShapeXNA(ret, BSShapeTypeFromBroadPhaseNativeType(ret.GetShapeType()));
+    }
+
+    public override BulletShape GetChildShapeFromCompoundShapeIndex(BulletShape cShape, int indx) {
+
+        if (cShape == null)
+            return null;
+        CompoundShape compoundShape = (cShape as BulletShapeXNA).shape as CompoundShape;
+        CollisionShape shape = compoundShape.GetChildShape(indx);
+        BulletShape retShape = new BulletShapeXNA(shape, BSShapeTypeFromBroadPhaseNativeType(shape.GetShapeType()));
+
+
+        return retShape;
+    }
+
+    public BSPhysicsShapeType BSShapeTypeFromBroadPhaseNativeType(BroadphaseNativeTypes pin)
+    {
+        BSPhysicsShapeType ret = BSPhysicsShapeType.SHAPE_UNKNOWN;
+        switch (pin)
+        {
+            case BroadphaseNativeTypes.BOX_SHAPE_PROXYTYPE:
+                ret =  BSPhysicsShapeType.SHAPE_BOX;
+                break;
+            case BroadphaseNativeTypes.TRIANGLE_SHAPE_PROXYTYPE:
+                ret =  BSPhysicsShapeType.SHAPE_UNKNOWN;
+                break;
+
+            case BroadphaseNativeTypes.TETRAHEDRAL_SHAPE_PROXYTYPE:
+                ret =  BSPhysicsShapeType.SHAPE_UNKNOWN;
+                break;
+            case BroadphaseNativeTypes.CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE:
+                ret =  BSPhysicsShapeType.SHAPE_MESH;
+                break;
+            case BroadphaseNativeTypes.CONVEX_HULL_SHAPE_PROXYTYPE:
+                ret =  BSPhysicsShapeType.SHAPE_HULL;
+                break;
+            case BroadphaseNativeTypes.CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE:
+                ret =  BSPhysicsShapeType.SHAPE_UNKNOWN;
+                break;
+            case BroadphaseNativeTypes.CUSTOM_POLYHEDRAL_SHAPE_TYPE:
+                ret =  BSPhysicsShapeType.SHAPE_UNKNOWN;
+                break;
+            //implicit convex shapes
+            case BroadphaseNativeTypes.IMPLICIT_CONVEX_SHAPES_START_HERE:
+                ret =  BSPhysicsShapeType.SHAPE_UNKNOWN;
+                break;
+            case BroadphaseNativeTypes.SPHERE_SHAPE_PROXYTYPE:
+                ret =  BSPhysicsShapeType.SHAPE_SPHERE;
+                break;
+            case BroadphaseNativeTypes.MULTI_SPHERE_SHAPE_PROXYTYPE:
+                ret =  BSPhysicsShapeType.SHAPE_UNKNOWN;
+                break;
+            case BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE:
+                ret =  BSPhysicsShapeType.SHAPE_CAPSULE;
+                break;
+            case BroadphaseNativeTypes.CONE_SHAPE_PROXYTYPE:
+                ret =  BSPhysicsShapeType.SHAPE_CONE;
+                break;
+            case BroadphaseNativeTypes.CONVEX_SHAPE_PROXYTYPE:
+                ret =  BSPhysicsShapeType.SHAPE_UNKNOWN;
+                break;
+            case BroadphaseNativeTypes.CYLINDER_SHAPE_PROXYTYPE:
+                ret =  BSPhysicsShapeType.SHAPE_CYLINDER;
+                break;
+            case BroadphaseNativeTypes.UNIFORM_SCALING_SHAPE_PROXYTYPE:
+                ret =  BSPhysicsShapeType.SHAPE_UNKNOWN;
+                break;
+            case BroadphaseNativeTypes.MINKOWSKI_SUM_SHAPE_PROXYTYPE:
+                ret =  BSPhysicsShapeType.SHAPE_UNKNOWN;
+                break;
+            case BroadphaseNativeTypes.MINKOWSKI_DIFFERENCE_SHAPE_PROXYTYPE:
+                ret =  BSPhysicsShapeType.SHAPE_UNKNOWN;
+                break;
+            case BroadphaseNativeTypes.BOX_2D_SHAPE_PROXYTYPE:
+                ret =  BSPhysicsShapeType.SHAPE_UNKNOWN;
+                break;
+            case BroadphaseNativeTypes.CONVEX_2D_SHAPE_PROXYTYPE:
+                ret =  BSPhysicsShapeType.SHAPE_UNKNOWN;
+                break;
+            case BroadphaseNativeTypes.CUSTOM_CONVEX_SHAPE_TYPE:
+                ret =  BSPhysicsShapeType.SHAPE_UNKNOWN;
+                break;
+            //concave shape
+            case BroadphaseNativeTypes.CONCAVE_SHAPES_START_HERE:
+                ret =  BSPhysicsShapeType.SHAPE_UNKNOWN;
+                break;
+            //keep all the convex shapetype below here, for the check IsConvexShape in broadphase proxy!
+            case BroadphaseNativeTypes.TRIANGLE_MESH_SHAPE_PROXYTYPE:
+                ret =  BSPhysicsShapeType.SHAPE_MESH;
+                break;
+            case BroadphaseNativeTypes.SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE:
+                ret =  BSPhysicsShapeType.SHAPE_MESH;
+                break;
+            ///used for demo integration FAST/Swift collision library and Bullet
+            case BroadphaseNativeTypes.FAST_CONCAVE_MESH_PROXYTYPE:
+                ret =  BSPhysicsShapeType.SHAPE_MESH;
+                break;
+            //terrain
+            case BroadphaseNativeTypes.TERRAIN_SHAPE_PROXYTYPE:
+                ret =  BSPhysicsShapeType.SHAPE_HEIGHTMAP;
+                break;
+            ///Used for GIMPACT Trimesh integration
+            case BroadphaseNativeTypes.GIMPACT_SHAPE_PROXYTYPE:
+                ret =  BSPhysicsShapeType.SHAPE_MESH;
+                break;
+            ///Multimaterial mesh
+            case BroadphaseNativeTypes.MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE:
+                ret =  BSPhysicsShapeType.SHAPE_MESH;
+                break;
+
+            case BroadphaseNativeTypes.EMPTY_SHAPE_PROXYTYPE:
+                ret =  BSPhysicsShapeType.SHAPE_UNKNOWN;
+                break;
+            case BroadphaseNativeTypes.STATIC_PLANE_PROXYTYPE:
+                ret =  BSPhysicsShapeType.SHAPE_GROUNDPLANE;
+                break;
+            case BroadphaseNativeTypes.CUSTOM_CONCAVE_SHAPE_TYPE:
+                ret =  BSPhysicsShapeType.SHAPE_UNKNOWN;
+                break;
+            case BroadphaseNativeTypes.CONCAVE_SHAPES_END_HERE:
+                ret =  BSPhysicsShapeType.SHAPE_UNKNOWN;
+                break;
+
+            case BroadphaseNativeTypes.COMPOUND_SHAPE_PROXYTYPE:
+                ret =  BSPhysicsShapeType.SHAPE_COMPOUND;
+                break;
+
+            case BroadphaseNativeTypes.SOFTBODY_SHAPE_PROXYTYPE:
+                ret =  BSPhysicsShapeType.SHAPE_MESH;
+                break;
+            case BroadphaseNativeTypes.HFFLUID_SHAPE_PROXYTYPE:
+                ret =  BSPhysicsShapeType.SHAPE_UNKNOWN;
+                break;
+            case BroadphaseNativeTypes.HFFLUID_BUOYANT_CONVEX_SHAPE_PROXYTYPE:
+                ret =  BSPhysicsShapeType.SHAPE_UNKNOWN;
+                break;
+            case BroadphaseNativeTypes.INVALID_SHAPE_PROXYTYPE:
+                ret =  BSPhysicsShapeType.SHAPE_UNKNOWN;
+                break;
+        }
+        return ret;
     }
 
-    public override BulletShape GetChildShapeFromCompoundShapeIndex(BulletShape cShape, int indx) { /* TODO */ return null; }
     public override void RemoveChildShapeFromCompoundShape(BulletShape cShape, BulletShape removeShape) { /* TODO */ }
+    public override void UpdateChildTransform(BulletShape pShape, int childIndex, Vector3 pos, Quaternion rot, bool shouldRecalculateLocalAabb) { /* TODO */ }
 
     public override BulletShape CreateGroundPlaneShape(uint pLocalId, float pheight, float pcollisionMargin)
     {
@@ -1246,18 +1598,144 @@ private sealed class BulletConstraintXNA : BulletConstraint
         return new BulletShapeXNA(m_planeshape, BSPhysicsShapeType.SHAPE_GROUNDPLANE);
     }
 
-    public override BulletConstraint CreateHingeConstraint(BulletWorld pWorld, BulletBody pBody1, BulletBody ppBody2, Vector3 ppivotInA, Vector3 ppivotInB, Vector3 paxisInA, Vector3 paxisInB, bool puseLinearReferenceFrameA, bool pdisableCollisionsBetweenLinkedBodies)
+    public override BulletConstraint Create6DofSpringConstraint(BulletWorld pWorld, BulletBody pBody1, BulletBody pBody2,
+                                    Vector3 pframe1, Quaternion pframe1rot, Vector3 pframe2, Quaternion pframe2rot,
+                                    bool puseLinearReferenceFrameA, bool pdisableCollisionsBetweenLinkedBodies)
+
+    {
+        Generic6DofSpringConstraint constrain = null;
+        DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
+        RigidBody body1 = (pBody1 as BulletBodyXNA).rigidBody;
+        RigidBody body2 = (pBody2 as BulletBodyXNA).rigidBody;
+        if (body1 != null && body2 != null)
+        {
+            IndexedVector3 frame1v = new IndexedVector3(pframe1.X, pframe1.Y, pframe1.Z);
+            IndexedQuaternion frame1rot = new IndexedQuaternion(pframe1rot.X, pframe1rot.Y, pframe1rot.Z, pframe1rot.W);
+            IndexedMatrix frame1 = IndexedMatrix.CreateFromQuaternion(frame1rot);
+            frame1._origin = frame1v;
+
+            IndexedVector3 frame2v = new IndexedVector3(pframe2.X, pframe2.Y, pframe2.Z);
+            IndexedQuaternion frame2rot = new IndexedQuaternion(pframe2rot.X, pframe2rot.Y, pframe2rot.Z, pframe2rot.W);
+            IndexedMatrix frame2 = IndexedMatrix.CreateFromQuaternion(frame2rot);
+            frame2._origin = frame1v;
+
+            constrain = new Generic6DofSpringConstraint(body1, body2, ref frame1, ref frame2, puseLinearReferenceFrameA);
+            world.AddConstraint(constrain, pdisableCollisionsBetweenLinkedBodies);
+
+            constrain.CalculateTransforms();
+        }
+
+        return new BulletConstraintXNA(constrain);
+    }
+
+    public override BulletConstraint CreateHingeConstraint(BulletWorld pWorld, BulletBody pBody1, BulletBody pBody2,
+                        Vector3 ppivotInA, Vector3 ppivotInB, Vector3 paxisInA, Vector3 paxisInB,
+                        bool puseLinearReferenceFrameA, bool pdisableCollisionsBetweenLinkedBodies)
     {
         HingeConstraint constrain = null;
-        DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world;
-        RigidBody rb1 = ((BulletBodyXNA)pBody1).rigidBody;
-        RigidBody rb2 = ((BulletBodyXNA)ppBody2).rigidBody;
+        DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
+        RigidBody rb1 = (pBody1 as BulletBodyXNA).rigidBody;
+        RigidBody rb2 = (pBody2 as BulletBodyXNA).rigidBody;
         if (rb1 != null && rb2 != null)
         {
             IndexedVector3 pivotInA = new IndexedVector3(ppivotInA.X, ppivotInA.Y, ppivotInA.Z);
             IndexedVector3 pivotInB = new IndexedVector3(ppivotInB.X, ppivotInB.Y, ppivotInB.Z);
             IndexedVector3 axisInA = new IndexedVector3(paxisInA.X, paxisInA.Y, paxisInA.Z);
             IndexedVector3 axisInB = new IndexedVector3(paxisInB.X, paxisInB.Y, paxisInB.Z);
+            constrain = new HingeConstraint(rb1, rb2, ref pivotInA, ref pivotInB, ref axisInA, ref axisInB, puseLinearReferenceFrameA);
+            world.AddConstraint(constrain, pdisableCollisionsBetweenLinkedBodies);
+        }
+        return new BulletConstraintXNA(constrain);
+    }
+
+    public override BulletConstraint CreateSliderConstraint(BulletWorld pWorld, BulletBody pBody1, BulletBody pBody2,
+                        Vector3 pframe1, Quaternion pframe1rot,
+                        Vector3 pframe2, Quaternion pframe2rot,
+                        bool puseLinearReferenceFrameA, bool pdisableCollisionsBetweenLinkedBodies)
+    {
+        SliderConstraint constrain = null;
+        DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
+        RigidBody rb1 = (pBody1 as BulletBodyXNA).rigidBody;
+        RigidBody rb2 = (pBody2 as BulletBodyXNA).rigidBody;
+        if (rb1 != null && rb2 != null)
+        {
+            IndexedVector3 frame1v = new IndexedVector3(pframe1.X, pframe1.Y, pframe1.Z);
+            IndexedQuaternion frame1rot = new IndexedQuaternion(pframe1rot.X, pframe1rot.Y, pframe1rot.Z, pframe1rot.W);
+            IndexedMatrix frame1 = IndexedMatrix.CreateFromQuaternion(frame1rot);
+            frame1._origin = frame1v;
+
+            IndexedVector3 frame2v = new IndexedVector3(pframe2.X, pframe2.Y, pframe2.Z);
+            IndexedQuaternion frame2rot = new IndexedQuaternion(pframe2rot.X, pframe2rot.Y, pframe2rot.Z, pframe2rot.W);
+            IndexedMatrix frame2 = IndexedMatrix.CreateFromQuaternion(frame2rot);
+            frame2._origin = frame1v;
+
+            constrain = new SliderConstraint(rb1, rb2, ref frame1, ref frame2, puseLinearReferenceFrameA);
+            world.AddConstraint(constrain, pdisableCollisionsBetweenLinkedBodies);
+        }
+        return new BulletConstraintXNA(constrain);
+    }
+
+    public override BulletConstraint CreateConeTwistConstraint(BulletWorld pWorld, BulletBody pBody1, BulletBody pBody2,
+                        Vector3 pframe1, Quaternion pframe1rot,
+                        Vector3 pframe2, Quaternion pframe2rot,
+                        bool pdisableCollisionsBetweenLinkedBodies)
+    {
+        ConeTwistConstraint constrain = null;
+        DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
+        RigidBody rb1 = (pBody1 as BulletBodyXNA).rigidBody;
+        RigidBody rb2 = (pBody2 as BulletBodyXNA).rigidBody;
+        if (rb1 != null && rb2 != null)
+        {
+            IndexedVector3 frame1v = new IndexedVector3(pframe1.X, pframe1.Y, pframe1.Z);
+            IndexedQuaternion frame1rot = new IndexedQuaternion(pframe1rot.X, pframe1rot.Y, pframe1rot.Z, pframe1rot.W);
+            IndexedMatrix frame1 = IndexedMatrix.CreateFromQuaternion(frame1rot);
+            frame1._origin = frame1v;
+
+            IndexedVector3 frame2v = new IndexedVector3(pframe2.X, pframe2.Y, pframe2.Z);
+            IndexedQuaternion frame2rot = new IndexedQuaternion(pframe2rot.X, pframe2rot.Y, pframe2rot.Z, pframe2rot.W);
+            IndexedMatrix frame2 = IndexedMatrix.CreateFromQuaternion(frame2rot);
+            frame2._origin = frame1v;
+
+            constrain = new ConeTwistConstraint(rb1, rb2, ref frame1, ref frame2);
+            world.AddConstraint(constrain, pdisableCollisionsBetweenLinkedBodies);
+        }
+        return new BulletConstraintXNA(constrain);
+    }
+
+    public override BulletConstraint CreateGearConstraint(BulletWorld pWorld, BulletBody pBody1, BulletBody pBody2,
+                        Vector3 paxisInA, Vector3 paxisInB,
+                        float pratio, bool pdisableCollisionsBetweenLinkedBodies)
+    {
+        Generic6DofConstraint constrain = null;
+        /*   BulletXNA does not have a gear constraint
+        GearConstraint constrain = null;
+        DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
+        RigidBody rb1 = (pBody1 as BulletBodyXNA).rigidBody;
+        RigidBody rb2 = (pBody2 as BulletBodyXNA).rigidBody;
+        if (rb1 != null && rb2 != null)
+        {
+            IndexedVector3 axis1 = new IndexedVector3(paxisInA.X, paxisInA.Y, paxisInA.Z);
+            IndexedVector3 axis2 = new IndexedVector3(paxisInB.X, paxisInB.Y, paxisInB.Z);
+            constrain = new GearConstraint(rb1, rb2, ref axis1, ref axis2, pratio);
+            world.AddConstraint(constrain, pdisableCollisionsBetweenLinkedBodies);
+        }
+        */
+        return new BulletConstraintXNA(constrain);
+    }
+
+    public override BulletConstraint CreatePoint2PointConstraint(BulletWorld pWorld, BulletBody pBody1, BulletBody pBody2,
+                        Vector3 ppivotInA, Vector3 ppivotInB,
+                        bool pdisableCollisionsBetweenLinkedBodies)
+    {
+        Point2PointConstraint constrain = null;
+        DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
+        RigidBody rb1 = (pBody1 as BulletBodyXNA).rigidBody;
+        RigidBody rb2 = (pBody2 as BulletBodyXNA).rigidBody;
+        if (rb1 != null && rb2 != null)
+        {
+            IndexedVector3 pivotInA = new IndexedVector3(ppivotInA.X, ppivotInA.Y, ppivotInA.Z);
+            IndexedVector3 pivotInB = new IndexedVector3(ppivotInB.X, ppivotInB.Y, ppivotInB.Z);
+            constrain = new Point2PointConstraint(rb1, rb2, ref pivotInA, ref pivotInB);
             world.AddConstraint(constrain, pdisableCollisionsBetweenLinkedBodies);
         }
         return new BulletConstraintXNA(constrain);
@@ -1265,9 +1743,9 @@ private sealed class BulletConstraintXNA : BulletConstraint
 
     public override BulletShape CreateHullShape(BulletWorld pWorld, int pHullCount, float[] pConvHulls)
     {
-        DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world;
+        DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
         CompoundShape compoundshape = new CompoundShape(false);
-        
+
         compoundshape.SetMargin(world.WorldSettings.Params.collisionMargin);
         int ii = 1;
 
@@ -1283,7 +1761,7 @@ private sealed class BulletConstraintXNA : BulletConstraint
             int ender = ((ii + 4) + (vertexCount*3));
             for (int iii = ii + 4; iii < ender; iii+=3)
             {
-               
+
                 virts.Add(new IndexedVector3(pConvHulls[iii], pConvHulls[iii + 1], pConvHulls[iii +2]));
             }
             ConvexHullShape convexShape = new ConvexHullShape(virts, vertexCount);
@@ -1291,26 +1769,39 @@ private sealed class BulletConstraintXNA : BulletConstraint
             compoundshape.AddChildShape(ref childTrans, convexShape);
             ii += (vertexCount*3 + 4);
         }
-        
+
         return new BulletShapeXNA(compoundshape, BSPhysicsShapeType.SHAPE_HULL);
     }
 
-    public override BulletShape BuildHullShapeFromMesh(BulletWorld world, BulletShape meshShape) { /* TODO */ return null; }
+    public override BulletShape BuildHullShapeFromMesh(BulletWorld world, BulletShape meshShape, HACDParams parms)
+    {
+        /* TODO */ return null;
+    }
+
+    public override BulletShape BuildConvexHullShapeFromMesh(BulletWorld world, BulletShape meshShape)
+    {
+        /* TODO */ return null;
+    }
+
+    public override BulletShape CreateConvexHullShape(BulletWorld pWorld, int pIndicesCount, int[] indices, int pVerticesCount, float[] verticesAsFloats)
+    {
+        /* TODO */ return null;
+    }
 
     public override BulletShape CreateMeshShape(BulletWorld pWorld, int pIndicesCount, int[] indices, int pVerticesCount, float[] verticesAsFloats)
     {
         //DumpRaw(indices,verticesAsFloats,pIndicesCount,pVerticesCount);
-        
+
         for (int iter = 0; iter < pVerticesCount; iter++)
         {
             if (verticesAsFloats[iter] > 0 && verticesAsFloats[iter] < 0.0001) verticesAsFloats[iter] = 0;
             if (verticesAsFloats[iter] < 0 && verticesAsFloats[iter] > -0.0001) verticesAsFloats[iter] = 0;
         }
-        
+
         ObjectArray<int> indicesarr = new ObjectArray<int>(indices);
         ObjectArray<float> vertices = new ObjectArray<float>(verticesAsFloats);
         DumpRaw(indicesarr,vertices,pIndicesCount,pVerticesCount);
-        DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world;
+        DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
         IndexedMesh mesh = new IndexedMesh();
         mesh.m_indexType = PHY_ScalarType.PHY_INTEGER;
         mesh.m_numTriangles = pIndicesCount/3;
@@ -1320,7 +1811,7 @@ private sealed class BulletConstraintXNA : BulletConstraint
         mesh.m_vertexStride = 3;
         mesh.m_vertexType = PHY_ScalarType.PHY_FLOAT;
         mesh.m_triangleIndexStride = 3;
-        
+
         TriangleIndexVertexArray tribuilder = new TriangleIndexVertexArray();
         tribuilder.AddIndexedMesh(mesh, PHY_ScalarType.PHY_INTEGER);
         BvhTriangleMeshShape meshShape = new BvhTriangleMeshShape(tribuilder, true,true);
@@ -1331,7 +1822,7 @@ private sealed class BulletConstraintXNA : BulletConstraint
     }
     public static void DumpRaw(ObjectArray<int>indices, ObjectArray<float> vertices, int pIndicesCount,int pVerticesCount )
     {
-        
+
         String fileName = "objTest3.raw";
         String completePath = System.IO.Path.Combine(Util.configDir(), fileName);
         StreamWriter sw = new StreamWriter(completePath);
@@ -1357,7 +1848,7 @@ private sealed class BulletConstraintXNA : BulletConstraint
             string s = vertices[indices[i * 3]].ToString("0.0000");
             s += " " + vertices[indices[i * 3 + 1]].ToString("0.0000");
             s += " " + vertices[indices[i * 3 + 2]].ToString("0.0000");
-        
+
             sw.Write(s + "\n");
         }
 
@@ -1379,7 +1870,7 @@ private sealed class BulletConstraintXNA : BulletConstraint
         mesh.m_vertexStride = 3;
         mesh.m_vertexType = PHY_ScalarType.PHY_FLOAT;
         mesh.m_triangleIndexStride = 3;
-        
+
         TriangleIndexVertexArray tribuilder = new TriangleIndexVertexArray();
         tribuilder.AddIndexedMesh(mesh, PHY_ScalarType.PHY_INTEGER);
 
@@ -1410,7 +1901,7 @@ private sealed class BulletConstraintXNA : BulletConstraint
             sw.Close();
     }
 
-    public override BulletShape CreateTerrainShape(uint id, Vector3 size, float minHeight, float maxHeight, float[] heightMap, 
+    public override BulletShape CreateTerrainShape(uint id, Vector3 size, float minHeight, float maxHeight, float[] heightMap,
                                                                 float scaleFactor, float collisionMargin)
     {
         const int upAxis = 2;
@@ -1426,7 +1917,7 @@ private sealed class BulletConstraintXNA : BulletConstraint
 
     public override bool TranslationalLimitMotor(BulletConstraint pConstraint, float ponOff, float targetVelocity, float maxMotorForce)
     {
-        TypedConstraint tconstrain = ((BulletConstraintXNA)pConstraint).constrain;
+        TypedConstraint tconstrain = (pConstraint as BulletConstraintXNA).constrain;
         bool onOff = ponOff != 0;
         bool ret = false;
 
@@ -1452,64 +1943,73 @@ private sealed class BulletConstraintXNA : BulletConstraint
         /* TODO */
         updatedEntityCount = 0;
         collidersCount = 0;
-        return 1;
+
+
+        int ret = PhysicsStep2(world,timeStep,maxSubSteps,fixedTimeStep,out updatedEntityCount,out world.physicsScene.m_updateArray, out collidersCount, out world.physicsScene.m_collisionArray);
+
+        return ret;
     }
 
-    private int PhysicsStep2(BulletWorld pWorld, float timeStep, int m_maxSubSteps, float m_fixedTimeStep, 
-                    out int updatedEntityCount, out List<BulletXNA.EntityProperties> updatedEntities, 
-                    out int collidersCount, out List<BulletXNA.CollisionDesc>colliders)
+    private int PhysicsStep2(BulletWorld pWorld, float timeStep, int m_maxSubSteps, float m_fixedTimeStep,
+                    out int updatedEntityCount, out EntityProperties[] updatedEntities,
+                    out int collidersCount, out CollisionDesc[] colliders)
     {
         int epic = PhysicsStepint(pWorld, timeStep, m_maxSubSteps, m_fixedTimeStep, out updatedEntityCount, out updatedEntities,
-                                out collidersCount, out colliders);
+                                out collidersCount, out colliders, m_maxCollisions, m_maxUpdatesPerFrame);
         return epic;
     }
 
-    private static int PhysicsStepint(BulletWorld pWorld,float timeStep, int m_maxSubSteps, float m_fixedTimeStep, out int updatedEntityCount, out List<BulletXNA.EntityProperties> updatedEntities, out int collidersCount, out List<BulletXNA.CollisionDesc> colliders)
+    private int PhysicsStepint(BulletWorld pWorld,float timeStep, int m_maxSubSteps, float m_fixedTimeStep, out int updatedEntityCount,
+        out  EntityProperties[] updatedEntities, out int collidersCount, out CollisionDesc[] colliders, int maxCollisions, int maxUpdates)
     {
         int numSimSteps = 0;
-      
+        Array.Clear(UpdatedObjects, 0, UpdatedObjects.Length);
+        Array.Clear(UpdatedCollisions, 0, UpdatedCollisions.Length);
+        LastEntityProperty=0;
+
+
 
-        //if (updatedEntities is null)
-        //    updatedEntities = new List<BulletXNA.EntityProperties>();
 
-        //if (colliders is null)
-        //    colliders = new List<BulletXNA.CollisionDesc>();
-        
+
+
+        LastCollisionDesc=0;
+
+        updatedEntityCount = 0;
+        collidersCount = 0;
+
 
         if (pWorld is BulletWorldXNA)
         {
-            DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world;
+            DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
 
+            world.LastCollisionDesc = 0;
+            world.LastEntityProperty = 0;
             numSimSteps = world.StepSimulation(timeStep, m_maxSubSteps, m_fixedTimeStep);
             int updates = 0;
 
-            updatedEntityCount = world.UpdatedObjects.Count;
-            updatedEntities = new List<BulletXNA.EntityProperties>(world.UpdatedObjects);
-            updatedEntityCount = updatedEntities.Count;
-            world.UpdatedObjects.Clear();
-            
+            PersistentManifold contactManifold;
+            CollisionObject objA;
+            CollisionObject objB;
+            ManifoldPoint manifoldPoint;
+            PairCachingGhostObject pairCachingGhostObject;
 
-            collidersCount = world.UpdatedCollisions.Count;
-            colliders = new List<BulletXNA.CollisionDesc>(world.UpdatedCollisions);
-
-            world.UpdatedCollisions.Clear();
             m_collisionsThisFrame = 0;
             int numManifolds = world.GetDispatcher().GetNumManifolds();
             for (int j = 0; j < numManifolds; j++)
             {
-                PersistentManifold contactManifold = world.GetDispatcher().GetManifoldByIndexInternal(j);
+                contactManifold = world.GetDispatcher().GetManifoldByIndexInternal(j);
                 int numContacts = contactManifold.GetNumContacts();
                 if (numContacts == 0)
                     continue;
 
-                CollisionObject objA = contactManifold.GetBody0() as CollisionObject;
-                CollisionObject objB = contactManifold.GetBody1() as CollisionObject;
+                objA = contactManifold.GetBody0() as CollisionObject;
+                objB = contactManifold.GetBody1() as CollisionObject;
 
-                ManifoldPoint manifoldPoint = contactManifold.GetContactPoint(0);
-                IndexedVector3 contactPoint = manifoldPoint.GetPositionWorldOnB();
-                IndexedVector3 contactNormal = -manifoldPoint.m_normalWorldOnB; // make relative to A
+                manifoldPoint = contactManifold.GetContactPoint(0);
+                //IndexedVector3 contactPoint = manifoldPoint.GetPositionWorldOnB();
+               // IndexedVector3 contactNormal = -manifoldPoint.m_normalWorldOnB; // make relative to A
 
-                RecordCollision(world, objA, objB, contactPoint, contactNormal);
+                RecordCollision(this, objA, objB, manifoldPoint.GetPositionWorldOnB(), -manifoldPoint.m_normalWorldOnB, manifoldPoint.GetDistance());
                 m_collisionsThisFrame ++;
                 if (m_collisionsThisFrame >= 9999999)
                     break;
@@ -1517,23 +2017,91 @@ private sealed class BulletConstraintXNA : BulletConstraint
 
             }
 
+            foreach (GhostObject ghostObject in specialCollisionObjects.Values)
+            {
+                pairCachingGhostObject = ghostObject as PairCachingGhostObject;
+                if (pairCachingGhostObject != null)
+                {
+                    RecordGhostCollisions(pairCachingGhostObject);
+                }
+
+            }
+
+
+            updatedEntityCount = LastEntityProperty;
+            updatedEntities = UpdatedObjects;
+
+            collidersCount = LastCollisionDesc;
+            colliders = UpdatedCollisions;
+
 
         }
         else
         {
             //if (updatedEntities is null)
-            updatedEntities = new List<BulletXNA.EntityProperties>();
-            updatedEntityCount = 0;
-            //if (colliders is null)
-            colliders = new List<BulletXNA.CollisionDesc>();
-            collidersCount = 0;
+            //updatedEntities = new List<BulletXNA.EntityProperties>();
+            //updatedEntityCount = 0;
+
+
+            //collidersCount = 0;
+
+            updatedEntities = new EntityProperties[0];
+
+
+            colliders = new CollisionDesc[0];
+
         }
         return numSimSteps;
     }
+    public void RecordGhostCollisions(PairCachingGhostObject obj)
+    {
+        IOverlappingPairCache cache = obj.GetOverlappingPairCache();
+        ObjectArray<BroadphasePair> pairs = cache.GetOverlappingPairArray();
+
+        DiscreteDynamicsWorld world = (PhysicsScene.World as BulletWorldXNA).world;
+        PersistentManifoldArray manifoldArray = new PersistentManifoldArray();
+        BroadphasePair collisionPair;
+        PersistentManifold contactManifold;
+
+        CollisionObject objA;
+        CollisionObject objB;
+
+        ManifoldPoint pt;
 
-    private static void RecordCollision(CollisionWorld world, CollisionObject objA, CollisionObject objB, IndexedVector3 contact, IndexedVector3 norm)
+        int numPairs = pairs.Count;
+
+        for (int i = 0; i < numPairs; i++)
+        {
+            manifoldArray.Clear();
+            if (LastCollisionDesc < UpdatedCollisions.Length)
+                break;
+            collisionPair = world.GetPairCache().FindPair(pairs[i].m_pProxy0, pairs[i].m_pProxy1);
+            if (collisionPair == null)
+                continue;
+
+            collisionPair.m_algorithm.GetAllContactManifolds(manifoldArray);
+            for (int j = 0; j < manifoldArray.Count; j++)
+            {
+                contactManifold = manifoldArray[j];
+                int numContacts = contactManifold.GetNumContacts();
+                objA = contactManifold.GetBody0() as CollisionObject;
+                objB = contactManifold.GetBody1() as CollisionObject;
+                for (int p = 0; p < numContacts; p++)
+                {
+                    pt = contactManifold.GetContactPoint(p);
+                    if (pt.GetDistance() < 0.0f)
+                    {
+                        RecordCollision(this, objA, objB, pt.GetPositionWorldOnA(), -pt.m_normalWorldOnB,pt.GetDistance());
+                        break;
+                    }
+                }
+            }
+        }
+
+    }
+    private static void RecordCollision(BSAPIXNA world, CollisionObject objA, CollisionObject objB, IndexedVector3 contact, IndexedVector3 norm, float penetration)
     {
-       
+
         IndexedVector3 contactNormal = norm;
         if ((objA.GetCollisionFlags() & BulletXNA.BulletCollision.CollisionFlags.BS_WANTS_COLLISIONS) == 0 &&
             (objB.GetCollisionFlags() & BulletXNA.BulletCollision.CollisionFlags.BS_WANTS_COLLISIONS) == 0)
@@ -1550,31 +2118,34 @@ private sealed class BulletConstraintXNA : BulletConstraint
             contactNormal = -contactNormal;
         }
 
-        ulong collisionID = ((ulong) idA << 32) | idB;
+        //ulong collisionID = ((ulong) idA << 32) | idB;
 
-        BulletXNA.CollisionDesc cDesc = new BulletXNA.CollisionDesc()
+        CollisionDesc cDesc = new CollisionDesc()
                                             {
                                                 aID = idA,
                                                 bID = idB,
-                                                point = contact,
-                                                normal = contactNormal
+                                                point = new Vector3(contact.X,contact.Y,contact.Z),
+                                                normal = new Vector3(contactNormal.X,contactNormal.Y,contactNormal.Z),
+                                                penetration = penetration
+
                                             };
-        world.UpdatedCollisions.Add(cDesc);
+        if (world.LastCollisionDesc < world.UpdatedCollisions.Length)
+            world.UpdatedCollisions[world.LastCollisionDesc++] = (cDesc);
         m_collisionsThisFrame++;
 
 
     }
-    private static EntityProperties GetDebugProperties(BulletWorld pWorld, BulletBody pBody)
+    private static EntityProperties GetDebugProperties(BulletWorld pWorld, BulletBody pCollisionObject)
     {
         EntityProperties ent = new EntityProperties();
-        DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world;
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
-        IndexedMatrix transform = body.GetWorldTransform();
-        IndexedVector3 LinearVelocity = body.GetInterpolationLinearVelocity();
-        IndexedVector3 AngularVelocity = body.GetInterpolationAngularVelocity();
+        DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
+        IndexedMatrix transform = collisionObject.GetWorldTransform();
+        IndexedVector3 LinearVelocity = collisionObject.GetInterpolationLinearVelocity();
+        IndexedVector3 AngularVelocity = collisionObject.GetInterpolationAngularVelocity();
         IndexedQuaternion rotation = transform.GetRotation();
         ent.Acceleration = Vector3.Zero;
-        ent.ID = (uint)body.GetUserPointer();
+        ent.ID = (uint)collisionObject.GetUserPointer();
         ent.Position = new Vector3(transform._origin.X,transform._origin.Y,transform._origin.Z);
         ent.Rotation = new Quaternion(rotation.X,rotation.Y,rotation.Z,rotation.W);
         ent.Velocity = new Vector3(LinearVelocity.X, LinearVelocity.Y, LinearVelocity.Z);
@@ -1582,28 +2153,29 @@ private sealed class BulletConstraintXNA : BulletConstraint
         return ent;
     }
 
-    public override bool UpdateParameter(BulletWorld world, uint localID, String parm, float value) { /* TODO */ return false; }
+    public override bool UpdateParameter(BulletWorld world, uint localID, String parm, float value) { /* TODO */
+        return false; }
 
     public override Vector3 GetLocalScaling(BulletShape pShape)
     {
-        CollisionShape shape = ((BulletShapeXNA)pShape).shape;
+        CollisionShape shape = (pShape as BulletShapeXNA).shape;
         IndexedVector3 scale = shape.GetLocalScaling();
         return new Vector3(scale.X,scale.Y,scale.Z);
     }
 
     public bool RayCastGround(BulletWorld pWorld, Vector3 _RayOrigin, float pRayHeight, BulletBody NotMe)
     {
-        DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world;
+        DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
         if (world != null)
         {
             if (NotMe is BulletBodyXNA && NotMe.HasPhysicalBody)
             {
-                CollisionObject AvoidBody = ((BulletBodyXNA)NotMe).body;
-                
+                CollisionObject AvoidBody = (NotMe as BulletBodyXNA).body;
+
                 IndexedVector3 rOrigin = new IndexedVector3(_RayOrigin.X, _RayOrigin.Y, _RayOrigin.Z);
                 IndexedVector3 rEnd = new IndexedVector3(_RayOrigin.X, _RayOrigin.Y, _RayOrigin.Z - pRayHeight);
                 using (
-                    ClosestNotMeRayResultCallback rayCallback = 
+                    ClosestNotMeRayResultCallback rayCallback =
                                             new ClosestNotMeRayResultCallback(rOrigin, rEnd, AvoidBody)
                     )
                 {
@@ -1619,4 +2191,130 @@ private sealed class BulletConstraintXNA : BulletConstraint
         return false;
     }
 }
+
+
+
+
+    public class SimMotionState : DefaultMotionState
+    {
+        public RigidBody Rigidbody;
+        public Vector3 ZeroVect;
+
+        private IndexedMatrix m_xform;
+
+        private EntityProperties m_properties;
+        private EntityProperties m_lastProperties;
+        private BSAPIXNA m_world;
+
+        const float POSITION_TOLERANCE = 0.05f;
+        const float VELOCITY_TOLERANCE = 0.001f;
+        const float ROTATION_TOLERANCE = 0.01f;
+        const float ANGULARVELOCITY_TOLERANCE = 0.01f;
+
+        public SimMotionState(BSAPIXNA pWorld, uint id, IndexedMatrix starTransform, object frameUpdates)
+        {
+            IndexedQuaternion OrientationQuaterion = starTransform.GetRotation();
+            m_properties = new EntityProperties()
+                               {
+                                   ID = id,
+                                   Position = new Vector3(starTransform._origin.X, starTransform._origin.Y,starTransform._origin.Z),
+                                   Rotation = new Quaternion(OrientationQuaterion.X,OrientationQuaterion.Y,OrientationQuaterion.Z,OrientationQuaterion.W)
+                               };
+            m_lastProperties = new EntityProperties()
+            {
+                ID = id,
+                Position = new Vector3(starTransform._origin.X, starTransform._origin.Y, starTransform._origin.Z),
+                Rotation = new Quaternion(OrientationQuaterion.X, OrientationQuaterion.Y, OrientationQuaterion.Z, OrientationQuaterion.W)
+            };
+            m_world = pWorld;
+            m_xform = starTransform;
+        }
+
+        public override void GetWorldTransform(out IndexedMatrix worldTrans)
+        {
+            worldTrans = m_xform;
+        }
+
+        public override void SetWorldTransform(IndexedMatrix worldTrans)
+        {
+            SetWorldTransform(ref worldTrans);
+        }
+
+        public override void SetWorldTransform(ref IndexedMatrix worldTrans)
+        {
+            SetWorldTransform(ref worldTrans, false);
+        }
+        public void SetWorldTransform(ref IndexedMatrix worldTrans, bool force)
+        {
+            m_xform = worldTrans;
+            // Put the new transform into m_properties
+            IndexedQuaternion OrientationQuaternion = m_xform.GetRotation();
+            IndexedVector3 LinearVelocityVector = Rigidbody.GetLinearVelocity();
+            IndexedVector3 AngularVelocityVector = Rigidbody.GetAngularVelocity();
+            m_properties.Position = new Vector3(m_xform._origin.X, m_xform._origin.Y, m_xform._origin.Z);
+            m_properties.Rotation = new Quaternion(OrientationQuaternion.X, OrientationQuaternion.Y,
+                                                   OrientationQuaternion.Z, OrientationQuaternion.W);
+            // A problem with stock Bullet is that we don't get an event when an object is deactivated.
+            // This means that the last non-zero values for linear and angular velocity
+            // are left in the viewer who does dead reconning and the objects look like
+            // they float off.
+            // BulletSim ships with a patch to Bullet which creates such an event.
+            m_properties.Velocity = new Vector3(LinearVelocityVector.X, LinearVelocityVector.Y, LinearVelocityVector.Z);
+            m_properties.RotationalVelocity = new Vector3(AngularVelocityVector.X, AngularVelocityVector.Y, AngularVelocityVector.Z);
+
+            if (force
+
+                || !AlmostEqual(ref m_lastProperties.Position, ref m_properties.Position, POSITION_TOLERANCE)
+                || !AlmostEqual(ref m_properties.Rotation, ref m_lastProperties.Rotation, ROTATION_TOLERANCE)
+                // If the Velocity and AngularVelocity are zero, most likely the object has
+                //    been deactivated. If they both are zero and they have become zero recently,
+                //    make sure a property update is sent so the zeros make it to the viewer.
+                || ((m_properties.Velocity == ZeroVect && m_properties.RotationalVelocity == ZeroVect)
+                    &&
+                    (m_properties.Velocity != m_lastProperties.Velocity ||
+                     m_properties.RotationalVelocity != m_lastProperties.RotationalVelocity))
+                //      If Velocity and AngularVelocity are non-zero but have changed, send an update.
+                || !AlmostEqual(ref m_properties.Velocity, ref m_lastProperties.Velocity, VELOCITY_TOLERANCE)
+                ||
+                !AlmostEqual(ref m_properties.RotationalVelocity, ref m_lastProperties.RotationalVelocity,
+                             ANGULARVELOCITY_TOLERANCE)
+                )
+
+
+            {
+                // Add this update to the list of updates for this frame.
+                m_lastProperties = m_properties;
+                if (m_world.LastEntityProperty < m_world.UpdatedObjects.Length)
+                    m_world.UpdatedObjects[m_world.LastEntityProperty++]=(m_properties);
+
+                //(*m_updatesThisFrame)[m_properties.ID] = &m_properties;
+            }
+
+
+
+
+        }
+        public override void SetRigidBody(RigidBody body)
+        {
+            Rigidbody = body;
+        }
+        internal static bool AlmostEqual(ref Vector3 v1, ref Vector3 v2, float nEpsilon)
+        {
+            return
+            (((v1.X - nEpsilon) < v2.X) && (v2.X < (v1.X + nEpsilon))) &&
+            (((v1.Y - nEpsilon) < v2.Y) && (v2.Y < (v1.Y + nEpsilon))) &&
+            (((v1.Z - nEpsilon) < v2.Z) && (v2.Z < (v1.Z + nEpsilon)));
+        }
+
+        internal static bool AlmostEqual(ref Quaternion v1, ref Quaternion v2, float nEpsilon)
+        {
+            return
+            (((v1.X - nEpsilon) < v2.X) && (v2.X < (v1.X + nEpsilon))) &&
+            (((v1.Y - nEpsilon) < v2.Y) && (v2.Y < (v1.Y + nEpsilon))) &&
+            (((v1.Z - nEpsilon) < v2.Z) && (v2.Z < (v1.Z + nEpsilon))) &&
+            (((v1.W - nEpsilon) < v2.W) && (v2.W < (v1.W + nEpsilon)));
+        }
+
+    }
 }
+
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSActorAvatarMove.cs b/OpenSim/Region/Physics/BulletSPlugin/BSActorAvatarMove.cs
new file mode 100755
index 0000000..ac8c30c
--- /dev/null
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSActorAvatarMove.cs
@@ -0,0 +1,351 @@
+/*
+ * 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.Linq;
+using System.Text;
+
+using OpenSim.Region.Physics.Manager;
+
+using OMV = OpenMetaverse;
+
+namespace OpenSim.Region.Physics.BulletSPlugin
+{
+public class BSActorAvatarMove : BSActor
+{
+    BSVMotor m_velocityMotor;
+
+    // Set to true if we think we're going up stairs.
+    //    This state is remembered because collisions will turn on and off as we go up stairs.
+    int m_walkingUpStairs;
+    float m_lastStepUp;
+
+    public BSActorAvatarMove(BSScene physicsScene, BSPhysObject pObj, string actorName)
+        : base(physicsScene, pObj, actorName)
+    {
+        m_velocityMotor = null;
+        m_walkingUpStairs = 0;
+        m_physicsScene.DetailLog("{0},BSActorAvatarMove,constructor", m_controllingPrim.LocalID);
+    }
+
+    // BSActor.isActive
+    public override bool isActive
+    {
+        get { return Enabled && m_controllingPrim.IsPhysicallyActive; }
+    }
+
+    // Release any connections and resources used by the actor.
+    // BSActor.Dispose()
+    public override void Dispose()
+    {
+        Enabled = false;
+    }
+
+    // Called when physical parameters (properties set in Bullet) need to be re-applied.
+    // Called at taint-time.
+    // BSActor.Refresh()
+    public override void Refresh()
+    {
+        m_physicsScene.DetailLog("{0},BSActorAvatarMove,refresh", m_controllingPrim.LocalID);
+
+        // If the object is physically active, add the hoverer prestep action
+        if (isActive)
+        {
+            ActivateAvatarMove();
+        }
+        else
+        {
+            DeactivateAvatarMove();
+        }
+    }
+
+    // The object's physical representation is being rebuilt so pick up any physical dependencies (constraints, ...).
+    //     Register a prestep action to restore physical requirements before the next simulation step.
+    // Called at taint-time.
+    // BSActor.RemoveDependencies()
+    public override void RemoveDependencies()
+    {
+        // Nothing to do for the hoverer since it is all software at pre-step action time.
+    }
+
+    // Usually called when target velocity changes to set the current velocity and the target
+    //     into the movement motor.
+    public void SetVelocityAndTarget(OMV.Vector3 vel, OMV.Vector3 targ, bool inTaintTime)
+    {
+        m_physicsScene.TaintedObject(inTaintTime, "BSActorAvatarMove.setVelocityAndTarget", delegate()
+        {
+            if (m_velocityMotor != null)
+            {
+                m_velocityMotor.Reset();
+                m_velocityMotor.SetTarget(targ);
+                m_velocityMotor.SetCurrent(vel);
+                m_velocityMotor.Enabled = true;
+            }
+        });
+    }
+
+    // If a hover motor has not been created, create one and start the hovering.
+    private void ActivateAvatarMove()
+    {
+        if (m_velocityMotor == null)
+        {
+            // Infinite decay and timescale values so motor only changes current to target values.
+            m_velocityMotor = new BSVMotor("BSCharacter.Velocity",
+                                                0.2f,                       // time scale
+                                                BSMotor.Infinite,           // decay time scale
+                                                1f                          // efficiency
+            );
+            // _velocityMotor.PhysicsScene = PhysicsScene; // DEBUG DEBUG so motor will output detail log messages.
+            SetVelocityAndTarget(m_controllingPrim.RawVelocity, m_controllingPrim.TargetVelocity, true /* inTaintTime */);
+
+            m_physicsScene.BeforeStep += Mover;
+
+            m_walkingUpStairs = 0;
+        }
+    }
+
+    private void DeactivateAvatarMove()
+    {
+        if (m_velocityMotor != null)
+        {
+            m_physicsScene.BeforeStep -= Mover;
+            m_velocityMotor = null;
+        }
+    }
+
+    // Called just before the simulation step. Update the vertical position for hoverness.
+    private void Mover(float timeStep)
+    {
+        // Don't do movement while the object is selected.
+        if (!isActive)
+            return;
+
+        // TODO: Decide if the step parameters should be changed depending on the avatar's
+        //     state (flying, colliding, ...). There is code in ODE to do this.
+
+        // COMMENTARY: when the user is making the avatar walk, except for falling, the velocity
+        //   specified for the avatar is the one that should be used. For falling, if the avatar
+        //   is not flying and is not colliding then it is presumed to be falling and the Z
+        //   component is not fooled with (thus allowing gravity to do its thing).
+        // When the avatar is standing, though, the user has specified a velocity of zero and
+        //   the avatar should be standing. But if the avatar is pushed by something in the world
+        //   (raising elevator platform, moving vehicle, ...) the avatar should be allowed to
+        //   move. Thus, the velocity cannot be forced to zero. The problem is that small velocity
+        //   errors can creap in and the avatar will slowly float off in some direction.
+        // So, the problem is that, when an avatar is standing, we cannot tell creaping error
+        //   from real pushing.
+        // The code below uses whether the collider is static or moving to decide whether to zero motion.
+
+        m_velocityMotor.Step(timeStep);
+        m_controllingPrim.IsStationary = false;
+
+        // If we're not supposed to be moving, make sure things are zero.
+        if (m_velocityMotor.ErrorIsZero() && m_velocityMotor.TargetValue == OMV.Vector3.Zero)
+        {
+            // The avatar shouldn't be moving
+            m_velocityMotor.Zero();
+
+            if (m_controllingPrim.IsColliding)
+            {
+                // If we are colliding with a stationary object, presume we're standing and don't move around
+                if (!m_controllingPrim.ColliderIsMoving)
+                {
+                    m_physicsScene.DetailLog("{0},BSCharacter.MoveMotor,collidingWithStationary,zeroingMotion", m_controllingPrim.LocalID);
+                    m_controllingPrim.IsStationary = true;
+                    m_controllingPrim.ZeroMotion(true /* inTaintTime */);
+                }
+
+                // Standing has more friction on the ground
+                if (m_controllingPrim.Friction != BSParam.AvatarStandingFriction)
+                {
+                    m_controllingPrim.Friction = BSParam.AvatarStandingFriction;
+                    m_physicsScene.PE.SetFriction(m_controllingPrim.PhysBody, m_controllingPrim.Friction);
+                }
+            }
+            else
+            {
+                if (m_controllingPrim.Flying)
+                {
+                    // Flying and not collising and velocity nearly zero.
+                    m_controllingPrim.ZeroMotion(true /* inTaintTime */);
+                }
+            }
+
+            m_physicsScene.DetailLog("{0},BSCharacter.MoveMotor,taint,stopping,target={1},colliding={2}",
+                            m_controllingPrim.LocalID, m_velocityMotor.TargetValue, m_controllingPrim.IsColliding);
+        }
+        else
+        {
+            // Supposed to be moving.
+            OMV.Vector3 stepVelocity = m_velocityMotor.CurrentValue;
+
+            if (m_controllingPrim.Friction != BSParam.AvatarFriction)
+            {
+                // Probably starting up walking. Set friction to moving friction.
+                m_controllingPrim.Friction = BSParam.AvatarFriction;
+                m_physicsScene.PE.SetFriction(m_controllingPrim.PhysBody, m_controllingPrim.Friction);
+            }
+
+            // If falling, we keep the world's downward vector no matter what the other axis specify.
+            // The check for RawVelocity.Z < 0 makes jumping work (temporary upward force).
+            if (!m_controllingPrim.Flying && !m_controllingPrim.IsColliding)
+            {
+                if (m_controllingPrim.RawVelocity.Z < 0)
+                    stepVelocity.Z = m_controllingPrim.RawVelocity.Z;
+                // DetailLog("{0},BSCharacter.MoveMotor,taint,overrideStepZWithWorldZ,stepVel={1}", LocalID, stepVelocity);
+            }
+
+            // 'stepVelocity' is now the speed we'd like the avatar to move in. Turn that into an instantanous force.
+            OMV.Vector3 moveForce = (stepVelocity - m_controllingPrim.RawVelocity) * m_controllingPrim.Mass;
+
+            // Add special movement force to allow avatars to walk up stepped surfaces.
+            moveForce += WalkUpStairs();
+
+            m_physicsScene.DetailLog("{0},BSCharacter.MoveMotor,move,stepVel={1},vel={2},mass={3},moveForce={4}",
+                            m_controllingPrim.LocalID, stepVelocity, m_controllingPrim.RawVelocity, m_controllingPrim.Mass, moveForce);
+            m_physicsScene.PE.ApplyCentralImpulse(m_controllingPrim.PhysBody, moveForce);
+        }
+    }
+
+    // Decide if the character is colliding with a low object and compute a force to pop the
+    //    avatar up so it can walk up and over the low objects.
+    private OMV.Vector3 WalkUpStairs()
+    {
+        OMV.Vector3 ret = OMV.Vector3.Zero;
+
+        m_physicsScene.DetailLog("{0},BSCharacter.WalkUpStairs,IsColliding={1},flying={2},targSpeed={3},collisions={4},avHeight={5}",
+                        m_controllingPrim.LocalID, m_controllingPrim.IsColliding, m_controllingPrim.Flying,
+                        m_controllingPrim.TargetVelocitySpeed, m_controllingPrim.CollisionsLastTick.Count, m_controllingPrim.Size.Z);
+        // This test is done if moving forward, not flying and is colliding with something.
+        // Check for stairs climbing if colliding, not flying and moving forward
+        if ( m_controllingPrim.IsColliding
+                    && !m_controllingPrim.Flying
+                    && m_controllingPrim.TargetVelocitySpeed > 0.1f )
+        {
+            // The range near the character's feet where we will consider stairs
+            // float nearFeetHeightMin = m_controllingPrim.RawPosition.Z - (m_controllingPrim.Size.Z / 2f) + 0.05f;
+            // Note: there is a problem with the computation of the capsule height. Thus RawPosition is off
+            //    from the height. Revisit size and this computation when height is scaled properly.
+            float nearFeetHeightMin = m_controllingPrim.RawPosition.Z - (m_controllingPrim.Size.Z / 2f) - 0.05f;
+            float nearFeetHeightMax = nearFeetHeightMin + BSParam.AvatarStepHeight;
+
+            // Look for a collision point that is near the character's feet and is oriented the same as the charactor is.
+            // Find the highest 'good' collision.
+            OMV.Vector3 highestTouchPosition = OMV.Vector3.Zero;
+            foreach (KeyValuePair<uint, ContactPoint> kvp in m_controllingPrim.CollisionsLastTick.m_objCollisionList)
+            {
+                // Don't care about collisions with the terrain
+                if (kvp.Key > m_physicsScene.TerrainManager.HighestTerrainID)
+                {
+                    OMV.Vector3 touchPosition = kvp.Value.Position;
+                    m_physicsScene.DetailLog("{0},BSCharacter.WalkUpStairs,min={1},max={2},touch={3}",
+                                    m_controllingPrim.LocalID, nearFeetHeightMin, nearFeetHeightMax, touchPosition);
+                    if (touchPosition.Z >= nearFeetHeightMin && touchPosition.Z <= nearFeetHeightMax)
+                    {
+                        // This contact is within the 'near the feet' range.
+                        // The normal should be our contact point to the object so it is pointing away
+                        //    thus the difference between our facing orientation and the normal should be small.
+                        OMV.Vector3 directionFacing = OMV.Vector3.UnitX * m_controllingPrim.RawOrientation;
+                        OMV.Vector3 touchNormal = OMV.Vector3.Normalize(kvp.Value.SurfaceNormal);
+                        float diff = Math.Abs(OMV.Vector3.Distance(directionFacing, touchNormal));
+                        if (diff < BSParam.AvatarStepApproachFactor)
+                        {
+                            if (highestTouchPosition.Z < touchPosition.Z)
+                                highestTouchPosition = touchPosition;
+                        }
+                    }
+                }
+            }
+            m_walkingUpStairs = 0;
+            // If there is a good step sensing, move the avatar over the step.
+            if (highestTouchPosition != OMV.Vector3.Zero)
+            {
+                // Remember that we are going up stairs. This is needed because collisions
+                //    will stop when we move up so this smoothes out that effect.
+                m_walkingUpStairs = BSParam.AvatarStepSmoothingSteps;
+
+                m_lastStepUp = highestTouchPosition.Z - nearFeetHeightMin;
+                ret = ComputeStairCorrection(m_lastStepUp);
+                m_physicsScene.DetailLog("{0},BSCharacter.WalkUpStairs,touchPos={1},nearFeetMin={2},ret={3}",
+                        m_controllingPrim.LocalID, highestTouchPosition, nearFeetHeightMin, ret);
+            }
+        }
+        else
+        {
+            // If we used to be going up stairs but are not now, smooth the case where collision goes away while
+            //    we are bouncing up the stairs.
+            if (m_walkingUpStairs > 0)
+            {
+                m_walkingUpStairs--;
+                ret = ComputeStairCorrection(m_lastStepUp);
+            }
+        }
+
+        return ret;
+    }
+
+    private OMV.Vector3 ComputeStairCorrection(float stepUp)
+    {
+        OMV.Vector3 ret = OMV.Vector3.Zero;
+        OMV.Vector3 displacement = OMV.Vector3.Zero;
+
+        if (stepUp > 0f)
+        {
+            // Found the stairs contact point. Push up a little to raise the character.
+            if (BSParam.AvatarStepForceFactor > 0f)
+            {
+                float upForce = stepUp * m_controllingPrim.Mass * BSParam.AvatarStepForceFactor;
+                ret = new OMV.Vector3(0f, 0f, upForce);
+            }
+
+            // Also move the avatar up for the new height
+            if (BSParam.AvatarStepUpCorrectionFactor > 0f)
+            {
+                // Move the avatar up related to the height of the collision
+                displacement = new OMV.Vector3(0f, 0f, stepUp * BSParam.AvatarStepUpCorrectionFactor);
+                m_controllingPrim.ForcePosition = m_controllingPrim.RawPosition + displacement;
+            }
+            else
+            {
+                if (BSParam.AvatarStepUpCorrectionFactor < 0f)
+                {
+                    // Move the avatar up about the specified step height
+                    displacement = new OMV.Vector3(0f, 0f, BSParam.AvatarStepHeight);
+                    m_controllingPrim.ForcePosition = m_controllingPrim.RawPosition + displacement;
+                }
+            }
+            m_physicsScene.DetailLog("{0},BSCharacter.WalkUpStairs.ComputeStairCorrection,disp={1},force={2}",
+                                        m_controllingPrim.LocalID, displacement, ret);
+
+        }
+        return ret;
+    }
+}
+}
+
+
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSActorHover.cs b/OpenSim/Region/Physics/BulletSPlugin/BSActorHover.cs
new file mode 100755
index 0000000..8a79809
--- /dev/null
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSActorHover.cs
@@ -0,0 +1,173 @@
+/*
+ * 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.Linq;
+using System.Text;
+
+using OpenSim.Region.Physics.Manager;
+
+using OMV = OpenMetaverse;
+
+namespace OpenSim.Region.Physics.BulletSPlugin
+{
+public class BSActorHover : BSActor
+{
+    private BSFMotor m_hoverMotor;
+
+    public BSActorHover(BSScene physicsScene, BSPhysObject pObj, string actorName)
+        : base(physicsScene, pObj, actorName)
+    {
+        m_hoverMotor = null;
+        m_physicsScene.DetailLog("{0},BSActorHover,constructor", m_controllingPrim.LocalID);
+    }
+
+    // BSActor.isActive
+    public override bool isActive
+    {
+        get { return Enabled; }
+    }
+
+    // Release any connections and resources used by the actor.
+    // BSActor.Dispose()
+    public override void Dispose()
+    {
+        Enabled = false;
+    }
+
+    // Called when physical parameters (properties set in Bullet) need to be re-applied.
+    // Called at taint-time.
+    // BSActor.Refresh()
+    public override void Refresh()
+    {
+        m_physicsScene.DetailLog("{0},BSActorHover,refresh", m_controllingPrim.LocalID);
+
+        // If not active any more, turn me off
+        if (!m_controllingPrim.HoverActive)
+        {
+            SetEnabled(false);
+        }
+
+        // If the object is physically active, add the hoverer prestep action
+        if (isActive)
+        {
+            ActivateHover();
+        }
+        else
+        {
+            DeactivateHover();
+        }
+    }
+
+    // The object's physical representation is being rebuilt so pick up any physical dependencies (constraints, ...).
+    //     Register a prestep action to restore physical requirements before the next simulation step.
+    // Called at taint-time.
+    // BSActor.RemoveDependencies()
+    public override void RemoveDependencies()
+    {
+        // Nothing to do for the hoverer since it is all software at pre-step action time.
+    }
+
+    // If a hover motor has not been created, create one and start the hovering.
+    private void ActivateHover()
+    {
+        if (m_hoverMotor == null)
+        {
+            // Turning the target on
+            m_hoverMotor = new BSFMotor("BSActorHover",
+                                        m_controllingPrim.HoverTau,               // timeScale
+                                        BSMotor.Infinite,           // decay time scale
+                                        1f                          // efficiency
+            );
+            m_hoverMotor.SetTarget(ComputeCurrentHoverHeight());
+            m_hoverMotor.SetCurrent(m_controllingPrim.RawPosition.Z);
+            m_hoverMotor.PhysicsScene = m_physicsScene; // DEBUG DEBUG so motor will output detail log messages.
+
+            m_physicsScene.BeforeStep += Hoverer;
+        }
+    }
+
+    private void DeactivateHover()
+    {
+        if (m_hoverMotor != null)
+        {
+            m_physicsScene.BeforeStep -= Hoverer;
+            m_hoverMotor = null;
+        }
+    }
+
+    // Called just before the simulation step. Update the vertical position for hoverness.
+    private void Hoverer(float timeStep)
+    {
+        // Don't do hovering while the object is selected.
+        if (!isActive)
+            return;
+
+        m_hoverMotor.SetCurrent(m_controllingPrim.RawPosition.Z);
+        m_hoverMotor.SetTarget(ComputeCurrentHoverHeight());
+        float targetHeight = m_hoverMotor.Step(timeStep);
+
+        // 'targetHeight' is where we'd like the Z of the prim to be at this moment.
+        // Compute the amount of force to push us there.
+        float moveForce = (targetHeight - m_controllingPrim.RawPosition.Z) * m_controllingPrim.RawMass;
+        // Undo anything the object thinks it's doing at the moment
+        moveForce = -m_controllingPrim.RawVelocity.Z * m_controllingPrim.Mass;
+
+        m_physicsScene.PE.ApplyCentralImpulse(m_controllingPrim.PhysBody, new OMV.Vector3(0f, 0f, moveForce));
+        m_physicsScene.DetailLog("{0},BSPrim.Hover,move,targHt={1},moveForce={2},mass={3}",
+                        m_controllingPrim.LocalID, targetHeight, moveForce, m_controllingPrim.RawMass);
+    }
+
+    // Based on current position, determine what we should be hovering at now.
+    // Must recompute often. What if we walked offa cliff>
+    private float ComputeCurrentHoverHeight()
+    {
+        float ret = m_controllingPrim.HoverHeight;
+        float groundHeight = m_physicsScene.TerrainManager.GetTerrainHeightAtXYZ(m_controllingPrim.RawPosition);
+
+        switch (m_controllingPrim.HoverType)
+        {
+            case PIDHoverType.Ground:
+                ret = groundHeight + m_controllingPrim.HoverHeight;
+                break;
+            case PIDHoverType.GroundAndWater:
+                float waterHeight = m_physicsScene.TerrainManager.GetWaterLevelAtXYZ(m_controllingPrim.RawPosition);
+                if (groundHeight > waterHeight)
+                {
+                    ret = groundHeight + m_controllingPrim.HoverHeight;
+                }
+                else
+                {
+                    ret = waterHeight + m_controllingPrim.HoverHeight;
+                }
+                break;
+        }
+        return ret;
+    }
+}
+}
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSActorLockAxis.cs b/OpenSim/Region/Physics/BulletSPlugin/BSActorLockAxis.cs
new file mode 100755
index 0000000..8b0fdeb
--- /dev/null
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSActorLockAxis.cs
@@ -0,0 +1,187 @@
+/*
+ * 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.Linq;
+using System.Text;
+
+using OMV = OpenMetaverse;
+
+namespace OpenSim.Region.Physics.BulletSPlugin
+{
+public class BSActorLockAxis : BSActor
+{
+    BSConstraint LockAxisConstraint = null;
+
+    public BSActorLockAxis(BSScene physicsScene, BSPhysObject pObj, string actorName)
+        : base(physicsScene, pObj, actorName)
+    {
+        m_physicsScene.DetailLog("{0},BSActorLockAxis,constructor", m_controllingPrim.LocalID);
+        LockAxisConstraint = null;
+    }
+
+    // BSActor.isActive
+    public override bool isActive
+    {
+        get { return Enabled && m_controllingPrim.IsPhysicallyActive; }
+    }
+
+    // Release any connections and resources used by the actor.
+    // BSActor.Dispose()
+    public override void Dispose()
+    {
+        RemoveAxisLockConstraint();
+    }
+
+    // Called when physical parameters (properties set in Bullet) need to be re-applied.
+    // Called at taint-time.
+    // BSActor.Refresh()
+    public override void Refresh()
+    {
+        m_physicsScene.DetailLog("{0},BSActorLockAxis,refresh,lockedAxis={1},enabled={2},pActive={3}",
+                                    m_controllingPrim.LocalID, m_controllingPrim.LockedAngularAxis, Enabled, m_controllingPrim.IsPhysicallyActive);
+        // If all the axis are free, we don't need to exist
+        if (m_controllingPrim.LockedAngularAxis == m_controllingPrim.LockedAxisFree)
+        {
+            Enabled = false;
+        }
+
+        // If the object is physically active, add the axis locking constraint
+        if (isActive)
+        {
+            AddAxisLockConstraint();
+        }
+        else
+        {
+            RemoveAxisLockConstraint();
+        }
+    }
+
+    // The object's physical representation is being rebuilt so pick up any physical dependencies (constraints, ...).
+    //     Register a prestep action to restore physical requirements before the next simulation step.
+    // Called at taint-time.
+    // BSActor.RemoveDependencies()
+    public override void RemoveDependencies()
+    {
+        if (LockAxisConstraint != null)
+        {
+            // If a constraint is set up, remove it from the physical scene
+            RemoveAxisLockConstraint();
+            // Schedule a call before the next simulation step to restore the constraint.
+            m_physicsScene.PostTaintObject("BSActorLockAxis:" + ActorName, m_controllingPrim.LocalID, delegate()
+            {
+                Refresh();
+            });
+        }
+    }
+
+    private void AddAxisLockConstraint()
+    {
+        if (LockAxisConstraint == null)
+        {
+            // Lock that axis by creating a 6DOF constraint that has one end in the world and
+            //    the other in the object.
+            // http://www.bulletphysics.org/Bullet/phpBB3/viewtopic.php?p=20817
+            // http://www.bulletphysics.org/Bullet/phpBB3/viewtopic.php?p=26380
+
+            // Remove any existing axis constraint (just to be sure)
+            RemoveAxisLockConstraint();
+
+            BSConstraint6Dof axisConstrainer = new BSConstraint6Dof(m_physicsScene.World, m_controllingPrim.PhysBody,
+                                OMV.Vector3.Zero, OMV.Quaternion.Identity,
+                                false /* useLinearReferenceFrameB */, true /* disableCollisionsBetweenLinkedBodies */);
+            LockAxisConstraint = axisConstrainer;
+            m_physicsScene.Constraints.AddConstraint(LockAxisConstraint);
+
+            // The constraint is tied to the world and oriented to the prim.
+
+            // Free to move linearly in the region
+            OMV.Vector3 linearLow = OMV.Vector3.Zero;
+            OMV.Vector3 linearHigh = m_physicsScene.TerrainManager.DefaultRegionSize;
+            if (m_controllingPrim.LockedLinearAxis.X != BSPhysObject.FreeAxis)
+            {
+                linearLow.X = m_controllingPrim.RawPosition.X;
+                linearHigh.X = m_controllingPrim.RawPosition.X;
+            }
+            if (m_controllingPrim.LockedLinearAxis.Y != BSPhysObject.FreeAxis)
+            {
+                linearLow.Y = m_controllingPrim.RawPosition.Y;
+                linearHigh.Y = m_controllingPrim.RawPosition.Y;
+            }
+            if (m_controllingPrim.LockedLinearAxis.Z != BSPhysObject.FreeAxis)
+            {
+                linearLow.Z = m_controllingPrim.RawPosition.Z;
+                linearHigh.Z = m_controllingPrim.RawPosition.Z;
+            }
+            axisConstrainer.SetLinearLimits(linearLow, linearHigh);
+
+            // Angular with some axis locked
+            float fPI = (float)Math.PI;
+            OMV.Vector3 angularLow = new OMV.Vector3(-fPI, -fPI, -fPI);
+            OMV.Vector3 angularHigh = new OMV.Vector3(fPI, fPI, fPI);
+            if (m_controllingPrim.LockedAngularAxis.X != BSPhysObject.FreeAxis)
+            {
+                angularLow.X = 0f;
+                angularHigh.X = 0f;
+            }
+            if (m_controllingPrim.LockedAngularAxis.Y != BSPhysObject.FreeAxis)
+            {
+                angularLow.Y = 0f;
+                angularHigh.Y = 0f;
+            }
+            if (m_controllingPrim.LockedAngularAxis.Z != BSPhysObject.FreeAxis)
+            {
+                angularLow.Z = 0f;
+                angularHigh.Z = 0f;
+            }
+            if (!axisConstrainer.SetAngularLimits(angularLow, angularHigh))
+            {
+                m_physicsScene.DetailLog("{0},BSActorLockAxis.AddAxisLockConstraint,failedSetAngularLimits", m_controllingPrim.LocalID);
+            }
+
+            m_physicsScene.DetailLog("{0},BSActorLockAxis.AddAxisLockConstraint,create,linLow={1},linHi={2},angLow={3},angHi={4}",
+                                        m_controllingPrim.LocalID, linearLow, linearHigh, angularLow, angularHigh);
+
+            // Constants from one of the posts mentioned above and used in Bullet's ConstraintDemo.
+            axisConstrainer.TranslationalLimitMotor(true /* enable */, 5.0f, 0.1f);
+
+            axisConstrainer.RecomputeConstraintVariables(m_controllingPrim.RawMass);
+        }
+    }
+
+    private void RemoveAxisLockConstraint()
+    {
+        if (LockAxisConstraint != null)
+        {
+            m_physicsScene.Constraints.RemoveAndDestroyConstraint(LockAxisConstraint);
+            LockAxisConstraint = null;
+            m_physicsScene.DetailLog("{0},BSActorLockAxis.RemoveAxisLockConstraint,destroyingConstraint", m_controllingPrim.LocalID);
+        }
+    }
+}
+}
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSActorMoveToTarget.cs b/OpenSim/Region/Physics/BulletSPlugin/BSActorMoveToTarget.cs
new file mode 100755
index 0000000..75ff24e
--- /dev/null
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSActorMoveToTarget.cs
@@ -0,0 +1,157 @@
+/*
+ * 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.Linq;
+using System.Text;
+
+using OpenSim.Region.Physics.Manager;
+
+using OMV = OpenMetaverse;
+
+namespace OpenSim.Region.Physics.BulletSPlugin
+{
+public class BSActorMoveToTarget : BSActor
+{
+    private BSVMotor m_targetMotor;
+
+    public BSActorMoveToTarget(BSScene physicsScene, BSPhysObject pObj, string actorName)
+        : base(physicsScene, pObj, actorName)
+    {
+        m_targetMotor = null;
+        m_physicsScene.DetailLog("{0},BSActorMoveToTarget,constructor", m_controllingPrim.LocalID);
+    }
+
+    // BSActor.isActive
+    public override bool isActive
+    {
+        get { return Enabled; }
+    }
+
+    // Release any connections and resources used by the actor.
+    // BSActor.Dispose()
+    public override void Dispose()
+    {
+        Enabled = false;
+    }
+
+    // Called when physical parameters (properties set in Bullet) need to be re-applied.
+    // Called at taint-time.
+    // BSActor.Refresh()
+    public override void Refresh()
+    {
+        m_physicsScene.DetailLog("{0},BSActorMoveToTarget,refresh,enabled={1},active={2},target={3},tau={4}",
+            m_controllingPrim.LocalID, Enabled, m_controllingPrim.MoveToTargetActive,
+            m_controllingPrim.MoveToTargetTarget, m_controllingPrim.MoveToTargetTau );
+
+        // If not active any more...
+        if (!m_controllingPrim.MoveToTargetActive)
+        {
+            Enabled = false;
+        }
+
+        if (isActive)
+        {
+            ActivateMoveToTarget();
+        }
+        else
+        {
+            DeactivateMoveToTarget();
+        }
+    }
+
+    // The object's physical representation is being rebuilt so pick up any physical dependencies (constraints, ...).
+    //     Register a prestep action to restore physical requirements before the next simulation step.
+    // Called at taint-time.
+    // BSActor.RemoveDependencies()
+    public override void RemoveDependencies()
+    {
+        // Nothing to do for the moveToTarget since it is all software at pre-step action time.
+    }
+
+    // If a hover motor has not been created, create one and start the hovering.
+    private void ActivateMoveToTarget()
+    {
+        if (m_targetMotor == null)
+        {
+            // We're taking over after this.
+            m_controllingPrim.ZeroMotion(true);
+
+            m_targetMotor = new BSVMotor("BSActorMoveToTargget.Activate",
+                                        m_controllingPrim.MoveToTargetTau,                    // timeScale
+                                        BSMotor.Infinite,           // decay time scale
+                                        1f                          // efficiency
+            );
+            m_targetMotor.PhysicsScene = m_physicsScene; // DEBUG DEBUG so motor will output detail log messages.
+            m_targetMotor.SetTarget(m_controllingPrim.MoveToTargetTarget);
+            m_targetMotor.SetCurrent(m_controllingPrim.RawPosition);
+
+            m_physicsScene.BeforeStep += Mover;
+        }
+    }
+
+    private void DeactivateMoveToTarget()
+    {
+        if (m_targetMotor != null)
+        {
+            m_physicsScene.BeforeStep -= Mover;
+            m_targetMotor = null;
+        }
+    }
+
+    // Called just before the simulation step. Update the vertical position for hoverness.
+    private void Mover(float timeStep)
+    {
+        // Don't do hovering while the object is selected.
+        if (!isActive)
+            return;
+
+        OMV.Vector3 origPosition = m_controllingPrim.RawPosition;     // DEBUG DEBUG (for printout below)
+
+        // 'movePosition' is where we'd like the prim to be at this moment.
+        OMV.Vector3 movePosition = m_controllingPrim.RawPosition + m_targetMotor.Step(timeStep);
+
+        // If we are very close to our target, turn off the movement motor.
+        if (m_targetMotor.ErrorIsZero())
+        {
+            m_physicsScene.DetailLog("{0},BSActorMoveToTarget.Mover,zeroMovement,movePos={1},pos={2},mass={3}",
+                                    m_controllingPrim.LocalID, movePosition, m_controllingPrim.RawPosition, m_controllingPrim.Mass);
+            m_controllingPrim.ForcePosition = m_targetMotor.TargetValue;
+            // Setting the position does not cause the physics engine to generate a property update. Force it.
+            m_physicsScene.PE.PushUpdate(m_controllingPrim.PhysBody);
+        }
+        else
+        {
+            m_controllingPrim.ForcePosition = movePosition;
+            // Setting the position does not cause the physics engine to generate a property update. Force it.
+            m_physicsScene.PE.PushUpdate(m_controllingPrim.PhysBody);
+        }
+        m_physicsScene.DetailLog("{0},BSActorMoveToTarget.Mover,move,fromPos={1},movePos={2}", m_controllingPrim.LocalID, origPosition, movePosition);
+    }
+}
+}
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSActorSetForce.cs b/OpenSim/Region/Physics/BulletSPlugin/BSActorSetForce.cs
new file mode 100755
index 0000000..96fa0b6
--- /dev/null
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSActorSetForce.cs
@@ -0,0 +1,137 @@
+/*
+ * 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.Linq;
+using System.Text;
+
+using OpenSim.Region.Physics.Manager;
+
+using OMV = OpenMetaverse;
+
+namespace OpenSim.Region.Physics.BulletSPlugin
+{
+public class BSActorSetForce : BSActor
+{
+    BSFMotor m_forceMotor;
+
+    public BSActorSetForce(BSScene physicsScene, BSPhysObject pObj, string actorName)
+        : base(physicsScene, pObj, actorName)
+    {
+        m_forceMotor = null;
+        m_physicsScene.DetailLog("{0},BSActorSetForce,constructor", m_controllingPrim.LocalID);
+    }
+
+    // BSActor.isActive
+    public override bool isActive
+    {
+        get { return Enabled && m_controllingPrim.IsPhysicallyActive; }
+    }
+
+    // Release any connections and resources used by the actor.
+    // BSActor.Dispose()
+    public override void Dispose()
+    {
+        Enabled = false;
+    }
+
+    // Called when physical parameters (properties set in Bullet) need to be re-applied.
+    // Called at taint-time.
+    // BSActor.Refresh()
+    public override void Refresh()
+    {
+        m_physicsScene.DetailLog("{0},BSActorSetForce,refresh", m_controllingPrim.LocalID);
+
+        // If not active any more, get rid of me (shouldn't ever happen, but just to be safe)
+        if (m_controllingPrim.RawForce == OMV.Vector3.Zero)
+        {
+            m_physicsScene.DetailLog("{0},BSActorSetForce,refresh,notSetForce,removing={1}", m_controllingPrim.LocalID, ActorName);
+            Enabled = false;
+            return;
+        }
+
+        // If the object is physically active, add the hoverer prestep action
+        if (isActive)
+        {
+            ActivateSetForce();
+        }
+        else
+        {
+            DeactivateSetForce();
+        }
+    }
+
+    // The object's physical representation is being rebuilt so pick up any physical dependencies (constraints, ...).
+    //     Register a prestep action to restore physical requirements before the next simulation step.
+    // Called at taint-time.
+    // BSActor.RemoveDependencies()
+    public override void RemoveDependencies()
+    {
+        // Nothing to do for the hoverer since it is all software at pre-step action time.
+    }
+
+    // If a hover motor has not been created, create one and start the hovering.
+    private void ActivateSetForce()
+    {
+        if (m_forceMotor == null)
+        {
+            // A fake motor that might be used someday
+            m_forceMotor = new BSFMotor("setForce", 1f, 1f, 1f);
+
+            m_physicsScene.BeforeStep += Mover;
+        }
+    }
+
+    private void DeactivateSetForce()
+    {
+        if (m_forceMotor != null)
+        {
+            m_physicsScene.BeforeStep -= Mover;
+            m_forceMotor = null;
+        }
+    }
+
+    // Called just before the simulation step. Update the vertical position for hoverness.
+    private void Mover(float timeStep)
+    {
+        // Don't do force while the object is selected.
+        if (!isActive)
+            return;
+
+        m_physicsScene.DetailLog("{0},BSActorSetForce,preStep,force={1}", m_controllingPrim.LocalID, m_controllingPrim.RawForce);
+        if (m_controllingPrim.PhysBody.HasPhysicalBody)
+        {
+            m_physicsScene.PE.ApplyCentralForce(m_controllingPrim.PhysBody, m_controllingPrim.RawForce);
+            m_controllingPrim.ActivateIfPhysical(false);
+        }
+
+        // TODO:
+    }
+}
+}
+
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSActorSetTorque.cs b/OpenSim/Region/Physics/BulletSPlugin/BSActorSetTorque.cs
new file mode 100755
index 0000000..65098e1
--- /dev/null
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSActorSetTorque.cs
@@ -0,0 +1,138 @@
+/*
+ * 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.Linq;
+using System.Text;
+
+using OpenSim.Region.Physics.Manager;
+
+using OMV = OpenMetaverse;
+
+namespace OpenSim.Region.Physics.BulletSPlugin
+{
+public class BSActorSetTorque : BSActor
+{
+    BSFMotor m_torqueMotor;
+
+    public BSActorSetTorque(BSScene physicsScene, BSPhysObject pObj, string actorName)
+        : base(physicsScene, pObj, actorName)
+    {
+        m_torqueMotor = null;
+        m_physicsScene.DetailLog("{0},BSActorSetTorque,constructor", m_controllingPrim.LocalID);
+    }
+
+    // BSActor.isActive
+    public override bool isActive
+    {
+        get { return Enabled && m_controllingPrim.IsPhysicallyActive; }
+    }
+
+    // Release any connections and resources used by the actor.
+    // BSActor.Dispose()
+    public override void Dispose()
+    {
+        Enabled = false;
+    }
+
+    // Called when physical parameters (properties set in Bullet) need to be re-applied.
+    // Called at taint-time.
+    // BSActor.Refresh()
+    public override void Refresh()
+    {
+        m_physicsScene.DetailLog("{0},BSActorSetTorque,refresh,torque={1}", m_controllingPrim.LocalID, m_controllingPrim.RawTorque);
+
+        // If not active any more, get rid of me (shouldn't ever happen, but just to be safe)
+        if (m_controllingPrim.RawTorque == OMV.Vector3.Zero)
+        {
+            m_physicsScene.DetailLog("{0},BSActorSetTorque,refresh,notSetTorque,disabling={1}", m_controllingPrim.LocalID, ActorName);
+            Enabled = false;
+            return;
+        }
+
+        // If the object is physically active, add the hoverer prestep action
+        if (isActive)
+        {
+            ActivateSetTorque();
+        }
+        else
+        {
+            DeactivateSetTorque();
+        }
+    }
+
+    // The object's physical representation is being rebuilt so pick up any physical dependencies (constraints, ...).
+    //     Register a prestep action to restore physical requirements before the next simulation step.
+    // Called at taint-time.
+    // BSActor.RemoveDependencies()
+    public override void RemoveDependencies()
+    {
+        // Nothing to do for the hoverer since it is all software at pre-step action time.
+    }
+
+    // If a hover motor has not been created, create one and start the hovering.
+    private void ActivateSetTorque()
+    {
+        if (m_torqueMotor == null)
+        {
+            // A fake motor that might be used someday
+            m_torqueMotor = new BSFMotor("setTorque", 1f, 1f, 1f);
+
+            m_physicsScene.BeforeStep += Mover;
+        }
+    }
+
+    private void DeactivateSetTorque()
+    {
+        if (m_torqueMotor != null)
+        {
+            m_physicsScene.BeforeStep -= Mover;
+            m_torqueMotor = null;
+        }
+    }
+
+    // Called just before the simulation step. Update the vertical position for hoverness.
+    private void Mover(float timeStep)
+    {
+        // Don't do force while the object is selected.
+        if (!isActive)
+            return;
+
+        m_physicsScene.DetailLog("{0},BSActorSetTorque,preStep,force={1}", m_controllingPrim.LocalID, m_controllingPrim.RawTorque);
+        if (m_controllingPrim.PhysBody.HasPhysicalBody)
+        {
+            m_controllingPrim.AddAngularForce(m_controllingPrim.RawTorque, false, true);
+            m_controllingPrim.ActivateIfPhysical(false);
+        }
+
+        // TODO:
+    }
+}
+}
+
+
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSActors.cs b/OpenSim/Region/Physics/BulletSPlugin/BSActors.cs
new file mode 100755
index 0000000..fff63e4
--- /dev/null
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSActors.cs
@@ -0,0 +1,160 @@
+/*
+ * 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;
+
+namespace OpenSim.Region.Physics.BulletSPlugin
+{
+public class BSActorCollection
+{
+    private BSScene m_physicsScene { get; set; }
+    private Dictionary<string, BSActor> m_actors;
+
+    public BSActorCollection(BSScene physicsScene)
+    {
+        m_physicsScene = physicsScene;
+        m_actors = new Dictionary<string, BSActor>();
+    }
+    public void Add(string name, BSActor actor)
+    {
+        lock (m_actors)
+        {
+            if (!m_actors.ContainsKey(name))
+            {
+                m_actors[name] = actor;
+            }
+        }
+    }
+    public bool RemoveAndRelease(string name)
+    {
+        bool ret = false;
+        lock (m_actors)
+        {
+            if (m_actors.ContainsKey(name))
+            {
+                BSActor beingRemoved = m_actors[name];
+                m_actors.Remove(name);
+                beingRemoved.Dispose();
+                ret = true;
+            }
+        }
+        return ret;
+    }
+    public void Clear()
+    {
+        lock (m_actors)
+        {
+            Release();
+            m_actors.Clear();
+        }
+    }
+    public void Dispose()
+    {
+        Clear();
+    }
+    public bool HasActor(string name)
+    {
+        return m_actors.ContainsKey(name);
+    }
+    public bool TryGetActor(string actorName, out BSActor theActor)
+    {
+        return m_actors.TryGetValue(actorName, out theActor);
+    }
+    public void ForEachActor(Action<BSActor> act)
+    {
+        lock (m_actors)
+        {
+            foreach (KeyValuePair<string, BSActor> kvp in m_actors)
+                act(kvp.Value);
+        }
+    }
+
+    public void Enable(bool enabl)
+    {
+        ForEachActor(a => a.SetEnabled(enabl));
+    }
+    public void Release()
+    {
+        ForEachActor(a => a.Dispose());
+    }
+    public void Refresh()
+    {
+        ForEachActor(a => a.Refresh());
+    }
+    public void RemoveDependencies()
+    {
+        ForEachActor(a => a.RemoveDependencies());
+    }
+}
+
+// =============================================================================
+/// <summary>
+/// Each physical object can have 'actors' who are pushing the object around.
+/// This can be used for hover, locking axis, making vehicles, etc.
+/// Each physical object can have multiple actors acting on it.
+///
+/// An actor usually registers itself with physics scene events (pre-step action)
+/// and modifies the parameters on the host physical object.
+/// </summary>
+public abstract class BSActor
+{
+    protected BSScene m_physicsScene { get; private set; }
+    protected BSPhysObject m_controllingPrim { get; private set; }
+    public virtual bool Enabled { get; set; }
+    public string ActorName { get; private set; }
+
+    public BSActor(BSScene physicsScene, BSPhysObject pObj, string actorName)
+    {
+        m_physicsScene = physicsScene;
+        m_controllingPrim = pObj;
+        ActorName = actorName;
+        Enabled = true;
+    }
+
+    // Return 'true' if activily updating the prim
+    public virtual bool isActive
+    {
+        get { return Enabled; }
+    }
+
+    // Turn the actor on an off. Only used by ActorCollection to set all enabled/disabled.
+    // Anyone else should assign true/false to 'Enabled'.
+    public void SetEnabled(bool setEnabled)
+    {
+        Enabled = setEnabled;
+    }
+    // Release any connections and resources used by the actor.
+    public abstract void Dispose();
+    // Called when physical parameters (properties set in Bullet) need to be re-applied.
+    public abstract void Refresh();
+    // The object's physical representation is being rebuilt so pick up any physical dependencies (constraints, ...).
+    //     Register a prestep action to restore physical requirements before the next simulation step.
+    public abstract void RemoveDependencies();
+
+}
+}
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSApiTemplate.cs b/OpenSim/Region/Physics/BulletSPlugin/BSApiTemplate.cs
index 8ad78ca..3378c93 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSApiTemplate.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSApiTemplate.cs
@@ -6,7 +6,7 @@
  * 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
+ *     * Redistributions in binary form must reproduce the above copyright
  *       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
@@ -70,6 +70,7 @@ public enum BSPhysicsShapeType
         SHAPE_COMPOUND  = 22,
         SHAPE_HEIGHTMAP = 23,
     SHAPE_AVATAR    = 24,
+    SHAPE_CONVEXHULL= 25,
 };
 
 // The native shapes have predefined shape hash keys
@@ -87,7 +88,7 @@ public enum FixedShapeKey : ulong
 [StructLayout(LayoutKind.Sequential)]
 public struct ShapeData
 {
-    public uint ID;
+    public UInt32 ID;
     public BSPhysicsShapeType Type;
     public Vector3 Position;
     public Quaternion Rotation;
@@ -111,7 +112,7 @@ public struct ShapeData
 [StructLayout(LayoutKind.Sequential)]
 public struct SweepHit
 {
-    public uint ID;
+    public UInt32 ID;
     public float Fraction;
     public Vector3 Normal;
     public Vector3 Point;
@@ -119,27 +120,47 @@ public struct SweepHit
 [StructLayout(LayoutKind.Sequential)]
 public struct RaycastHit
 {
-    public uint ID;
+    public UInt32 ID;
     public float Fraction;
     public Vector3 Normal;
 }
 [StructLayout(LayoutKind.Sequential)]
 public struct CollisionDesc
 {
-    public uint aID;
-    public uint bID;
+    public UInt32 aID;
+    public UInt32 bID;
     public Vector3 point;
     public Vector3 normal;
+    public float penetration;
 }
 [StructLayout(LayoutKind.Sequential)]
 public struct EntityProperties
 {
-    public uint ID;
+    public UInt32 ID;
     public Vector3 Position;
     public Quaternion Rotation;
     public Vector3 Velocity;
     public Vector3 Acceleration;
     public Vector3 RotationalVelocity;
+
+    public override string ToString()
+    {
+        StringBuilder buff = new StringBuilder();
+        buff.Append("<i=");
+        buff.Append(ID.ToString());
+        buff.Append(",p=");
+        buff.Append(Position.ToString());
+        buff.Append(",r=");
+        buff.Append(Rotation.ToString());
+        buff.Append(",v=");
+        buff.Append(Velocity.ToString());
+        buff.Append(",a=");
+        buff.Append(Acceleration.ToString());
+        buff.Append(",rv=");
+        buff.Append(RotationalVelocity.ToString());
+        buff.Append(">");
+        return buff.ToString();
+    }
 }
 
 // Format of this structure must match the definition in the C++ code
@@ -154,32 +175,6 @@ public struct ConfigurationParameters
     public float collisionMargin;
     public float gravity;
 
-    public float XlinearDamping;
-    public float XangularDamping;
-    public float XdeactivationTime;
-    public float XlinearSleepingThreshold;
-    public float XangularSleepingThreshold;
-        public float XccdMotionThreshold;
-        public float XccdSweptSphereRadius;
-    public float XcontactProcessingThreshold;
-
-    public float XterrainImplementation;
-    public float XterrainFriction;
-    public float XterrainHitFraction;
-    public float XterrainRestitution;
-    public float XterrainCollisionMargin;
-
-    public float XavatarFriction;
-    public float XavatarStandingFriction;
-    public float XavatarDensity;
-    public float XavatarRestitution;
-    public float XavatarCapsuleWidth;
-    public float XavatarCapsuleDepth;
-    public float XavatarCapsuleHeight;
-        public float XavatarContactProcessingThreshold;
-
-    public float XvehicleAngularDamping;
-
         public float maxPersistantManifoldPoolSize;
         public float maxCollisionAlgorithmPoolSize;
         public float shouldDisableContactPoolDynamicAllocation;
@@ -188,22 +183,30 @@ public struct ConfigurationParameters
         public float shouldSplitSimulationIslands;
         public float shouldEnableFrictionCaching;
         public float numberOfSolverIterations;
+    public float useSingleSidedMeshes;
+        public float globalContactBreakingThreshold;
 
-    public float XlinksetImplementation;
-    public float XlinkConstraintUseFrameOffset;
-    public float XlinkConstraintEnableTransMotor;
-    public float XlinkConstraintTransMotorMaxVel;
-    public float XlinkConstraintTransMotorMaxForce;
-    public float XlinkConstraintERP;
-    public float XlinkConstraintCFM;
-    public float XlinkConstraintSolverIterations;
-
-    public float XphysicsLoggingFrames;
+    public float physicsLoggingFrames;
 
     public const float numericTrue = 1f;
     public const float numericFalse = 0f;
 }
 
+// Parameters passed for the conversion of a mesh to a hull using Bullet's HACD library.
+[StructLayout(LayoutKind.Sequential)]
+public struct HACDParams
+{
+                                            // usual default values
+        public float maxVerticesPerHull;                // 100
+        public float minClusters;                               // 2
+        public float compacityWeight;                   // 0.1
+        public float volumeWeight;                              // 0.0
+        public float concavity;                             // 100
+        public float addExtraDistPoints;                // false
+        public float addNeighboursDistPoints;   // false
+        public float addFacesPoints;                    // false
+        public float shouldAdjustCollisionMargin;       // false
+}
 
 // The states a bullet collision object can have
 public enum ActivationState : uint
@@ -238,9 +241,10 @@ public enum CollisionFlags : uint
     CF_DISABLE_VISUALIZE_OBJECT      = 1 << 5,
     CF_DISABLE_SPU_COLLISION_PROCESS = 1 << 6,
     // Following used by BulletSim to control collisions and updates
-    BS_SUBSCRIBE_COLLISION_EVENTS    = 1 << 10,
-    BS_FLOATS_ON_WATER               = 1 << 11,
-    BS_VEHICLE_COLLISIONS            = 1 << 12,
+    BS_SUBSCRIBE_COLLISION_EVENTS    = 1 << 10, // return collision events from unmanaged to managed
+    BS_FLOATS_ON_WATER               = 1 << 11, // the object should float at water level
+    BS_VEHICLE_COLLISIONS            = 1 << 12, // return collisions for vehicle ground checking
+    BS_RETURN_ROOT_COMPOUND_SHAPE    = 1 << 13, // return the pos/rot of the root shape in a compound shape
     BS_NONE                          = 0,
     BS_ALL                           = 0xFFFFFFFF
 };
@@ -294,7 +298,7 @@ public abstract class BSAPITemplate
 {
 // Returns the name of the underlying Bullet engine
 public abstract string BulletEngineName { get; }
-public abstract string BulletEngineVersion { get; protected set;} 
+public abstract string BulletEngineVersion { get; protected set;}
 
 // Initialization and simulation
 public abstract BulletWorld Initialize(Vector3 maxPosition, ConfigurationParameters parms,
@@ -305,7 +309,7 @@ public abstract BulletWorld Initialize(Vector3 maxPosition, ConfigurationParamet
 public abstract int PhysicsStep(BulletWorld world, float timeStep, int maxSubSteps, float fixedTimeStep,
                         out int updatedEntityCount, out int collidersCount);
 
-public abstract bool UpdateParameter(BulletWorld world, uint localID, String parm, float value);
+public abstract bool UpdateParameter(BulletWorld world, UInt32 localID, String parm, float value);
 
 public abstract void Shutdown(BulletWorld sim);
 
@@ -320,7 +324,13 @@ public abstract BulletShape CreateMeshShape(BulletWorld world,
 public abstract BulletShape CreateHullShape(BulletWorld world,
                 int hullCount, float[] hulls);
 
-public abstract BulletShape BuildHullShapeFromMesh(BulletWorld world, BulletShape meshShape);
+public abstract BulletShape BuildHullShapeFromMesh(BulletWorld world, BulletShape meshShape, HACDParams parms);
+
+public abstract BulletShape BuildConvexHullShapeFromMesh(BulletWorld world, BulletShape meshShape);
+
+public abstract BulletShape CreateConvexHullShape(BulletWorld world,
+                int indicesCount, int[] indices,
+                int verticesCount, float[] vertices );
 
 public abstract BulletShape BuildNativeShape(BulletWorld world, ShapeData shapeData);
 
@@ -342,26 +352,28 @@ public abstract BulletShape RemoveChildShapeFromCompoundShapeIndex(BulletShape c
 
 public abstract void RemoveChildShapeFromCompoundShape(BulletShape cShape, BulletShape removeShape);
 
+public abstract void UpdateChildTransform(BulletShape pShape, int childIndex, Vector3 pos, Quaternion rot, bool shouldRecalculateLocalAabb);
+
 public abstract void RecalculateCompoundShapeLocalAabb(BulletShape cShape);
 
-public abstract BulletShape DuplicateCollisionShape(BulletWorld sim, BulletShape srcShape, uint id);
+public abstract BulletShape DuplicateCollisionShape(BulletWorld sim, BulletShape srcShape, UInt32 id);
 
 public abstract bool DeleteCollisionShape(BulletWorld world, BulletShape shape);
 
 public abstract CollisionObjectTypes GetBodyType(BulletBody obj);
 
-public abstract BulletBody CreateBodyFromShape(BulletWorld sim, BulletShape shape, uint id, Vector3 pos, Quaternion rot);
+public abstract BulletBody CreateBodyFromShape(BulletWorld sim, BulletShape shape, UInt32 id, Vector3 pos, Quaternion rot);
 
-public abstract BulletBody CreateBodyWithDefaultMotionState(BulletShape shape, uint id, Vector3 pos, Quaternion rot);
+public abstract BulletBody CreateBodyWithDefaultMotionState(BulletShape shape, UInt32 id, Vector3 pos, Quaternion rot);
 
-public abstract BulletBody CreateGhostFromShape(BulletWorld sim, BulletShape shape, uint id, Vector3 pos, Quaternion rot);
+public abstract BulletBody CreateGhostFromShape(BulletWorld sim, BulletShape shape, UInt32 id, Vector3 pos, Quaternion rot);
 
 public abstract void DestroyObject(BulletWorld sim, BulletBody obj);
 
 // =====================================================================================
-public abstract BulletShape CreateGroundPlaneShape(uint id, float height, float collisionMargin);
+public abstract BulletShape CreateGroundPlaneShape(UInt32 id, float height, float collisionMargin);
 
-public abstract BulletShape CreateTerrainShape(uint id, Vector3 size, float minHeight, float maxHeight, float[] heightMap, 
+public abstract BulletShape CreateTerrainShape(UInt32 id, Vector3 size, float minHeight, float maxHeight, float[] heightMap,
                                                                 float scaleFactor, float collisionMargin);
 
 // =====================================================================================
@@ -375,11 +387,38 @@ public abstract BulletConstraint Create6DofConstraintToPoint(BulletWorld world,
                     Vector3 joinPoint,
                     bool useLinearReferenceFrameA, bool disableCollisionsBetweenLinkedBodies);
 
+public abstract BulletConstraint Create6DofConstraintFixed(BulletWorld world, BulletBody obj1,
+                    Vector3 frameInBloc, Quaternion frameInBrot,
+                    bool useLinearReferenceFrameB, bool disableCollisionsBetweenLinkedBodies);
+
+public abstract BulletConstraint Create6DofSpringConstraint(BulletWorld world, BulletBody obj1, BulletBody obj2,
+                    Vector3 frame1loc, Quaternion frame1rot,
+                    Vector3 frame2loc, Quaternion frame2rot,
+                    bool useLinearReferenceFrameA, bool disableCollisionsBetweenLinkedBodies);
+
 public abstract BulletConstraint CreateHingeConstraint(BulletWorld world, BulletBody obj1, BulletBody obj2,
                     Vector3 pivotinA, Vector3 pivotinB,
                     Vector3 axisInA, Vector3 axisInB,
                     bool useLinearReferenceFrameA, bool disableCollisionsBetweenLinkedBodies);
 
+public abstract BulletConstraint CreateSliderConstraint(BulletWorld world, BulletBody obj1, BulletBody obj2,
+                    Vector3 frameInAloc, Quaternion frameInArot,
+                    Vector3 frameInBloc, Quaternion frameInBrot,
+                    bool useLinearReferenceFrameA, bool disableCollisionsBetweenLinkedBodies);
+
+public abstract BulletConstraint CreateConeTwistConstraint(BulletWorld world, BulletBody obj1, BulletBody obj2,
+                    Vector3 frameInAloc, Quaternion frameInArot,
+                    Vector3 frameInBloc, Quaternion frameInBrot,
+                    bool disableCollisionsBetweenLinkedBodies);
+
+public abstract BulletConstraint CreateGearConstraint(BulletWorld world, BulletBody obj1, BulletBody obj2,
+                    Vector3 axisInA, Vector3 axisInB,
+                    float ratio, bool disableCollisionsBetweenLinkedBodies);
+
+public abstract BulletConstraint CreatePoint2PointConstraint(BulletWorld world, BulletBody obj1, BulletBody obj2,
+                    Vector3 pivotInA, Vector3 pivotInB,
+                    bool disableCollisionsBetweenLinkedBodies);
+
 public abstract void SetConstraintEnable(BulletConstraint constrain, float numericTrueFalse);
 
 public abstract void SetConstraintNumSolverIterations(BulletConstraint constrain, float iterations);
@@ -607,7 +646,7 @@ public abstract BulletConstraint GetConstraintRef(BulletBody obj, int index);
 
 public abstract int GetNumConstraintRefs(BulletBody obj);
 
-public abstract bool SetCollisionGroupMask(BulletBody body, uint filter, uint mask);
+public abstract bool SetCollisionGroupMask(BulletBody body, UInt32 filter, UInt32 mask);
 
 // =====================================================================================
 // btCollisionShape entries
@@ -646,17 +685,21 @@ public abstract float GetMargin(BulletShape shape);
 
 // =====================================================================================
 // Debugging
-public abstract void DumpRigidBody(BulletWorld sim, BulletBody collisionObject);
+public virtual void DumpRigidBody(BulletWorld sim, BulletBody collisionObject) { }
+
+public virtual void DumpCollisionShape(BulletWorld sim, BulletShape collisionShape) { }
+
+public virtual void DumpConstraint(BulletWorld sim, BulletConstraint constrain) { }
 
-public abstract void DumpCollisionShape(BulletWorld sim, BulletShape collisionShape);
+public virtual void DumpActivationInfo(BulletWorld sim) { }
 
-public abstract void DumpConstraint(BulletWorld sim, BulletConstraint constrain);
+public virtual void DumpAllInfo(BulletWorld sim) { }
 
-public abstract void DumpActivationInfo(BulletWorld sim);
+public virtual void DumpPhysicsStatistics(BulletWorld sim) { }
 
-public abstract void DumpAllInfo(BulletWorld sim);
+public virtual void ResetBroadphasePool(BulletWorld sim) { }
 
-public abstract void DumpPhysicsStatistics(BulletWorld sim);
+public virtual void ResetConstraintSolver(BulletWorld sim) { }
 
 };
 }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
index 103d8fc..542f732 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
@@ -45,11 +45,7 @@ public sealed class BSCharacter : BSPhysObject
     private bool _selected;
     private OMV.Vector3 _position;
     private float _mass;
-    private float _avatarDensity;
     private float _avatarVolume;
-    private OMV.Vector3 _force;
-    private OMV.Vector3 _velocity;
-    private OMV.Vector3 _torque;
     private float _collisionScore;
     private OMV.Vector3 _acceleration;
     private OMV.Quaternion _orientation;
@@ -58,25 +54,17 @@ public sealed class BSCharacter : BSPhysObject
     private bool _flying;
     private bool _setAlwaysRun;
     private bool _throttleUpdates;
-    private bool _isColliding;
-    private bool _collidingObj;
     private bool _floatOnWater;
     private OMV.Vector3 _rotationalVelocity;
     private bool _kinematic;
     private float _buoyancy;
 
-    // The friction and velocity of the avatar is modified depending on whether walking or not.
-    private float _currentFriction;         // the friction currently being used (changed by setVelocity).
-
-    private BSVMotor _velocityMotor;
+    private BSActorAvatarMove m_moveActor;
+    private const string AvatarMoveActorName = "BSCharacter.AvatarMove";
 
     private OMV.Vector3 _PIDTarget;
     private bool _usePID;
     private float _PIDTau;
-    private bool _useHoverPID;
-    private float _PIDHoverHeight;
-    private PIDHoverType _PIDHoverType;
-    private float _PIDHoverTao;
 
     public BSCharacter(uint localID, String avName, BSScene parent_scene, OMV.Vector3 pos, OMV.Vector3 size, bool isFlying)
             : base(parent_scene, localID, avName, "BSCharacter")
@@ -86,10 +74,10 @@ public sealed class BSCharacter : BSPhysObject
 
         _flying = isFlying;
         _orientation = OMV.Quaternion.Identity;
-        _velocity = OMV.Vector3.Zero;
+        RawVelocity = OMV.Vector3.Zero;
         _buoyancy = ComputeBuoyancyFromFlying(isFlying);
-        _currentFriction = BSParam.AvatarStandingFriction;
-        _avatarDensity = BSParam.AvatarDensity;
+        Friction = BSParam.AvatarStandingFriction;
+        Density = BSParam.AvatarDensity / BSParam.DensityScaleFactor;
 
         // Old versions of ScenePresence passed only the height. If width and/or depth are zero,
         //     replace with the default values.
@@ -103,17 +91,22 @@ public sealed class BSCharacter : BSPhysObject
         // set _avatarVolume and _mass based on capsule size, _density and Scale
         ComputeAvatarVolumeAndMass();
 
-        SetupMovementMotor();
+        // The avatar's movement is controlled by this motor that speeds up and slows down
+        //    the avatar seeking to reach the motor's target speed.
+        // This motor runs as a prestep action for the avatar so it will keep the avatar
+        //    standing as well as moving. Destruction of the avatar will destroy the pre-step action.
+        m_moveActor = new BSActorAvatarMove(PhysScene, this, AvatarMoveActorName);
+        PhysicalActors.Add(AvatarMoveActorName, m_moveActor);
 
         DetailLog("{0},BSCharacter.create,call,size={1},scale={2},density={3},volume={4},mass={5}",
-                            LocalID, _size, Scale, _avatarDensity, _avatarVolume, RawMass);
+                            LocalID, _size, Scale, Density, _avatarVolume, RawMass);
 
         // do actual creation in taint time
-        PhysicsScene.TaintedObject("BSCharacter.create", delegate()
+        PhysScene.TaintedObject("BSCharacter.create", delegate()
         {
             DetailLog("{0},BSCharacter.create,taint", LocalID);
             // New body and shape into PhysBody and PhysShape
-            PhysicsScene.Shapes.GetBodyAndShape(true, PhysicsScene.World, this);
+            PhysScene.Shapes.GetBodyAndShape(true, PhysScene.World, this);
 
             SetPhysicalProperties();
         });
@@ -126,114 +119,63 @@ public sealed class BSCharacter : BSPhysObject
         base.Destroy();
 
         DetailLog("{0},BSCharacter.Destroy", LocalID);
-        PhysicsScene.TaintedObject("BSCharacter.destroy", delegate()
+        PhysScene.TaintedObject("BSCharacter.destroy", delegate()
         {
-            PhysicsScene.Shapes.DereferenceBody(PhysBody, true, null);
+            PhysScene.Shapes.DereferenceBody(PhysBody, null /* bodyCallback */);
             PhysBody.Clear();
-            PhysicsScene.Shapes.DereferenceShape(PhysShape, true, null);
-            PhysShape.Clear();
+            PhysShape.Dereference(PhysScene);
+            PhysShape = new BSShapeNull();
         });
     }
 
     private void SetPhysicalProperties()
     {
-        PhysicsScene.PE.RemoveObjectFromWorld(PhysicsScene.World, PhysBody);
+        PhysScene.PE.RemoveObjectFromWorld(PhysScene.World, PhysBody);
 
         ZeroMotion(true);
         ForcePosition = _position;
 
-        // Set the velocity and compute the proper friction
-        _velocityMotor.Reset();
-        _velocityMotor.SetTarget(_velocity);
-        _velocityMotor.SetCurrent(_velocity);
-        ForceVelocity = _velocity;
+        // Set the velocity
+        if (m_moveActor != null)
+            m_moveActor.SetVelocityAndTarget(RawVelocity, RawVelocity, false);
+
+        ForceVelocity = RawVelocity;
 
         // This will enable or disable the flying buoyancy of the avatar.
         // Needs to be reset especially when an avatar is recreated after crossing a region boundry.
         Flying = _flying;
 
-        PhysicsScene.PE.SetRestitution(PhysBody, BSParam.AvatarRestitution);
-        PhysicsScene.PE.SetMargin(PhysShape, PhysicsScene.Params.collisionMargin);
-        PhysicsScene.PE.SetLocalScaling(PhysShape, Scale);
-        PhysicsScene.PE.SetContactProcessingThreshold(PhysBody, BSParam.ContactProcessingThreshold);
+        PhysScene.PE.SetRestitution(PhysBody, BSParam.AvatarRestitution);
+        PhysScene.PE.SetMargin(PhysShape.physShapeInfo, PhysScene.Params.collisionMargin);
+        PhysScene.PE.SetLocalScaling(PhysShape.physShapeInfo, Scale);
+        PhysScene.PE.SetContactProcessingThreshold(PhysBody, BSParam.ContactProcessingThreshold);
         if (BSParam.CcdMotionThreshold > 0f)
         {
-            PhysicsScene.PE.SetCcdMotionThreshold(PhysBody, BSParam.CcdMotionThreshold);
-            PhysicsScene.PE.SetCcdSweptSphereRadius(PhysBody, BSParam.CcdSweptSphereRadius);
+            PhysScene.PE.SetCcdMotionThreshold(PhysBody, BSParam.CcdMotionThreshold);
+            PhysScene.PE.SetCcdSweptSphereRadius(PhysBody, BSParam.CcdSweptSphereRadius);
         }
 
         UpdatePhysicalMassProperties(RawMass, false);
 
         // Make so capsule does not fall over
-        PhysicsScene.PE.SetAngularFactorV(PhysBody, OMV.Vector3.Zero);
+        PhysScene.PE.SetAngularFactorV(PhysBody, OMV.Vector3.Zero);
 
-        PhysicsScene.PE.AddToCollisionFlags(PhysBody, CollisionFlags.CF_CHARACTER_OBJECT);
+        // The avatar mover sets some parameters.
+        PhysicalActors.Refresh();
 
-        PhysicsScene.PE.AddObjectToWorld(PhysicsScene.World, PhysBody);
+        PhysScene.PE.AddToCollisionFlags(PhysBody, CollisionFlags.CF_CHARACTER_OBJECT);
+
+        PhysScene.PE.AddObjectToWorld(PhysScene.World, PhysBody);
 
         // PhysicsScene.PE.ForceActivationState(PhysBody, ActivationState.ACTIVE_TAG);
-        PhysicsScene.PE.ForceActivationState(PhysBody, ActivationState.DISABLE_DEACTIVATION);
-        PhysicsScene.PE.UpdateSingleAabb(PhysicsScene.World, PhysBody);
+        PhysScene.PE.ForceActivationState(PhysBody, ActivationState.DISABLE_DEACTIVATION);
+        PhysScene.PE.UpdateSingleAabb(PhysScene.World, PhysBody);
 
         // Do this after the object has been added to the world
         PhysBody.collisionType = CollisionType.Avatar;
-        PhysBody.ApplyCollisionMask(PhysicsScene);
+        PhysBody.ApplyCollisionMask(PhysScene);
     }
 
-    // The avatar's movement is controlled by this motor that speeds up and slows down
-    //    the avatar seeking to reach the motor's target speed.
-    // This motor runs as a prestep action for the avatar so it will keep the avatar
-    //    standing as well as moving. Destruction of the avatar will destroy the pre-step action.
-    private void SetupMovementMotor()
-    {
-
-        // Someday, use a PID motor for asymmetric speed up and slow down
-        // _velocityMotor = new BSPIDVMotor("BSCharacter.Velocity", 3f, 5f, BSMotor.InfiniteVector, 1f);
-
-        // Infinite decay and timescale values so motor only changes current to target values.
-        _velocityMotor = new BSVMotor("BSCharacter.Velocity", 
-                                            0.2f,                       // time scale
-                                            BSMotor.Infinite,           // decay time scale
-                                            BSMotor.InfiniteVector,     // friction timescale
-                                            1f                          // efficiency
-        );
-        // _velocityMotor.PhysicsScene = PhysicsScene; // DEBUG DEBUG so motor will output detail log messages.
-
-        RegisterPreStepAction("BSCharactor.Movement", LocalID, delegate(float timeStep)
-        {
-            // TODO: Decide if the step parameters should be changed depending on the avatar's
-            //     state (flying, colliding, ...). There is code in ODE to do this.
-
-            OMV.Vector3 stepVelocity = _velocityMotor.Step(timeStep);
-
-            // If falling, we keep the world's downward vector no matter what the other axis specify.
-            if (!Flying && !IsColliding)
-            {
-                stepVelocity.Z = _velocity.Z;
-                // DetailLog("{0},BSCharacter.MoveMotor,taint,overrideStepZWithWorldZ,stepVel={1}", LocalID, stepVelocity);
-            }
-
-            // 'stepVelocity' is now the speed we'd like the avatar to move in. Turn that into an instantanous force.
-            OMV.Vector3 moveForce = (stepVelocity - _velocity) * Mass / PhysicsScene.LastTimeStep;
-
-            /*
-            // If moveForce is very small, zero things so we don't keep sending microscopic updates to the user
-            float moveForceMagnitudeSquared = moveForce.LengthSquared();
-            if (moveForceMagnitudeSquared < 0.0001)
-            {
-                DetailLog("{0},BSCharacter.MoveMotor,zeroMovement,stepVel={1},vel={2},mass={3},magSq={4},moveForce={5}",
-                                        LocalID, stepVelocity, _velocity, Mass, moveForceMagnitudeSquared, moveForce);
-                ForceVelocity = OMV.Vector3.Zero;
-            }
-            else
-            {
-                AddForce(moveForce, false, true);
-            }
-            */
-            // DetailLog("{0},BSCharacter.MoveMotor,move,stepVel={1},vel={2},mass={3},moveForce={4}", LocalID, stepVelocity, _velocity, Mass, moveForce);
-            AddForce(moveForce, false, true);
-        });
-    }
 
     public override void RequestPhysicsterseUpdate()
     {
@@ -259,16 +201,16 @@ public sealed class BSCharacter : BSPhysObject
             Scale = ComputeAvatarScale(_size);
             ComputeAvatarVolumeAndMass();
             DetailLog("{0},BSCharacter.setSize,call,size={1},scale={2},density={3},volume={4},mass={5}",
-                            LocalID, _size, Scale, _avatarDensity, _avatarVolume, RawMass);
+                            LocalID, _size, Scale, Density, _avatarVolume, RawMass);
 
-            PhysicsScene.TaintedObject("BSCharacter.setSize", delegate()
+            PhysScene.TaintedObject("BSCharacter.setSize", delegate()
             {
-                if (PhysBody.HasPhysicalBody && PhysShape.HasPhysicalShape)
+                if (PhysBody.HasPhysicalBody && PhysShape.physShapeInfo.HasPhysicalShape)
                 {
-                    PhysicsScene.PE.SetLocalScaling(PhysShape, Scale);
+                    PhysScene.PE.SetLocalScaling(PhysShape.physShapeInfo, Scale);
                     UpdatePhysicalMassProperties(RawMass, true);
                     // Make sure this change appears as a property update event
-                    PhysicsScene.PE.PushUpdate(PhysBody);
+                    PhysScene.PE.PushUpdate(PhysBody);
                 }
             });
 
@@ -279,11 +221,6 @@ public sealed class BSCharacter : BSPhysObject
     {
         set { BaseShape = value; }
     }
-    // I want the physics engine to make an avatar capsule
-    public override BSPhysicsShapeType PreferredPhysicalShape
-    {
-        get {return BSPhysicsShapeType.SHAPE_CAPSULE; }
-    }
 
     public override bool Grabbed {
         set { _grabbed = value; }
@@ -291,6 +228,10 @@ public sealed class BSCharacter : BSPhysObject
     public override bool Selected {
         set { _selected = value; }
     }
+    public override bool IsSelected
+    {
+        get { return _selected; }
+    }
     public override void CrossingFailure() { return; }
     public override void link(PhysicsActor obj) { return; }
     public override void delink() { return; }
@@ -301,29 +242,29 @@ public sealed class BSCharacter : BSPhysObject
     // Called at taint time!
     public override void ZeroMotion(bool inTaintTime)
     {
-        _velocity = OMV.Vector3.Zero;
+        RawVelocity = OMV.Vector3.Zero;
         _acceleration = OMV.Vector3.Zero;
         _rotationalVelocity = OMV.Vector3.Zero;
 
         // Zero some other properties directly into the physics engine
-        PhysicsScene.TaintedObject(inTaintTime, "BSCharacter.ZeroMotion", delegate()
+        PhysScene.TaintedObject(inTaintTime, "BSCharacter.ZeroMotion", delegate()
         {
             if (PhysBody.HasPhysicalBody)
-                PhysicsScene.PE.ClearAllForces(PhysBody);
+                PhysScene.PE.ClearAllForces(PhysBody);
         });
     }
     public override void ZeroAngularMotion(bool inTaintTime)
     {
         _rotationalVelocity = OMV.Vector3.Zero;
 
-        PhysicsScene.TaintedObject(inTaintTime, "BSCharacter.ZeroMotion", delegate()
+        PhysScene.TaintedObject(inTaintTime, "BSCharacter.ZeroMotion", delegate()
         {
             if (PhysBody.HasPhysicalBody)
             {
-                PhysicsScene.PE.SetInterpolationAngularVelocity(PhysBody, OMV.Vector3.Zero);
-                PhysicsScene.PE.SetAngularVelocity(PhysBody, OMV.Vector3.Zero);
+                PhysScene.PE.SetInterpolationAngularVelocity(PhysBody, OMV.Vector3.Zero);
+                PhysScene.PE.SetAngularVelocity(PhysBody, OMV.Vector3.Zero);
                 // The next also get rid of applied linear force but the linear velocity is untouched.
-                PhysicsScene.PE.ClearForces(PhysBody);
+                PhysScene.PE.ClearForces(PhysBody);
             }
         });
     }
@@ -344,25 +285,26 @@ public sealed class BSCharacter : BSPhysObject
         }
         set {
             _position = value;
-            PositionSanityCheck();
 
-            PhysicsScene.TaintedObject("BSCharacter.setPosition", delegate()
+            PhysScene.TaintedObject("BSCharacter.setPosition", delegate()
             {
                 DetailLog("{0},BSCharacter.SetPosition,taint,pos={1},orient={2}", LocalID, _position, _orientation);
-                if (PhysBody.HasPhysicalBody)
-                    PhysicsScene.PE.SetTranslation(PhysBody, _position, _orientation);
+                PositionSanityCheck();
+                ForcePosition = _position;
             });
         }
     }
     public override OMV.Vector3 ForcePosition {
         get {
-            _position = PhysicsScene.PE.GetPosition(PhysBody);
+            _position = PhysScene.PE.GetPosition(PhysBody);
             return _position;
         }
         set {
             _position = value;
-            PositionSanityCheck();
-            PhysicsScene.PE.SetTranslation(PhysBody, _position, _orientation);
+            if (PhysBody.HasPhysicalBody)
+            {
+                PhysScene.PE.SetTranslation(PhysBody, _position, _orientation);
+            }
         }
     }
 
@@ -375,25 +317,27 @@ public sealed class BSCharacter : BSPhysObject
         bool ret = false;
 
         // TODO: check for out of bounds
-        if (!PhysicsScene.TerrainManager.IsWithinKnownTerrain(_position))
+        if (!PhysScene.TerrainManager.IsWithinKnownTerrain(RawPosition))
         {
             // The character is out of the known/simulated area.
-            // Upper levels of code will handle the transition to other areas so, for
-            //     the time, we just ignore the position.
-            return ret;
+            // Force the avatar position to be within known. ScenePresence will use the position
+            //    plus the velocity to decide if the avatar is moving out of the region.
+            RawPosition = PhysScene.TerrainManager.ClampPositionIntoKnownTerrain(RawPosition);
+            DetailLog("{0},BSCharacter.PositionSanityCheck,notWithinKnownTerrain,clampedPos={1}", LocalID, RawPosition);
+            return true;
         }
 
         // If below the ground, move the avatar up
-        float terrainHeight = PhysicsScene.TerrainManager.GetTerrainHeightAtXYZ(_position);
+        float terrainHeight = PhysScene.TerrainManager.GetTerrainHeightAtXYZ(RawPosition);
         if (Position.Z < terrainHeight)
         {
-            DetailLog("{0},BSCharacter.PositionAdjustUnderGround,call,pos={1},terrain={2}", LocalID, _position, terrainHeight);
-            _position.Z = terrainHeight + 2.0f;
+            DetailLog("{0},BSCharacter.PositionSanityCheck,adjustForUnderGround,pos={1},terrain={2}", LocalID, _position, terrainHeight);
+            _position.Z = terrainHeight + BSParam.AvatarBelowGroundUpCorrectionMeters;
             ret = true;
         }
         if ((CurrentCollisionFlags & CollisionFlags.BS_FLOATS_ON_WATER) != 0)
         {
-            float waterHeight = PhysicsScene.TerrainManager.GetWaterLevelAtXYZ(_position);
+            float waterHeight = PhysScene.TerrainManager.GetWaterLevelAtXYZ(_position);
             if (Position.Z < waterHeight)
             {
                 _position.Z = waterHeight;
@@ -414,11 +358,10 @@ public sealed class BSCharacter : BSPhysObject
         {
             // The new position value must be pushed into the physics engine but we can't
             //    just assign to "Position" because of potential call loops.
-            PhysicsScene.TaintedObject(inTaintTime, "BSCharacter.PositionSanityCheck", delegate()
+            PhysScene.TaintedObject(inTaintTime, "BSCharacter.PositionSanityCheck", delegate()
             {
                 DetailLog("{0},BSCharacter.PositionSanityCheck,taint,pos={1},orient={2}", LocalID, _position, _orientation);
-                if (PhysBody.HasPhysicalBody)
-                    PhysicsScene.PE.SetTranslation(PhysBody, _position, _orientation);
+                ForcePosition = _position;
             });
             ret = true;
         }
@@ -428,25 +371,25 @@ public sealed class BSCharacter : BSPhysObject
     public override float Mass { get { return _mass; } }
 
     // used when we only want this prim's mass and not the linkset thing
-    public override float RawMass { 
+    public override float RawMass {
         get {return _mass; }
     }
     public override void UpdatePhysicalMassProperties(float physMass, bool inWorld)
     {
-        OMV.Vector3 localInertia = PhysicsScene.PE.CalculateLocalInertia(PhysShape, physMass);
-        PhysicsScene.PE.SetMassProps(PhysBody, physMass, localInertia);
+        OMV.Vector3 localInertia = PhysScene.PE.CalculateLocalInertia(PhysShape.physShapeInfo, physMass);
+        PhysScene.PE.SetMassProps(PhysBody, physMass, localInertia);
     }
 
     public override OMV.Vector3 Force {
-        get { return _force; }
+        get { return RawForce; }
         set {
-            _force = value;
+            RawForce = value;
             // m_log.DebugFormat("{0}: Force = {1}", LogHeader, _force);
-            PhysicsScene.TaintedObject("BSCharacter.SetForce", delegate()
+            PhysScene.TaintedObject("BSCharacter.SetForce", delegate()
             {
-                DetailLog("{0},BSCharacter.setForce,taint,force={1}", LocalID, _force);
+                DetailLog("{0},BSCharacter.setForce,taint,force={1}", LocalID, RawForce);
                 if (PhysBody.HasPhysicalBody)
-                    PhysicsScene.PE.SetObjectForce(PhysBody, _force);
+                    PhysScene.PE.SetObjectForce(PhysBody, RawForce);
             });
         }
     }
@@ -469,77 +412,49 @@ public sealed class BSCharacter : BSPhysObject
     {
         get
         {
-            return _velocityMotor.TargetValue;
+            return base.m_targetVelocity;
         }
         set
         {
             DetailLog("{0},BSCharacter.setTargetVelocity,call,vel={1}", LocalID, value);
+            m_targetVelocity = value;
             OMV.Vector3 targetVel = value;
             if (_setAlwaysRun)
-                targetVel *= BSParam.AvatarAlwaysRunFactor;
+                targetVel *= new OMV.Vector3(BSParam.AvatarAlwaysRunFactor, BSParam.AvatarAlwaysRunFactor, 0f);
 
-            PhysicsScene.TaintedObject("BSCharacter.setTargetVelocity", delegate()
-            {
-                _velocityMotor.Reset();
-                _velocityMotor.SetTarget(targetVel);
-                _velocityMotor.SetCurrent(_velocity);
-                _velocityMotor.Enabled = true;
-            });
+            if (m_moveActor != null)
+                m_moveActor.SetVelocityAndTarget(RawVelocity, targetVel, false /* inTaintTime */);
         }
     }
     // Directly setting velocity means this is what the user really wants now.
     public override OMV.Vector3 Velocity {
-        get { return _velocity; }
+        get { return RawVelocity; }
         set {
-            _velocity = value;
-            // m_log.DebugFormat("{0}: set velocity = {1}", LogHeader, _velocity);
-            PhysicsScene.TaintedObject("BSCharacter.setVelocity", delegate()
+            RawVelocity = value;
+            // m_log.DebugFormat("{0}: set velocity = {1}", LogHeader, RawVelocity);
+            PhysScene.TaintedObject("BSCharacter.setVelocity", delegate()
             {
-                _velocityMotor.Reset();
-                _velocityMotor.SetCurrent(_velocity);
-                _velocityMotor.SetTarget(_velocity);
-                // Even though the motor is initialized, it's not used and the velocity goes straight into the avatar.
-                _velocityMotor.Enabled = false;
-
-                DetailLog("{0},BSCharacter.setVelocity,taint,vel={1}", LocalID, _velocity);
-                ForceVelocity = _velocity;
+                if (m_moveActor != null)
+                    m_moveActor.SetVelocityAndTarget(RawVelocity, RawVelocity, true /* inTaintTime */);
+
+                DetailLog("{0},BSCharacter.setVelocity,taint,vel={1}", LocalID, RawVelocity);
+                ForceVelocity = RawVelocity;
             });
         }
     }
     public override OMV.Vector3 ForceVelocity {
-        get { return _velocity; }
+        get { return RawVelocity; }
         set {
-            PhysicsScene.AssertInTaintTime("BSCharacter.ForceVelocity");
+            PhysScene.AssertInTaintTime("BSCharacter.ForceVelocity");
 
-            _velocity = value;
-            // Depending on whether the avatar is moving or not, change the friction
-            //    to keep the avatar from slipping around
-            if (_velocity.Length() == 0)
-            {
-                if (_currentFriction != BSParam.AvatarStandingFriction)
-                {
-                    _currentFriction = BSParam.AvatarStandingFriction;
-                    if (PhysBody.HasPhysicalBody)
-                        PhysicsScene.PE.SetFriction(PhysBody, _currentFriction);
-                }
-            }
-            else
-            {
-                if (_currentFriction != BSParam.AvatarFriction)
-                {
-                    _currentFriction = BSParam.AvatarFriction;
-                    if (PhysBody.HasPhysicalBody)
-                        PhysicsScene.PE.SetFriction(PhysBody, _currentFriction);
-                }
-            }
-
-            PhysicsScene.PE.SetLinearVelocity(PhysBody, _velocity);
-            PhysicsScene.PE.Activate(PhysBody, true);
+            RawVelocity = value;
+            PhysScene.PE.SetLinearVelocity(PhysBody, RawVelocity);
+            PhysScene.PE.Activate(PhysBody, true);
         }
     }
     public override OMV.Vector3 Torque {
-        get { return _torque; }
-        set { _torque = value;
+        get { return RawTorque; }
+        set { RawTorque = value;
         }
     }
     public override float CollisionScore {
@@ -564,7 +479,7 @@ public sealed class BSCharacter : BSPhysObject
             if (_orientation != value)
             {
                 _orientation = value;
-                PhysicsScene.TaintedObject("BSCharacter.setOrientation", delegate()
+                PhysScene.TaintedObject("BSCharacter.setOrientation", delegate()
                 {
                     ForceOrientation = _orientation;
                 });
@@ -576,7 +491,7 @@ public sealed class BSCharacter : BSPhysObject
     {
         get
         {
-            _orientation = PhysicsScene.PE.GetOrientation(PhysBody);
+            _orientation = PhysScene.PE.GetOrientation(PhysBody);
             return _orientation;
         }
         set
@@ -585,7 +500,7 @@ public sealed class BSCharacter : BSPhysObject
             if (PhysBody.HasPhysicalBody)
             {
                 // _position = PhysicsScene.PE.GetPosition(BSBody);
-                PhysicsScene.PE.SetTranslation(PhysBody, _position, _orientation);
+                PhysScene.PE.SetTranslation(PhysBody, _position, _orientation);
             }
         }
     }
@@ -605,6 +520,9 @@ public sealed class BSCharacter : BSPhysObject
     public override bool IsStatic {
         get { return false; }
     }
+    public override bool IsPhysicallyActive {
+        get { return true; }
+    }
     public override bool Flying {
         get { return _flying; }
         set {
@@ -631,14 +549,14 @@ public sealed class BSCharacter : BSPhysObject
     public override bool FloatOnWater {
         set {
             _floatOnWater = value;
-            PhysicsScene.TaintedObject("BSCharacter.setFloatOnWater", delegate()
+            PhysScene.TaintedObject("BSCharacter.setFloatOnWater", delegate()
             {
                 if (PhysBody.HasPhysicalBody)
                 {
                     if (_floatOnWater)
-                        CurrentCollisionFlags = PhysicsScene.PE.AddToCollisionFlags(PhysBody, CollisionFlags.BS_FLOATS_ON_WATER);
+                        CurrentCollisionFlags = PhysScene.PE.AddToCollisionFlags(PhysBody, CollisionFlags.BS_FLOATS_ON_WATER);
                     else
-                        CurrentCollisionFlags = PhysicsScene.PE.RemoveFromCollisionFlags(PhysBody, CollisionFlags.BS_FLOATS_ON_WATER);
+                        CurrentCollisionFlags = PhysScene.PE.RemoveFromCollisionFlags(PhysBody, CollisionFlags.BS_FLOATS_ON_WATER);
                 }
             });
         }
@@ -659,7 +577,7 @@ public sealed class BSCharacter : BSPhysObject
     public override float Buoyancy {
         get { return _buoyancy; }
         set { _buoyancy = value;
-            PhysicsScene.TaintedObject("BSCharacter.setBuoyancy", delegate()
+            PhysScene.TaintedObject("BSCharacter.setBuoyancy", delegate()
             {
                 DetailLog("{0},BSCharacter.setBuoyancy,taint,buoy={1}", LocalID, _buoyancy);
                 ForceBuoyancy = _buoyancy;
@@ -668,15 +586,16 @@ public sealed class BSCharacter : BSPhysObject
     }
     public override float ForceBuoyancy {
         get { return _buoyancy; }
-        set { 
-            PhysicsScene.AssertInTaintTime("BSCharacter.ForceBuoyancy");
+        set {
+            PhysScene.AssertInTaintTime("BSCharacter.ForceBuoyancy");
 
             _buoyancy = value;
             DetailLog("{0},BSCharacter.setForceBuoyancy,taint,buoy={1}", LocalID, _buoyancy);
             // Buoyancy is faked by changing the gravity applied to the object
-            float grav = PhysicsScene.Params.gravity * (1f - _buoyancy);
+            float  grav = BSParam.Gravity * (1f - _buoyancy);
+            Gravity = new OMV.Vector3(0f, 0f, grav);
             if (PhysBody.HasPhysicalBody)
-                PhysicsScene.PE.SetGravity(PhysBody, new OMV.Vector3(0f, 0f, grav));
+                PhysScene.PE.SetGravity(PhysBody, Gravity);
         }
     }
 
@@ -691,53 +610,25 @@ public sealed class BSCharacter : BSPhysObject
         set { _PIDTau = value; }
     }
 
-    // Used for llSetHoverHeight and maybe vehicle height
-    // Hover Height will override MoveTo target's Z
-    public override bool PIDHoverActive {
-        set { _useHoverPID = value; }
-    }
-    public override float PIDHoverHeight {
-        set { _PIDHoverHeight = value; }
-    }
-    public override PIDHoverType PIDHoverType {
-        set { _PIDHoverType = value; }
-    }
-    public override float PIDHoverTau {
-        set { _PIDHoverTao = value; }
-    }
-
-    // For RotLookAt
-    public override OMV.Quaternion APIDTarget { set { return; } }
-    public override bool APIDActive { set { return; } }
-    public override float APIDStrength { set { return; } }
-    public override float APIDDamping { set { return; } }
-
     public override void AddForce(OMV.Vector3 force, bool pushforce)
     {
         // Since this force is being applied in only one step, make this a force per second.
-        OMV.Vector3 addForce = force / PhysicsScene.LastTimeStep;
+        OMV.Vector3 addForce = force / PhysScene.LastTimeStep;
         AddForce(addForce, pushforce, false);
     }
     private void AddForce(OMV.Vector3 force, bool pushforce, bool inTaintTime) {
         if (force.IsFinite())
         {
-            float magnitude = force.Length();
-            if (magnitude > BSParam.MaxAddForceMagnitude)
-            {
-                // Force has a limit
-                force = force / magnitude * BSParam.MaxAddForceMagnitude;
-            }
-
-            OMV.Vector3 addForce = force;
+            OMV.Vector3 addForce = Util.ClampV(force, BSParam.MaxAddForceMagnitude);
             // DetailLog("{0},BSCharacter.addForce,call,force={1}", LocalID, addForce);
 
-            PhysicsScene.TaintedObject(inTaintTime, "BSCharacter.AddForce", delegate()
+            PhysScene.TaintedObject(inTaintTime, "BSCharacter.AddForce", delegate()
             {
                 // Bullet adds this central force to the total force for this tick
                 // DetailLog("{0},BSCharacter.addForce,taint,force={1}", LocalID, addForce);
                 if (PhysBody.HasPhysicalBody)
                 {
-                    PhysicsScene.PE.ApplyCentralForce(PhysBody, addForce);
+                    PhysScene.PE.ApplyCentralForce(PhysBody, addForce);
                 }
             });
         }
@@ -748,7 +639,7 @@ public sealed class BSCharacter : BSPhysObject
         }
     }
 
-    public override void AddAngularForce(OMV.Vector3 force, bool pushforce) {
+    public override void AddAngularForce(OMV.Vector3 force, bool pushforce, bool inTaintTime) {
     }
     public override void SetMomentum(OMV.Vector3 momentum) {
     }
@@ -756,7 +647,7 @@ public sealed class BSCharacter : BSPhysObject
     private OMV.Vector3 ComputeAvatarScale(OMV.Vector3 size)
     {
         OMV.Vector3 newScale;
-        
+
         // Bullet's capsule total height is the "passed height + radius * 2";
         // The base capsule is 1 diameter and 2 height (passed radius=0.5, passed height = 1)
         // The number we pass in for 'scaling' is the multiplier to get that base
@@ -794,34 +685,48 @@ public sealed class BSCharacter : BSPhysObject
                         * Math.Min(Size.X, Size.Y) / 2
                         * Size.Y / 2f    // plus the volume of the capsule end caps
                         );
-        _mass = _avatarDensity * _avatarVolume;
+        _mass = Density * BSParam.DensityScaleFactor * _avatarVolume;
     }
 
     // The physics engine says that properties have updated. Update same and inform
     // the world that things have changed.
     public override void UpdateProperties(EntityProperties entprop)
     {
-        _position = entprop.Position;
+        // Don't change position if standing on a stationary object.
+        if (!IsStationary)
+            _position = entprop.Position;
+
         _orientation = entprop.Rotation;
-        _velocity = entprop.Velocity;
+
+        // Smooth velocity. OpenSimulator is VERY sensitive to changes in velocity of the avatar
+        //    and will send agent updates to the clients if velocity changes by more than
+        //    0.001m/s. Bullet introduces a lot of jitter in the velocity which causes many
+        //    extra updates.
+        if (!entprop.Velocity.ApproxEquals(RawVelocity, 0.1f))
+            RawVelocity = entprop.Velocity;
+
         _acceleration = entprop.Acceleration;
         _rotationalVelocity = entprop.RotationalVelocity;
 
         // Do some sanity checking for the avatar. Make sure it's above ground and inbounds.
-        PositionSanityCheck(true);
+        if (PositionSanityCheck(true))
+        {
+            DetailLog("{0},BSCharacter.UpdateProperties,updatePosForSanity,pos={1}", LocalID, _position);
+            entprop.Position = _position;
+        }
 
         // remember the current and last set values
         LastEntityProperties = CurrentEntityProperties;
         CurrentEntityProperties = entprop;
 
         // Tell the linkset about value changes
-        Linkset.UpdateProperties(this, true);
+        // Linkset.UpdateProperties(UpdatedProperties.EntPropUpdates, this);
 
         // Avatars don't report their changes the usual way. Changes are checked for in the heartbeat loop.
         // base.RequestPhysicsterseUpdate();
 
         DetailLog("{0},BSCharacter.UpdateProperties,call,pos={1},orient={2},vel={3},accel={4},rotVel={5}",
-                LocalID, _position, _orientation, _velocity, _acceleration, _rotationalVelocity);
+                LocalID, _position, _orientation, RawVelocity, _acceleration, _rotationalVelocity);
     }
 }
 }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSConstraint.cs b/OpenSim/Region/Physics/BulletSPlugin/BSConstraint.cs
index b813974..42b5c49 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSConstraint.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSConstraint.cs
@@ -85,7 +85,9 @@ public abstract class BSConstraint : IDisposable
     {
         bool ret = false;
         if (m_enabled)
+        {
             ret = PhysicsScene.PE.SetAngularLimits(m_constraint, low, high);
+        }
         return ret;
     }
 
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSConstraint6Dof.cs b/OpenSim/Region/Physics/BulletSPlugin/BSConstraint6Dof.cs
index ecb1b32..d0949f5 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSConstraint6Dof.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSConstraint6Dof.cs
@@ -57,6 +57,7 @@ public sealed class BSConstraint6Dof : BSConstraint
                             obj1.ID, obj1.AddrString, obj2.ID, obj2.AddrString);
     }
 
+    // 6 Dof constraint based on a midpoint between the two constrained bodies
     public BSConstraint6Dof(BulletWorld world, BulletBody obj1, BulletBody obj2,
                     Vector3 joinPoint,
                     bool useLinearReferenceFrameA, bool disableCollisionsBetweenLinkedBodies)
@@ -94,6 +95,21 @@ public sealed class BSConstraint6Dof : BSConstraint
         }
     }
 
+    // A 6 Dof constraint that is fixed in the world and constrained to a on-the-fly created static object
+    public BSConstraint6Dof(BulletWorld world, BulletBody obj1, Vector3 frameInBloc, Quaternion frameInBrot,
+                    bool useLinearReferenceFrameB, bool disableCollisionsBetweenLinkedBodies)
+        : base(world)
+    {
+        m_body1 = obj1;
+        m_body2 = obj1; // Look out for confusion down the road
+        m_constraint = PhysicsScene.PE.Create6DofConstraintFixed(m_world, m_body1,
+                                frameInBloc, frameInBrot,
+                                useLinearReferenceFrameB, disableCollisionsBetweenLinkedBodies);
+        m_enabled = true;
+        world.physicsScene.DetailLog("{0},BS6DofConstraint,createFixed,wID={1},rID={2},rBody={3}",
+                            BSScene.DetailLogZero, world.worldID, obj1.ID, obj1.AddrString);
+    }
+
     public bool SetFrames(Vector3 frameA, Quaternion frameArot, Vector3 frameB, Quaternion frameBrot)
     {
         bool ret = false;
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSConstraintCollection.cs b/OpenSim/Region/Physics/BulletSPlugin/BSConstraintCollection.cs
index 2aeff25..5c8d94e 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSConstraintCollection.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSConstraintCollection.cs
@@ -117,8 +117,7 @@ public sealed class BSConstraintCollection : IDisposable
             if (this.TryGetConstraint(body1, body2, out constrain))
             {
                 // remove the constraint from our collection
-                RemoveAndDestroyConstraint(constrain);
-                ret = true;
+                ret = RemoveAndDestroyConstraint(constrain);
             }
         }
 
@@ -126,17 +125,19 @@ public sealed class BSConstraintCollection : IDisposable
     }
 
     // The constraint MUST exist in the collection
+    // Could be called if the constraint was previously removed.
+    // Return 'true' if the constraint was actually removed and disposed.
     public bool RemoveAndDestroyConstraint(BSConstraint constrain)
     {
+        bool removed = false;
         lock (m_constraints)
         {
             // remove the constraint from our collection
-            m_constraints.Remove(constrain);
+            removed = m_constraints.Remove(constrain);
         }
-        // tell the engine that all its structures need to be freed
+        // Dispose() is safe to call multiple times
         constrain.Dispose();
-        // we destroyed something
-        return true;
+        return removed;
     }
 
     // Remove all constraints that reference the passed body.
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSConstraintHinge.cs b/OpenSim/Region/Physics/BulletSPlugin/BSConstraintHinge.cs
index 7714a03..ed89f63 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSConstraintHinge.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSConstraintHinge.cs
@@ -45,7 +45,7 @@ public sealed class BSConstraintHinge : BSConstraint
         m_body1 = obj1;
         m_body2 = obj2;
         m_constraint = PhysicsScene.PE.CreateHingeConstraint(world, obj1, obj2,
-                                pivotInA, pivotInB, axisInA, axisInB, 
+                                pivotInA, pivotInB, axisInA, axisInB,
                                 useLinearReferenceFrameA, disableCollisionsBetweenLinkedBodies);
         m_enabled = true;
     }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
index 13c2539..c16b7d3 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
@@ -35,17 +35,19 @@ using System.Collections.Generic;
 using System.Reflection;
 using System.Runtime.InteropServices;
 using OpenMetaverse;
+using OpenSim.Framework;
 using OpenSim.Region.Physics.Manager;
 
 namespace OpenSim.Region.Physics.BulletSPlugin
 {
-    public sealed class BSDynamics
+    public sealed class BSDynamics : BSActor
     {
         private static string LogHeader = "[BULLETSIM VEHICLE]";
 
-        private BSScene PhysicsScene { get; set; }
         // the prim this dynamic controller belongs to
-        private BSPrim Prim { get; set; }
+        private BSPrim ControllingPrim { get; set; }
+
+        private bool m_haveRegisteredForSceneEvents;
 
         // mass of the vehicle fetched each time we're calles
         private float m_vehicleMass;
@@ -108,10 +110,9 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         private float m_VhoverEfficiency = 0f;
         private float m_VhoverTimescale = 0f;
         private float m_VhoverTargetHeight = -1.0f;     // if <0 then no hover, else its the current target height
-        private float m_VehicleBuoyancy = 0f;           //KF: m_VehicleBuoyancy is set by VEHICLE_BUOYANCY for a vehicle.
-                    // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity)
-                    // KF: So far I have found no good method to combine a script-requested .Z velocity and gravity.
-                    // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity.
+        // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity)
+        private float m_VehicleBuoyancy = 0f;
+        private Vector3 m_VehicleGravity = Vector3.Zero;    // Gravity computed when buoyancy set
 
         //Attractor properties
         private BSVMotor m_verticalAttractionMotor = new BSVMotor("VerticalAttraction");
@@ -124,22 +125,51 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         static readonly float PIOverFour = ((float)Math.PI) / 4f;
         static readonly float PIOverTwo = ((float)Math.PI) / 2f;
 
-        public BSDynamics(BSScene myScene, BSPrim myPrim)
+        // For debugging, flags to turn on and off individual corrections.
+        public bool enableAngularVerticalAttraction;
+        public bool enableAngularDeflection;
+        public bool enableAngularBanking;
+
+        public BSDynamics(BSScene myScene, BSPrim myPrim, string actorName)
+            : base(myScene, myPrim, actorName)
         {
-            PhysicsScene = myScene;
-            Prim = myPrim;
+            ControllingPrim = myPrim;
             Type = Vehicle.TYPE_NONE;
+            m_haveRegisteredForSceneEvents = false;
+            SetupVehicleDebugging();
+        }
+
+        // Stopgap debugging enablement. Allows source level debugging but still checking
+        //    in changes by making enablement of debugging flags from INI file.
+        public void SetupVehicleDebugging()
+        {
+            enableAngularVerticalAttraction = true;
+            enableAngularDeflection = false;
+            enableAngularBanking = true;
+            if (BSParam.VehicleDebuggingEnable)
+            {
+                enableAngularVerticalAttraction = true;
+                enableAngularDeflection = false;
+                enableAngularBanking = false;
+            }
         }
 
         // Return 'true' if this vehicle is doing vehicle things
         public bool IsActive
         {
-            get { return Type != Vehicle.TYPE_NONE && Prim.IsPhysical; }
+            get { return (Type != Vehicle.TYPE_NONE && ControllingPrim.IsPhysicallyActive); }
+        }
+
+        // Return 'true' if this a vehicle that should be sitting on the ground
+        public bool IsGroundVehicle
+        {
+            get { return (Type == Vehicle.TYPE_CAR || Type == Vehicle.TYPE_SLED); }
         }
 
-        internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue)
+        #region Vehicle parameter setting
+        public void ProcessFloatVehicleParam(Vehicle pParam, float pValue)
         {
-            VDetailLog("{0},ProcessFloatVehicleParam,param={1},val={2}", Prim.LocalID, pParam, pValue);
+            VDetailLog("{0},ProcessFloatVehicleParam,param={1},val={2}", ControllingPrim.LocalID, pParam, pValue);
             switch (pParam)
             {
                 case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY:
@@ -167,6 +197,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     break;
                 case Vehicle.BUOYANCY:
                     m_VehicleBuoyancy = ClampInRange(-1f, pValue, 1f);
+                    m_VehicleGravity = ControllingPrim.ComputeGravity(m_VehicleBuoyancy);
                     break;
                 case Vehicle.HOVER_EFFICIENCY:
                     m_VhoverEfficiency = ClampInRange(0f, pValue, 1f);
@@ -204,15 +235,14 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 // set all of the components to the same value
                 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
                     m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue);
-                    m_angularMotor.FrictionTimescale = m_angularFrictionTimescale;
                     break;
                 case Vehicle.ANGULAR_MOTOR_DIRECTION:
                     m_angularMotorDirection = new Vector3(pValue, pValue, pValue);
+                    m_angularMotor.Zero();
                     m_angularMotor.SetTarget(m_angularMotorDirection);
                     break;
                 case Vehicle.LINEAR_FRICTION_TIMESCALE:
                     m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue);
-                    m_linearMotor.FrictionTimescale = m_linearFrictionTimescale;
                     break;
                 case Vehicle.LINEAR_MOTOR_DIRECTION:
                     m_linearMotorDirection = new Vector3(pValue, pValue, pValue);
@@ -228,12 +258,11 @@ namespace OpenSim.Region.Physics.BulletSPlugin
 
         internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue)
         {
-            VDetailLog("{0},ProcessVectorVehicleParam,param={1},val={2}", Prim.LocalID, pParam, pValue);
+            VDetailLog("{0},ProcessVectorVehicleParam,param={1},val={2}", ControllingPrim.LocalID, pParam, pValue);
             switch (pParam)
             {
                 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
                     m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
-                    m_angularMotor.FrictionTimescale = m_angularFrictionTimescale;
                     break;
                 case Vehicle.ANGULAR_MOTOR_DIRECTION:
                     // Limit requested angular speed to 2 rps= 4 pi rads/sec
@@ -241,11 +270,11 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     pValue.Y = ClampInRange(-12.56f, pValue.Y, 12.56f);
                     pValue.Z = ClampInRange(-12.56f, pValue.Z, 12.56f);
                     m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
+                    m_angularMotor.Zero();
                     m_angularMotor.SetTarget(m_angularMotorDirection);
                     break;
                 case Vehicle.LINEAR_FRICTION_TIMESCALE:
                     m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
-                    m_linearMotor.FrictionTimescale = m_linearFrictionTimescale;
                     break;
                 case Vehicle.LINEAR_MOTOR_DIRECTION:
                     m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
@@ -263,7 +292,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
 
         internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue)
         {
-            VDetailLog("{0},ProcessRotationalVehicleParam,param={1},val={2}", Prim.LocalID, pParam, pValue);
+            VDetailLog("{0},ProcessRotationalVehicleParam,param={1},val={2}", ControllingPrim.LocalID, pParam, pValue);
             switch (pParam)
             {
                 case Vehicle.REFERENCE_FRAME:
@@ -277,7 +306,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
 
         internal void ProcessVehicleFlags(int pParam, bool remove)
         {
-            VDetailLog("{0},ProcessVehicleFlags,param={1},remove={2}", Prim.LocalID, pParam, remove);
+            VDetailLog("{0},ProcessVehicleFlags,param={1},remove={2}", ControllingPrim.LocalID, pParam, remove);
             VehicleFlag parm = (VehicleFlag)pParam;
             if (pParam == -1)
                 m_flags = (VehicleFlag)0;
@@ -290,9 +319,9 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             }
         }
 
-        internal void ProcessTypeChange(Vehicle pType)
+        public void ProcessTypeChange(Vehicle pType)
         {
-            VDetailLog("{0},ProcessTypeChange,type={1}", Prim.LocalID, pType);
+            VDetailLog("{0},ProcessTypeChange,type={1}", ControllingPrim.LocalID, pType);
             // Set Defaults For Type
             Type = pType;
             switch (pType)
@@ -526,81 +555,136 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     break;
             }
 
-            // Update any physical parameters based on this type.
-            Refresh();
-
-            m_linearMotor = new BSVMotor("LinearMotor", m_linearMotorTimescale,
-                                m_linearMotorDecayTimescale, m_linearFrictionTimescale,
-                                1f);
-            m_linearMotor.PhysicsScene = PhysicsScene;  // DEBUG DEBUG DEBUG (enables detail logging)
+            m_linearMotor = new BSVMotor("LinearMotor", m_linearMotorTimescale, m_linearMotorDecayTimescale, 1f);
+            m_linearMotor.PhysicsScene = m_physicsScene;  // DEBUG DEBUG DEBUG (enables detail logging)
 
-            m_angularMotor = new BSVMotor("AngularMotor", m_angularMotorTimescale,
-                                m_angularMotorDecayTimescale, m_angularFrictionTimescale,
-                                1f);
-            m_angularMotor.PhysicsScene = PhysicsScene;  // DEBUG DEBUG DEBUG (enables detail logging)
+            m_angularMotor = new BSVMotor("AngularMotor", m_angularMotorTimescale, m_angularMotorDecayTimescale, 1f);
+            m_angularMotor.PhysicsScene = m_physicsScene;  // DEBUG DEBUG DEBUG (enables detail logging)
 
+            /*  Not implemented
             m_verticalAttractionMotor = new BSVMotor("VerticalAttraction", m_verticalAttractionTimescale,
                                 BSMotor.Infinite, BSMotor.InfiniteVector,
                                 m_verticalAttractionEfficiency);
             // Z goes away and we keep X and Y
-            m_verticalAttractionMotor.FrictionTimescale = new Vector3(BSMotor.Infinite, BSMotor.Infinite, 0.1f);
             m_verticalAttractionMotor.PhysicsScene = PhysicsScene;  // DEBUG DEBUG DEBUG (enables detail logging)
+             */
+
+            if (this.Type == Vehicle.TYPE_NONE)
+            {
+                UnregisterForSceneEvents();
+            }
+            else
+            {
+                RegisterForSceneEvents();
+            }
+
+            // Update any physical parameters based on this type.
+            Refresh();
+        }
+        #endregion // Vehicle parameter setting
+
+        // BSActor.Refresh()
+        public override void Refresh()
+        {
+            // If asking for a refresh, reset the physical parameters before the next simulation step.
+            // Called whether active or not since the active state may be updated before the next step.
+            m_physicsScene.PostTaintObject("BSDynamics.Refresh", ControllingPrim.LocalID, delegate()
+            {
+                SetPhysicalParameters();
+            });
         }
 
         // Some of the properties of this prim may have changed.
         // Do any updating needed for a vehicle
-        public void Refresh()
+        private void SetPhysicalParameters()
         {
             if (IsActive)
             {
                 // Remember the mass so we don't have to fetch it every step
-                m_vehicleMass = Prim.Linkset.LinksetMass;
+                m_vehicleMass = ControllingPrim.TotalMass;
 
                 // Friction affects are handled by this vehicle code
-                float friction = 0f;
-                PhysicsScene.PE.SetFriction(Prim.PhysBody, friction);
+                m_physicsScene.PE.SetFriction(ControllingPrim.PhysBody, BSParam.VehicleFriction);
+                m_physicsScene.PE.SetRestitution(ControllingPrim.PhysBody, BSParam.VehicleRestitution);
 
                 // Moderate angular movement introduced by Bullet.
                 // TODO: possibly set AngularFactor and LinearFactor for the type of vehicle.
                 //     Maybe compute linear and angular factor and damping from params.
-                float angularDamping = BSParam.VehicleAngularDamping;
-                PhysicsScene.PE.SetAngularDamping(Prim.PhysBody, angularDamping);
+                m_physicsScene.PE.SetAngularDamping(ControllingPrim.PhysBody, BSParam.VehicleAngularDamping);
+                m_physicsScene.PE.SetLinearFactor(ControllingPrim.PhysBody, BSParam.VehicleLinearFactor);
+                m_physicsScene.PE.SetAngularFactorV(ControllingPrim.PhysBody, BSParam.VehicleAngularFactor);
 
                 // Vehicles report collision events so we know when it's on the ground
-                PhysicsScene.PE.AddToCollisionFlags(Prim.PhysBody, CollisionFlags.BS_VEHICLE_COLLISIONS);
+                m_physicsScene.PE.AddToCollisionFlags(ControllingPrim.PhysBody, CollisionFlags.BS_VEHICLE_COLLISIONS);
+
+                ControllingPrim.Inertia = m_physicsScene.PE.CalculateLocalInertia(ControllingPrim.PhysShape.physShapeInfo, m_vehicleMass);
+                m_physicsScene.PE.SetMassProps(ControllingPrim.PhysBody, m_vehicleMass, ControllingPrim.Inertia);
+                m_physicsScene.PE.UpdateInertiaTensor(ControllingPrim.PhysBody);
+
+                // Set the gravity for the vehicle depending on the buoyancy
+                // TODO: what should be done if prim and vehicle buoyancy differ?
+                m_VehicleGravity = ControllingPrim.ComputeGravity(m_VehicleBuoyancy);
+                // The actual vehicle gravity is set to zero in Bullet so we can do all the application of same.
+                m_physicsScene.PE.SetGravity(ControllingPrim.PhysBody, Vector3.Zero);
+
+                VDetailLog("{0},BSDynamics.SetPhysicalParameters,mass={1},inert={2},vehGrav={3},aDamp={4},frict={5},rest={6},lFact={7},aFact={8}",
+                        ControllingPrim.LocalID, m_vehicleMass, ControllingPrim.Inertia, m_VehicleGravity,
+                        BSParam.VehicleAngularDamping, BSParam.VehicleFriction, BSParam.VehicleRestitution,
+                        BSParam.VehicleLinearFactor, BSParam.VehicleAngularFactor
+                        );
+            }
+            else
+            {
+                if (ControllingPrim.PhysBody.HasPhysicalBody)
+                    m_physicsScene.PE.RemoveFromCollisionFlags(ControllingPrim.PhysBody, CollisionFlags.BS_VEHICLE_COLLISIONS);
+            }
+        }
 
-                Vector3 localInertia = PhysicsScene.PE.CalculateLocalInertia(Prim.PhysShape, m_vehicleMass);
-                PhysicsScene.PE.SetMassProps(Prim.PhysBody, m_vehicleMass, localInertia);
-                PhysicsScene.PE.UpdateInertiaTensor(Prim.PhysBody);
+        // BSActor.RemoveBodyDependencies
+        public override void RemoveDependencies()
+        {
+            Refresh();
+        }
 
-                Vector3 grav = PhysicsScene.DefaultGravity * (1f - Prim.Buoyancy);
-                PhysicsScene.PE.SetGravity(Prim.PhysBody, grav);
+        // BSActor.Release()
+        public override void Dispose()
+        {
+            UnregisterForSceneEvents();
+            Type = Vehicle.TYPE_NONE;
+            Enabled = false;
+            return;
+        }
 
-                VDetailLog("{0},BSDynamics.Refresh,mass={1},frict={2},inert={3},aDamp={4}",
-                                Prim.LocalID, m_vehicleMass, friction, localInertia, angularDamping);
-            }
-            else
+        private void RegisterForSceneEvents()
+        {
+            if (!m_haveRegisteredForSceneEvents)
             {
-                PhysicsScene.PE.RemoveFromCollisionFlags(Prim.PhysBody, CollisionFlags.BS_VEHICLE_COLLISIONS);
+                m_physicsScene.BeforeStep += this.Step;
+                m_physicsScene.AfterStep += this.PostStep;
+                ControllingPrim.OnPreUpdateProperty += this.PreUpdateProperty;
+                m_haveRegisteredForSceneEvents = true;
             }
         }
 
-        public bool RemoveBodyDependencies(BSPhysObject prim)
+        private void UnregisterForSceneEvents()
         {
-            // If active, we need to add our properties back when the body is rebuilt.
-            return IsActive;
+            if (m_haveRegisteredForSceneEvents)
+            {
+                m_physicsScene.BeforeStep -= this.Step;
+                m_physicsScene.AfterStep -= this.PostStep;
+                ControllingPrim.OnPreUpdateProperty -= this.PreUpdateProperty;
+                m_haveRegisteredForSceneEvents = false;
+            }
         }
 
-        public void RestoreBodyDependencies(BSPhysObject prim)
+        private void PreUpdateProperty(ref EntityProperties entprop)
         {
-            if (Prim.LocalID != prim.LocalID)
+            // A temporary kludge to suppress the rotational effects introduced on vehicles by Bullet
+            // TODO: handle physics introduced by Bullet with computed vehicle physics.
+            if (IsActive)
             {
-                // The call should be on us by our prim. Error if not.
-                PhysicsScene.Logger.ErrorFormat("{0} RestoreBodyDependencies: called by not my prim. passedLocalID={1}, vehiclePrimLocalID={2}",
-                                LogHeader, prim.LocalID, Prim.LocalID);
-                return;
+                entprop.RotationalVelocity = Vector3.Zero;
             }
-            Refresh();
         }
 
         #region Known vehicle value functions
@@ -617,70 +701,87 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         private Vector3 m_knownPosition;
         private Vector3 m_knownVelocity;
         private Vector3 m_knownForce;
+        private Vector3 m_knownForceImpulse;
         private Quaternion m_knownOrientation;
         private Vector3 m_knownRotationalVelocity;
         private Vector3 m_knownRotationalForce;
+        private Vector3 m_knownRotationalImpulse;
         private Vector3 m_knownForwardVelocity;    // vehicle relative forward speed
 
         private const int m_knownChangedPosition           = 1 << 0;
         private const int m_knownChangedVelocity           = 1 << 1;
         private const int m_knownChangedForce              = 1 << 2;
-        private const int m_knownChangedOrientation        = 1 << 3;
-        private const int m_knownChangedRotationalVelocity = 1 << 4;
-        private const int m_knownChangedRotationalForce    = 1 << 5;
-        private const int m_knownChangedTerrainHeight      = 1 << 6;
-        private const int m_knownChangedWaterLevel         = 1 << 7;
-        private const int m_knownChangedForwardVelocity    = 1 << 8;
-
-        private void ForgetKnownVehicleProperties()
+        private const int m_knownChangedForceImpulse       = 1 << 3;
+        private const int m_knownChangedOrientation        = 1 << 4;
+        private const int m_knownChangedRotationalVelocity = 1 << 5;
+        private const int m_knownChangedRotationalForce    = 1 << 6;
+        private const int m_knownChangedRotationalImpulse  = 1 << 7;
+        private const int m_knownChangedTerrainHeight      = 1 << 8;
+        private const int m_knownChangedWaterLevel         = 1 << 9;
+        private const int m_knownChangedForwardVelocity    = 1 <<10;
+
+        public void ForgetKnownVehicleProperties()
         {
             m_knownHas = 0;
             m_knownChanged = 0;
         }
         // Push all the changed values back into the physics engine
-        private void PushKnownChanged()
+        public void PushKnownChanged()
         {
             if (m_knownChanged != 0)
             {
                 if ((m_knownChanged & m_knownChangedPosition) != 0)
-                    Prim.ForcePosition = m_knownPosition;
+                    ControllingPrim.ForcePosition = m_knownPosition;
 
                 if ((m_knownChanged & m_knownChangedOrientation) != 0)
-                    Prim.ForceOrientation = m_knownOrientation;
+                    ControllingPrim.ForceOrientation = m_knownOrientation;
 
                 if ((m_knownChanged & m_knownChangedVelocity) != 0)
                 {
-                    Prim.ForceVelocity = m_knownVelocity;
-                    PhysicsScene.PE.SetInterpolationLinearVelocity(Prim.PhysBody, VehicleVelocity);
+                    ControllingPrim.ForceVelocity = m_knownVelocity;
+                    // Fake out Bullet by making it think the velocity is the same as last time.
+                    // Bullet does a bunch of smoothing for changing parameters.
+                    //    Since the vehicle is demanding this setting, we override Bullet's smoothing
+                    //    by telling Bullet the value was the same last time.
+                    // PhysicsScene.PE.SetInterpolationLinearVelocity(Prim.PhysBody, m_knownVelocity);
                 }
 
                 if ((m_knownChanged & m_knownChangedForce) != 0)
-                    Prim.AddForce((Vector3)m_knownForce, false, true);
+                    ControllingPrim.AddForce((Vector3)m_knownForce, false /*pushForce*/, true /*inTaintTime*/);
+
+                if ((m_knownChanged & m_knownChangedForceImpulse) != 0)
+                    ControllingPrim.AddForceImpulse((Vector3)m_knownForceImpulse, false /*pushforce*/, true /*inTaintTime*/);
 
                 if ((m_knownChanged & m_knownChangedRotationalVelocity) != 0)
                 {
-                    Prim.ForceRotationalVelocity = m_knownRotationalVelocity;
-                    // Fake out Bullet by making it think the velocity is the same as last time.
-                    PhysicsScene.PE.SetInterpolationAngularVelocity(Prim.PhysBody, m_knownRotationalVelocity);
+                    ControllingPrim.ForceRotationalVelocity = m_knownRotationalVelocity;
+                    // PhysicsScene.PE.SetInterpolationAngularVelocity(Prim.PhysBody, m_knownRotationalVelocity);
                 }
 
+                if ((m_knownChanged & m_knownChangedRotationalImpulse) != 0)
+                    ControllingPrim.ApplyTorqueImpulse((Vector3)m_knownRotationalImpulse, true /*inTaintTime*/);
+
                 if ((m_knownChanged & m_knownChangedRotationalForce) != 0)
-                    Prim.AddAngularForce((Vector3)m_knownRotationalForce, false, true);
+                {
+                    ControllingPrim.AddAngularForce((Vector3)m_knownRotationalForce, false /*pushForce*/, true /*inTaintTime*/);
+                }
 
                 // If we set one of the values (ie, the physics engine didn't do it) we must force
                 //      an UpdateProperties event to send the changes up to the simulator.
-                PhysicsScene.PE.PushUpdate(Prim.PhysBody);
+                m_physicsScene.PE.PushUpdate(ControllingPrim.PhysBody);
             }
             m_knownChanged = 0;
         }
 
         // Since the computation of terrain height can be a little involved, this routine
         //    is used to fetch the height only once for each vehicle simulation step.
+        Vector3 lastRememberedHeightPos;
         private float GetTerrainHeight(Vector3 pos)
         {
-            if ((m_knownHas & m_knownChangedTerrainHeight) == 0)
+            if ((m_knownHas & m_knownChangedTerrainHeight) == 0 || pos != lastRememberedHeightPos)
             {
-                m_knownTerrainHeight = Prim.PhysicsScene.TerrainManager.GetTerrainHeightAtXYZ(pos);
+                lastRememberedHeightPos = pos;
+                m_knownTerrainHeight = ControllingPrim.PhysScene.TerrainManager.GetTerrainHeightAtXYZ(pos);
                 m_knownHas |= m_knownChangedTerrainHeight;
             }
             return m_knownTerrainHeight;
@@ -692,7 +793,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         {
             if ((m_knownHas & m_knownChangedWaterLevel) == 0)
             {
-                m_knownWaterLevel = Prim.PhysicsScene.TerrainManager.GetWaterLevelAtXYZ(pos);
+                m_knownWaterLevel = ControllingPrim.PhysScene.TerrainManager.GetWaterLevelAtXYZ(pos);
                 m_knownHas |= m_knownChangedWaterLevel;
             }
             return (float)m_knownWaterLevel;
@@ -704,7 +805,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             {
                 if ((m_knownHas & m_knownChangedPosition) == 0)
                 {
-                    m_knownPosition = Prim.ForcePosition;
+                    m_knownPosition = ControllingPrim.ForcePosition;
                     m_knownHas |= m_knownChangedPosition;
                 }
                 return m_knownPosition;
@@ -723,7 +824,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             {
                 if ((m_knownHas & m_knownChangedOrientation) == 0)
                 {
-                    m_knownOrientation = Prim.ForceOrientation;
+                    m_knownOrientation = ControllingPrim.ForceOrientation;
                     m_knownHas |= m_knownChangedOrientation;
                 }
                 return m_knownOrientation;
@@ -742,10 +843,10 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             {
                 if ((m_knownHas & m_knownChangedVelocity) == 0)
                 {
-                    m_knownVelocity = Prim.ForceVelocity;
+                    m_knownVelocity = ControllingPrim.ForceVelocity;
                     m_knownHas |= m_knownChangedVelocity;
                 }
-                return (Vector3)m_knownVelocity;
+                return m_knownVelocity;
             }
             set
             {
@@ -755,15 +856,26 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             }
         }
 
-        private void VehicleAddForce(Vector3 aForce)
+        private void VehicleAddForce(Vector3 pForce)
         {
             if ((m_knownHas & m_knownChangedForce) == 0)
             {
                 m_knownForce = Vector3.Zero;
+                m_knownHas |= m_knownChangedForce;
             }
-            m_knownForce += aForce;
+            m_knownForce += pForce;
             m_knownChanged |= m_knownChangedForce;
-            m_knownHas |= m_knownChangedForce;
+        }
+
+        private void VehicleAddForceImpulse(Vector3 pImpulse)
+        {
+            if ((m_knownHas & m_knownChangedForceImpulse) == 0)
+            {
+                m_knownForceImpulse = Vector3.Zero;
+                m_knownHas |= m_knownChangedForceImpulse;
+            }
+            m_knownForceImpulse += pImpulse;
+            m_knownChanged |= m_knownChangedForceImpulse;
         }
 
         private Vector3 VehicleRotationalVelocity
@@ -772,7 +884,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             {
                 if ((m_knownHas & m_knownChangedRotationalVelocity) == 0)
                 {
-                    m_knownRotationalVelocity = Prim.ForceRotationalVelocity;
+                    m_knownRotationalVelocity = ControllingPrim.ForceRotationalVelocity;
                     m_knownHas |= m_knownChangedRotationalVelocity;
                 }
                 return (Vector3)m_knownRotationalVelocity;
@@ -794,6 +906,17 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             m_knownChanged |= m_knownChangedRotationalForce;
             m_knownHas |= m_knownChangedRotationalForce;
         }
+        private void VehicleAddRotationalImpulse(Vector3 pImpulse)
+        {
+            if ((m_knownHas & m_knownChangedRotationalImpulse) == 0)
+            {
+                m_knownRotationalImpulse = Vector3.Zero;
+                m_knownHas |= m_knownChangedRotationalImpulse;
+            }
+            m_knownRotationalImpulse += pImpulse;
+            m_knownChanged |= m_knownChangedRotationalImpulse;
+        }
+
         // Vehicle relative forward velocity
         private Vector3 VehicleForwardVelocity
         {
@@ -822,9 +945,6 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         {
             if (!IsActive) return;
 
-            if (PhysicsScene.VehiclePhysicalLoggingEnabled)
-                PhysicsScene.PE.DumpRigidBody(PhysicsScene.World, Prim.PhysBody);
-
             ForgetKnownVehicleProperties();
 
             MoveLinear(pTimestep);
@@ -839,106 +959,116 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             //      for the physics engine to note the changes so an UpdateProperties event will happen.
             PushKnownChanged();
 
-            if (PhysicsScene.VehiclePhysicalLoggingEnabled)
-                PhysicsScene.PE.DumpRigidBody(PhysicsScene.World, Prim.PhysBody);
+            if (m_physicsScene.VehiclePhysicalLoggingEnabled)
+                m_physicsScene.PE.DumpRigidBody(m_physicsScene.World, ControllingPrim.PhysBody);
 
-            VDetailLog("{0},BSDynamics.Step,done,pos={1},force={2},velocity={3},angvel={4}",
-                    Prim.LocalID, VehiclePosition, Prim.Force, VehicleVelocity, VehicleRotationalVelocity);
+            VDetailLog("{0},BSDynamics.Step,done,pos={1}, force={2},velocity={3},angvel={4}",
+                    ControllingPrim.LocalID, VehiclePosition, m_knownForce, VehicleVelocity, VehicleRotationalVelocity);
         }
 
-        // Apply the effect of the linear motor and other linear motions (like hover and float).
-        private void MoveLinear(float pTimestep)
+        // Called after the simulation step
+        internal void PostStep(float pTimestep)
         {
-            Vector3 linearMotorContribution = m_linearMotor.Step(pTimestep);
+            if (!IsActive) return;
 
-            // The movement computed in the linear motor is relative to the vehicle
-            //     coordinates. Rotate the movement to world coordinates.
-            linearMotorContribution *= VehicleOrientation;
+            if (m_physicsScene.VehiclePhysicalLoggingEnabled)
+                m_physicsScene.PE.DumpRigidBody(m_physicsScene.World, ControllingPrim.PhysBody);
+        }
 
-            // ==================================================================
-            // Buoyancy: force to overcome gravity.
-            // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g;
-            // So, if zero, don't change anything (let gravity happen). If one, negate the effect of gravity.
-            Vector3 buoyancyContribution =  Prim.PhysicsScene.DefaultGravity * m_VehicleBuoyancy;
+        // Apply the effect of the linear motor and other linear motions (like hover and float).
+        private void MoveLinear(float pTimestep)
+        {
+            ComputeLinearVelocity(pTimestep);
 
-            Vector3 terrainHeightContribution = ComputeLinearTerrainHeightCorrection(pTimestep);
+            ComputeLinearTerrainHeightCorrection(pTimestep);
 
-            Vector3 hoverContribution = ComputeLinearHover(pTimestep);
+            ComputeLinearHover(pTimestep);
 
             ComputeLinearBlockingEndPoint(pTimestep);
 
-            Vector3 limitMotorUpContribution = ComputeLinearMotorUp(pTimestep);
+            ComputeLinearMotorUp(pTimestep);
 
-            // ==================================================================
-            Vector3 newVelocity = linearMotorContribution
-                            + terrainHeightContribution
-                            + hoverContribution
-                            + limitMotorUpContribution;
-
-            Vector3 newForce = buoyancyContribution;
+            ApplyGravity(pTimestep);
 
             // If not changing some axis, reduce out velocity
-            if ((m_flags & (VehicleFlag.NO_X)) != 0)
-                newVelocity.X = 0;
-            if ((m_flags & (VehicleFlag.NO_Y)) != 0)
-                newVelocity.Y = 0;
-            if ((m_flags & (VehicleFlag.NO_Z)) != 0)
-                newVelocity.Z = 0;
+            if ((m_flags & (VehicleFlag.NO_X | VehicleFlag.NO_Y | VehicleFlag.NO_Z)) != 0)
+            {
+                Vector3 vel = VehicleVelocity;
+                if ((m_flags & (VehicleFlag.NO_X)) != 0)
+                    vel.X = 0;
+                if ((m_flags & (VehicleFlag.NO_Y)) != 0)
+                    vel.Y = 0;
+                if ((m_flags & (VehicleFlag.NO_Z)) != 0)
+                    vel.Z = 0;
+                VehicleVelocity = vel;
+            }
 
             // ==================================================================
             // Clamp high or low velocities
-            float newVelocityLengthSq = newVelocity.LengthSquared();
-            if (newVelocityLengthSq > 1000f)
+            float newVelocityLengthSq = VehicleVelocity.LengthSquared();
+            if (newVelocityLengthSq > BSParam.VehicleMaxLinearVelocitySquared)
             {
-                newVelocity /= newVelocity.Length();
-                newVelocity *= 1000f;
+                Vector3 origVelW = VehicleVelocity;         // DEBUG DEBUG
+                VehicleVelocity /= VehicleVelocity.Length();
+                VehicleVelocity *= BSParam.VehicleMaxLinearVelocity;
+                VDetailLog("{0},  MoveLinear,clampMax,origVelW={1},lenSq={2},maxVelSq={3},,newVelW={4}",
+                            ControllingPrim.LocalID, origVelW, newVelocityLengthSq, BSParam.VehicleMaxLinearVelocitySquared, VehicleVelocity);
             }
             else if (newVelocityLengthSq < 0.001f)
-                newVelocity = Vector3.Zero;
+                VehicleVelocity = Vector3.Zero;
 
-            // ==================================================================
-            // Stuff new linear velocity into the vehicle.
-            // Since the velocity is just being set, it is not scaled by pTimeStep. Bullet will do that for us.
-            VehicleVelocity = newVelocity;
+            VDetailLog("{0},  MoveLinear,done,isColl={1},newVel={2}", ControllingPrim.LocalID, ControllingPrim.IsColliding, VehicleVelocity );
+
+        } // end MoveLinear()
+
+        public void ComputeLinearVelocity(float pTimestep)
+        {
+            // Step the motor from the current value. Get the correction needed this step.
+            Vector3 origVelW = VehicleVelocity;             // DEBUG
+            Vector3 currentVelV = VehicleVelocity * Quaternion.Inverse(VehicleOrientation);
+            Vector3 linearMotorCorrectionV = m_linearMotor.Step(pTimestep, currentVelV);
+
+            // Friction reduces vehicle motion
+            Vector3 frictionFactorW = ComputeFrictionFactor(m_linearFrictionTimescale, pTimestep);
+            linearMotorCorrectionV -= (currentVelV * frictionFactorW);
 
-            // Other linear forces are applied as forces.
-            Vector3 totalDownForce = newForce * m_vehicleMass;
-            if (!totalDownForce.ApproxEquals(Vector3.Zero, 0.01f))
+            // Motor is vehicle coordinates. Rotate it to world coordinates
+            Vector3 linearMotorVelocityW = linearMotorCorrectionV * VehicleOrientation;
+
+            // If we're a ground vehicle, don't add any upward Z movement
+            if ((m_flags & VehicleFlag.LIMIT_MOTOR_UP) != 0)
             {
-                VehicleAddForce(totalDownForce);
+                if (linearMotorVelocityW.Z > 0f)
+                    linearMotorVelocityW.Z = 0f;
             }
 
-            VDetailLog("{0},  MoveLinear,done,newVel={1},totDown={2},IsColliding={3}",
-                                Prim.LocalID, newVelocity, totalDownForce, Prim.IsColliding);
-            VDetailLog("{0},  MoveLinear,done,linContrib={1},terrContrib={2},hoverContrib={3},limitContrib={4},buoyContrib={5}",
-                                Prim.LocalID,
-                                linearMotorContribution, terrainHeightContribution, hoverContribution, 
-                                limitMotorUpContribution, buoyancyContribution
-            );
+            // Add this correction to the velocity to make it faster/slower.
+            VehicleVelocity += linearMotorVelocityW;
 
-        } // end MoveLinear()
 
-        public Vector3 ComputeLinearTerrainHeightCorrection(float pTimestep)
+
+            VDetailLog("{0},  MoveLinear,velocity,origVelW={1},velV={2},correctV={3},correctW={4},newVelW={5},fricFact={6}",
+                        ControllingPrim.LocalID, origVelW, currentVelV, linearMotorCorrectionV,
+                        linearMotorVelocityW, VehicleVelocity, frictionFactorW);
+        }
+
+        public void ComputeLinearTerrainHeightCorrection(float pTimestep)
         {
-            Vector3 ret = Vector3.Zero;
             // If below the terrain, move us above the ground a little.
             // TODO: Consider taking the rotated size of the object or possibly casting a ray.
             if (VehiclePosition.Z < GetTerrainHeight(VehiclePosition))
             {
-                // TODO: correct position by applying force rather than forcing position.
+                // Force position because applying force won't get the vehicle through the terrain
                 Vector3 newPosition = VehiclePosition;
                 newPosition.Z = GetTerrainHeight(VehiclePosition) + 1f;
                 VehiclePosition = newPosition;
                 VDetailLog("{0},  MoveLinear,terrainHeight,terrainHeight={1},pos={2}",
-                        Prim.LocalID, GetTerrainHeight(VehiclePosition), VehiclePosition);
+                        ControllingPrim.LocalID, GetTerrainHeight(VehiclePosition), VehiclePosition);
             }
-            return ret;
         }
 
-        public Vector3 ComputeLinearHover(float pTimestep)
+        public void ComputeLinearHover(float pTimestep)
         {
-            Vector3 ret = Vector3.Zero;
-
             // m_VhoverEfficiency: 0=bouncy, 1=totally damped
             // m_VhoverTimescale: time to achieve height
             if ((m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0)
@@ -963,7 +1093,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     if (VehiclePosition.Z > m_VhoverTargetHeight)
                         m_VhoverTargetHeight = VehiclePosition.Z;
                 }
-                
+
                 if ((m_flags & VehicleFlag.LOCK_HOVER_HEIGHT) != 0)
                 {
                     if (Math.Abs(VehiclePosition.Z - m_VhoverTargetHeight) > 0.2f)
@@ -971,26 +1101,42 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                         Vector3 pos = VehiclePosition;
                         pos.Z = m_VhoverTargetHeight;
                         VehiclePosition = pos;
+
+                        VDetailLog("{0},  MoveLinear,hover,pos={1},lockHoverHeight", ControllingPrim.LocalID, pos);
                     }
                 }
                 else
                 {
                     // Error is positive if below the target and negative if above.
-                    float verticalError = m_VhoverTargetHeight - VehiclePosition.Z;
+                    Vector3 hpos = VehiclePosition;
+                    float verticalError = m_VhoverTargetHeight - hpos.Z;
+                    float verticalCorrection = verticalError / m_VhoverTimescale;
+                    verticalCorrection *= m_VhoverEfficiency;
+
+                    hpos.Z += verticalCorrection;
+                    VehiclePosition = hpos;
+
+                    // Since we are hovering, we need to do the opposite of falling -- get rid of world Z
+                    Vector3 vel = VehicleVelocity;
+                    vel.Z = 0f;
+                    VehicleVelocity = vel;
+
+                    /*
                     float verticalCorrectionVelocity = verticalError / m_VhoverTimescale;
+                    Vector3 verticalCorrection = new Vector3(0f, 0f, verticalCorrectionVelocity);
+                    verticalCorrection *= m_vehicleMass;
 
                     // TODO: implement m_VhoverEfficiency correctly
-                    if (Math.Abs(verticalError) > m_VhoverEfficiency)
-                    {
-                        ret = new Vector3(0f, 0f, verticalCorrectionVelocity);
-                    }
+                    VehicleAddForceImpulse(verticalCorrection);
+                     */
+
+                    VDetailLog("{0},  MoveLinear,hover,pos={1},eff={2},hoverTS={3},height={4},target={5},err={6},corr={7}",
+                                    ControllingPrim.LocalID, VehiclePosition, m_VhoverEfficiency,
+                                    m_VhoverTimescale, m_VhoverHeight, m_VhoverTargetHeight,
+                                    verticalError, verticalCorrection);
                 }
 
-                VDetailLog("{0},  MoveLinear,hover,pos={1},ret={2},hoverTS={3},height={4},target={5}",
-                                Prim.LocalID, VehiclePosition, ret, m_VhoverTimescale, m_VhoverHeight, m_VhoverTargetHeight);
             }
-
-            return ret;
         }
 
         public bool ComputeLinearBlockingEndPoint(float pTimestep)
@@ -1030,7 +1176,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 {
                     VehiclePosition = pos;
                     VDetailLog("{0},  MoveLinear,blockingEndPoint,block={1},origPos={2},pos={3}",
-                                Prim.LocalID, m_BlockingEndPoint, posChange, pos);
+                                ControllingPrim.LocalID, m_BlockingEndPoint, posChange, pos);
                 }
             }
             return changed;
@@ -1041,34 +1187,75 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         //    used with conjunction with banking: the strength of the banking will decay when the
         //    vehicle no longer experiences collisions. The decay timescale is the same as
         //    VEHICLE_BANKING_TIMESCALE. This is to help prevent ground vehicles from steering
-        //    when they are in mid jump. 
+        //    when they are in mid jump.
         // TODO: this code is wrong. Also, what should it do for boats (height from water)?
         //    This is just using the ground and a general collision check. Should really be using
         //    a downward raycast to find what is below.
-        public Vector3 ComputeLinearMotorUp(float pTimestep)
+        public void ComputeLinearMotorUp(float pTimestep)
         {
-            Vector3 ret = Vector3.Zero;
-            float distanceAboveGround = 0f;
-
             if ((m_flags & (VehicleFlag.LIMIT_MOTOR_UP)) != 0)
             {
+                // This code tries to decide if the object is not on the ground and then pushing down
+                /*
                 float targetHeight = Type == Vehicle.TYPE_BOAT ? GetWaterLevel(VehiclePosition) : GetTerrainHeight(VehiclePosition);
                 distanceAboveGround = VehiclePosition.Z - targetHeight;
                 // Not colliding if the vehicle is off the ground
                 if (!Prim.IsColliding)
                 {
                     // downForce = new Vector3(0, 0, -distanceAboveGround / m_bankingTimescale);
-                    ret = new Vector3(0, 0, -distanceAboveGround);
+                    VehicleVelocity += new Vector3(0, 0, -distanceAboveGround);
                 }
                 // TODO: this calculation is wrong. From the description at
                 //     (http://wiki.secondlife.com/wiki/Category:LSL_Vehicle), the downForce
                 //     has a decay factor. This says this force should
                 //     be computed with a motor.
                 // TODO: add interaction with banking.
-            }
-            VDetailLog("{0},  MoveLinear,limitMotorUp,distAbove={1},colliding={2},ret={3}",
+                VDetailLog("{0},  MoveLinear,limitMotorUp,distAbove={1},colliding={2},ret={3}",
                                 Prim.LocalID, distanceAboveGround, Prim.IsColliding, ret);
-            return ret;
+                 */
+
+                // Another approach is to measure if we're going up. If going up and not colliding,
+                //     the vehicle is in the air.  Fix that by pushing down.
+                if (!ControllingPrim.IsColliding && VehicleVelocity.Z > 0.1)
+                {
+                    // Get rid of any of the velocity vector that is pushing us up.
+                    float upVelocity = VehicleVelocity.Z;
+                    VehicleVelocity += new Vector3(0, 0, -upVelocity);
+
+                    /*
+                    // If we're pointed up into the air, we should nose down
+                    Vector3 pointingDirection = Vector3.UnitX * VehicleOrientation;
+                    // The rotation around the Y axis is pitch up or down
+                    if (pointingDirection.Y > 0.01f)
+                    {
+                        float angularCorrectionForce = -(float)Math.Asin(pointingDirection.Y);
+                        Vector3 angularCorrectionVector = new Vector3(0f, angularCorrectionForce, 0f);
+                        // Rotate into world coordinates and apply to vehicle
+                        angularCorrectionVector *= VehicleOrientation;
+                        VehicleAddAngularForce(angularCorrectionVector);
+                        VDetailLog("{0},  MoveLinear,limitMotorUp,newVel={1},pntDir={2},corrFrc={3},aCorr={4}",
+                                    Prim.LocalID, VehicleVelocity, pointingDirection, angularCorrectionForce, angularCorrectionVector);
+                    }
+                        */
+                    VDetailLog("{0},  MoveLinear,limitMotorUp,collide={1},upVel={2},newVel={3}",
+                                    ControllingPrim.LocalID, ControllingPrim.IsColliding, upVelocity, VehicleVelocity);
+                }
+            }
+        }
+
+        private void ApplyGravity(float pTimeStep)
+        {
+            Vector3 appliedGravity = m_VehicleGravity * m_vehicleMass;
+
+            // Hack to reduce downward force if the vehicle is probably sitting on the ground
+            if (ControllingPrim.IsColliding && IsGroundVehicle)
+                appliedGravity *= BSParam.VehicleGroundGravityFudge;
+
+            VehicleAddForce(appliedGravity);
+
+            VDetailLog("{0},  MoveLinear,applyGravity,vehGrav={1},collid={2},fudge={3},mass={4},appliedForce={3}",
+                            ControllingPrim.LocalID, m_VehicleGravity,
+                            ControllingPrim.IsColliding, BSParam.VehicleGroundGravityFudge, m_vehicleMass, appliedGravity);
         }
 
         // =======================================================================
@@ -1079,55 +1266,24 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         //     set directly on the vehicle.
         private void MoveAngular(float pTimestep)
         {
-            // The user wants this many radians per second angular change?
-            Vector3 angularMotorContribution = m_angularMotor.Step(pTimestep);
+            ComputeAngularTurning(pTimestep);
 
-            // ==================================================================
-            // From http://wiki.secondlife.com/wiki/LlSetVehicleFlags :
-            //    This flag prevents linear deflection parallel to world z-axis. This is useful
-            //    for preventing ground vehicles with large linear deflection, like bumper cars,
-            //    from climbing their linear deflection into the sky. 
-            // That is, NO_DEFLECTION_UP says angular motion should not add any pitch or roll movement
-            if ((m_flags & (VehicleFlag.NO_DEFLECTION_UP)) != 0)
-            {
-                angularMotorContribution.X = 0f;
-                angularMotorContribution.Y = 0f;
-                VDetailLog("{0},  MoveAngular,noDeflectionUp,angularMotorContrib={1}", Prim.LocalID, angularMotorContribution);
-            }
-
-            Vector3 verticalAttractionContribution = ComputeAngularVerticalAttraction();
+            ComputeAngularVerticalAttraction();
 
-            Vector3 deflectionContribution = ComputeAngularDeflection();
+            ComputeAngularDeflection();
 
-            Vector3 bankingContribution = ComputeAngularBanking();
+            ComputeAngularBanking();
 
             // ==================================================================
-            m_lastVertAttractor = verticalAttractionContribution;
-
-            m_lastAngularVelocity = angularMotorContribution
-                                    + verticalAttractionContribution
-                                    + deflectionContribution
-                                    + bankingContribution;
-
-            // ==================================================================
-            // Apply the correction velocity.
-            // TODO: Should this be applied as an angular force (torque)?
-            if (!m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f))
+            if (VehicleRotationalVelocity.ApproxEquals(Vector3.Zero, 0.0001f))
             {
-                VehicleRotationalVelocity = m_lastAngularVelocity;
-
-                VDetailLog("{0},  MoveAngular,done,nonZero,angMotorContrib={1},vertAttrContrib={2},bankContrib={3},deflectContrib={4},totalContrib={5}",
-                                    Prim.LocalID,
-                                    angularMotorContribution, verticalAttractionContribution,
-                                    bankingContribution, deflectionContribution,
-                                    m_lastAngularVelocity
-                                    );
+                // The vehicle is not adding anything angular wise.
+                VehicleRotationalVelocity = Vector3.Zero;
+                VDetailLog("{0},  MoveAngular,done,zero", ControllingPrim.LocalID);
             }
             else
             {
-                // The vehicle is not adding anything angular wise.
-                VehicleRotationalVelocity = Vector3.Zero;
-                VDetailLog("{0},  MoveAngular,done,zero", Prim.LocalID);
+                VDetailLog("{0},  MoveAngular,done,nonZero,angVel={1}", ControllingPrim.LocalID, VehicleRotationalVelocity);
             }
 
             // ==================================================================
@@ -1158,10 +1314,42 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     torqueFromOffset.Z = 0;
 
                 VehicleAddAngularForce(torqueFromOffset * m_vehicleMass);
-                VDetailLog("{0},  BSDynamic.MoveAngular,motorOffset,applyTorqueImpulse={1}", Prim.LocalID, torqueFromOffset);
+                VDetailLog("{0},  BSDynamic.MoveAngular,motorOffset,applyTorqueImpulse={1}", ControllingPrim.LocalID, torqueFromOffset);
             }
 
         }
+
+        private void ComputeAngularTurning(float pTimestep)
+        {
+            // The user wants this many radians per second angular change?
+            Vector3 currentAngularV = VehicleRotationalVelocity * Quaternion.Inverse(VehicleOrientation);
+            Vector3 angularMotorContributionV = m_angularMotor.Step(pTimestep, currentAngularV);
+
+            // ==================================================================
+            // From http://wiki.secondlife.com/wiki/LlSetVehicleFlags :
+            //    This flag prevents linear deflection parallel to world z-axis. This is useful
+            //    for preventing ground vehicles with large linear deflection, like bumper cars,
+            //    from climbing their linear deflection into the sky.
+            // That is, NO_DEFLECTION_UP says angular motion should not add any pitch or roll movement
+            // TODO: This is here because this is where ODE put it but documentation says it
+            //    is a linear effect. Where should this check go?
+            //if ((m_flags & (VehicleFlag.NO_DEFLECTION_UP)) != 0)
+           // {
+            //    angularMotorContributionV.X = 0f;
+            //    angularMotorContributionV.Y = 0f;
+          //  }
+
+            // Reduce any velocity by friction.
+            Vector3 frictionFactorW = ComputeFrictionFactor(m_angularFrictionTimescale, pTimestep);
+            angularMotorContributionV -= (currentAngularV * frictionFactorW);
+
+            VehicleRotationalVelocity += angularMotorContributionV * VehicleOrientation;
+
+
+
+            VDetailLog("{0},  MoveAngular,angularTurning,angContribV={1}", ControllingPrim.LocalID, angularMotorContributionV);
+        }
+
         // From http://wiki.secondlife.com/wiki/Linden_Vehicle_Tutorial:
         //      Some vehicles, like boats, should always keep their up-side up. This can be done by
         //      enabling the "vertical attractor" behavior that springs the vehicle's local z-axis to
@@ -1170,15 +1358,84 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         //      and then set the VEHICLE_VERTICAL_ATTRACTION_EFFICIENCY to control the damping. An
         //      efficiency of 0.0 will cause the spring to wobble around its equilibrium, while an
         //      efficiency of 1.0 will cause the spring to reach its equilibrium with exponential decay.
-        public Vector3 ComputeAngularVerticalAttraction()
+        public void ComputeAngularVerticalAttraction()
         {
-            Vector3 ret = Vector3.Zero;
 
             // If vertical attaction timescale is reasonable
-            if (m_verticalAttractionTimescale < m_verticalAttractionCutoff)
+            if (enableAngularVerticalAttraction && m_verticalAttractionTimescale < m_verticalAttractionCutoff)
             {
+                //Another formula to try got from :
+                //http://answers.unity3d.com/questions/10425/how-to-stabilize-angular-motion-alignment-of-hover.html
+
+                Vector3 VehicleUpAxis = Vector3.UnitZ * VehicleOrientation;
+
+                // Flipping what was originally a timescale into a speed variable and then multiplying it by 2
+                //    since only computing half the distance between the angles.
+                float VerticalAttractionSpeed = (1 / m_verticalAttractionTimescale) * 2.0f;
+
+                // Make a prediction of where the up axis will be when this is applied rather then where it is now as
+                //     this makes for a smoother adjustment and less fighting between the various forces.
+                Vector3 predictedUp = VehicleUpAxis * Quaternion.CreateFromAxisAngle(VehicleRotationalVelocity, 0f);
+
+                // This is only half the distance to the target so it will take 2 seconds to complete the turn.
+                Vector3 torqueVector = Vector3.Cross(predictedUp, Vector3.UnitZ);
+
+                // Scale vector by our timescale since it is an acceleration it is r/s^2 or radians a timescale squared
+                Vector3 vertContributionV = torqueVector * VerticalAttractionSpeed * VerticalAttractionSpeed;
+
+                VehicleRotationalVelocity += vertContributionV;
+
+                VDetailLog("{0},  MoveAngular,verticalAttraction,UpAxis={1},PredictedUp={2},torqueVector={3},contrib={4}",
+                                ControllingPrim.LocalID,
+                                VehicleUpAxis,
+                                predictedUp,
+                                torqueVector,
+                                vertContributionV);
+                //=====================================================================
+                /*
+                // Possible solution derived from a discussion at:
+                // http://stackoverflow.com/questions/14939657/computing-vector-from-quaternion-works-computing-quaternion-from-vector-does-no
+
+                // Create a rotation that is only the vehicle's rotation around Z
+                Vector3 currentEuler = Vector3.Zero;
+                VehicleOrientation.GetEulerAngles(out currentEuler.X, out currentEuler.Y, out currentEuler.Z);
+                Quaternion justZOrientation = Quaternion.CreateFromAxisAngle(Vector3.UnitZ, currentEuler.Z);
+
+                // Create the axis that is perpendicular to the up vector and the rotated up vector.
+                Vector3 differenceAxis = Vector3.Cross(Vector3.UnitZ * justZOrientation, Vector3.UnitZ * VehicleOrientation);
+                // Compute the angle between those to vectors.
+                double differenceAngle = Math.Acos((double)Vector3.Dot(Vector3.UnitZ, Vector3.Normalize(Vector3.UnitZ * VehicleOrientation)));
+                // 'differenceAngle' is the angle to rotate and 'differenceAxis' is the plane to rotate in to get the vehicle vertical
+
+                // Reduce the change by the time period it is to change in. Timestep is handled when velocity is applied.
+                // TODO: add 'efficiency'.
+                differenceAngle /= m_verticalAttractionTimescale;
+
+                // Create the quaterian representing the correction angle
+                Quaternion correctionRotation = Quaternion.CreateFromAxisAngle(differenceAxis, (float)differenceAngle);
+
+                // Turn that quaternion into Euler values to make it into velocities to apply.
+                Vector3 vertContributionV = Vector3.Zero;
+                correctionRotation.GetEulerAngles(out vertContributionV.X, out vertContributionV.Y, out vertContributionV.Z);
+                vertContributionV *= -1f;
+
+                VehicleRotationalVelocity += vertContributionV;
+
+                VDetailLog("{0},  MoveAngular,verticalAttraction,diffAxis={1},diffAng={2},corrRot={3},contrib={4}",
+                                ControllingPrim.LocalID,
+                                differenceAxis,
+                                differenceAngle,
+                                correctionRotation,
+                                vertContributionV);
+                 */
+
+                // ===================================================================
+                /*
+                Vector3 vertContributionV = Vector3.Zero;
+                Vector3 origRotVelW = VehicleRotationalVelocity;        // DEBUG DEBUG
+
                 // Take a vector pointing up and convert it from world to vehicle relative coords.
-                Vector3 verticalError = Vector3.UnitZ * VehicleOrientation;
+                Vector3 verticalError = Vector3.Normalize(Vector3.UnitZ * VehicleOrientation);
 
                 // If vertical attraction correction is needed, the vector that was pointing up (UnitZ)
                 //    is now:
@@ -1190,49 +1447,57 @@ namespace OpenSim.Region.Physics.BulletSPlugin
 
                 // Y error means needed rotation around X axis and visa versa.
                 // Since the error goes from zero to one, the asin is the corresponding angle.
-                ret.X = (float)Math.Asin(verticalError.Y);
+                vertContributionV.X = (float)Math.Asin(verticalError.Y);
                 // (Tilt forward (positive X) needs to tilt back (rotate negative) around Y axis.)
-                ret.Y = -(float)Math.Asin(verticalError.X);
+                vertContributionV.Y = -(float)Math.Asin(verticalError.X);
 
                 // If verticalError.Z is negative, the vehicle is upside down. Add additional push.
                 if (verticalError.Z < 0f)
                 {
-                    ret.X += PIOverFour;
-                    ret.Y += PIOverFour;
+                    vertContributionV.X += Math.Sign(vertContributionV.X) * PIOverFour;
+                    // vertContribution.Y -= PIOverFour;
                 }
 
-                // 'ret' is now the necessary velocity to correct tilt in one second.
+                // 'vertContrbution' is now the necessary angular correction to correct tilt in one second.
                 //     Correction happens over a number of seconds.
-                Vector3 unscaledContrib = ret;
-                ret /= m_verticalAttractionTimescale;
+                Vector3 unscaledContribVerticalErrorV = vertContributionV;     // DEBUG DEBUG
+
+                // The correction happens over the user's time period
+                vertContributionV /= m_verticalAttractionTimescale;
 
-                VDetailLog("{0},  MoveAngular,verticalAttraction,,verticalError={1},unscaled={2},eff={3},ts={4},vertAttr={5}",
-                                Prim.LocalID, verticalError, unscaledContrib, m_verticalAttractionEfficiency, m_verticalAttractionTimescale, ret);
+                // Rotate the vehicle rotation to the world coordinates.
+                VehicleRotationalVelocity += (vertContributionV * VehicleOrientation);
+
+                VDetailLog("{0},  MoveAngular,verticalAttraction,,origRotVW={1},vertError={2},unscaledV={3},eff={4},ts={5},vertContribV={6}",
+                                Prim.LocalID, origRotVelW, verticalError, unscaledContribVerticalErrorV,
+                                m_verticalAttractionEfficiency, m_verticalAttractionTimescale, vertContributionV);
+                */
             }
-            return ret;
         }
 
-        // Return the angular correction to correct the direction the vehicle is pointing to be
+        // Angular correction to correct the direction the vehicle is pointing to be
         //      the direction is should want to be pointing.
         // The vehicle is moving in some direction and correct its orientation to it is pointing
         //     in that direction.
         // TODO: implement reference frame.
-        public Vector3 ComputeAngularDeflection()
+        public void ComputeAngularDeflection()
         {
-            Vector3 ret = Vector3.Zero;
-            return ret;         // DEBUG DEBUG DEBUG
-            // Disable angular deflection for the moment.
             // Since angularMotorUp and angularDeflection are computed independently, they will calculate
             //     approximately the same X or Y correction. When added together (when contributions are combined)
             //     this creates an over-correction and then wabbling as the target is overshot.
             // TODO: rethink how the different correction computations inter-relate.
 
-            if (m_angularDeflectionEfficiency != 0)
+            if (enableAngularDeflection && m_angularDeflectionEfficiency != 0 && VehicleForwardSpeed > 0.2)
             {
+                Vector3 deflectContributionV = Vector3.Zero;
+
                 // The direction the vehicle is moving
                 Vector3 movingDirection = VehicleVelocity;
                 movingDirection.Normalize();
 
+                // If the vehicle is going backward, it is still pointing forward
+                movingDirection *= Math.Sign(VehicleForwardSpeed);
+
                 // The direction the vehicle is pointing
                 Vector3 pointingDirection = Vector3.UnitX * VehicleOrientation;
                 pointingDirection.Normalize();
@@ -1241,6 +1506,9 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 Vector3 deflectionError = movingDirection - pointingDirection;
 
                 // Don't try to correct very large errors (not our job)
+                // if (Math.Abs(deflectionError.X) > PIOverFour) deflectionError.X = PIOverTwo * Math.Sign(deflectionError.X);
+                // if (Math.Abs(deflectionError.Y) > PIOverFour) deflectionError.Y = PIOverTwo * Math.Sign(deflectionError.Y);
+                // if (Math.Abs(deflectionError.Z) > PIOverFour) deflectionError.Z = PIOverTwo * Math.Sign(deflectionError.Z);
                 if (Math.Abs(deflectionError.X) > PIOverFour) deflectionError.X = 0f;
                 if (Math.Abs(deflectionError.Y) > PIOverFour) deflectionError.Y = 0f;
                 if (Math.Abs(deflectionError.Z) > PIOverFour) deflectionError.Z = 0f;
@@ -1248,18 +1516,19 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 // ret = m_angularDeflectionCorrectionMotor(1f, deflectionError);
 
                 // Scale the correction by recovery timescale and efficiency
-                ret = (-deflectionError) * m_angularDeflectionEfficiency;
-                ret /= m_angularDeflectionTimescale;
+                deflectContributionV = (-deflectionError) * m_angularDeflectionEfficiency;
+                deflectContributionV /= m_angularDeflectionTimescale;
+
+                VehicleRotationalVelocity += deflectContributionV * VehicleOrientation;
 
                 VDetailLog("{0},  MoveAngular,Deflection,movingDir={1},pointingDir={2},deflectError={3},ret={4}",
-                    Prim.LocalID, movingDirection, pointingDirection, deflectionError, ret);
+                    ControllingPrim.LocalID, movingDirection, pointingDirection, deflectionError, deflectContributionV);
                 VDetailLog("{0},  MoveAngular,Deflection,fwdSpd={1},defEff={2},defTS={3}",
-                    Prim.LocalID, VehicleForwardSpeed, m_angularDeflectionEfficiency, m_angularDeflectionTimescale);
+                    ControllingPrim.LocalID, VehicleForwardSpeed, m_angularDeflectionEfficiency, m_angularDeflectionTimescale);
             }
-            return ret;
         }
 
-        // Return an angular change to rotate the vehicle around the Z axis when the vehicle
+        // Angular change to rotate the vehicle around the Z axis when the vehicle
         //     is tipped around the X axis.
         // From http://wiki.secondlife.com/wiki/Linden_Vehicle_Tutorial:
         //      The vertical attractor feature must be enabled in order for the banking behavior to
@@ -1267,13 +1536,13 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         //      produce a angular velocity around the yaw-axis, causing the vehicle to turn. The magnitude
         //      of the yaw effect will be proportional to the
         //          VEHICLE_BANKING_EFFICIENCY, the angle of the roll rotation, and sometimes the vehicle's
-        //                 velocity along its preferred axis of motion. 
+        //                 velocity along its preferred axis of motion.
         //          The VEHICLE_BANKING_EFFICIENCY can vary between -1 and +1. When it is positive then any
         //                  positive rotation (by the right-hand rule) about the roll-axis will effect a
         //                  (negative) torque around the yaw-axis, making it turn to the right--that is the
         //                  vehicle will lean into the turn, which is how real airplanes and motorcycle's work.
         //                  Negating the banking coefficient will make it so that the vehicle leans to the
-        //                  outside of the turn (not very "physical" but might allow interesting vehicles so why not?). 
+        //                  outside of the turn (not very "physical" but might allow interesting vehicles so why not?).
         //           The VEHICLE_BANKING_MIX is a fake (i.e. non-physical) parameter that is useful for making
         //                  banking vehicles do what you want rather than what the laws of physics allow.
         //                  For example, consider a real motorcycle...it must be moving forward in order for
@@ -1285,46 +1554,44 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         //                  totally static (0.0) and totally dynamic (1.0). By "static" we mean that the
         //                  banking effect depends only on the vehicle's rotation about its roll-axis compared
         //                  to "dynamic" where the banking is also proportional to its velocity along its
-        //                  roll-axis. Finding the best value of the "mixture" will probably require trial and error. 
+        //                  roll-axis. Finding the best value of the "mixture" will probably require trial and error.
         //      The time it takes for the banking behavior to defeat a preexisting angular velocity about the
         //      world z-axis is determined by the VEHICLE_BANKING_TIMESCALE. So if you want the vehicle to
         //      bank quickly then give it a banking timescale of about a second or less, otherwise you can
-        //      make a sluggish vehicle by giving it a timescale of several seconds. 
-        public Vector3 ComputeAngularBanking()
+        //      make a sluggish vehicle by giving it a timescale of several seconds.
+        public void ComputeAngularBanking()
         {
-            Vector3 ret = Vector3.Zero;
-
-            if (m_bankingEfficiency != 0 && m_verticalAttractionTimescale < m_verticalAttractionCutoff)
+            if (enableAngularBanking && m_bankingEfficiency != 0 && m_verticalAttractionTimescale < m_verticalAttractionCutoff)
             {
-                // This works by rotating a unit vector to the orientation of the vehicle. The
-                //    roll (tilt) will be Y component of a tilting Z vector (zero for no tilt
-                //    up to one for full over).
+                Vector3 bankingContributionV = Vector3.Zero;
+
+                // Rotate a UnitZ vector (pointing up) to how the vehicle is oriented.
+                // As the vehicle rolls to the right or left, the Y value will increase from
+                //     zero (straight up) to 1 or -1 (full tilt right  or left)
                 Vector3 rollComponents = Vector3.UnitZ * VehicleOrientation;
 
                 // Figure out the yaw value for this much roll.
-                float turnComponent = rollComponents.Y * rollComponents.Y * m_bankingEfficiency;
-                // Keep the sign
-                if (rollComponents.Y < 0f)
-                    turnComponent = -turnComponent;
-
-                // TODO: there must be a better computation of the banking force.
-                float bankingTurnForce = turnComponent;
-
+                float yawAngle = m_angularMotorDirection.X * m_bankingEfficiency;
                 //        actual error  =       static turn error            +           dynamic turn error
-                float mixedBankingError = bankingTurnForce * (1f - m_bankingMix) + bankingTurnForce * m_bankingMix * VehicleForwardSpeed;
+                float mixedYawAngle =(yawAngle * (1f - m_bankingMix)) + ((yawAngle * m_bankingMix) * VehicleForwardSpeed);
+
                 // TODO: the banking effect should not go to infinity but what to limit it to?
-                mixedBankingError = ClampInRange(-20f, mixedBankingError, 20f);
+                //     And what should happen when this is being added to a user defined yaw that is already PI*4?
+                mixedYawAngle = ClampInRange(-12, mixedYawAngle, 12);
 
                 // Build the force vector to change rotation from what it is to what it should be
-                ret.Z = -mixedBankingError;
+                bankingContributionV.Z = -mixedYawAngle;
+
+                // Don't do it all at once. Fudge because 1 second is too fast with most user defined roll as PI*4.
+                bankingContributionV /= m_bankingTimescale * BSParam.VehicleAngularBankingTimescaleFudge;
 
-                // Don't do it all at once.
-                ret /= m_bankingTimescale;
+                //VehicleRotationalVelocity += bankingContributionV * VehicleOrientation;
+                VehicleRotationalVelocity += bankingContributionV;
 
-                VDetailLog("{0},  MoveAngular,Banking,rollComp={1},speed={2},turnComp={3},bankErr={4},mixedBankErr={5},ret={6}",
-                            Prim.LocalID, rollComponents, VehicleForwardSpeed, turnComponent, bankingTurnForce, mixedBankingError, ret);
+
+                VDetailLog("{0},  MoveAngular,Banking,rollComp={1},speed={2},rollComp={3},yAng={4},mYAng={5},ret={6}",
+                            ControllingPrim.LocalID, rollComponents, VehicleForwardSpeed, rollComponents, yawAngle, mixedYawAngle, bankingContributionV);
             }
-            return ret;
         }
 
         // This is from previous instantiations of XXXDynamics.cs.
@@ -1362,11 +1629,28 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             if (rotq != m_rot)
             {
                 VehicleOrientation = m_rot;
-                VDetailLog("{0},  LimitRotation,done,orig={1},new={2}", Prim.LocalID, rotq, m_rot);
+                VDetailLog("{0},  LimitRotation,done,orig={1},new={2}", ControllingPrim.LocalID, rotq, m_rot);
             }
 
         }
 
+        // Given a friction vector (reduction in seconds) and a timestep, return the factor to reduce
+        //     some value by to apply this friction.
+        private Vector3 ComputeFrictionFactor(Vector3 friction, float pTimestep)
+        {
+            Vector3 frictionFactor = Vector3.Zero;
+            if (friction != BSMotor.InfiniteVector)
+            {
+                // frictionFactor = (Vector3.One / FrictionTimescale) * timeStep;
+                // Individual friction components can be 'infinite' so compute each separately.
+                frictionFactor.X = (friction.X == BSMotor.Infinite) ? 0f : (1f / friction.X);
+                frictionFactor.Y = (friction.Y == BSMotor.Infinite) ? 0f : (1f / friction.Y);
+                frictionFactor.Z = (friction.Z == BSMotor.Infinite) ? 0f : (1f / friction.Z);
+                frictionFactor *= pTimestep;
+            }
+            return frictionFactor;
+        }
+
         private float ClampInRange(float low, float val, float high)
         {
             return Math.Max(low, Math.Min(val, high));
@@ -1376,8 +1660,8 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         // Invoke the detailed logger and output something if it's enabled.
         private void VDetailLog(string msg, params Object[] args)
         {
-            if (Prim.PhysicsScene.VehicleLoggingEnabled)
-                Prim.PhysicsScene.DetailLog(msg, args);
+            if (ControllingPrim.PhysScene.VehicleLoggingEnabled)
+                ControllingPrim.PhysScene.DetailLog(msg, args);
         }
     }
 }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
index 756faed..76c2187 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
@@ -52,7 +52,7 @@ public abstract class BSLinkset
         Manual       = 2    // linkset tied together manually (code moves all the pieces)
     }
     // Create the correct type of linkset for this child
-    public static BSLinkset Factory(BSScene physScene, BSPhysObject parent)
+    public static BSLinkset Factory(BSScene physScene, BSPrimLinkable parent)
     {
         BSLinkset ret = null;
 
@@ -71,31 +71,28 @@ public abstract class BSLinkset
                 ret = new BSLinksetCompound(physScene, parent);
                 break;
         }
+        if (ret == null)
+        {
+            physScene.Logger.ErrorFormat("[BULLETSIM LINKSET] Factory could not create linkset. Parent name={1}, ID={2}", parent.Name, parent.LocalID);
+        }
         return ret;
     }
 
-    public BSPhysObject LinksetRoot { get; protected set; }
+    public BSPrimLinkable LinksetRoot { get; protected set; }
 
-    public BSScene PhysicsScene { get; private set; }
+    protected BSScene m_physicsScene { get; private set; }
 
     static int m_nextLinksetID = 1;
     public int LinksetID { get; private set; }
 
     // The children under the root in this linkset.
-    protected HashSet<BSPhysObject> m_children;
+    protected HashSet<BSPrimLinkable> 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.
     protected object m_linksetActivityLock = new Object();
 
-    // Some linksets have a preferred physical shape.
-    // Returns SHAPE_UNKNOWN if there is no preference. Causes the correct shape to be selected.
-    public virtual BSPhysicsShapeType PreferredPhysicalShape(BSPhysObject requestor)
-    {
-        return BSPhysicsShapeType.SHAPE_UNKNOWN;
-    }
-
     // We keep the prim's mass in the linkset structure since it could be dependent on other prims
     public float LinksetMass { get; protected set; }
 
@@ -111,25 +108,27 @@ public abstract class BSLinkset
         get { return ComputeLinksetGeometricCenter(); }
     }
 
-    protected BSLinkset(BSScene scene, BSPhysObject parent)
+    protected BSLinkset(BSScene scene, BSPrimLinkable parent)
     {
         // A simple linkset of one (no children)
         LinksetID = m_nextLinksetID++;
         // We create LOTS of linksets.
         if (m_nextLinksetID <= 0)
             m_nextLinksetID = 1;
-        PhysicsScene = scene;
+        m_physicsScene = scene;
         LinksetRoot = parent;
-        m_children = new HashSet<BSPhysObject>();
+        m_children = new HashSet<BSPrimLinkable>();
         LinksetMass = parent.RawMass;
         Rebuilding = false;
+
+        parent.ClearDisplacement();
     }
 
     // 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 its membership.
     // Called at runtime.
-    public BSLinkset AddMeToLinkset(BSPhysObject child)
+    public BSLinkset AddMeToLinkset(BSPrimLinkable child)
     {
         lock (m_linksetActivityLock)
         {
@@ -145,7 +144,7 @@ public abstract class BSLinkset
     // Returns a new linkset for the child which is a linkset of one (just the
     //    orphened child).
     // Called at runtime.
-    public BSLinkset RemoveMeFromLinkset(BSPhysObject child)
+    public BSLinkset RemoveMeFromLinkset(BSPrimLinkable child)
     {
         lock (m_linksetActivityLock)
         {
@@ -159,11 +158,11 @@ public abstract class BSLinkset
         }
 
         // The child is down to a linkset of just itself
-        return BSLinkset.Factory(PhysicsScene, child);
+        return BSLinkset.Factory(m_physicsScene, child);
     }
 
     // Return 'true' if the passed object is the root object of this linkset
-    public bool IsRoot(BSPhysObject requestor)
+    public bool IsRoot(BSPrimLinkable requestor)
     {
         return (requestor.LocalID == LinksetRoot.LocalID);
     }
@@ -174,14 +173,14 @@ public abstract class BSLinkset
     public bool HasAnyChildren { get { return (m_children.Count > 0); } }
 
     // Return 'true' if this child is in this linkset
-    public bool HasChild(BSPhysObject child)
+    public bool HasChild(BSPrimLinkable child)
     {
         bool ret = false;
         lock (m_linksetActivityLock)
         {
             ret = m_children.Contains(child);
             /* Safer version but the above should work
-            foreach (BSPhysObject bp in m_children)
+            foreach (BSPrimLinkable bp in m_children)
             {
                 if (child.LocalID == bp.LocalID)
                 {
@@ -196,14 +195,14 @@ public abstract class BSLinkset
 
     // Perform an action on each member of the linkset including root prim.
     // Depends on the action on whether this should be done at taint time.
-    public delegate bool ForEachMemberAction(BSPhysObject obj);
+    public delegate bool ForEachMemberAction(BSPrimLinkable obj);
     public virtual bool ForEachMember(ForEachMemberAction action)
     {
         bool ret = false;
         lock (m_linksetActivityLock)
         {
             action(LinksetRoot);
-            foreach (BSPhysObject po in m_children)
+            foreach (BSPrimLinkable po in m_children)
             {
                 if (action(po))
                     break;
@@ -214,16 +213,16 @@ public abstract class BSLinkset
 
     // I am the root of a linkset and a new child is being added
     // Called while LinkActivity is locked.
-    protected abstract void AddChildToLinkset(BSPhysObject child);
-    
+    protected abstract void AddChildToLinkset(BSPrimLinkable child);
+
     // 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.
-    protected abstract void RemoveChildFromLinkset(BSPhysObject child);
+    protected abstract void RemoveChildFromLinkset(BSPrimLinkable child);
 
     // When physical properties are changed the linkset needs to recalculate
     //   its internal properties.
     // May be called at runtime or taint-time.
-    public virtual void Refresh(BSPhysObject requestor)
+    public virtual void Refresh(BSPrimLinkable requestor)
     {
         LinksetMass = ComputeLinksetMass();
     }
@@ -238,31 +237,26 @@ public abstract class BSLinkset
     //     has not yet been fully constructed.
     // Return 'true' if any properties updated on the passed object.
     // Called at taint-time!
-    public abstract bool MakeDynamic(BSPhysObject child);
+    public abstract bool MakeDynamic(BSPrimLinkable child);
 
     // The object is going static (non-physical). Do any setup necessary
     //     for a static linkset.
     // Return 'true' if any properties updated on the passed object.
     // Called at taint-time!
-    public abstract bool MakeStatic(BSPhysObject child);
+    public abstract bool MakeStatic(BSPrimLinkable child);
 
     // Called when a parameter update comes from the physics engine for any object
     //      of the linkset is received.
     // Passed flag is update came from physics engine (true) or the user (false).
     // Called at taint-time!!
-    public abstract void UpdateProperties(BSPhysObject physObject, bool physicalUpdate);
+    public abstract void UpdateProperties(UpdatedProperties whichUpdated, BSPrimLinkable physObject);
 
     // Routine used when rebuilding the body of the root of the linkset
     // Destroy all the constraints have have been made to root.
     // This is called when the root body is changing.
     // Returns 'true' of something was actually removed and would need restoring
     // Called at taint-time!!
-    public abstract bool RemoveBodyDependencies(BSPrim child);
-
-    // Companion to RemoveBodyDependencies(). If RemoveBodyDependencies() returns 'true',
-    //     this routine will restore the removed constraints.
-    // Called at taint-time!!
-    public abstract void RestoreBodyDependencies(BSPrim child);
+    public abstract bool RemoveDependencies(BSPrimLinkable child);
 
     // ================================================================
     protected virtual float ComputeLinksetMass()
@@ -272,7 +266,7 @@ public abstract class BSLinkset
         {
             lock (m_linksetActivityLock)
             {
-                foreach (BSPhysObject bp in m_children)
+                foreach (BSPrimLinkable bp in m_children)
                 {
                     mass += bp.RawMass;
                 }
@@ -281,6 +275,7 @@ public abstract class BSLinkset
         return mass;
     }
 
+    // Computes linkset's center of mass in world coordinates.
     protected virtual OMV.Vector3 ComputeLinksetCenterOfMass()
     {
         OMV.Vector3 com;
@@ -289,7 +284,7 @@ public abstract class BSLinkset
             com = LinksetRoot.Position * LinksetRoot.RawMass;
             float totalMass = LinksetRoot.RawMass;
 
-            foreach (BSPhysObject bp in m_children)
+            foreach (BSPrimLinkable bp in m_children)
             {
                 com += bp.Position * bp.RawMass;
                 totalMass += bp.RawMass;
@@ -308,9 +303,9 @@ public abstract class BSLinkset
         {
             com = LinksetRoot.Position;
 
-            foreach (BSPhysObject bp in m_children)
+            foreach (BSPrimLinkable bp in m_children)
             {
-                com += bp.Position * bp.RawMass;
+                com += bp.Position;
             }
             com /= (m_children.Count + 1);
         }
@@ -321,8 +316,8 @@ public abstract class BSLinkset
     // Invoke the detailed logger and output something if it's enabled.
     protected void DetailLog(string msg, params Object[] args)
     {
-        if (PhysicsScene.PhysicsLogging.Enabled)
-            PhysicsScene.DetailLog(msg, args);
+        if (m_physicsScene.PhysicsLogging.Enabled)
+            m_physicsScene.DetailLog(msg, args);
     }
 
 }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs
index bd03d31..350a5d1 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs
@@ -35,59 +35,74 @@ using OMV = OpenMetaverse;
 namespace OpenSim.Region.Physics.BulletSPlugin
 {
 
+    /*
 // When a child is linked, the relationship position of the child to the parent
 //    is remembered so the child's world position can be recomputed when it is
 //    removed from the linkset.
 sealed class BSLinksetCompoundInfo : BSLinksetInfo
 {
-    public OMV.Vector3 OffsetPos;
+    public int Index;
+    public OMV.Vector3 OffsetFromRoot;
+    public OMV.Vector3 OffsetFromCenterOfMass;
     public OMV.Quaternion OffsetRot;
-    public BSLinksetCompoundInfo(OMV.Vector3 p, OMV.Quaternion r)
+    public BSLinksetCompoundInfo(int indx, OMV.Vector3 p, OMV.Quaternion r)
     {
-        OffsetPos = p;
+        Index = indx;
+        OffsetFromRoot = p;
+        OffsetFromCenterOfMass = p;
         OffsetRot = r;
     }
+    // 'centerDisplacement' is the distance from the root the the center-of-mass (Bullet 'zero' of the shape)
+    public BSLinksetCompoundInfo(int indx, BSPrimLinkable root, BSPrimLinkable child, OMV.Vector3 centerDisplacement)
+    {
+        // Each child position and rotation is given relative to the center-of-mass.
+        OMV.Quaternion invRootOrientation = OMV.Quaternion.Inverse(root.RawOrientation);
+        OMV.Vector3 displacementFromRoot = (child.RawPosition - root.RawPosition) * invRootOrientation;
+        OMV.Vector3 displacementFromCOM = displacementFromRoot - centerDisplacement;
+        OMV.Quaternion displacementRot = child.RawOrientation * invRootOrientation;
+
+        // Save relative position for recomputing child's world position after moving linkset.
+        Index = indx;
+        OffsetFromRoot = displacementFromRoot;
+        OffsetFromCenterOfMass = displacementFromCOM;
+        OffsetRot = displacementRot;
+    }
     public override void Clear()
     {
-        OffsetPos = OMV.Vector3.Zero;
+        Index = 0;
+        OffsetFromRoot = OMV.Vector3.Zero;
+        OffsetFromCenterOfMass = OMV.Vector3.Zero;
         OffsetRot = OMV.Quaternion.Identity;
     }
     public override string ToString()
     {
         StringBuilder buff = new StringBuilder();
-        buff.Append("<p=");
-        buff.Append(OffsetPos.ToString());
+        buff.Append("<i=");
+        buff.Append(Index.ToString());
+        buff.Append(",p=");
+        buff.Append(OffsetFromRoot.ToString());
+        buff.Append(",m=");
+        buff.Append(OffsetFromCenterOfMass.ToString());
         buff.Append(",r=");
         buff.Append(OffsetRot.ToString());
         buff.Append(">");
         return buff.ToString();
     }
 };
+     */
 
 public sealed class BSLinksetCompound : BSLinkset
 {
     private static string LogHeader = "[BULLETSIM LINKSET COMPOUND]";
 
-    public BSLinksetCompound(BSScene scene, BSPhysObject parent) : base(scene, parent)
+    public BSLinksetCompound(BSScene scene, BSPrimLinkable parent)
+        : base(scene, parent)
     {
     }
 
-    // For compound implimented linksets, if there are children, use compound shape for the root.
-    public override BSPhysicsShapeType PreferredPhysicalShape(BSPhysObject requestor)
-    { 
-        // Returning 'unknown' means we don't have a preference.
-        BSPhysicsShapeType ret = BSPhysicsShapeType.SHAPE_UNKNOWN;
-        if (IsRoot(requestor) && HasAnyChildren)
-        {
-            ret = BSPhysicsShapeType.SHAPE_COMPOUND;
-        }
-        // DetailLog("{0},BSLinksetCompound.PreferredPhysicalShape,call,shape={1}", LinksetRoot.LocalID, ret);
-        return ret;
-    }
-
     // When physical properties are changed the linkset needs to recalculate
     //   its internal properties.
-    public override void Refresh(BSPhysObject requestor)
+    public override void Refresh(BSPrimLinkable requestor)
     {
         base.Refresh(requestor);
 
@@ -96,16 +111,16 @@ public sealed class BSLinksetCompound : BSLinkset
     }
 
     // Schedule a refresh to happen after all the other taint processing.
-    private void ScheduleRebuild(BSPhysObject requestor)
+    private void ScheduleRebuild(BSPrimLinkable requestor)
     {
-        DetailLog("{0},BSLinksetCompound.ScheduleRebuild,,rebuilding={1},hasChildren={2}", 
-                            requestor.LocalID, Rebuilding, HasAnyChildren);
+        DetailLog("{0},BSLinksetCompound.ScheduleRebuild,,rebuilding={1},hasChildren={2},actuallyScheduling={3}",
+                            requestor.LocalID, Rebuilding, HasAnyChildren, (!Rebuilding && HasAnyChildren));
         // When rebuilding, it is possible to set properties that would normally require a rebuild.
         //    If already rebuilding, don't request another rebuild.
         //    If a linkset with just a root prim (simple non-linked prim) don't bother rebuilding.
         if (!Rebuilding && HasAnyChildren)
         {
-            PhysicsScene.PostTaintObject("BSLinksetCompound.ScheduleRebuild", LinksetRoot.LocalID, delegate()
+            m_physicsScene.PostTaintObject("BSLinksetCompound.ScheduleRebuild", LinksetRoot.LocalID, delegate()
             {
                 if (HasAnyChildren)
                     RecomputeLinksetCompound();
@@ -118,7 +133,7 @@ public sealed class BSLinksetCompound : BSLinkset
     //     has not yet been fully constructed.
     // Return 'true' if any properties updated on the passed object.
     // Called at taint-time!
-    public override bool MakeDynamic(BSPhysObject child)
+    public override bool MakeDynamic(BSPrimLinkable child)
     {
         bool ret = false;
         DetailLog("{0},BSLinksetCompound.MakeDynamic,call,IsRoot={1}", child.LocalID, IsRoot(child));
@@ -127,138 +142,131 @@ public sealed class BSLinksetCompound : BSLinkset
             // The root is going dynamic. Rebuild the linkset so parts and mass get computed properly.
             ScheduleRebuild(LinksetRoot);
         }
-        else
-        {
-            // The origional prims are removed from the world as the shape of the root compound
-            //     shape takes over.
-            PhysicsScene.PE.AddToCollisionFlags(child.PhysBody, CollisionFlags.CF_NO_CONTACT_RESPONSE);
-            PhysicsScene.PE.ForceActivationState(child.PhysBody, ActivationState.DISABLE_SIMULATION);
-            // We don't want collisions from the old linkset children.
-            PhysicsScene.PE.RemoveFromCollisionFlags(child.PhysBody, CollisionFlags.BS_SUBSCRIBE_COLLISION_EVENTS);
-
-            child.PhysBody.collisionType = CollisionType.LinksetChild;
-
-            ret = true;
-        }
         return ret;
     }
 
-    // The object is going static (non-physical). Do any setup necessary for a static linkset.
+    // The object is going static (non-physical). We do not do anything for static linksets.
     // Return 'true' if any properties updated on the passed object.
-    // This doesn't normally happen -- OpenSim removes the objects from the physical
-    //     world if it is a static linkset.
     // Called at taint-time!
-    public override bool MakeStatic(BSPhysObject child)
+    public override bool MakeStatic(BSPrimLinkable child)
     {
         bool ret = false;
         DetailLog("{0},BSLinksetCompound.MakeStatic,call,IsRoot={1}", child.LocalID, IsRoot(child));
         if (IsRoot(child))
         {
+            // Schedule a rebuild to verify that the root shape is set to the real shape.
             ScheduleRebuild(LinksetRoot);
         }
-        else
-        {
-            // The non-physical children can come back to life.
-            PhysicsScene.PE.RemoveFromCollisionFlags(child.PhysBody, CollisionFlags.CF_NO_CONTACT_RESPONSE);
-
-            child.PhysBody.collisionType = CollisionType.LinksetChild;
-
-            // Don't force activation so setting of DISABLE_SIMULATION can stay if used.
-            PhysicsScene.PE.Activate(child.PhysBody, false);
-            ret = true;
-        }
         return ret;
     }
 
-    public override void UpdateProperties(BSPhysObject updated, bool physicalUpdate)
+    // 'physicalUpdate' is true if these changes came directly from the physics engine. Don't need to rebuild then.
+    // Called at taint-time.
+    public override void UpdateProperties(UpdatedProperties whichUpdated, BSPrimLinkable updated)
     {
+        if (!LinksetRoot.IsPhysicallyActive)
+        {
+            // No reason to do this physical stuff for static linksets.
+            DetailLog("{0},BSLinksetCompound.UpdateProperties,notPhysical", LinksetRoot.LocalID);
+            return;
+        }
+
         // The user moving a child around requires the rebuilding of the linkset compound shape
         // One problem is this happens when a border is crossed -- the simulator implementation
-        //    is to store the position into the group which causes the move of the object
+        //    stores the position into the group which causes the move of the object
         //    but it also means all the child positions get updated.
         //    What would cause an unnecessary rebuild so we make sure the linkset is in a
         //    region before bothering to do a rebuild.
-        if (!IsRoot(updated) 
-                && !physicalUpdate 
-                && PhysicsScene.TerrainManager.IsWithinKnownTerrain(LinksetRoot.RawPosition))
+        if (!IsRoot(updated) && m_physicsScene.TerrainManager.IsWithinKnownTerrain(LinksetRoot.RawPosition))
         {
-            updated.LinksetInfo = null;
-            ScheduleRebuild(updated);
+            // If a child of the linkset is updating only the position or rotation, that can be done
+            //    without rebuilding the linkset.
+            // If a handle for the child can be fetch, we update the child here. If a rebuild was
+            //    scheduled by someone else, the rebuild will just replace this setting.
+
+            bool updatedChild = false;
+            // Anything other than updating position or orientation usually means a physical update
+            //     and that is caused by us updating the object.
+            if ((whichUpdated & ~(UpdatedProperties.Position | UpdatedProperties.Orientation)) == 0)
+            {
+                // Find the physical instance of the child
+                if (LinksetRoot.PhysShape.HasPhysicalShape && m_physicsScene.PE.IsCompound(LinksetRoot.PhysShape.physShapeInfo))
+                {
+                    // It is possible that the linkset is still under construction and the child is not yet
+                    //    inserted into the compound shape. A rebuild of the linkset in a pre-step action will
+                    //    build the whole thing with the new position or rotation.
+                    // The index must be checked because Bullet references the child array but does no validity
+                    //    checking of the child index passed.
+                    int numLinksetChildren = m_physicsScene.PE.GetNumberOfCompoundChildren(LinksetRoot.PhysShape.physShapeInfo);
+                    if (updated.LinksetChildIndex < numLinksetChildren)
+                    {
+                        BulletShape linksetChildShape = m_physicsScene.PE.GetChildShapeFromCompoundShapeIndex(LinksetRoot.PhysShape.physShapeInfo, updated.LinksetChildIndex);
+                        if (linksetChildShape.HasPhysicalShape)
+                        {
+                            // Found the child shape within the compound shape
+                            m_physicsScene.PE.UpdateChildTransform(LinksetRoot.PhysShape.physShapeInfo, updated.LinksetChildIndex,
+                                                                        updated.RawPosition - LinksetRoot.RawPosition,
+                                                                        updated.RawOrientation * OMV.Quaternion.Inverse(LinksetRoot.RawOrientation),
+                                                                        true /* shouldRecalculateLocalAabb */);
+                            updatedChild = true;
+                            DetailLog("{0},BSLinksetCompound.UpdateProperties,changeChildPosRot,whichUpdated={1},pos={2},rot={3}",
+                                                        updated.LocalID, whichUpdated, updated.RawPosition, updated.RawOrientation);
+                        }
+                        else    // DEBUG DEBUG
+                        {       // DEBUG DEBUG
+                            DetailLog("{0},BSLinksetCompound.UpdateProperties,couldNotUpdateChild,noChildShape,shape={1}",
+                                                                        updated.LocalID, linksetChildShape);
+                        }       // DEBUG DEBUG
+                    }
+                    else    // DEBUG DEBUG
+                    {       // DEBUG DEBUG
+                        // the child is not yet in the compound shape. This is non-fatal.
+                        DetailLog("{0},BSLinksetCompound.UpdateProperties,couldNotUpdateChild,childNotInCompoundShape,numChildren={1},index={2}",
+                                                                    updated.LocalID, numLinksetChildren, updated.LinksetChildIndex);
+                    }       // DEBUG DEBUG
+                }
+                else    // DEBUG DEBUG
+                {       // DEBUG DEBUG
+                    DetailLog("{0},BSLinksetCompound.UpdateProperties,couldNotUpdateChild,noBodyOrNotCompound", updated.LocalID);
+                }       // DEBUG DEBUG
+
+                if (!updatedChild)
+                {
+                    // If couldn't do the individual child, the linkset needs a rebuild to incorporate the new child info.
+                    // Note: there are several ways through this code that will not update the child if
+                    //    the linkset is being rebuilt. In this case, scheduling a rebuild is a NOOP since
+                    //    there will already be a rebuild scheduled.
+                    DetailLog("{0},BSLinksetCompound.UpdateProperties,couldNotUpdateChild.schedulingRebuild,whichUpdated={1}",
+                                                                    updated.LocalID, whichUpdated);
+                    updated.LinksetInfo = null; // setting to 'null' causes relative position to be recomputed.
+                    ScheduleRebuild(updated);
+                }
+            }
         }
     }
 
     // Routine called when rebuilding the body of some member of the linkset.
-    // Since we don't keep in world relationships, do nothing unless it's a child changing.
+    // If one of the bodies is being changed, the linkset needs rebuilding.
+    // For instance, a linkset is built and then a mesh asset is read in and the mesh is recreated.
     // Returns 'true' of something was actually removed and would need restoring
     // Called at taint-time!!
-    public override bool RemoveBodyDependencies(BSPrim child)
+    public override bool RemoveDependencies(BSPrimLinkable child)
     {
         bool ret = false;
 
         DetailLog("{0},BSLinksetCompound.RemoveBodyDependencies,refreshIfChild,rID={1},rBody={2},isRoot={3}",
-                        child.LocalID, LinksetRoot.LocalID, LinksetRoot.PhysBody.AddrString, IsRoot(child));
+                        child.LocalID, LinksetRoot.LocalID, LinksetRoot.PhysBody, IsRoot(child));
 
-        if (!IsRoot(child))
-        {
-            // Because it is a convenient time, recompute child world position and rotation based on
-            //    its position in the linkset.
-            RecomputeChildWorldPosition(child, true);
-        }
-
-        // Cannot schedule a refresh/rebuild here because this routine is called when
-        //     the linkset is being rebuilt.
-        // InternalRefresh(LinksetRoot);
+        ScheduleRebuild(child);
 
         return ret;
     }
 
-    // Companion to RemoveBodyDependencies(). If RemoveBodyDependencies() returns 'true',
-    //     this routine will restore the removed constraints.
-    // Called at taint-time!!
-    public override void RestoreBodyDependencies(BSPrim child)
-    {
-    }
-
-    // When the linkset is built, the child shape is added to the compound shape relative to the
-    //    root shape. The linkset then moves around but this does not move the actual child
-    //    prim. The child prim's location must be recomputed based on the location of the root shape.
-    private void RecomputeChildWorldPosition(BSPhysObject child, bool inTaintTime)
-    {
-        BSLinksetCompoundInfo lci = child.LinksetInfo as BSLinksetCompoundInfo;
-        if (lci != null)
-        {
-            if (inTaintTime)
-            {
-                OMV.Vector3 oldPos = child.RawPosition;
-                child.ForcePosition = LinksetRoot.RawPosition + lci.OffsetPos;
-                child.ForceOrientation = LinksetRoot.RawOrientation * lci.OffsetRot;
-                DetailLog("{0},BSLinksetCompound.RecomputeChildWorldPosition,oldPos={1},lci={2},newPos={3}",
-                                            child.LocalID, oldPos, lci, child.RawPosition);
-            }
-            else
-            {
-                // TaintedObject is not used here so the raw position is set now and not at taint-time.
-                child.Position = LinksetRoot.RawPosition + lci.OffsetPos;
-                child.Orientation = LinksetRoot.RawOrientation * lci.OffsetRot;
-            }
-        }
-        else
-        {
-            // This happens when children have been added to the linkset but the linkset
-            //     has not been constructed yet. So like, at taint time, adding children to a linkset
-            //     and then changing properties of the children (makePhysical, for instance)
-            //     but the post-print action of actually rebuilding the linkset has not yet happened.
-            // PhysicsScene.Logger.WarnFormat("{0} Restoring linkset child position failed because of no relative position computed. ID={1}",
-            //                                 LogHeader, child.LocalID);
-            DetailLog("{0},BSLinksetCompound.recomputeChildWorldPosition,noRelativePositonInfo", child.LocalID);
-        }
-    }
-
     // ================================================================
 
     // Add a new child to the linkset.
     // Called while LinkActivity is locked.
-    protected override void AddChildToLinkset(BSPhysObject child)
+    protected override void AddChildToLinkset(BSPrimLinkable child)
     {
         if (!HasChild(child))
         {
@@ -274,8 +282,10 @@ public sealed class BSLinksetCompound : BSLinkset
 
     // Remove the specified child from the linkset.
     // Safe to call even if the child is not really in the linkset.
-    protected override void RemoveChildFromLinkset(BSPhysObject child)
+    protected override void RemoveChildFromLinkset(BSPrimLinkable child)
     {
+        child.ClearDisplacement();
+
         if (m_children.Remove(child))
         {
             DetailLog("{0},BSLinksetCompound.RemoveChildFromLinkset,call,rID={1},rBody={2},cID={3},cBody={4}",
@@ -284,7 +294,7 @@ public sealed class BSLinksetCompound : BSLinkset
                             child.LocalID, child.PhysBody.AddrString);
 
             // Cause the child's body to be rebuilt and thus restored to normal operation
-            RecomputeChildWorldPosition(child, false);
+            child.LinksetInfo = null;
             child.ForceBodyShapeRebuild(false);
 
             if (!HasAnyChildren)
@@ -295,7 +305,7 @@ public sealed class BSLinksetCompound : BSLinkset
             else
             {
                 // Rebuild the compound shape with the child removed
-                ScheduleRebuild(child);
+                ScheduleRebuild(LinksetRoot);
             }
         }
         return;
@@ -306,87 +316,113 @@ public sealed class BSLinksetCompound : BSLinkset
     // Constraint linksets are rebuilt every time.
     // Note that this works for rebuilding just the root after a linkset is taken apart.
     // Called at taint time!!
+    private bool UseBulletSimRootOffsetHack = false;    // Attempt to have Bullet track the coords of root compound shape
+    private bool disableCOM = true; // For basic linkset debugging, turn off the center-of-mass setting
     private void RecomputeLinksetCompound()
     {
         try
         {
-            // Suppress rebuilding while rebuilding
             Rebuilding = true;
 
-            // Cause the root shape to be rebuilt as a compound object with just the root in it
+            // No matter what is being done, force the root prim's PhysBody and PhysShape to get set
+            //     to what they should be as if the root was not in a linkset.
+            // Not that bad since we only get into this routine if there are children in the linkset and
+            //     something has been updated/changed.
             LinksetRoot.ForceBodyShapeRebuild(true);
 
-            DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,start,rBody={1},rShape={2},numChildren={3}",
-                            LinksetRoot.LocalID, LinksetRoot.PhysBody, LinksetRoot.PhysShape, NumberOfChildren);
+            // There is no reason to build all this physical stuff for a non-physical linkset.
+            if (!LinksetRoot.IsPhysicallyActive)
+            {
+                // Clean up any old linkset shape and make sure the root shape is set to the root object.
+                DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,notPhysical", LinksetRoot.LocalID);
+
+                return; // Note the 'finally' clause at the botton which will get executed.
+            }
+
+            // Get a new compound shape to build the linkset shape in.
+            BSShape linksetShape = BSShapeCompound.GetReference(m_physicsScene);
 
-            // Add a shape for each of the other children in the linkset
-            ForEachMember(delegate(BSPhysObject cPrim)
+            // The center of mass for the linkset is the geometric center of the group.
+            // Compute a displacement for each component so it is relative to the center-of-mass.
+            // Bullet presumes an object's origin (relative <0,0,0>) is its center-of-mass
+            OMV.Vector3 centerOfMassW = ComputeLinksetCenterOfMass();
+
+            OMV.Quaternion invRootOrientation = OMV.Quaternion.Normalize(OMV.Quaternion.Inverse(LinksetRoot.RawOrientation));
+
+            // 'centerDisplacement' is the value to subtract from children to give physical offset position
+            OMV.Vector3 centerDisplacementV = (centerOfMassW - LinksetRoot.RawPosition) * invRootOrientation;
+            if (UseBulletSimRootOffsetHack || disableCOM)
             {
+                centerDisplacementV = OMV.Vector3.Zero;
+                LinksetRoot.ClearDisplacement();
+            }
+            else
+            {
+                LinksetRoot.SetEffectiveCenterOfMassDisplacement(centerDisplacementV);
+            }
+            DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,COM,rootPos={1},com={2},comDisp={3}",
+                                LinksetRoot.LocalID, LinksetRoot.RawPosition, centerOfMassW, centerDisplacementV);
+
+            // Add the shapes of all the components of the linkset
+            int memberIndex = 1;
+            ForEachMember(delegate(BSPrimLinkable cPrim)
+            {
+                // Root shape is always index zero.
+                cPrim.LinksetChildIndex = IsRoot(cPrim) ? 0 : memberIndex;
+
+                // Get a reference to the shape of the child and add that shape to the linkset compound shape
+                BSShape childShape = cPrim.PhysShape.GetReference(m_physicsScene, cPrim);
+                OMV.Vector3 offsetPos = (cPrim.RawPosition - LinksetRoot.RawPosition) * invRootOrientation - centerDisplacementV;
+                OMV.Quaternion offsetRot = OMV.Quaternion.Normalize(cPrim.RawOrientation) * invRootOrientation;
+                m_physicsScene.PE.AddChildShapeToCompoundShape(linksetShape.physShapeInfo, childShape.physShapeInfo, offsetPos, offsetRot);
+                DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,addChild,indx={1},cShape={2},offPos={3},offRot={4}",
+                                    LinksetRoot.LocalID, memberIndex, childShape, offsetPos, offsetRot);
+
+                // Since we are borrowing the shape of the child, disable the origional child body
                 if (!IsRoot(cPrim))
                 {
-                    // Compute the displacement of the child from the root of the linkset.
-                    // This info is saved in the child prim so the relationship does not
-                    //    change over time and the new child position can be computed
-                    //    when the linkset is being disassembled (the linkset may have moved).
-                    BSLinksetCompoundInfo lci = cPrim.LinksetInfo as BSLinksetCompoundInfo;
-                    if (lci == null)
-                    {
-                        // Each child position and rotation is given relative to the root.
-                        OMV.Quaternion invRootOrientation = OMV.Quaternion.Inverse(LinksetRoot.RawOrientation);
-                        OMV.Vector3 displacementPos = (cPrim.RawPosition - LinksetRoot.RawPosition) * invRootOrientation;
-                        OMV.Quaternion displacementRot = cPrim.RawOrientation * invRootOrientation;
-
-                        // Save relative position for recomputing child's world position after moving linkset.
-                        lci = new BSLinksetCompoundInfo(displacementPos, displacementRot);
-                        cPrim.LinksetInfo = lci;
-                        DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,creatingRelPos,lci={1}", cPrim.LocalID, lci);
-                    }
+                    m_physicsScene.PE.AddToCollisionFlags(cPrim.PhysBody, CollisionFlags.CF_NO_CONTACT_RESPONSE);
+                    m_physicsScene.PE.ForceActivationState(cPrim.PhysBody, ActivationState.DISABLE_SIMULATION);
+                    // We don't want collisions from the old linkset children.
+                    m_physicsScene.PE.RemoveFromCollisionFlags(cPrim.PhysBody, CollisionFlags.BS_SUBSCRIBE_COLLISION_EVENTS);
+                    cPrim.PhysBody.collisionType = CollisionType.LinksetChild;
+                }
 
-                    DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,addMemberToShape,mID={1},mShape={2},dispPos={3},dispRot={4}",
-                        LinksetRoot.LocalID, cPrim.LocalID, cPrim.PhysShape, lci.OffsetPos, lci.OffsetRot);
+                memberIndex++;
 
-                    if (cPrim.PhysShape.isNativeShape)
-                    {
-                        // A native shape is turning into a hull collision shape because native
-                        //    shapes are not shared so we have to hullify it so it will be tracked
-                        //    and freed at the correct time. This also solves the scaling problem
-                        //    (native shapes scaled but hull/meshes are assumed to not be).
-                        // TODO: decide of the native shape can just be used in the compound shape.
-                        //    Use call to CreateGeomNonSpecial().
-                        BulletShape saveShape = cPrim.PhysShape;
-                        cPrim.PhysShape.Clear();        // Don't let the create free the child's shape
-                        // PhysicsScene.Shapes.CreateGeomNonSpecial(true, cPrim, null);
-                        PhysicsScene.Shapes.CreateGeomMeshOrHull(cPrim, null);
-                        BulletShape newShape = cPrim.PhysShape;
-                        cPrim.PhysShape = saveShape;
-                        PhysicsScene.PE.AddChildShapeToCompoundShape(LinksetRoot.PhysShape, newShape, lci.OffsetPos, lci.OffsetRot);
-                    }
-                    else
-                    {
-                        // For the shared shapes (meshes and hulls), just use the shape in the child.
-                        // The reference count added here will be decremented when the compound shape
-                        //     is destroyed in BSShapeCollection (the child shapes are looped over and dereferenced).
-                        if (PhysicsScene.Shapes.ReferenceShape(cPrim.PhysShape))
-                        {
-                            PhysicsScene.Logger.ErrorFormat("{0} Rebuilt sharable shape when building linkset! Region={1}, primID={2}, shape={3}",
-                                                LogHeader, PhysicsScene.RegionName, cPrim.LocalID, cPrim.PhysShape);
-                        }
-                        PhysicsScene.PE.AddChildShapeToCompoundShape(LinksetRoot.PhysShape, cPrim.PhysShape, lci.OffsetPos, lci.OffsetRot);
-                    }
-                }
                 return false;   // 'false' says to move onto the next child in the list
             });
 
+            // Replace the root shape with the built compound shape.
+            // Object removed and added to world to get collision cache rebuilt for new shape.
+            LinksetRoot.PhysShape.Dereference(m_physicsScene);
+            LinksetRoot.PhysShape = linksetShape;
+            m_physicsScene.PE.RemoveObjectFromWorld(m_physicsScene.World, LinksetRoot.PhysBody);
+            m_physicsScene.PE.SetCollisionShape(m_physicsScene.World, LinksetRoot.PhysBody, linksetShape.physShapeInfo);
+            m_physicsScene.PE.AddObjectToWorld(m_physicsScene.World, LinksetRoot.PhysBody);
+            DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,addBody,body={1},shape={2}",
+                                        LinksetRoot.LocalID, LinksetRoot.PhysBody, linksetShape);
+
             // With all of the linkset packed into the root prim, it has the mass of everyone.
             LinksetMass = ComputeLinksetMass();
             LinksetRoot.UpdatePhysicalMassProperties(LinksetMass, true);
+
+            if (UseBulletSimRootOffsetHack)
+            {
+                // Enable the physical position updator to return the position and rotation of the root shape.
+                // This enables a feature in the C++ code to return the world coordinates of the first shape in the
+                //     compound shape. This eleviates the need to offset the returned physical position by the
+                //     center-of-mass offset.
+                m_physicsScene.PE.AddToCollisionFlags(LinksetRoot.PhysBody, CollisionFlags.BS_RETURN_ROOT_COMPOUND_SHAPE);
+            }
         }
         finally
         {
             Rebuilding = false;
         }
 
-        PhysicsScene.PE.RecalculateCompoundShapeLocalAabb(LinksetRoot.PhysShape);
+        // See that the Aabb surrounds the new shape
+        m_physicsScene.PE.RecalculateCompoundShapeLocalAabb(LinksetRoot.PhysShape.physShapeInfo);
     }
 }
 }
\ No newline at end of file
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs
index d0b2a56..a06a44d 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs
@@ -36,7 +36,7 @@ public sealed class BSLinksetConstraints : BSLinkset
 {
     // private static string LogHeader = "[BULLETSIM LINKSET CONSTRAINTS]";
 
-    public BSLinksetConstraints(BSScene scene, BSPhysObject parent) : base(scene, parent)
+    public BSLinksetConstraints(BSScene scene, BSPrimLinkable parent) : base(scene, parent)
     {
     }
 
@@ -44,14 +44,14 @@ public sealed class BSLinksetConstraints : BSLinkset
     //   its internal properties.
     // This is queued in the 'post taint' queue so the
     //   refresh will happen once after all the other taints are applied.
-    public override void Refresh(BSPhysObject requestor)
+    public override void Refresh(BSPrimLinkable requestor)
     {
         base.Refresh(requestor);
 
         if (HasAnyChildren && IsRoot(requestor))
         {
             // Queue to happen after all the other taint processing
-            PhysicsScene.PostTaintObject("BSLinksetContraints.Refresh", requestor.LocalID, delegate()
+            m_physicsScene.PostTaintObject("BSLinksetContraints.Refresh", requestor.LocalID, delegate()
                 {
                     if (HasAnyChildren && IsRoot(requestor))
                         RecomputeLinksetConstraints();
@@ -65,7 +65,7 @@ public sealed class BSLinksetConstraints : BSLinkset
     //     has not yet been fully constructed.
     // Return 'true' if any properties updated on the passed object.
     // Called at taint-time!
-    public override bool MakeDynamic(BSPhysObject child)
+    public override bool MakeDynamic(BSPrimLinkable child)
     {
         // What is done for each object in BSPrim is what we want.
         return false;
@@ -76,14 +76,14 @@ public sealed class BSLinksetConstraints : BSLinkset
     // This doesn't normally happen -- OpenSim removes the objects from the physical
     //     world if it is a static linkset.
     // Called at taint-time!
-    public override bool MakeStatic(BSPhysObject child)
+    public override bool MakeStatic(BSPrimLinkable child)
     {
         // What is done for each object in BSPrim is what we want.
         return false;
     }
 
     // Called at taint-time!!
-    public override void UpdateProperties(BSPhysObject updated, bool inTaintTime)
+    public override void UpdateProperties(UpdatedProperties whichUpdated, BSPrimLinkable pObj)
     {
         // Nothing to do for constraints on property updates
     }
@@ -93,11 +93,11 @@ public sealed class BSLinksetConstraints : BSLinkset
     //     up to rebuild the constraints before the next simulation step.
     // Returns 'true' of something was actually removed and would need restoring
     // Called at taint-time!!
-    public override bool RemoveBodyDependencies(BSPrim child)
+    public override bool RemoveDependencies(BSPrimLinkable child)
     {
         bool ret = false;
 
-        DetailLog("{0},BSLinksetConstraint.RemoveBodyDependencies,removeChildrenForRoot,rID={1},rBody={2}",
+        DetailLog("{0},BSLinksetConstraint.RemoveDependencies,removeChildrenForRoot,rID={1},rBody={2}",
                                     child.LocalID, LinksetRoot.LocalID, LinksetRoot.PhysBody.AddrString);
 
         lock (m_linksetActivityLock)
@@ -110,19 +110,11 @@ public sealed class BSLinksetConstraints : BSLinkset
         return ret;
     }
 
-    // Companion to RemoveBodyDependencies(). If RemoveBodyDependencies() returns 'true',
-    // this routine will restore the removed constraints.
-    // Called at taint-time!!
-    public override void RestoreBodyDependencies(BSPrim child)
-    {
-        // The Refresh operation queued by RemoveBodyDependencies() will build any missing constraints.
-    }
-
     // ================================================================
 
     // Add a new child to the linkset.
     // Called while LinkActivity is locked.
-    protected override void AddChildToLinkset(BSPhysObject child)
+    protected override void AddChildToLinkset(BSPrimLinkable child)
     {
         if (!HasChild(child))
         {
@@ -138,19 +130,19 @@ public sealed class BSLinksetConstraints : BSLinkset
 
     // Remove the specified child from the linkset.
     // Safe to call even if the child is not really in my linkset.
-    protected override void RemoveChildFromLinkset(BSPhysObject child)
+    protected override void RemoveChildFromLinkset(BSPrimLinkable child)
     {
         if (m_children.Remove(child))
         {
-            BSPhysObject rootx = LinksetRoot; // capture the root and body as of now
-            BSPhysObject childx = child;
+            BSPrimLinkable rootx = LinksetRoot; // capture the root and body as of now
+            BSPrimLinkable childx = child;
 
             DetailLog("{0},BSLinksetConstraints.RemoveChildFromLinkset,call,rID={1},rBody={2},cID={3},cBody={4}",
                             childx.LocalID,
                             rootx.LocalID, rootx.PhysBody.AddrString,
                             childx.LocalID, childx.PhysBody.AddrString);
 
-            PhysicsScene.TaintedObject("BSLinksetConstraints.RemoveChildFromLinkset", delegate()
+            m_physicsScene.TaintedObject("BSLinksetConstraints.RemoveChildFromLinkset", delegate()
             {
                 PhysicallyUnlinkAChildFromRoot(rootx, childx);
             });
@@ -167,13 +159,13 @@ public sealed class BSLinksetConstraints : BSLinkset
 
     // Create a constraint between me (root of linkset) and the passed prim (the child).
     // Called at taint time!
-    private void PhysicallyLinkAChildToRoot(BSPhysObject rootPrim, BSPhysObject childPrim)
+    private void PhysicallyLinkAChildToRoot(BSPrimLinkable rootPrim, BSPrimLinkable childPrim)
     {
         // Don't build the constraint when asked. Put it off until just before the simulation step.
         Refresh(rootPrim);
     }
 
-    private BSConstraint BuildConstraint(BSPhysObject rootPrim, BSPhysObject childPrim)
+    private BSConstraint BuildConstraint(BSPrimLinkable rootPrim, BSPrimLinkable childPrim)
     {
         // Zero motion for children so they don't interpolate
         childPrim.ZeroMotion(true);
@@ -195,7 +187,7 @@ public sealed class BSLinksetConstraints : BSLinkset
         // http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?t=4818
 
         BSConstraint6Dof constrain = new BSConstraint6Dof(
-                            PhysicsScene.World, rootPrim.PhysBody, childPrim.PhysBody, midPoint, true, true );
+                            m_physicsScene.World, rootPrim.PhysBody, childPrim.PhysBody, midPoint, true, true );
                             // PhysicsScene.World, childPrim.BSBody, rootPrim.BSBody, midPoint, true, true );
 
         /* NOTE: below is an attempt to build constraint with full frame computation, etc.
@@ -224,15 +216,15 @@ public sealed class BSLinksetConstraints : BSLinkset
         // ==================================================================================
         */
 
-        PhysicsScene.Constraints.AddConstraint(constrain);
+        m_physicsScene.Constraints.AddConstraint(constrain);
 
         // zero linear and angular limits makes the objects unable to move in relation to each other
         constrain.SetLinearLimits(OMV.Vector3.Zero, OMV.Vector3.Zero);
         constrain.SetAngularLimits(OMV.Vector3.Zero, OMV.Vector3.Zero);
 
         // tweek the constraint to increase stability
-        constrain.UseFrameOffset(BSParam.BoolNumeric(BSParam.LinkConstraintUseFrameOffset));
-        constrain.TranslationalLimitMotor(BSParam.BoolNumeric(BSParam.LinkConstraintEnableTransMotor),
+        constrain.UseFrameOffset(BSParam.LinkConstraintUseFrameOffset);
+        constrain.TranslationalLimitMotor(BSParam.LinkConstraintEnableTransMotor,
                         BSParam.LinkConstraintTransMotorMaxVel,
                         BSParam.LinkConstraintTransMotorMaxForce);
         constrain.SetCFMAndERP(BSParam.LinkConstraintCFM, BSParam.LinkConstraintERP);
@@ -247,7 +239,7 @@ public sealed class BSLinksetConstraints : BSLinkset
     // The root and child bodies are passed in because we need to remove the constraint between
     //      the bodies that were present at unlink time.
     // Called at taint time!
-    private bool PhysicallyUnlinkAChildFromRoot(BSPhysObject rootPrim, BSPhysObject childPrim)
+    private bool PhysicallyUnlinkAChildFromRoot(BSPrimLinkable rootPrim, BSPrimLinkable childPrim)
     {
         bool ret = false;
         DetailLog("{0},BSLinksetConstraint.PhysicallyUnlinkAChildFromRoot,taint,root={1},rBody={2},child={3},cBody={4}",
@@ -256,10 +248,10 @@ public sealed class BSLinksetConstraints : BSLinkset
                             childPrim.LocalID, childPrim.PhysBody.AddrString);
 
         // Find the constraint for this link and get rid of it from the overall collection and from my list
-        if (PhysicsScene.Constraints.RemoveAndDestroyConstraint(rootPrim.PhysBody, childPrim.PhysBody))
+        if (m_physicsScene.Constraints.RemoveAndDestroyConstraint(rootPrim.PhysBody, childPrim.PhysBody))
         {
             // Make the child refresh its location
-            PhysicsScene.PE.PushUpdate(childPrim.PhysBody);
+            m_physicsScene.PE.PushUpdate(childPrim.PhysBody);
             ret = true;
         }
 
@@ -269,11 +261,11 @@ public sealed class BSLinksetConstraints : BSLinkset
     // Remove linkage between myself and any possible children I might have.
     // Returns 'true' of any constraints were destroyed.
     // Called at taint time!
-    private bool PhysicallyUnlinkAllChildrenFromRoot(BSPhysObject rootPrim)
+    private bool PhysicallyUnlinkAllChildrenFromRoot(BSPrimLinkable rootPrim)
     {
         DetailLog("{0},BSLinksetConstraint.PhysicallyUnlinkAllChildren,taint", rootPrim.LocalID);
 
-        return PhysicsScene.Constraints.RemoveAndDestroyConstraint(rootPrim.PhysBody);
+        return m_physicsScene.Constraints.RemoveAndDestroyConstraint(rootPrim.PhysBody);
     }
 
     // Call each of the constraints that make up this linkset and recompute the
@@ -289,7 +281,7 @@ public sealed class BSLinksetConstraints : BSLinkset
         DetailLog("{0},BSLinksetConstraint.RecomputeLinksetConstraints,set,rBody={1},linksetMass={2}",
                             LinksetRoot.LocalID, LinksetRoot.PhysBody.AddrString, linksetMass);
 
-        foreach (BSPhysObject child in m_children)
+        foreach (BSPrimLinkable child in m_children)
         {
             // A child in the linkset physically shows the mass of the whole linkset.
             // This allows Bullet to apply enough force on the child to move the whole linkset.
@@ -297,7 +289,7 @@ public sealed class BSLinksetConstraints : BSLinkset
             child.UpdatePhysicalMassProperties(linksetMass, true);
 
             BSConstraint constrain;
-            if (!PhysicsScene.Constraints.TryGetConstraint(LinksetRoot.PhysBody, child.PhysBody, out constrain))
+            if (!m_physicsScene.Constraints.TryGetConstraint(LinksetRoot.PhysBody, child.PhysBody, out constrain))
             {
                 // If constraint doesn't exist yet, create it.
                 constrain = BuildConstraint(LinksetRoot, child);
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSMaterials.cs b/OpenSim/Region/Physics/BulletSPlugin/BSMaterials.cs
index 92d62ff..ee77d6e 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSMaterials.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSMaterials.cs
@@ -180,11 +180,14 @@ public static class BSMaterials
     // Use reflection to set the value in the attribute structure.
     private static void SetAttributeValue(int matType, string attribName, float val)
     {
+        // Get the current attribute values for this material
         MaterialAttributes thisAttrib = Attributes[matType];
+        // Find the field for the passed attribute name (eg, find field named 'friction')
         FieldInfo fieldInfo = thisAttrib.GetType().GetField(attribName.ToLower());
         if (fieldInfo != null)
         {
             fieldInfo.SetValue(thisAttrib, val);
+            // Copy new attributes back to array -- since MaterialAttributes is 'struct', passed by value, not reference.
             Attributes[matType] = thisAttrib;
         }
     }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs b/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs
index 817a5f7..ef662b5 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs
@@ -59,22 +59,17 @@ public abstract class BSMotor
     {
         if (PhysicsScene != null)
         {
-            if (PhysicsScene.VehicleLoggingEnabled)
-            {
-                PhysicsScene.DetailLog(msg, parms);
-            }
+            PhysicsScene.DetailLog(msg, parms);
         }
     }
 }
 
 // Motor which moves CurrentValue to TargetValue over TimeScale seconds.
-// The TargetValue decays in TargetValueDecayTimeScale and
-//     the CurrentValue will be held back by FrictionTimeScale.
+// The TargetValue decays in TargetValueDecayTimeScale.
 // This motor will "zero itself" over time in that the targetValue will
 //    decay to zero and the currentValue will follow it to that zero.
 //    The overall effect is for the returned correction value to go from large
-//    values (the total difference between current and target minus friction)
-//    to small and eventually zero values.
+//    values to small and eventually zero values.
 // TimeScale and TargetDelayTimeScale may be 'infinite' which means no decay.
 
 // For instance, if something is moving at speed X and the desired speed is Y,
@@ -91,7 +86,6 @@ public class BSVMotor : BSMotor
 
     public virtual float TimeScale { get; set; }
     public virtual float TargetValueDecayTimeScale { get; set; }
-    public virtual Vector3 FrictionTimescale { get; set; }
     public virtual float Efficiency { get; set; }
 
     public virtual float ErrorZeroThreshold { get; set; }
@@ -100,10 +94,13 @@ public class BSVMotor : BSMotor
     public virtual Vector3 CurrentValue { get; protected set; }
     public virtual Vector3 LastError { get; protected set; }
 
-    public virtual bool ErrorIsZero
-    { get {
-        return (LastError == Vector3.Zero || LastError.LengthSquared() <= ErrorZeroThreshold);
-        }
+    public virtual bool ErrorIsZero()
+    {
+        return ErrorIsZero(LastError);
+    }
+    public virtual bool ErrorIsZero(Vector3 err)
+    {
+        return (err == Vector3.Zero || err.ApproxEquals(Vector3.Zero, ErrorZeroThreshold));
     }
 
     public BSVMotor(string useName)
@@ -111,16 +108,14 @@ public class BSVMotor : BSMotor
     {
         TimeScale = TargetValueDecayTimeScale = BSMotor.Infinite;
         Efficiency = 1f;
-        FrictionTimescale = BSMotor.InfiniteVector;
         CurrentValue = TargetValue = Vector3.Zero;
         ErrorZeroThreshold = 0.001f;
     }
-    public BSVMotor(string useName, float timeScale, float decayTimeScale, Vector3 frictionTimeScale, float efficiency) 
+    public BSVMotor(string useName, float timeScale, float decayTimeScale, float efficiency)
         : this(useName)
     {
         TimeScale = timeScale;
         TargetValueDecayTimeScale = decayTimeScale;
-        FrictionTimescale = frictionTimeScale;
         Efficiency = efficiency;
         CurrentValue = TargetValue = Vector3.Zero;
     }
@@ -138,7 +133,8 @@ public class BSVMotor : BSMotor
         CurrentValue = TargetValue = Vector3.Zero;
     }
 
-    // Compute the next step and return the new current value
+    // Compute the next step and return the new current value.
+    // Returns the correction needed to move 'current' to 'target'.
     public virtual Vector3 Step(float timeStep)
     {
         if (!Enabled) return TargetValue;
@@ -148,9 +144,10 @@ public class BSVMotor : BSMotor
 
         Vector3 correction = Vector3.Zero;
         Vector3 error = TargetValue - CurrentValue;
-        if (!error.ApproxEquals(Vector3.Zero, ErrorZeroThreshold))
+        LastError = error;
+        if (!ErrorIsZero(error))
         {
-            correction = Step(timeStep, error);
+            correction = StepError(timeStep, error);
 
             CurrentValue += correction;
 
@@ -163,44 +160,40 @@ public class BSVMotor : BSMotor
                 TargetValue *= (1f - decayFactor);
             }
 
-            // The amount we can correct the error is reduced by the friction
-            Vector3 frictionFactor = Vector3.Zero;
-            if (FrictionTimescale != BSMotor.InfiniteVector)
-            {
-                // frictionFactor = (Vector3.One / FrictionTimescale) * timeStep;
-                // Individual friction components can be 'infinite' so compute each separately.
-                frictionFactor.X = (FrictionTimescale.X == BSMotor.Infinite) ? 0f : (1f / FrictionTimescale.X);
-                frictionFactor.Y = (FrictionTimescale.Y == BSMotor.Infinite) ? 0f : (1f / FrictionTimescale.Y);
-                frictionFactor.Z = (FrictionTimescale.Z == BSMotor.Infinite) ? 0f : (1f / FrictionTimescale.Z);
-                frictionFactor *= timeStep;
-                CurrentValue *= (Vector3.One - frictionFactor);
-            }
-
             MDetailLog("{0},  BSVMotor.Step,nonZero,{1},origCurr={2},origTarget={3},timeStep={4},err={5},corr={6}",
                                 BSScene.DetailLogZero, UseName, origCurrVal, origTarget,
                                 timeStep, error, correction);
-            MDetailLog("{0},  BSVMotor.Step,nonZero,{1},tgtDecayTS={2},decayFact={3},frictTS={4},frictFact={5},tgt={6},curr={7}",
-                                BSScene.DetailLogZero, UseName,
-                                TargetValueDecayTimeScale, decayFactor, FrictionTimescale, frictionFactor,
-                                TargetValue, CurrentValue);
+            MDetailLog("{0},  BSVMotor.Step,nonZero,{1},tgtDecayTS={2},decayFact={3},tgt={4},curr={5}",
+                                BSScene.DetailLogZero, UseName, TargetValueDecayTimeScale, decayFactor, TargetValue, CurrentValue);
         }
         else
         {
             // Difference between what we have and target is small. Motor is done.
+            if (TargetValue.ApproxEquals(Vector3.Zero, ErrorZeroThreshold))
+            {
+                // The target can step down to nearly zero but not get there.  If close to zero
+                //     it is really zero.
+                TargetValue = Vector3.Zero;
+            }
             CurrentValue = TargetValue;
-            MDetailLog("{0},  BSVMotor.Step,zero,{1},origTgt={2},origCurr={3},ret={4}",
-                        BSScene.DetailLogZero, UseName, origCurrVal, origTarget, CurrentValue);
+            MDetailLog("{0},  BSVMotor.Step,zero,{1},origTgt={2},origCurr={3},currTgt={4},currCurr={5}",
+                        BSScene.DetailLogZero, UseName, origCurrVal, origTarget, TargetValue, CurrentValue);
         }
 
-        return CurrentValue;
+        return correction;
+    }
+    // version of step that sets the current value before doing the step
+    public virtual Vector3 Step(float timeStep, Vector3 current)
+    {
+        CurrentValue = current;
+        return Step(timeStep);
     }
-    public virtual Vector3 Step(float timeStep, Vector3 error)
+    public virtual Vector3 StepError(float timeStep, Vector3 error)
     {
         if (!Enabled) return Vector3.Zero;
 
-        LastError = error;
         Vector3 returnCorrection = Vector3.Zero;
-        if (!error.ApproxEquals(Vector3.Zero, ErrorZeroThreshold))
+        if (!ErrorIsZero(error))
         {
             // correction =  error / secondsItShouldTakeToCorrect
             Vector3 correctionAmount;
@@ -222,9 +215,9 @@ public class BSVMotor : BSMotor
         // maximum number of outputs to generate.
         int maxOutput = 50;
         MDetailLog("{0},BSVMotor.Test,{1},===================================== BEGIN Test Output", BSScene.DetailLogZero, UseName);
-        MDetailLog("{0},BSVMotor.Test,{1},timeScale={2},targDlyTS={3},frictTS={4},eff={5},curr={6},tgt={7}",
+        MDetailLog("{0},BSVMotor.Test,{1},timeScale={2},targDlyTS={3},eff={4},curr={5},tgt={6}",
                                 BSScene.DetailLogZero, UseName,
-                                TimeScale, TargetValueDecayTimeScale, FrictionTimescale, Efficiency, 
+                                TimeScale, TargetValueDecayTimeScale, Efficiency,
                                 CurrentValue, TargetValue);
 
         LastError = BSMotor.InfiniteVector;
@@ -235,43 +228,141 @@ public class BSVMotor : BSMotor
                             BSScene.DetailLogZero, UseName, CurrentValue, TargetValue, LastError, lastStep);
         }
         MDetailLog("{0},BSVMotor.Test,{1},===================================== END Test Output", BSScene.DetailLogZero, UseName);
-        
+
 
     }
 
     public override string ToString()
     {
-        return String.Format("<{0},curr={1},targ={2},lastErr={3},decayTS={4},frictTS={5}>",
-            UseName, CurrentValue, TargetValue, LastError, TargetValueDecayTimeScale, FrictionTimescale);
+        return String.Format("<{0},curr={1},targ={2},lastErr={3},decayTS={4}>",
+            UseName, CurrentValue, TargetValue, LastError, TargetValueDecayTimeScale);
     }
 }
 
+// ============================================================================
+// ============================================================================
 public class BSFMotor : BSMotor
 {
-    public float TimeScale { get; set; }
-    public float DecayTimeScale { get; set; }
-    public float Friction { get; set; }
-    public float Efficiency { get; set; }
+    public virtual float TimeScale { get; set; }
+    public virtual float TargetValueDecayTimeScale { get; set; }
+    public virtual float Efficiency { get; set; }
 
-    public float Target { get; private set; }
-    public float CurrentValue { get; private set; }
+    public virtual float ErrorZeroThreshold { get; set; }
 
-    public BSFMotor(string useName, float timeScale, float decayTimescale, float friction, float efficiency)
+    public virtual float TargetValue { get; protected set; }
+    public virtual float CurrentValue { get; protected set; }
+    public virtual float LastError { get; protected set; }
+
+    public virtual bool ErrorIsZero()
+    {
+        return ErrorIsZero(LastError);
+    }
+    public virtual bool ErrorIsZero(float err)
+    {
+        return (err >= -ErrorZeroThreshold && err <= ErrorZeroThreshold);
+    }
+
+    public BSFMotor(string useName, float timeScale, float decayTimescale, float efficiency)
         : base(useName)
     {
+        TimeScale = TargetValueDecayTimeScale = BSMotor.Infinite;
+        Efficiency = 1f;
+        CurrentValue = TargetValue = 0f;
+        ErrorZeroThreshold = 0.01f;
     }
-    public void SetCurrent(float target)
+    public void SetCurrent(float current)
     {
+        CurrentValue = current;
     }
     public void SetTarget(float target)
     {
+        TargetValue = target;
     }
+    public override void Zero()
+    {
+        base.Zero();
+        CurrentValue = TargetValue = 0f;
+    }
+
     public virtual float Step(float timeStep)
     {
-        return 0f;
+        if (!Enabled) return TargetValue;
+
+        float origTarget = TargetValue;       // DEBUG
+        float origCurrVal = CurrentValue;     // DEBUG
+
+        float correction = 0f;
+        float error = TargetValue - CurrentValue;
+        LastError = error;
+        if (!ErrorIsZero(error))
+        {
+            correction = StepError(timeStep, error);
+
+            CurrentValue += correction;
+
+            // The desired value reduces to zero which also reduces the difference with current.
+            // If the decay time is infinite, don't decay at all.
+            float decayFactor = 0f;
+            if (TargetValueDecayTimeScale != BSMotor.Infinite)
+            {
+                decayFactor = (1.0f / TargetValueDecayTimeScale) * timeStep;
+                TargetValue *= (1f - decayFactor);
+            }
+
+            MDetailLog("{0},  BSFMotor.Step,nonZero,{1},origCurr={2},origTarget={3},timeStep={4},err={5},corr={6}",
+                                BSScene.DetailLogZero, UseName, origCurrVal, origTarget,
+                                timeStep, error, correction);
+            MDetailLog("{0},  BSFMotor.Step,nonZero,{1},tgtDecayTS={2},decayFact={3},tgt={4},curr={5}",
+                                BSScene.DetailLogZero, UseName, TargetValueDecayTimeScale, decayFactor, TargetValue, CurrentValue);
+        }
+        else
+        {
+            // Difference between what we have and target is small. Motor is done.
+            if (Util.InRange<float>(TargetValue, -ErrorZeroThreshold, ErrorZeroThreshold))
+            {
+                // The target can step down to nearly zero but not get there.  If close to zero
+                //     it is really zero.
+                TargetValue = 0f;
+            }
+            CurrentValue = TargetValue;
+            MDetailLog("{0},  BSFMotor.Step,zero,{1},origTgt={2},origCurr={3},ret={4}",
+                        BSScene.DetailLogZero, UseName, origCurrVal, origTarget, CurrentValue);
+        }
+
+        return CurrentValue;
+    }
+
+    public virtual float StepError(float timeStep, float error)
+    {
+        if (!Enabled) return 0f;
+
+        float returnCorrection = 0f;
+        if (!ErrorIsZero(error))
+        {
+            // correction =  error / secondsItShouldTakeToCorrect
+            float correctionAmount;
+            if (TimeScale == 0f || TimeScale == BSMotor.Infinite)
+                correctionAmount = error * timeStep;
+            else
+                correctionAmount = error / TimeScale * timeStep;
+
+            returnCorrection = correctionAmount;
+            MDetailLog("{0},  BSFMotor.Step,nonZero,{1},timeStep={2},timeScale={3},err={4},corr={5}",
+                                    BSScene.DetailLogZero, UseName, timeStep, TimeScale, error, correctionAmount);
+        }
+        return returnCorrection;
+    }
+
+    public override string ToString()
+    {
+        return String.Format("<{0},curr={1},targ={2},lastErr={3},decayTS={4}>",
+            UseName, CurrentValue, TargetValue, LastError, TargetValueDecayTimeScale);
     }
+
 }
 
+// ============================================================================
+// ============================================================================
 // Proportional, Integral, Derivitive Motor
 // Good description at http://www.answers.com/topic/pid-controller . Includes processes for choosing p, i and d factors.
 public class BSPIDVMotor : BSVMotor
@@ -281,6 +372,12 @@ public class BSPIDVMotor : BSVMotor
     public Vector3 integralFactor { get; set; }
     public Vector3 derivFactor { get; set; }
 
+    // The factors are vectors for the three dimensions. This is the proportional of each
+    //    that is applied. This could be multiplied through the actual factors but it
+    //    is sometimes easier to manipulate the factors and their mix separately.
+    //      to
+    public Vector3 FactorMix;
+
     // Arbritrary factor range.
     // EfficiencyHigh means move quickly to the correct number. EfficiencyLow means might over correct.
     public float EfficiencyHigh = 0.4f;
@@ -295,6 +392,7 @@ public class BSPIDVMotor : BSVMotor
         proportionFactor = new Vector3(1.00f, 1.00f, 1.00f);
         integralFactor = new Vector3(1.00f, 1.00f, 1.00f);
         derivFactor = new Vector3(1.00f, 1.00f, 1.00f);
+        FactorMix = new Vector3(0.5f, 0.25f, 0.25f);
         RunningIntegration = Vector3.Zero;
         LastError = Vector3.Zero;
     }
@@ -310,20 +408,24 @@ public class BSPIDVMotor : BSVMotor
         set
         {
             base.Efficiency = Util.Clamp(value, 0f, 1f);
+
             // Compute factors based on efficiency.
             // If efficiency is high (1f), use a factor value that moves the error value to zero with little overshoot.
             // If efficiency is low (0f), use a factor value that overcorrects.
             // TODO: might want to vary contribution of different factor depending on efficiency.
             float factor = ((1f - this.Efficiency) * EfficiencyHigh + EfficiencyLow) / 3f;
             // float factor = (1f - this.Efficiency) * EfficiencyHigh + EfficiencyLow;
+
             proportionFactor = new Vector3(factor, factor, factor);
             integralFactor = new Vector3(factor, factor, factor);
             derivFactor = new Vector3(factor, factor, factor);
+
+            MDetailLog("{0},BSPIDVMotor.setEfficiency,eff={1},factor={2}", BSScene.DetailLogZero, Efficiency, factor);
         }
     }
 
-    // Ignore Current and Target Values and just advance the PID computation on this error.
-    public override Vector3 Step(float timeStep, Vector3 error)
+    // Advance the PID computation on this error.
+    public override Vector3 StepError(float timeStep, Vector3 error)
     {
         if (!Enabled) return Vector3.Zero;
 
@@ -331,15 +433,17 @@ public class BSPIDVMotor : BSVMotor
         RunningIntegration += error * timeStep;
 
         // A simple derivitive is the rate of change from the last error.
-        Vector3 derivFactor = (error - LastError) * timeStep;
+        Vector3 derivitive = (error - LastError) * timeStep;
         LastError = error;
 
-        // Correction = -(proportionOfPresentError +      accumulationOfPastError    +     rateOfChangeOfError)
-        Vector3 ret   = -(
-                          error * proportionFactor
-                        + RunningIntegration * integralFactor 
-                        + derivFactor * derivFactor
-                        );
+        // Correction = (proportionOfPresentError + accumulationOfPastError + rateOfChangeOfError)
+        Vector3 ret   =   error * timeStep   * proportionFactor * FactorMix.X
+                        + RunningIntegration * integralFactor   * FactorMix.Y
+                        + derivitive         * derivFactor      * FactorMix.Z
+                        ;
+
+        MDetailLog("{0},BSPIDVMotor.step,ts={1},err={2},runnInt={3},deriv={4},ret={5}",
+                        BSScene.DetailLogZero, timeStep, error, RunningIntegration, derivitive, ret);
 
         return ret;
     }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSParam.cs b/OpenSim/Region/Physics/BulletSPlugin/BSParam.cs
index 69ac8cd..3ca7e16 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSParam.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSParam.cs
@@ -26,6 +26,7 @@
  */
 using System;
 using System.Collections.Generic;
+using System.Reflection;
 using System.Text;
 
 using OpenSim.Region.Physics.Manager;
@@ -37,14 +38,41 @@ namespace OpenSim.Region.Physics.BulletSPlugin
 {
 public static class BSParam
 {
+    private static string LogHeader = "[BULLETSIM PARAMETERS]";
+
+    // Tuning notes:
+    // From: http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?t=6575
+    //    Contact points can be added even if the distance is positive. The constraint solver can deal with
+    //    contacts with positive distances as well as negative (penetration). Contact points are discarded
+    //    if the distance exceeds a certain threshold.
+    //    Bullet has a contact processing threshold and a contact breaking threshold.
+    //    If the distance is larger than the contact breaking threshold, it will be removed after one frame.
+    //    If the distance is larger than the contact processing threshold, the constraint solver will ignore it.
+
+    //    This is separate/independent from the collision margin. The collision margin increases the object a bit
+    //    to improve collision detection performance and accuracy.
+    // ===================
+    // From:
+
     // Level of Detail values kept as float because that's what the Meshmerizer wants
     public static float MeshLOD { get; private set; }
+    public static float MeshCircularLOD { get; private set; }
     public static float MeshMegaPrimLOD { get; private set; }
     public static float MeshMegaPrimThreshold { get; private set; }
     public static float SculptLOD { get; private set; }
 
+    public static int CrossingFailuresBeforeOutOfBounds { get; private set; }
+    public static float UpdateVelocityChangeThreshold { get; private set; }
+
     public static float MinimumObjectMass { get; private set; }
     public static float MaximumObjectMass { get; private set; }
+    public static float MaxLinearVelocity { get; private set; }
+    public static float MaxLinearVelocitySquared { get; private set; }
+    public static float MaxAngularVelocity { get; private set; }
+    public static float MaxAngularVelocitySquared { get; private set; }
+    public static float MaxAddForceMagnitude { get; private set; }
+    public static float MaxAddForceMagnitudeSquared { get; private set; }
+    public static float DensityScaleFactor { get; private set; }
 
     public static float LinearDamping { get; private set; }
     public static float AngularDamping { get; private set; }
@@ -58,13 +86,36 @@ public static class BSParam
     public static bool ShouldMeshSculptedPrim { get; private set; }   // cause scuplted prims to get meshed
     public static bool ShouldForceSimplePrimMeshing { get; private set; }   // if a cube or sphere, let Bullet do internal shapes
     public static bool ShouldUseHullsForPhysicalObjects { get; private set; }   // 'true' if should create hulls for physical objects
+    public static bool ShouldRemoveZeroWidthTriangles { get; private set; }
+    public static bool ShouldUseBulletHACD { get; set; }
+    public static bool ShouldUseSingleConvexHullForPrims { get; set; }
 
     public static float TerrainImplementation { get; private set; }
+    public static int TerrainMeshMagnification { get; private set; }
     public static float TerrainFriction { get; private set; }
     public static float TerrainHitFraction { get; private set; }
     public static float TerrainRestitution { get; private set; }
+    public static float TerrainContactProcessingThreshold { get; private set; }
     public static float TerrainCollisionMargin { get; private set; }
 
+    public static float DefaultFriction { get; private set; }
+    public static float DefaultDensity { get; private set; }
+    public static float DefaultRestitution { get; private set; }
+    public static float CollisionMargin { get; private set; }
+    public static float Gravity { get; private set; }
+
+    // Physics Engine operation
+        public static float MaxPersistantManifoldPoolSize { get; private set; }
+        public static float MaxCollisionAlgorithmPoolSize { get; private set; }
+        public static bool ShouldDisableContactPoolDynamicAllocation { get; private set; }
+        public static bool ShouldForceUpdateAllAabbs { get; private set; }
+        public static bool ShouldRandomizeSolverOrder { get; private set; }
+        public static bool ShouldSplitSimulationIslands { get; private set; }
+        public static bool ShouldEnableFrictionCaching { get; private set; }
+        public static float NumberOfSolverIterations { get; private set; }
+    public static bool UseSingleSidedMeshes { get; private set; }
+    public static float GlobalContactBreakingThreshold { get; private set; }
+
     // Avatar parameters
     public static float AvatarFriction { get; private set; }
     public static float AvatarStandingFriction { get; private set; }
@@ -75,12 +126,48 @@ public static class BSParam
     public static float AvatarCapsuleDepth { get; private set; }
     public static float AvatarCapsuleHeight { get; private set; }
         public static float AvatarContactProcessingThreshold { get; private set; }
-
+        public static float AvatarBelowGroundUpCorrectionMeters { get; private set; }
+        public static float AvatarStepHeight { get; private set; }
+        public static float AvatarStepApproachFactor { get; private set; }
+        public static float AvatarStepForceFactor { get; private set; }
+        public static float AvatarStepUpCorrectionFactor { get; private set; }
+        public static int AvatarStepSmoothingSteps { get; private set; }
+
+    // Vehicle parameters
+    public static float VehicleMaxLinearVelocity { get; private set; }
+    public static float VehicleMaxLinearVelocitySquared { get; private set; }
+    public static float VehicleMaxAngularVelocity { get; private set; }
+    public static float VehicleMaxAngularVelocitySq { get; private set; }
     public static float VehicleAngularDamping { get; private set; }
-
+    public static float VehicleFriction { get; private set; }
+    public static float VehicleRestitution { get; private set; }
+    public static Vector3 VehicleLinearFactor { get; private set; }
+    public static Vector3 VehicleAngularFactor { get; private set; }
+    public static float VehicleGroundGravityFudge { get; private set; }
+    public static float VehicleAngularBankingTimescaleFudge { get; private set; }
+    public static bool VehicleDebuggingEnable { get; private set; }
+
+    // Convex Hulls
+    public static int CSHullMaxDepthSplit { get; private set; }
+    public static int CSHullMaxDepthSplitForSimpleShapes { get; private set; }
+    public static float CSHullConcavityThresholdPercent { get; private set; }
+    public static float CSHullVolumeConservationThresholdPercent { get; private set; }
+    public static int CSHullMaxVertices { get; private set; }
+    public static float CSHullMaxSkinWidth { get; private set; }
+        public static float BHullMaxVerticesPerHull { get; private set; }               // 100
+        public static float BHullMinClusters { get; private set; }                              // 2
+        public static float BHullCompacityWeight { get; private set; }                  // 0.1
+        public static float BHullVolumeWeight { get; private set; }                             // 0.0
+        public static float BHullConcavity { get; private set; }                                    // 100
+        public static bool BHullAddExtraDistPoints { get; private set; }                // false
+        public static bool BHullAddNeighboursDistPoints { get; private set; }   // false
+        public static bool BHullAddFacesPoints { get; private set; }                    // false
+        public static bool BHullShouldAdjustCollisionMargin { get; private set; }       // false
+
+    // Linkset implementation parameters
     public static float LinksetImplementation { get; private set; }
-    public static float LinkConstraintUseFrameOffset { get; private set; }
-    public static float LinkConstraintEnableTransMotor { get; private set; }
+    public static bool LinkConstraintUseFrameOffset { get; private set; }
+    public static bool LinkConstraintEnableTransMotor { get; private set; }
     public static float LinkConstraintTransMotorMaxVel { get; private set; }
     public static float LinkConstraintTransMotorMaxForce { get; private set; }
     public static float LinkConstraintERP { get; private set; }
@@ -90,51 +177,152 @@ public static class BSParam
     public static float PID_D { get; private set; }    // derivative
     public static float PID_P { get; private set; }    // proportional
 
-    // Various constants that come from that other virtual world that shall not be named
+    // Various constants that come from that other virtual world that shall not be named.
     public const float MinGravityZ = -1f;
     public const float MaxGravityZ = 28f;
     public const float MinFriction = 0f;
     public const float MaxFriction = 255f;
-    public const float MinDensity = 0f;
+    public const float MinDensity = 0.01f;
     public const float MaxDensity = 22587f;
     public const float MinRestitution = 0f;
     public const float MaxRestitution = 1f;
-    public const float MaxAddForceMagnitude = 20000f;
 
-    // ===========================================================================
-    public delegate void ParamUser(BSScene scene, IConfig conf, string paramName, float val);
-    public delegate float ParamGet(BSScene scene);
-    public delegate void ParamSet(BSScene scene, string paramName, uint localID, float val);
-    public delegate void SetOnObject(BSScene scene, BSPhysObject obj, float val);
+    // =====================================================================================
+    // =====================================================================================
 
-    public struct ParameterDefn
+    // Base parameter definition that gets and sets parameter values via a string
+    public abstract class ParameterDefnBase
     {
         public string name;         // string name of the parameter
         public string desc;         // a short description of what the parameter means
-        public float defaultValue;  // default value if not specified anywhere else
-        public ParamUser userParam; // get the value from the configuration file
-        public ParamGet getter;     // return the current value stored for this parameter
-        public ParamSet setter;     // set the current value for this parameter
-        public SetOnObject onObject;    // set the value on an object in the physical domain
-        public ParameterDefn(string n, string d, float v, ParamUser u, ParamGet g, ParamSet s)
+        public ParameterDefnBase(string pName, string pDesc)
+        {
+            name = pName;
+            desc = pDesc;
+        }
+        // Set the parameter value to the default
+        public abstract void AssignDefault(BSScene s);
+        // Get the value as a string
+        public abstract string GetValue(BSScene s);
+        // Set the value to this string value
+        public abstract void SetValue(BSScene s, string valAsString);
+        // set the value on a particular object (usually sets in physics engine)
+        public abstract void SetOnObject(BSScene s, BSPhysObject obj);
+        public abstract bool HasSetOnObject { get; }
+    }
+
+    // Specific parameter definition for a parameter of a specific type.
+    public delegate T PGetValue<T>(BSScene s);
+    public delegate void PSetValue<T>(BSScene s, T val);
+    public delegate void PSetOnObject<T>(BSScene scene, BSPhysObject obj);
+    public sealed class ParameterDefn<T> : ParameterDefnBase
+    {
+        private T defaultValue;
+        private PSetValue<T> setter;
+        private PGetValue<T> getter;
+        private PSetOnObject<T> objectSet;
+        public ParameterDefn(string pName, string pDesc, T pDefault, PGetValue<T> pGetter, PSetValue<T> pSetter)
+            : base(pName, pDesc)
+        {
+            defaultValue = pDefault;
+            setter = pSetter;
+            getter = pGetter;
+            objectSet = null;
+        }
+        public ParameterDefn(string pName, string pDesc, T pDefault, PGetValue<T> pGetter, PSetValue<T> pSetter, PSetOnObject<T> pObjSetter)
+            : base(pName, pDesc)
         {
-            name = n;
-            desc = d;
-            defaultValue = v;
-            userParam = u;
-            getter = g;
-            setter = s;
-            onObject = null;
+            defaultValue = pDefault;
+            setter = pSetter;
+            getter = pGetter;
+            objectSet = pObjSetter;
         }
-        public ParameterDefn(string n, string d, float v, ParamUser u, ParamGet g, ParamSet s, SetOnObject o)
+        // Simple parameter variable where property name is the same as the INI file name
+        //     and the value is only a simple get and set.
+        public ParameterDefn(string pName, string pDesc, T pDefault)
+            : base(pName, pDesc)
         {
-            name = n;
-            desc = d;
-            defaultValue = v;
-            userParam = u;
-            getter = g;
-            setter = s;
-            onObject = o;
+            defaultValue = pDefault;
+            setter = (s, v) => { SetValueByName(s, name, v); };
+            getter = (s) => { return GetValueByName(s, name); };
+            objectSet = null;
+        }
+        // Use reflection to find the property named 'pName' in BSParam and assign 'val' to same.
+        private void SetValueByName(BSScene s, string pName, T val)
+        {
+            PropertyInfo prop = typeof(BSParam).GetProperty(pName, BindingFlags.Public | BindingFlags.Static | BindingFlags.FlattenHierarchy);
+            if (prop == null)
+            {
+                // This should only be output when someone adds a new INI parameter and misspells the name.
+                s.Logger.ErrorFormat("{0} SetValueByName: did not find '{1}'. Verify specified property name is the same as the given INI parameters name.", LogHeader, pName);
+            }
+            else
+            {
+                prop.SetValue(null, val, null);
+            }
+        }
+        // Use reflection to find the property named 'pName' in BSParam and return the value in same.
+        private T GetValueByName(BSScene s, string pName)
+        {
+            PropertyInfo prop = typeof(BSParam).GetProperty(pName, BindingFlags.Public | BindingFlags.Static | BindingFlags.FlattenHierarchy);
+            if (prop == null)
+            {
+                // This should only be output when someone adds a new INI parameter and misspells the name.
+                s.Logger.ErrorFormat("{0} GetValueByName: did not find '{1}'. Verify specified property name is the same as the given INI parameter name.", LogHeader, pName);
+            }
+            return (T)prop.GetValue(null, null);
+        }
+        public override void AssignDefault(BSScene s)
+        {
+            setter(s, defaultValue);
+        }
+        public override string GetValue(BSScene s)
+        {
+            return getter(s).ToString();
+        }
+        public override void SetValue(BSScene s, string valAsString)
+        {
+            // Get the generic type of the setter
+            Type genericType = setter.GetType().GetGenericArguments()[0];
+            // Find the 'Parse' method on that type
+            System.Reflection.MethodInfo parser = null;
+            try
+            {
+                parser = genericType.GetMethod("Parse", new Type[] { typeof(String) } );
+            }
+            catch (Exception e)
+            {
+                s.Logger.ErrorFormat("{0} Exception getting parser for type '{1}': {2}", LogHeader, genericType, e);
+                parser = null;
+            }
+            if (parser != null)
+            {
+                // Parse the input string
+                try
+                {
+                    T setValue = (T)parser.Invoke(genericType, new Object[] { valAsString });
+                    // Store the parsed value
+                    setter(s, setValue);
+                    // s.Logger.DebugFormat("{0} Parameter {1} = {2}", LogHeader, name, setValue);
+                }
+                catch
+                {
+                    s.Logger.ErrorFormat("{0} Failed parsing parameter value '{1}' as type '{2}'", LogHeader, valAsString, genericType);
+                }
+            }
+            else
+            {
+                s.Logger.ErrorFormat("{0} Could not find parameter parser for type '{1}'", LogHeader, genericType);
+            }
+        }
+        public override bool HasSetOnObject
+        {
+            get { return objectSet != null; }
+        }
+        public override void SetOnObject(BSScene s, BSPhysObject obj)
+        {
+            if (objectSet != null)
+                objectSet(s, obj);
         }
     }
 
@@ -144,359 +332,356 @@ public static class BSParam
     //    location somewhere in the program and make an entry in this table with the
     //    getters and setters.
     // It is easiest to find an existing definition and copy it.
-    // Parameter values are floats. Booleans are converted to a floating value.
     //
-    // A ParameterDefn() takes the following parameters:
+    // A ParameterDefn<T>() takes the following parameters:
     //    -- the text name of the parameter. This is used for console input and ini file.
     //    -- a short text description of the parameter. This shows up in the console listing.
-    //    -- a default value (float)
-    //    -- a delegate for fetching the parameter from the ini file.
-    //          Should handle fetching the right type from the ini file and converting it.
-    //    -- a delegate for getting the value as a float
-    //    -- a delegate for setting the value from a float
+    //    -- a default value
+    //    -- a delegate for getting the value
+    //    -- a delegate for setting the value
     //    -- an optional delegate to update the value in the world. Most often used to
     //          push the new value to an in-world object.
     //
     // The single letter parameters for the delegates are:
     //    s = BSScene
     //    o = BSPhysObject
-    //    p = string parameter name
-    //    l = localID of referenced object
-    //    v = value (float)
-    //    cf = parameter configuration class (for fetching values from ini file)
-    private static ParameterDefn[] ParameterDefinitions =
+    //    v = value (appropriate type)
+    private static ParameterDefnBase[] ParameterDefinitions =
     {
-        new ParameterDefn("MeshSculptedPrim", "Whether to create meshes for sculpties",
-            ConfigurationParameters.numericTrue,
-            (s,cf,p,v) => { ShouldMeshSculptedPrim = cf.GetBoolean(p, BSParam.BoolNumeric(v)); },
-            (s) => { return BSParam.NumericBool(ShouldMeshSculptedPrim); },
-            (s,p,l,v) => { ShouldMeshSculptedPrim = BSParam.BoolNumeric(v); } ),
-        new ParameterDefn("ForceSimplePrimMeshing", "If true, only use primitive meshes for objects",
-            ConfigurationParameters.numericFalse,
-            (s,cf,p,v) => { ShouldForceSimplePrimMeshing = cf.GetBoolean(p, BSParam.BoolNumeric(v)); },
-            (s) => { return BSParam.NumericBool(ShouldForceSimplePrimMeshing); },
-            (s,p,l,v) => { ShouldForceSimplePrimMeshing = BSParam.BoolNumeric(v); } ),
-        new ParameterDefn("UseHullsForPhysicalObjects", "If true, create hulls for physical objects",
-            ConfigurationParameters.numericTrue,
-            (s,cf,p,v) => { ShouldUseHullsForPhysicalObjects = cf.GetBoolean(p, BSParam.BoolNumeric(v)); },
-            (s) => { return BSParam.NumericBool(ShouldUseHullsForPhysicalObjects); },
-            (s,p,l,v) => { ShouldUseHullsForPhysicalObjects = BSParam.BoolNumeric(v); } ),
-
-        new ParameterDefn("MeshLevelOfDetail", "Level of detail to render meshes (32, 16, 8 or 4. 32=most detailed)",
-            8f,
-            (s,cf,p,v) => { MeshLOD = (float)cf.GetInt(p, (int)v); },
+        new ParameterDefn<bool>("MeshSculptedPrim", "Whether to create meshes for sculpties",
+            true,
+            (s) => { return ShouldMeshSculptedPrim; },
+            (s,v) => { ShouldMeshSculptedPrim = v; } ),
+        new ParameterDefn<bool>("ForceSimplePrimMeshing", "If true, only use primitive meshes for objects",
+            false,
+            (s) => { return ShouldForceSimplePrimMeshing; },
+            (s,v) => { ShouldForceSimplePrimMeshing = v; } ),
+        new ParameterDefn<bool>("UseHullsForPhysicalObjects", "If true, create hulls for physical objects",
+            true,
+            (s) => { return ShouldUseHullsForPhysicalObjects; },
+            (s,v) => { ShouldUseHullsForPhysicalObjects = v; } ),
+        new ParameterDefn<bool>("ShouldRemoveZeroWidthTriangles", "If true, remove degenerate triangles from meshes",
+            true ),
+        new ParameterDefn<bool>("ShouldUseBulletHACD", "If true, use the Bullet version of HACD",
+            false ),
+        new ParameterDefn<bool>("ShouldUseSingleConvexHullForPrims", "If true, use a single convex hull shape for physical prims",
+            true ),
+
+        new ParameterDefn<int>("CrossingFailuresBeforeOutOfBounds", "How forgiving we are about getting into adjactent regions",
+            5 ),
+        new ParameterDefn<float>("UpdateVelocityChangeThreshold", "Change in updated velocity required before reporting change to simulator",
+            0.1f ),
+
+        new ParameterDefn<float>("MeshLevelOfDetail", "Level of detail to render meshes (32, 16, 8 or 4. 32=most detailed)",
+            32f,
             (s) => { return MeshLOD; },
-            (s,p,l,v) => { MeshLOD = v; } ),
-        new ParameterDefn("MeshLevelOfDetailMegaPrim", "Level of detail to render meshes larger than threshold meters",
-            16f,
-            (s,cf,p,v) => { MeshMegaPrimLOD = (float)cf.GetInt(p, (int)v); },
-            (s) => { return MeshMegaPrimLOD; },
-            (s,p,l,v) => { MeshMegaPrimLOD = v; } ),
-        new ParameterDefn("MeshLevelOfDetailMegaPrimThreshold", "Size (in meters) of a mesh before using MeshMegaPrimLOD",
+            (s,v) => { MeshLOD = v; } ),
+        new ParameterDefn<float>("MeshLevelOfDetailCircular", "Level of detail for prims with circular cuts or shapes",
+            32f,
+            (s) => { return MeshCircularLOD; },
+            (s,v) => { MeshCircularLOD = v; } ),
+        new ParameterDefn<float>("MeshLevelOfDetailMegaPrimThreshold", "Size (in meters) of a mesh before using MeshMegaPrimLOD",
             10f,
-            (s,cf,p,v) => { MeshMegaPrimThreshold = (float)cf.GetInt(p, (int)v); },
             (s) => { return MeshMegaPrimThreshold; },
-            (s,p,l,v) => { MeshMegaPrimThreshold = v; } ),
-        new ParameterDefn("SculptLevelOfDetail", "Level of detail to render sculpties (32, 16, 8 or 4. 32=most detailed)",
+            (s,v) => { MeshMegaPrimThreshold = v; } ),
+        new ParameterDefn<float>("MeshLevelOfDetailMegaPrim", "Level of detail to render meshes larger than threshold meters",
+            32f,
+            (s) => { return MeshMegaPrimLOD; },
+            (s,v) => { MeshMegaPrimLOD = v; } ),
+        new ParameterDefn<float>("SculptLevelOfDetail", "Level of detail to render sculpties (32, 16, 8 or 4. 32=most detailed)",
             32f,
-            (s,cf,p,v) => { SculptLOD = (float)cf.GetInt(p, (int)v); },
             (s) => { return SculptLOD; },
-            (s,p,l,v) => { SculptLOD = v; } ),
+            (s,v) => { SculptLOD = 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)",
+        new ParameterDefn<int>("MaxSubStep", "In simulation step, maximum number of substeps",
+            10,
+            (s) => { return s.m_maxSubSteps; },
+            (s,v) => { s.m_maxSubSteps = (int)v; } ),
+        new ParameterDefn<float>("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("NominalFrameRate", "The base frame rate we claim",
+            (s) => { return s.m_fixedTimeStep; },
+            (s,v) => { s.m_fixedTimeStep = v; } ),
+        new ParameterDefn<float>("NominalFrameRate", "The base frame rate we claim",
             55f,
-            (s,cf,p,v) => { s.NominalFrameRate = cf.GetInt(p, (int)v); },
-            (s) => { return (float)s.NominalFrameRate; },
-            (s,p,l,v) => { s.NominalFrameRate = (int)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("MaxTaintsToProcessPerStep", "Number of update taints to process before each simulation step",
-            500f,
-            (s,cf,p,v) => { s.m_taintsToProcessPerStep = cf.GetInt(p, (int)v); },
-            (s) => { return (float)s.m_taintsToProcessPerStep; },
-            (s,p,l,v) => { s.m_taintsToProcessPerStep = (int)v; } ),
-        new ParameterDefn("MinObjectMass", "Minimum object mass (0.0001)",
+            (s) => { return s.NominalFrameRate; },
+            (s,v) => { s.NominalFrameRate = (int)v; } ),
+        new ParameterDefn<int>("MaxCollisionsPerFrame", "Max collisions returned at end of each frame",
+            2048,
+            (s) => { return s.m_maxCollisionsPerFrame; },
+            (s,v) => { s.m_maxCollisionsPerFrame = (int)v; } ),
+        new ParameterDefn<int>("MaxUpdatesPerFrame", "Max updates returned at end of each frame",
+            8000,
+            (s) => { return s.m_maxUpdatesPerFrame; },
+            (s,v) => { s.m_maxUpdatesPerFrame = (int)v; } ),
+
+        new ParameterDefn<float>("MinObjectMass", "Minimum object mass (0.0001)",
             0.0001f,
-            (s,cf,p,v) => { MinimumObjectMass = cf.GetFloat(p, v); },
-            (s) => { return (float)MinimumObjectMass; },
-            (s,p,l,v) => { MinimumObjectMass = v; } ),
-        new ParameterDefn("MaxObjectMass", "Maximum object mass (10000.01)",
+            (s) => { return MinimumObjectMass; },
+            (s,v) => { MinimumObjectMass = v; } ),
+        new ParameterDefn<float>("MaxObjectMass", "Maximum object mass (10000.01)",
             10000.01f,
-            (s,cf,p,v) => { MaximumObjectMass = cf.GetFloat(p, v); },
-            (s) => { return (float)MaximumObjectMass; },
-            (s,p,l,v) => { MaximumObjectMass = v; } ),
-
-        new ParameterDefn("PID_D", "Derivitive factor for motion smoothing",
-            2200f,
-            (s,cf,p,v) => { PID_D = cf.GetFloat(p, v); },
-            (s) => { return (float)PID_D; },
-            (s,p,l,v) => { PID_D = v; } ),
-        new ParameterDefn("PID_P", "Parameteric factor for motion smoothing",
-            900f,
-            (s,cf,p,v) => { PID_P = cf.GetFloat(p, v); },
-            (s) => { return (float)PID_P; },
-            (s,p,l,v) => { PID_P = v; } ),
-
-        new ParameterDefn("DefaultFriction", "Friction factor used on new objects",
+            (s) => { return MaximumObjectMass; },
+            (s,v) => { MaximumObjectMass = v; } ),
+        new ParameterDefn<float>("MaxLinearVelocity", "Maximum velocity magnitude that can be assigned to an object",
+            1000.0f,
+            (s) => { return MaxLinearVelocity; },
+            (s,v) => { MaxLinearVelocity = v; MaxLinearVelocitySquared = v * v; } ),
+        new ParameterDefn<float>("MaxAngularVelocity", "Maximum rotational velocity magnitude that can be assigned to an object",
+            1000.0f,
+            (s) => { return MaxAngularVelocity; },
+            (s,v) => { MaxAngularVelocity = v;  MaxAngularVelocitySquared = v * v; } ),
+        // LL documentation says thie number should be 20f for llApplyImpulse and 200f for llRezObject
+        new ParameterDefn<float>("MaxAddForceMagnitude", "Maximum force that can be applied by llApplyImpulse (SL says 20f)",
+            20000.0f,
+            (s) => { return MaxAddForceMagnitude; },
+            (s,v) => { MaxAddForceMagnitude = v; MaxAddForceMagnitudeSquared = v * v; } ),
+        // Density is passed around as 100kg/m3. This scales that to 1kg/m3.
+        new ParameterDefn<float>("DensityScaleFactor", "Conversion for simulator/viewer density (100kg/m3) to physical density (1kg/m3)",
+            0.01f ),
+
+        new ParameterDefn<float>("PID_D", "Derivitive factor for motion smoothing",
+            2200f ),
+        new ParameterDefn<float>("PID_P", "Parameteric factor for motion smoothing",
+            900f ),
+
+        new ParameterDefn<float>("DefaultFriction", "Friction factor used on new objects",
             0.2f,
-            (s,cf,p,v) => { s.UnmanagedParams[0].defaultFriction = cf.GetFloat(p, v); },
-            (s) => { return s.UnmanagedParams[0].defaultFriction; },
-            (s,p,l,v) => { s.UnmanagedParams[0].defaultFriction = v; } ),
-        new ParameterDefn("DefaultDensity", "Density for new objects" ,
+            (s) => { return DefaultFriction; },
+            (s,v) => { DefaultFriction = v; s.UnmanagedParams[0].defaultFriction = v; } ),
+        new ParameterDefn<float>("DefaultDensity", "Density for new objects" ,
             10.000006836f,  // Aluminum g/cm3
-            (s,cf,p,v) => { s.UnmanagedParams[0].defaultDensity = cf.GetFloat(p, v); },
-            (s) => { return s.UnmanagedParams[0].defaultDensity; },
-            (s,p,l,v) => { s.UnmanagedParams[0].defaultDensity = v; } ),
-        new ParameterDefn("DefaultRestitution", "Bouncyness of an object" ,
+            (s) => { return DefaultDensity; },
+            (s,v) => { DefaultDensity = v; s.UnmanagedParams[0].defaultDensity = v; } ),
+        new ParameterDefn<float>("DefaultRestitution", "Bouncyness of an object" ,
             0f,
-            (s,cf,p,v) => { s.UnmanagedParams[0].defaultRestitution = cf.GetFloat(p, v); },
-            (s) => { return s.UnmanagedParams[0].defaultRestitution; },
-            (s,p,l,v) => { s.UnmanagedParams[0].defaultRestitution = v; } ),
-        new ParameterDefn("CollisionMargin", "Margin around objects before collisions are calculated (must be zero!)",
+            (s) => { return DefaultRestitution; },
+            (s,v) => { DefaultRestitution = v; s.UnmanagedParams[0].defaultRestitution = v; } ),
+        new ParameterDefn<float>("CollisionMargin", "Margin around objects before collisions are calculated (must be zero!)",
             0.04f,
-            (s,cf,p,v) => { s.UnmanagedParams[0].collisionMargin = cf.GetFloat(p, v); },
-            (s) => { return s.UnmanagedParams[0].collisionMargin; },
-            (s,p,l,v) => { s.UnmanagedParams[0].collisionMargin = v; } ),
-        new ParameterDefn("Gravity", "Vertical force of gravity (negative means down)",
+            (s) => { return CollisionMargin; },
+            (s,v) => { CollisionMargin = v; s.UnmanagedParams[0].collisionMargin = v; } ),
+        new ParameterDefn<float>("Gravity", "Vertical force of gravity (negative means down)",
             -9.80665f,
-            (s,cf,p,v) => { s.UnmanagedParams[0].gravity = cf.GetFloat(p, v); },
-            (s) => { return s.UnmanagedParams[0].gravity; },
-            (s,p,l,v) => { s.UpdateParameterObject((x)=>{s.UnmanagedParams[0].gravity=x;}, p, PhysParameterEntry.APPLY_TO_NONE, v); },
-            (s,o,v) => { s.PE.SetGravity(o.PhysBody, new Vector3(0f,0f,v)); } ),
+            (s) => { return Gravity; },
+            (s,v) => { Gravity = v; s.UnmanagedParams[0].gravity = v; },
+            (s,o) => { s.PE.SetGravity(o.PhysBody, new Vector3(0f,0f,Gravity)); } ),
 
 
-        new ParameterDefn("LinearDamping", "Factor to damp linear movement per second (0.0 - 1.0)",
+        new ParameterDefn<float>("LinearDamping", "Factor to damp linear movement per second (0.0 - 1.0)",
             0f,
-            (s,cf,p,v) => { LinearDamping = cf.GetFloat(p, v); },
             (s) => { return LinearDamping; },
-            (s,p,l,v) => { s.UpdateParameterObject((x)=>{LinearDamping=x;}, p, l, v); },
-            (s,o,v) => { s.PE.SetDamping(o.PhysBody, v, AngularDamping); } ),
-        new ParameterDefn("AngularDamping", "Factor to damp angular movement per second (0.0 - 1.0)",
+            (s,v) => { LinearDamping = v; },
+            (s,o) => { s.PE.SetDamping(o.PhysBody, LinearDamping, AngularDamping); } ),
+        new ParameterDefn<float>("AngularDamping", "Factor to damp angular movement per second (0.0 - 1.0)",
             0f,
-            (s,cf,p,v) => { AngularDamping = cf.GetFloat(p, v); },
             (s) => { return AngularDamping; },
-            (s,p,l,v) => { s.UpdateParameterObject((x)=>{AngularDamping=x;}, p, l, v); },
-            (s,o,v) => { s.PE.SetDamping(o.PhysBody, LinearDamping, v); } ),
-        new ParameterDefn("DeactivationTime", "Seconds before considering an object potentially static",
+            (s,v) => { AngularDamping = v; },
+            (s,o) => { s.PE.SetDamping(o.PhysBody, LinearDamping, AngularDamping); } ),
+        new ParameterDefn<float>("DeactivationTime", "Seconds before considering an object potentially static",
             0.2f,
-            (s,cf,p,v) => { DeactivationTime = cf.GetFloat(p, v); },
             (s) => { return DeactivationTime; },
-            (s,p,l,v) => { s.UpdateParameterObject((x)=>{DeactivationTime=x;}, p, l, v); },
-            (s,o,v) => { s.PE.SetDeactivationTime(o.PhysBody, v); } ),
-        new ParameterDefn("LinearSleepingThreshold", "Seconds to measure linear movement before considering static",
+            (s,v) => { DeactivationTime = v; },
+            (s,o) => { s.PE.SetDeactivationTime(o.PhysBody, DeactivationTime); } ),
+        new ParameterDefn<float>("LinearSleepingThreshold", "Seconds to measure linear movement before considering static",
             0.8f,
-            (s,cf,p,v) => { LinearSleepingThreshold = cf.GetFloat(p, v); },
             (s) => { return LinearSleepingThreshold; },
-            (s,p,l,v) => { s.UpdateParameterObject((x)=>{LinearSleepingThreshold=x;}, p, l, v); },
-            (s,o,v) => { s.PE.SetSleepingThresholds(o.PhysBody, v, v); } ),
-        new ParameterDefn("AngularSleepingThreshold", "Seconds to measure angular movement before considering static",
+            (s,v) => { LinearSleepingThreshold = v;},
+            (s,o) => { s.PE.SetSleepingThresholds(o.PhysBody, LinearSleepingThreshold, AngularSleepingThreshold); } ),
+        new ParameterDefn<float>("AngularSleepingThreshold", "Seconds to measure angular movement before considering static",
             1.0f,
-            (s,cf,p,v) => { AngularSleepingThreshold = cf.GetFloat(p, v); },
             (s) => { return AngularSleepingThreshold; },
-            (s,p,l,v) => { s.UpdateParameterObject((x)=>{AngularSleepingThreshold=x;}, p, l, v); },
-            (s,o,v) => { s.PE.SetSleepingThresholds(o.PhysBody, v, v); } ),
-        new ParameterDefn("CcdMotionThreshold", "Continuious collision detection threshold (0 means no CCD)" ,
-            0f,     // set to zero to disable
-            (s,cf,p,v) => { CcdMotionThreshold = cf.GetFloat(p, v); },
+            (s,v) => { AngularSleepingThreshold = v;},
+            (s,o) => { s.PE.SetSleepingThresholds(o.PhysBody, LinearSleepingThreshold, AngularSleepingThreshold); } ),
+        new ParameterDefn<float>("CcdMotionThreshold", "Continuious collision detection threshold (0 means no CCD)" ,
+            0.0f,     // set to zero to disable
             (s) => { return CcdMotionThreshold; },
-            (s,p,l,v) => { s.UpdateParameterObject((x)=>{CcdMotionThreshold=x;}, p, l, v); },
-            (s,o,v) => { s.PE.SetCcdMotionThreshold(o.PhysBody, v); } ),
-        new ParameterDefn("CcdSweptSphereRadius", "Continuious collision detection test radius" ,
-            0f,
-            (s,cf,p,v) => { CcdSweptSphereRadius = cf.GetFloat(p, v); },
+            (s,v) => { CcdMotionThreshold = v;},
+            (s,o) => { s.PE.SetCcdMotionThreshold(o.PhysBody, CcdMotionThreshold); } ),
+        new ParameterDefn<float>("CcdSweptSphereRadius", "Continuious collision detection test radius" ,
+            0.2f,
             (s) => { return CcdSweptSphereRadius; },
-            (s,p,l,v) => { s.UpdateParameterObject((x)=>{CcdSweptSphereRadius=x;}, p, l, v); },
-            (s,o,v) => { s.PE.SetCcdSweptSphereRadius(o.PhysBody, v); } ),
-        new ParameterDefn("ContactProcessingThreshold", "Distance between contacts before doing collision check" ,
-            0.1f,
-            (s,cf,p,v) => { ContactProcessingThreshold = cf.GetFloat(p, v); },
+            (s,v) => { CcdSweptSphereRadius = v;},
+            (s,o) => { s.PE.SetCcdSweptSphereRadius(o.PhysBody, CcdSweptSphereRadius); } ),
+        new ParameterDefn<float>("ContactProcessingThreshold", "Distance above which contacts can be discarded (0 means no discard)" ,
+            0.0f,
             (s) => { return ContactProcessingThreshold; },
-            (s,p,l,v) => { s.UpdateParameterObject((x)=>{ContactProcessingThreshold=x;}, p, l, v); },
-            (s,o,v) => { s.PE.SetContactProcessingThreshold(o.PhysBody, v); } ),
-
-            new ParameterDefn("TerrainImplementation", "Type of shape to use for terrain (0=heightmap, 1=mesh)",
-            (float)BSTerrainPhys.TerrainImplementation.Mesh,
-            (s,cf,p,v) => { TerrainImplementation = cf.GetFloat(p,v); },
-            (s) => { return TerrainImplementation; },
-            (s,p,l,v) => { TerrainImplementation = v; } ),
-        new ParameterDefn("TerrainFriction", "Factor to reduce movement against terrain surface" ,
-            0.3f,
-            (s,cf,p,v) => { TerrainFriction = cf.GetFloat(p, v); },
-            (s) => { return TerrainFriction; },
-            (s,p,l,v) => { TerrainFriction = v;  /* TODO: set on real terrain */} ),
-        new ParameterDefn("TerrainHitFraction", "Distance to measure hit collisions" ,
-            0.8f,
-            (s,cf,p,v) => { TerrainHitFraction = cf.GetFloat(p, v); },
-            (s) => { return TerrainHitFraction; },
-            (s,p,l,v) => { TerrainHitFraction = v; /* TODO: set on real terrain */ } ),
-        new ParameterDefn("TerrainRestitution", "Bouncyness" ,
+            (s,v) => { ContactProcessingThreshold = v;},
+            (s,o) => { s.PE.SetContactProcessingThreshold(o.PhysBody, ContactProcessingThreshold); } ),
+
+            new ParameterDefn<float>("TerrainImplementation", "Type of shape to use for terrain (0=heightmap, 1=mesh)",
+            (float)BSTerrainPhys.TerrainImplementation.Mesh ),
+        new ParameterDefn<int>("TerrainMeshMagnification", "Number of times the 256x256 heightmap is multiplied to create the terrain mesh" ,
+            2 ),
+        new ParameterDefn<float>("TerrainFriction", "Factor to reduce movement against terrain surface" ,
+            0.3f ),
+        new ParameterDefn<float>("TerrainHitFraction", "Distance to measure hit collisions" ,
+            0.8f ),
+        new ParameterDefn<float>("TerrainRestitution", "Bouncyness" ,
+            0f ),
+        new ParameterDefn<float>("TerrainContactProcessingThreshold", "Distance from terrain to stop processing collisions" ,
+            0.0f ),
+        new ParameterDefn<float>("TerrainCollisionMargin", "Margin where collision checking starts" ,
+            0.08f ),
+
+        new ParameterDefn<float>("AvatarFriction", "Factor to reduce movement against an avatar. Changed on avatar recreation.",
+            0.2f ),
+        new ParameterDefn<float>("AvatarStandingFriction", "Avatar friction when standing. Changed on avatar recreation.",
+            0.95f ),
+        new ParameterDefn<float>("AvatarAlwaysRunFactor", "Speed multiplier if avatar is set to always run",
+            1.3f ),
+        new ParameterDefn<float>("AvatarDensity", "Density of an avatar. Changed on avatar recreation.",
+            3.5f) ,
+        new ParameterDefn<float>("AvatarRestitution", "Bouncyness. Changed on avatar recreation.",
+            0f ),
+        new ParameterDefn<float>("AvatarCapsuleWidth", "The distance between the sides of the avatar capsule",
+            0.6f ) ,
+        new ParameterDefn<float>("AvatarCapsuleDepth", "The distance between the front and back of the avatar capsule",
+            0.45f ),
+        new ParameterDefn<float>("AvatarCapsuleHeight", "Default height of space around avatar",
+            1.5f ),
+            new ParameterDefn<float>("AvatarContactProcessingThreshold", "Distance from capsule to check for collisions",
+            0.1f ),
+            new ParameterDefn<float>("AvatarBelowGroundUpCorrectionMeters", "Meters to move avatar up if it seems to be below ground",
+            1.0f ),
+            new ParameterDefn<float>("AvatarStepHeight", "Height of a step obstacle to consider step correction",
+            0.6f ) ,
+            new ParameterDefn<float>("AvatarStepApproachFactor", "Factor to control angle of approach to step (0=straight on)",
+            0.6f ),
+            new ParameterDefn<float>("AvatarStepForceFactor", "Controls the amount of force up applied to step up onto a step",
+            1.0f ),
+            new ParameterDefn<float>("AvatarStepUpCorrectionFactor", "Multiplied by height of step collision to create up movement at step",
+            1.0f ),
+            new ParameterDefn<int>("AvatarStepSmoothingSteps", "Number of frames after a step collision that we continue walking up stairs",
+            2 ),
+
+        new ParameterDefn<float>("VehicleMaxLinearVelocity", "Maximum velocity magnitude that can be assigned to a vehicle",
+            1000.0f,
+            (s) => { return (float)VehicleMaxLinearVelocity; },
+            (s,v) => { VehicleMaxLinearVelocity = v; VehicleMaxLinearVelocitySquared = v * v; } ),
+        new ParameterDefn<float>("VehicleMaxAngularVelocity", "Maximum rotational velocity magnitude that can be assigned to a vehicle",
+            12.0f,
+            (s) => { return (float)VehicleMaxAngularVelocity; },
+            (s,v) => { VehicleMaxAngularVelocity = v; VehicleMaxAngularVelocitySq = v * v; } ),
+        new ParameterDefn<float>("VehicleAngularDamping", "Factor to damp vehicle angular movement per second (0.0 - 1.0)",
+            0.0f ),
+        new ParameterDefn<Vector3>("VehicleLinearFactor", "Fraction of physical linear changes applied to vehicle (<0,0,0> to <1,1,1>)",
+            new Vector3(1f, 1f, 1f) ),
+        new ParameterDefn<Vector3>("VehicleAngularFactor", "Fraction of physical angular changes applied to vehicle (<0,0,0> to <1,1,1>)",
+            new Vector3(1f, 1f, 1f) ),
+        new ParameterDefn<float>("VehicleFriction", "Friction of vehicle on the ground (0.0 - 1.0)",
+            0.0f ),
+        new ParameterDefn<float>("VehicleRestitution", "Bouncyness factor for vehicles (0.0 - 1.0)",
+            0.0f ),
+        new ParameterDefn<float>("VehicleGroundGravityFudge", "Factor to multiply gravity if a ground vehicle is probably on the ground (0.0 - 1.0)",
+            0.2f ),
+        new ParameterDefn<float>("VehicleAngularBankingTimescaleFudge", "Factor to multiple angular banking timescale. Tune to increase realism.",
+            60.0f ),
+        new ParameterDefn<bool>("VehicleDebuggingEnable", "Turn on/off vehicle debugging",
+            false ),
+
+            new ParameterDefn<float>("MaxPersistantManifoldPoolSize", "Number of manifolds pooled (0 means default of 4096)",
             0f,
-            (s,cf,p,v) => { TerrainRestitution = cf.GetFloat(p, v); },
-            (s) => { return TerrainRestitution; },
-            (s,p,l,v) => { TerrainRestitution = v;  /* TODO: set on real terrain */ } ),
-        new ParameterDefn("TerrainCollisionMargin", "Margin where collision checking starts" ,
-            0.04f,
-            (s,cf,p,v) => { TerrainCollisionMargin = cf.GetFloat(p, v); },
-            (s) => { return TerrainCollisionMargin; },
-            (s,p,l,v) => { TerrainCollisionMargin = v;  /* TODO: set on real terrain */ } ),
-
-        new ParameterDefn("AvatarFriction", "Factor to reduce movement against an avatar. Changed on avatar recreation.",
-            0.2f,
-            (s,cf,p,v) => { AvatarFriction = cf.GetFloat(p, v); },
-            (s) => { return AvatarFriction; },
-            (s,p,l,v) => { s.UpdateParameterObject((x)=>{AvatarFriction=x;}, p, l, v); } ),
-        new ParameterDefn("AvatarStandingFriction", "Avatar friction when standing. Changed on avatar recreation.",
-            10.0f,
-            (s,cf,p,v) => { AvatarStandingFriction = cf.GetFloat(p, v); },
-            (s) => { return AvatarStandingFriction; },
-            (s,p,l,v) => { AvatarStandingFriction = v; } ),
-        new ParameterDefn("AvatarAlwaysRunFactor", "Speed multiplier if avatar is set to always run",
-            1.3f,
-            (s,cf,p,v) => { AvatarAlwaysRunFactor = cf.GetFloat(p, v); },
-            (s) => { return AvatarAlwaysRunFactor; },
-            (s,p,l,v) => { AvatarAlwaysRunFactor = v; } ),
-        new ParameterDefn("AvatarDensity", "Density of an avatar. Changed on avatar recreation.",
-            3.5f,
-            (s,cf,p,v) => { AvatarDensity = cf.GetFloat(p, v); },
-            (s) => { return AvatarDensity; },
-            (s,p,l,v) => { s.UpdateParameterObject((x)=>{AvatarDensity=x;}, p, l, v); } ),
-        new ParameterDefn("AvatarRestitution", "Bouncyness. Changed on avatar recreation.",
+            (s) => { return MaxPersistantManifoldPoolSize; },
+            (s,v) => { MaxPersistantManifoldPoolSize = v; s.UnmanagedParams[0].maxPersistantManifoldPoolSize = v; } ),
+            new ParameterDefn<float>("MaxCollisionAlgorithmPoolSize", "Number of collisions pooled (0 means default of 4096)",
             0f,
-            (s,cf,p,v) => { AvatarRestitution = cf.GetFloat(p, v); },
-            (s) => { return AvatarRestitution; },
-            (s,p,l,v) => { s.UpdateParameterObject((x)=>{AvatarRestitution=x;}, p, l, v); } ),
-        new ParameterDefn("AvatarCapsuleWidth", "The distance between the sides of the avatar capsule",
-            0.6f,
-            (s,cf,p,v) => { AvatarCapsuleWidth = cf.GetFloat(p, v); },
-            (s) => { return AvatarCapsuleWidth; },
-            (s,p,l,v) => { s.UpdateParameterObject((x)=>{AvatarCapsuleWidth=x;}, p, l, v); } ),
-        new ParameterDefn("AvatarCapsuleDepth", "The distance between the front and back of the avatar capsule",
-            0.45f,
-            (s,cf,p,v) => { AvatarCapsuleDepth = cf.GetFloat(p, v); },
-            (s) => { return AvatarCapsuleDepth; },
-            (s,p,l,v) => { s.UpdateParameterObject((x)=>{AvatarCapsuleDepth=x;}, p, l, v); } ),
-        new ParameterDefn("AvatarCapsuleHeight", "Default height of space around avatar",
-            1.5f,
-            (s,cf,p,v) => { AvatarCapsuleHeight = cf.GetFloat(p, v); },
-            (s) => { return AvatarCapsuleHeight; },
-            (s,p,l,v) => { s.UpdateParameterObject((x)=>{AvatarCapsuleHeight=x;}, p, l, v); } ),
-            new ParameterDefn("AvatarContactProcessingThreshold", "Distance from capsule to check for collisions",
-            0.1f,
-            (s,cf,p,v) => { AvatarContactProcessingThreshold = cf.GetFloat(p, v); },
-            (s) => { return AvatarContactProcessingThreshold; },
-            (s,p,l,v) => { s.UpdateParameterObject((x)=>{AvatarContactProcessingThreshold=x;}, p, l, v); } ),
-
-        new ParameterDefn("VehicleAngularDamping", "Factor to damp vehicle angular movement per second (0.0 - 1.0)",
-            0.95f,
-            (s,cf,p,v) => { VehicleAngularDamping = cf.GetFloat(p, v); },
-            (s) => { return VehicleAngularDamping; },
-            (s,p,l,v) => { VehicleAngularDamping = v; } ),
-
-            new ParameterDefn("MaxPersistantManifoldPoolSize", "Number of manifolds pooled (0 means default of 4096)",
+            (s) => { return MaxCollisionAlgorithmPoolSize; },
+            (s,v) => { MaxCollisionAlgorithmPoolSize = v; s.UnmanagedParams[0].maxCollisionAlgorithmPoolSize = v; } ),
+            new ParameterDefn<bool>("ShouldDisableContactPoolDynamicAllocation", "Enable to allow large changes in object count",
+            false,
+            (s) => { return ShouldDisableContactPoolDynamicAllocation; },
+            (s,v) => { ShouldDisableContactPoolDynamicAllocation = v;
+                        s.UnmanagedParams[0].shouldDisableContactPoolDynamicAllocation = NumericBool(v); } ),
+            new ParameterDefn<bool>("ShouldForceUpdateAllAabbs", "Enable to recomputer AABBs every simulator step",
+            false,
+            (s) => { return ShouldForceUpdateAllAabbs; },
+            (s,v) => { ShouldForceUpdateAllAabbs = v; s.UnmanagedParams[0].shouldForceUpdateAllAabbs = NumericBool(v); } ),
+            new ParameterDefn<bool>("ShouldRandomizeSolverOrder", "Enable for slightly better stacking interaction",
+            true,
+            (s) => { return ShouldRandomizeSolverOrder; },
+            (s,v) => { ShouldRandomizeSolverOrder = v; s.UnmanagedParams[0].shouldRandomizeSolverOrder = NumericBool(v); } ),
+            new ParameterDefn<bool>("ShouldSplitSimulationIslands", "Enable splitting active object scanning islands",
+            true,
+            (s) => { return ShouldSplitSimulationIslands; },
+            (s,v) => { ShouldSplitSimulationIslands = v; s.UnmanagedParams[0].shouldSplitSimulationIslands = NumericBool(v); } ),
+            new ParameterDefn<bool>("ShouldEnableFrictionCaching", "Enable friction computation caching",
+            true,
+            (s) => { return ShouldEnableFrictionCaching; },
+            (s,v) => { ShouldEnableFrictionCaching = v; s.UnmanagedParams[0].shouldEnableFrictionCaching = NumericBool(v); } ),
+            new ParameterDefn<float>("NumberOfSolverIterations", "Number of internal iterations (0 means default)",
+            0f,     // zero says use Bullet default
+            (s) => { return NumberOfSolverIterations; },
+            (s,v) => { NumberOfSolverIterations = v; s.UnmanagedParams[0].numberOfSolverIterations = v; } ),
+            new ParameterDefn<bool>("UseSingleSidedMeshes", "Whether to compute collisions based on single sided meshes.",
+            true,
+            (s) => { return UseSingleSidedMeshes; },
+            (s,v) => { UseSingleSidedMeshes = v; s.UnmanagedParams[0].useSingleSidedMeshes = NumericBool(v); } ),
+            new ParameterDefn<float>("GlobalContactBreakingThreshold", "Amount of shape radius before breaking a collision contact (0 says Bullet default (0.2))",
             0f,
-            (s,cf,p,v) => { s.UnmanagedParams[0].maxPersistantManifoldPoolSize = cf.GetFloat(p, v); },
-            (s) => { return s.UnmanagedParams[0].maxPersistantManifoldPoolSize; },
-            (s,p,l,v) => { s.UnmanagedParams[0].maxPersistantManifoldPoolSize = v; } ),
-            new ParameterDefn("MaxCollisionAlgorithmPoolSize", "Number of collisions pooled (0 means default of 4096)",
+            (s) => { return GlobalContactBreakingThreshold; },
+            (s,v) => { GlobalContactBreakingThreshold = v; s.UnmanagedParams[0].globalContactBreakingThreshold = v; } ),
+
+            new ParameterDefn<int>("CSHullMaxDepthSplit", "CS impl: max depth to split for hull. 1-10 but > 7 is iffy",
+            7 ),
+            new ParameterDefn<int>("CSHullMaxDepthSplitForSimpleShapes", "CS impl: max depth setting for simple prim shapes",
+            2 ),
+            new ParameterDefn<float>("CSHullConcavityThresholdPercent", "CS impl: concavity threshold percent (0-20)",
+            5f ),
+            new ParameterDefn<float>("CSHullVolumeConservationThresholdPercent", "percent volume conservation to collapse hulls (0-30)",
+            5f ),
+            new ParameterDefn<int>("CSHullMaxVertices", "CS impl: maximum number of vertices in output hulls. Keep < 50.",
+            32 ),
+            new ParameterDefn<float>("CSHullMaxSkinWidth", "CS impl: skin width to apply to output hulls.",
+            0f ),
+
+            new ParameterDefn<float>("BHullMaxVerticesPerHull", "Bullet impl: max number of vertices per created hull",
+            100f ),
+            new ParameterDefn<float>("BHullMinClusters", "Bullet impl: minimum number of hulls to create per mesh",
+            2f ),
+            new ParameterDefn<float>("BHullCompacityWeight", "Bullet impl: weight factor for how compact to make hulls",
+            0.1f ),
+            new ParameterDefn<float>("BHullVolumeWeight", "Bullet impl: weight factor for volume in created hull",
+            0f ),
+            new ParameterDefn<float>("BHullConcavity", "Bullet impl: weight factor for how convex a created hull can be",
+            100f ),
+            new ParameterDefn<bool>("BHullAddExtraDistPoints", "Bullet impl: whether to add extra vertices for long distance vectors",
+            false ),
+            new ParameterDefn<bool>("BHullAddNeighboursDistPoints", "Bullet impl: whether to add extra vertices between neighbor hulls",
+            false ),
+            new ParameterDefn<bool>("BHullAddFacesPoints", "Bullet impl: whether to add extra vertices to break up hull faces",
+            false ),
+            new ParameterDefn<bool>("BHullShouldAdjustCollisionMargin", "Bullet impl: whether to shrink resulting hulls to account for collision margin",
+            false ),
+
+            new ParameterDefn<float>("LinksetImplementation", "Type of linkset implementation (0=Constraint, 1=Compound, 2=Manual)",
+            (float)BSLinkset.LinksetImplementation.Compound ),
+            new ParameterDefn<bool>("LinkConstraintUseFrameOffset", "For linksets built with constraints, enable frame offsetFor linksets built with constraints, enable frame offset.",
+            false ),
+            new ParameterDefn<bool>("LinkConstraintEnableTransMotor", "Whether to enable translational motor on linkset constraints",
+            true ),
+            new ParameterDefn<float>("LinkConstraintTransMotorMaxVel", "Maximum velocity to be applied by translational motor in linkset constraints",
+            5.0f ),
+            new ParameterDefn<float>("LinkConstraintTransMotorMaxForce", "Maximum force to be applied by translational motor in linkset constraints",
+            0.1f ),
+            new ParameterDefn<float>("LinkConstraintCFM", "Amount constraint can be violated. 0=no violation, 1=infinite. Default=0.1",
+            0.1f ),
+            new ParameterDefn<float>("LinkConstraintERP", "Amount constraint is corrected each tick. 0=none, 1=all. Default = 0.2",
+            0.1f ),
+            new ParameterDefn<float>("LinkConstraintSolverIterations", "Number of solver iterations when computing constraint. (0 = Bullet default)",
+            40 ),
+
+        new ParameterDefn<int>("PhysicsMetricFrames", "Frames between outputting detailed phys metrics. (0 is off)",
+            0,
+            (s) => { return s.PhysicsMetricDumpFrames; },
+            (s,v) => { s.PhysicsMetricDumpFrames = v; } ),
+        new ParameterDefn<float>("ResetBroadphasePool", "Setting this is any value resets the broadphase collision pool",
             0f,
-            (s,cf,p,v) => { s.UnmanagedParams[0].maxCollisionAlgorithmPoolSize = cf.GetFloat(p, v); },
-            (s) => { return s.UnmanagedParams[0].maxCollisionAlgorithmPoolSize; },
-            (s,p,l,v) => { s.UnmanagedParams[0].maxCollisionAlgorithmPoolSize = v; } ),
-            new ParameterDefn("ShouldDisableContactPoolDynamicAllocation", "Enable to allow large changes in object count",
-            ConfigurationParameters.numericFalse,
-            (s,cf,p,v) => { s.UnmanagedParams[0].shouldDisableContactPoolDynamicAllocation = BSParam.NumericBool(cf.GetBoolean(p, BSParam.BoolNumeric(v))); },
-            (s) => { return s.UnmanagedParams[0].shouldDisableContactPoolDynamicAllocation; },
-            (s,p,l,v) => { s.UnmanagedParams[0].shouldDisableContactPoolDynamicAllocation = v; } ),
-            new ParameterDefn("ShouldForceUpdateAllAabbs", "Enable to recomputer AABBs every simulator step",
-            ConfigurationParameters.numericFalse,
-            (s,cf,p,v) => { s.UnmanagedParams[0].shouldForceUpdateAllAabbs = BSParam.NumericBool(cf.GetBoolean(p, BSParam.BoolNumeric(v))); },
-            (s) => { return s.UnmanagedParams[0].shouldForceUpdateAllAabbs; },
-            (s,p,l,v) => { s.UnmanagedParams[0].shouldForceUpdateAllAabbs = v; } ),
-            new ParameterDefn("ShouldRandomizeSolverOrder", "Enable for slightly better stacking interaction",
-            ConfigurationParameters.numericTrue,
-            (s,cf,p,v) => { s.UnmanagedParams[0].shouldRandomizeSolverOrder = BSParam.NumericBool(cf.GetBoolean(p, BSParam.BoolNumeric(v))); },
-            (s) => { return s.UnmanagedParams[0].shouldRandomizeSolverOrder; },
-            (s,p,l,v) => { s.UnmanagedParams[0].shouldRandomizeSolverOrder = v; } ),
-            new ParameterDefn("ShouldSplitSimulationIslands", "Enable splitting active object scanning islands",
-            ConfigurationParameters.numericTrue,
-            (s,cf,p,v) => { s.UnmanagedParams[0].shouldSplitSimulationIslands = BSParam.NumericBool(cf.GetBoolean(p, BSParam.BoolNumeric(v))); },
-            (s) => { return s.UnmanagedParams[0].shouldSplitSimulationIslands; },
-            (s,p,l,v) => { s.UnmanagedParams[0].shouldSplitSimulationIslands = v; } ),
-            new ParameterDefn("ShouldEnableFrictionCaching", "Enable friction computation caching",
-            ConfigurationParameters.numericFalse,
-            (s,cf,p,v) => { s.UnmanagedParams[0].shouldEnableFrictionCaching = BSParam.NumericBool(cf.GetBoolean(p, BSParam.BoolNumeric(v))); },
-            (s) => { return s.UnmanagedParams[0].shouldEnableFrictionCaching; },
-            (s,p,l,v) => { s.UnmanagedParams[0].shouldEnableFrictionCaching = v; } ),
-            new ParameterDefn("NumberOfSolverIterations", "Number of internal iterations (0 means default)",
-            0f,     // zero says use Bullet default
-            (s,cf,p,v) => { s.UnmanagedParams[0].numberOfSolverIterations = cf.GetFloat(p, v); },
-            (s) => { return s.UnmanagedParams[0].numberOfSolverIterations; },
-            (s,p,l,v) => { s.UnmanagedParams[0].numberOfSolverIterations = v; } ),
-
-            new ParameterDefn("LinksetImplementation", "Type of linkset implementation (0=Constraint, 1=Compound, 2=Manual)",
-            (float)BSLinkset.LinksetImplementation.Compound,
-            (s,cf,p,v) => { LinksetImplementation = cf.GetFloat(p,v); },
-            (s) => { return LinksetImplementation; },
-            (s,p,l,v) => { LinksetImplementation = 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) => { LinkConstraintUseFrameOffset = BSParam.NumericBool(cf.GetBoolean(p, BSParam.BoolNumeric(v))); },
-            (s) => { return LinkConstraintUseFrameOffset; },
-            (s,p,l,v) => { LinkConstraintUseFrameOffset = v; } ),
-            new ParameterDefn("LinkConstraintEnableTransMotor", "Whether to enable translational motor on linkset constraints",
-            ConfigurationParameters.numericTrue,
-            (s,cf,p,v) => { LinkConstraintEnableTransMotor = BSParam.NumericBool(cf.GetBoolean(p, BSParam.BoolNumeric(v))); },
-            (s) => { return LinkConstraintEnableTransMotor; },
-            (s,p,l,v) => { LinkConstraintEnableTransMotor = v; } ),
-            new ParameterDefn("LinkConstraintTransMotorMaxVel", "Maximum velocity to be applied by translational motor in linkset constraints",
-            5.0f,
-            (s,cf,p,v) => { LinkConstraintTransMotorMaxVel = cf.GetFloat(p, v); },
-            (s) => { return LinkConstraintTransMotorMaxVel; },
-            (s,p,l,v) => { LinkConstraintTransMotorMaxVel = v; } ),
-            new ParameterDefn("LinkConstraintTransMotorMaxForce", "Maximum force to be applied by translational motor in linkset constraints",
-            0.1f,
-            (s,cf,p,v) => { LinkConstraintTransMotorMaxForce = cf.GetFloat(p, v); },
-            (s) => { return LinkConstraintTransMotorMaxForce; },
-            (s,p,l,v) => { LinkConstraintTransMotorMaxForce = v; } ),
-            new ParameterDefn("LinkConstraintCFM", "Amount constraint can be violated. 0=no violation, 1=infinite. Default=0.1",
-            0.1f,
-            (s,cf,p,v) => { LinkConstraintCFM = cf.GetFloat(p, v); },
-            (s) => { return LinkConstraintCFM; },
-            (s,p,l,v) => { LinkConstraintCFM = v; } ),
-            new ParameterDefn("LinkConstraintERP", "Amount constraint is corrected each tick. 0=none, 1=all. Default = 0.2",
-            0.1f,
-            (s,cf,p,v) => { LinkConstraintERP = cf.GetFloat(p, v); },
-            (s) => { return LinkConstraintERP; },
-            (s,p,l,v) => { LinkConstraintERP = v; } ),
-            new ParameterDefn("LinkConstraintSolverIterations", "Number of solver iterations when computing constraint. (0 = Bullet default)",
-            40,
-            (s,cf,p,v) => { LinkConstraintSolverIterations = cf.GetFloat(p, v); },
-            (s) => { return LinkConstraintSolverIterations; },
-            (s,p,l,v) => { LinkConstraintSolverIterations = v; } ),
-
-        new ParameterDefn("PhysicsMetricFrames", "Frames between outputting detailed phys metrics. (0 is off)",
+            (s) => { return 0f; },
+            (s,v) => { BSParam.ResetBroadphasePoolTainted(s, v); } ),
+        new ParameterDefn<float>("ResetConstraintSolver", "Setting this is any value resets the constraint solver",
             0f,
-            (s,cf,p,v) => { s.PhysicsMetricDumpFrames = cf.GetFloat(p, (int)v); },
-            (s) => { return (float)s.PhysicsMetricDumpFrames; },
-            (s,p,l,v) => { s.PhysicsMetricDumpFrames = (int)v; } ),
+            (s) => { return 0f; },
+            (s,v) => { BSParam.ResetConstraintSolverTainted(s, v); } ),
     };
 
     // Convert a boolean to our numeric true and false values
@@ -515,13 +700,13 @@ public static class BSParam
     //    ParameterDefn structure.
     // Case does not matter as names are compared after converting to lower case.
     // Returns 'false' if the parameter is not found.
-    internal static bool TryGetParameter(string paramName, out ParameterDefn defn)
+    internal static bool TryGetParameter(string paramName, out ParameterDefnBase defn)
     {
         bool ret = false;
-        ParameterDefn foundDefn = new ParameterDefn();
+        ParameterDefnBase foundDefn = null;
         string pName = paramName.ToLower();
 
-        foreach (ParameterDefn parm in ParameterDefinitions)
+        foreach (ParameterDefnBase parm in ParameterDefinitions)
         {
             if (pName == parm.name.ToLower())
             {
@@ -537,18 +722,18 @@ public static class BSParam
     // Pass through the settable parameters and set the default values
     internal static void SetParameterDefaultValues(BSScene physicsScene)
     {
-        foreach (ParameterDefn parm in ParameterDefinitions)
+        foreach (ParameterDefnBase parm in ParameterDefinitions)
         {
-            parm.setter(physicsScene, parm.name, PhysParameterEntry.APPLY_TO_NONE, parm.defaultValue);
+            parm.AssignDefault(physicsScene);
         }
     }
 
     // Get user set values out of the ini file.
     internal static void SetParameterConfigurationValues(BSScene physicsScene, IConfig cfg)
     {
-        foreach (ParameterDefn parm in ParameterDefinitions)
+        foreach (ParameterDefnBase parm in ParameterDefinitions)
         {
-            parm.userParam(physicsScene, cfg, parm.name, parm.defaultValue);
+            parm.SetValue(physicsScene, cfg.GetString(parm.name, parm.GetValue(physicsScene)));
         }
     }
 
@@ -563,20 +748,38 @@ public static class BSParam
             List<PhysParameterEntry> entries = new List<PhysParameterEntry>();
             for (int ii = 0; ii < ParameterDefinitions.Length; ii++)
             {
-                ParameterDefn pd = ParameterDefinitions[ii];
+                ParameterDefnBase 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);
-            });
+            // make the list alphabetical for ease of finding anything
+            entries.Sort((ppe1, ppe2) => { return ppe1.name.CompareTo(ppe2.name); });
 
             SettableParameters = entries.ToArray();
         }
     }
 
+    // =====================================================================
+    // =====================================================================
+    // There are parameters that, when set, cause things to happen in the physics engine.
+    // This causes the broadphase collision cache to be cleared.
+    private static void ResetBroadphasePoolTainted(BSScene pPhysScene, float v)
+    {
+        BSScene physScene = pPhysScene;
+        physScene.TaintedObject("BSParam.ResetBroadphasePoolTainted", delegate()
+        {
+            physScene.PE.ResetBroadphasePool(physScene.World);
+        });
+    }
 
+    // This causes the constraint solver cache to be cleared and reset.
+    private static void ResetConstraintSolverTainted(BSScene pPhysScene, float v)
+    {
+        BSScene physScene = pPhysScene;
+        physScene.TaintedObject("BSParam.ResetConstraintSolver", delegate()
+        {
+            physScene.PE.ResetConstraintSolver(physScene.World);
+        });
+    }
 }
 }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
index e7cb3e0..cca887a 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
@@ -38,12 +38,12 @@ namespace OpenSim.Region.Physics.BulletSPlugin
  * Class to wrap all objects.
  * The rest of BulletSim doesn't need to keep checking for avatars or prims
  *        unless the difference is significant.
- * 
+ *
  *  Variables in the physicsl objects are in three forms:
  *      VariableName: used by the simulator and performs taint operations, etc
  *      RawVariableName: direct reference to the BulletSim storage for the variable value
  *      ForceVariableName: direct reference (store and fetch) to the value in the physics engine.
- *  The last two (and certainly the last one) should be referenced only in taint-time.
+ *  The last one should only be referenced in taint-time.
  */
 
 /*
@@ -52,9 +52,19 @@ namespace OpenSim.Region.Physics.BulletSPlugin
  * SOP.ApplyImpulse     SOP.ApplyAngularImpulse     SOP.SetAngularImpulse   SOP.SetForce
  * SOG.ApplyImpulse     SOG.ApplyAngularImpulse     SOG.SetAngularImpulse
  * PA.AddForce          PA.AddAngularForce          PA.Torque = v           PA.Force = v
- * BS.ApplyCentralForce BS.ApplyTorque              
+ * BS.ApplyCentralForce BS.ApplyTorque
  */
 
+// Flags used to denote which properties updates when making UpdateProperties calls to linksets, etc.
+public enum UpdatedProperties : uint
+{
+    Position                = 1 << 0,
+    Orientation             = 1 << 1,
+    Velocity                = 1 << 2,
+    Acceleration            = 1 << 3,
+    RotationalVelocity      = 1 << 4,
+    EntPropUpdates          = Position | Orientation | Velocity | Acceleration | RotationalVelocity,
+}
 public abstract class BSPhysObject : PhysicsActor
 {
     protected BSPhysObject()
@@ -62,41 +72,61 @@ public abstract class BSPhysObject : PhysicsActor
     }
     protected BSPhysObject(BSScene parentScene, uint localID, string name, string typeName)
     {
-        PhysicsScene = parentScene;
+        PhysScene = parentScene;
         LocalID = localID;
         PhysObjectName = name;
+        Name = name;    // PhysicsActor also has the name of the object. Someday consolidate.
         TypeName = typeName;
 
+        // The collection of things that push me around
+        PhysicalActors = new BSActorCollection(PhysScene);
+
+        // Initialize variables kept in base.
+        GravModifier = 1.0f;
+        Gravity = new OMV.Vector3(0f, 0f, BSParam.Gravity);
+        HoverActive = false;
+
         // We don't have any physical representation yet.
         PhysBody = new BulletBody(localID);
-        PhysShape = new BulletShape();
+        PhysShape = new BSShapeNull();
 
-        // A linkset of just me
-        Linkset = BSLinkset.Factory(PhysicsScene, this);
-        LastAssetBuildFailed = false;
+        PrimAssetState = PrimAssetCondition.Unknown;
 
-        // Default material type
-        Material = MaterialAttributes.Material.Wood;
+        // Default material type. Also sets Friction, Restitution and Density.
+        SetMaterial((int)MaterialAttributes.Material.Wood);
 
         CollisionCollection = new CollisionEventUpdate();
+        CollisionsLastReported = CollisionCollection;
+        CollisionsLastTick = new CollisionEventUpdate();
+        CollisionsLastTickStep = -1;
+
         SubscribedEventsMs = 0;
         CollidingStep = 0;
         CollidingGroundStep = 0;
+        CollisionAccumulation = 0;
+        ColliderIsMoving = false;
+        CollisionScore = 0;
+
+        // All axis free.
+        LockedLinearAxis = LockedAxisFree;
+        LockedAngularAxis = LockedAxisFree;
     }
 
     // Tell the object to clean up.
     public virtual void Destroy()
     {
-        UnRegisterAllPreStepActions();
+        PhysicalActors.Enable(false);
+        PhysScene.TaintedObject("BSPhysObject.Destroy", delegate()
+        {
+            PhysicalActors.Dispose();
+        });
     }
 
-    public BSScene PhysicsScene { get; protected set; }
+    public BSScene PhysScene { get; protected set; }
     // public override uint LocalID { get; set; } // Use the LocalID definition in PhysicsActor
     public string PhysObjectName { get; protected set; }
     public string TypeName { get; protected set; }
 
-    public BSLinkset Linkset { get; set; }
-    public BSLinksetInfo LinksetInfo { get; set; }
 
     // Return the object mass without calculating it or having side effects
     public abstract float RawMass { get; }
@@ -104,26 +134,26 @@ public abstract class BSPhysObject : PhysicsActor
     // 'inWorld' true if the object has already been added to the dynamic world.
     public abstract void UpdatePhysicalMassProperties(float mass, bool inWorld);
 
+    // The gravity being applied to the object. A function of default grav, GravityModifier and Buoyancy.
+    public virtual OMV.Vector3 Gravity { get; set; }
     // The last value calculated for the prim's inertia
     public OMV.Vector3 Inertia { get; set; }
 
     // Reference to the physical body (btCollisionObject) of this object
     public BulletBody PhysBody;
     // Reference to the physical shape (btCollisionShape) of this object
-    public BulletShape PhysShape;
+    public BSShape PhysShape;
 
-    // 'true' if the mesh's underlying asset failed to build.
-    // This will keep us from looping after the first time the build failed.
-    public bool LastAssetBuildFailed { get; set; }
+    // The physical representation of the prim might require an asset fetch.
+    // The asset state is first 'Unknown' then 'Waiting' then either 'Failed' or 'Fetched'.
+    public enum PrimAssetCondition
+    {
+        Unknown, Waiting, Failed, Fetched
+    }
+    public PrimAssetCondition PrimAssetState { get; set; }
 
     // The objects base shape information. Null if not a prim type shape.
     public PrimitiveBaseShape BaseShape { get; protected set; }
-    // Some types of objects have preferred physical representations.
-    // Returns SHAPE_UNKNOWN if there is no preference.
-    public virtual BSPhysicsShapeType PreferredPhysicalShape
-    {
-        get { return BSPhysicsShapeType.SHAPE_UNKNOWN; }
-    }
 
     // When the physical properties are updated, an EntityProperty holds the update values.
     // Keep the current and last EntityProperties to enable computation of differences
@@ -132,23 +162,35 @@ public abstract class BSPhysObject : PhysicsActor
     public EntityProperties LastEntityProperties { get; set; }
 
     public virtual OMV.Vector3 Scale { get; set; }
+
+    // It can be confusing for an actor to know if it should move or update an object
+    //    depeneding on the setting of 'selected', 'physical, ...
+    // This flag is the true test -- if true, the object is being acted on in the physical world
+    public abstract bool IsPhysicallyActive { get; }
+
+    // Detailed state of the object.
     public abstract bool IsSolid { get; }
     public abstract bool IsStatic { get; }
+    public abstract bool IsSelected { get; }
 
     // Materialness
     public MaterialAttributes.Material Material { get; private set; }
     public override void SetMaterial(int material)
     {
         Material = (MaterialAttributes.Material)material;
+
+        // Setting the material sets the material attributes also.
+        MaterialAttributes matAttrib = BSMaterials.GetAttributes(Material, false);
+        Friction = matAttrib.friction;
+        Restitution = matAttrib.restitution;
+        Density = matAttrib.density / BSParam.DensityScaleFactor;
+        // DetailLog("{0},{1}.SetMaterial,Mat={2},frict={3},rest={4},den={5}", LocalID, TypeName, Material, Friction, Restitution, Density);
     }
 
     // Stop all physical motion.
     public abstract void ZeroMotion(bool inTaintTime);
     public abstract void ZeroAngularMotion(bool inTaintTime);
 
-    // Step the vehicle simulation for this object. A NOOP if the vehicle was not configured.
-    public virtual void StepVehicle(float timeStep) { }
-
     // Update the physical location and motion of the object. Called with data from Bullet.
     public abstract void UpdateProperties(EntityProperties entprop);
 
@@ -158,18 +200,16 @@ public abstract class BSPhysObject : PhysicsActor
     public abstract OMV.Quaternion RawOrientation { get; set; }
     public abstract OMV.Quaternion ForceOrientation { get; set; }
 
-    // The system is telling us the velocity it wants to move at.
-    // protected OMV.Vector3 m_targetVelocity;  // use the definition in PhysicsActor
-    public override OMV.Vector3 TargetVelocity
+    public OMV.Vector3 RawVelocity { get; set; }
+    public abstract OMV.Vector3 ForceVelocity { get; set; }
+
+    public OMV.Vector3 RawForce { get; set; }
+    public OMV.Vector3 RawTorque { get; set; }
+    public override void AddAngularForce(OMV.Vector3 force, bool pushforce)
     {
-        get { return m_targetVelocity; }
-        set
-        {
-            m_targetVelocity = value;
-            Velocity = value;
-        }
+        AddAngularForce(force, pushforce, false);
     }
-    public abstract OMV.Vector3 ForceVelocity { get; set; }
+    public abstract void AddAngularForce(OMV.Vector3 force, bool pushforce, bool inTaintTime);
 
     public abstract OMV.Vector3 ForceRotationalVelocity { get; set; }
 
@@ -177,6 +217,105 @@ public abstract class BSPhysObject : PhysicsActor
 
     public virtual bool ForceBodyShapeRebuild(bool inTaintTime) { return false; }
 
+    public override bool PIDActive { set { MoveToTargetActive = value; } }
+    public override OMV.Vector3 PIDTarget { set { MoveToTargetTarget = value; } }
+    public override float PIDTau { set { MoveToTargetTau = value; } }
+
+    public bool MoveToTargetActive { get; set; }
+    public OMV.Vector3 MoveToTargetTarget { get; set; }
+    public float MoveToTargetTau { get; set; }
+
+    // Used for llSetHoverHeight and maybe vehicle height. Hover Height will override MoveTo target's Z
+    public override bool PIDHoverActive { set { HoverActive = value; } }
+    public override float PIDHoverHeight { set { HoverHeight = value; } }
+    public override PIDHoverType PIDHoverType { set { HoverType = value; } }
+    public override float PIDHoverTau { set { HoverTau = value; } }
+
+    public bool HoverActive { get; set; }
+    public float HoverHeight { get;  set; }
+    public PIDHoverType HoverType { get;  set; }
+    public float HoverTau { get; set; }
+
+    // For RotLookAt
+    public override OMV.Quaternion APIDTarget { set { return; } }
+    public override bool APIDActive { set { return; } }
+    public override float APIDStrength { set { return; } }
+    public override float APIDDamping { set { return; } }
+
+    // The current velocity forward
+    public virtual float ForwardSpeed
+    {
+        get
+        {
+            OMV.Vector3 characterOrientedVelocity = RawVelocity * OMV.Quaternion.Inverse(OMV.Quaternion.Normalize(RawOrientation));
+            return characterOrientedVelocity.X;
+        }
+    }
+    // The forward speed we are trying to achieve (TargetVelocity)
+    public virtual float TargetVelocitySpeed
+    {
+        get
+        {
+            OMV.Vector3 characterOrientedVelocity = TargetVelocity * OMV.Quaternion.Inverse(OMV.Quaternion.Normalize(RawOrientation));
+            return characterOrientedVelocity.X;
+        }
+    }
+
+    // The user can optionally set the center of mass. The user's setting will override any
+    //    computed center-of-mass (like in linksets).
+    // Note this is a displacement from the root's coordinates. Zero means use the root prim as center-of-mass.
+    public OMV.Vector3? UserSetCenterOfMassDisplacement { get; set; }
+
+    public OMV.Vector3 LockedLinearAxis { get; set; } // zero means locked. one means free.
+    public OMV.Vector3 LockedAngularAxis { get; set; } // zero means locked. one means free.
+    public const float FreeAxis = 1f;
+    public readonly OMV.Vector3 LockedAxisFree = new OMV.Vector3(FreeAxis, FreeAxis, FreeAxis);  // All axis are free
+
+    // Enable physical actions. Bullet will keep sleeping non-moving physical objects so
+    //     they need waking up when parameters are changed.
+    // Called in taint-time!!
+    public void ActivateIfPhysical(bool forceIt)
+    {
+        if (IsPhysical && PhysBody.HasPhysicalBody)
+            PhysScene.PE.Activate(PhysBody, forceIt);
+    }
+
+    // 'actors' act on the physical object to change or constrain its motion. These can range from
+    //       hovering to complex vehicle motion.
+    // May be called at non-taint time as this just adds the actor to the action list and the real
+    //    work is done during the simulation step.
+    // Note that, if the actor is already in the list and we are disabling same, the actor is just left
+    //    in the list disabled.
+    public delegate BSActor CreateActor();
+    public void EnableActor(bool enableActor, string actorName, CreateActor creator)
+    {
+        lock (PhysicalActors)
+        {
+            BSActor theActor;
+            if (PhysicalActors.TryGetActor(actorName, out theActor))
+            {
+                // The actor already exists so just turn it on or off
+                DetailLog("{0},BSPhysObject.EnableActor,enablingExistingActor,name={1},enable={2}", LocalID, actorName, enableActor);
+                theActor.Enabled = enableActor;
+            }
+            else
+            {
+                // The actor does not exist. If it should, create it.
+                if (enableActor)
+                {
+                    DetailLog("{0},BSPhysObject.EnableActor,creatingActor,name={1}", LocalID, actorName);
+                    theActor = creator();
+                    PhysicalActors.Add(actorName, theActor);
+                    theActor.Enabled = true;
+                }
+                else
+                {
+                    DetailLog("{0},BSPhysObject.EnableActor,notCreatingActorSinceNotEnabled,name={1}", LocalID, actorName);
+                }
+            }
+        }
+    }
+
     #region Collisions
 
     // Requested number of milliseconds between collision events. Zero means disabled.
@@ -191,41 +330,56 @@ public abstract class BSPhysObject : PhysicsActor
     protected long CollidingObjectStep { get; set; }
     // The collision flags we think are set in Bullet
     protected CollisionFlags CurrentCollisionFlags { get; set; }
+    // On a collision, check the collider and remember if the last collider was moving
+    //    Used to modify the standing of avatars (avatars on stationary things stand still)
+    public bool ColliderIsMoving;
+    // Used by BSCharacter to manage standing (and not slipping)
+    public bool IsStationary;
+
+    // Count of collisions for this object
+    protected long CollisionAccumulation { get; set; }
 
     public override bool IsColliding {
-        get { return (CollidingStep == PhysicsScene.SimulationStep); }
+        get { return (CollidingStep == PhysScene.SimulationStep); }
         set {
             if (value)
-                CollidingStep = PhysicsScene.SimulationStep;
+                CollidingStep = PhysScene.SimulationStep;
             else
                 CollidingStep = 0;
             }
     }
     public override bool CollidingGround {
-        get { return (CollidingGroundStep == PhysicsScene.SimulationStep); }
+        get { return (CollidingGroundStep == PhysScene.SimulationStep); }
         set
         {
             if (value)
-                CollidingGroundStep = PhysicsScene.SimulationStep;
+                CollidingGroundStep = PhysScene.SimulationStep;
             else
                 CollidingGroundStep = 0;
         }
     }
     public override bool CollidingObj {
-        get { return (CollidingObjectStep == PhysicsScene.SimulationStep); }
-        set { 
+        get { return (CollidingObjectStep == PhysScene.SimulationStep); }
+        set {
             if (value)
-                CollidingObjectStep = PhysicsScene.SimulationStep;
+                CollidingObjectStep = PhysScene.SimulationStep;
             else
                 CollidingObjectStep = 0;
         }
     }
 
-    // The collisions that have been collected this tick
+    // The collisions that have been collected for the next collision reporting (throttled by subscription)
     protected CollisionEventUpdate CollisionCollection;
+    // This is the collision collection last reported to the Simulator.
+    public CollisionEventUpdate CollisionsLastReported;
+    // Remember the collisions recorded in the last tick for fancy collision checking
+    //     (like a BSCharacter walking up stairs).
+    public CollisionEventUpdate CollisionsLastTick;
+    private long CollisionsLastTickStep = -1;
 
     // The simulation step is telling this object about a collision.
     // Return 'true' if a collision was processed and should be sent up.
+    // Return 'false' if this object is not enabled/subscribed/appropriate for or has already seen this collision.
     // Called at taint time from within the Step() function
     public virtual bool Collide(uint collidingWith, BSPhysObject collidee,
                     OMV.Vector3 contactPoint, OMV.Vector3 contactNormal, float pentrationDepth)
@@ -233,27 +387,35 @@ public abstract class BSPhysObject : PhysicsActor
         bool ret = false;
 
         // The following lines make IsColliding(), CollidingGround() and CollidingObj work
-        CollidingStep = PhysicsScene.SimulationStep;
-        if (collidingWith <= PhysicsScene.TerrainManager.HighestTerrainID)
+        CollidingStep = PhysScene.SimulationStep;
+        if (collidingWith <= PhysScene.TerrainManager.HighestTerrainID)
         {
-            CollidingGroundStep = PhysicsScene.SimulationStep;
+            CollidingGroundStep = PhysScene.SimulationStep;
         }
         else
         {
-            CollidingObjectStep = PhysicsScene.SimulationStep;
+            CollidingObjectStep = PhysScene.SimulationStep;
         }
 
-        // prims in the same linkset cannot collide with each other
-        if (collidee != null && (this.Linkset.LinksetID == collidee.Linkset.LinksetID))
+        CollisionAccumulation++;
+
+        // For movement tests, remember if we are colliding with an object that is moving.
+        ColliderIsMoving = collidee != null ? (collidee.RawVelocity != OMV.Vector3.Zero) : false;
+
+        // Make a collection of the collisions that happened the last simulation tick.
+        // This is different than the collection created for sending up to the simulator as it is cleared every tick.
+        if (CollisionsLastTickStep != PhysScene.SimulationStep)
         {
-            return ret;
+            CollisionsLastTick = new CollisionEventUpdate();
+            CollisionsLastTickStep = PhysScene.SimulationStep;
         }
+        CollisionsLastTick.AddCollider(collidingWith, new ContactPoint(contactPoint, contactNormal, pentrationDepth));
 
-        // if someone has subscribed for collision events....
+        // If someone has subscribed for collision events log the collision so it will be reported up
         if (SubscribedEvents()) {
             CollisionCollection.AddCollider(collidingWith, new ContactPoint(contactPoint, contactNormal, pentrationDepth));
-            DetailLog("{0},{1}.Collison.AddCollider,call,with={2},point={3},normal={4},depth={5}",
-                            LocalID, TypeName, collidingWith, contactPoint, contactNormal, pentrationDepth);
+            DetailLog("{0},{1}.Collison.AddCollider,call,with={2},point={3},normal={4},depth={5},colliderMoving={6}",
+                            LocalID, TypeName, collidingWith, contactPoint, contactNormal, pentrationDepth, ColliderIsMoving);
 
             ret = true;
         }
@@ -267,13 +429,14 @@ public abstract class BSPhysObject : PhysicsActor
     public virtual bool SendCollisions()
     {
         bool ret = true;
+
         // If the 'no collision' call, force it to happen right now so quick collision_end
-        bool force = (CollisionCollection.Count == 0);
+        bool force = (CollisionCollection.Count == 0 && CollisionsLastReported.Count != 0);
 
         // throttle the collisions to the number of milliseconds specified in the subscription
-        if (force || (PhysicsScene.SimulationNowTime >= NextCollisionOkTime))
+        if (force || (PhysScene.SimulationNowTime >= NextCollisionOkTime))
         {
-            NextCollisionOkTime = PhysicsScene.SimulationNowTime + SubscribedEventsMs;
+            NextCollisionOkTime = PhysScene.SimulationNowTime + SubscribedEventsMs;
 
             // We are called if we previously had collisions. If there are no collisions
             //   this time, send up one last empty event so OpenSim can sense collision end.
@@ -283,12 +446,15 @@ public abstract class BSPhysObject : PhysicsActor
                 ret = false;
             }
 
-            // DetailLog("{0},{1}.SendCollisionUpdate,call,numCollisions={2}", LocalID, TypeName, CollisionCollection.Count);
+            DetailLog("{0},{1}.SendCollisionUpdate,call,numCollisions={2}", LocalID, TypeName, CollisionCollection.Count);
             base.SendCollisionUpdate(CollisionCollection);
 
+            // Remember the collisions from this tick for some collision specific processing.
+            CollisionsLastReported = CollisionCollection;
+
             // The CollisionCollection instance is passed around in the simulator.
             // Make sure we don't have a handle to that one and that a new one is used for next time.
-            //    This fixes an interesting 'gotcha'. If we call CollisionCollection.Clear() here, 
+            //    This fixes an interesting 'gotcha'. If we call CollisionCollection.Clear() here,
             //    a race condition is created for the other users of this instance.
             CollisionCollection = new CollisionEventUpdate();
         }
@@ -305,10 +471,10 @@ public abstract class BSPhysObject : PhysicsActor
             // make sure first collision happens
             NextCollisionOkTime = Util.EnvironmentTickCountSubtract(SubscribedEventsMs);
 
-            PhysicsScene.TaintedObject(TypeName+".SubscribeEvents", delegate()
+            PhysScene.TaintedObject(TypeName+".SubscribeEvents", delegate()
             {
                 if (PhysBody.HasPhysicalBody)
-                    CurrentCollisionFlags = PhysicsScene.PE.AddToCollisionFlags(PhysBody, CollisionFlags.BS_SUBSCRIBE_COLLISION_EVENTS);
+                    CurrentCollisionFlags = PhysScene.PE.AddToCollisionFlags(PhysBody, CollisionFlags.BS_SUBSCRIBE_COLLISION_EVENTS);
             });
         }
         else
@@ -320,66 +486,53 @@ public abstract class BSPhysObject : PhysicsActor
     public override void UnSubscribeEvents() {
         // DetailLog("{0},{1}.UnSubscribeEvents,unsubscribing", LocalID, TypeName);
         SubscribedEventsMs = 0;
-        PhysicsScene.TaintedObject(TypeName+".UnSubscribeEvents", delegate()
+        PhysScene.TaintedObject(TypeName+".UnSubscribeEvents", delegate()
         {
             // Make sure there is a body there because sometimes destruction happens in an un-ideal order.
             if (PhysBody.HasPhysicalBody)
-                CurrentCollisionFlags = PhysicsScene.PE.RemoveFromCollisionFlags(PhysBody, CollisionFlags.BS_SUBSCRIBE_COLLISION_EVENTS);
+                CurrentCollisionFlags = PhysScene.PE.RemoveFromCollisionFlags(PhysBody, CollisionFlags.BS_SUBSCRIBE_COLLISION_EVENTS);
         });
     }
     // Return 'true' if the simulator wants collision events
     public override bool SubscribedEvents() {
         return (SubscribedEventsMs > 0);
     }
+    // Because 'CollisionScore' is called many times while sorting, it should not be recomputed
+    //    each time called. So this is built to be light weight for each collision and to do
+    //    all the processing when the user asks for the info.
+    public void ComputeCollisionScore()
+    {
+        // Scale the collision count by the time since the last collision.
+        // The "+1" prevents dividing by zero.
+        long timeAgo = PhysScene.SimulationStep - CollidingStep + 1;
+        CollisionScore = CollisionAccumulation / timeAgo;
+    }
+    public override float CollisionScore { get; set; }
 
     #endregion // Collisions
 
     #region Per Simulation Step actions
-    // There are some actions that must be performed for a physical object before each simulation step.
-    // These actions are optional so, rather than scanning all the physical objects and asking them
-    //     if they have anything to do, a physical object registers for an event call before the step is performed.
-    // This bookkeeping makes it easy to add, remove and clean up after all these registrations.
-    private Dictionary<string, BSScene.PreStepAction> RegisteredActions = new Dictionary<string, BSScene.PreStepAction>();
-    protected void RegisterPreStepAction(string op, uint id, BSScene.PreStepAction actn)
-    {
-        string identifier = op + "-" + id.ToString();
-        RegisteredActions[identifier] = actn;
-        PhysicsScene.BeforeStep += actn;
-        DetailLog("{0},BSPhysObject.RegisterPreStepAction,id={1}", LocalID, identifier);
-    }
 
-    // Unregister a pre step action. Safe to call if the action has not been registered.
-    protected void UnRegisterPreStepAction(string op, uint id)
-    {
-        string identifier = op + "-" + id.ToString();
-        bool removed = false;
-        if (RegisteredActions.ContainsKey(identifier))
-        {
-            PhysicsScene.BeforeStep -= RegisteredActions[identifier];
-            RegisteredActions.Remove(identifier);
-            removed = true;
-        }
-        DetailLog("{0},BSPhysObject.UnRegisterPreStepAction,id={1},removed={2}", LocalID, identifier, removed);
-    }
+    public BSActorCollection PhysicalActors;
 
-    protected void UnRegisterAllPreStepActions()
+    // When an update to the physical properties happens, this event is fired to let
+    //    different actors to modify the update before it is passed around
+    public delegate void PreUpdatePropertyAction(ref EntityProperties entprop);
+    public event PreUpdatePropertyAction OnPreUpdateProperty;
+    protected void TriggerPreUpdatePropertyAction(ref EntityProperties entprop)
     {
-        foreach (KeyValuePair<string, BSScene.PreStepAction> kvp in RegisteredActions)
-        {
-            PhysicsScene.BeforeStep -= kvp.Value;
-        }
-        RegisteredActions.Clear();
-        DetailLog("{0},BSPhysObject.UnRegisterAllPreStepActions,", LocalID);
+        PreUpdatePropertyAction actions = OnPreUpdateProperty;
+        if (actions != null)
+            actions(ref entprop);
     }
 
-    
     #endregion // Per Simulation Step actions
 
     // High performance detailed logging routine used by the physical objects.
     protected void DetailLog(string msg, params Object[] args)
     {
-        if (PhysicsScene.PhysicsLogging.Enabled)
-            PhysicsScene.DetailLog(msg, args);
+        if (PhysScene.PhysicsLogging.Enabled)
+            PhysScene.DetailLog(msg, args);
     }
 
 }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPlugin.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPlugin.cs
index 65be52a..9442854 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPlugin.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPlugin.cs
@@ -59,7 +59,7 @@ public class BSPlugin : IPhysicsPlugin
     {
         if (_mScene == null)
         {
-            _mScene = new BSScene(sceneIdentifier);
+            _mScene = new BSScene(GetName(), sceneIdentifier);
         }
         return (_mScene);
     }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
index 826261c..f5b0361 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
@@ -39,7 +39,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
 {
 
     [Serializable]
-public sealed class BSPrim : BSPhysObject
+public class BSPrim : BSPhysObject
 {
     private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
     private static readonly string LogHeader = "[BULLETS PRIM]";
@@ -50,39 +50,38 @@ public sealed class BSPrim : BSPhysObject
     private bool _grabbed;
     private bool _isSelected;
     private bool _isVolumeDetect;
+
+    // _position is what the simulator thinks the positions of the prim is.
     private OMV.Vector3 _position;
+
     private float _mass;    // the mass of this object
-    private float _density;
-    private OMV.Vector3 _force;
-    private OMV.Vector3 _velocity;
-    private OMV.Vector3 _torque;
-    private float _collisionScore;
     private OMV.Vector3 _acceleration;
     private OMV.Quaternion _orientation;
     private int _physicsActorType;
     private bool _isPhysical;
     private bool _flying;
-    private float _friction;
-    private float _restitution;
     private bool _setAlwaysRun;
     private bool _throttleUpdates;
-    private bool _isColliding;
-    private bool _collidingGround;
-    private bool _collidingObj;
     private bool _floatOnWater;
     private OMV.Vector3 _rotationalVelocity;
     private bool _kinematic;
     private float _buoyancy;
 
-    private BSDynamics _vehicle;
+    private int CrossingFailures { get; set; }
 
-    private OMV.Vector3 _PIDTarget;
-    private bool _usePID;
-    private float _PIDTau;
-    private bool _useHoverPID;
-    private float _PIDHoverHeight;
-    private PIDHoverType _PIDHoverType;
-    private float _PIDHoverTao;
+    // Keep a handle to the vehicle actor so it is easy to set parameters on same.
+    public BSDynamics VehicleActor;
+    public const string VehicleActorName = "BasicVehicle";
+
+    // Parameters for the hover actor
+    public const string HoverActorName = "HoverActor";
+    // Parameters for the axis lock actor
+    public const String LockedAxisActorName = "BSPrim.LockedAxis";
+    // Parameters for the move to target actor
+    public const string MoveToTargetActorName = "MoveToTargetActor";
+    // Parameters for the setForce and setTorque actors
+    public const string SetForceActorName = "SetForceActor";
+    public const string SetTorqueActorName = "SetTorqueActor";
 
     public BSPrim(uint localID, String primName, BSScene parent_scene, OMV.Vector3 pos, OMV.Vector3 size,
                        OMV.Quaternion rotation, PrimitiveBaseShape pbs, bool pisPhysical)
@@ -95,32 +94,28 @@ public sealed class BSPrim : BSPhysObject
         Scale = size;   // prims are the size the user wants them to be (different for BSCharactes).
         _orientation = rotation;
         _buoyancy = 0f;
-        _velocity = OMV.Vector3.Zero;
+        RawVelocity = OMV.Vector3.Zero;
         _rotationalVelocity = OMV.Vector3.Zero;
         BaseShape = pbs;
         _isPhysical = pisPhysical;
         _isVolumeDetect = false;
 
-        // Someday set default attributes based on the material but, for now, we don't know the prim material yet.
-        // MaterialAttributes primMat = BSMaterials.GetAttributes(Material, pisPhysical);
-        _density = PhysicsScene.Params.defaultDensity;
-        _friction = PhysicsScene.Params.defaultFriction;
-        _restitution = PhysicsScene.Params.defaultRestitution;
-
-        _vehicle = new BSDynamics(PhysicsScene, this);            // add vehicleness
+        // We keep a handle to the vehicle actor so we can set vehicle parameters later.
+        VehicleActor = new BSDynamics(PhysScene, this, VehicleActorName);
+        PhysicalActors.Add(VehicleActorName, VehicleActor);
 
         _mass = CalculateMass();
 
-        // Cause linkset variables to be initialized (like mass)
-        Linkset.Refresh(this);
-
-        DetailLog("{0},BSPrim.constructor,call", LocalID);
+        // DetailLog("{0},BSPrim.constructor,call", LocalID);
         // do the actual object creation at taint time
-        PhysicsScene.TaintedObject("BSPrim.create", delegate()
+        PhysScene.TaintedObject("BSPrim.create", delegate()
         {
+            // Make sure the object is being created with some sanity.
+            ExtremeSanityCheck(true /* inTaintTime */);
+
             CreateGeomAndObject(true);
 
-            CurrentCollisionFlags = PhysicsScene.PE.GetCollisionFlags(PhysBody);
+            CurrentCollisionFlags = PhysScene.PE.GetCollisionFlags(PhysBody);
         });
     }
 
@@ -130,26 +125,17 @@ public sealed class BSPrim : BSPhysObject
         // m_log.DebugFormat("{0}: Destroy, id={1}", LogHeader, LocalID);
         base.Destroy();
 
-        // Undo any links between me and any other object
-        BSPhysObject parentBefore = Linkset.LinksetRoot;
-        int childrenBefore = Linkset.NumberOfChildren;
-
-        Linkset = Linkset.RemoveMeFromLinkset(this);
-
-        DetailLog("{0},BSPrim.Destroy,call,parentBefore={1},childrenBefore={2},parentAfter={3},childrenAfter={4}",
-            LocalID, parentBefore.LocalID, childrenBefore, Linkset.LinksetRoot.LocalID, Linkset.NumberOfChildren);
-
         // Undo any vehicle properties
         this.VehicleType = (int)Vehicle.TYPE_NONE;
 
-        PhysicsScene.TaintedObject("BSPrim.destroy", delegate()
+        PhysScene.TaintedObject("BSPrim.Destroy", delegate()
         {
             DetailLog("{0},BSPrim.Destroy,taint,", LocalID);
             // If there are physical body and shape, release my use of same.
-            PhysicsScene.Shapes.DereferenceBody(PhysBody, true, null);
+            PhysScene.Shapes.DereferenceBody(PhysBody, null);
             PhysBody.Clear();
-            PhysicsScene.Shapes.DereferenceShape(PhysShape, true, null);
-            PhysShape.Clear();
+            PhysShape.Dereference(PhysScene);
+            PhysShape = new BSShapeNull();
         });
     }
 
@@ -171,17 +157,13 @@ public sealed class BSPrim : BSPhysObject
     public override PrimitiveBaseShape Shape {
         set {
             BaseShape = value;
+            PrimAssetState = PrimAssetCondition.Unknown;
             ForceBodyShapeRebuild(false);
         }
     }
-    // Whatever the linkset wants is what I want.
-    public override BSPhysicsShapeType PreferredPhysicalShape
-        { get { return Linkset.PreferredPhysicalShape(this); } }
-
     public override bool ForceBodyShapeRebuild(bool inTaintTime)
     {
-        LastAssetBuildFailed = false;
-        PhysicsScene.TaintedObject(inTaintTime, "BSPrim.ForceBodyShapeRebuild", delegate()
+        PhysScene.TaintedObject(inTaintTime, "BSPrim.ForceBodyShapeRebuild", delegate()
         {
             _mass = CalculateMass();   // changing the shape changes the mass
             CreateGeomAndObject(true);
@@ -198,7 +180,7 @@ public sealed class BSPrim : BSPhysObject
             if (value != _isSelected)
             {
                 _isSelected = value;
-                PhysicsScene.TaintedObject("BSPrim.setSelected", delegate()
+                PhysScene.TaintedObject("BSPrim.setSelected", delegate()
                 {
                     DetailLog("{0},BSPrim.selected,taint,selected={1}", LocalID, _isSelected);
                     SetObjectDynamic(false);
@@ -206,37 +188,31 @@ public sealed class BSPrim : BSPhysObject
             }
         }
     }
-    public override void CrossingFailure() { return; }
+    public override bool IsSelected
+    {
+        get { return _isSelected; }
+    }
 
-    // link me to the specified parent
-    public override void link(PhysicsActor obj) {
-        BSPrim parent = obj as BSPrim;
-        if (parent != null)
+    public override void CrossingFailure()
+    {
+        CrossingFailures++;
+        if (CrossingFailures > BSParam.CrossingFailuresBeforeOutOfBounds)
         {
-            BSPhysObject parentBefore = Linkset.LinksetRoot;
-            int childrenBefore = Linkset.NumberOfChildren;
-
-            Linkset = parent.Linkset.AddMeToLinkset(this);
-
-            DetailLog("{0},BSPrim.link,call,parentBefore={1}, childrenBefore=={2}, parentAfter={3}, childrenAfter={4}",
-                LocalID, parentBefore.LocalID, childrenBefore, Linkset.LinksetRoot.LocalID, Linkset.NumberOfChildren);
+            base.RaiseOutOfBounds(RawPosition);
+        }
+        else if (CrossingFailures == BSParam.CrossingFailuresBeforeOutOfBounds)
+        {
+            m_log.WarnFormat("{0} Too many crossing failures for {1}", LogHeader, Name);
         }
         return;
     }
 
+    // link me to the specified parent
+    public override void link(PhysicsActor obj) {
+    }
+
     // delink me from my linkset
     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
-
-        BSPhysObject parentBefore = Linkset.LinksetRoot;
-        int childrenBefore = Linkset.NumberOfChildren;
-
-        Linkset = Linkset.RemoveMeFromLinkset(this);
-
-        DetailLog("{0},BSPrim.delink,parentBefore={1},childrenBefore={2},parentAfter={3},childrenAfter={4}, ",
-            LocalID, parentBefore.LocalID, childrenBefore, Linkset.LinksetRoot.LocalID, Linkset.NumberOfChildren);
-        return;
     }
 
     // Set motion values to zero.
@@ -245,28 +221,28 @@ public sealed class BSPrim : BSPhysObject
     // Called at taint time!
     public override void ZeroMotion(bool inTaintTime)
     {
-        _velocity = OMV.Vector3.Zero;
+        RawVelocity = OMV.Vector3.Zero;
         _acceleration = OMV.Vector3.Zero;
         _rotationalVelocity = OMV.Vector3.Zero;
 
         // Zero some other properties in the physics engine
-        PhysicsScene.TaintedObject(inTaintTime, "BSPrim.ZeroMotion", delegate()
+        PhysScene.TaintedObject(inTaintTime, "BSPrim.ZeroMotion", delegate()
         {
             if (PhysBody.HasPhysicalBody)
-                PhysicsScene.PE.ClearAllForces(PhysBody);
+                PhysScene.PE.ClearAllForces(PhysBody);
         });
     }
     public override void ZeroAngularMotion(bool inTaintTime)
     {
         _rotationalVelocity = OMV.Vector3.Zero;
         // Zero some other properties in the physics engine
-        PhysicsScene.TaintedObject(inTaintTime, "BSPrim.ZeroMotion", delegate()
+        PhysScene.TaintedObject(inTaintTime, "BSPrim.ZeroMotion", delegate()
         {
             // DetailLog("{0},BSPrim.ZeroAngularMotion,call,rotVel={1}", LocalID, _rotationalVelocity);
             if (PhysBody.HasPhysicalBody)
             {
-                PhysicsScene.PE.SetInterpolationAngularVelocity(PhysBody, _rotationalVelocity);
-                PhysicsScene.PE.SetAngularVelocity(PhysBody, _rotationalVelocity);
+                PhysScene.PE.SetInterpolationAngularVelocity(PhysBody, _rotationalVelocity);
+                PhysScene.PE.SetAngularVelocity(PhysBody, _rotationalVelocity);
             }
         });
     }
@@ -274,6 +250,25 @@ public sealed class BSPrim : BSPhysObject
     public override void LockAngularMotion(OMV.Vector3 axis)
     {
         DetailLog("{0},BSPrim.LockAngularMotion,call,axis={1}", LocalID, axis);
+
+        // "1" means free, "0" means locked
+        OMV.Vector3 locking = LockedAxisFree;
+        if (axis.X != 1) locking.X = 0f;
+        if (axis.Y != 1) locking.Y = 0f;
+        if (axis.Z != 1) locking.Z = 0f;
+        LockedAngularAxis = locking;
+
+        EnableActor(LockedAngularAxis != LockedAxisFree, LockedAxisActorName, delegate()
+        {
+            return new BSActorLockAxis(PhysScene, this, LockedAxisActorName);
+        });
+
+        // Update parameters so the new actor's Refresh() action is called at the right time.
+        PhysScene.TaintedObject("BSPrim.LockAngularMotion", delegate()
+        {
+            UpdatePhysicalParameters();
+        });
+
         return;
     }
 
@@ -284,15 +279,8 @@ public sealed class BSPrim : BSPhysObject
     }
     public override OMV.Vector3 Position {
         get {
-            /* NOTE: this refetch is not necessary. The simulator knows about linkset children
-             *    and does not fetch this position info for children. Thus this is commented out.
-            // child prims move around based on their parent. Need to get the latest location
-            if (!Linkset.IsRoot(this))
-                _position = Linkset.PositionGet(this);
-            */
-
             // don't do the GetObjectPosition for root elements because this function is called a zillion times.
-            // _position = PhysicsScene.PE.GetObjectPosition2(PhysicsScene.World, BSBody);
+            // _position = ForcePosition;
             return _position;
         }
         set {
@@ -306,26 +294,24 @@ public sealed class BSPrim : BSPhysObject
             _position = value;
             PositionSanityCheck(false);
 
-            // A linkset might need to know if a component information changed.
-            Linkset.UpdateProperties(this, false);
-
-            PhysicsScene.TaintedObject("BSPrim.setPosition", delegate()
+            PhysScene.TaintedObject("BSPrim.setPosition", delegate()
             {
                 DetailLog("{0},BSPrim.SetPosition,taint,pos={1},orient={2}", LocalID, _position, _orientation);
                 ForcePosition = _position;
             });
         }
     }
+
     public override OMV.Vector3 ForcePosition {
         get {
-            _position = PhysicsScene.PE.GetPosition(PhysBody);
+            _position = PhysScene.PE.GetPosition(PhysBody);
             return _position;
         }
         set {
             _position = value;
             if (PhysBody.HasPhysicalBody)
             {
-                PhysicsScene.PE.SetTranslation(PhysBody, _position, _orientation);
+                PhysScene.PE.SetTranslation(PhysBody, _position, _orientation);
                 ActivateIfPhysical(false);
             }
         }
@@ -338,7 +324,11 @@ public sealed class BSPrim : BSPhysObject
     {
         bool ret = false;
 
-        if (!PhysicsScene.TerrainManager.IsWithinKnownTerrain(_position))
+        // We don't care where non-physical items are placed
+        if (!IsPhysicallyActive)
+            return ret;
+
+        if (!PhysScene.TerrainManager.IsWithinKnownTerrain(RawPosition))
         {
             // The physical object is out of the known/simulated area.
             // Upper levels of code will handle the transition to other areas so, for
@@ -346,39 +336,74 @@ public sealed class BSPrim : BSPhysObject
             return ret;
         }
 
-        float terrainHeight = PhysicsScene.TerrainManager.GetTerrainHeightAtXYZ(_position);
+        float terrainHeight = PhysScene.TerrainManager.GetTerrainHeightAtXYZ(RawPosition);
         OMV.Vector3 upForce = OMV.Vector3.Zero;
-        if (RawPosition.Z < terrainHeight)
+        float approxSize = Math.Max(Size.X, Math.Max(Size.Y, Size.Z));
+        if ((RawPosition.Z + approxSize / 2f) < terrainHeight)
         {
-            DetailLog("{0},BSPrim.PositionAdjustUnderGround,call,pos={1},terrain={2}", LocalID, _position, terrainHeight);
+            DetailLog("{0},BSPrim.PositionAdjustUnderGround,call,pos={1},terrain={2}", LocalID, RawPosition, terrainHeight);
             float targetHeight = terrainHeight + (Size.Z / 2f);
-            // Upforce proportional to the distance away from the terrain. Correct the error in 1 sec.
-            upForce.Z = (terrainHeight - RawPosition.Z) * 1f;
+            // If the object is below ground it just has to be moved up because pushing will
+            //     not get it through the terrain
+            _position.Z = targetHeight;
+            if (inTaintTime)
+            {
+                ForcePosition = _position;
+            }
+            // If we are throwing the object around, zero its other forces
+            ZeroMotion(inTaintTime);
             ret = true;
         }
 
         if ((CurrentCollisionFlags & CollisionFlags.BS_FLOATS_ON_WATER) != 0)
         {
-            float waterHeight = PhysicsScene.TerrainManager.GetWaterLevelAtXYZ(_position);
+            float waterHeight = PhysScene.TerrainManager.GetWaterLevelAtXYZ(_position);
             // TODO: a floating motor so object will bob in the water
             if (Math.Abs(RawPosition.Z - waterHeight) > 0.1f)
             {
                 // Upforce proportional to the distance away from the water. Correct the error in 1 sec.
                 upForce.Z = (waterHeight - RawPosition.Z) * 1f;
+
+                // Apply upforce and overcome gravity.
+                OMV.Vector3 correctionForce = upForce - PhysScene.DefaultGravity;
+                DetailLog("{0},BSPrim.PositionSanityCheck,applyForce,pos={1},upForce={2},correctionForce={3}", LocalID, _position, upForce, correctionForce);
+                AddForce(correctionForce, false, inTaintTime);
                 ret = true;
             }
         }
 
-        // The above code computes a force to apply to correct any out-of-bounds problems. Apply same.
-        // TODO: This should be intergrated with a geneal physics action mechanism.
-        // TODO: This should be moderated with PID'ness.
-        if (ret)
+        return ret;
+    }
+
+    // Occasionally things will fly off and really get lost.
+    // Find the wanderers and bring them back.
+    // Return 'true' if some parameter need some sanity.
+    private bool ExtremeSanityCheck(bool inTaintTime)
+    {
+        bool ret = false;
+
+        uint wayOutThere = Constants.RegionSize * Constants.RegionSize;
+        // There have been instances of objects getting thrown way out of bounds and crashing
+        //    the border crossing code.
+        if (   _position.X < -Constants.RegionSize || _position.X > wayOutThere
+            || _position.Y < -Constants.RegionSize || _position.Y > wayOutThere
+            || _position.Z < -Constants.RegionSize || _position.Z > wayOutThere)
+        {
+            _position = new OMV.Vector3(10, 10, 50);
+            ZeroMotion(inTaintTime);
+            ret = true;
+        }
+        if (RawVelocity.LengthSquared() > BSParam.MaxLinearVelocity)
         {
-            // Apply upforce and overcome gravity.
-            OMV.Vector3 correctionForce = upForce - PhysicsScene.DefaultGravity;
-            DetailLog("{0},BSPrim.PositionSanityCheck,applyForce,pos={1},upForce={2},correctionForce={3}", LocalID, _position, upForce, correctionForce);
-            AddForce(correctionForce, false, inTaintTime);
+            RawVelocity = Util.ClampV(RawVelocity, BSParam.MaxLinearVelocity);
+            ret = true;
         }
+        if (_rotationalVelocity.LengthSquared() > BSParam.MaxAngularVelocitySquared)
+        {
+            _rotationalVelocity = Util.ClampV(_rotationalVelocity, BSParam.MaxAngularVelocity);
+            ret = true;
+        }
+
         return ret;
     }
 
@@ -387,72 +412,69 @@ public sealed class BSPrim : BSPhysObject
         // If the simulator cares about the mass of the linkset, it will sum it itself.
     public override float Mass
     {
-        get
-        {
-            return _mass;
-        }
+        get { return _mass; }
+    }
+    // TotalMass returns the mass of the large object the prim may be in (overridden by linkset code)
+    public virtual float TotalMass
+    {
+        get { return _mass; }
     }
-
     // used when we only want this prim's mass and not the linkset thing
-    public override float RawMass { 
+    public override float RawMass {
         get { return _mass; }
     }
     // Set the physical mass to the passed mass.
     // Note that this does not change _mass!
     public override void UpdatePhysicalMassProperties(float physMass, bool inWorld)
     {
-        if (PhysBody.HasPhysicalBody)
+        if (PhysBody.HasPhysicalBody && PhysShape.HasPhysicalShape)
         {
             if (IsStatic)
             {
-                PhysicsScene.PE.SetGravity(PhysBody, PhysicsScene.DefaultGravity);
+                PhysScene.PE.SetGravity(PhysBody, PhysScene.DefaultGravity);
                 Inertia = OMV.Vector3.Zero;
-                PhysicsScene.PE.SetMassProps(PhysBody, 0f, Inertia);
-                PhysicsScene.PE.UpdateInertiaTensor(PhysBody);
+                PhysScene.PE.SetMassProps(PhysBody, 0f, Inertia);
+                PhysScene.PE.UpdateInertiaTensor(PhysBody);
             }
             else
             {
-                OMV.Vector3 grav = ComputeGravity();
-
                 if (inWorld)
                 {
                     // Changing interesting properties doesn't change proxy and collision cache
                     //    information. The Bullet solution is to re-add the object to the world
                     //    after parameters are changed.
-                    PhysicsScene.PE.RemoveObjectFromWorld(PhysicsScene.World, PhysBody);
+                    PhysScene.PE.RemoveObjectFromWorld(PhysScene.World, PhysBody);
                 }
 
                 // The computation of mass props requires gravity to be set on the object.
-                PhysicsScene.PE.SetGravity(PhysBody, grav);
+                Gravity = ComputeGravity(Buoyancy);
+                PhysScene.PE.SetGravity(PhysBody, Gravity);
 
-                Inertia = PhysicsScene.PE.CalculateLocalInertia(PhysShape, physMass);
-                PhysicsScene.PE.SetMassProps(PhysBody, physMass, Inertia);
-                PhysicsScene.PE.UpdateInertiaTensor(PhysBody);
+                Inertia = PhysScene.PE.CalculateLocalInertia(PhysShape.physShapeInfo, physMass);
+                PhysScene.PE.SetMassProps(PhysBody, physMass, Inertia);
+                PhysScene.PE.UpdateInertiaTensor(PhysBody);
 
-                // center of mass is at the zero of the object
-                // DEBUG DEBUG PhysicsScene.PE.SetCenterOfMassByPosRot(PhysBody, ForcePosition, ForceOrientation);
-                DetailLog("{0},BSPrim.UpdateMassProperties,mass={1},localInertia={2},grav={3},inWorld={4}", LocalID, physMass, Inertia, grav, inWorld);
+                DetailLog("{0},BSPrim.UpdateMassProperties,mass={1},localInertia={2},grav={3},inWorld={4}",
+                                            LocalID, physMass, Inertia, Gravity, inWorld);
 
                 if (inWorld)
                 {
                     AddObjectToPhysicalWorld();
                 }
-
-                // Must set gravity after it has been added to the world because, for unknown reasons,
-                //     adding the object resets the object's gravity to world gravity
-                PhysicsScene.PE.SetGravity(PhysBody, grav);
-
             }
         }
     }
 
     // Return what gravity should be set to this very moment
-    private OMV.Vector3 ComputeGravity()
+    public OMV.Vector3 ComputeGravity(float buoyancy)
     {
-        OMV.Vector3 ret = PhysicsScene.DefaultGravity;
+        OMV.Vector3 ret = PhysScene.DefaultGravity;
 
         if (!IsStatic)
-            ret *= (1f - Buoyancy);
+        {
+            ret *= (1f - buoyancy);
+            ret *= GravModifier;
+        }
 
         return ret;
     }
@@ -460,93 +482,70 @@ public sealed class BSPrim : BSPhysObject
     // Is this used?
     public override OMV.Vector3 CenterOfMass
     {
-        get { return Linkset.CenterOfMass; }
+        get { return RawPosition; }
     }
 
     // Is this used?
     public override OMV.Vector3 GeometricCenter
     {
-        get { return Linkset.GeometricCenter; }
+        get { return RawPosition; }
     }
 
     public override OMV.Vector3 Force {
-        get { return _force; }
+        get { return RawForce; }
         set {
-            _force = value;
-            if (_force != OMV.Vector3.Zero)
+            RawForce = value;
+            EnableActor(RawForce != OMV.Vector3.Zero, SetForceActorName, delegate()
             {
-                // If the force is non-zero, it must be reapplied each tick because
-                //    Bullet clears the forces applied last frame.
-                RegisterPreStepAction("BSPrim.setForce", LocalID,
-                    delegate(float timeStep)
-                    {
-                        DetailLog("{0},BSPrim.setForce,preStep,force={1}", LocalID, _force);
-                        if (PhysBody.HasPhysicalBody)
-                        {
-                            PhysicsScene.PE.ApplyCentralForce(PhysBody, _force);
-                            ActivateIfPhysical(false);
-                        }
-                    }
-                );
-            }
-            else
-            {
-                UnRegisterPreStepAction("BSPrim.setForce", LocalID);
-            }
+                return new BSActorSetForce(PhysScene, this, SetForceActorName);
+            });
         }
     }
 
     public override int VehicleType {
         get {
-            return (int)_vehicle.Type;   // if we are a vehicle, return that type
+            return (int)VehicleActor.Type;
         }
         set {
             Vehicle type = (Vehicle)value;
 
-            PhysicsScene.TaintedObject("setVehicleType", delegate()
+            PhysScene.TaintedObject("setVehicleType", delegate()
             {
-                // Done at taint time so we're sure the physics engine is not using the variables
-                // Vehicle code changes the parameters for this vehicle type.
-                _vehicle.ProcessTypeChange(type);
+                ZeroMotion(true /* inTaintTime */);
+                VehicleActor.ProcessTypeChange(type);
                 ActivateIfPhysical(false);
-
-                // If an active vehicle, register the vehicle code to be called before each step
-                if (_vehicle.Type == Vehicle.TYPE_NONE)
-                    UnRegisterPreStepAction("BSPrim.Vehicle", LocalID);
-                else
-                    RegisterPreStepAction("BSPrim.Vehicle", LocalID, _vehicle.Step);
             });
         }
     }
     public override void VehicleFloatParam(int param, float value)
     {
-        PhysicsScene.TaintedObject("BSPrim.VehicleFloatParam", delegate()
+        PhysScene.TaintedObject("BSPrim.VehicleFloatParam", delegate()
         {
-            _vehicle.ProcessFloatVehicleParam((Vehicle)param, value);
+            VehicleActor.ProcessFloatVehicleParam((Vehicle)param, value);
             ActivateIfPhysical(false);
         });
     }
     public override void VehicleVectorParam(int param, OMV.Vector3 value)
     {
-        PhysicsScene.TaintedObject("BSPrim.VehicleVectorParam", delegate()
+        PhysScene.TaintedObject("BSPrim.VehicleVectorParam", delegate()
         {
-            _vehicle.ProcessVectorVehicleParam((Vehicle)param, value);
+            VehicleActor.ProcessVectorVehicleParam((Vehicle)param, value);
             ActivateIfPhysical(false);
         });
     }
     public override void VehicleRotationParam(int param, OMV.Quaternion rotation)
     {
-        PhysicsScene.TaintedObject("BSPrim.VehicleRotationParam", delegate()
+        PhysScene.TaintedObject("BSPrim.VehicleRotationParam", delegate()
         {
-            _vehicle.ProcessRotationVehicleParam((Vehicle)param, rotation);
+            VehicleActor.ProcessRotationVehicleParam((Vehicle)param, rotation);
             ActivateIfPhysical(false);
         });
     }
     public override void VehicleFlags(int param, bool remove)
     {
-        PhysicsScene.TaintedObject("BSPrim.VehicleFlags", delegate()
+        PhysScene.TaintedObject("BSPrim.VehicleFlags", delegate()
         {
-            _vehicle.ProcessVehicleFlags(param, remove);
+            VehicleActor.ProcessVehicleFlags(param, remove);
         });
     }
 
@@ -556,7 +555,7 @@ public sealed class BSPrim : BSPhysObject
         if (_isVolumeDetect != newValue)
         {
             _isVolumeDetect = newValue;
-            PhysicsScene.TaintedObject("BSPrim.SetVolumeDetect", delegate()
+            PhysScene.TaintedObject("BSPrim.SetVolumeDetect", delegate()
             {
                 // DetailLog("{0},setVolumeDetect,taint,volDetect={1}", LocalID, _isVolumeDetect);
                 SetObjectDynamic(true);
@@ -564,56 +563,110 @@ public sealed class BSPrim : BSPhysObject
         }
         return;
     }
+    public override void SetMaterial(int material)
+    {
+        base.SetMaterial(material);
+        PhysScene.TaintedObject("BSPrim.SetMaterial", delegate()
+        {
+            UpdatePhysicalParameters();
+        });
+    }
+    public override float Friction
+    {
+        get { return base.Friction; }
+        set
+        {
+            if (base.Friction != value)
+            {
+                base.Friction = value;
+                PhysScene.TaintedObject("BSPrim.setFriction", delegate()
+                {
+                    UpdatePhysicalParameters();
+                });
+            }
+        }
+    }
+    public override float Restitution
+    {
+        get { return base.Restitution; }
+        set
+        {
+            if (base.Restitution != value)
+            {
+                base.Restitution = value;
+                PhysScene.TaintedObject("BSPrim.setRestitution", delegate()
+                {
+                    UpdatePhysicalParameters();
+                });
+            }
+        }
+    }
+    // The simulator/viewer keep density as 100kg/m3.
+    // Remember to use BSParam.DensityScaleFactor to create the physical density.
+    public override float Density
+    {
+        get { return base.Density; }
+        set
+        {
+            if (base.Density != value)
+            {
+                base.Density = value;
+                PhysScene.TaintedObject("BSPrim.setDensity", delegate()
+                {
+                    UpdatePhysicalParameters();
+                });
+            }
+        }
+    }
+    public override float GravModifier
+    {
+        get { return base.GravModifier; }
+        set
+        {
+            if (base.GravModifier != value)
+            {
+                base.GravModifier = value;
+                PhysScene.TaintedObject("BSPrim.setGravityModifier", delegate()
+                {
+                    UpdatePhysicalParameters();
+                });
+            }
+        }
+    }
     public override OMV.Vector3 Velocity {
-        get { return _velocity; }
+        get { return RawVelocity; }
         set {
-            _velocity = value;
-            PhysicsScene.TaintedObject("BSPrim.setVelocity", delegate()
+            RawVelocity = value;
+            PhysScene.TaintedObject("BSPrim.setVelocity", delegate()
             {
-                // DetailLog("{0},BSPrim.SetVelocity,taint,vel={1}", LocalID, _velocity);
-                ForceVelocity = _velocity;
+                // DetailLog("{0},BSPrim.SetVelocity,taint,vel={1}", LocalID, RawVelocity);
+                ForceVelocity = RawVelocity;
             });
         }
     }
     public override OMV.Vector3 ForceVelocity {
-        get { return _velocity; }
+        get { return RawVelocity; }
         set {
-            PhysicsScene.AssertInTaintTime("BSPrim.ForceVelocity");
+            PhysScene.AssertInTaintTime("BSPrim.ForceVelocity");
 
-            _velocity = value;
+            RawVelocity = Util.ClampV(value, BSParam.MaxLinearVelocity);
             if (PhysBody.HasPhysicalBody)
             {
-                PhysicsScene.PE.SetLinearVelocity(PhysBody, _velocity);
+                DetailLog("{0},BSPrim.ForceVelocity,taint,vel={1}", LocalID, RawVelocity);
+                PhysScene.PE.SetLinearVelocity(PhysBody, RawVelocity);
                 ActivateIfPhysical(false);
             }
         }
     }
     public override OMV.Vector3 Torque {
-        get { return _torque; }
+        get { return RawTorque; }
         set {
-            _torque = value;
-            if (_torque != OMV.Vector3.Zero)
+            RawTorque = value;
+            EnableActor(RawTorque != OMV.Vector3.Zero, SetTorqueActorName, delegate()
             {
-                // If the torque is non-zero, it must be reapplied each tick because
-                //    Bullet clears the forces applied last frame.
-                RegisterPreStepAction("BSPrim.setTorque", LocalID,
-                    delegate(float timeStep)
-                    {
-                        if (PhysBody.HasPhysicalBody)
-                            AddAngularForce(_torque, false, true);
-                    }
-                );
-            }
-            else
-            {
-                UnRegisterPreStepAction("BSPrim.setTorque", LocalID);
-            }
-            // DetailLog("{0},BSPrim.SetTorque,call,torque={1}", LocalID, _torque);
-        }
-    }
-    public override float CollisionScore {
-        get { return _collisionScore; }
-        set { _collisionScore = value;
+                return new BSActorSetTorque(PhysScene, this, SetTorqueActorName);
+            });
+            DetailLog("{0},BSPrim.SetTorque,call,torque={1}", LocalID, RawTorque);
         }
     }
     public override OMV.Vector3 Acceleration {
@@ -627,14 +680,6 @@ public sealed class BSPrim : BSPhysObject
     }
     public override OMV.Quaternion Orientation {
         get {
-            /* NOTE: this refetch is not necessary. The simulator knows about linkset children
-             *    and does not fetch this position info for children. Thus this is commented out.
-            // Children move around because tied to parent. Get a fresh value.
-            if (!Linkset.IsRoot(this))
-            {
-                _orientation = Linkset.OrientationGet(this);
-            }
-             */
             return _orientation;
         }
         set {
@@ -642,17 +687,9 @@ public sealed class BSPrim : BSPhysObject
                 return;
             _orientation = value;
 
-            // A linkset might need to know if a component information changed.
-            Linkset.UpdateProperties(this, false);
-
-            PhysicsScene.TaintedObject("BSPrim.setOrientation", delegate()
+            PhysScene.TaintedObject("BSPrim.setOrientation", delegate()
             {
-                if (PhysBody.HasPhysicalBody)
-                {
-                    // _position = PhysicsScene.PE.GetObjectPosition(PhysicsScene.World, BSBody);
-                    // DetailLog("{0},BSPrim.setOrientation,taint,pos={1},orient={2}", LocalID, _position, _orientation);
-                    PhysicsScene.PE.SetTranslation(PhysBody, _position, _orientation);
-                }
+                ForceOrientation = _orientation;
             });
         }
     }
@@ -661,13 +698,14 @@ public sealed class BSPrim : BSPhysObject
     {
         get
         {
-            _orientation = PhysicsScene.PE.GetOrientation(PhysBody);
+            _orientation = PhysScene.PE.GetOrientation(PhysBody);
             return _orientation;
         }
         set
         {
             _orientation = value;
-            PhysicsScene.PE.SetTranslation(PhysBody, _position, _orientation);
+            if (PhysBody.HasPhysicalBody)
+                PhysScene.PE.SetTranslation(PhysBody, _position, _orientation);
         }
     }
     public override int PhysicsActorType {
@@ -680,12 +718,13 @@ public sealed class BSPrim : BSPhysObject
             if (_isPhysical != value)
             {
                 _isPhysical = value;
-                PhysicsScene.TaintedObject("BSPrim.setIsPhysical", delegate()
+                PhysScene.TaintedObject("BSPrim.setIsPhysical", delegate()
                 {
                     DetailLog("{0},setIsPhysical,taint,isPhys={1}", LocalID, _isPhysical);
                     SetObjectDynamic(true);
                     // whether phys-to-static or static-to-phys, the object is not moving.
                     ZeroMotion(true);
+
                 });
             }
         }
@@ -703,6 +742,12 @@ public sealed class BSPrim : BSPhysObject
         get { return !IsPhantom && !_isVolumeDetect; }
     }
 
+    // The object is moving and is actively being dynamic in the physical world
+    public override bool IsPhysicallyActive
+    {
+        get { return !_isSelected && IsPhysical; }
+    }
+
     // Make gravity work if the object is physical and not selected
     // Called at taint-time!!
     private void SetObjectDynamic(bool forceRebuild)
@@ -717,19 +762,24 @@ public sealed class BSPrim : BSPhysObject
     //     isSolid: other objects bounce off of this object
     //     isVolumeDetect: other objects pass through but can generate collisions
     //     collisionEvents: whether this object returns collision events
-    private void UpdatePhysicalParameters()
+    public virtual void UpdatePhysicalParameters()
     {
-        // DetailLog("{0},BSPrim.UpdatePhysicalParameters,entry,body={1},shape={2}", LocalID, BSBody, BSShape);
+        if (!PhysBody.HasPhysicalBody)
+        {
+            // This would only happen if updates are called for during initialization when the body is not set up yet.
+            // DetailLog("{0},BSPrim.UpdatePhysicalParameters,taint,calledWithNoPhysBody", LocalID);
+            return;
+        }
 
         // Mangling all the physical properties requires the object not be in the physical world.
         // This is a NOOP if the object is not in the world (BulletSim and Bullet ignore objects not found).
-        PhysicsScene.PE.RemoveObjectFromWorld(PhysicsScene.World, PhysBody);
+        PhysScene.PE.RemoveObjectFromWorld(PhysScene.World, PhysBody);
 
         // Set up the object physicalness (does gravity and collisions move this object)
         MakeDynamic(IsStatic);
 
         // Update vehicle specific parameters (after MakeDynamic() so can change physical parameters)
-        _vehicle.Refresh();
+        PhysicalActors.Refresh();
 
         // Arrange for collision events if the simulator wants them
         EnableCollisions(SubscribedEvents());
@@ -740,16 +790,11 @@ public sealed class BSPrim : BSPhysObject
         AddObjectToPhysicalWorld();
 
         // Rebuild its shape
-        PhysicsScene.PE.UpdateSingleAabb(PhysicsScene.World, PhysBody);
-
-        // Recompute any linkset parameters.
-        // When going from non-physical to physical, this re-enables the constraints that
-        //     had been automatically disabled when the mass was set to zero.
-        // For compound based linksets, this enables and disables interactions of the children.
-        Linkset.Refresh(this);
+        PhysScene.PE.UpdateSingleAabb(PhysScene.World, PhysBody);
 
         DetailLog("{0},BSPrim.UpdatePhysicalParameters,taintExit,static={1},solid={2},mass={3},collide={4},cf={5:X},cType={6},body={7},shape={8}",
-                        LocalID, IsStatic, IsSolid, Mass, SubscribedEvents(), CurrentCollisionFlags, PhysBody.collisionType, PhysBody, PhysShape);
+                                    LocalID, IsStatic, IsSolid, Mass, SubscribedEvents(),
+                                    CurrentCollisionFlags, PhysBody.collisionType, PhysBody, PhysShape);
     }
 
     // "Making dynamic" means changing to and from static.
@@ -757,59 +802,55 @@ public sealed class BSPrim : BSPhysObject
     // When dynamic, the object can fall and be pushed by others.
     // This is independent of its 'solidness' which controls what passes through
     //    this object and what interacts with it.
-    private void MakeDynamic(bool makeStatic)
+    protected virtual void MakeDynamic(bool makeStatic)
     {
         if (makeStatic)
         {
             // Become a Bullet 'static' object type
-            CurrentCollisionFlags = PhysicsScene.PE.AddToCollisionFlags(PhysBody, CollisionFlags.CF_STATIC_OBJECT);
+            CurrentCollisionFlags = PhysScene.PE.AddToCollisionFlags(PhysBody, CollisionFlags.CF_STATIC_OBJECT);
             // Stop all movement
             ZeroMotion(true);
 
             // Set various physical properties so other object interact properly
-            MaterialAttributes matAttrib = BSMaterials.GetAttributes(Material, false);
-            PhysicsScene.PE.SetFriction(PhysBody, matAttrib.friction);
-            PhysicsScene.PE.SetRestitution(PhysBody, matAttrib.restitution);
+            PhysScene.PE.SetFriction(PhysBody, Friction);
+            PhysScene.PE.SetRestitution(PhysBody, Restitution);
+            PhysScene.PE.SetContactProcessingThreshold(PhysBody, BSParam.ContactProcessingThreshold);
 
             // Mass is zero which disables a bunch of physics stuff in Bullet
             UpdatePhysicalMassProperties(0f, false);
             // Set collision detection parameters
             if (BSParam.CcdMotionThreshold > 0f)
             {
-                PhysicsScene.PE.SetCcdMotionThreshold(PhysBody, BSParam.CcdMotionThreshold);
-                PhysicsScene.PE.SetCcdSweptSphereRadius(PhysBody, BSParam.CcdSweptSphereRadius);
+                PhysScene.PE.SetCcdMotionThreshold(PhysBody, BSParam.CcdMotionThreshold);
+                PhysScene.PE.SetCcdSweptSphereRadius(PhysBody, BSParam.CcdSweptSphereRadius);
             }
 
             // The activation state is 'disabled' so Bullet will not try to act on it.
             // PhysicsScene.PE.ForceActivationState(PhysBody, ActivationState.DISABLE_SIMULATION);
             // Start it out sleeping and physical actions could wake it up.
-            PhysicsScene.PE.ForceActivationState(PhysBody, ActivationState.ISLAND_SLEEPING);
+            PhysScene.PE.ForceActivationState(PhysBody, ActivationState.ISLAND_SLEEPING);
 
             // This collides like a static object
             PhysBody.collisionType = CollisionType.Static;
-
-            // There can be special things needed for implementing linksets
-            Linkset.MakeStatic(this);
         }
         else
         {
             // Not a Bullet static object
-            CurrentCollisionFlags = PhysicsScene.PE.RemoveFromCollisionFlags(PhysBody, CollisionFlags.CF_STATIC_OBJECT);
+            CurrentCollisionFlags = PhysScene.PE.RemoveFromCollisionFlags(PhysBody, CollisionFlags.CF_STATIC_OBJECT);
 
             // Set various physical properties so other object interact properly
-            MaterialAttributes matAttrib = BSMaterials.GetAttributes(Material, true);
-            PhysicsScene.PE.SetFriction(PhysBody, matAttrib.friction);
-            PhysicsScene.PE.SetRestitution(PhysBody, matAttrib.restitution);
+            PhysScene.PE.SetFriction(PhysBody, Friction);
+            PhysScene.PE.SetRestitution(PhysBody, Restitution);
+            // DetailLog("{0},BSPrim.MakeDynamic,frict={1},rest={2}", LocalID, Friction, Restitution);
 
             // per http://www.bulletphysics.org/Bullet/phpBB3/viewtopic.php?t=3382
             // Since this can be called multiple times, only zero forces when becoming physical
             // PhysicsScene.PE.ClearAllForces(BSBody);
 
             // For good measure, make sure the transform is set through to the motion state
-            PhysicsScene.PE.SetTranslation(PhysBody, _position, _orientation);
-
-            // Center of mass is at the center of the object
-            // DEBUG DEBUG PhysicsScene.PE.SetCenterOfMassByPosRot(Linkset.LinksetRoot.PhysBody, _position, _orientation);
+            ForcePosition = _position;
+            ForceVelocity = RawVelocity;
+            ForceRotationalVelocity = _rotationalVelocity;
 
             // A dynamic object has mass
             UpdatePhysicalMassProperties(RawMass, false);
@@ -817,25 +858,22 @@ public sealed class BSPrim : BSPhysObject
             // Set collision detection parameters
             if (BSParam.CcdMotionThreshold > 0f)
             {
-                PhysicsScene.PE.SetCcdMotionThreshold(PhysBody, BSParam.CcdMotionThreshold);
-                PhysicsScene.PE.SetCcdSweptSphereRadius(PhysBody, BSParam.CcdSweptSphereRadius);
+                PhysScene.PE.SetCcdMotionThreshold(PhysBody, BSParam.CcdMotionThreshold);
+                PhysScene.PE.SetCcdSweptSphereRadius(PhysBody, BSParam.CcdSweptSphereRadius);
             }
 
             // Various values for simulation limits
-            PhysicsScene.PE.SetDamping(PhysBody, BSParam.LinearDamping, BSParam.AngularDamping);
-            PhysicsScene.PE.SetDeactivationTime(PhysBody, BSParam.DeactivationTime);
-            PhysicsScene.PE.SetSleepingThresholds(PhysBody, BSParam.LinearSleepingThreshold, BSParam.AngularSleepingThreshold);
-            PhysicsScene.PE.SetContactProcessingThreshold(PhysBody, BSParam.ContactProcessingThreshold);
+            PhysScene.PE.SetDamping(PhysBody, BSParam.LinearDamping, BSParam.AngularDamping);
+            PhysScene.PE.SetDeactivationTime(PhysBody, BSParam.DeactivationTime);
+            PhysScene.PE.SetSleepingThresholds(PhysBody, BSParam.LinearSleepingThreshold, BSParam.AngularSleepingThreshold);
+            PhysScene.PE.SetContactProcessingThreshold(PhysBody, BSParam.ContactProcessingThreshold);
 
             // This collides like an object.
             PhysBody.collisionType = CollisionType.Dynamic;
 
             // Force activation of the object so Bullet will act on it.
             // Must do the ForceActivationState2() to overcome the DISABLE_SIMULATION from static objects.
-            PhysicsScene.PE.ForceActivationState(PhysBody, ActivationState.ACTIVE_TAG);
-
-            // There might be special things needed for implementing linksets.
-            Linkset.MakeDynamic(this);
+            PhysScene.PE.ForceActivationState(PhysBody, ActivationState.ACTIVE_TAG);
         }
     }
 
@@ -845,7 +883,7 @@ public sealed class BSPrim : BSPhysObject
     //     the functions after this one set up the state of a possibly newly created collision body.
     private void MakeSolid(bool makeSolid)
     {
-        CollisionObjectTypes bodyType = (CollisionObjectTypes)PhysicsScene.PE.GetBodyType(PhysBody);
+        CollisionObjectTypes bodyType = (CollisionObjectTypes)PhysScene.PE.GetBodyType(PhysBody);
         if (makeSolid)
         {
             // Verify the previous code created the correct shape for this type of thing.
@@ -853,7 +891,7 @@ public sealed class BSPrim : BSPhysObject
             {
                 m_log.ErrorFormat("{0} MakeSolid: physical body of wrong type for solidity. id={1}, type={2}", LogHeader, LocalID, bodyType);
             }
-            CurrentCollisionFlags = PhysicsScene.PE.RemoveFromCollisionFlags(PhysBody, CollisionFlags.CF_NO_CONTACT_RESPONSE);
+            CurrentCollisionFlags = PhysScene.PE.RemoveFromCollisionFlags(PhysBody, CollisionFlags.CF_NO_CONTACT_RESPONSE);
         }
         else
         {
@@ -861,32 +899,23 @@ public sealed class BSPrim : BSPhysObject
             {
                 m_log.ErrorFormat("{0} MakeSolid: physical body of wrong type for non-solidness. id={1}, type={2}", LogHeader, LocalID, bodyType);
             }
-            CurrentCollisionFlags = PhysicsScene.PE.AddToCollisionFlags(PhysBody, CollisionFlags.CF_NO_CONTACT_RESPONSE);
+            CurrentCollisionFlags = PhysScene.PE.AddToCollisionFlags(PhysBody, CollisionFlags.CF_NO_CONTACT_RESPONSE);
 
             // Change collision info from a static object to a ghosty collision object
             PhysBody.collisionType = CollisionType.VolumeDetect;
         }
     }
 
-    // Enable physical actions. Bullet will keep sleeping non-moving physical objects so
-    //     they need waking up when parameters are changed.
-    // Called in taint-time!!
-    private void ActivateIfPhysical(bool forceIt)
-    {
-        if (IsPhysical && PhysBody.HasPhysicalBody)
-            PhysicsScene.PE.Activate(PhysBody, forceIt);
-    }
-
     // Turn on or off the flag controlling whether collision events are returned to the simulator.
     private void EnableCollisions(bool wantsCollisionEvents)
     {
         if (wantsCollisionEvents)
         {
-            CurrentCollisionFlags = PhysicsScene.PE.AddToCollisionFlags(PhysBody, CollisionFlags.BS_SUBSCRIBE_COLLISION_EVENTS);
+            CurrentCollisionFlags = PhysScene.PE.AddToCollisionFlags(PhysBody, CollisionFlags.BS_SUBSCRIBE_COLLISION_EVENTS);
         }
         else
         {
-            CurrentCollisionFlags = PhysicsScene.PE.RemoveFromCollisionFlags(PhysBody, CollisionFlags.BS_SUBSCRIBE_COLLISION_EVENTS);
+            CurrentCollisionFlags = PhysScene.PE.RemoveFromCollisionFlags(PhysBody, CollisionFlags.BS_SUBSCRIBE_COLLISION_EVENTS);
         }
     }
 
@@ -897,12 +926,12 @@ public sealed class BSPrim : BSPhysObject
     {
         if (PhysBody.HasPhysicalBody)
         {
-            PhysicsScene.PE.AddObjectToWorld(PhysicsScene.World, PhysBody);
+            PhysScene.PE.AddObjectToWorld(PhysScene.World, PhysBody);
         }
         else
         {
             m_log.ErrorFormat("{0} Attempt to add physical object without body. id={1}", LogHeader, LocalID);
-            DetailLog("{0},BSPrim.UpdatePhysicalParameters,addObjectWithoutBody,cType={1}", LocalID, PhysBody.collisionType);
+            DetailLog("{0},BSPrim.AddObjectToPhysicalWorld,addObjectWithoutBody,cType={1}", LocalID, PhysBody.collisionType);
         }
     }
 
@@ -932,12 +961,12 @@ public sealed class BSPrim : BSPhysObject
     public override bool FloatOnWater {
         set {
             _floatOnWater = value;
-            PhysicsScene.TaintedObject("BSPrim.setFloatOnWater", delegate()
+            PhysScene.TaintedObject("BSPrim.setFloatOnWater", delegate()
             {
                 if (_floatOnWater)
-                    CurrentCollisionFlags = PhysicsScene.PE.AddToCollisionFlags(PhysBody, CollisionFlags.BS_FLOATS_ON_WATER);
+                    CurrentCollisionFlags = PhysScene.PE.AddToCollisionFlags(PhysBody, CollisionFlags.BS_FLOATS_ON_WATER);
                 else
-                    CurrentCollisionFlags = PhysicsScene.PE.RemoveFromCollisionFlags(PhysBody, CollisionFlags.BS_FLOATS_ON_WATER);
+                    CurrentCollisionFlags = PhysScene.PE.RemoveFromCollisionFlags(PhysBody, CollisionFlags.BS_FLOATS_ON_WATER);
             });
         }
     }
@@ -947,10 +976,10 @@ public sealed class BSPrim : BSPhysObject
         }
         set {
             _rotationalVelocity = value;
+            Util.ClampV(_rotationalVelocity, BSParam.MaxAngularVelocity);
             // m_log.DebugFormat("{0}: RotationalVelocity={1}", LogHeader, _rotationalVelocity);
-            PhysicsScene.TaintedObject("BSPrim.setRotationalVelocity", delegate()
+            PhysScene.TaintedObject("BSPrim.setRotationalVelocity", delegate()
             {
-                DetailLog("{0},BSPrim.SetRotationalVel,taint,rotvel={1}", LocalID, _rotationalVelocity);
                 ForceRotationalVelocity = _rotationalVelocity;
             });
         }
@@ -960,10 +989,12 @@ public sealed class BSPrim : BSPhysObject
             return _rotationalVelocity;
         }
         set {
-            _rotationalVelocity = value;
+            _rotationalVelocity = Util.ClampV(value, BSParam.MaxAngularVelocity);
             if (PhysBody.HasPhysicalBody)
             {
-                PhysicsScene.PE.SetAngularVelocity(PhysBody, _rotationalVelocity);
+                DetailLog("{0},BSPrim.ForceRotationalVel,taint,rotvel={1}", LocalID, _rotationalVelocity);
+                PhysScene.PE.SetAngularVelocity(PhysBody, _rotationalVelocity);
+                // PhysicsScene.PE.SetInterpolationAngularVelocity(PhysBody, _rotationalVelocity);
                 ActivateIfPhysical(false);
             }
         }
@@ -978,7 +1009,7 @@ public sealed class BSPrim : BSPhysObject
         get { return _buoyancy; }
         set {
             _buoyancy = value;
-            PhysicsScene.TaintedObject("BSPrim.setBuoyancy", delegate()
+            PhysScene.TaintedObject("BSPrim.setBuoyancy", delegate()
             {
                 ForceBuoyancy = _buoyancy;
             });
@@ -990,96 +1021,113 @@ public sealed class BSPrim : BSPhysObject
             _buoyancy = value;
             // DetailLog("{0},BSPrim.setForceBuoyancy,taint,buoy={1}", LocalID, _buoyancy);
             // Force the recalculation of the various inertia,etc variables in the object
-            DetailLog("{0},BSPrim.ForceBuoyancy,buoy={1},mass={2}", LocalID, _buoyancy, _mass);
-            UpdatePhysicalMassProperties(_mass, true);
+            UpdatePhysicalMassProperties(RawMass, true);
+            DetailLog("{0},BSPrim.ForceBuoyancy,buoy={1},mass={2},grav={3}", LocalID, _buoyancy, RawMass, Gravity);
             ActivateIfPhysical(false);
         }
     }
 
-    // Used for MoveTo
-    public override OMV.Vector3 PIDTarget {
-        set { _PIDTarget = value; }
-    }
-    public override float PIDTau {
-        set { _PIDTau = value; }
-    }
     public override bool PIDActive {
-        set { _usePID = value; }
+        set {
+            base.MoveToTargetActive = value;
+            EnableActor(MoveToTargetActive, MoveToTargetActorName, delegate()
+            {
+                return new BSActorMoveToTarget(PhysScene, this, MoveToTargetActorName);
+            });
+        }
     }
 
     // Used for llSetHoverHeight and maybe vehicle height
     // Hover Height will override MoveTo target's Z
     public override bool PIDHoverActive {
-        set { _useHoverPID = value; }
-    }
-    public override float PIDHoverHeight {
-        set { _PIDHoverHeight = value; }
-    }
-    public override PIDHoverType PIDHoverType {
-        set { _PIDHoverType = value; }
-    }
-    public override float PIDHoverTau {
-        set { _PIDHoverTao = value; }
+        set {
+            base.HoverActive = value;
+            EnableActor(HoverActive, HoverActorName, delegate()
+            {
+                return new BSActorHover(PhysScene, this, HoverActorName);
+            });
+        }
     }
 
-    // For RotLookAt
-    public override OMV.Quaternion APIDTarget { set { return; } }
-    public override bool APIDActive { set { return; } }
-    public override float APIDStrength { set { return; } }
-    public override float APIDDamping { set { return; } }
-
     public override void AddForce(OMV.Vector3 force, bool pushforce) {
+        // Per documentation, max force is limited.
+        OMV.Vector3 addForce = Util.ClampV(force, BSParam.MaxAddForceMagnitude);
+
         // Since this force is being applied in only one step, make this a force per second.
-        OMV.Vector3 addForce = force / PhysicsScene.LastTimeStep;
-        AddForce(addForce, pushforce, false);
+        addForce /= PhysScene.LastTimeStep;
+        AddForce(addForce, pushforce, false /* inTaintTime */);
     }
+
     // Applying a force just adds this to the total force on the object.
     // This added force will only last the next simulation tick.
     public void AddForce(OMV.Vector3 force, bool pushforce, bool inTaintTime) {
         // for an object, doesn't matter if force is a pushforce or not
-        if (force.IsFinite())
+        if (IsPhysicallyActive)
         {
-            float magnitude = force.Length();
-            if (magnitude > BSParam.MaxAddForceMagnitude)
+            if (force.IsFinite())
             {
-                // Force has a limit
-                force = force / magnitude * BSParam.MaxAddForceMagnitude;
-            }
-
-            OMV.Vector3 addForce = force;
-            DetailLog("{0},BSPrim.addForce,call,force={1}", LocalID, addForce);
+                // DetailLog("{0},BSPrim.addForce,call,force={1}", LocalID, addForce);
 
-            PhysicsScene.TaintedObject(inTaintTime, "BSPrim.AddForce", delegate()
-            {
-                // Bullet adds this central force to the total force for this tick
-                DetailLog("{0},BSPrim.addForce,taint,force={1}", LocalID, addForce);
-                if (PhysBody.HasPhysicalBody)
+                OMV.Vector3 addForce = force;
+                PhysScene.TaintedObject(inTaintTime, "BSPrim.AddForce", delegate()
                 {
-                    PhysicsScene.PE.ApplyCentralForce(PhysBody, addForce);
-                    ActivateIfPhysical(false);
-                }
-            });
+                    // Bullet adds this central force to the total force for this tick
+                    DetailLog("{0},BSPrim.addForce,taint,force={1}", LocalID, addForce);
+                    if (PhysBody.HasPhysicalBody)
+                    {
+                        PhysScene.PE.ApplyCentralForce(PhysBody, addForce);
+                        ActivateIfPhysical(false);
+                    }
+                });
+            }
+            else
+            {
+                m_log.WarnFormat("{0}: AddForce: Got a NaN force applied to a prim. LocalID={1}", LogHeader, LocalID);
+                return;
+            }
         }
-        else
+    }
+
+    public void AddForceImpulse(OMV.Vector3 impulse, bool pushforce, bool inTaintTime) {
+        // for an object, doesn't matter if force is a pushforce or not
+        if (!IsPhysicallyActive)
         {
-            m_log.WarnFormat("{0}: Got a NaN force applied to a prim. LocalID={1}", LogHeader, LocalID);
-            return;
+            if (impulse.IsFinite())
+            {
+                OMV.Vector3 addImpulse = Util.ClampV(impulse, BSParam.MaxAddForceMagnitude);
+                // DetailLog("{0},BSPrim.addForceImpulse,call,impulse={1}", LocalID, impulse);
+
+                PhysScene.TaintedObject(inTaintTime, "BSPrim.AddImpulse", delegate()
+                {
+                    // Bullet adds this impulse immediately to the velocity
+                    DetailLog("{0},BSPrim.addForceImpulse,taint,impulseforce={1}", LocalID, addImpulse);
+                    if (PhysBody.HasPhysicalBody)
+                    {
+                        PhysScene.PE.ApplyCentralImpulse(PhysBody, addImpulse);
+                        ActivateIfPhysical(false);
+                    }
+                });
+            }
+            else
+            {
+                m_log.WarnFormat("{0}: AddForceImpulse: Got a NaN impulse applied to a prim. LocalID={1}", LogHeader, LocalID);
+                return;
+            }
         }
     }
 
-    public override void AddAngularForce(OMV.Vector3 force, bool pushforce) {
-        AddAngularForce(force, pushforce, false);
-    }
-    public void AddAngularForce(OMV.Vector3 force, bool pushforce, bool inTaintTime)
+    // BSPhysObject.AddAngularForce()
+    public override void AddAngularForce(OMV.Vector3 force, bool pushforce, bool inTaintTime)
     {
         if (force.IsFinite())
         {
             OMV.Vector3 angForce = force;
-            PhysicsScene.TaintedObject(inTaintTime, "BSPrim.AddAngularForce", delegate()
+            PhysScene.TaintedObject(inTaintTime, "BSPrim.AddAngularForce", delegate()
             {
                 if (PhysBody.HasPhysicalBody)
                 {
-                    PhysicsScene.PE.ApplyTorque(PhysBody, angForce);
+                    DetailLog("{0},BSPrim.AddAngularForce,taint,angForce={1}", LocalID, angForce);
+                    PhysScene.PE.ApplyTorque(PhysBody, angForce);
                     ActivateIfPhysical(false);
                 }
             });
@@ -1098,11 +1146,11 @@ public sealed class BSPrim : BSPhysObject
     public void ApplyTorqueImpulse(OMV.Vector3 impulse, bool inTaintTime)
     {
         OMV.Vector3 applyImpulse = impulse;
-        PhysicsScene.TaintedObject(inTaintTime, "BSPrim.ApplyTorqueImpulse", delegate()
+        PhysScene.TaintedObject(inTaintTime, "BSPrim.ApplyTorqueImpulse", delegate()
         {
             if (PhysBody.HasPhysicalBody)
             {
-                PhysicsScene.PE.ApplyTorqueImpulse(PhysBody, applyImpulse);
+                PhysScene.PE.ApplyTorqueImpulse(PhysBody, applyImpulse);
                 ActivateIfPhysical(false);
             }
         });
@@ -1387,19 +1435,10 @@ public sealed class BSPrim : BSPhysObject
         profileEnd = 1.0f - (float)BaseShape.ProfileEnd * 2.0e-5f;
         volume *= (profileEnd - profileBegin);
 
-        returnMass = _density * volume;
-
-        /* Comment out code that computes the mass of the linkset. That is done in the Linkset class.
-        if (IsRootOfLinkset)
-        {
-            foreach (BSPrim prim in _childrenPrims)
-            {
-                returnMass += prim.CalculateMass();
-            }
-        }
-         */
+        returnMass = Density * BSParam.DensityScaleFactor * volume;
 
         returnMass = Util.Clamp(returnMass, BSParam.MinimumObjectMass, BSParam.MaximumObjectMass);
+        // DetailLog("{0},BSPrim.CalculateMass,den={1},vol={2},mass={3}", LocalID, Density, volume, returnMass);
 
         return returnMass;
     }// end CalculateMass
@@ -1410,104 +1449,68 @@ public sealed class BSPrim : BSPhysObject
     // Called at taint-time!!!
     public void CreateGeomAndObject(bool forceRebuild)
     {
-        // If this prim is part of a linkset, we must remove and restore the physical
-        //    links if the body is rebuilt.
-        bool needToRestoreLinkset = false;
-        bool needToRestoreVehicle = false;
-
         // Create the correct physical representation for this type of object.
-        // Updates PhysBody and PhysShape with the new information.
-        // Ignore 'forceRebuild'. This routine makes the right choices and changes of necessary.
-        PhysicsScene.Shapes.GetBodyAndShape(false, PhysicsScene.World, this, null, delegate(BulletBody dBody)
+        // Updates base.PhysBody and base.PhysShape with the new information.
+        // Ignore 'forceRebuild'. 'GetBodyAndShape' makes the right choices and changes of necessary.
+        PhysScene.Shapes.GetBodyAndShape(false /*forceRebuild */, PhysScene.World, this, delegate(BulletBody pBody, BulletShape pShape)
         {
             // Called if the current prim body is about to be destroyed.
             // Remove all the physical dependencies on the old body.
             // (Maybe someday make the changing of BSShape an event to be subscribed to by BSLinkset, ...)
-            needToRestoreLinkset = Linkset.RemoveBodyDependencies(this);
-            needToRestoreVehicle = _vehicle.RemoveBodyDependencies(this);
+            // Note: this virtual function is overloaded by BSPrimLinkable to remove linkset constraints.
+            RemoveDependencies();
         });
 
-        if (needToRestoreLinkset)
-        {
-            // If physical body dependencies were removed, restore them
-            Linkset.RestoreBodyDependencies(this);
-        }
-        if (needToRestoreVehicle)
-        {
-            // If physical body dependencies were removed, restore them
-            _vehicle.RestoreBodyDependencies(this);
-        }
-
         // Make sure the properties are set on the new object
         UpdatePhysicalParameters();
         return;
     }
 
-    // The physics engine says that properties have updated. Update same and inform
-    // the world that things have changed.
-    // TODO: do we really need to check for changed? Maybe just copy values and call RequestPhysicsterseUpdate()
-    enum UpdatedProperties {
-        Position      = 1 << 0,
-        Rotation      = 1 << 1,
-        Velocity      = 1 << 2,
-        Acceleration  = 1 << 3,
-        RotationalVel = 1 << 4
+    // Called at taint-time
+    protected virtual void RemoveDependencies()
+    {
+        PhysicalActors.RemoveDependencies();
     }
 
-    const float ROTATION_TOLERANCE = 0.01f;
-    const float VELOCITY_TOLERANCE = 0.001f;
-    const float POSITION_TOLERANCE = 0.05f;
-    const float ACCELERATION_TOLERANCE = 0.01f;
-    const float ROTATIONAL_VELOCITY_TOLERANCE = 0.01f;
-
+    // The physics engine says that properties have updated. Update same and inform
+    // the world that things have changed.
     public override void UpdateProperties(EntityProperties entprop)
     {
-        // Updates only for individual prims and for the root object of a linkset.
-        if (Linkset.IsRoot(this))
-        {
-            // A temporary kludge to suppress the rotational effects introduced on vehicles by Bullet
-            // TODO: handle physics introduced by Bullet with computed vehicle physics.
-            if (_vehicle.IsActive)
-            {
-                entprop.RotationalVelocity = OMV.Vector3.Zero;
-            }
+        // Let anyone (like the actors) modify the updated properties before they are pushed into the object and the simulator.
+        TriggerPreUpdatePropertyAction(ref entprop);
 
-            // Assign directly 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;
+        // DetailLog("{0},BSPrim.UpdateProperties,entry,entprop={1}", LocalID, entprop);   // DEBUG DEBUG
 
-            // The sanity check can change the velocity and/or position.
-            if (IsPhysical && PositionSanityCheck(true))
-            {
-                entprop.Position = _position;
-                entprop.Velocity = _velocity;
-            }
+        // Assign directly to the local variables so the normal set actions do not happen
+        _position = entprop.Position;
+        _orientation = entprop.Rotation;
+        // DEBUG DEBUG DEBUG -- smooth velocity changes a bit. The simulator seems to be
+        //    very sensitive to velocity changes.
+        if (entprop.Velocity == OMV.Vector3.Zero || !entprop.Velocity.ApproxEquals(RawVelocity, BSParam.UpdateVelocityChangeThreshold))
+            RawVelocity = entprop.Velocity;
+        _acceleration = entprop.Acceleration;
+        _rotationalVelocity = entprop.RotationalVelocity;
 
-            OMV.Vector3 direction = OMV.Vector3.UnitX * _orientation;   // DEBUG DEBUG DEBUG
-            DetailLog("{0},BSPrim.UpdateProperties,call,pos={1},orient={2},dir={3},vel={4},rotVel={5}",
-                    LocalID, _position, _orientation, direction, _velocity, _rotationalVelocity);
+        // DetailLog("{0},BSPrim.UpdateProperties,afterAssign,entprop={1}", LocalID, entprop);   // DEBUG DEBUG
 
-            // remember the current and last set values
-            LastEntityProperties = CurrentEntityProperties;
-            CurrentEntityProperties = entprop;
-
-            base.RequestPhysicsterseUpdate();
-        }
-            /*
-        else
+        // The sanity check can change the velocity and/or position.
+        if (PositionSanityCheck(true /* inTaintTime */ ))
         {
-            // For debugging, report the movement of children
-            DetailLog("{0},BSPrim.UpdateProperties,child,pos={1},orient={2},vel={3},accel={4},rotVel={5}",
-                    LocalID, entprop.Position, entprop.Rotation, entprop.Velocity,
-                    entprop.Acceleration, entprop.RotationalVelocity);
+            entprop.Position = _position;
+            entprop.Velocity = RawVelocity;
+            entprop.RotationalVelocity = _rotationalVelocity;
+            entprop.Acceleration = _acceleration;
         }
-             */
 
-        // The linkset implimentation might want to know about this.
-        Linkset.UpdateProperties(this, true);
+        OMV.Vector3 direction = OMV.Vector3.UnitX * _orientation;   // DEBUG DEBUG DEBUG
+        DetailLog("{0},BSPrim.UpdateProperties,call,entProp={1},dir={2}", LocalID, entprop, direction);
+
+        // remember the current and last set values
+        LastEntityProperties = CurrentEntityProperties;
+        CurrentEntityProperties = entprop;
+
+        // Note that BSPrim can be overloaded by BSPrimLinkable which controls updates from root and children prims.
+        base.RequestPhysicsterseUpdate();
     }
 }
 }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrimDisplaced.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrimDisplaced.cs
new file mode 100755
index 0000000..f5ee671
--- /dev/null
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrimDisplaced.cs
@@ -0,0 +1,165 @@
+/*
+ * 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 copyright
+ *       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.
+ *
+ * The quotations from http://wiki.secondlife.com/wiki/Linden_Vehicle_Tutorial
+ * are Copyright (c) 2009 Linden Research, Inc and are used under their license
+ * of Creative Commons Attribution-Share Alike 3.0
+ * (http://creativecommons.org/licenses/by-sa/3.0/).
+ */
+
+using System;
+using System.Collections.Generic;
+using System.Reflection;
+using System.Runtime.InteropServices;
+using OpenMetaverse;
+using OpenSim.Framework;
+using OpenSim.Region.Physics.Manager;
+
+using OMV = OpenMetaverse;
+
+namespace OpenSim.Region.Physics.BulletSPlugin
+{
+public class BSPrimDisplaced : BSPrim
+{
+    // The purpose of this module is to do any mapping between what the simulator thinks
+    //    the prim position and orientation is and what the physical position/orientation.
+    //    This difference happens because Bullet assumes the center-of-mass is the <0,0,0>
+    //    of the prim/linkset. The simulator tracks the location of the prim/linkset by
+    //    the location of the root prim. So, if center-of-mass is anywhere but the origin
+    //    of the root prim, the physical origin is displaced from the simulator origin.
+    //
+    // This routine works by capturing the Force* setting of position/orientation/... and
+    //    adjusting the simulator values (being set) into the physical values.
+    //    The conversion is also done in the opposite direction (physical origin -> simulator origin).
+    //
+    // The updateParameter call is also captured and the values from the physics engine
+    //    are converted into simulator origin values before being passed to the base
+    //    class.
+
+    public virtual OMV.Vector3 PositionDisplacement { get; set; }
+    public virtual OMV.Quaternion OrientationDisplacement { get; set; }
+
+    public BSPrimDisplaced(uint localID, String primName, BSScene parent_scene, OMV.Vector3 pos, OMV.Vector3 size,
+                       OMV.Quaternion rotation, PrimitiveBaseShape pbs, bool pisPhysical)
+        : base(localID, primName, parent_scene, pos, size, rotation, pbs, pisPhysical)
+    {
+        ClearDisplacement();
+    }
+
+    public void ClearDisplacement()
+    {
+        PositionDisplacement = OMV.Vector3.Zero;
+        OrientationDisplacement = OMV.Quaternion.Identity;
+    }
+
+    // Set this sets and computes the displacement from the passed prim to the center-of-mass.
+    // A user set value for center-of-mass overrides whatever might be passed in here.
+    // The displacement is in local coordinates (relative to root prim in linkset oriented coordinates).
+    public virtual void SetEffectiveCenterOfMassDisplacement(Vector3 centerOfMassDisplacement)
+    {
+        Vector3 comDisp;
+        if (UserSetCenterOfMassDisplacement.HasValue)
+            comDisp = (OMV.Vector3)UserSetCenterOfMassDisplacement;
+        else
+            comDisp = centerOfMassDisplacement;
+
+        DetailLog("{0},BSPrimDisplaced.SetEffectiveCenterOfMassDisplacement,userSet={1},comDisp={2}",
+                                    LocalID, UserSetCenterOfMassDisplacement.HasValue, comDisp);
+        if (comDisp == Vector3.Zero)
+        {
+            // If there is no diplacement. Things get reset.
+            PositionDisplacement = OMV.Vector3.Zero;
+            OrientationDisplacement = OMV.Quaternion.Identity;
+        }
+        else
+        {
+            // Remember the displacement from root as well as the origional rotation of the
+            //    new center-of-mass.
+            PositionDisplacement = comDisp;
+            OrientationDisplacement = OMV.Quaternion.Identity;
+        }
+    }
+
+    public override Vector3 ForcePosition
+    {
+        get { return base.ForcePosition; }
+        set
+        {
+            if (PositionDisplacement != OMV.Vector3.Zero)
+            {
+                OMV.Vector3 displacedPos = value - (PositionDisplacement * RawOrientation);
+                DetailLog("{0},BSPrimDisplaced.ForcePosition,val={1},disp={2},newPos={3}", LocalID, value, PositionDisplacement, displacedPos);
+                base.ForcePosition = displacedPos;
+            }
+            else
+            {
+                base.ForcePosition = value;
+            }
+        }
+    }
+
+    public override Quaternion ForceOrientation
+    {
+        get { return base.ForceOrientation; }
+        set
+        {
+            // TODO:
+            base.ForceOrientation = value;
+        }
+    }
+
+    // TODO: decide if this is the right place for these variables.
+    //     Somehow incorporate the optional settability by the user.
+    // Is this used?
+    public override OMV.Vector3 CenterOfMass
+    {
+        get { return RawPosition; }
+    }
+
+    // Is this used?
+    public override OMV.Vector3 GeometricCenter
+    {
+        get { return RawPosition; }
+    }
+
+    public override void UpdateProperties(EntityProperties entprop)
+    {
+        // Undo any center-of-mass displacement that might have been done.
+        if (PositionDisplacement != OMV.Vector3.Zero || OrientationDisplacement != OMV.Quaternion.Identity)
+        {
+            // Correct for any rotation around the center-of-mass
+            // TODO!!!
+            
+            OMV.Vector3 displacedPos = entprop.Position + (PositionDisplacement * entprop.Rotation);
+            DetailLog("{0},BSPrimDisplaced.ForcePosition,physPos={1},disp={2},newPos={3}", LocalID, entprop.Position, PositionDisplacement, displacedPos);
+            entprop.Position = displacedPos;
+            // entprop.Rotation = something;
+        }
+
+        base.UpdateProperties(entprop);
+    }
+}
+}
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrimLinkable.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrimLinkable.cs
new file mode 100755
index 0000000..235da78
--- /dev/null
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrimLinkable.cs
@@ -0,0 +1,192 @@
+/*
+ * 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.Linq;
+using System.Text;
+
+using OpenSim.Framework;
+
+using OMV = OpenMetaverse;
+
+namespace OpenSim.Region.Physics.BulletSPlugin
+{
+public class BSPrimLinkable : BSPrimDisplaced
+{
+    public BSLinkset Linkset { get; set; }
+    // The index of this child prim.
+    public int LinksetChildIndex { get; set; }
+
+    public BSLinksetInfo LinksetInfo { get; set; }
+
+    public BSPrimLinkable(uint localID, String primName, BSScene parent_scene, OMV.Vector3 pos, OMV.Vector3 size,
+                       OMV.Quaternion rotation, PrimitiveBaseShape pbs, bool pisPhysical)
+        : base(localID, primName, parent_scene, pos, size, rotation, pbs, pisPhysical)
+    {
+        Linkset = BSLinkset.Factory(PhysScene, this);
+
+        PhysScene.TaintedObject("BSPrimLinksetCompound.Refresh", delegate()
+        {
+            Linkset.Refresh(this);
+        });
+    }
+
+    public override void Destroy()
+    {
+        Linkset = Linkset.RemoveMeFromLinkset(this);
+        base.Destroy();
+    }
+
+    public override void link(Manager.PhysicsActor obj)
+    {
+        BSPrimLinkable parent = obj as BSPrimLinkable;
+        if (parent != null)
+        {
+            BSPhysObject parentBefore = Linkset.LinksetRoot;
+            int childrenBefore = Linkset.NumberOfChildren;
+
+            Linkset = parent.Linkset.AddMeToLinkset(this);
+
+            DetailLog("{0},BSPrimLinkset.link,call,parentBefore={1}, childrenBefore=={2}, parentAfter={3}, childrenAfter={4}",
+                LocalID, parentBefore.LocalID, childrenBefore, Linkset.LinksetRoot.LocalID, Linkset.NumberOfChildren);
+        }
+        return;
+    }
+
+    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
+
+        BSPhysObject parentBefore = Linkset.LinksetRoot;
+        int childrenBefore = Linkset.NumberOfChildren;
+
+        Linkset = Linkset.RemoveMeFromLinkset(this);
+
+        DetailLog("{0},BSPrimLinkset.delink,parentBefore={1},childrenBefore={2},parentAfter={3},childrenAfter={4}, ",
+            LocalID, parentBefore.LocalID, childrenBefore, Linkset.LinksetRoot.LocalID, Linkset.NumberOfChildren);
+        return;
+    }
+
+    // When simulator changes position, this might be moving a child of the linkset.
+    public override OMV.Vector3 Position
+    {
+        get { return base.Position; }
+        set
+        {
+            base.Position = value;
+            PhysScene.TaintedObject("BSPrimLinkset.setPosition", delegate()
+            {
+                Linkset.UpdateProperties(UpdatedProperties.Position, this);
+            });
+        }
+    }
+
+    // When simulator changes orientation, this might be moving a child of the linkset.
+    public override OMV.Quaternion Orientation
+    {
+        get { return base.Orientation; }
+        set
+        {
+            base.Orientation = value;
+            PhysScene.TaintedObject("BSPrimLinkset.setOrientation", delegate()
+            {
+                Linkset.UpdateProperties(UpdatedProperties.Orientation, this);
+            });
+        }
+    }
+
+    public override float TotalMass
+    {
+        get { return Linkset.LinksetMass; }
+    }
+
+    public override void UpdatePhysicalParameters()
+    {
+        base.UpdatePhysicalParameters();
+        // Recompute any linkset parameters.
+        // When going from non-physical to physical, this re-enables the constraints that
+        //     had been automatically disabled when the mass was set to zero.
+        // For compound based linksets, this enables and disables interactions of the children.
+        if (Linkset != null)    // null can happen during initialization
+            Linkset.Refresh(this);
+    }
+
+    protected override void MakeDynamic(bool makeStatic)
+    {
+        base.MakeDynamic(makeStatic);
+        if (makeStatic)
+            Linkset.MakeStatic(this);
+        else
+            Linkset.MakeDynamic(this);
+    }
+
+    // Body is being taken apart. Remove physical dependencies and schedule a rebuild.
+    protected override void RemoveDependencies()
+    {
+        Linkset.RemoveDependencies(this);
+        base.RemoveDependencies();
+    }
+
+    public override void UpdateProperties(EntityProperties entprop)
+    {
+        if (Linkset.IsRoot(this))
+        {
+            // Properties are only updated for the roots of a linkset.
+            // TODO: this will have to change when linksets are articulated.
+            base.UpdateProperties(entprop);
+        }
+            /*
+        else
+        {
+            // For debugging, report the movement of children
+            DetailLog("{0},BSPrim.UpdateProperties,child,pos={1},orient={2},vel={3},accel={4},rotVel={5}",
+                    LocalID, entprop.Position, entprop.Rotation, entprop.Velocity,
+                    entprop.Acceleration, entprop.RotationalVelocity);
+        }
+             */
+        // The linkset might like to know about changing locations
+        Linkset.UpdateProperties(UpdatedProperties.EntPropUpdates, this);
+    }
+
+    public override bool Collide(uint collidingWith, BSPhysObject collidee,
+                    OMV.Vector3 contactPoint, OMV.Vector3 contactNormal, float pentrationDepth)
+    {
+        // prims in the same linkset cannot collide with each other
+        BSPrimLinkable convCollidee = collidee as BSPrimLinkable;
+        if (convCollidee != null && (this.Linkset.LinksetID == convCollidee.Linkset.LinksetID))
+        {
+            return false;
+        }
+
+        // TODO: handle collisions of other objects with with children of linkset.
+        //    This is a problem for LinksetCompound since the children are packed into the root.
+
+        return base.Collide(collidingWith, collidee, contactPoint, contactNormal, pentrationDepth);
+    }
+}
+}
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs b/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
index 7017194..a4a8794 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
@@ -26,6 +26,7 @@
  */
 using System;
 using System.Collections.Generic;
+using System.Linq;
 using System.Reflection;
 using System.Runtime.InteropServices;
 using System.Text;
@@ -81,14 +82,13 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
     internal long m_simulationStep = 0;
     internal float NominalFrameRate { get; set; }
     public long SimulationStep { get { return m_simulationStep; } }
-    internal int m_taintsToProcessPerStep;
     internal float LastTimeStep { get; private set; }
 
     // Physical objects can register for prestep or poststep events
     public delegate void PreStepAction(float timeStep);
     public delegate void PostStepAction(float timeStep);
     public event PreStepAction BeforeStep;
-    public event PreStepAction AfterStep;
+    public event PostStepAction AfterStep;
 
     // 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
@@ -161,17 +161,22 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
     private int m_physicsLoggingFileMinutes;
     private bool m_physicsLoggingDoFlush;
     private bool m_physicsPhysicalDumpEnabled;
-    public float PhysicsMetricDumpFrames { get; set; }
+    public int PhysicsMetricDumpFrames { get; set; }
     // 'true' of the vehicle code is to log lots of details
     public bool VehicleLoggingEnabled { get; private set; }
     public bool VehiclePhysicalLoggingEnabled { get; private set; }
 
     #region Construction and Initialization
-    public BSScene(string identifier)
+    public BSScene(string engineType, string identifier)
     {
         m_initialized = false;
-        // we are passed the name of the region we're working for.
+
+        // The name of the region we're working for is passed to us. Keep for identification.
         RegionName = identifier;
+
+        // Set identifying variables in the PhysicsScene interface.
+        EngineType = engineType;
+        Name = EngineType + "/" + RegionName;
     }
 
     public override void Initialise(IMesher meshmerizer, IConfigSource config)
@@ -311,6 +316,10 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
                 break;
             case "bulletxna":
                 ret = new BSAPIXNA(engineName, this);
+                // Disable some features that are not implemented in BulletXNA
+                m_log.InfoFormat("{0} Disabling some physics features not implemented by BulletXNA", LogHeader);
+                BSParam.ShouldUseBulletHACD = false;
+                BSParam.ShouldUseSingleConvexHullForPrims = false;
                 break;
         }
 
@@ -382,12 +391,14 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
         if (!m_initialized) return null;
 
         BSCharacter actor = new BSCharacter(localID, avName, this, position, size, isFlying);
-        lock (PhysObjects) PhysObjects.Add(localID, actor);
+        lock (PhysObjects)
+            PhysObjects.Add(localID, actor);
 
         // TODO: Remove kludge someday.
         // We must generate a collision for avatars whether they collide or not.
         // This is required by OpenSim to update avatar animations, etc.
-        lock (m_avatars) m_avatars.Add(actor);
+        lock (m_avatars)
+            m_avatars.Add(actor);
 
         return actor;
     }
@@ -403,9 +414,11 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
         {
             try
             {
-                lock (PhysObjects) PhysObjects.Remove(actor.LocalID);
+                lock (PhysObjects)
+                    PhysObjects.Remove(bsactor.LocalID);
                 // Remove kludge someday
-                lock (m_avatars) m_avatars.Remove(bsactor);
+                lock (m_avatars)
+                    m_avatars.Remove(bsactor);
             }
             catch (Exception e)
             {
@@ -414,13 +427,18 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
             bsactor.Destroy();
             // bsactor.dispose();
         }
+        else
+        {
+            m_log.ErrorFormat("{0}: Requested to remove avatar that is not a BSCharacter. ID={1}, type={2}",
+                                        LogHeader, actor.LocalID, actor.GetType().Name);
+        }
     }
 
     public override void RemovePrim(PhysicsActor prim)
     {
         if (!m_initialized) return;
 
-        BSPrim bsprim = prim as BSPrim;
+        BSPhysObject bsprim = prim as BSPhysObject;
         if (bsprim != null)
         {
             DetailLog("{0},RemovePrim,call", bsprim.LocalID);
@@ -449,9 +467,9 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
 
         if (!m_initialized) return null;
 
-        DetailLog("{0},AddPrimShape,call", localID);
+        // DetailLog("{0},BSScene.AddPrimShape,call", localID);
 
-        BSPrim prim = new BSPrim(localID, primName, this, position, size, rotation, pbs, isPhysical);
+        BSPhysObject prim = new BSPrimLinkable(localID, primName, this, position, size, rotation, pbs, isPhysical);
         lock (PhysObjects) PhysObjects.Add(localID, prim);
         return prim;
     }
@@ -486,6 +504,7 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
         ProcessTaints();
 
         // Some of the physical objects requre individual, pre-step calls
+        //      (vehicles and avatar movement, in particular)
         TriggerPreStepEvent(timeStep);
 
         // the prestep actions might have added taints
@@ -527,7 +546,7 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
             collidersCount = 0;
         }
 
-        if ((m_simulationStep % PhysicsMetricDumpFrames) == 0)
+        if (PhysicsMetricDumpFrames != 0 && ((m_simulationStep % PhysicsMetricDumpFrames) == 0))
             PE.DumpPhysicsStatistics(World);
 
         // Get a value for 'now' so all the collision and update routines don't have to get their own.
@@ -543,8 +562,9 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
                 uint cB = m_collisionArray[ii].bID;
                 Vector3 point = m_collisionArray[ii].point;
                 Vector3 normal = m_collisionArray[ii].normal;
-                SendCollision(cA, cB, point, normal, 0.01f);
-                SendCollision(cB, cA, point, -normal, 0.01f);
+                float penetration = m_collisionArray[ii].penetration;
+                SendCollision(cA, cB, point, normal, penetration);
+                SendCollision(cB, cA, point, -normal, penetration);
             }
         }
 
@@ -682,7 +702,21 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
 
     public override Dictionary<uint, float> GetTopColliders()
     {
-        return new Dictionary<uint, float>();
+        Dictionary<uint, float> topColliders;
+
+        lock (PhysObjects)
+        {
+            foreach (KeyValuePair<uint, BSPhysObject> kvp in PhysObjects)
+            {
+                kvp.Value.ComputeCollisionScore();
+            }
+
+            List<BSPhysObject> orderedPrims = new List<BSPhysObject>(PhysObjects.Values);
+            orderedPrims.OrderByDescending(p => p.CollisionScore);
+            topColliders = orderedPrims.Take(25).ToDictionary(p => p.LocalID, p => p.CollisionScore);
+        }
+
+        return topColliders;
     }
 
     public override bool IsThreaded { get { return false;  } }
@@ -694,8 +728,8 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
     //    TriggerPreStepEvent
     //    DoOneTimeTaints
     //    Step()
-    //       ProcessAndForwardCollisions
-    //       ProcessAndForwardPropertyUpdates
+    //       ProcessAndSendToSimulatorCollisions
+    //       ProcessAndSendToSimulatorPropertyUpdates
     //    TriggerPostStepEvent
 
     // Calls to the PhysicsActors can't directly call into the physics engine
@@ -733,7 +767,7 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
 
     private void TriggerPostStepEvent(float timeStep)
     {
-        PreStepAction actions = AfterStep;
+        PostStepAction actions = AfterStep;
         if (actions != null)
             actions(timeStep);
 
@@ -825,15 +859,13 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
         {
             DetailLog("{0},BSScene.AssertInTaintTime,NOT IN TAINT TIME,Region={1},Where={2}", DetailLogZero, RegionName, whereFrom);
             m_log.ErrorFormat("{0} NOT IN TAINT TIME!! Region={1}, Where={2}", LogHeader, RegionName, whereFrom);
-            Util.PrintCallStack(DetailLog);
+            // Util.PrintCallStack(DetailLog);
         }
         return InTaintTime;
     }
 
     #endregion // Taints
 
-    #region INI and command line parameter processing
-
     #region IPhysicsParameters
     // Get the list of parameters this physics engine supports
     public PhysParameterEntry[] GetParameterList()
@@ -848,64 +880,65 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
     //   will use the next time since it's pinned and shared memory.
     // Some of the values require calling into the physics engine to get the new
     //   value activated ('terrainFriction' for instance).
-    public bool SetPhysicsParameter(string parm, float val, uint localID)
+    public bool SetPhysicsParameter(string parm, string val, uint localID)
     {
         bool ret = false;
-        BSParam.ParameterDefn theParam;
+
+        BSParam.ParameterDefnBase theParam;
         if (BSParam.TryGetParameter(parm, out theParam))
         {
-            theParam.setter(this, parm, localID, val);
+            // Set the value in the C# code
+            theParam.SetValue(this, val);
+
+            // Optionally set the parameter in the unmanaged code
+            if (theParam.HasSetOnObject)
+            {
+                // update all the localIDs specified
+                // 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
+                List<uint> objectIDs = new List<uint>();
+                switch (localID)
+                {
+                    case PhysParameterEntry.APPLY_TO_NONE:
+                        // This will cause a call into the physical world if some operation is specified (SetOnObject).
+                        objectIDs.Add(TERRAIN_ID);
+                        TaintedUpdateParameter(parm, objectIDs, val);
+                        break;
+                    case PhysParameterEntry.APPLY_TO_ALL:
+                        lock (PhysObjects) objectIDs = new List<uint>(PhysObjects.Keys);
+                        TaintedUpdateParameter(parm, objectIDs, val);
+                        break;
+                    default:
+                        // setting only one localID
+                        objectIDs.Add(localID);
+                        TaintedUpdateParameter(parm, objectIDs, val);
+                        break;
+                }
+            }
+
             ret = true;
         }
         return ret;
     }
 
-    // update all the localIDs specified
-    // 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
-    internal delegate void AssignVal(float x);
-    internal void UpdateParameterObject(AssignVal setDefault, string parm, uint localID, float val)
-    {
-        List<uint> objectIDs = new List<uint>();
-        switch (localID)
-        {
-            case PhysParameterEntry.APPLY_TO_NONE:
-                setDefault(val);            // setting only the default value
-                // This will cause a call into the physical world if some operation is specified (SetOnObject).
-                objectIDs.Add(TERRAIN_ID);
-                TaintedUpdateParameter(parm, objectIDs, val);
-                break;
-            case PhysParameterEntry.APPLY_TO_ALL:
-                setDefault(val);  // setting ALL also sets the default value
-                lock (PhysObjects) objectIDs = new List<uint>(PhysObjects.Keys);
-                TaintedUpdateParameter(parm, objectIDs, val);
-                break;
-            default:
-                // setting only one localID
-                objectIDs.Add(localID);
-                TaintedUpdateParameter(parm, objectIDs, val);
-                break;
-        }
-    }
-
     // schedule the actual updating of the paramter to when the phys engine is not busy
-    private void TaintedUpdateParameter(string parm, List<uint> lIDs, float val)
+    private void TaintedUpdateParameter(string parm, List<uint> lIDs, string val)
     {
-        float xval = val;
+        string xval = val;
         List<uint> xlIDs = lIDs;
         string xparm = parm;
         TaintedObject("BSScene.UpdateParameterSet", delegate() {
-            BSParam.ParameterDefn thisParam;
+            BSParam.ParameterDefnBase thisParam;
             if (BSParam.TryGetParameter(xparm, out thisParam))
             {
-                if (thisParam.onObject != null)
+                if (thisParam.HasSetOnObject)
                 {
                     foreach (uint lID in xlIDs)
                     {
                         BSPhysObject theObject = null;
-                        PhysObjects.TryGetValue(lID, out theObject);
-                        thisParam.onObject(this, theObject, xval);
+                        if (PhysObjects.TryGetValue(lID, out theObject))
+                            thisParam.SetOnObject(this, theObject);
                     }
                 }
             }
@@ -914,14 +947,14 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
 
     // Get parameter.
     // Return 'false' if not able to get the parameter.
-    public bool GetPhysicsParameter(string parm, out float value)
+    public bool GetPhysicsParameter(string parm, out string value)
     {
-        float val = 0f;
+        string val = String.Empty;
         bool ret = false;
-        BSParam.ParameterDefn theParam;
+        BSParam.ParameterDefnBase theParam;
         if (BSParam.TryGetParameter(parm, out theParam))
         {
-            val = theParam.getter(this);
+            val = theParam.GetValue(this);
             ret = true;
         }
         value = val;
@@ -930,8 +963,6 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
 
     #endregion IPhysicsParameters
 
-    #endregion Runtime settable parameters
-
     // Invoke the detailed logger and output something if it's enabled.
     public void DetailLog(string msg, params Object[] args)
     {
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs b/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs
index d361f18..64aaa15 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs
@@ -38,38 +38,15 @@ public sealed class BSShapeCollection : IDisposable
 {
     private static string LogHeader = "[BULLETSIM SHAPE COLLECTION]";
 
-    private BSScene PhysicsScene { get; set; }
+    private BSScene m_physicsScene { get; set; }
 
     private Object m_collectionActivityLock = new Object();
 
-    // Description of a Mesh
-    private struct MeshDesc
-    {
-        public BulletShape shape;
-        public int referenceCount;
-        public DateTime lastReferenced;
-        public UInt64 shapeKey;
-    }
-
-    // Description of a hull.
-    // Meshes and hulls have the same shape hash key but we only need hulls for efficient collision calculations.
-    private struct HullDesc
-    {
-        public BulletShape shape;
-        public int referenceCount;
-        public DateTime lastReferenced;
-        public UInt64 shapeKey;
-    }
-
-    // The sharable set of meshes and hulls. Indexed by their shape hash.
-    private Dictionary<System.UInt64, MeshDesc> Meshes = new Dictionary<System.UInt64, MeshDesc>();
-    private Dictionary<System.UInt64, HullDesc> Hulls = new Dictionary<System.UInt64, HullDesc>();
-
     private bool DDetail = false;
 
     public BSShapeCollection(BSScene physScene)
     {
-        PhysicsScene = physScene;
+        m_physicsScene = physScene;
         // Set the next to 'true' for very detailed shape update detailed logging (detailed details?)
         // While detailed debugging is still active, this is better than commenting out all the
         //     DetailLog statements. When debugging slows down, this and the protected logging
@@ -86,22 +63,18 @@ public sealed class BSShapeCollection : IDisposable
     // Mostly used for changing bodies out from under Linksets.
     // Useful for other cases where parameters need saving.
     // Passing 'null' says no callback.
-    public delegate void ShapeDestructionCallback(BulletShape shape);
-    public delegate void BodyDestructionCallback(BulletBody body);
+    public delegate void PhysicalDestructionCallback(BulletBody pBody, BulletShape pShape);
 
     // Called to update/change the body and shape for an object.
-    // First checks the shape and updates that if necessary then makes
-    //    sure the body is of the right type.
+    // The object has some shape and body on it. Here we decide if that is the correct shape
+    //    for the current state of the object (static/dynamic/...).
+    // If bodyCallback is not null, it is called if either the body or the shape are changed
+    //    so dependencies (like constraints) can be removed before the physical object is dereferenced.
     // Return 'true' if either the body or the shape changed.
-    // 'shapeCallback' and 'bodyCallback' are, if non-null, functions called just before
-    //    the current shape or body is destroyed. This allows the caller to remove any
-    //    higher level dependencies on the shape or body. Mostly used for LinkSets to
-    //    remove the physical constraints before the body is destroyed.
-    // Called at taint-time!!
-    public bool GetBodyAndShape(bool forceRebuild, BulletWorld sim, BSPhysObject prim,
-                    ShapeDestructionCallback shapeCallback, BodyDestructionCallback bodyCallback)
+    // Called at taint-time.
+    public bool GetBodyAndShape(bool forceRebuild, BulletWorld sim, BSPhysObject prim, PhysicalDestructionCallback bodyCallback)
     {
-        PhysicsScene.AssertInTaintTime("BSShapeCollection.GetBodyAndShape");
+        m_physicsScene.AssertInTaintTime("BSShapeCollection.GetBodyAndShape");
 
         bool ret = false;
 
@@ -111,13 +84,12 @@ public sealed class BSShapeCollection : IDisposable
             // Do we have the correct geometry for this type of object?
             // Updates prim.BSShape with information/pointers to shape.
             // Returns 'true' of BSShape is changed to a new shape.
-            bool newGeom = CreateGeom(forceRebuild, prim, shapeCallback);
+            bool newGeom = CreateGeom(forceRebuild, prim, bodyCallback);
             // If we had to select a new shape geometry for the object,
             //    rebuild the body around it.
             // Updates prim.BSBody with information/pointers to requested body
             // Returns 'true' if BSBody was changed.
-            bool newBody = CreateBody((newGeom || forceRebuild), prim, PhysicsScene.World,
-                                    prim.PhysShape, bodyCallback);
+            bool newBody = CreateBody((newGeom || forceRebuild), prim, m_physicsScene.World, bodyCallback);
             ret = newGeom || newBody;
         }
         DetailLog("{0},BSShapeCollection.GetBodyAndShape,taintExit,force={1},ret={2},body={3},shape={4}",
@@ -128,279 +100,20 @@ public sealed class BSShapeCollection : IDisposable
 
     public bool GetBodyAndShape(bool forceRebuild, BulletWorld sim, BSPhysObject prim)
     {
-        return GetBodyAndShape(forceRebuild, sim, prim, null, null);
-    }
-
-    // Track another user of a body.
-    // We presume the caller has allocated the body.
-    // Bodies only have one user so the body is just put into the world if not already there.
-    public void ReferenceBody(BulletBody body, bool inTaintTime)
-    {
-        lock (m_collectionActivityLock)
-        {
-            if (DDetail) DetailLog("{0},BSShapeCollection.ReferenceBody,newBody,body={1}", body.ID, body);
-            PhysicsScene.TaintedObject(inTaintTime, "BSShapeCollection.ReferenceBody", delegate()
-            {
-                if (!PhysicsScene.PE.IsInWorld(PhysicsScene.World, body))
-                {
-                    PhysicsScene.PE.AddObjectToWorld(PhysicsScene.World, body);
-                    if (DDetail) DetailLog("{0},BSShapeCollection.ReferenceBody,addedToWorld,ref={1}", body.ID, body);
-                }
-            });
-        }
-    }
-
-    // Release the usage of a body.
-    // Called when releasing use of a BSBody. BSShape is handled separately.
-    public void DereferenceBody(BulletBody body, bool inTaintTime, BodyDestructionCallback bodyCallback )
-    {
-        if (!body.HasPhysicalBody)
-            return;
-
-        lock (m_collectionActivityLock)
-        {
-            PhysicsScene.TaintedObject(inTaintTime, "BSShapeCollection.DereferenceBody", delegate()
-            {
-                if (DDetail) DetailLog("{0},BSShapeCollection.DereferenceBody,DestroyingBody,body={1},inTaintTime={2}",
-                                            body.ID, body, inTaintTime);
-                // If the caller needs to know the old body is going away, pass the event up.
-                if (bodyCallback != null) bodyCallback(body);
-
-                if (PhysicsScene.PE.IsInWorld(PhysicsScene.World, body))
-                {
-                    PhysicsScene.PE.RemoveObjectFromWorld(PhysicsScene.World, body);
-                    if (DDetail) DetailLog("{0},BSShapeCollection.DereferenceBody,removingFromWorld. Body={1}", body.ID, body);
-                }
-
-                // Zero any reference to the shape so it is not freed when the body is deleted.
-                PhysicsScene.PE.SetCollisionShape(PhysicsScene.World, body, null);
-                PhysicsScene.PE.DestroyObject(PhysicsScene.World, body);
-            });
-        }
-    }
-
-    // Track the datastructures and use count for a shape.
-    // When creating a hull, this is called first to reference the mesh
-    //     and then again to reference the hull.
-    // Meshes and hulls for the same shape have the same hash key.
-    // NOTE that native shapes are not added to the mesh list or removed.
-    // Returns 'true' if this is the initial reference to the shape. Otherwise reused.
-    public bool ReferenceShape(BulletShape shape)
-    {
-        bool ret = false;
-        switch (shape.type)
-        {
-            case BSPhysicsShapeType.SHAPE_MESH:
-                MeshDesc meshDesc;
-                if (Meshes.TryGetValue(shape.shapeKey, out meshDesc))
-                {
-                    // There is an existing instance of this mesh.
-                    meshDesc.referenceCount++;
-                    if (DDetail) DetailLog("{0},BSShapeCollection.ReferenceShape,existingMesh,key={1},cnt={2}",
-                                BSScene.DetailLogZero, shape.shapeKey.ToString("X"), meshDesc.referenceCount);
-                }
-                else
-                {
-                    // This is a new reference to a mesh
-                    meshDesc.shape = shape.Clone();
-                    meshDesc.shapeKey = shape.shapeKey;
-                    // We keep a reference to the underlying IMesh data so a hull can be built
-                    meshDesc.referenceCount = 1;
-                    if (DDetail) DetailLog("{0},BSShapeCollection.ReferenceShape,newMesh,key={1},cnt={2}",
-                                BSScene.DetailLogZero, shape.shapeKey.ToString("X"), meshDesc.referenceCount);
-                    ret = true;
-                }
-                meshDesc.lastReferenced = System.DateTime.Now;
-                Meshes[shape.shapeKey] = meshDesc;
-                break;
-            case BSPhysicsShapeType.SHAPE_HULL:
-                HullDesc hullDesc;
-                if (Hulls.TryGetValue(shape.shapeKey, out hullDesc))
-                {
-                    // There is an existing instance of this hull.
-                    hullDesc.referenceCount++;
-                    if (DDetail) DetailLog("{0},BSShapeCollection.ReferenceShape,existingHull,key={1},cnt={2}",
-                                BSScene.DetailLogZero, shape.shapeKey.ToString("X"), hullDesc.referenceCount);
-                }
-                else
-                {
-                    // This is a new reference to a hull
-                    hullDesc.shape = shape.Clone();
-                    hullDesc.shapeKey = shape.shapeKey;
-                    hullDesc.referenceCount = 1;
-                    if (DDetail) DetailLog("{0},BSShapeCollection.ReferenceShape,newHull,key={1},cnt={2}",
-                                BSScene.DetailLogZero, shape.shapeKey.ToString("X"), hullDesc.referenceCount);
-                    ret = true;
-
-                }
-                hullDesc.lastReferenced = System.DateTime.Now;
-                Hulls[shape.shapeKey] = hullDesc;
-                break;
-            case BSPhysicsShapeType.SHAPE_UNKNOWN:
-                break;
-            default:
-                // Native shapes are not tracked and they don't go into any list
-                break;
-        }
-        return ret;
-    }
-
-    // Release the usage of a shape.
-    public void DereferenceShape(BulletShape shape, bool inTaintTime, ShapeDestructionCallback shapeCallback)
-    {
-        if (!shape.HasPhysicalShape)
-            return;
-
-        PhysicsScene.TaintedObject(inTaintTime, "BSShapeCollection.DereferenceShape", delegate()
-        {
-            if (shape.HasPhysicalShape)
-            {
-                if (shape.isNativeShape)
-                {
-                    // Native shapes are not tracked and are released immediately
-                    if (DDetail) DetailLog("{0},BSShapeCollection.DereferenceShape,deleteNativeShape,ptr={1},taintTime={2}",
-                                    BSScene.DetailLogZero, shape.AddrString, inTaintTime);
-                    if (shapeCallback != null) shapeCallback(shape);
-                    PhysicsScene.PE.DeleteCollisionShape(PhysicsScene.World, shape);
-                }
-                else
-                {
-                    switch (shape.type)
-                    {
-                        case BSPhysicsShapeType.SHAPE_HULL:
-                            DereferenceHull(shape, shapeCallback);
-                            break;
-                        case BSPhysicsShapeType.SHAPE_MESH:
-                            DereferenceMesh(shape, shapeCallback);
-                            break;
-                        case BSPhysicsShapeType.SHAPE_COMPOUND:
-                            DereferenceCompound(shape, shapeCallback);
-                            break;
-                        case BSPhysicsShapeType.SHAPE_UNKNOWN:
-                            break;
-                        default:
-                            break;
-                    }
-                }
-            }
-        });
-    }
-
-    // Count down the reference count for a mesh shape
-    // Called at taint-time.
-    private void DereferenceMesh(BulletShape shape, ShapeDestructionCallback shapeCallback)
-    {
-        MeshDesc meshDesc;
-        if (Meshes.TryGetValue(shape.shapeKey, out meshDesc))
-        {
-            meshDesc.referenceCount--;
-            // TODO: release the Bullet storage
-            if (shapeCallback != null) shapeCallback(shape);
-            meshDesc.lastReferenced = System.DateTime.Now;
-            Meshes[shape.shapeKey] = meshDesc;
-            if (DDetail) DetailLog("{0},BSShapeCollection.DereferenceMesh,shape={1},refCnt={2}",
-                                BSScene.DetailLogZero, shape, meshDesc.referenceCount);
-
-        }
-    }
-
-    // Count down the reference count for a hull shape
-    // Called at taint-time.
-    private void DereferenceHull(BulletShape shape, ShapeDestructionCallback shapeCallback)
-    {
-        HullDesc hullDesc;
-        if (Hulls.TryGetValue(shape.shapeKey, out hullDesc))
-        {
-            hullDesc.referenceCount--;
-            // TODO: release the Bullet storage (aging old entries?)
-
-            // Tell upper layers that, if they have dependencies on this shape, this link is going away
-            if (shapeCallback != null) shapeCallback(shape);
-
-            hullDesc.lastReferenced = System.DateTime.Now;
-            Hulls[shape.shapeKey] = hullDesc;
-            if (DDetail) DetailLog("{0},BSShapeCollection.DereferenceHull,shape={1},refCnt={2}",
-                    BSScene.DetailLogZero, shape, hullDesc.referenceCount);
-        }
-    }
-
-    // Remove a reference to a compound shape.
-    // Taking a compound shape apart is a little tricky because if you just delete the
-    //      physical shape, it will free all the underlying children. We can't do that because
-    //      they could be shared. So, this removes each of the children from the compound and
-    //      dereferences them separately before destroying the compound collision object itself.
-    // Called at taint-time.
-    private void DereferenceCompound(BulletShape shape, ShapeDestructionCallback shapeCallback)
-    {
-        if (!PhysicsScene.PE.IsCompound(shape))
-        {
-            // Failed the sanity check!!
-            PhysicsScene.Logger.ErrorFormat("{0} Attempt to free a compound shape that is not compound!! type={1}, ptr={2}",
-                                        LogHeader, shape.type, shape.AddrString);
-            if (DDetail) DetailLog("{0},BSShapeCollection.DereferenceCompound,notACompoundShape,type={1},ptr={2}",
-                                        BSScene.DetailLogZero, shape.type, shape.AddrString);
-            return;
-        }
-
-        int numChildren = PhysicsScene.PE.GetNumberOfCompoundChildren(shape);
-        if (DDetail) DetailLog("{0},BSShapeCollection.DereferenceCompound,shape={1},children={2}", BSScene.DetailLogZero, shape, numChildren);
-
-        for (int ii = numChildren - 1; ii >= 0; ii--)
-        {
-            BulletShape childShape = PhysicsScene.PE.RemoveChildShapeFromCompoundShapeIndex(shape, ii);
-            DereferenceAnonCollisionShape(childShape);
-        }
-        PhysicsScene.PE.DeleteCollisionShape(PhysicsScene.World, shape);
+        return GetBodyAndShape(forceRebuild, sim, prim, null);
     }
 
-    // Sometimes we have a pointer to a collision shape but don't know what type it is.
-    // Figure out type and call the correct dereference routine.
-    // Called at taint-time.
-    private void DereferenceAnonCollisionShape(BulletShape shapeInfo)
+    // If the existing prim's shape is to be replaced, remove the tie to the existing shape
+    //     before replacing it.
+    private void DereferenceExistingShape(BSPhysObject prim, PhysicalDestructionCallback shapeCallback)
     {
-        MeshDesc meshDesc;
-        HullDesc hullDesc;
-
-        if (TryGetMeshByPtr(shapeInfo, out meshDesc))
-        {
-            shapeInfo.type = BSPhysicsShapeType.SHAPE_MESH;
-            shapeInfo.shapeKey = meshDesc.shapeKey;
-        }
-        else
-        {
-            if (TryGetHullByPtr(shapeInfo, out hullDesc))
-            {
-                shapeInfo.type = BSPhysicsShapeType.SHAPE_HULL;
-                shapeInfo.shapeKey = hullDesc.shapeKey;
-            }
-            else
-            {
-                if (PhysicsScene.PE.IsCompound(shapeInfo))
-                {
-                    shapeInfo.type = BSPhysicsShapeType.SHAPE_COMPOUND;
-                }
-                else
-                {
-                    if (PhysicsScene.PE.IsNativeShape(shapeInfo))
-                    {
-                        shapeInfo.isNativeShape = true;
-                        shapeInfo.type = BSPhysicsShapeType.SHAPE_BOX; // (technically, type doesn't matter)
-                    }
-                }
-            }
-        }
-
-        if (DDetail) DetailLog("{0},BSShapeCollection.DereferenceAnonCollisionShape,shape={1}", BSScene.DetailLogZero, shapeInfo);
-
-        if (shapeInfo.type != BSPhysicsShapeType.SHAPE_UNKNOWN)
+        if (prim.PhysShape.HasPhysicalShape)
         {
-            DereferenceShape(shapeInfo, true, null);
-        }
-        else
-        {
-            PhysicsScene.Logger.ErrorFormat("{0} Could not decypher shape type. Region={1}, addr={2}",
-                                    LogHeader, PhysicsScene.RegionName, shapeInfo.AddrString);
+            if (shapeCallback != null)
+                shapeCallback(prim.PhysBody, prim.PhysShape.physShapeInfo);
+            prim.PhysShape.Dereference(m_physicsScene);
         }
+        prim.PhysShape = new BSShapeNull();
     }
 
     // Create the geometry information in Bullet for later use.
@@ -411,95 +124,75 @@ public sealed class BSShapeCollection : IDisposable
     // Info in prim.BSShape is updated to the new shape.
     // Returns 'true' if the geometry was rebuilt.
     // Called at taint-time!
-    private bool CreateGeom(bool forceRebuild, BSPhysObject prim, ShapeDestructionCallback shapeCallback)
+    private bool CreateGeom(bool forceRebuild, BSPhysObject prim, PhysicalDestructionCallback shapeCallback)
     {
         bool ret = false;
         bool haveShape = false;
+        bool nativeShapePossible = true;
+        PrimitiveBaseShape pbs = prim.BaseShape;
 
-        if (!haveShape && prim.PreferredPhysicalShape == BSPhysicsShapeType.SHAPE_CAPSULE)
+        // Kludge to create the capsule for the avatar.
+        // TDOD: Remove/redo this when BSShapeAvatar is working!!
+        BSCharacter theChar = prim as BSCharacter;
+        if (theChar != null)
         {
-            // an avatar capsule is close to a native shape (it is not shared)
-            GetReferenceToNativeShape(prim, BSPhysicsShapeType.SHAPE_CAPSULE, FixedShapeKey.KEY_CAPSULE, shapeCallback);
-            if (DDetail) DetailLog("{0},BSShapeCollection.CreateGeom,avatarCapsule,shape={1}", prim.LocalID, prim.PhysShape);
+            DereferenceExistingShape(prim, shapeCallback);
+            prim.PhysShape = BSShapeNative.GetReference(m_physicsScene, prim,
+                                            BSPhysicsShapeType.SHAPE_CAPSULE, FixedShapeKey.KEY_CAPSULE);
             ret = true;
             haveShape = true;
         }
 
-        // Compound shapes are handled special as they are rebuilt from scratch.
-        // This isn't too great a hardship since most of the child shapes will have already been created.
-        if (!haveShape && prim.PreferredPhysicalShape == BSPhysicsShapeType.SHAPE_COMPOUND)
-        {
-            ret = GetReferenceToCompoundShape(prim, shapeCallback);
-            if (DDetail) DetailLog("{0},BSShapeCollection.CreateGeom,compoundShape,shape={1}", prim.LocalID, prim.PhysShape);
-            haveShape = true;
-        }
-
-        if (!haveShape)
-        {
-            ret = CreateGeomNonSpecial(forceRebuild, prim, shapeCallback);
-        }
-
-        return ret;
-    }
-
-    // Create a mesh/hull shape or a native shape if 'nativeShapePossible' is 'true'.
-    public bool CreateGeomNonSpecial(bool forceRebuild, BSPhysObject prim, ShapeDestructionCallback shapeCallback)
-    {
-        bool ret = false;
-        bool haveShape = false;
-        bool nativeShapePossible = true;
-        PrimitiveBaseShape pbs = prim.BaseShape;
-
         // If the prim attributes are simple, this could be a simple Bullet native shape
+        // Native shapes work whether to object is static or physical.
         if (!haveShape
-                && pbs != null
                 && nativeShapePossible
-                && ((pbs.SculptEntry && !BSParam.ShouldMeshSculptedPrim)
-                    || (pbs.ProfileBegin == 0 && pbs.ProfileEnd == 0
-                        && pbs.ProfileHollow == 0
-                        && pbs.PathTwist == 0 && pbs.PathTwistBegin == 0
-                        && pbs.PathBegin == 0 && pbs.PathEnd == 0
-                        && pbs.PathTaperX == 0 && pbs.PathTaperY == 0
-                        && pbs.PathScaleX == 100 && pbs.PathScaleY == 100
-                        && pbs.PathShearX == 0 && pbs.PathShearY == 0) ) )
+                && pbs != null
+                && PrimHasNoCuts(pbs)
+                && ( !pbs.SculptEntry || (pbs.SculptEntry && !BSParam.ShouldMeshSculptedPrim) )
+            )
         {
             // Get the scale of any existing shape so we can see if the new shape is same native type and same size.
             OMV.Vector3 scaleOfExistingShape = OMV.Vector3.Zero;
             if (prim.PhysShape.HasPhysicalShape)
-                scaleOfExistingShape = PhysicsScene.PE.GetLocalScaling(prim.PhysShape);
+                scaleOfExistingShape = m_physicsScene.PE.GetLocalScaling(prim.PhysShape.physShapeInfo);
 
             if (DDetail) DetailLog("{0},BSShapeCollection.CreateGeom,maybeNative,force={1},primScale={2},primSize={3},primShape={4}",
-                        prim.LocalID, forceRebuild, prim.Scale, prim.Size, prim.PhysShape.type);
+                        prim.LocalID, forceRebuild, prim.Scale, prim.Size, prim.PhysShape.physShapeInfo.shapeType);
 
-            // It doesn't look like Bullet scales spheres so make sure the scales are all equal
+            // It doesn't look like Bullet scales native spheres so make sure the scales are all equal
             if ((pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1)
                                 && pbs.Scale.X == pbs.Scale.Y && pbs.Scale.Y == pbs.Scale.Z)
             {
                 haveShape = true;
                 if (forceRebuild
-                        || prim.Scale != scaleOfExistingShape
-                        || prim.PhysShape.type != BSPhysicsShapeType.SHAPE_SPHERE
-                        )
+                        || prim.PhysShape.ShapeType != BSPhysicsShapeType.SHAPE_SPHERE
+                    )
                 {
-                    ret = GetReferenceToNativeShape(prim, BSPhysicsShapeType.SHAPE_SPHERE,
-                                            FixedShapeKey.KEY_SPHERE, shapeCallback);
-                    if (DDetail) DetailLog("{0},BSShapeCollection.CreateGeom,sphere,force={1},shape={2}",
-                                        prim.LocalID, forceRebuild, prim.PhysShape);
+                    DereferenceExistingShape(prim, shapeCallback);
+                    prim.PhysShape = BSShapeNative.GetReference(m_physicsScene, prim,
+                                            BSPhysicsShapeType.SHAPE_SPHERE, FixedShapeKey.KEY_SPHERE);
+                    ret = true;
                 }
+                if (DDetail) DetailLog("{0},BSShapeCollection.CreateGeom,sphere,force={1},rebuilt={2},shape={3}",
+                                        prim.LocalID, forceRebuild, ret, prim.PhysShape);
             }
+            // If we didn't make a sphere, maybe a box will work.
             if (!haveShape && pbs.ProfileShape == ProfileShape.Square && pbs.PathCurve == (byte)Extrusion.Straight)
             {
                 haveShape = true;
                 if (forceRebuild
                         || prim.Scale != scaleOfExistingShape
-                        || prim.PhysShape.type != BSPhysicsShapeType.SHAPE_BOX
+                        || prim.PhysShape.ShapeType != BSPhysicsShapeType.SHAPE_BOX
                         )
                 {
-                    ret = GetReferenceToNativeShape( prim, BSPhysicsShapeType.SHAPE_BOX,
-                                            FixedShapeKey.KEY_BOX, shapeCallback);
-                    if (DDetail) DetailLog("{0},BSShapeCollection.CreateGeom,box,force={1},shape={2}",
-                                        prim.LocalID, forceRebuild, prim.PhysShape);
+                    DereferenceExistingShape(prim, shapeCallback);
+                    prim.PhysShape = BSShapeNative.GetReference(m_physicsScene, prim,
+                                            BSPhysicsShapeType.SHAPE_BOX, FixedShapeKey.KEY_BOX);
+                    ret = true;
                 }
+                if (DDetail) DetailLog("{0},BSShapeCollection.CreateGeom,box,force={1},rebuilt={2},shape={3}",
+                                        prim.LocalID, forceRebuild, ret, prim.PhysShape);
             }
         }
 
@@ -512,7 +205,20 @@ public sealed class BSShapeCollection : IDisposable
         return ret;
     }
 
-    public bool CreateGeomMeshOrHull(BSPhysObject prim, ShapeDestructionCallback shapeCallback)
+    // return 'true' if this shape description does not include any cutting or twisting.
+    public static bool PrimHasNoCuts(PrimitiveBaseShape pbs)
+    {
+        return pbs.ProfileBegin == 0 && pbs.ProfileEnd == 0
+            && pbs.ProfileHollow == 0
+            && pbs.PathTwist == 0 && pbs.PathTwistBegin == 0
+            && pbs.PathBegin == 0 && pbs.PathEnd == 0
+            && pbs.PathTaperX == 0 && pbs.PathTaperY == 0
+            && pbs.PathScaleX == 100 && pbs.PathScaleY == 100
+            && pbs.PathShearX == 0 && pbs.PathShearY == 0;
+    }
+
+    // return 'true' if the prim's shape was changed.
+    private bool CreateGeomMeshOrHull(BSPhysObject prim, PhysicalDestructionCallback shapeCallback)
     {
 
         bool ret = false;
@@ -520,404 +226,110 @@ public sealed class BSShapeCollection : IDisposable
         //     made. Native shapes work in either case.
         if (prim.IsPhysical && BSParam.ShouldUseHullsForPhysicalObjects)
         {
-            // Update prim.BSShape to reference a hull of this shape.
-            ret = GetReferenceToHull(prim,shapeCallback);
-            if (DDetail) DetailLog("{0},BSShapeCollection.CreateGeom,hull,shape={1},key={2}",
-                                    prim.LocalID, prim.PhysShape, prim.PhysShape.shapeKey.ToString("X"));
-        }
-        else
-        {
-            ret = GetReferenceToMesh(prim, shapeCallback);
-            if (DDetail) DetailLog("{0},BSShapeCollection.CreateGeom,mesh,shape={1},key={2}",
-                                    prim.LocalID, prim.PhysShape, prim.PhysShape.shapeKey.ToString("X"));
-        }
-        return ret;
-    }
-
-    // Creates a native shape and assignes it to prim.BSShape.
-    // "Native" shapes are never shared. they are created here and destroyed in DereferenceShape().
-    private bool GetReferenceToNativeShape(BSPhysObject prim,
-                            BSPhysicsShapeType shapeType, FixedShapeKey shapeKey,
-                            ShapeDestructionCallback shapeCallback)
-    {
-        // release any previous shape
-        DereferenceShape(prim.PhysShape, true, shapeCallback);
-
-        BulletShape newShape = BuildPhysicalNativeShape(prim, shapeType, shapeKey);
-
-        // Don't need to do a 'ReferenceShape()' here because native shapes are not shared.
-        if (DDetail) DetailLog("{0},BSShapeCollection.AddNativeShapeToPrim,create,newshape={1},scale={2}",
-                                prim.LocalID, newShape, prim.Scale);
-
-        // native shapes are scaled by Bullet
-        prim.PhysShape = newShape;
-        return true;
-    }
-
-    private BulletShape BuildPhysicalNativeShape(BSPhysObject prim, BSPhysicsShapeType shapeType,
-                                    FixedShapeKey shapeKey)
-    {
-        BulletShape newShape;
-        // Need to make sure the passed shape information is for the native type.
-        ShapeData nativeShapeData = new ShapeData();
-        nativeShapeData.Type = shapeType;
-        nativeShapeData.ID = prim.LocalID;
-        nativeShapeData.Scale = prim.Scale;
-        nativeShapeData.Size = prim.Scale;  // unneeded, I think.
-        nativeShapeData.MeshKey = (ulong)shapeKey;
-        nativeShapeData.HullKey = (ulong)shapeKey;
-
-        if (shapeType == BSPhysicsShapeType.SHAPE_CAPSULE)
-        {
-
-            newShape = PhysicsScene.PE.BuildCapsuleShape(PhysicsScene.World, 1f, 1f, prim.Scale);
-            if (DDetail) DetailLog("{0},BSShapeCollection.BuiletPhysicalNativeShape,capsule,scale={1}", prim.LocalID, prim.Scale);
-        }
-        else
-        {
-            // Native shapes are scaled in Bullet so set the scaling to the size
-            newShape = PhysicsScene.PE.BuildNativeShape(PhysicsScene.World, nativeShapeData);
-
-        }
-        if (!newShape.HasPhysicalShape)
-        {
-            PhysicsScene.Logger.ErrorFormat("{0} BuildPhysicalNativeShape failed. ID={1}, shape={2}",
-                                    LogHeader, prim.LocalID, shapeType);
-        }
-        newShape.shapeKey = (System.UInt64)shapeKey;
-        newShape.isNativeShape = true;
-
-        return newShape;
-    }
-
-    // Builds a mesh shape in the physical world and updates prim.BSShape.
-    // Dereferences previous shape in BSShape and adds a reference for this new shape.
-    // Returns 'true' of a mesh was actually built. Otherwise .
-    // Called at taint-time!
-    private bool GetReferenceToMesh(BSPhysObject prim, ShapeDestructionCallback shapeCallback)
-    {
-        BulletShape newShape = new BulletShape();
-
-        float lod;
-        System.UInt64 newMeshKey = ComputeShapeKey(prim.Size, prim.BaseShape, out lod);
-
-        // if this new shape is the same as last time, don't recreate the mesh
-        if (newMeshKey == prim.PhysShape.shapeKey && prim.PhysShape.type == BSPhysicsShapeType.SHAPE_MESH)
-            return false;
+            // Use a simple, single mesh convex hull shape if the object is simple enough
+            BSShape potentialHull = null;
 
-        if (DDetail) DetailLog("{0},BSShapeCollection.GetReferenceToMesh,create,oldKey={1},newKey={2}",
-                                prim.LocalID, prim.PhysShape.shapeKey.ToString("X"), newMeshKey.ToString("X"));
-
-        // Since we're recreating new, get rid of the reference to the previous shape
-        DereferenceShape(prim.PhysShape, true, shapeCallback);
-
-        newShape = CreatePhysicalMesh(prim.PhysObjectName, newMeshKey, prim.BaseShape, prim.Size, lod);
-        // Take evasive action if the mesh was not constructed.
-        newShape = VerifyMeshCreated(newShape, prim);
-
-        ReferenceShape(newShape);
-
-        prim.PhysShape = newShape;
-
-        return true;        // 'true' means a new shape has been added to this prim
-    }
-
-    private BulletShape CreatePhysicalMesh(string objName, System.UInt64 newMeshKey, PrimitiveBaseShape pbs, OMV.Vector3 size, float lod)
-    {
-        BulletShape newShape = new BulletShape();
-        IMesh meshData = null;
+            PrimitiveBaseShape pbs = prim.BaseShape;
+            if (BSParam.ShouldUseSingleConvexHullForPrims
+                && pbs != null
+                && !pbs.SculptEntry
+                && PrimHasNoCuts(pbs)
+                )
+            {
+                potentialHull = BSShapeConvexHull.GetReference(m_physicsScene, false /* forceRebuild */, prim);
+            }
+            else
+            {
+                potentialHull = BSShapeHull.GetReference(m_physicsScene, false /*forceRebuild*/, prim);
+            }
 
-        MeshDesc meshDesc;
-        if (Meshes.TryGetValue(newMeshKey, out meshDesc))
-        {
-            // If the mesh has already been built just use it.
-            newShape = meshDesc.shape.Clone();
+            // If the current shape is not what is on the prim at the moment, time to change.
+            if (!prim.PhysShape.HasPhysicalShape
+                        || potentialHull.ShapeType != prim.PhysShape.ShapeType
+                        || potentialHull.physShapeInfo.shapeKey != prim.PhysShape.physShapeInfo.shapeKey)
+            {
+                DereferenceExistingShape(prim, shapeCallback);
+                prim.PhysShape = potentialHull;
+                ret = true;
+            }
+            else
+            {
+                // The current shape on the prim is the correct one. We don't need the potential reference.
+                potentialHull.Dereference(m_physicsScene);
+            }
+            if (DDetail) DetailLog("{0},BSShapeCollection.CreateGeom,hull,shape={1}", prim.LocalID, prim.PhysShape);
         }
         else
         {
-            meshData = PhysicsScene.mesher.CreateMesh(objName, pbs, size, lod, true, false);
-
-            if (meshData != null)
+            // Update prim.BSShape to reference a mesh of this shape.
+            BSShape potentialMesh = BSShapeMesh.GetReference(m_physicsScene, false /*forceRebuild*/, prim);
+            // If the current shape is not what is on the prim at the moment, time to change.
+            if (!prim.PhysShape.HasPhysicalShape
+                        || potentialMesh.ShapeType != prim.PhysShape.ShapeType
+                        || potentialMesh.physShapeInfo.shapeKey != prim.PhysShape.physShapeInfo.shapeKey)
             {
-                int[] indices = meshData.getIndexListAsInt();
-                List<OMV.Vector3> vertices = meshData.getVertexList();
-
-                float[] verticesAsFloats = new float[vertices.Count * 3];
-                int vi = 0;
-                foreach (OMV.Vector3 vv in vertices)
-                {
-                    verticesAsFloats[vi++] = vv.X;
-                    verticesAsFloats[vi++] = vv.Y;
-                    verticesAsFloats[vi++] = vv.Z;
-                }
-
-                // m_log.DebugFormat("{0}: BSShapeCollection.CreatePhysicalMesh: calling CreateMesh. lid={1}, key={2}, indices={3}, vertices={4}",
-                //                  LogHeader, prim.LocalID, newMeshKey, indices.Length, vertices.Count);
-
-                newShape = PhysicsScene.PE.CreateMeshShape(PhysicsScene.World,
-                                    indices.GetLength(0), indices, vertices.Count, verticesAsFloats);
+                DereferenceExistingShape(prim, shapeCallback);
+                prim.PhysShape = potentialMesh;
+                ret = true;
             }
+            else
+            {
+                // We don't need this reference to the mesh that is already being using.
+                potentialMesh.Dereference(m_physicsScene);
+            }
+            if (DDetail) DetailLog("{0},BSShapeCollection.CreateGeom,mesh,shape={1}", prim.LocalID, prim.PhysShape);
         }
-        newShape.shapeKey = newMeshKey;
-
-        return newShape;
-    }
-
-    // See that hull shape exists in the physical world and update prim.BSShape.
-    // We could be creating the hull because scale changed or whatever.
-    private bool GetReferenceToHull(BSPhysObject prim, ShapeDestructionCallback shapeCallback)
-    {
-        BulletShape newShape;
-
-        float lod;
-        System.UInt64 newHullKey = ComputeShapeKey(prim.Size, prim.BaseShape, out lod);
-
-        // if the hull hasn't changed, don't rebuild it
-        if (newHullKey == prim.PhysShape.shapeKey && prim.PhysShape.type == BSPhysicsShapeType.SHAPE_HULL)
-            return false;
-
-        if (DDetail) DetailLog("{0},BSShapeCollection.GetReferenceToHull,create,oldKey={1},newKey={2}",
-                        prim.LocalID, prim.PhysShape.shapeKey.ToString("X"), newHullKey.ToString("X"));
-
-        // Remove usage of the previous shape.
-        DereferenceShape(prim.PhysShape, true, shapeCallback);
-
-        newShape = CreatePhysicalHull(prim.PhysObjectName, newHullKey, prim.BaseShape, prim.Size, lod);
-        newShape = VerifyMeshCreated(newShape, prim);
-
-        ReferenceShape(newShape);
-
-        prim.PhysShape = newShape;
-        return true;        // 'true' means a new shape has been added to this prim
+        return ret;
     }
 
-    List<ConvexResult> m_hulls;
-    private BulletShape CreatePhysicalHull(string objName, System.UInt64 newHullKey, PrimitiveBaseShape pbs, OMV.Vector3 size, float lod)
+    // Track another user of a body.
+    // We presume the caller has allocated the body.
+    // Bodies only have one user so the body is just put into the world if not already there.
+    private void ReferenceBody(BulletBody body)
     {
-
-        BulletShape newShape = new BulletShape();
-        IntPtr hullPtr = IntPtr.Zero;
-
-        HullDesc hullDesc;
-        if (Hulls.TryGetValue(newHullKey, out hullDesc))
-        {
-            // If the hull shape already is created, just use it.
-            newShape = hullDesc.shape.Clone();
-        }
-        else
+        lock (m_collectionActivityLock)
         {
-            // Build a new hull in the physical world
-            // Pass true for physicalness as this creates some sort of bounding box which we don't need
-            IMesh meshData = PhysicsScene.mesher.CreateMesh(objName, pbs, size, lod, true, false);
-            if (meshData != null)
+            if (DDetail) DetailLog("{0},BSShapeCollection.ReferenceBody,newBody,body={1}", body.ID, body);
+            if (!m_physicsScene.PE.IsInWorld(m_physicsScene.World, body))
             {
-
-                int[] indices = meshData.getIndexListAsInt();
-                List<OMV.Vector3> vertices = meshData.getVertexList();
-
-                //format conversion from IMesh format to DecompDesc format
-                List<int> convIndices = new List<int>();
-                List<float3> convVertices = new List<float3>();
-                for (int ii = 0; ii < indices.GetLength(0); ii++)
-                {
-                    convIndices.Add(indices[ii]);
-                }
-                foreach (OMV.Vector3 vv in vertices)
-                {
-                    convVertices.Add(new float3(vv.X, vv.Y, vv.Z));
-                }
-
-                // setup and do convex hull conversion
-                m_hulls = new List<ConvexResult>();
-                DecompDesc dcomp = new DecompDesc();
-                dcomp.mIndices = convIndices;
-                dcomp.mVertices = convVertices;
-                ConvexBuilder convexBuilder = new ConvexBuilder(HullReturn);
-                // create the hull into the _hulls variable
-                convexBuilder.process(dcomp);
-
-                // Convert the vertices and indices for passing to unmanaged.
-                // The hull information is passed as a large floating point array.
-                // The format is:
-                //  convHulls[0] = number of hulls
-                //  convHulls[1] = number of vertices in first hull
-                //  convHulls[2] = hull centroid X coordinate
-                //  convHulls[3] = hull centroid Y coordinate
-                //  convHulls[4] = hull centroid Z coordinate
-                //  convHulls[5] = first hull vertex X
-                //  convHulls[6] = first hull vertex Y
-                //  convHulls[7] = first hull vertex Z
-                //  convHulls[8] = second hull vertex X
-                //  ...
-                //  convHulls[n] = number of vertices in second hull
-                //  convHulls[n+1] = second hull centroid X coordinate
-                //  ...
-                //
-                // TODO: is is very inefficient. Someday change the convex hull generator to return
-                //   data structures that do not need to be converted in order to pass to Bullet.
-                //   And maybe put the values directly into pinned memory rather than marshaling.
-                int hullCount = m_hulls.Count;
-                int totalVertices = 1;          // include one for the count of the hulls
-                foreach (ConvexResult cr in m_hulls)
-                {
-                    totalVertices += 4;                         // add four for the vertex count and centroid
-                    totalVertices += cr.HullIndices.Count * 3;  // we pass just triangles
-                }
-                float[] convHulls = new float[totalVertices];
-
-                convHulls[0] = (float)hullCount;
-                int jj = 1;
-                foreach (ConvexResult cr in m_hulls)
-                {
-                    // copy vertices for index access
-                    float3[] verts = new float3[cr.HullVertices.Count];
-                    int kk = 0;
-                    foreach (float3 ff in cr.HullVertices)
-                    {
-                        verts[kk++] = ff;
-                    }
-
-                    // add to the array one hull's worth of data
-                    convHulls[jj++] = cr.HullIndices.Count;
-                    convHulls[jj++] = 0f;   // centroid x,y,z
-                    convHulls[jj++] = 0f;
-                    convHulls[jj++] = 0f;
-                    foreach (int ind in cr.HullIndices)
-                    {
-                        convHulls[jj++] = verts[ind].x;
-                        convHulls[jj++] = verts[ind].y;
-                        convHulls[jj++] = verts[ind].z;
-                    }
-                }
-                // create the hull data structure in Bullet
-                newShape = PhysicsScene.PE.CreateHullShape(PhysicsScene.World, hullCount, convHulls);
+                m_physicsScene.PE.AddObjectToWorld(m_physicsScene.World, body);
+                if (DDetail) DetailLog("{0},BSShapeCollection.ReferenceBody,addedToWorld,ref={1}", body.ID, body);
             }
         }
-
-        newShape.shapeKey = newHullKey;
-
-        return newShape;
     }
 
-    // Callback from convex hull creater with a newly created hull.
-    // Just add it to our collection of hulls for this shape.
-    private void HullReturn(ConvexResult result)
-    {
-        m_hulls.Add(result);
-        return;
-    }
-
-    // Compound shapes are always built from scratch.
-    // This shouldn't be to bad since most of the parts will be meshes that had been built previously.
-    private bool GetReferenceToCompoundShape(BSPhysObject prim, ShapeDestructionCallback shapeCallback)
+    // Release the usage of a body.
+    // Called when releasing use of a BSBody. BSShape is handled separately.
+    // Called in taint time.
+    public void DereferenceBody(BulletBody body, PhysicalDestructionCallback bodyCallback )
     {
-        // Remove reference to the old shape
-        // Don't need to do this as the shape is freed when the new root shape is created below.
-        // DereferenceShape(prim.PhysShape, true, shapeCallback);
+        if (!body.HasPhysicalBody)
+            return;
 
+        m_physicsScene.AssertInTaintTime("BSShapeCollection.DereferenceBody");
 
-        BulletShape cShape = PhysicsScene.PE.CreateCompoundShape(PhysicsScene.World, false);
+        lock (m_collectionActivityLock)
+        {
+            if (DDetail) DetailLog("{0},BSShapeCollection.DereferenceBody,DestroyingBody,body={1}", body.ID, body);
+            // If the caller needs to know the old body is going away, pass the event up.
+            if (bodyCallback != null)
+                bodyCallback(body, null);
 
-        // Create the shape for the root prim and add it to the compound shape. Cannot be a native shape.
-        CreateGeomMeshOrHull(prim, shapeCallback);
-        PhysicsScene.PE.AddChildShapeToCompoundShape(cShape, prim.PhysShape, OMV.Vector3.Zero, OMV.Quaternion.Identity);
-        if (DDetail) DetailLog("{0},BSShapeCollection.GetReferenceToCompoundShape,addRootPrim,compShape={1},rootShape={2}",
-                                    prim.LocalID, cShape, prim.PhysShape);
+            // Removing an object not in the world is a NOOP
+            m_physicsScene.PE.RemoveObjectFromWorld(m_physicsScene.World, body);
 
-        prim.PhysShape = cShape;
+            // Zero any reference to the shape so it is not freed when the body is deleted.
+            m_physicsScene.PE.SetCollisionShape(m_physicsScene.World, body, null);
 
-        return true;
-    }
-
-    // Create a hash of all the shape parameters to be used as a key
-    //    for this particular shape.
-    private System.UInt64 ComputeShapeKey(OMV.Vector3 size, PrimitiveBaseShape pbs, out float retLod)
-    {
-        // level of detail based on size and type of the object
-        float lod = BSParam.MeshLOD;
-        if (pbs.SculptEntry)
-            lod = BSParam.SculptLOD;
-
-        // Mega prims usually get more detail because one can interact with shape approximations at this size.
-        float maxAxis = Math.Max(size.X, Math.Max(size.Y, size.Z));
-        if (maxAxis > BSParam.MeshMegaPrimThreshold)
-            lod = BSParam.MeshMegaPrimLOD;
-
-        retLod = lod;
-        return pbs.GetMeshKey(size, lod);
-    }
-    // For those who don't want the LOD
-    private System.UInt64 ComputeShapeKey(OMV.Vector3 size, PrimitiveBaseShape pbs)
-    {
-        float lod;
-        return ComputeShapeKey(size, pbs, out lod);
-    }
-
-    // The creation of a mesh or hull can fail if an underlying asset is not available.
-    // There are two cases: 1) the asset is not in the cache and it needs to be fetched;
-    //     and 2) the asset cannot be converted (like failed decompression of JPEG2000s).
-    //     The first case causes the asset to be fetched. The second case requires
-    //     us to not loop forever.
-    // Called after creating a physical mesh or hull. If the physical shape was created,
-    //     just return.
-    private BulletShape VerifyMeshCreated(BulletShape newShape, BSPhysObject prim)
-    {
-        // If the shape was successfully created, nothing more to do
-        if (newShape.HasPhysicalShape)
-            return newShape;
-
-        // If this mesh has an underlying asset and we have not failed getting it before, fetch the asset
-        if (prim.BaseShape.SculptEntry && !prim.LastAssetBuildFailed && prim.BaseShape.SculptTexture != OMV.UUID.Zero)
-        {
-            prim.LastAssetBuildFailed = true;
-            BSPhysObject xprim = prim;
-            DetailLog("{0},BSShapeCollection.VerifyMeshCreated,fetchAsset,lID={1},lastFailed={2}",
-                            LogHeader, prim.LocalID, prim.LastAssetBuildFailed);
-            Util.FireAndForget(delegate
-                {
-                    RequestAssetDelegate assetProvider = PhysicsScene.RequestAssetMethod;
-                    if (assetProvider != null)
-                    {
-                        BSPhysObject yprim = xprim; // probably not necessary, but, just in case.
-                        assetProvider(yprim.BaseShape.SculptTexture, delegate(AssetBase asset)
-                        {
-                            if (!yprim.BaseShape.SculptEntry)
-                                return;
-                            if (yprim.BaseShape.SculptTexture.ToString() != asset.ID)
-                                return;
-
-                            yprim.BaseShape.SculptData = asset.Data;
-                            // This will cause the prim to see that the filler shape is not the right
-                            //    one and try again to build the object.
-                            // No race condition with the normal shape setting since the rebuild is at taint time.
-                            yprim.ForceBodyShapeRebuild(false);
-
-                        });
-                    }
-                });
-        }
-        else
-        {
-            if (prim.LastAssetBuildFailed)
-            {
-                PhysicsScene.Logger.ErrorFormat("{0} Mesh failed to fetch asset. lID={1}, texture={2}",
-                                            LogHeader, prim.LocalID, prim.BaseShape.SculptTexture);
-            }
+            m_physicsScene.PE.DestroyObject(m_physicsScene.World, body);
         }
-
-        // While we figure out the real problem, stick a simple native shape on the object.
-        BulletShape fillinShape =
-            BuildPhysicalNativeShape(prim, BSPhysicsShapeType.SHAPE_BOX, FixedShapeKey.KEY_BOX);
-
-        return fillinShape;
     }
 
     // Create a body object in Bullet.
     // Updates prim.BSBody with the information about the new body if one is created.
     // Returns 'true' if an object was actually created.
     // Called at taint-time.
-    private bool CreateBody(bool forceRebuild, BSPhysObject prim, BulletWorld sim, BulletShape shape,
-                            BodyDestructionCallback bodyCallback)
+    private bool CreateBody(bool forceRebuild, BSPhysObject prim, BulletWorld sim, PhysicalDestructionCallback bodyCallback)
     {
         bool ret = false;
 
@@ -928,33 +340,34 @@ public sealed class BSShapeCollection : IDisposable
         // If not a solid object, body is a GhostObject. Otherwise a RigidBody.
         if (!mustRebuild)
         {
-            CollisionObjectTypes bodyType = (CollisionObjectTypes)PhysicsScene.PE.GetBodyType(prim.PhysBody);
+            CollisionObjectTypes bodyType = (CollisionObjectTypes)m_physicsScene.PE.GetBodyType(prim.PhysBody);
             if (prim.IsSolid && bodyType != CollisionObjectTypes.CO_RIGID_BODY
                 || !prim.IsSolid && bodyType != CollisionObjectTypes.CO_GHOST_OBJECT)
             {
                 // If the collisionObject is not the correct type for solidness, rebuild what's there
                 mustRebuild = true;
+                if (DDetail) DetailLog("{0},BSShapeCollection.CreateBody,forceRebuildBecauseChangingBodyType,bodyType={1}", prim.LocalID, bodyType);
             }
         }
 
         if (mustRebuild || forceRebuild)
         {
             // Free any old body
-            DereferenceBody(prim.PhysBody, true, bodyCallback);
+            DereferenceBody(prim.PhysBody, bodyCallback);
 
             BulletBody aBody;
             if (prim.IsSolid)
             {
-                aBody = PhysicsScene.PE.CreateBodyFromShape(sim, shape, prim.LocalID, prim.RawPosition, prim.RawOrientation);
-                if (DDetail) DetailLog("{0},BSShapeCollection.CreateBody,mesh,body={1}", prim.LocalID, aBody);
+                aBody = m_physicsScene.PE.CreateBodyFromShape(sim, prim.PhysShape.physShapeInfo, prim.LocalID, prim.RawPosition, prim.RawOrientation);
+                if (DDetail) DetailLog("{0},BSShapeCollection.CreateBody,rigid,body={1}", prim.LocalID, aBody);
             }
             else
             {
-                aBody = PhysicsScene.PE.CreateGhostFromShape(sim, shape, prim.LocalID, prim.RawPosition, prim.RawOrientation);
+                aBody = m_physicsScene.PE.CreateGhostFromShape(sim, prim.PhysShape.physShapeInfo, prim.LocalID, prim.RawPosition, prim.RawOrientation);
                 if (DDetail) DetailLog("{0},BSShapeCollection.CreateBody,ghost,body={1}", prim.LocalID, aBody);
             }
 
-            ReferenceBody(aBody, true);
+            ReferenceBody(aBody);
 
             prim.PhysBody = aBody;
 
@@ -964,46 +377,10 @@ public sealed class BSShapeCollection : IDisposable
         return ret;
     }
 
-    private bool TryGetMeshByPtr(BulletShape shape, out MeshDesc outDesc)
-    {
-        bool ret = false;
-        MeshDesc foundDesc = new MeshDesc();
-        foreach (MeshDesc md in Meshes.Values)
-        {
-            if (md.shape.ReferenceSame(shape))
-            {
-                foundDesc = md;
-                ret = true;
-                break;
-            }
-
-        }
-        outDesc = foundDesc;
-        return ret;
-    }
-
-    private bool TryGetHullByPtr(BulletShape shape, out HullDesc outDesc)
-    {
-        bool ret = false;
-        HullDesc foundDesc = new HullDesc();
-        foreach (HullDesc hd in Hulls.Values)
-        {
-            if (hd.shape.ReferenceSame(shape))
-            {
-                foundDesc = hd;
-                ret = true;
-                break;
-            }
-
-        }
-        outDesc = foundDesc;
-        return ret;
-    }
-
     private void DetailLog(string msg, params Object[] args)
     {
-        if (PhysicsScene.PhysicsLogging.Enabled)
-            PhysicsScene.DetailLog(msg, args);
+        if (m_physicsScene.PhysicsLogging.Enabled)
+            m_physicsScene.DetailLog(msg, args);
     }
 }
 }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSShapes.cs b/OpenSim/Region/Physics/BulletSPlugin/BSShapes.cs
index c75eb9b..9d47657 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSShapes.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSShapes.cs
@@ -27,118 +27,287 @@
 
 using System;
 using System.Collections.Generic;
-using System.Linq;
 using System.Text;
 
+using OpenSim.Framework;
+using OpenSim.Region.Physics.Manager;
+using OpenSim.Region.Physics.ConvexDecompositionDotNet;
+
+using OMV = OpenMetaverse;
+
 namespace OpenSim.Region.Physics.BulletSPlugin
 {
 public abstract class BSShape
 {
-    public IntPtr ptr { get; set; }
-    public BSPhysicsShapeType type { get; set; }
-    public System.UInt64 key { get; set; }
+    private static string LogHeader = "[BULLETSIM SHAPE]";
+
     public int referenceCount { get; set; }
     public DateTime lastReferenced { get; set; }
+    public BulletShape physShapeInfo { get; set; }
 
     public BSShape()
     {
-        ptr = IntPtr.Zero;
-        type = BSPhysicsShapeType.SHAPE_UNKNOWN;
-        key = 0;
-        referenceCount = 0;
+        referenceCount = 1;
         lastReferenced = DateTime.Now;
+        physShapeInfo = new BulletShape();
     }
-
-    // Get a reference to a physical shape. Create if it doesn't exist
-    public static BSShape GetShapeReference(BSScene physicsScene, bool forceRebuild, BSPhysObject prim)
+    public BSShape(BulletShape pShape)
     {
-        BSShape ret = null;
-
-        if (prim.PreferredPhysicalShape == BSPhysicsShapeType.SHAPE_CAPSULE)
-        {
-            // an avatar capsule is close to a native shape (it is not shared)
-            ret = BSShapeNative.GetReference(physicsScene, prim, BSPhysicsShapeType.SHAPE_CAPSULE,
-                                        FixedShapeKey.KEY_CAPSULE);
-            physicsScene.DetailLog("{0},BSShape.GetShapeReference,avatarCapsule,shape={1}", prim.LocalID, ret);
-        }
-
-        // Compound shapes are handled special as they are rebuilt from scratch.
-        // This isn't too great a hardship since most of the child shapes will already been created.
-        if (ret == null  && prim.PreferredPhysicalShape == BSPhysicsShapeType.SHAPE_COMPOUND)
-        {
-            // Getting a reference to a compound shape gets you the compound shape with the root prim shape added
-            ret = BSShapeCompound.GetReference(prim);
-            physicsScene.DetailLog("{0},BSShapeCollection.CreateGeom,compoundShape,shape={1}", prim.LocalID, ret);
-        }
+        referenceCount = 1;
+        lastReferenced = DateTime.Now;
+        physShapeInfo = pShape;
+    }
 
-        if (ret == null)
-            ret = GetShapeReferenceNonSpecial(physicsScene, forceRebuild, prim);
+    // Get another reference to this shape.
+    public abstract BSShape GetReference(BSScene pPhysicsScene, BSPhysObject pPrim);
 
-        return ret;
-    }
-    public static BSShape GetShapeReferenceNonSpecial(BSScene physicsScene, bool forceRebuild, BSPhysObject prim)
+    // Called when this shape is being used again.
+    // Used internally. External callers should call instance.GetReference() to properly copy/reference
+    //       the shape.
+    protected virtual void IncrementReference()
     {
-        return null;
+        referenceCount++;
+        lastReferenced = DateTime.Now;
     }
-    public static BSShape GetShapeReferenceNonNative(BSScene physicsScene, bool forceRebuild, BSPhysObject prim)
+
+    // Called when this shape is being used again.
+    protected virtual void DecrementReference()
     {
-        return null;
+        referenceCount--;
+        lastReferenced = DateTime.Now;
     }
 
     // Release the use of a physical shape.
     public abstract void Dereference(BSScene physicsScene);
 
-    // All shapes have a static call to get a reference to the physical shape
-    // protected abstract static BSShape GetReference();
+    // Return 'true' if there is an allocated physics physical shape under this class instance.
+    public virtual bool HasPhysicalShape
+    {
+        get
+        {
+            if (physShapeInfo != null)
+                return physShapeInfo.HasPhysicalShape;
+            return false;
+        }
+    }
+    public virtual BSPhysicsShapeType ShapeType
+    {
+        get
+        {
+            BSPhysicsShapeType ret = BSPhysicsShapeType.SHAPE_UNKNOWN;
+            if (physShapeInfo != null && physShapeInfo.HasPhysicalShape)
+                ret = physShapeInfo.shapeType;
+            return ret;
+        }
+    }
 
     // Returns a string for debugging that uniquily identifies the memory used by this instance
-    public string AddrString
+    public virtual string AddrString
     {
-        get { return ptr.ToString("X"); }
+        get
+        {
+            if (physShapeInfo != null)
+                return physShapeInfo.AddrString;
+            return "unknown";
+        }
     }
 
     public override string ToString()
     {
         StringBuilder buff = new StringBuilder();
-        buff.Append("<p=");
-        buff.Append(AddrString);
-        buff.Append(",s=");
-        buff.Append(type.ToString());
-        buff.Append(",k=");
-        buff.Append(key.ToString("X"));
+        if (physShapeInfo == null)
+        {
+            buff.Append("<noPhys");
+        }
+        else
+        {
+            buff.Append("<phy=");
+            buff.Append(physShapeInfo.ToString());
+        }
         buff.Append(",c=");
         buff.Append(referenceCount.ToString());
         buff.Append(">");
         return buff.ToString();
     }
+
+    #region Common shape routines
+    // Create a hash of all the shape parameters to be used as a key for this particular shape.
+    public static System.UInt64 ComputeShapeKey(OMV.Vector3 size, PrimitiveBaseShape pbs, out float retLod)
+    {
+        // level of detail based on size and type of the object
+        float lod = BSParam.MeshLOD;
+        if (pbs.SculptEntry)
+            lod = BSParam.SculptLOD;
+
+        // Mega prims usually get more detail because one can interact with shape approximations at this size.
+        float maxAxis = Math.Max(size.X, Math.Max(size.Y, size.Z));
+        if (maxAxis > BSParam.MeshMegaPrimThreshold)
+            lod = BSParam.MeshMegaPrimLOD;
+
+        retLod = lod;
+        return pbs.GetMeshKey(size, lod);
+    }
+
+    // The creation of a mesh or hull can fail if an underlying asset is not available.
+    // There are two cases: 1) the asset is not in the cache and it needs to be fetched;
+    //     and 2) the asset cannot be converted (like failed decompression of JPEG2000s).
+    //     The first case causes the asset to be fetched. The second case requires
+    //     us to not loop forever.
+    // Called after creating a physical mesh or hull. If the physical shape was created,
+    //     just return.
+    public static BulletShape VerifyMeshCreated(BSScene physicsScene, BulletShape newShape, BSPhysObject prim)
+    {
+        // If the shape was successfully created, nothing more to do
+        if (newShape.HasPhysicalShape)
+            return newShape;
+
+        // VerifyMeshCreated is called after trying to create the mesh. If we think the asset had been
+        //    fetched but we end up here again, the meshing of the asset must have failed.
+        // Prevent trying to keep fetching the mesh by declaring failure.
+        if (prim.PrimAssetState == BSPhysObject.PrimAssetCondition.Fetched)
+        {
+            prim.PrimAssetState = BSPhysObject.PrimAssetCondition.Failed;
+            physicsScene.Logger.WarnFormat("{0} Fetched asset would not mesh. {1}, texture={2}",
+                                            LogHeader, prim.PhysObjectName, prim.BaseShape.SculptTexture);
+            physicsScene.DetailLog("{0},BSShape.VerifyMeshCreated,setFailed,objNam={1},tex={2}",
+                                            prim.LocalID, prim.PhysObjectName, prim.BaseShape.SculptTexture);
+        }
+        else
+        {
+            // If this mesh has an underlying asset and we have not failed getting it before, fetch the asset
+            if (prim.BaseShape.SculptEntry
+                && prim.PrimAssetState != BSPhysObject.PrimAssetCondition.Failed
+                && prim.PrimAssetState != BSPhysObject.PrimAssetCondition.Waiting
+                && prim.BaseShape.SculptTexture != OMV.UUID.Zero
+                )
+            {
+                physicsScene.DetailLog("{0},BSShape.VerifyMeshCreated,fetchAsset,objNam={1},tex={2}",
+                                            prim.LocalID, prim.PhysObjectName, prim.BaseShape.SculptTexture);
+                // Multiple requestors will know we're waiting for this asset
+                prim.PrimAssetState = BSPhysObject.PrimAssetCondition.Waiting;
+
+                BSPhysObject xprim = prim;
+                Util.FireAndForget(delegate
+                    {
+                        // physicsScene.DetailLog("{0},BSShape.VerifyMeshCreated,inFireAndForget", xprim.LocalID);
+                        RequestAssetDelegate assetProvider = physicsScene.RequestAssetMethod;
+                        if (assetProvider != null)
+                        {
+                            BSPhysObject yprim = xprim; // probably not necessary, but, just in case.
+                            assetProvider(yprim.BaseShape.SculptTexture, delegate(AssetBase asset)
+                            {
+                                // physicsScene.DetailLog("{0},BSShape.VerifyMeshCreated,assetProviderCallback", xprim.LocalID);
+                                bool assetFound = false;
+                                string mismatchIDs = String.Empty;  // DEBUG DEBUG
+                                if (asset != null && yprim.BaseShape.SculptEntry)
+                                {
+                                    if (yprim.BaseShape.SculptTexture.ToString() == asset.ID)
+                                    {
+                                        yprim.BaseShape.SculptData = asset.Data;
+                                        // This will cause the prim to see that the filler shape is not the right
+                                        //    one and try again to build the object.
+                                        // No race condition with the normal shape setting since the rebuild is at taint time.
+                                        yprim.PrimAssetState = BSPhysObject.PrimAssetCondition.Fetched;
+                                        yprim.ForceBodyShapeRebuild(false /* inTaintTime */);
+                                        assetFound = true;
+                                    }
+                                    else
+                                    {
+                                        mismatchIDs = yprim.BaseShape.SculptTexture.ToString() + "/" + asset.ID;
+                                    }
+                                }
+                                if (!assetFound)
+                                {
+                                    yprim.PrimAssetState = BSPhysObject.PrimAssetCondition.Failed;
+                                }
+                                physicsScene.DetailLog("{0},BSShape.VerifyMeshCreated,fetchAssetCallback,found={1},isSculpt={2},ids={3}",
+                                            yprim.LocalID, assetFound, yprim.BaseShape.SculptEntry, mismatchIDs );
+                            });
+                        }
+                        else
+                        {
+                            xprim.PrimAssetState = BSPhysObject.PrimAssetCondition.Failed;
+                            physicsScene.Logger.ErrorFormat("{0} Physical object requires asset but no asset provider. Name={1}",
+                                                        LogHeader, physicsScene.Name);
+                        }
+                    });
+            }
+            else
+            {
+                if (prim.PrimAssetState == BSPhysObject.PrimAssetCondition.Failed)
+                {
+                    physicsScene.Logger.WarnFormat("{0} Mesh failed to fetch asset. obj={1}, texture={2}",
+                                                LogHeader, prim.PhysObjectName, prim.BaseShape.SculptTexture);
+                    physicsScene.DetailLog("{0},BSShape.VerifyMeshCreated,wasFailed,objNam={1},tex={2}",
+                                                prim.LocalID, prim.PhysObjectName, prim.BaseShape.SculptTexture);
+                }
+            }
+         }
+
+        // While we wait for the mesh defining asset to be loaded, stick in a simple box for the object.
+        BSShape fillShape = BSShapeNative.GetReference(physicsScene, prim, BSPhysicsShapeType.SHAPE_BOX, FixedShapeKey.KEY_BOX);
+        physicsScene.DetailLog("{0},BSShape.VerifyMeshCreated,boxTempShape", prim.LocalID);
+
+        return fillShape.physShapeInfo;
+     }
+
+    #endregion // Common shape routines
 }
 
+// ============================================================================================================
 public class BSShapeNull : BSShape
 {
     public BSShapeNull() : base()
     {
     }
     public static BSShape GetReference() { return new BSShapeNull();  }
+    public override BSShape GetReference(BSScene pPhysicsScene, BSPhysObject pPrim) { return new BSShapeNull();  }
     public override void Dereference(BSScene physicsScene) { /* The magic of garbage collection will make this go away */ }
 }
 
+// ============================================================================================================
 public class BSShapeNative : BSShape
 {
     private static string LogHeader = "[BULLETSIM SHAPE NATIVE]";
-    public BSShapeNative() : base()
+    public BSShapeNative(BulletShape pShape) : base(pShape)
     {
     }
-    public static BSShape GetReference(BSScene physicsScene, BSPhysObject prim, 
-                    BSPhysicsShapeType shapeType, FixedShapeKey shapeKey) 
+
+    public static BSShape GetReference(BSScene physicsScene, BSPhysObject prim,
+                                            BSPhysicsShapeType shapeType, FixedShapeKey shapeKey)
     {
         // Native shapes are not shared and are always built anew.
-        //return new BSShapeNative(physicsScene, prim, shapeType, shapeKey);
-        return null;
+        return new BSShapeNative(CreatePhysicalNativeShape(physicsScene, prim, shapeType, shapeKey));
+    }
+
+    public override BSShape GetReference(BSScene pPhysicsScene, BSPhysObject pPrim)
+    {
+        // Native shapes are not shared so we return a new shape.
+        return new BSShapeNative(CreatePhysicalNativeShape(pPhysicsScene, pPrim,
+                                    physShapeInfo.shapeType, (FixedShapeKey)physShapeInfo.shapeKey) );
     }
 
-    private BSShapeNative(BSScene physicsScene, BSPhysObject prim,
-                    BSPhysicsShapeType shapeType, FixedShapeKey shapeKey)
+    // Make this reference to the physical shape go away since native shapes are not shared.
+    public override void Dereference(BSScene physicsScene)
     {
+        // Native shapes are not tracked and are released immediately
+        lock (physShapeInfo)
+        {
+            if (physShapeInfo.HasPhysicalShape)
+            {
+                physicsScene.DetailLog("{0},BSShapeNative.Dereference,deleteNativeShape,shape={1}", BSScene.DetailLogZero, this);
+                physicsScene.PE.DeleteCollisionShape(physicsScene.World, physShapeInfo);
+            }
+            physShapeInfo.Clear();
+            // Garbage collection will free up this instance.
+        }
+    }
+
+    private static BulletShape CreatePhysicalNativeShape(BSScene physicsScene, BSPhysObject prim,
+                                            BSPhysicsShapeType shapeType, FixedShapeKey shapeKey)
+    {
+        BulletShape newShape;
+
         ShapeData nativeShapeData = new ShapeData();
         nativeShapeData.Type = shapeType;
         nativeShapeData.ID = prim.LocalID;
@@ -147,86 +316,760 @@ public class BSShapeNative : BSShape
         nativeShapeData.MeshKey = (ulong)shapeKey;
         nativeShapeData.HullKey = (ulong)shapeKey;
 
-       
-        /*
         if (shapeType == BSPhysicsShapeType.SHAPE_CAPSULE)
         {
-            ptr = PhysicsScene.PE.BuildCapsuleShape(physicsScene.World, 1f, 1f, prim.Scale);
-            physicsScene.DetailLog("{0},BSShapeCollection.BuiletPhysicalNativeShape,capsule,scale={1}", prim.LocalID, prim.Scale);
+            newShape = physicsScene.PE.BuildCapsuleShape(physicsScene.World, 1f, 1f, prim.Scale);
+            physicsScene.DetailLog("{0},BSShapeNative,capsule,scale={1}", prim.LocalID, prim.Scale);
         }
         else
         {
-            ptr = PhysicsScene.PE.BuildNativeShape(physicsScene.World, nativeShapeData);
+            newShape = physicsScene.PE.BuildNativeShape(physicsScene.World, nativeShapeData);
         }
-        if (ptr == IntPtr.Zero)
+        if (!newShape.HasPhysicalShape)
         {
             physicsScene.Logger.ErrorFormat("{0} BuildPhysicalNativeShape failed. ID={1}, shape={2}",
                                     LogHeader, prim.LocalID, shapeType);
         }
-        type = shapeType;
-        key = (UInt64)shapeKey;
-         */
-    }
-    // Make this reference to the physical shape go away since native shapes are not shared.
-    public override void Dereference(BSScene physicsScene)
-    {
-        /*
-        // Native shapes are not tracked and are released immediately
-        physicsScene.DetailLog("{0},BSShapeCollection.DereferenceShape,deleteNativeShape,shape={1}", BSScene.DetailLogZero, this);
-        PhysicsScene.PE.DeleteCollisionShape(physicsScene.World, this);
-        ptr = IntPtr.Zero;
-        // Garbage collection will free up this instance.
-         */
+        newShape.shapeType = shapeType;
+        newShape.isNativeShape = true;
+        newShape.shapeKey = (UInt64)shapeKey;
+        return newShape;
     }
+
 }
 
+// ============================================================================================================
 public class BSShapeMesh : BSShape
 {
     private static string LogHeader = "[BULLETSIM SHAPE MESH]";
-    private static Dictionary<System.UInt64, BSShapeMesh> Meshes = new Dictionary<System.UInt64, BSShapeMesh>();
+    public static Dictionary<System.UInt64, BSShapeMesh> Meshes = new Dictionary<System.UInt64, BSShapeMesh>();
 
-    public BSShapeMesh() : base()
+    public BSShapeMesh(BulletShape pShape) : base(pShape)
     {
     }
-    public static BSShape GetReference() { return new BSShapeNull();  }
-    public override void Dereference(BSScene physicsScene) { }
+    public static BSShape GetReference(BSScene physicsScene, bool forceRebuild, BSPhysObject prim)
+    {
+        float lod;
+        System.UInt64 newMeshKey = BSShape.ComputeShapeKey(prim.Size, prim.BaseShape, out lod);
+
+        BSShapeMesh retMesh = null;
+        lock (Meshes)
+        {
+            if (Meshes.TryGetValue(newMeshKey, out retMesh))
+            {
+                // The mesh has already been created. Return a new reference to same.
+                retMesh.IncrementReference();
+            }
+            else
+            {
+                retMesh = new BSShapeMesh(new BulletShape());
+                // An instance of this mesh has not been created. Build and remember same.
+                BulletShape newShape = retMesh.CreatePhysicalMesh(physicsScene, prim, newMeshKey, prim.BaseShape, prim.Size, lod);
+
+                // Check to see if mesh was created (might require an asset).
+                newShape = VerifyMeshCreated(physicsScene, newShape, prim);
+                if (!newShape.isNativeShape)
+                {
+                    // If a mesh was what was created, remember the built shape for later sharing.
+                    Meshes.Add(newMeshKey, retMesh);
+                }
+
+                retMesh.physShapeInfo = newShape;
+            }
+        }
+        physicsScene.DetailLog("{0},BSShapeMesh,getReference,mesh={1},size={2},lod={3}", prim.LocalID, retMesh, prim.Size, lod);
+        return retMesh;
+    }
+    public override BSShape GetReference(BSScene pPhysicsScene, BSPhysObject pPrim)
+    {
+        // Another reference to this shape is just counted.
+        IncrementReference();
+        return this;
+    }
+    public override void Dereference(BSScene physicsScene)
+    {
+        lock (Meshes)
+        {
+            this.DecrementReference();
+            physicsScene.DetailLog("{0},BSShapeMesh.Dereference,shape={1}", BSScene.DetailLogZero, this);
+            // TODO: schedule aging and destruction of unused meshes.
+        }
+    }
+    // Loop through all the known meshes and return the description based on the physical address.
+    public static bool TryGetMeshByPtr(BulletShape pShape, out BSShapeMesh outMesh)
+    {
+        bool ret = false;
+        BSShapeMesh foundDesc = null;
+        lock (Meshes)
+        {
+            foreach (BSShapeMesh sm in Meshes.Values)
+            {
+                if (sm.physShapeInfo.ReferenceSame(pShape))
+                {
+                    foundDesc = sm;
+                    ret = true;
+                    break;
+                }
+
+            }
+        }
+        outMesh = foundDesc;
+        return ret;
+    }
+    private BulletShape CreatePhysicalMesh(BSScene physicsScene, BSPhysObject prim, System.UInt64 newMeshKey,
+                                            PrimitiveBaseShape pbs, OMV.Vector3 size, float lod)
+    {
+        BulletShape newShape = new BulletShape();
+
+        IMesh meshData = physicsScene.mesher.CreateMesh(prim.PhysObjectName, pbs, size, lod,
+                                        false,  // say it is not physical so a bounding box is not built
+                                        false   // do not cache the mesh and do not use previously built versions
+                                        );
+
+        if (meshData != null)
+        {
+            if (prim.PrimAssetState == BSPhysObject.PrimAssetCondition.Fetched)
+            {
+                // Release the fetched asset data once it has been used.
+                pbs.SculptData = new byte[0];
+                prim.PrimAssetState = BSPhysObject.PrimAssetCondition.Unknown;
+            }
+
+            int[] indices = meshData.getIndexListAsInt();
+            int realIndicesIndex = indices.Length;
+            float[] verticesAsFloats = meshData.getVertexListAsFloat();
+
+            if (BSParam.ShouldRemoveZeroWidthTriangles)
+            {
+                // Remove degenerate triangles. These are triangles with two of the vertices
+                //    are the same. This is complicated by the problem that vertices are not
+                //    made unique in sculpties so we have to compare the values in the vertex.
+                realIndicesIndex = 0;
+                for (int tri = 0; tri < indices.Length; tri += 3)
+                {
+                    // Compute displacements into vertex array for each vertex of the triangle
+                    int v1 = indices[tri + 0] * 3;
+                    int v2 = indices[tri + 1] * 3;
+                    int v3 = indices[tri + 2] * 3;
+                // Check to see if any two of the vertices are the same
+                    if (!( (  verticesAsFloats[v1 + 0] == verticesAsFloats[v2 + 0]
+                           && verticesAsFloats[v1 + 1] == verticesAsFloats[v2 + 1]
+                           && verticesAsFloats[v1 + 2] == verticesAsFloats[v2 + 2])
+                        || (  verticesAsFloats[v2 + 0] == verticesAsFloats[v3 + 0]
+                           && verticesAsFloats[v2 + 1] == verticesAsFloats[v3 + 1]
+                           && verticesAsFloats[v2 + 2] == verticesAsFloats[v3 + 2])
+                        || (  verticesAsFloats[v1 + 0] == verticesAsFloats[v3 + 0]
+                           && verticesAsFloats[v1 + 1] == verticesAsFloats[v3 + 1]
+                           && verticesAsFloats[v1 + 2] == verticesAsFloats[v3 + 2]) )
+                    )
+                    {
+                        // None of the vertices of the triangles are the same. This is a good triangle;
+                        indices[realIndicesIndex + 0] = indices[tri + 0];
+                        indices[realIndicesIndex + 1] = indices[tri + 1];
+                        indices[realIndicesIndex + 2] = indices[tri + 2];
+                        realIndicesIndex += 3;
+                    }
+                }
+            }
+            physicsScene.DetailLog("{0},BSShapeMesh.CreatePhysicalMesh,key={1},origTri={2},realTri={3},numVerts={4}",
+                        BSScene.DetailLogZero, newMeshKey.ToString("X"), indices.Length / 3, realIndicesIndex / 3, verticesAsFloats.Length / 3);
+
+            if (realIndicesIndex != 0)
+            {
+                newShape = physicsScene.PE.CreateMeshShape(physicsScene.World,
+                                    realIndicesIndex, indices, verticesAsFloats.Length / 3, verticesAsFloats);
+            }
+            else
+            {
+                physicsScene.Logger.DebugFormat("{0} All mesh triangles degenerate. Prim {1} at {2} in {3}",
+                                    LogHeader, prim.PhysObjectName, prim.RawPosition, physicsScene.Name);
+            }
+        }
+        newShape.shapeKey = newMeshKey;
+
+        return newShape;
+    }
 }
 
+// ============================================================================================================
 public class BSShapeHull : BSShape
 {
     private static string LogHeader = "[BULLETSIM SHAPE HULL]";
-    private static Dictionary<System.UInt64, BSShapeHull> Hulls = new Dictionary<System.UInt64, BSShapeHull>();
+    public static Dictionary<System.UInt64, BSShapeHull> Hulls = new Dictionary<System.UInt64, BSShapeHull>();
 
-    public BSShapeHull() : base()
+    public BSShapeHull(BulletShape pShape) : base(pShape)
     {
     }
-    public static BSShape GetReference() { return new BSShapeNull();  }
-    public override void Dereference(BSScene physicsScene) { }
+    public static BSShape GetReference(BSScene physicsScene, bool forceRebuild, BSPhysObject prim)
+    {
+        float lod;
+        System.UInt64 newHullKey = BSShape.ComputeShapeKey(prim.Size, prim.BaseShape, out lod);
+
+        BSShapeHull retHull = null;
+        lock (Hulls)
+        {
+            if (Hulls.TryGetValue(newHullKey, out retHull))
+            {
+                // The mesh has already been created. Return a new reference to same.
+                retHull.IncrementReference();
+            }
+            else
+            {
+                retHull = new BSShapeHull(new BulletShape());
+                // An instance of this mesh has not been created. Build and remember same.
+                BulletShape newShape = retHull.CreatePhysicalHull(physicsScene, prim, newHullKey, prim.BaseShape, prim.Size, lod);
+
+                // Check to see if hull was created (might require an asset).
+                newShape = VerifyMeshCreated(physicsScene, newShape, prim);
+                if (!newShape.isNativeShape)
+                {
+                    // If a mesh was what was created, remember the built shape for later sharing.
+                    Hulls.Add(newHullKey, retHull);
+                }
+                retHull.physShapeInfo = newShape;
+            }
+        }
+        physicsScene.DetailLog("{0},BSShapeHull,getReference,hull={1},size={2},lod={3}", prim.LocalID, retHull, prim.Size, lod);
+        return retHull;
+    }
+    public override BSShape GetReference(BSScene pPhysicsScene, BSPhysObject pPrim)
+    {
+        // Another reference to this shape is just counted.
+        IncrementReference();
+        return this;
+    }
+    public override void Dereference(BSScene physicsScene)
+    {
+        lock (Hulls)
+        {
+            this.DecrementReference();
+            physicsScene.DetailLog("{0},BSShapeHull.Dereference,shape={1}", BSScene.DetailLogZero, this);
+            // TODO: schedule aging and destruction of unused meshes.
+        }
+    }
+    List<ConvexResult> m_hulls;
+    private BulletShape CreatePhysicalHull(BSScene physicsScene, BSPhysObject prim, System.UInt64 newHullKey,
+                                            PrimitiveBaseShape pbs, OMV.Vector3 size, float lod)
+    {
+        BulletShape newShape = new BulletShape();
+        IntPtr hullPtr = IntPtr.Zero;
+
+        if (BSParam.ShouldUseBulletHACD)
+        {
+            // Build the hull shape from an existing mesh shape.
+            // The mesh should have already been created in Bullet.
+            physicsScene.DetailLog("{0},BSShapeHull.CreatePhysicalHull,shouldUseBulletHACD,entry", prim.LocalID);
+            BSShape meshShape = BSShapeMesh.GetReference(physicsScene, true, prim);
+
+            if (meshShape.physShapeInfo.HasPhysicalShape)
+            {
+                HACDParams parms;
+                parms.maxVerticesPerHull = BSParam.BHullMaxVerticesPerHull;
+                parms.minClusters = BSParam.BHullMinClusters;
+                parms.compacityWeight = BSParam.BHullCompacityWeight;
+                parms.volumeWeight = BSParam.BHullVolumeWeight;
+                parms.concavity = BSParam.BHullConcavity;
+                parms.addExtraDistPoints = BSParam.NumericBool(BSParam.BHullAddExtraDistPoints);
+                parms.addNeighboursDistPoints = BSParam.NumericBool(BSParam.BHullAddNeighboursDistPoints);
+                parms.addFacesPoints = BSParam.NumericBool(BSParam.BHullAddFacesPoints);
+                parms.shouldAdjustCollisionMargin = BSParam.NumericBool(BSParam.BHullShouldAdjustCollisionMargin);
+
+                physicsScene.DetailLog("{0},BSShapeHull.CreatePhysicalHull,hullFromMesh,beforeCall", prim.LocalID, newShape.HasPhysicalShape);
+                newShape = physicsScene.PE.BuildHullShapeFromMesh(physicsScene.World, meshShape.physShapeInfo, parms);
+                physicsScene.DetailLog("{0},BSShapeHull.CreatePhysicalHull,hullFromMesh,hasBody={1}", prim.LocalID, newShape.HasPhysicalShape);
+
+                // Now done with the mesh shape.
+                meshShape.Dereference(physicsScene);
+            }
+            physicsScene.DetailLog("{0},BSShapeHull.CreatePhysicalHull,shouldUseBulletHACD,exit,hasBody={1}", prim.LocalID, newShape.HasPhysicalShape);
+        }
+        if (!newShape.HasPhysicalShape)
+        {
+            // Build a new hull in the physical world using the C# HACD algorigthm.
+            // Pass true for physicalness as this prevents the creation of bounding box which is not needed
+            IMesh meshData = physicsScene.mesher.CreateMesh(prim.PhysObjectName, pbs, size, lod, true /* isPhysical */, false /* shouldCache */);
+            if (meshData != null)
+            {
+                if (prim.PrimAssetState == BSPhysObject.PrimAssetCondition.Fetched)
+                {
+                    // Release the fetched asset data once it has been used.
+                    pbs.SculptData = new byte[0];
+                    prim.PrimAssetState = BSPhysObject.PrimAssetCondition.Unknown;
+                }
+
+                int[] indices = meshData.getIndexListAsInt();
+                List<OMV.Vector3> vertices = meshData.getVertexList();
+
+                //format conversion from IMesh format to DecompDesc format
+                List<int> convIndices = new List<int>();
+                List<float3> convVertices = new List<float3>();
+                for (int ii = 0; ii < indices.GetLength(0); ii++)
+                {
+                    convIndices.Add(indices[ii]);
+                }
+                foreach (OMV.Vector3 vv in vertices)
+                {
+                    convVertices.Add(new float3(vv.X, vv.Y, vv.Z));
+                }
+
+                uint maxDepthSplit = (uint)BSParam.CSHullMaxDepthSplit;
+                if (BSParam.CSHullMaxDepthSplit != BSParam.CSHullMaxDepthSplitForSimpleShapes)
+                {
+                    // Simple primitive shapes we know are convex so they are better implemented with
+                    //    fewer hulls.
+                    // Check for simple shape (prim without cuts) and reduce split parameter if so.
+                    if (BSShapeCollection.PrimHasNoCuts(pbs))
+                    {
+                        maxDepthSplit = (uint)BSParam.CSHullMaxDepthSplitForSimpleShapes;
+                    }
+                }
+
+                // setup and do convex hull conversion
+                m_hulls = new List<ConvexResult>();
+                DecompDesc dcomp = new DecompDesc();
+                dcomp.mIndices = convIndices;
+                dcomp.mVertices = convVertices;
+                dcomp.mDepth = maxDepthSplit;
+                dcomp.mCpercent = BSParam.CSHullConcavityThresholdPercent;
+                dcomp.mPpercent = BSParam.CSHullVolumeConservationThresholdPercent;
+                dcomp.mMaxVertices = (uint)BSParam.CSHullMaxVertices;
+                dcomp.mSkinWidth = BSParam.CSHullMaxSkinWidth;
+                ConvexBuilder convexBuilder = new ConvexBuilder(HullReturn);
+                // create the hull into the _hulls variable
+                convexBuilder.process(dcomp);
+
+                physicsScene.DetailLog("{0},BSShapeCollection.CreatePhysicalHull,key={1},inVert={2},inInd={3},split={4},hulls={5}",
+                                    BSScene.DetailLogZero, newHullKey, indices.GetLength(0), vertices.Count, maxDepthSplit, m_hulls.Count);
+
+                // Convert the vertices and indices for passing to unmanaged.
+                // The hull information is passed as a large floating point array.
+                // The format is:
+                //  convHulls[0] = number of hulls
+                //  convHulls[1] = number of vertices in first hull
+                //  convHulls[2] = hull centroid X coordinate
+                //  convHulls[3] = hull centroid Y coordinate
+                //  convHulls[4] = hull centroid Z coordinate
+                //  convHulls[5] = first hull vertex X
+                //  convHulls[6] = first hull vertex Y
+                //  convHulls[7] = first hull vertex Z
+                //  convHulls[8] = second hull vertex X
+                //  ...
+                //  convHulls[n] = number of vertices in second hull
+                //  convHulls[n+1] = second hull centroid X coordinate
+                //  ...
+                //
+                // TODO: is is very inefficient. Someday change the convex hull generator to return
+                //   data structures that do not need to be converted in order to pass to Bullet.
+                //   And maybe put the values directly into pinned memory rather than marshaling.
+                int hullCount = m_hulls.Count;
+                int totalVertices = 1;          // include one for the count of the hulls
+                foreach (ConvexResult cr in m_hulls)
+                {
+                    totalVertices += 4;                         // add four for the vertex count and centroid
+                    totalVertices += cr.HullIndices.Count * 3;  // we pass just triangles
+                }
+                float[] convHulls = new float[totalVertices];
+
+                convHulls[0] = (float)hullCount;
+                int jj = 1;
+                foreach (ConvexResult cr in m_hulls)
+                {
+                    // copy vertices for index access
+                    float3[] verts = new float3[cr.HullVertices.Count];
+                    int kk = 0;
+                    foreach (float3 ff in cr.HullVertices)
+                    {
+                        verts[kk++] = ff;
+                    }
+
+                    // add to the array one hull's worth of data
+                    convHulls[jj++] = cr.HullIndices.Count;
+                    convHulls[jj++] = 0f;   // centroid x,y,z
+                    convHulls[jj++] = 0f;
+                    convHulls[jj++] = 0f;
+                    foreach (int ind in cr.HullIndices)
+                    {
+                        convHulls[jj++] = verts[ind].x;
+                        convHulls[jj++] = verts[ind].y;
+                        convHulls[jj++] = verts[ind].z;
+                    }
+                }
+                // create the hull data structure in Bullet
+                newShape = physicsScene.PE.CreateHullShape(physicsScene.World, hullCount, convHulls);
+            }
+            newShape.shapeKey = newHullKey;
+        }
+        return newShape;
+    }
+    // Callback from convex hull creater with a newly created hull.
+    // Just add it to our collection of hulls for this shape.
+    private void HullReturn(ConvexResult result)
+    {
+        m_hulls.Add(result);
+        return;
+    }
+    // Loop through all the known hulls and return the description based on the physical address.
+    public static bool TryGetHullByPtr(BulletShape pShape, out BSShapeHull outHull)
+    {
+        bool ret = false;
+        BSShapeHull foundDesc = null;
+        lock (Hulls)
+        {
+            foreach (BSShapeHull sh in Hulls.Values)
+            {
+                if (sh.physShapeInfo.ReferenceSame(pShape))
+                {
+                    foundDesc = sh;
+                    ret = true;
+                    break;
+                }
+
+            }
+        }
+        outHull = foundDesc;
+        return ret;
+    }
 }
 
+// ============================================================================================================
 public class BSShapeCompound : BSShape
 {
     private static string LogHeader = "[BULLETSIM SHAPE COMPOUND]";
-    public BSShapeCompound() : base()
+    public BSShapeCompound(BulletShape pShape) : base(pShape)
     {
     }
-    public static BSShape GetReference(BSPhysObject prim) 
-    { 
-        return new BSShapeNull();
+    public static BSShape GetReference(BSScene physicsScene)
+    {
+        // Base compound shapes are not shared so this returns a raw shape.
+        // A built compound shape can be reused in linksets.
+        return new BSShapeCompound(CreatePhysicalCompoundShape(physicsScene));
+    }
+    public override BSShape GetReference(BSScene physicsScene, BSPhysObject prim)
+    {
+        // Calling this reference means we want another handle to an existing compound shape
+        //     (usually linksets) so return this copy.
+        IncrementReference();
+        return this;
+    }
+    // Dereferencing a compound shape releases the hold on all the child shapes.
+    public override void Dereference(BSScene physicsScene)
+    {
+        lock (physShapeInfo)
+        {
+            this.DecrementReference();
+            physicsScene.DetailLog("{0},BSShapeCompound.Dereference,shape={1}", BSScene.DetailLogZero, this);
+            if (referenceCount <= 0)
+            {
+                if (!physicsScene.PE.IsCompound(physShapeInfo))
+                {
+                    // Failed the sanity check!!
+                    physicsScene.Logger.ErrorFormat("{0} Attempt to free a compound shape that is not compound!! type={1}, ptr={2}",
+                                                LogHeader, physShapeInfo.shapeType, physShapeInfo.AddrString);
+                    physicsScene.DetailLog("{0},BSShapeCollection.DereferenceCompound,notACompoundShape,type={1},ptr={2}",
+                                                BSScene.DetailLogZero, physShapeInfo.shapeType, physShapeInfo.AddrString);
+                    return;
+                }
+
+                int numChildren = physicsScene.PE.GetNumberOfCompoundChildren(physShapeInfo);
+                physicsScene.DetailLog("{0},BSShapeCollection.DereferenceCompound,shape={1},children={2}",
+                                        BSScene.DetailLogZero, physShapeInfo, numChildren);
+
+                // Loop through all the children dereferencing each.
+                for (int ii = numChildren - 1; ii >= 0; ii--)
+                {
+                    BulletShape childShape = physicsScene.PE.RemoveChildShapeFromCompoundShapeIndex(physShapeInfo, ii);
+                    DereferenceAnonCollisionShape(physicsScene, childShape);
+                }
+                physicsScene.PE.DeleteCollisionShape(physicsScene.World, physShapeInfo);
+            }
+        }
+    }
+    private static BulletShape CreatePhysicalCompoundShape(BSScene physicsScene)
+    {
+        BulletShape cShape = physicsScene.PE.CreateCompoundShape(physicsScene.World, false);
+        return cShape;
+    }
+    // Sometimes we have a pointer to a collision shape but don't know what type it is.
+    // Figure out type and call the correct dereference routine.
+    // Called at taint-time.
+    private void DereferenceAnonCollisionShape(BSScene physicsScene, BulletShape pShape)
+    {
+        BSShapeMesh meshDesc;
+        if (BSShapeMesh.TryGetMeshByPtr(pShape, out meshDesc))
+        {
+            meshDesc.Dereference(physicsScene);
+        }
+        else
+        {
+            BSShapeHull hullDesc;
+            if (BSShapeHull.TryGetHullByPtr(pShape, out hullDesc))
+            {
+                hullDesc.Dereference(physicsScene);
+            }
+            else
+            {
+                BSShapeConvexHull chullDesc;
+                if (BSShapeConvexHull.TryGetHullByPtr(pShape, out chullDesc))
+                {
+                    chullDesc.Dereference(physicsScene);
+                }
+                else
+                {
+                    if (physicsScene.PE.IsCompound(pShape))
+                    {
+                        BSShapeCompound recursiveCompound = new BSShapeCompound(pShape);
+                        recursiveCompound.Dereference(physicsScene);
+                    }
+                    else
+                    {
+                        if (physicsScene.PE.IsNativeShape(pShape))
+                        {
+                            BSShapeNative nativeShape = new BSShapeNative(pShape);
+                            nativeShape.Dereference(physicsScene);
+                        }
+                    }
+                }
+            }
+        }
     }
-    public override void Dereference(BSScene physicsScene) { }
 }
 
+// ============================================================================================================
+public class BSShapeConvexHull : BSShape
+{
+    private static string LogHeader = "[BULLETSIM SHAPE CONVEX HULL]";
+    public static Dictionary<System.UInt64, BSShapeConvexHull> ConvexHulls = new Dictionary<System.UInt64, BSShapeConvexHull>();
+
+    public BSShapeConvexHull(BulletShape pShape) : base(pShape)
+    {
+    }
+    public static BSShape GetReference(BSScene physicsScene, bool forceRebuild, BSPhysObject prim)
+    {
+        float lod;
+        System.UInt64 newMeshKey = BSShape.ComputeShapeKey(prim.Size, prim.BaseShape, out lod);
+
+        physicsScene.DetailLog("{0},BSShapeMesh,getReference,newKey={1},size={2},lod={3}",
+                                prim.LocalID, newMeshKey.ToString("X"), prim.Size, lod);
+
+        BSShapeConvexHull retConvexHull = null;
+        lock (ConvexHulls)
+        {
+            if (ConvexHulls.TryGetValue(newMeshKey, out retConvexHull))
+            {
+                // The mesh has already been created. Return a new reference to same.
+                retConvexHull.IncrementReference();
+            }
+            else
+            {
+                retConvexHull = new BSShapeConvexHull(new BulletShape());
+                BulletShape convexShape = null;
+
+                // Get a handle to a mesh to build the hull from
+                BSShape baseMesh = BSShapeMesh.GetReference(physicsScene, false /* forceRebuild */, prim);
+                if (baseMesh.physShapeInfo.isNativeShape)
+                {
+                    // We get here if the mesh was not creatable. Could be waiting for an asset from the disk.
+                    // In the short term, we return the native shape and a later ForceBodyShapeRebuild should
+                    //     get back to this code with a buildable mesh.
+                    // TODO: not sure the temp native shape is freed when the mesh is rebuilt. When does this get freed?
+                    convexShape = baseMesh.physShapeInfo;
+                }
+                else
+                {
+                    convexShape = physicsScene.PE.BuildConvexHullShapeFromMesh(physicsScene.World, baseMesh.physShapeInfo);
+                    convexShape.shapeKey = newMeshKey;
+                    ConvexHulls.Add(convexShape.shapeKey, retConvexHull);
+                }
+
+                // Done with the base mesh
+                baseMesh.Dereference(physicsScene);
+
+                retConvexHull.physShapeInfo = convexShape;
+            }
+        }
+        return retConvexHull;
+    }
+    public override BSShape GetReference(BSScene physicsScene, BSPhysObject prim)
+    {
+        // Calling this reference means we want another handle to an existing shape
+        //     (usually linksets) so return this copy.
+        IncrementReference();
+        return this;
+    }
+    // Dereferencing a compound shape releases the hold on all the child shapes.
+    public override void Dereference(BSScene physicsScene)
+    {
+        lock (ConvexHulls)
+        {
+            this.DecrementReference();
+            physicsScene.DetailLog("{0},BSShapeConvexHull.Dereference,shape={1}", BSScene.DetailLogZero, this);
+            // TODO: schedule aging and destruction of unused meshes.
+        }
+    }
+    // Loop through all the known hulls and return the description based on the physical address.
+    public static bool TryGetHullByPtr(BulletShape pShape, out BSShapeConvexHull outHull)
+    {
+        bool ret = false;
+        BSShapeConvexHull foundDesc = null;
+        lock (ConvexHulls)
+        {
+            foreach (BSShapeConvexHull sh in ConvexHulls.Values)
+            {
+                if (sh.physShapeInfo.ReferenceSame(pShape))
+                {
+                    foundDesc = sh;
+                    ret = true;
+                    break;
+                }
+
+            }
+        }
+        outHull = foundDesc;
+        return ret;
+    }
+}
+
+// ============================================================================================================
 public class BSShapeAvatar : BSShape
 {
     private static string LogHeader = "[BULLETSIM SHAPE AVATAR]";
     public BSShapeAvatar() : base()
     {
     }
-    public static BSShape GetReference(BSPhysObject prim) 
-    { 
+    public static BSShape GetReference(BSPhysObject prim)
+    {
+        return new BSShapeNull();
+    }
+    public override BSShape GetReference(BSScene pPhysicsScene, BSPhysObject pPrim)
+    {
         return new BSShapeNull();
     }
     public override void Dereference(BSScene physicsScene) { }
+
+    // From the front:
+    //     A---A
+    //    /     \
+    //   B-------B
+    //  /         \        +Z
+    // C-----------C        |
+    // \           /   -Y --+-- +Y
+    //  \         /         |
+    //   \       /         -Z
+    //    D-----D
+    //     \   /
+    //      E-E
+
+    // From the top A and E are just lines.
+    //              B, C and D are hexagons:
+    //
+    //     C1--C2            +X
+    //    /      \            |
+    //  C0        C3     -Y --+-- +Y
+    //    \      /            |
+    //     C5--C4            -X
+
+    // Zero goes directly through the middle so the offsets are from that middle axis
+    //     and up and down from a middle horizon (A and E are the same distance from the zero).
+    // The height, width and depth is one. All scaling is done by the simulator.
+
+    // Z component -- how far the level is from the middle zero
+    private const float Aup = 0.5f;
+    private const float Bup = 0.4f;
+    private const float Cup = 0.3f;
+    private const float Dup = -0.4f;
+    private const float Eup = -0.5f;
+
+    // Y component -- distance from center to x0 and x3
+    private const float Awid = 0.25f;
+    private const float Bwid = 0.3f;
+    private const float Cwid = 0.5f;
+    private const float Dwid = 0.3f;
+    private const float Ewid = 0.2f;
+
+    // Y component -- distance from center to x1, x2, x4 and x5
+    private const float Afwid = 0.0f;
+    private const float Bfwid = 0.2f;
+    private const float Cfwid = 0.4f;
+    private const float Dfwid = 0.2f;
+    private const float Efwid = 0.0f;
+
+    // X component -- distance from zero to the front or back of a level
+    private const float Adep = 0f;
+    private const float Bdep = 0.3f;
+    private const float Cdep = 0.5f;
+    private const float Ddep = 0.2f;
+    private const float Edep = 0f;
+
+    private OMV.Vector3[] avatarVertices = {
+           new OMV.Vector3( 0.0f, -Awid,  Aup),   // A0
+           new OMV.Vector3( 0.0f, +Awid,  Aup),   // A3
+
+           new OMV.Vector3( 0.0f, -Bwid,  Bup),   // B0
+           new OMV.Vector3(+Bdep, -Bfwid, Bup),   // B1
+           new OMV.Vector3(+Bdep, +Bfwid, Bup),   // B2
+           new OMV.Vector3( 0.0f, +Bwid,  Bup),   // B3
+           new OMV.Vector3(-Bdep, +Bfwid, Bup),   // B4
+           new OMV.Vector3(-Bdep, -Bfwid, Bup),   // B5
+
+           new OMV.Vector3( 0.0f, -Cwid,  Cup),   // C0
+           new OMV.Vector3(+Cdep, -Cfwid, Cup),   // C1
+           new OMV.Vector3(+Cdep, +Cfwid, Cup),   // C2
+           new OMV.Vector3( 0.0f, +Cwid,  Cup),   // C3
+           new OMV.Vector3(-Cdep, +Cfwid, Cup),   // C4
+           new OMV.Vector3(-Cdep, -Cfwid, Cup),   // C5
+
+           new OMV.Vector3( 0.0f, -Dwid,  Dup),   // D0
+           new OMV.Vector3(+Ddep, -Dfwid, Dup),   // D1
+           new OMV.Vector3(+Ddep, +Dfwid, Dup),   // D2
+           new OMV.Vector3( 0.0f, +Dwid,  Dup),   // D3
+           new OMV.Vector3(-Ddep, +Dfwid, Dup),   // D4
+           new OMV.Vector3(-Ddep, -Dfwid, Dup),   // D5
+
+           new OMV.Vector3( 0.0f, -Ewid,  Eup),   // E0
+           new OMV.Vector3( 0.0f, +Ewid,  Eup),   // E3
+    };
+
+    // Offsets of the vertices in the vertices array
+    private enum Ind : int
+    {
+        A0, A3,
+        B0, B1, B2, B3, B4, B5,
+        C0, C1, C2, C3, C4, C5,
+        D0, D1, D2, D3, D4, D5,
+        E0, E3
+    }
+
+    // Comments specify trianges and quads in clockwise direction
+    private Ind[] avatarIndices = {
+        Ind.A0, Ind.B0, Ind.B1,                         // A0,B0,B1
+        Ind.A0, Ind.B1, Ind.B2, Ind.B2, Ind.A3, Ind.A0, // A0,B1,B2,A3
+        Ind.A3, Ind.B2, Ind.B3,                         // A3,B2,B3
+        Ind.A3, Ind.B3, Ind.B4,                         // A3,B3,B4
+        Ind.A3, Ind.B4, Ind.B5, Ind.B5, Ind.A0, Ind.A3, // A3,B4,B5,A0
+        Ind.A0, Ind.B5, Ind.B0,                         // A0,B5,B0
+
+        Ind.B0, Ind.C0, Ind.C1, Ind.C1, Ind.B1, Ind.B0, // B0,C0,C1,B1
+        Ind.B1, Ind.C1, Ind.C2, Ind.C2, Ind.B2, Ind.B1, // B1,C1,C2,B2
+        Ind.B2, Ind.C2, Ind.C3, Ind.C3, Ind.B3, Ind.B2, // B2,C2,C3,B3
+        Ind.B3, Ind.C3, Ind.C4, Ind.C4, Ind.B4, Ind.B3, // B3,C3,C4,B4
+        Ind.B4, Ind.C4, Ind.C5, Ind.C5, Ind.B5, Ind.B4, // B4,C4,C5,B5
+        Ind.B5, Ind.C5, Ind.C0, Ind.C0, Ind.B0, Ind.B5, // B5,C5,C0,B0
+
+        Ind.C0, Ind.D0, Ind.D1, Ind.D1, Ind.C1, Ind.C0, // C0,D0,D1,C1
+        Ind.C1, Ind.D1, Ind.D2, Ind.D2, Ind.C2, Ind.C1, // C1,D1,D2,C2
+        Ind.C2, Ind.D2, Ind.D3, Ind.D3, Ind.C3, Ind.C2, // C2,D2,D3,C3
+        Ind.C3, Ind.D3, Ind.D4, Ind.D4, Ind.C4, Ind.C3, // C3,D3,D4,C4
+        Ind.C4, Ind.D4, Ind.D5, Ind.D5, Ind.C5, Ind.C4, // C4,D4,D5,C5
+        Ind.C5, Ind.D5, Ind.D0, Ind.D0, Ind.C0, Ind.C5, // C5,D5,D0,C0
+
+        Ind.E0, Ind.D0, Ind.D1,                         // E0,D0,D1
+        Ind.E0, Ind.D1, Ind.D2, Ind.D2, Ind.E3, Ind.E0, // E0,D1,D2,E3
+        Ind.E3, Ind.D2, Ind.D3,                         // E3,D2,D3
+        Ind.E3, Ind.D3, Ind.D4,                         // E3,D3,D4
+        Ind.E3, Ind.D4, Ind.D5, Ind.D5, Ind.E0, Ind.E3, // E3,D4,D5,E0
+        Ind.E0, Ind.D5, Ind.D0,                         // E0,D5,D0
+
+    };
+
 }
 }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSTerrainHeightmap.cs b/OpenSim/Region/Physics/BulletSPlugin/BSTerrainHeightmap.cs
index e4fecc3..c7deb4e 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSTerrainHeightmap.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSTerrainHeightmap.cs
@@ -68,7 +68,7 @@ public sealed class BSTerrainHeightmap : BSTerrainPhys
 
     // This minCoords and maxCoords passed in give the size of the terrain (min and max Z
     //         are the high and low points of the heightmap).
-    public BSTerrainHeightmap(BSScene physicsScene, Vector3 regionBase, uint id, float[] initialMap, 
+    public BSTerrainHeightmap(BSScene physicsScene, Vector3 regionBase, uint id, float[] initialMap,
                                                     Vector3 minCoords, Vector3 maxCoords)
         : base(physicsScene, regionBase, id)
     {
@@ -92,7 +92,7 @@ public sealed class BSTerrainHeightmap : BSTerrainPhys
     private void BuildHeightmapTerrain()
     {
         // Create the terrain shape from the mapInfo
-        m_mapInfo.terrainShape = PhysicsScene.PE.CreateTerrainShape( m_mapInfo.ID, 
+        m_mapInfo.terrainShape = m_physicsScene.PE.CreateTerrainShape( m_mapInfo.ID,
                                 new Vector3(m_mapInfo.sizeX, m_mapInfo.sizeY, 0), m_mapInfo.minZ, m_mapInfo.maxZ,
                                 m_mapInfo.heightMap, 1f, BSParam.TerrainCollisionMargin);
 
@@ -103,26 +103,26 @@ public sealed class BSTerrainHeightmap : BSTerrainPhys
         centerPos.Y = m_mapInfo.minCoords.Y + (m_mapInfo.sizeY / 2f);
         centerPos.Z = m_mapInfo.minZ + ((m_mapInfo.maxZ - m_mapInfo.minZ) / 2f);
 
-        m_mapInfo.terrainBody = PhysicsScene.PE.CreateBodyWithDefaultMotionState(m_mapInfo.terrainShape, 
+        m_mapInfo.terrainBody = m_physicsScene.PE.CreateBodyWithDefaultMotionState(m_mapInfo.terrainShape,
                                 m_mapInfo.ID, centerPos, Quaternion.Identity);
 
         // Set current terrain attributes
-        PhysicsScene.PE.SetFriction(m_mapInfo.terrainBody, BSParam.TerrainFriction);
-        PhysicsScene.PE.SetHitFraction(m_mapInfo.terrainBody, BSParam.TerrainHitFraction);
-        PhysicsScene.PE.SetRestitution(m_mapInfo.terrainBody, BSParam.TerrainRestitution);
-        PhysicsScene.PE.SetCollisionFlags(m_mapInfo.terrainBody, CollisionFlags.CF_STATIC_OBJECT);
+        m_physicsScene.PE.SetFriction(m_mapInfo.terrainBody, BSParam.TerrainFriction);
+        m_physicsScene.PE.SetHitFraction(m_mapInfo.terrainBody, BSParam.TerrainHitFraction);
+        m_physicsScene.PE.SetRestitution(m_mapInfo.terrainBody, BSParam.TerrainRestitution);
+        m_physicsScene.PE.SetCollisionFlags(m_mapInfo.terrainBody, CollisionFlags.CF_STATIC_OBJECT);
 
         // Return the new terrain to the world of physical objects
-        PhysicsScene.PE.AddObjectToWorld(PhysicsScene.World, m_mapInfo.terrainBody);
+        m_physicsScene.PE.AddObjectToWorld(m_physicsScene.World, m_mapInfo.terrainBody);
 
         // redo its bounding box now that it is in the world
-        PhysicsScene.PE.UpdateSingleAabb(PhysicsScene.World, m_mapInfo.terrainBody);
+        m_physicsScene.PE.UpdateSingleAabb(m_physicsScene.World, m_mapInfo.terrainBody);
 
         m_mapInfo.terrainBody.collisionType = CollisionType.Terrain;
-        m_mapInfo.terrainBody.ApplyCollisionMask(PhysicsScene);
+        m_mapInfo.terrainBody.ApplyCollisionMask(m_physicsScene);
 
         // Make it so the terrain will not move or be considered for movement.
-        PhysicsScene.PE.ForceActivationState(m_mapInfo.terrainBody, ActivationState.DISABLE_SIMULATION);
+        m_physicsScene.PE.ForceActivationState(m_mapInfo.terrainBody, ActivationState.DISABLE_SIMULATION);
 
         return;
     }
@@ -134,9 +134,9 @@ public sealed class BSTerrainHeightmap : BSTerrainPhys
         {
             if (m_mapInfo.terrainBody.HasPhysicalBody)
             {
-                PhysicsScene.PE.RemoveObjectFromWorld(PhysicsScene.World, m_mapInfo.terrainBody);
+                m_physicsScene.PE.RemoveObjectFromWorld(m_physicsScene.World, m_mapInfo.terrainBody);
                 // Frees both the body and the shape.
-                PhysicsScene.PE.DestroyObject(PhysicsScene.World, m_mapInfo.terrainBody);
+                m_physicsScene.PE.DestroyObject(m_physicsScene.World, m_mapInfo.terrainBody);
             }
         }
         m_mapInfo = null;
@@ -155,7 +155,7 @@ public sealed class BSTerrainHeightmap : BSTerrainPhys
         catch
         {
             // Sometimes they give us wonky values of X and Y. Give a warning and return something.
-            PhysicsScene.Logger.WarnFormat("{0} Bad request for terrain height. terrainBase={1}, pos={2}",
+            m_physicsScene.Logger.WarnFormat("{0} Bad request for terrain height. terrainBase={1}, pos={2}",
                                 LogHeader, m_mapInfo.terrainRegionBase, pos);
             ret = BSTerrainManager.HEIGHT_GETHEIGHT_RET;
         }
@@ -165,7 +165,7 @@ public sealed class BSTerrainHeightmap : BSTerrainPhys
     // The passed position is relative to the base of the region.
     public override float GetWaterLevelAtXYZ(Vector3 pos)
     {
-        return PhysicsScene.SimpleWaterLevel;
+        return m_physicsScene.SimpleWaterLevel;
     }
 }
 }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSTerrainManager.cs b/OpenSim/Region/Physics/BulletSPlugin/BSTerrainManager.cs
index 2e9db39..c4807c4 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSTerrainManager.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSTerrainManager.cs
@@ -50,14 +50,14 @@ public abstract class BSTerrainPhys : IDisposable
         Mesh        = 1
     }
 
-    public BSScene PhysicsScene { get; private set; }
+    protected BSScene m_physicsScene { get; private set; }
     // Base of the region in world coordinates. Coordinates inside the region are relative to this.
     public Vector3 TerrainBase { get; private set; }
     public uint ID { get; private set; }
 
     public BSTerrainPhys(BSScene physicsScene, Vector3 regionBase, uint id)
     {
-        PhysicsScene = physicsScene;
+        m_physicsScene = physicsScene;
         TerrainBase = regionBase;
         ID = id;
     }
@@ -86,7 +86,7 @@ public sealed class BSTerrainManager : IDisposable
     public Vector3 DefaultRegionSize = new Vector3(Constants.RegionSize, Constants.RegionSize, Constants.RegionHeight);
 
     // The scene that I am part of
-    private BSScene PhysicsScene { get; set; }
+    private BSScene m_physicsScene { get; set; }
 
     // The ground plane created to keep thing from falling to infinity.
     private BulletBody m_groundPlane;
@@ -113,7 +113,7 @@ public sealed class BSTerrainManager : IDisposable
 
     public BSTerrainManager(BSScene physicsScene)
     {
-        PhysicsScene = physicsScene;
+        m_physicsScene = physicsScene;
         m_terrains = new Dictionary<Vector3,BSTerrainPhys>();
 
         // Assume one region of default size
@@ -132,32 +132,37 @@ public sealed class BSTerrainManager : IDisposable
     //    safe to call Bullet in real time. We hope no one is moving prims around yet.
     public void CreateInitialGroundPlaneAndTerrain()
     {
+        DetailLog("{0},BSTerrainManager.CreateInitialGroundPlaneAndTerrain,region={1}", BSScene.DetailLogZero, m_physicsScene.RegionName);
         // The ground plane is here to catch things that are trying to drop to negative infinity
-        BulletShape groundPlaneShape = PhysicsScene.PE.CreateGroundPlaneShape(BSScene.GROUNDPLANE_ID, 1f, BSParam.TerrainCollisionMargin);
-        m_groundPlane = PhysicsScene.PE.CreateBodyWithDefaultMotionState(groundPlaneShape, 
+        BulletShape groundPlaneShape = m_physicsScene.PE.CreateGroundPlaneShape(BSScene.GROUNDPLANE_ID, 1f, BSParam.TerrainCollisionMargin);
+        m_groundPlane = m_physicsScene.PE.CreateBodyWithDefaultMotionState(groundPlaneShape,
                                         BSScene.GROUNDPLANE_ID, Vector3.Zero, Quaternion.Identity);
 
-        PhysicsScene.PE.AddObjectToWorld(PhysicsScene.World, m_groundPlane);
-        PhysicsScene.PE.UpdateSingleAabb(PhysicsScene.World, m_groundPlane);
+        m_physicsScene.PE.AddObjectToWorld(m_physicsScene.World, m_groundPlane);
+        m_physicsScene.PE.UpdateSingleAabb(m_physicsScene.World, m_groundPlane);
         // Ground plane does not move
-        PhysicsScene.PE.ForceActivationState(m_groundPlane, ActivationState.DISABLE_SIMULATION);
+        m_physicsScene.PE.ForceActivationState(m_groundPlane, ActivationState.DISABLE_SIMULATION);
         // Everything collides with the ground plane.
         m_groundPlane.collisionType = CollisionType.Groundplane;
-        m_groundPlane.ApplyCollisionMask(PhysicsScene);
+        m_groundPlane.ApplyCollisionMask(m_physicsScene);
 
-        // Build an initial terrain and put it in the world. This quickly gets replaced by the real region terrain.
-        BSTerrainPhys initialTerrain = new BSTerrainHeightmap(PhysicsScene, Vector3.Zero, BSScene.TERRAIN_ID, DefaultRegionSize);
-        m_terrains.Add(Vector3.Zero, initialTerrain);
+        BSTerrainPhys initialTerrain = new BSTerrainHeightmap(m_physicsScene, Vector3.Zero, BSScene.TERRAIN_ID, DefaultRegionSize);
+        lock (m_terrains)
+        {
+            // Build an initial terrain and put it in the world. This quickly gets replaced by the real region terrain.
+            m_terrains.Add(Vector3.Zero, initialTerrain);
+        }
     }
 
     // Release all the terrain structures we might have allocated
     public void ReleaseGroundPlaneAndTerrain()
     {
+        DetailLog("{0},BSTerrainManager.ReleaseGroundPlaneAndTerrain,region={1}", BSScene.DetailLogZero, m_physicsScene.RegionName);
         if (m_groundPlane.HasPhysicalBody)
         {
-            if (PhysicsScene.PE.RemoveObjectFromWorld(PhysicsScene.World, m_groundPlane))
+            if (m_physicsScene.PE.RemoveObjectFromWorld(m_physicsScene.World, m_groundPlane))
             {
-                PhysicsScene.PE.DestroyObject(PhysicsScene.World, m_groundPlane);
+                m_physicsScene.PE.DestroyObject(m_physicsScene.World, m_groundPlane);
             }
             m_groundPlane.Clear();
         }
@@ -183,7 +188,7 @@ public sealed class BSTerrainManager : IDisposable
         float[] localHeightMap = heightMap;
         // If there are multiple requests for changes to the same terrain between ticks,
         //      only do that last one.
-        PhysicsScene.PostTaintObject("TerrainManager.SetTerrain-"+ m_worldOffset.ToString(), 0, delegate()
+        m_physicsScene.PostTaintObject("TerrainManager.SetTerrain-"+ m_worldOffset.ToString(), 0, delegate()
         {
             if (m_worldOffset != Vector3.Zero && MegaRegionParentPhysicsScene != null)
             {
@@ -193,11 +198,9 @@ public sealed class BSTerrainManager : IDisposable
                 //    the terrain is added to our parent
                 if (MegaRegionParentPhysicsScene is BSScene)
                 {
-                    DetailLog("{0},SetTerrain.ToParent,offset={1},worldMax={2}",
-                                    BSScene.DetailLogZero, m_worldOffset, m_worldMax);
-                    ((BSScene)MegaRegionParentPhysicsScene).TerrainManager.UpdateTerrain(
-                                    BSScene.CHILDTERRAIN_ID, localHeightMap, 
-                                    m_worldOffset, m_worldOffset + DefaultRegionSize, true);
+                    DetailLog("{0},SetTerrain.ToParent,offset={1},worldMax={2}", BSScene.DetailLogZero, m_worldOffset, m_worldMax);
+                    ((BSScene)MegaRegionParentPhysicsScene).TerrainManager.AddMegaRegionChildTerrain(
+                                        BSScene.CHILDTERRAIN_ID, localHeightMap, m_worldOffset, m_worldOffset + DefaultRegionSize);
                 }
             }
             else
@@ -205,26 +208,36 @@ public sealed class BSTerrainManager : IDisposable
                 // If not doing the mega-prim thing, just change the terrain
                 DetailLog("{0},SetTerrain.Existing", BSScene.DetailLogZero);
 
-                UpdateTerrain(BSScene.TERRAIN_ID, localHeightMap,
-                                    m_worldOffset, m_worldOffset + DefaultRegionSize, true);
+                UpdateTerrain(BSScene.TERRAIN_ID, localHeightMap, m_worldOffset, m_worldOffset + DefaultRegionSize);
             }
         });
     }
 
-    // If called with no mapInfo for the terrain, this will create a new mapInfo and terrain
+    // Another region is calling this region and passing a terrain.
+    // A region that is not the mega-region root will pass its terrain to the root region so the root region
+    //      physics engine will have all the terrains.
+    private void AddMegaRegionChildTerrain(uint id, float[] heightMap, Vector3 minCoords, Vector3 maxCoords)
+    {
+        // Since we are called by another region's thread, the action must be rescheduled onto our processing thread.
+        m_physicsScene.PostTaintObject("TerrainManager.AddMegaRegionChild" + minCoords.ToString(), id, delegate()
+        {
+            UpdateTerrain(id, heightMap, minCoords, maxCoords);
+        });
+    }
+
+    // If called for terrain has has not been previously allocated, a new terrain will be built
     //     based on the passed information. The 'id' should be either the terrain id or
     //     BSScene.CHILDTERRAIN_ID. If the latter, a new child terrain ID will be allocated and used.
     //     The latter feature is for creating child terrains for mega-regions.
-    // If called with a mapInfo in m_heightMaps and there is an existing terrain body, a new
+    // If there is an existing terrain body, a new
     //     terrain shape is created and added to the body.
     //     This call is most often used to update the heightMap and parameters of the terrain.
     // (The above does suggest that some simplification/refactoring is in order.)
     // Called during taint-time.
-    private void UpdateTerrain(uint id, float[] heightMap, 
-                            Vector3 minCoords, Vector3 maxCoords, bool inTaintTime)
+    private void UpdateTerrain(uint id, float[] heightMap, Vector3 minCoords, Vector3 maxCoords)
     {
-        DetailLog("{0},BSTerrainManager.UpdateTerrain,call,minC={1},maxC={2},inTaintTime={3}",
-                            BSScene.DetailLogZero, minCoords, maxCoords, inTaintTime);
+        DetailLog("{0},BSTerrainManager.UpdateTerrain,call,id={1},minC={2},maxC={3}",
+                            BSScene.DetailLogZero, id, minCoords, maxCoords);
 
         // Find high and low points of passed heightmap.
         // The min and max passed in is usually the area objects can be in (maximum
@@ -253,7 +266,7 @@ public sealed class BSTerrainManager : IDisposable
             if (m_terrains.TryGetValue(terrainRegionBase, out terrainPhys))
             {
                 // There is already a terrain in this spot. Free the old and build the new.
-                DetailLog("{0},UpdateTerrain:UpdateExisting,call,id={1},base={2},minC={3},maxC={4}",
+                DetailLog("{0},BSTErrainManager.UpdateTerrain:UpdateExisting,call,id={1},base={2},minC={3},maxC={4}",
                                 BSScene.DetailLogZero, id, terrainRegionBase, minCoords, minCoords);
 
                 // Remove old terrain from the collection
@@ -263,6 +276,7 @@ public sealed class BSTerrainManager : IDisposable
 
                 if (MegaRegionParentPhysicsScene == null)
                 {
+                    // This terrain is not part of the mega-region scheme. Create vanilla terrain.
                     BSTerrainPhys newTerrainPhys = BuildPhysicalTerrain(terrainRegionBase, id, heightMap, minCoords, maxCoords);
                     m_terrains.Add(terrainRegionBase, newTerrainPhys);
 
@@ -291,8 +305,8 @@ public sealed class BSTerrainManager : IDisposable
                 if (newTerrainID >= BSScene.CHILDTERRAIN_ID)
                     newTerrainID = ++m_terrainCount;
 
-                DetailLog("{0},UpdateTerrain:NewTerrain,taint,newID={1},minCoord={2},maxCoord={3}",
-                                            BSScene.DetailLogZero, newTerrainID, minCoords, minCoords);
+                DetailLog("{0},BSTerrainManager.UpdateTerrain:NewTerrain,taint,newID={1},minCoord={2},maxCoord={3}",
+                                            BSScene.DetailLogZero, newTerrainID, minCoords, maxCoords);
                 BSTerrainPhys newTerrainPhys = BuildPhysicalTerrain(terrainRegionBase, id, heightMap, minCoords, maxCoords);
                 m_terrains.Add(terrainRegionBase, newTerrainPhys);
 
@@ -304,26 +318,26 @@ public sealed class BSTerrainManager : IDisposable
     // TODO: redo terrain implementation selection to allow other base types than heightMap.
     private BSTerrainPhys BuildPhysicalTerrain(Vector3 terrainRegionBase, uint id, float[] heightMap, Vector3 minCoords, Vector3 maxCoords)
     {
-        PhysicsScene.Logger.DebugFormat("{0} Terrain for {1}/{2} created with {3}", 
-                                            LogHeader, PhysicsScene.RegionName, terrainRegionBase, 
+        m_physicsScene.Logger.DebugFormat("{0} Terrain for {1}/{2} created with {3}",
+                                            LogHeader, m_physicsScene.RegionName, terrainRegionBase,
                                             (BSTerrainPhys.TerrainImplementation)BSParam.TerrainImplementation);
         BSTerrainPhys newTerrainPhys = null;
         switch ((int)BSParam.TerrainImplementation)
         {
             case (int)BSTerrainPhys.TerrainImplementation.Heightmap:
-                newTerrainPhys = new BSTerrainHeightmap(PhysicsScene, terrainRegionBase, id,
+                newTerrainPhys = new BSTerrainHeightmap(m_physicsScene, terrainRegionBase, id,
                                             heightMap, minCoords, maxCoords);
                 break;
             case (int)BSTerrainPhys.TerrainImplementation.Mesh:
-                newTerrainPhys = new BSTerrainMesh(PhysicsScene, terrainRegionBase, id,
+                newTerrainPhys = new BSTerrainMesh(m_physicsScene, terrainRegionBase, id,
                                             heightMap, minCoords, maxCoords);
                 break;
             default:
-                PhysicsScene.Logger.ErrorFormat("{0} Bad terrain implementation specified. Type={1}/{2},Region={3}/{4}",
-                                            LogHeader, 
-                                            (int)BSParam.TerrainImplementation, 
+                m_physicsScene.Logger.ErrorFormat("{0} Bad terrain implementation specified. Type={1}/{2},Region={3}/{4}",
+                                            LogHeader,
+                                            (int)BSParam.TerrainImplementation,
                                             BSParam.TerrainImplementation,
-                                            PhysicsScene.RegionName, terrainRegionBase);
+                                            m_physicsScene.RegionName, terrainRegionBase);
                 break;
         }
         return newTerrainPhys;
@@ -337,6 +351,53 @@ public sealed class BSTerrainManager : IDisposable
         return GetTerrainPhysicalAtXYZ(pos, out physTerrain, out terrainBaseXYZ);
     }
 
+    // Return a new position that is over known terrain if the position is outside our terrain.
+    public Vector3 ClampPositionIntoKnownTerrain(Vector3 pPos)
+    {
+        Vector3 ret = pPos;
+
+        // First, base addresses are never negative so correct for that possible problem.
+        if (ret.X < 0f || ret.Y < 0f)
+        {
+            ret.X = Util.Clamp<float>(ret.X, 0f, 1000000f);
+            ret.Y = Util.Clamp<float>(ret.Y, 0f, 1000000f);
+            DetailLog("{0},BSTerrainManager.ClampPositionToKnownTerrain,zeroingNegXorY,oldPos={1},newPos={2}",
+                                        BSScene.DetailLogZero, pPos, ret);
+        }
+
+        // Can't do this function if we don't know about any terrain.
+        if (m_terrains.Count == 0)
+            return ret;
+
+        int loopPrevention = 10;
+        Vector3 terrainBaseXYZ;
+        BSTerrainPhys physTerrain;
+        while (!GetTerrainPhysicalAtXYZ(ret, out physTerrain, out terrainBaseXYZ))
+        {
+            // The passed position is not within a known terrain area.
+            // NOTE that GetTerrainPhysicalAtXYZ will set 'terrainBaseXYZ' to the base of the unfound region.
+
+            // Must be off the top of a region. Find an adjacent region to move into.
+            Vector3 adjacentTerrainBase = FindAdjacentTerrainBase(terrainBaseXYZ);
+
+            ret.X = Math.Min(ret.X, adjacentTerrainBase.X + (ret.X % DefaultRegionSize.X));
+            ret.Y = Math.Min(ret.Y, adjacentTerrainBase.Y + (ret.X % DefaultRegionSize.Y));
+            DetailLog("{0},BSTerrainManager.ClampPositionToKnownTerrain,findingAdjacentRegion,adjacentRegBase={1},oldPos={2},newPos={3}",
+                                        BSScene.DetailLogZero, adjacentTerrainBase, pPos, ret);
+
+            if (loopPrevention-- < 0f)
+            {
+                // The 'while' is a little dangerous so this prevents looping forever if the
+                //    mapping of the terrains ever gets messed up (like nothing at <0,0>) or
+                //    the list of terrains is in transition.
+                DetailLog("{0},BSTerrainManager.ClampPositionToKnownTerrain,suppressingFindAdjacentRegionLoop", BSScene.DetailLogZero);
+                break;
+            }
+        }
+
+        return ret;
+    }
+
     // Given an X and Y, find the height of the terrain.
     // Since we could be handling multiple terrains for a mega-region,
     //    the base of the region is calcuated assuming all regions are
@@ -368,8 +429,8 @@ public sealed class BSTerrainManager : IDisposable
         }
         else
         {
-            PhysicsScene.Logger.ErrorFormat("{0} GetTerrainHeightAtXY: terrain not found: region={1}, x={2}, y={3}",
-                    LogHeader, PhysicsScene.RegionName, tX, tY);
+            m_physicsScene.Logger.ErrorFormat("{0} GetTerrainHeightAtXY: terrain not found: region={1}, x={2}, y={3}",
+                    LogHeader, m_physicsScene.RegionName, tX, tY);
             DetailLog("{0},BSTerrainManager.GetTerrainHeightAtXYZ,terrainNotFound,pos={1},base={2}",
                                 BSScene.DetailLogZero, pos, terrainBaseXYZ);
         }
@@ -390,8 +451,8 @@ public sealed class BSTerrainManager : IDisposable
         }
         else
         {
-            PhysicsScene.Logger.ErrorFormat("{0} GetWaterHeightAtXY: terrain not found: pos={1}, terrainBase={2}, height={3}",
-                    LogHeader, PhysicsScene.RegionName, pos, terrainBaseXYZ, ret);
+            m_physicsScene.Logger.ErrorFormat("{0} GetWaterHeightAtXY: terrain not found: pos={1}, terrainBase={2}, height={3}",
+                    LogHeader, m_physicsScene.RegionName, pos, terrainBaseXYZ, ret);
         }
         return ret;
     }
@@ -400,18 +461,69 @@ public sealed class BSTerrainManager : IDisposable
     //    the descriptor class and the 'base' fo the addresses therein.
     private bool GetTerrainPhysicalAtXYZ(Vector3 pos, out BSTerrainPhys outPhysTerrain, out Vector3 outTerrainBase)
     {
-        int offsetX = ((int)(pos.X / (int)DefaultRegionSize.X)) * (int)DefaultRegionSize.X;
-        int offsetY = ((int)(pos.Y / (int)DefaultRegionSize.Y)) * (int)DefaultRegionSize.Y;
-        Vector3 terrainBaseXYZ = new Vector3(offsetX, offsetY, 0f);
+        bool ret = false;
+
+        Vector3 terrainBaseXYZ = Vector3.Zero;
+        if (pos.X < 0f || pos.Y < 0f)
+        {
+            // We don't handle negative addresses so just make up a base that will not be found.
+            terrainBaseXYZ = new Vector3(-DefaultRegionSize.X, -DefaultRegionSize.Y, 0f);
+        }
+        else
+        {
+            int offsetX = ((int)(pos.X / (int)DefaultRegionSize.X)) * (int)DefaultRegionSize.X;
+            int offsetY = ((int)(pos.Y / (int)DefaultRegionSize.Y)) * (int)DefaultRegionSize.Y;
+            terrainBaseXYZ = new Vector3(offsetX, offsetY, 0f);
+        }
 
         BSTerrainPhys physTerrain = null;
         lock (m_terrains)
         {
-            m_terrains.TryGetValue(terrainBaseXYZ, out physTerrain);
+            ret = m_terrains.TryGetValue(terrainBaseXYZ, out physTerrain);
         }
         outTerrainBase = terrainBaseXYZ;
         outPhysTerrain = physTerrain;
-        return (physTerrain != null);
+        return ret;
+    }
+
+    // Given a terrain base, return a terrain base for a terrain that is closer to <0,0> than
+    //   this one. Usually used to return an out of bounds object to a known place.
+    private Vector3 FindAdjacentTerrainBase(Vector3 pTerrainBase)
+    {
+        Vector3 ret = pTerrainBase;
+
+        // Can't do this function if we don't know about any terrain.
+        if (m_terrains.Count == 0)
+            return ret;
+
+        // Just some sanity
+        ret.X = Util.Clamp<float>(ret.X, 0f, 1000000f);
+        ret.Y = Util.Clamp<float>(ret.Y, 0f, 1000000f);
+        ret.Z = 0f;
+
+        lock (m_terrains)
+        {
+            // Once down to the <0,0> region, we have to be done.
+            while (ret.X > 0f || ret.Y > 0f)
+            {
+                if (ret.X > 0f)
+                {
+                    ret.X = Math.Max(0f, ret.X - DefaultRegionSize.X);
+                    DetailLog("{0},BSTerrainManager.FindAdjacentTerrainBase,reducingX,terrainBase={1}", BSScene.DetailLogZero, ret);
+                    if (m_terrains.ContainsKey(ret))
+                        break;
+                }
+                if (ret.Y > 0f)
+                {
+                    ret.Y = Math.Max(0f, ret.Y - DefaultRegionSize.Y);
+                    DetailLog("{0},BSTerrainManager.FindAdjacentTerrainBase,reducingY,terrainBase={1}", BSScene.DetailLogZero, ret);
+                    if (m_terrains.ContainsKey(ret))
+                        break;
+                }
+            }
+        }
+
+        return ret;
     }
 
     // Although no one seems to check this, I do support combining.
@@ -452,7 +564,7 @@ public sealed class BSTerrainManager : IDisposable
 
     private void DetailLog(string msg, params Object[] args)
     {
-        PhysicsScene.PhysicsLogging.Write(msg, args);
+        m_physicsScene.PhysicsLogging.Write(msg, args);
     }
 }
 }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSTerrainMesh.cs b/OpenSim/Region/Physics/BulletSPlugin/BSTerrainMesh.cs
index 1d55ce3..e4ca098 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSTerrainMesh.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSTerrainMesh.cs
@@ -51,7 +51,7 @@ public sealed class BSTerrainMesh : BSTerrainPhys
     BulletShape m_terrainShape;
     BulletBody m_terrainBody;
 
-    public BSTerrainMesh(BSScene physicsScene, Vector3 regionBase, uint id, Vector3 regionSize) 
+    public BSTerrainMesh(BSScene physicsScene, Vector3 regionBase, uint id, Vector3 regionSize)
         : base(physicsScene, regionBase, id)
     {
     }
@@ -62,7 +62,7 @@ public sealed class BSTerrainMesh : BSTerrainPhys
     }
 
     // Create terrain mesh from a heightmap.
-    public BSTerrainMesh(BSScene physicsScene, Vector3 regionBase, uint id, float[] initialMap, 
+    public BSTerrainMesh(BSScene physicsScene, Vector3 regionBase, uint id, float[] initialMap,
                                                     Vector3 minCoords, Vector3 maxCoords)
         : base(physicsScene, regionBase, id)
     {
@@ -76,27 +76,43 @@ public sealed class BSTerrainMesh : BSTerrainPhys
         m_sizeX = (int)(maxCoords.X - minCoords.X);
         m_sizeY = (int)(maxCoords.Y - minCoords.Y);
 
-        if (!BSTerrainMesh.ConvertHeightmapToMesh(PhysicsScene, initialMap,
-                            m_sizeX, m_sizeY,
-                            (float)m_sizeX, (float)m_sizeY,
-                            Vector3.Zero, 1.0f,
-                            out indicesCount, out indices, out verticesCount, out vertices))
+        bool meshCreationSuccess = false;
+        if (BSParam.TerrainMeshMagnification == 1)
+        {
+            // If a magnification of one, use the old routine that is tried and true.
+            meshCreationSuccess = BSTerrainMesh.ConvertHeightmapToMesh(m_physicsScene,
+                                            initialMap, m_sizeX, m_sizeY,       // input size
+                                            Vector3.Zero,                       // base for mesh
+                                            out indicesCount, out indices, out verticesCount, out vertices);
+        }
+        else
+        {
+            // Other magnifications use the newer routine
+            meshCreationSuccess = BSTerrainMesh.ConvertHeightmapToMesh2(m_physicsScene,
+                                            initialMap, m_sizeX, m_sizeY,       // input size
+                                            BSParam.TerrainMeshMagnification,
+                                            physicsScene.TerrainManager.DefaultRegionSize,
+                                            Vector3.Zero,                       // base for mesh
+                                            out indicesCount, out indices, out verticesCount, out vertices);
+        }
+        if (!meshCreationSuccess)
         {
             // DISASTER!!
-            PhysicsScene.DetailLog("{0},BSTerrainMesh.create,failedConversionOfHeightmap", ID);
-            PhysicsScene.Logger.ErrorFormat("{0} Failed conversion of heightmap to mesh! base={1}", LogHeader, TerrainBase);
+            m_physicsScene.DetailLog("{0},BSTerrainMesh.create,failedConversionOfHeightmap,id={1}", BSScene.DetailLogZero, ID);
+            m_physicsScene.Logger.ErrorFormat("{0} Failed conversion of heightmap to mesh! base={1}", LogHeader, TerrainBase);
             // Something is very messed up and a crash is in our future.
             return;
         }
-        PhysicsScene.DetailLog("{0},BSTerrainMesh.create,meshed,indices={1},indSz={2},vertices={3},vertSz={4}", 
-                                ID, indicesCount, indices.Length, verticesCount, vertices.Length);
 
-        m_terrainShape = PhysicsScene.PE.CreateMeshShape(PhysicsScene.World, indicesCount, indices, verticesCount, vertices);
+        m_physicsScene.DetailLog("{0},BSTerrainMesh.create,meshed,id={1},indices={2},indSz={3},vertices={4},vertSz={5}",
+                                BSScene.DetailLogZero, ID, indicesCount, indices.Length, verticesCount, vertices.Length);
+
+        m_terrainShape = m_physicsScene.PE.CreateMeshShape(m_physicsScene.World, indicesCount, indices, verticesCount, vertices);
         if (!m_terrainShape.HasPhysicalShape)
         {
             // DISASTER!!
-            PhysicsScene.DetailLog("{0},BSTerrainMesh.create,failedCreationOfShape", ID);
-            physicsScene.Logger.ErrorFormat("{0} Failed creation of terrain mesh! base={1}", LogHeader, TerrainBase);
+            m_physicsScene.DetailLog("{0},BSTerrainMesh.create,failedCreationOfShape,id={1}", BSScene.DetailLogZero, ID);
+            m_physicsScene.Logger.ErrorFormat("{0} Failed creation of terrain mesh! base={1}", LogHeader, TerrainBase);
             // Something is very messed up and a crash is in our future.
             return;
         }
@@ -104,44 +120,54 @@ public sealed class BSTerrainMesh : BSTerrainPhys
         Vector3 pos = regionBase;
         Quaternion rot = Quaternion.Identity;
 
-        m_terrainBody = PhysicsScene.PE.CreateBodyWithDefaultMotionState(m_terrainShape, ID, pos, rot);
+        m_terrainBody = m_physicsScene.PE.CreateBodyWithDefaultMotionState(m_terrainShape, ID, pos, rot);
         if (!m_terrainBody.HasPhysicalBody)
         {
             // DISASTER!!
-            physicsScene.Logger.ErrorFormat("{0} Failed creation of terrain body! base={1}", LogHeader, TerrainBase);
+            m_physicsScene.Logger.ErrorFormat("{0} Failed creation of terrain body! base={1}", LogHeader, TerrainBase);
             // Something is very messed up and a crash is in our future.
             return;
         }
+        physicsScene.PE.SetShapeCollisionMargin(m_terrainShape, BSParam.TerrainCollisionMargin);
 
         // Set current terrain attributes
-        PhysicsScene.PE.SetFriction(m_terrainBody, BSParam.TerrainFriction);
-        PhysicsScene.PE.SetHitFraction(m_terrainBody, BSParam.TerrainHitFraction);
-        PhysicsScene.PE.SetRestitution(m_terrainBody, BSParam.TerrainRestitution);
-        PhysicsScene.PE.SetCollisionFlags(m_terrainBody, CollisionFlags.CF_STATIC_OBJECT);
+        m_physicsScene.PE.SetFriction(m_terrainBody, BSParam.TerrainFriction);
+        m_physicsScene.PE.SetHitFraction(m_terrainBody, BSParam.TerrainHitFraction);
+        m_physicsScene.PE.SetRestitution(m_terrainBody, BSParam.TerrainRestitution);
+        m_physicsScene.PE.SetContactProcessingThreshold(m_terrainBody, BSParam.TerrainContactProcessingThreshold);
+        m_physicsScene.PE.SetCollisionFlags(m_terrainBody, CollisionFlags.CF_STATIC_OBJECT);
 
         // Static objects are not very massive.
-        PhysicsScene.PE.SetMassProps(m_terrainBody, 0f, Vector3.Zero);
+        m_physicsScene.PE.SetMassProps(m_terrainBody, 0f, Vector3.Zero);
 
         // Put the new terrain to the world of physical objects
-        PhysicsScene.PE.AddObjectToWorld(PhysicsScene.World, m_terrainBody);
+        m_physicsScene.PE.AddObjectToWorld(m_physicsScene.World, m_terrainBody);
 
         // Redo its bounding box now that it is in the world
-        PhysicsScene.PE.UpdateSingleAabb(PhysicsScene.World, m_terrainBody);
+        m_physicsScene.PE.UpdateSingleAabb(m_physicsScene.World, m_terrainBody);
 
         m_terrainBody.collisionType = CollisionType.Terrain;
-        m_terrainBody.ApplyCollisionMask(PhysicsScene);
+        m_terrainBody.ApplyCollisionMask(m_physicsScene);
+
+        if (BSParam.UseSingleSidedMeshes)
+        {
+            m_physicsScene.DetailLog("{0},BSTerrainMesh.settingCustomMaterial,id={1}", BSScene.DetailLogZero, id);
+            m_physicsScene.PE.AddToCollisionFlags(m_terrainBody, CollisionFlags.CF_CUSTOM_MATERIAL_CALLBACK);
+        }
 
         // Make it so the terrain will not move or be considered for movement.
-        PhysicsScene.PE.ForceActivationState(m_terrainBody, ActivationState.DISABLE_SIMULATION);
+        m_physicsScene.PE.ForceActivationState(m_terrainBody, ActivationState.DISABLE_SIMULATION);
     }
 
     public override void Dispose()
     {
         if (m_terrainBody.HasPhysicalBody)
         {
-            PhysicsScene.PE.RemoveObjectFromWorld(PhysicsScene.World, m_terrainBody);
+            m_physicsScene.PE.RemoveObjectFromWorld(m_physicsScene.World, m_terrainBody);
             // Frees both the body and the shape.
-            PhysicsScene.PE.DestroyObject(PhysicsScene.World, m_terrainBody);
+            m_physicsScene.PE.DestroyObject(m_physicsScene.World, m_terrainBody);
+            m_terrainBody.Clear();
+            m_terrainShape.Clear();
         }
     }
 
@@ -159,7 +185,7 @@ public sealed class BSTerrainMesh : BSTerrainPhys
         catch
         {
             // Sometimes they give us wonky values of X and Y. Give a warning and return something.
-            PhysicsScene.Logger.WarnFormat("{0} Bad request for terrain height. terrainBase={1}, pos={2}",
+            m_physicsScene.Logger.WarnFormat("{0} Bad request for terrain height. terrainBase={1}, pos={2}",
                                                 LogHeader, TerrainBase, pos);
             ret = BSTerrainManager.HEIGHT_GETHEIGHT_RET;
         }
@@ -169,17 +195,14 @@ public sealed class BSTerrainMesh : BSTerrainPhys
     // The passed position is relative to the base of the region.
     public override float GetWaterLevelAtXYZ(Vector3 pos)
     {
-        return PhysicsScene.SimpleWaterLevel;
+        return m_physicsScene.SimpleWaterLevel;
     }
 
     // Convert the passed heightmap to mesh information suitable for CreateMeshShape2().
     // Return 'true' if successfully created.
-    public static bool ConvertHeightmapToMesh(
-                                BSScene physicsScene,
+    public static bool ConvertHeightmapToMesh( BSScene physicsScene,
                                 float[] heightMap, int sizeX, int sizeY,    // parameters of incoming heightmap
-                                float extentX, float extentY,               // zero based range for output vertices
                                 Vector3 extentBase,                         // base to be added to all vertices
-                                float magnification,                        // number of vertices to create between heightMap coords
                                 out int indicesCountO, out int[] indicesO,
                                 out int verticesCountO, out float[] verticesO)
     {
@@ -200,16 +223,15 @@ public sealed class BSTerrainMesh : BSTerrainPhys
         //    of the heightmap.
         try
         {
-            // One vertice per heightmap value plus the vertices off the top and bottom edge.
+            // One vertice per heightmap value plus the vertices off the side and bottom edge.
             int totalVertices = (sizeX + 1) * (sizeY + 1);
             vertices = new float[totalVertices * 3];
             int totalIndices = sizeX * sizeY * 6;
             indices = new int[totalIndices];
 
-            float magX = (float)sizeX / extentX;
-            float magY = (float)sizeY / extentY;
-            physicsScene.DetailLog("{0},BSTerrainMesh.ConvertHeightMapToMesh,totVert={1},totInd={2},extentBase={3},magX={4},magY={5}",
-                                    BSScene.DetailLogZero, totalVertices, totalIndices, extentBase, magX, magY);
+            if (physicsScene != null)
+                physicsScene.DetailLog("{0},BSTerrainMesh.ConvertHeightMapToMesh,totVert={1},totInd={2},extentBase={3}",
+                                    BSScene.DetailLogZero, totalVertices, totalIndices, extentBase);
             float minHeight = float.MaxValue;
             // Note that sizeX+1 vertices are created since there is land between this and the next region.
             for (int yy = 0; yy <= sizeY; yy++)
@@ -222,8 +244,8 @@ public sealed class BSTerrainMesh : BSTerrainPhys
                     if (xx == sizeX) offset -= 1;
                     float height = heightMap[offset];
                     minHeight = Math.Min(minHeight, height);
-                    vertices[verticesCount + 0] = (float)xx * magX + extentBase.X;
-                    vertices[verticesCount + 1] = (float)yy * magY + extentBase.Y;
+                    vertices[verticesCount + 0] = (float)xx + extentBase.X;
+                    vertices[verticesCount + 1] = (float)yy + extentBase.Y;
                     vertices[verticesCount + 2] = height + extentBase.Z;
                     verticesCount += 3;
                 }
@@ -250,7 +272,161 @@ public sealed class BSTerrainMesh : BSTerrainPhys
         }
         catch (Exception e)
         {
-            physicsScene.Logger.ErrorFormat("{0} Failed conversion of heightmap to mesh. For={1}/{2}, e={3}",
+            if (physicsScene != null)
+                physicsScene.Logger.ErrorFormat("{0} Failed conversion of heightmap to mesh. For={1}/{2}, e={3}",
+                                                LogHeader, physicsScene.RegionName, extentBase, e);
+        }
+
+        indicesCountO = indicesCount;
+        indicesO = indices;
+        verticesCountO = verticesCount;
+        verticesO = vertices;
+
+        return ret;
+    }
+
+    private class HeightMapGetter
+    {
+        private float[] m_heightMap;
+        private int m_sizeX;
+        private int m_sizeY;
+        public HeightMapGetter(float[] pHeightMap, int pSizeX, int pSizeY)
+        {
+            m_heightMap = pHeightMap;
+            m_sizeX = pSizeX;
+            m_sizeY = pSizeY;
+        }
+        // The heightmap is extended as an infinite plane at the last height
+        public float GetHeight(int xx, int yy)
+        {
+            int offset = 0;
+            // Extend the height with the height from the last row or column
+            if (yy >= m_sizeY)
+                if (xx >= m_sizeX)
+                    offset = (m_sizeY - 1) * m_sizeX + (m_sizeX - 1);
+                else
+                    offset = (m_sizeY - 1) * m_sizeX + xx;
+            else
+                if (xx >= m_sizeX)
+                    offset = yy * m_sizeX + (m_sizeX - 1);
+                else
+                    offset = yy * m_sizeX + xx;
+
+            return m_heightMap[offset];
+        }
+    }
+
+    // Convert the passed heightmap to mesh information suitable for CreateMeshShape2().
+    // Version that handles magnification.
+    // Return 'true' if successfully created.
+    public static bool ConvertHeightmapToMesh2( BSScene physicsScene,
+                                float[] heightMap, int sizeX, int sizeY,    // parameters of incoming heightmap
+                                int magnification,                          // number of vertices per heighmap step
+                                Vector3 extent,                             // dimensions of the output mesh
+                                Vector3 extentBase,                         // base to be added to all vertices
+                                out int indicesCountO, out int[] indicesO,
+                                out int verticesCountO, out float[] verticesO)
+    {
+        bool ret = false;
+
+        int indicesCount = 0;
+        int verticesCount = 0;
+        int[] indices = new int[0];
+        float[] vertices = new float[0];
+
+        HeightMapGetter hmap = new HeightMapGetter(heightMap, sizeX, sizeY);
+
+        // The vertices dimension of the output mesh
+        int meshX = sizeX * magnification;
+        int meshY = sizeY * magnification;
+        // The output size of one mesh step
+        float meshXStep = extent.X / meshX;
+        float meshYStep = extent.Y / meshY;
+
+        // Create an array of vertices that is meshX+1 by meshY+1 (note the loop
+        //    from zero to <= meshX). The triangle indices are then generated as two triangles
+        //    per heightmap point. There are meshX by meshY of these squares. The extra row and
+        //    column of vertices are used to complete the triangles of the last row and column
+        //    of the heightmap.
+        try
+        {
+            // Vertices for the output heightmap plus one on the side and bottom to complete triangles
+            int totalVertices = (meshX + 1) * (meshY + 1);
+            vertices = new float[totalVertices * 3];
+            int totalIndices = meshX * meshY * 6;
+            indices = new int[totalIndices];
+
+            if (physicsScene != null)
+                physicsScene.DetailLog("{0},BSTerrainMesh.ConvertHeightMapToMesh2,inSize={1},outSize={2},totVert={3},totInd={4},extentBase={5}",
+                                    BSScene.DetailLogZero, new Vector2(sizeX, sizeY), new Vector2(meshX, meshY),
+                                    totalVertices, totalIndices, extentBase);
+
+            float minHeight = float.MaxValue;
+            // Note that sizeX+1 vertices are created since there is land between this and the next region.
+            // Loop through the output vertices and compute the mediun height in between the input vertices
+            for (int yy = 0; yy <= meshY; yy++)
+            {
+                for (int xx = 0; xx <= meshX; xx++)     // Hint: the "<=" means we go around sizeX + 1 times
+                {
+                    float offsetY = (float)yy * (float)sizeY / (float)meshY;     // The Y that is closest to the mesh point
+                    int stepY = (int)offsetY;
+                    float fractionalY = offsetY - (float)stepY;
+                    float offsetX = (float)xx * (float)sizeX / (float)meshX;     // The X that is closest to the mesh point
+                    int stepX = (int)offsetX;
+                    float fractionalX = offsetX - (float)stepX;
+
+                    // physicsScene.DetailLog("{0},BSTerrainMesh.ConvertHeightMapToMesh2,xx={1},yy={2},offX={3},stepX={4},fractX={5},offY={6},stepY={7},fractY={8}",
+                    //                 BSScene.DetailLogZero, xx, yy, offsetX, stepX, fractionalX, offsetY, stepY, fractionalY);
+
+                    // get the four corners of the heightmap square the mesh point is in
+                    float heightUL = hmap.GetHeight(stepX    , stepY    );
+                    float heightUR = hmap.GetHeight(stepX + 1, stepY    );
+                    float heightLL = hmap.GetHeight(stepX    , stepY + 1);
+                    float heightLR = hmap.GetHeight(stepX + 1, stepY + 1);
+
+                    // bilinear interplolation
+                    float height = heightUL * (1 - fractionalX) * (1 - fractionalY)
+                                 + heightUR * fractionalX       * (1 - fractionalY)
+                                 + heightLL * (1 - fractionalX) * fractionalY
+                                 + heightLR * fractionalX       * fractionalY;
+
+                    // physicsScene.DetailLog("{0},BSTerrainMesh.ConvertHeightMapToMesh2,heightUL={1},heightUR={2},heightLL={3},heightLR={4},heightMap={5}",
+                    //                 BSScene.DetailLogZero, heightUL, heightUR, heightLL, heightLR, height);
+
+                    minHeight = Math.Min(minHeight, height);
+
+                    vertices[verticesCount + 0] = (float)xx * meshXStep + extentBase.X;
+                    vertices[verticesCount + 1] = (float)yy * meshYStep + extentBase.Y;
+                    vertices[verticesCount + 2] = height + extentBase.Z;
+                    verticesCount += 3;
+                }
+            }
+            // The number of vertices generated
+            verticesCount /= 3;
+
+            // Loop through all the heightmap squares and create indices for the two triangles for that square
+            for (int yy = 0; yy < meshY; yy++)
+            {
+                for (int xx = 0; xx < meshX; xx++)
+                {
+                    int offset = yy * (meshX + 1) + xx;
+                    // Each vertices is presumed to be the upper left corner of a box of two triangles
+                    indices[indicesCount + 0] = offset;
+                    indices[indicesCount + 1] = offset + 1;
+                    indices[indicesCount + 2] = offset + meshX + 1; // accounting for the extra column
+                    indices[indicesCount + 3] = offset + 1;
+                    indices[indicesCount + 4] = offset + meshX + 2;
+                    indices[indicesCount + 5] = offset + meshX + 1;
+                    indicesCount += 6;
+                }
+            }
+
+            ret = true;
+        }
+        catch (Exception e)
+        {
+            if (physicsScene != null)
+                physicsScene.Logger.ErrorFormat("{0} Failed conversion of heightmap to mesh. For={1}/{2}, e={3}",
                                                 LogHeader, physicsScene.RegionName, extentBase, e);
         }
 
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BulletSimData.cs b/OpenSim/Region/Physics/BulletSPlugin/BulletSimData.cs
index 662dd68..d5060e3 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BulletSimData.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BulletSimData.cs
@@ -104,18 +104,20 @@ public class BulletShape
 {
     public BulletShape()
     {
-        type = BSPhysicsShapeType.SHAPE_UNKNOWN;
+        shapeType = BSPhysicsShapeType.SHAPE_UNKNOWN;
         shapeKey = (System.UInt64)FixedShapeKey.KEY_NONE;
         isNativeShape = false;
     }
-    public BSPhysicsShapeType type;
+    public BSPhysicsShapeType shapeType;
     public System.UInt64 shapeKey;
     public bool isNativeShape;
 
     public virtual void Clear() { }
     public virtual bool HasPhysicalShape { get { return false; } }
+
     // Make another reference to this physical object.
     public virtual BulletShape Clone() { return new BulletShape(); }
+
     // Return 'true' if this and other refer to the same physical object
     public virtual bool ReferenceSame(BulletShape xx) { return false; }
 
@@ -131,7 +133,7 @@ public class BulletShape
         buff.Append("<p=");
         buff.Append(AddrString);
         buff.Append(",s=");
-        buff.Append(type.ToString());
+        buff.Append(shapeType.ToString());
         buff.Append(",k=");
         buff.Append(shapeKey.ToString("X"));
         buff.Append(",n=");
@@ -215,45 +217,49 @@ public static class BulletSimData
 {
 
 // Map of collisionTypes to flags for collision groups and masks.
+// An object's 'group' is the collison groups this object belongs to
+// An object's 'filter' is the groups another object has to belong to in order to collide with me
+// A collision happens if ((obj1.group & obj2.filter) != 0) || ((obj2.group & obj1.filter) != 0)
+//
 // As mentioned above, don't use the CollisionFilterGroups definitions directly in the code
 //     but, instead, use references to this dictionary. Finding and debugging
 //     collision flag problems will be made easier.
-public static Dictionary<CollisionType, CollisionTypeFilterGroup> CollisionTypeMasks 
+public static Dictionary<CollisionType, CollisionTypeFilterGroup> CollisionTypeMasks
             = new Dictionary<CollisionType, CollisionTypeFilterGroup>()
 {
-    { CollisionType.Avatar, 
-                new CollisionTypeFilterGroup(CollisionType.Avatar, 
-                                (uint)CollisionFilterGroups.BCharacterGroup, 
+    { CollisionType.Avatar,
+                new CollisionTypeFilterGroup(CollisionType.Avatar,
+                                (uint)CollisionFilterGroups.BCharacterGroup,
                                 (uint)CollisionFilterGroups.BAllGroup)
     },
-    { CollisionType.Groundplane, 
-                new CollisionTypeFilterGroup(CollisionType.Groundplane, 
-                                (uint)CollisionFilterGroups.BGroundPlaneGroup, 
+    { CollisionType.Groundplane,
+                new CollisionTypeFilterGroup(CollisionType.Groundplane,
+                                (uint)CollisionFilterGroups.BGroundPlaneGroup,
                                 (uint)CollisionFilterGroups.BAllGroup)
     },
-    { CollisionType.Terrain, 
-                new CollisionTypeFilterGroup(CollisionType.Terrain, 
-                                (uint)CollisionFilterGroups.BTerrainGroup, 
+    { CollisionType.Terrain,
+                new CollisionTypeFilterGroup(CollisionType.Terrain,
+                                (uint)CollisionFilterGroups.BTerrainGroup,
                                 (uint)(CollisionFilterGroups.BAllGroup & ~CollisionFilterGroups.BStaticGroup))
     },
-    { CollisionType.Static, 
-                new CollisionTypeFilterGroup(CollisionType.Static, 
-                                (uint)CollisionFilterGroups.BStaticGroup, 
+    { CollisionType.Static,
+                new CollisionTypeFilterGroup(CollisionType.Static,
+                                (uint)CollisionFilterGroups.BStaticGroup,
                                 (uint)(CollisionFilterGroups.BCharacterGroup | CollisionFilterGroups.BSolidGroup))
     },
-    { CollisionType.Dynamic, 
-                new CollisionTypeFilterGroup(CollisionType.Dynamic, 
-                                (uint)CollisionFilterGroups.BSolidGroup, 
+    { CollisionType.Dynamic,
+                new CollisionTypeFilterGroup(CollisionType.Dynamic,
+                                (uint)CollisionFilterGroups.BSolidGroup,
                                 (uint)(CollisionFilterGroups.BAllGroup))
     },
-    { CollisionType.VolumeDetect, 
-                new CollisionTypeFilterGroup(CollisionType.VolumeDetect, 
-                                (uint)CollisionFilterGroups.BSensorTrigger, 
+    { CollisionType.VolumeDetect,
+                new CollisionTypeFilterGroup(CollisionType.VolumeDetect,
+                                (uint)CollisionFilterGroups.BSensorTrigger,
                                 (uint)(~CollisionFilterGroups.BSensorTrigger))
     },
     { CollisionType.LinksetChild,
-                new CollisionTypeFilterGroup(CollisionType.LinksetChild, 
-                                (uint)CollisionFilterGroups.BLinksetChildGroup, 
+                new CollisionTypeFilterGroup(CollisionType.LinksetChild,
+                                (uint)CollisionFilterGroups.BLinksetChildGroup,
                                 (uint)(CollisionFilterGroups.BNoneGroup))
                                 // (uint)(CollisionFilterGroups.BCharacterGroup | CollisionFilterGroups.BSolidGroup))
     },
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BulletSimTODO.txt b/OpenSim/Region/Physics/BulletSPlugin/BulletSimTODO.txt
index a8a4ff5..5792ae6 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BulletSimTODO.txt
+++ b/OpenSim/Region/Physics/BulletSPlugin/BulletSimTODO.txt
@@ -1,68 +1,113 @@
+PROBLEMS TO LOOK INTO
+=================================================
+Nebadon vehicle ride, get up, ride again. Second time vehicle does not act correctly.
+        Have to rez new vehicle and delete the old to fix situation.
+Hitting RESET on Nebadon's vehicle while riding causes vehicle to get into odd
+        position state where it will not settle onto ground properly, etc
+Two of Nebadon vehicles in a sim max the CPU. This is new.
+A sitting, active vehicle bobs up and down a small amount.
+
 CURRENT PRIORITIES
 =================================================
-Redo BulletSimAPI to allow native C# implementation of Bullet option.
-Avatar movement
-        flying into a wall doesn't stop avatar who keeps appearing to move through the obstacle
-        walking up stairs is not calibrated correctly (stairs out of Kepler cabin)
-        avatar capsule rotation completed
-llMoveToTarget
+Use the HACD convex hull routine in Bullet rather than the C# version.  
+        Speed up hullifying large meshes.
 Enable vehicle border crossings (at least as poorly as ODE)
         Terrain skirts
         Avatar created in previous region and not new region when crossing border
         Vehicle recreated in new sim at small Z value (offset from root value?) (DONE)
-Vehicle movement on terrain smoothness
+Deleting a linkset while standing on the root will leave the physical shape of the root behind.
+        Not sure if it is because standing on it. Done with large prim linksets.
+Linkset child rotations.
+        Nebadon spiral tube has middle sections which are rotated wrong.
+        Select linked spiral tube. Delink and note where the middle section ends up.
+Refarb compound linkset creation to create a pseudo-root for center-of-mass
+        Let children change their shape to physical indendently and just add shapes to compound
+Vehicle angular vertical attraction
+vehicle angular banking
+Center-of-gravity
+Vehicle angular deflection
+        Preferred orientation angular correction fix
+when should angular and linear motor targets be zeroed? when selected?
+        Need a vehicle.clear()? Or an 'else' in prestep if not physical.
+Teravus llMoveToTarget script debug
+        Mixing of hover, buoyancy/gravity, moveToTarget, into one force
+        Setting hover height to zero disables hover even if hover flags are on (from SL wiki)
+limitMotorUp calibration (more down?)
+llRotLookAt
+llLookAt
+Avatars walking up stairs (HALF DONE)
+Avatar movement
+        flying into a wall doesn't stop avatar who keeps appearing to move through the obstacle (DONE)
+        walking up stairs is not calibrated correctly (stairs out of Kepler cabin) (DONE)
+        avatar capsule rotation completed (NOT DONE - Bullet's capsule shape is not the solution)
 Vehicle script tuning/debugging
         Avanti speed script
         Weapon shooter script
-limitMotorUp calibration (more down?)
-Boats float low in the water
+Move material definitions (friction, ...) into simulator.
 Add material densities to the material types.
-
-CRASHES
-=================================================
-20121129.1411: editting/moving phys object across region boundries causes crash
-    getPos-> btRigidBody::upcast -> getBodyType -> BOOM
-20121128.1600: mesh object not rezzing (no physics mesh).
-    Causes many errors. Doesn't stop after first error with box shape.
-    Eventually crashes when deleting the object.
-20121206.1434: rez Sam-pan into OSGrid BulletSim11 region
-        Immediate simulator crash. Mono does not output any stacktrace and
-        log just stops after reporting taint-time linking of the linkset.
+One sided meshes? Should terrain be built into a closed shape?
+        When meshes get partially wedged into the terrain, they cannot push themselves out.
+        It is possible that Bullet processes collisions whether entering or leaving a mesh.
+        Ref: http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?t=4869
 
 VEHICLES TODO LIST:
 =================================================
-Angular motor direction is global coordinates rather than local coordinates
+LINEAR_MOTOR_DIRECTION values should be clamped to reasonable numbers.
+        What are the limits in SL?
+        Same for other velocity settings.
+UBit improvements to remove rubber-banding of avatars sitting on vehicle child prims:
+        https://github.com/UbitUmarov/Ubit-opensim
 Border crossing with linked vehicle causes crash
+        20121129.1411: editting/moving phys object across region boundries causes crash
+            getPos-> btRigidBody::upcast -> getBodyType -> BOOM
 Vehicles (Move smoothly)
-Add vehicle collisions so IsColliding is properly reported.
-        Needed for banking, limitMotorUp, movementLimiting, ...
-VehicleAddForce is not scaled by the simulation step but it is only
-        applied for one step. Should it be scaled?
 Some vehicles should not be able to turn if no speed or off ground.
+What to do if vehicle and prim buoyancy differ?
 Cannot edit/move a vehicle being ridden: it jumps back to the origional position.
 Neb car jiggling left and right
         Happens on terrain and any other mesh object. Flat cubes are much smoother.
         This has been reduced but not eliminated.
 Implement referenceFrame for all the motion routines.
-Angular motion around Z moves the vehicle in world Z and not vehicle Z in ODE.
-        Verify that angular motion specified around Z moves in the vehicle coordinates.
+For limitMotorUp, use raycast down to find if vehicle is in the air.
 Verify llGetVel() is returning a smooth and good value for vehicle movement.
 llGetVel() should return the root's velocity if requested in a child prim.
 Implement function efficiency for lineaar and angular motion.
 After getting off a vehicle, the root prim is phantom (can be walked through)
     Need to force a position update for the root prim after compound shape destruction
 Linkset explosion after three "rides" on Nebadon lite vehicle (LinksetConstraint)
-For limitMotorUp, use raycast down to find if vehicle is in the air.
 Remove vehicle angular velocity zeroing in BSPrim.UpdateProperties().
         A kludge that isn't fixing the real problem of Bullet adding extra motion.
 Incorporate inter-relationship of angular corrections. For instance, angularDeflection
         and angularMotorUp will compute same X or Y correction. When added together
         creates over-correction and over-shoot and wabbling.
+Vehicle attributes are not restored when a vehicle is rezzed on region creation
+        Create vehicle, setup vehicle properties, restart region, vehicle is not reinitialized.
 
-BULLETSIM TODO LIST:
+GENERAL TODO LIST:
 =================================================
+Explore btGImpactMeshShape as alternative to convex hulls for simplified physical objects.
+        Regular triangle meshes don't do physical collisions.
+Resitution of a prim works on another prim but not on terrain.
+        The dropped prim doesn't bounce properly on the terrain.
+Add a sanity check for PIDTarget location.
+Level-of-detail for mesh creation. Prims with circular interiors require lod of 32.
+        Is much saved with lower LODs? At the moment, all set to 32.
+Collisions are inconsistant: arrows are supposed to hit and report collision. Often don't.
+        If arrow show at prim, collision reported about 1/3 of time. If collision reported,
+        both arrow and prim report it. The arrow bounces off the prim 9 out of 10 times.
+        Shooting 5m sphere "arrows" at 60m/s.
+llMoveToTarget objects are not effected by gravity until target is removed.
+Compute CCD parameters based on body size
+Can solver iterations be changed per body/shape? Can be for constraints but what
+        about regular vehicles?
+Implement llSetPhysicalMaterial.
+        extend it with Center-of-mass, rolling friction, density
+Implement llSetForceAndTorque.
+Change BSPrim.moveToTarget to used forces rather than changing position
+        Changing position allows one to move through walls
 Implement an avatar mesh shape. The Bullet capsule is way too limited.
         Consider just hand creating a vertex/index array in a new BSShapeAvatar.
+Verify/fix phantom, volume-detect objects do not fall to infinity. Should stop at terrain.
 Revisit CollisionMargin. Builders notice the 0.04 spacing between prims.
 Duplicating a physical prim causes old prim to jump away
         Dup a phys prim and the original become unselected and thus interacts w/ selected prim.
@@ -86,6 +131,8 @@ setForce should set a constant force. Different than AddImpulse.
 Implement raycast.
 Implement ShapeCollection.Dispose()
 Implement water as a plain so raycasting and collisions can happen with same.
+Add collision penetration return
+        Add field passed back by BulletSim.dll and fill with info in ManifoldConstact.GetDistance()
 Add osGetPhysicsEngineName() so scripters can tell whether BulletSim or ODE
         Also osGetPhysicsEngineVerion() maybe.
 Linkset.Position and Linkset.Orientation requre rewrite to properly return
@@ -107,6 +154,12 @@ Physical and phantom will drop through the terrain
 
 LINKSETS
 ======================================================
+Child prims do not report collisions
+Allow children of a linkset to be phantom:
+        http://opensim-dev.2196679.n2.nabble.com/Setting-a-single-child-prim-to-Phantom-tp7578513.html
+        Add OS_STATUS_PHANTOM_PRIM to llSetLinkPrimitaveParamsFast.
+Editing a child of a linkset causes the child to go phantom
+        Move a child prim once when it is physical and can never move it again without it going phantom
 Offset the center of the linkset to be the geometric center of all the prims
         Not quite the same as the center-of-gravity
 Linksets should allow collisions to individual children
@@ -117,11 +170,9 @@ LinksetCompound: when one of the children changes orientation (like tires
 Verify/think through scripts in children of linksets. What do they reference
         and return when getting position, velocity, ...
 Confirm constraint linksets still work after making all the changes for compound linksets.
+Use PostTaint callback to do rebuilds for constraint linksets to reduce rebuilding
 Add 'changed' flag or similar to reduce the number of times a linkset is rebuilt.
         For compound linksets, add ability to remove or reposition individual child shapes.
-Disable activity of passive linkset children.
-        Since the linkset is a compound object, the old prims are left lying
-        around and need to be phantomized so they don't collide, ...
 Speed up creation of large physical linksets
         For instance, sitting in Neb's car (130 prims) takes several seconds to become physical.
         REALLY bad for very large physical linksets (freezes the sim for many seconds).
@@ -131,25 +182,28 @@ Eliminate collisions between objects in a linkset. (LinksetConstraint)
 
 MORE
 ======================================================
-Test avatar walking up stairs. How does compare with SL.
-    Radius of the capsule affects ability to climb edges.
+Compute avatar size and scale correctly. Now it is a bit off from the capsule size.
+Create tests for different interface components
+        Have test objects/scripts measure themselves and turn color if correct/bad
+        Test functions in SL and calibrate correctness there
+        Create auto rezzer and tracker to run through the tests
+Do we need to do convex hulls all the time? Can complex meshes be left meshes?
+        There is some problem with meshes and collisions
+        Hulls are not as detailed as meshes. Hulled vehicles insides are different shape.
 Debounce avatar contact so legs don't keep folding up when standing.
 Implement LSL physics controls. Like STATUS_ROTATE_X.
 Add border extensions to terrain to help region crossings and objects leaving region.
 Use a different capsule shape for avatar when sitting
         LL uses a pyrimidal shape scaled by the avatar's bounding box
         http://wiki.secondlife.com/wiki/File:Avmeshforms.png
-
 Performance test with lots of avatars. Can BulletSim support a thousand?
 Optimize collisions in C++: only send up to the object subscribed to collisions.
     Use collision subscription and remove the collsion(A,B) and collision(B,A)
 Check whether SimMotionState needs large if statement (see TODO).
-
 Implement 'top colliders' info.
 Avatar jump
 Performance measurement and changes to make quicker.
 Implement detailed physics stats (GetStats()).
-
 Measure performance improvement from hulls
 Test not using ghost objects for volume detect implementation.
 Performance of closures and delegates for taint processing
@@ -157,9 +211,7 @@ Performance of closures and delegates for taint processing
         Is any slowdown introduced by the existing implementation significant?
 Is there are more efficient method of implementing pre and post step actions?
         See http://www.codeproject.com/Articles/29922/Weak-Events-in-C
-
 Physics Arena central pyramid: why is one side permiable?
-
 In SL, perfect spheres don't seem to have rolling friction. Add special case.
 Enforce physical parameter min/max:
         Gravity: [-1, 28]
@@ -168,9 +220,12 @@ Enforce physical parameter min/max:
         Restitution [0, 1]
         http://wiki.secondlife.com/wiki/Physics_Material_Settings_test
 Avatar attachments have no mass? http://forums-archive.secondlife.com/54/f0/31796/1.html
+Keep avatar scaling correct. http://pennycow.blogspot.fr/2011/07/matter-of-scale.html
 
 INTERNAL IMPROVEMENT/CLEANUP
 =================================================
+Can the 'inTaintTime' flag be cleaned up and used? For instance, a call to
+        BSScene.TaintedObject() could immediately execute the callback if already in taint time.
 Create the physical wrapper classes (BulletBody, BulletShape) by methods on
         BSAPITemplate and make their actual implementation Bullet engine specific.
         For the short term, just call the existing functions in ShapeCollection.
@@ -190,22 +245,19 @@ Generalize Dynamics and PID with standardized motors.
 Generalize Linkset and vehicles into PropertyManagers
     Methods for Refresh, RemoveBodyDependencies, RestoreBodyDependencies
         Potentially add events for shape destruction, etc.
-Complete implemention of preStepActions
-        Replace vehicle step call with prestep event.
-        Is there a need for postStepActions? postStepTaints?
+Better mechanism for resetting linkset set and vehicle parameters when body rebuilt.
+        BSPrim.CreateGeomAndObject is kludgy with the callbacks, etc.
 Implement linkset by setting position of children when root updated. (LinksetManual)
         Linkset implementation using manual prim movement.
 LinkablePrim class? Would that simplify/centralize the linkset logic?
 BSScene.UpdateParameterSet() is broken. How to set params on objects?
-Remove HeightmapInfo from terrain specification
-    Since C++ code does not need terrain height, this structure et al are not needed.
 Add floating motor for BS_FLOATS_ON_WATER so prim and avatar will
-    bob at the water level. BSPrim.PositionSanityCheck().
+    bob at the water level. BSPrim.PositionSanityCheck()
 Should taints check for existance or activeness of target?
         When destroying linksets/etc, taints can be generated for objects that are
         actually gone when the taint happens. Crashes don't happen because the taint closure
         keeps the object from being freed, but that is just an accident.
-        Possibly have and 'active' flag that is checked by the taint processor?
+        Possibly have an 'active' flag that is checked by the taint processor?
 Parameters for physics logging should be moved from BSScene to BSParam (at least boolean ones)
 Can some of the physical wrapper classes (BulletBody, BulletWorld, BulletShape) be 'sealed'?
 There are TOO MANY interfaces from BulletSim core to Bullet itself
@@ -270,3 +322,43 @@ llSetBuoyancy()	(DONE)
         (Resolution: Bullet resets object gravity when added to world. Moved set gravity)
 Avatar density is WAY off. Compare and calibrate with what's in SL. (DONE)
         (Resolution: set default density to 3.5 (from 60) which is closer to SL)
+Redo BulletSimAPI to allow native C# implementation of Bullet option (DONE)
+        (Resolution: added BSAPITemplate and then interfaces for C++ Bullet and C# BulletXNA
+Meshes rendering as bounding boxes (DONE)
+        (Resolution: Added test for mesh/sculpties in native shapes so it didn't think it was a box)
+llMoveToTarget (Resolution: added simple motor to update the position.)
+Angular motor direction is global coordinates rather than local coordinates (DONE)
+Add vehicle collisions so IsColliding is properly reported. (DONE)
+        Needed for banking, limitMotorUp, movementLimiting, ...
+        (Resolution: added CollisionFlags.BS_VEHICLE_COLLISION and code to use it)
+VehicleAddForce is not scaled by the simulation step but it is only
+        applied for one step. Should it be scaled? (DONE)
+        (Resolution: use force for timed things, Impulse for immediate, non-timed things)
+Complete implemention of preStepActions (DONE)
+        Replace vehicle step call with prestep event.
+        Is there a need for postStepActions? postStepTaints?
+Disable activity of passive linkset children. (DONE)
+        Since the linkset is a compound object, the old prims are left lying
+        around and need to be phantomized so they don't collide, ...
+Remove HeightmapInfo from terrain specification (DONE)
+    Since C++ code does not need terrain height, this structure et al are not needed.
+Surfboard go wonky when turning (DONE)
+        Angular motor direction is global coordinates rather than local coordinates?
+        (Resolution: made angular motor direction correct coordinate system)
+Mantis 6040 script http://opensimulator.org/mantis/view.php?id=6040 (DONE)
+        Msg Kayaker on OSGrid when working
+        (Resolution: LINEAR_DIRECTION is in vehicle coords. Test script does the
+        same in SL as in OS/BulletSim)
+Boats float low in the water (DONE)
+Boats floating at proper level (DONE)
+When is force introduced by SetForce removed? The prestep action could go forever. (DONE)
+        (Resolution: setForce registers a prestep action which keeps applying the force)
+Child movement in linkset (don't rebuild linkset) (DONE 20130122))
+Avatar standing on a moving object should start to move with the object. (DONE 20130125)
+Angular motion around Z moves the vehicle in world Z and not vehicle Z in ODE.
+        Verify that angular motion specified around Z moves in the vehicle coordinates.
+        DONE 20130120: BulletSim properly applies force in vehicle relative coordinates.
+Nebadon vehicles turning funny in arena (DONE)
+Lock axis (DONE 20130401)
+Terrain detail: double terrain mesh detail (DONE)
+
diff --git a/OpenSim/Region/Physics/BulletSPlugin/Properties/AssemblyInfo.cs b/OpenSim/Region/Physics/BulletSPlugin/Properties/AssemblyInfo.cs
index 0d1db3b..02b03a8 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/Properties/AssemblyInfo.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/Properties/AssemblyInfo.cs
@@ -29,5 +29,5 @@ using System.Runtime.InteropServices;
 //      Build Number
 //      Revision
 //
-[assembly: AssemblyVersion("0.7.5.*")]
-[assembly: AssemblyFileVersion("1.0.0.0")]
+[assembly: AssemblyVersion("0.7.6.*")]
+
diff --git a/bin/lib32/BulletSim.dll b/bin/lib32/BulletSim.dll
index 1f41e04..1e94d05 100755
--- a/bin/lib32/BulletSim.dll
+++ b/bin/lib32/BulletSim.dll
diff --git a/bin/lib32/libBulletSim.dylib b/bin/lib32/libBulletSim.dylib
new file mode 100755
index 0000000..02f8a7f
--- /dev/null
+++ b/bin/lib32/libBulletSim.dylib
diff --git a/bin/lib32/libBulletSim.so b/bin/lib32/libBulletSim.so
index 85ff318..6216a65 100755
--- a/bin/lib32/libBulletSim.so
+++ b/bin/lib32/libBulletSim.so
diff --git a/bin/lib64/BulletSim.dll b/bin/lib64/BulletSim.dll
index 674af3e..68cfcc4 100755
--- a/bin/lib64/BulletSim.dll
+++ b/bin/lib64/BulletSim.dll
diff --git a/bin/lib64/libBulletSim.so b/bin/lib64/libBulletSim.so
index 14892a4..08c00af 100755
--- a/bin/lib64/libBulletSim.so
+++ b/bin/lib64/libBulletSim.so