60 files changed, 12005 insertions, 3675 deletions

diff --git a/OpenSim/Region/Physics/BasicPhysicsPlugin/AssemblyInfo.cs b/OpenSim/Region/Physics/BasicPhysicsPlugin/AssemblyInfo.cs
index fb9cb66..6fd6f7e 100644
--- a/OpenSim/Region/Physics/BasicPhysicsPlugin/AssemblyInfo.cs
+++ b/OpenSim/Region/Physics/BasicPhysicsPlugin/AssemblyInfo.cs
@@ -55,4 +55,4 @@ using System.Runtime.InteropServices;
 // You can specify all values by your own or you can build default build and revision
 // numbers with the '*' character (the default):
 
-[assembly : AssemblyVersion("0.7.5.*")]
+[assembly : AssemblyVersion("0.7.6.*")]
diff --git a/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsPlugin.cs b/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsPlugin.cs
index 7ab2a03..373c7e0 100644
--- a/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsPlugin.cs
+++ b/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsPlugin.cs
@@ -49,7 +49,7 @@ namespace OpenSim.Region.Physics.BasicPhysicsPlugin
 
         public PhysicsScene GetScene(string sceneIdentifier)
         {
-            return new BasicScene(sceneIdentifier);
+            return new BasicScene(GetName(), sceneIdentifier);
         }
 
         public string GetName()
diff --git a/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsScene.cs b/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsScene.cs
index f5826ed..0816b7b 100644
--- a/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsScene.cs
+++ b/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsScene.cs
@@ -49,8 +49,10 @@ namespace OpenSim.Region.Physics.BasicPhysicsPlugin
 
         //protected internal string sceneIdentifier;
 
-        public BasicScene(string _sceneIdentifier)
+        public BasicScene(string engineType, string _sceneIdentifier)
         {
+            EngineType = engineType;
+            Name = EngineType + "/" + _sceneIdentifier;
             //sceneIdentifier = _sceneIdentifier;
         }
 
@@ -100,6 +102,8 @@ namespace OpenSim.Region.Physics.BasicPhysicsPlugin
 
         public override float Simulate(float timeStep)
         {
+//            Console.WriteLine("Simulating");
+
             float fps = 0;
             for (int i = 0; i < _actors.Count; ++i)
             {
@@ -107,8 +111,11 @@ namespace OpenSim.Region.Physics.BasicPhysicsPlugin
                 Vector3 actorPosition = actor.Position;
                 Vector3 actorVelocity = actor.Velocity;
 
-                actorPosition.X += actor.Velocity.X*timeStep;
-                actorPosition.Y += actor.Velocity.Y*timeStep;
+//                Console.WriteLine(
+//                    "Processing actor {0}, starting pos {1}, starting vel {2}", i, actorPosition, actorVelocity);
+
+                actorPosition.X += actor.Velocity.X * timeStep;
+                actorPosition.Y += actor.Velocity.Y * timeStep;
 
                 if (actor.Position.Y < 0)
                 {
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSAPIUnman.cs b/OpenSim/Region/Physics/BulletSPlugin/BSAPIUnman.cs
new file mode 100755
index 0000000..77ea3ed
--- /dev/null
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSAPIUnman.cs
@@ -0,0 +1,1987 @@
+/*
+ * 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.Reflection;
+using System.Runtime.InteropServices;
+using System.Security;
+using System.Text;
+
+using OpenSim.Framework;
+
+using OpenMetaverse;
+
+namespace OpenSim.Region.Physics.BulletSPlugin
+{
+public sealed class BSAPIUnman : BSAPITemplate
+{
+
+private sealed class BulletWorldUnman : BulletWorld
+{
+    public IntPtr ptr;
+    public BulletWorldUnman(uint id, BSScene physScene, IntPtr xx)
+        : base(id, physScene)
+    {
+        ptr = xx;
+    }
+}
+
+private sealed class BulletBodyUnman : BulletBody
+{
+    public IntPtr ptr;
+    public BulletBodyUnman(uint id, IntPtr xx)
+        : base(id)
+    {
+        ptr = xx;
+    }
+    public override bool HasPhysicalBody
+    {
+        get { return ptr != IntPtr.Zero; }
+    }
+    public override void Clear()
+    {
+        ptr = IntPtr.Zero;
+    }
+    public override string AddrString
+    {
+        get { return ptr.ToString("X"); }
+    }
+}
+
+private sealed class BulletShapeUnman : BulletShape
+{
+    public IntPtr ptr;
+    public BulletShapeUnman(IntPtr xx, BSPhysicsShapeType typ) 
+        : base()
+    {
+        ptr = xx;
+        type = typ;
+    }
+    public override bool HasPhysicalShape
+    {
+        get { return ptr != IntPtr.Zero; }
+    }
+    public override void Clear()
+    {
+        ptr = IntPtr.Zero;
+    }
+    public override BulletShape Clone()
+    {
+        return new BulletShapeUnman(ptr, type);
+    }
+    public override bool ReferenceSame(BulletShape other)
+    {
+        BulletShapeUnman otheru = other as BulletShapeUnman;
+        return (otheru != null) && (this.ptr == otheru.ptr);
+
+    }
+    public override string AddrString
+    {
+        get { return ptr.ToString("X"); }
+    }
+}
+private sealed class BulletConstraintUnman : BulletConstraint
+{
+    public BulletConstraintUnman(IntPtr xx) : base()
+    {
+        ptr = xx;
+    }
+    public IntPtr ptr;
+
+    public override void Clear()
+    {
+        ptr = IntPtr.Zero;
+    }
+    public override bool HasPhysicalConstraint { get { return ptr != IntPtr.Zero; } }
+
+    // Used for log messages for a unique display of the memory/object allocated to this instance
+    public override string AddrString
+    {
+        get { return ptr.ToString("X"); }
+    }
+}
+
+// We pin the memory passed between the managed and unmanaged code.
+GCHandle m_paramsHandle;
+private GCHandle m_collisionArrayPinnedHandle;
+private GCHandle m_updateArrayPinnedHandle;
+
+// Handle to the callback used by the unmanaged code to call into the managed code.
+// Used for debug logging.
+// Need to store the handle in a persistant variable so it won't be freed.
+private BSAPICPP.DebugLogCallback m_DebugLogCallbackHandle;
+
+private BSScene PhysicsScene { get; set; }
+
+public override string BulletEngineName { get { return "BulletUnmanaged"; } }
+public override string BulletEngineVersion { get; protected set; }
+
+public BSAPIUnman(string paramName, BSScene physScene)
+{
+    PhysicsScene = physScene;
+
+    // Do something fancy with the paramName to get the right DLL implementation
+    //     like "Bullet-2.80-OpenCL-Intel" loading the version for Intel based OpenCL implementation, etc.
+    if (Util.IsWindows())
+        Util.LoadArchSpecificWindowsDll("BulletSim.dll");
+    // If not Windows, loading is performed by the
+    // Mono loader as specified in
+    // "bin/Physics/OpenSim.Region.Physics.BulletSPlugin.dll.config".
+}
+
+// Initialization and simulation
+public override BulletWorld Initialize(Vector3 maxPosition, ConfigurationParameters parms,
+                                                                                        int maxCollisions,  ref CollisionDesc[] collisionArray,
+                                                                                        int maxUpdates, ref EntityProperties[] updateArray
+                                            )
+{
+    // Pin down the memory that will be used to pass object collisions and updates back from unmanaged code
+    m_paramsHandle = GCHandle.Alloc(parms, GCHandleType.Pinned);
+    m_collisionArrayPinnedHandle = GCHandle.Alloc(collisionArray, GCHandleType.Pinned);
+    m_updateArrayPinnedHandle = GCHandle.Alloc(updateArray, GCHandleType.Pinned);
+
+    // If Debug logging level, enable logging from the unmanaged code
+    m_DebugLogCallbackHandle = null;
+    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)
+            // The handle is saved in a variable to make sure it doesn't get freed after this call
+            m_DebugLogCallbackHandle = new BSAPICPP.DebugLogCallback(BulletLoggerPhysLog);
+        else
+            m_DebugLogCallbackHandle = new BSAPICPP.DebugLogCallback(BulletLogger);
+    }
+
+    // Get the version of the DLL
+    // TODO: this doesn't work yet. Something wrong with marshaling the returned string.
+    // BulletEngineVersion = BulletSimAPI.GetVersion2();
+    BulletEngineVersion = "";
+
+    // Call the unmanaged code with the buffers and other information
+    return new BulletWorldUnman(0, PhysicsScene, BSAPICPP.Initialize2(maxPosition, m_paramsHandle.AddrOfPinnedObject(),
+                                    maxCollisions, m_collisionArrayPinnedHandle.AddrOfPinnedObject(),
+                                    maxUpdates, m_updateArrayPinnedHandle.AddrOfPinnedObject(),
+                                    m_DebugLogCallbackHandle));
+
+}
+
+// Called directly from unmanaged code so don't do much
+private void BulletLogger(string msg)
+{
+    BSScene.m_log.Debug("[BULLETS UNMANAGED]:" + msg);
+}
+
+// Called directly from unmanaged code so don't do much
+private void BulletLoggerPhysLog(string msg)
+{
+    PhysicsScene.DetailLog("[BULLETS UNMANAGED]:" + msg);
+}
+
+public override int PhysicsStep(BulletWorld world, float timeStep, int maxSubSteps, float fixedTimeStep,
+                                        out int updatedEntityCount, out int collidersCount)
+{
+    BulletWorldUnman worldu = world as BulletWorldUnman;
+    return BSAPICPP.PhysicsStep2(worldu.ptr, timeStep, maxSubSteps, fixedTimeStep, out updatedEntityCount, out collidersCount);
+}
+
+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)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.PushUpdate2(bodyu.ptr);
+}
+
+public override bool UpdateParameter(BulletWorld world, uint localID, String parm, float value)
+{
+    BulletWorldUnman worldu = world as BulletWorldUnman;
+    return BSAPICPP.UpdateParameter2(worldu.ptr, localID, parm, value);
+}
+
+// =====================================================================================
+// Mesh, hull, shape and body creation helper routines
+public override BulletShape CreateMeshShape(BulletWorld world,
+                int indicesCount, int[] indices,
+                int verticesCount, float[] vertices)
+{
+    BulletWorldUnman worldu = world as BulletWorldUnman;
+    return new BulletShapeUnman(
+                    BSAPICPP.CreateMeshShape2(worldu.ptr, indicesCount, indices, verticesCount, vertices),
+                    BSPhysicsShapeType.SHAPE_MESH);
+}
+
+public override BulletShape CreateHullShape(BulletWorld world, int hullCount, float[] hulls)
+{
+    BulletWorldUnman worldu = world as BulletWorldUnman;
+    return new BulletShapeUnman(
+                    BSAPICPP.CreateHullShape2(worldu.ptr, hullCount, hulls), 
+                    BSPhysicsShapeType.SHAPE_HULL);
+}
+
+public override BulletShape BuildHullShapeFromMesh(BulletWorld world, BulletShape meshShape)
+{
+    BulletWorldUnman worldu = world as BulletWorldUnman;
+    BulletShapeUnman shapeu = meshShape as BulletShapeUnman;
+    return new BulletShapeUnman(
+                    BSAPICPP.BuildHullShapeFromMesh2(worldu.ptr, shapeu.ptr),
+                    BSPhysicsShapeType.SHAPE_HULL);
+}
+
+public override BulletShape BuildNativeShape(BulletWorld world, ShapeData shapeData)
+{
+    BulletWorldUnman worldu = world as BulletWorldUnman;
+    return new BulletShapeUnman(BSAPICPP.BuildNativeShape2(worldu.ptr, shapeData), shapeData.Type);
+}
+
+public override bool IsNativeShape(BulletShape shape)
+{
+    BulletShapeUnman shapeu = shape as BulletShapeUnman;
+    if (shapeu != null && shapeu.HasPhysicalShape)
+        return BSAPICPP.IsNativeShape2(shapeu.ptr);
+    return false;
+}
+
+public override void SetShapeCollisionMargin(BulletShape shape, float margin)
+{
+    BulletShapeUnman shapeu = shape as BulletShapeUnman;
+    if (shapeu != null && shapeu.HasPhysicalShape)
+        BSAPICPP.SetShapeCollisionMargin(shapeu.ptr, margin);
+}
+
+public override BulletShape BuildCapsuleShape(BulletWorld world, float radius, float height, Vector3 scale)
+{
+    BulletWorldUnman worldu = world as BulletWorldUnman;
+    return new BulletShapeUnman(
+                   BSAPICPP.BuildCapsuleShape2(worldu.ptr, radius, height, scale),
+                   BSPhysicsShapeType.SHAPE_CAPSULE);
+}
+
+public override BulletShape CreateCompoundShape(BulletWorld world, bool enableDynamicAabbTree)
+{
+    BulletWorldUnman worldu = world as BulletWorldUnman;
+    return new BulletShapeUnman(
+                    BSAPICPP.CreateCompoundShape2(worldu.ptr, enableDynamicAabbTree),
+                    BSPhysicsShapeType.SHAPE_COMPOUND);
+
+}
+
+public override int GetNumberOfCompoundChildren(BulletShape shape)
+{
+    BulletShapeUnman shapeu = shape as BulletShapeUnman;
+    if (shapeu != null && shapeu.HasPhysicalShape)
+        return BSAPICPP.GetNumberOfCompoundChildren2(shapeu.ptr);
+    return 0;
+}
+
+public override void AddChildShapeToCompoundShape(BulletShape shape, BulletShape addShape, Vector3 pos, Quaternion rot)
+{
+    BulletShapeUnman shapeu = shape as BulletShapeUnman;
+    BulletShapeUnman addShapeu = addShape as BulletShapeUnman;
+    BSAPICPP.AddChildShapeToCompoundShape2(shapeu.ptr, addShapeu.ptr, pos, rot);
+}
+
+public override BulletShape GetChildShapeFromCompoundShapeIndex(BulletShape shape, int indx)
+{
+    BulletShapeUnman shapeu = shape as BulletShapeUnman;
+    return new BulletShapeUnman(BSAPICPP.GetChildShapeFromCompoundShapeIndex2(shapeu.ptr, indx), BSPhysicsShapeType.SHAPE_UNKNOWN);
+}
+
+public override BulletShape RemoveChildShapeFromCompoundShapeIndex(BulletShape shape, int indx)
+{
+    BulletShapeUnman shapeu = shape as BulletShapeUnman;
+    return new BulletShapeUnman(BSAPICPP.RemoveChildShapeFromCompoundShapeIndex2(shapeu.ptr, indx), BSPhysicsShapeType.SHAPE_UNKNOWN);
+}
+
+public override void RemoveChildShapeFromCompoundShape(BulletShape shape, BulletShape removeShape)
+{
+    BulletShapeUnman shapeu = shape as BulletShapeUnman;
+    BulletShapeUnman removeShapeu = removeShape as BulletShapeUnman;
+    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;
+    BSAPICPP.RecalculateCompoundShapeLocalAabb2(shapeu.ptr);
+}
+
+public override BulletShape DuplicateCollisionShape(BulletWorld world, BulletShape srcShape, uint id)
+{
+    BulletWorldUnman worldu = world as BulletWorldUnman;
+    BulletShapeUnman srcShapeu = srcShape as BulletShapeUnman;
+    return new BulletShapeUnman(BSAPICPP.DuplicateCollisionShape2(worldu.ptr, srcShapeu.ptr, id), srcShape.type);
+}
+
+public override bool DeleteCollisionShape(BulletWorld world, BulletShape shape)
+{
+    BulletWorldUnman worldu = world as BulletWorldUnman;
+    BulletShapeUnman shapeu = shape as BulletShapeUnman;
+    return BSAPICPP.DeleteCollisionShape2(worldu.ptr, shapeu.ptr);
+}
+
+public override CollisionObjectTypes GetBodyType(BulletBody obj)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return (CollisionObjectTypes)BSAPICPP.GetBodyType2(bodyu.ptr);
+}
+
+public override BulletBody CreateBodyFromShape(BulletWorld world, BulletShape shape, uint id, Vector3 pos, Quaternion rot)
+{
+    BulletWorldUnman worldu = world as BulletWorldUnman;
+    BulletShapeUnman shapeu = shape as BulletShapeUnman;
+    return new BulletBodyUnman(id, BSAPICPP.CreateBodyFromShape2(worldu.ptr, shapeu.ptr, id, pos, rot));
+}
+
+public override BulletBody CreateBodyWithDefaultMotionState(BulletShape shape, uint id, Vector3 pos, Quaternion rot)
+{
+    BulletShapeUnman shapeu = shape as BulletShapeUnman;
+    return new BulletBodyUnman(id, BSAPICPP.CreateBodyWithDefaultMotionState2(shapeu.ptr, id, pos, rot));
+}
+
+public override BulletBody CreateGhostFromShape(BulletWorld world, BulletShape shape, uint id, Vector3 pos, Quaternion rot)
+{
+    BulletWorldUnman worldu = world as BulletWorldUnman;
+    BulletShapeUnman shapeu = shape as BulletShapeUnman;
+    return new BulletBodyUnman(id, BSAPICPP.CreateGhostFromShape2(worldu.ptr, shapeu.ptr, id, pos, rot));
+}
+
+public override void DestroyObject(BulletWorld world, BulletBody obj)
+{
+    BulletWorldUnman worldu = world as BulletWorldUnman;
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.DestroyObject2(worldu.ptr, bodyu.ptr);
+}
+
+// =====================================================================================
+// Terrain creation and helper routines
+public override BulletShape CreateGroundPlaneShape(uint id, float height, float collisionMargin)
+{
+    return new BulletShapeUnman(BSAPICPP.CreateGroundPlaneShape2(id, height, collisionMargin), BSPhysicsShapeType.SHAPE_GROUNDPLANE);
+}
+
+public override BulletShape CreateTerrainShape(uint id, Vector3 size, float minHeight, float maxHeight, float[] heightMap,
+                                float scaleFactor, float collisionMargin)
+{
+    return new BulletShapeUnman(BSAPICPP.CreateTerrainShape2(id, size, minHeight, maxHeight, heightMap, scaleFactor, collisionMargin),
+                                                BSPhysicsShapeType.SHAPE_TERRAIN);
+}
+
+// =====================================================================================
+// Constraint creation and helper routines
+public override BulletConstraint Create6DofConstraint(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.Create6DofConstraint2(worldu.ptr, bodyu1.ptr, bodyu2.ptr, frame1loc, frame1rot,
+                    frame2loc, frame2rot, useLinearReferenceFrameA, disableCollisionsBetweenLinkedBodies));
+}
+
+public override BulletConstraint Create6DofConstraintToPoint(BulletWorld world, BulletBody obj1, BulletBody obj2,
+                    Vector3 joinPoint,
+                    bool useLinearReferenceFrameA, bool disableCollisionsBetweenLinkedBodies)
+{
+    BulletWorldUnman worldu = world as BulletWorldUnman;
+    BulletBodyUnman bodyu1 = obj1 as BulletBodyUnman;
+    BulletBodyUnman bodyu2 = obj2 as BulletBodyUnman;
+    return new BulletConstraintUnman(BSAPICPP.Create6DofConstraintToPoint2(worldu.ptr, bodyu1.ptr, bodyu2.ptr,
+                    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,
+                    bool useLinearReferenceFrameA, bool disableCollisionsBetweenLinkedBodies)
+{
+    BulletWorldUnman worldu = world as BulletWorldUnman;
+    BulletBodyUnman bodyu1 = obj1 as BulletBodyUnman;
+    BulletBodyUnman bodyu2 = obj2 as BulletBodyUnman;
+    return new BulletConstraintUnman(BSAPICPP.CreateHingeConstraint2(worldu.ptr, bodyu1.ptr, bodyu2.ptr,
+                    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;
+    BSAPICPP.SetConstraintEnable2(constrainu.ptr, numericTrueFalse);
+}
+
+public override void SetConstraintNumSolverIterations(BulletConstraint constrain, float iterations)
+{
+    BulletConstraintUnman constrainu = constrain as BulletConstraintUnman;
+    BSAPICPP.SetConstraintNumSolverIterations2(constrainu.ptr, iterations);
+}
+
+public override bool SetFrames(BulletConstraint constrain,
+                Vector3 frameA, Quaternion frameArot, Vector3 frameB, Quaternion frameBrot)
+{
+    BulletConstraintUnman constrainu = constrain as BulletConstraintUnman;
+    return BSAPICPP.SetFrames2(constrainu.ptr, frameA, frameArot, frameB, frameBrot);
+}
+
+public override bool SetLinearLimits(BulletConstraint constrain, Vector3 low, Vector3 hi)
+{
+    BulletConstraintUnman constrainu = constrain as BulletConstraintUnman;
+    return BSAPICPP.SetLinearLimits2(constrainu.ptr, low, hi);
+}
+
+public override bool SetAngularLimits(BulletConstraint constrain, Vector3 low, Vector3 hi)
+{
+    BulletConstraintUnman constrainu = constrain as BulletConstraintUnman;
+    return BSAPICPP.SetAngularLimits2(constrainu.ptr, low, hi);
+}
+
+public override bool UseFrameOffset(BulletConstraint constrain, float enable)
+{
+    BulletConstraintUnman constrainu = constrain as BulletConstraintUnman;
+    return BSAPICPP.UseFrameOffset2(constrainu.ptr, enable);
+}
+
+public override bool TranslationalLimitMotor(BulletConstraint constrain, float enable, float targetVel, float maxMotorForce)
+{
+    BulletConstraintUnman constrainu = constrain as BulletConstraintUnman;
+    return BSAPICPP.TranslationalLimitMotor2(constrainu.ptr, enable, targetVel, maxMotorForce);
+}
+
+public override bool SetBreakingImpulseThreshold(BulletConstraint constrain, float threshold)
+{
+    BulletConstraintUnman constrainu = constrain as BulletConstraintUnman;
+    return BSAPICPP.SetBreakingImpulseThreshold2(constrainu.ptr, threshold);
+}
+
+public override bool CalculateTransforms(BulletConstraint constrain)
+{
+    BulletConstraintUnman constrainu = constrain as BulletConstraintUnman;
+    return BSAPICPP.CalculateTransforms2(constrainu.ptr);
+}
+
+public override bool SetConstraintParam(BulletConstraint constrain, ConstraintParams paramIndex, float value, ConstraintParamAxis axis)
+{
+    BulletConstraintUnman constrainu = constrain as BulletConstraintUnman;
+    return BSAPICPP.SetConstraintParam2(constrainu.ptr, paramIndex, value, axis);
+}
+
+public override bool DestroyConstraint(BulletWorld world, BulletConstraint constrain)
+{
+    BulletWorldUnman worldu = world as BulletWorldUnman;
+    BulletConstraintUnman constrainu = constrain as BulletConstraintUnman;
+    return BSAPICPP.DestroyConstraint2(worldu.ptr, constrainu.ptr);
+}
+
+// =====================================================================================
+// btCollisionWorld entries
+public override void UpdateSingleAabb(BulletWorld world, BulletBody obj)
+{
+    BulletWorldUnman worldu = world as BulletWorldUnman;
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.UpdateSingleAabb2(worldu.ptr, bodyu.ptr);
+}
+
+public override void UpdateAabbs(BulletWorld world)
+{
+    BulletWorldUnman worldu = world as BulletWorldUnman;
+    BSAPICPP.UpdateAabbs2(worldu.ptr);
+}
+
+public override bool GetForceUpdateAllAabbs(BulletWorld world)
+{
+    BulletWorldUnman worldu = world as BulletWorldUnman;
+    return BSAPICPP.GetForceUpdateAllAabbs2(worldu.ptr);
+}
+
+public override void SetForceUpdateAllAabbs(BulletWorld world, bool force)
+{
+    BulletWorldUnman worldu = world as BulletWorldUnman;
+    BSAPICPP.SetForceUpdateAllAabbs2(worldu.ptr, force);
+}
+
+// =====================================================================================
+// btDynamicsWorld entries
+public override bool AddObjectToWorld(BulletWorld world, BulletBody obj)
+{
+    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);
+
+    if (ret)
+    {
+        BSAPICPP.SetGravity2(bodyu.ptr, origGrav);
+        obj.ApplyCollisionMask(world.physicsScene);
+    }
+    return ret;
+}
+
+public override bool RemoveObjectFromWorld(BulletWorld world, BulletBody obj)
+{
+    BulletWorldUnman worldu = world as BulletWorldUnman;
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.RemoveObjectFromWorld2(worldu.ptr, bodyu.ptr);
+}
+
+public override bool AddConstraintToWorld(BulletWorld world, BulletConstraint constrain, bool disableCollisionsBetweenLinkedObjects)
+{
+    BulletWorldUnman worldu = world as BulletWorldUnman;
+    BulletConstraintUnman constrainu = constrain as BulletConstraintUnman;
+    return BSAPICPP.AddConstraintToWorld2(worldu.ptr, constrainu.ptr, disableCollisionsBetweenLinkedObjects);
+}
+
+public override bool RemoveConstraintFromWorld(BulletWorld world, BulletConstraint constrain)
+{
+    BulletWorldUnman worldu = world as BulletWorldUnman;
+    BulletConstraintUnman constrainu = constrain as BulletConstraintUnman;
+    return BSAPICPP.RemoveConstraintFromWorld2(worldu.ptr, constrainu.ptr);
+}
+// =====================================================================================
+// btCollisionObject entries
+public override Vector3 GetAnisotripicFriction(BulletConstraint constrain)
+{
+    BulletConstraintUnman constrainu = constrain as BulletConstraintUnman;
+    return BSAPICPP.GetAnisotripicFriction2(constrainu.ptr);
+}
+
+public override Vector3 SetAnisotripicFriction(BulletConstraint constrain, Vector3 frict)
+{
+    BulletConstraintUnman constrainu = constrain as BulletConstraintUnman;
+    return BSAPICPP.SetAnisotripicFriction2(constrainu.ptr, frict);
+}
+
+public override bool HasAnisotripicFriction(BulletConstraint constrain)
+{
+    BulletConstraintUnman constrainu = constrain as BulletConstraintUnman;
+    return BSAPICPP.HasAnisotripicFriction2(constrainu.ptr);
+}
+
+public override void SetContactProcessingThreshold(BulletBody obj, float val)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.SetContactProcessingThreshold2(bodyu.ptr, val);
+}
+
+public override float GetContactProcessingThreshold(BulletBody obj)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.GetContactProcessingThreshold2(bodyu.ptr);
+}
+
+public override bool IsStaticObject(BulletBody obj)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.IsStaticObject2(bodyu.ptr);
+}
+
+public override bool IsKinematicObject(BulletBody obj)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.IsKinematicObject2(bodyu.ptr);
+}
+
+public override bool IsStaticOrKinematicObject(BulletBody obj)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.IsStaticOrKinematicObject2(bodyu.ptr);
+}
+
+public override bool HasContactResponse(BulletBody obj)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.HasContactResponse2(bodyu.ptr);
+}
+
+public override void SetCollisionShape(BulletWorld world, BulletBody obj, BulletShape shape)
+{
+    BulletWorldUnman worldu = world as BulletWorldUnman;
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BulletShapeUnman shapeu = shape as BulletShapeUnman;
+    if (worldu != null && bodyu != null)
+    {
+        // Special case to allow the caller to zero out the reference to any physical shape
+        if (shapeu != null)
+            BSAPICPP.SetCollisionShape2(worldu.ptr, bodyu.ptr, shapeu.ptr);
+        else
+            BSAPICPP.SetCollisionShape2(worldu.ptr, bodyu.ptr, IntPtr.Zero);
+    }
+}
+
+public override BulletShape GetCollisionShape(BulletBody obj)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return new BulletShapeUnman(BSAPICPP.GetCollisionShape2(bodyu.ptr), BSPhysicsShapeType.SHAPE_UNKNOWN);
+}
+
+public override int GetActivationState(BulletBody obj)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.GetActivationState2(bodyu.ptr);
+}
+
+public override void SetActivationState(BulletBody obj, int state)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.SetActivationState2(bodyu.ptr, state);
+}
+
+public override void SetDeactivationTime(BulletBody obj, float dtime)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.SetDeactivationTime2(bodyu.ptr, dtime);
+}
+
+public override float GetDeactivationTime(BulletBody obj)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.GetDeactivationTime2(bodyu.ptr);
+}
+
+public override void ForceActivationState(BulletBody obj, ActivationState state)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.ForceActivationState2(bodyu.ptr, state);
+}
+
+public override void Activate(BulletBody obj, bool forceActivation)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.Activate2(bodyu.ptr, forceActivation);
+}
+
+public override bool IsActive(BulletBody obj)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.IsActive2(bodyu.ptr);
+}
+
+public override void SetRestitution(BulletBody obj, float val)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.SetRestitution2(bodyu.ptr, val);
+}
+
+public override float GetRestitution(BulletBody obj)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.GetRestitution2(bodyu.ptr);
+}
+
+public override void SetFriction(BulletBody obj, float val)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.SetFriction2(bodyu.ptr, val);
+}
+
+public override float GetFriction(BulletBody obj)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.GetFriction2(bodyu.ptr);
+}
+
+public override Vector3 GetPosition(BulletBody obj)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.GetPosition2(bodyu.ptr);
+}
+
+public override Quaternion GetOrientation(BulletBody obj)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.GetOrientation2(bodyu.ptr);
+}
+
+public override void SetTranslation(BulletBody obj, Vector3 position, Quaternion rotation)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.SetTranslation2(bodyu.ptr, position, rotation);
+}
+
+    /*
+public override IntPtr GetBroadphaseHandle(BulletBody obj)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.GetBroadphaseHandle2(bodyu.ptr);
+}
+
+public override void SetBroadphaseHandle(BulletBody obj, IntPtr handle)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.SetUserPointer2(bodyu.ptr, handle);
+}
+     */
+
+public override void SetInterpolationLinearVelocity(BulletBody obj, Vector3 vel)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.SetInterpolationLinearVelocity2(bodyu.ptr, vel);
+}
+
+public override void SetInterpolationAngularVelocity(BulletBody obj, Vector3 vel)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.SetInterpolationAngularVelocity2(bodyu.ptr, vel);
+}
+
+public override void SetInterpolationVelocity(BulletBody obj, Vector3 linearVel, Vector3 angularVel)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.SetInterpolationVelocity2(bodyu.ptr, linearVel, angularVel);
+}
+
+public override float GetHitFraction(BulletBody obj)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.GetHitFraction2(bodyu.ptr);
+}
+
+public override void SetHitFraction(BulletBody obj, float val)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.SetHitFraction2(bodyu.ptr, val);
+}
+
+public override CollisionFlags GetCollisionFlags(BulletBody obj)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.GetCollisionFlags2(bodyu.ptr);
+}
+
+public override CollisionFlags SetCollisionFlags(BulletBody obj, CollisionFlags flags)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.SetCollisionFlags2(bodyu.ptr, flags);
+}
+
+public override CollisionFlags AddToCollisionFlags(BulletBody obj, CollisionFlags flags)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.AddToCollisionFlags2(bodyu.ptr, flags);
+}
+
+public override CollisionFlags RemoveFromCollisionFlags(BulletBody obj, CollisionFlags flags)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.RemoveFromCollisionFlags2(bodyu.ptr, flags);
+}
+
+public override float GetCcdMotionThreshold(BulletBody obj)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.GetCcdMotionThreshold2(bodyu.ptr);
+}
+
+
+public override void SetCcdMotionThreshold(BulletBody obj, float val)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.SetCcdMotionThreshold2(bodyu.ptr, val);
+}
+
+public override float GetCcdSweptSphereRadius(BulletBody obj)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.GetCcdSweptSphereRadius2(bodyu.ptr);
+}
+
+public override void SetCcdSweptSphereRadius(BulletBody obj, float val)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.SetCcdSweptSphereRadius2(bodyu.ptr, val);
+}
+
+public override IntPtr GetUserPointer(BulletBody obj)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.GetUserPointer2(bodyu.ptr);
+}
+
+public override void SetUserPointer(BulletBody obj, IntPtr val)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.SetUserPointer2(bodyu.ptr, val);
+}
+
+// =====================================================================================
+// btRigidBody entries
+public override void ApplyGravity(BulletBody obj)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.ApplyGravity2(bodyu.ptr);
+}
+
+public override void SetGravity(BulletBody obj, Vector3 val)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.SetGravity2(bodyu.ptr, val);
+}
+
+public override Vector3 GetGravity(BulletBody obj)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.GetGravity2(bodyu.ptr);
+}
+
+public override void SetDamping(BulletBody obj, float lin_damping, float ang_damping)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.SetDamping2(bodyu.ptr, lin_damping, ang_damping);
+}
+
+public override void SetLinearDamping(BulletBody obj, float lin_damping)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.SetLinearDamping2(bodyu.ptr, lin_damping);
+}
+
+public override void SetAngularDamping(BulletBody obj, float ang_damping)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.SetAngularDamping2(bodyu.ptr, ang_damping);
+}
+
+public override float GetLinearDamping(BulletBody obj)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.GetLinearDamping2(bodyu.ptr);
+}
+
+public override float GetAngularDamping(BulletBody obj)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.GetAngularDamping2(bodyu.ptr);
+}
+
+public override float GetLinearSleepingThreshold(BulletBody obj)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.GetLinearSleepingThreshold2(bodyu.ptr);
+}
+
+public override void ApplyDamping(BulletBody obj, float timeStep)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.ApplyDamping2(bodyu.ptr, timeStep);
+}
+
+public override void SetMassProps(BulletBody obj, float mass, Vector3 inertia)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.SetMassProps2(bodyu.ptr, mass, inertia);
+}
+
+public override Vector3 GetLinearFactor(BulletBody obj)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.GetLinearFactor2(bodyu.ptr);
+}
+
+public override void SetLinearFactor(BulletBody obj, Vector3 factor)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.SetLinearFactor2(bodyu.ptr, factor);
+}
+
+public override void SetCenterOfMassByPosRot(BulletBody obj, Vector3 pos, Quaternion rot)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.SetCenterOfMassByPosRot2(bodyu.ptr, pos, rot);
+}
+
+// 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;
+    BSAPICPP.ApplyCentralForce2(bodyu.ptr, force);
+}
+
+// Set the force being applied to the object as if its mass is one.
+public override void SetObjectForce(BulletBody obj, Vector3 force)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.SetObjectForce2(bodyu.ptr, force);
+}
+
+public override Vector3 GetTotalForce(BulletBody obj)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.GetTotalForce2(bodyu.ptr);
+}
+
+public override Vector3 GetTotalTorque(BulletBody obj)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.GetTotalTorque2(bodyu.ptr);
+}
+
+public override Vector3 GetInvInertiaDiagLocal(BulletBody obj)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.GetInvInertiaDiagLocal2(bodyu.ptr);
+}
+
+public override void SetInvInertiaDiagLocal(BulletBody obj, Vector3 inert)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.SetInvInertiaDiagLocal2(bodyu.ptr, inert);
+}
+
+public override void SetSleepingThresholds(BulletBody obj, float lin_threshold, float ang_threshold)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    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;
+    BSAPICPP.ApplyTorque2(bodyu.ptr, 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;
+    BSAPICPP.ApplyForce2(bodyu.ptr, force, 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;
+    BSAPICPP.ApplyCentralImpulse2(bodyu.ptr, 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;
+    BSAPICPP.ApplyTorqueImpulse2(bodyu.ptr, 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;
+    BSAPICPP.ApplyImpulse2(bodyu.ptr, imp, pos);
+}
+
+public override void ClearForces(BulletBody obj)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.ClearForces2(bodyu.ptr);
+}
+
+public override void ClearAllForces(BulletBody obj)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.ClearAllForces2(bodyu.ptr);
+}
+
+public override void UpdateInertiaTensor(BulletBody obj)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.UpdateInertiaTensor2(bodyu.ptr);
+}
+
+public override Vector3 GetLinearVelocity(BulletBody obj)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.GetLinearVelocity2(bodyu.ptr);
+}
+
+public override Vector3 GetAngularVelocity(BulletBody obj)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.GetAngularVelocity2(bodyu.ptr);
+}
+
+public override void SetLinearVelocity(BulletBody obj, Vector3 vel)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.SetLinearVelocity2(bodyu.ptr, vel);
+}
+
+public override void SetAngularVelocity(BulletBody obj, Vector3 angularVelocity)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.SetAngularVelocity2(bodyu.ptr, angularVelocity);
+}
+
+public override Vector3 GetVelocityInLocalPoint(BulletBody obj, Vector3 pos)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.GetVelocityInLocalPoint2(bodyu.ptr, pos);
+}
+
+public override void Translate(BulletBody obj, Vector3 trans)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.Translate2(bodyu.ptr, trans);
+}
+
+public override void UpdateDeactivation(BulletBody obj, float timeStep)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.UpdateDeactivation2(bodyu.ptr, timeStep);
+}
+
+public override bool WantsSleeping(BulletBody obj)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.WantsSleeping2(bodyu.ptr);
+}
+
+public override void SetAngularFactor(BulletBody obj, float factor)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.SetAngularFactor2(bodyu.ptr, factor);
+}
+
+public override void SetAngularFactorV(BulletBody obj, Vector3 factor)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.SetAngularFactorV2(bodyu.ptr, factor);
+}
+
+public override Vector3 GetAngularFactor(BulletBody obj)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.GetAngularFactor2(bodyu.ptr);
+}
+
+public override bool IsInWorld(BulletWorld world, BulletBody obj)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.IsInWorld2(bodyu.ptr);
+}
+
+public override void AddConstraintRef(BulletBody obj, BulletConstraint constrain)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BulletConstraintUnman constrainu = constrain as BulletConstraintUnman;
+    BSAPICPP.AddConstraintRef2(bodyu.ptr, constrainu.ptr);
+}
+
+public override void RemoveConstraintRef(BulletBody obj, BulletConstraint constrain)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BulletConstraintUnman constrainu = constrain as BulletConstraintUnman;
+    BSAPICPP.RemoveConstraintRef2(bodyu.ptr, constrainu.ptr);
+}
+
+public override BulletConstraint GetConstraintRef(BulletBody obj, int index)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return new BulletConstraintUnman(BSAPICPP.GetConstraintRef2(bodyu.ptr, index));
+}
+
+public override int GetNumConstraintRefs(BulletBody obj)
+{
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.GetNumConstraintRefs2(bodyu.ptr);
+}
+
+public override bool SetCollisionGroupMask(BulletBody body, uint filter, uint mask)
+{
+    BulletBodyUnman bodyu = body as BulletBodyUnman;
+    return BSAPICPP.SetCollisionGroupMask2(bodyu.ptr, filter, mask);
+}
+
+// =====================================================================================
+// btCollisionShape entries
+
+public override float GetAngularMotionDisc(BulletShape shape)
+{
+    BulletShapeUnman shapeu = shape as BulletShapeUnman;
+    return BSAPICPP.GetAngularMotionDisc2(shapeu.ptr);
+}
+
+public override float GetContactBreakingThreshold(BulletShape shape, float defaultFactor)
+{
+    BulletShapeUnman shapeu = shape as BulletShapeUnman;
+    return BSAPICPP.GetContactBreakingThreshold2(shapeu.ptr, defaultFactor);
+}
+
+public override bool IsPolyhedral(BulletShape shape)
+{
+    BulletShapeUnman shapeu = shape as BulletShapeUnman;
+    return BSAPICPP.IsPolyhedral2(shapeu.ptr);
+}
+
+public override bool IsConvex2d(BulletShape shape)
+{
+    BulletShapeUnman shapeu = shape as BulletShapeUnman;
+    return BSAPICPP.IsConvex2d2(shapeu.ptr);
+}
+
+public override bool IsConvex(BulletShape shape)
+{
+    BulletShapeUnman shapeu = shape as BulletShapeUnman;
+    return BSAPICPP.IsConvex2(shapeu.ptr);
+}
+
+public override bool IsNonMoving(BulletShape shape)
+{
+    BulletShapeUnman shapeu = shape as BulletShapeUnman;
+    return BSAPICPP.IsNonMoving2(shapeu.ptr);
+}
+
+public override bool IsConcave(BulletShape shape)
+{
+    BulletShapeUnman shapeu = shape as BulletShapeUnman;
+    return BSAPICPP.IsConcave2(shapeu.ptr);
+}
+
+public override bool IsCompound(BulletShape shape)
+{
+    BulletShapeUnman shapeu = shape as BulletShapeUnman;
+    return BSAPICPP.IsCompound2(shapeu.ptr);
+}
+
+public override bool IsSoftBody(BulletShape shape)
+{
+    BulletShapeUnman shapeu = shape as BulletShapeUnman;
+    return BSAPICPP.IsSoftBody2(shapeu.ptr);
+}
+
+public override bool IsInfinite(BulletShape shape)
+{
+    BulletShapeUnman shapeu = shape as BulletShapeUnman;
+    return BSAPICPP.IsInfinite2(shapeu.ptr);
+}
+
+public override void SetLocalScaling(BulletShape shape, Vector3 scale)
+{
+    BulletShapeUnman shapeu = shape as BulletShapeUnman;
+    BSAPICPP.SetLocalScaling2(shapeu.ptr, scale);
+}
+
+public override Vector3 GetLocalScaling(BulletShape shape)
+{
+    BulletShapeUnman shapeu = shape as BulletShapeUnman;
+    return BSAPICPP.GetLocalScaling2(shapeu.ptr);
+}
+
+public override Vector3 CalculateLocalInertia(BulletShape shape, float mass)
+{
+    BulletShapeUnman shapeu = shape as BulletShapeUnman;
+    return BSAPICPP.CalculateLocalInertia2(shapeu.ptr, mass);
+}
+
+public override int GetShapeType(BulletShape shape)
+{
+    BulletShapeUnman shapeu = shape as BulletShapeUnman;
+    return BSAPICPP.GetShapeType2(shapeu.ptr);
+}
+
+public override void SetMargin(BulletShape shape, float val)
+{
+    BulletShapeUnman shapeu = shape as BulletShapeUnman;
+    BSAPICPP.SetMargin2(shapeu.ptr, val);
+}
+
+public override float GetMargin(BulletShape shape)
+{
+    BulletShapeUnman shapeu = shape as BulletShapeUnman;
+    return BSAPICPP.GetMargin2(shapeu.ptr);
+}
+
+// =====================================================================================
+// Debugging
+public override void DumpRigidBody(BulletWorld world, BulletBody collisionObject)
+{
+    BulletWorldUnman worldu = world as BulletWorldUnman;
+    BulletBodyUnman bodyu = collisionObject as BulletBodyUnman;
+    BSAPICPP.DumpRigidBody2(worldu.ptr, bodyu.ptr);
+}
+
+public override void DumpCollisionShape(BulletWorld world, BulletShape collisionShape)
+{
+    BulletWorldUnman worldu = world as BulletWorldUnman;
+    BulletShapeUnman shapeu = collisionShape as BulletShapeUnman;
+    BSAPICPP.DumpCollisionShape2(worldu.ptr, shapeu.ptr);
+}
+
+public override void DumpConstraint(BulletWorld world, BulletConstraint constrain)
+{
+    BulletWorldUnman worldu = world as BulletWorldUnman;
+    BulletConstraintUnman constrainu = constrain as BulletConstraintUnman;
+    BSAPICPP.DumpConstraint2(worldu.ptr, constrainu.ptr);
+}
+
+public override void DumpActivationInfo(BulletWorld world)
+{
+    BulletWorldUnman worldu = world as BulletWorldUnman;
+    BSAPICPP.DumpActivationInfo2(worldu.ptr);
+}
+
+public override void DumpAllInfo(BulletWorld world)
+{
+    BulletWorldUnman worldu = world as BulletWorldUnman;
+    BSAPICPP.DumpAllInfo2(worldu.ptr);
+}
+
+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);
+}
+
+// =====================================================================================
+// =====================================================================================
+// =====================================================================================
+// =====================================================================================
+// =====================================================================================
+// The actual interface to the unmanaged code
+static class BSAPICPP
+{
+// ===============================================================================
+// Link back to the managed code for outputting log messages
+[UnmanagedFunctionPointer(CallingConvention.Cdecl)]
+public delegate void DebugLogCallback([MarshalAs(UnmanagedType.LPStr)]string msg);
+
+// ===============================================================================
+// Initialization and simulation
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern IntPtr Initialize2(Vector3 maxPosition, IntPtr parms,
+                                                                                        int maxCollisions,  IntPtr collisionArray,
+                                                                                        int maxUpdates, IntPtr updateArray,
+                                            DebugLogCallback logRoutine);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern int PhysicsStep2(IntPtr world, float timeStep, int maxSubSteps, float fixedTimeStep,
+                        out int updatedEntityCount, out int collidersCount);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void Shutdown2(IntPtr sim);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool PushUpdate2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool UpdateParameter2(IntPtr world, uint localID, String parm, float value);
+
+// =====================================================================================
+// Mesh, hull, shape and body creation helper routines
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern IntPtr CreateMeshShape2(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 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);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern IntPtr BuildNativeShape2(IntPtr world, ShapeData shapeData);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool IsNativeShape2(IntPtr shape);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+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);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern IntPtr CreateCompoundShape2(IntPtr sim, bool enableDynamicAabbTree);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern int GetNumberOfCompoundChildren2(IntPtr cShape);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void AddChildShapeToCompoundShape2(IntPtr cShape, IntPtr addShape, Vector3 pos, Quaternion rot);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern IntPtr GetChildShapeFromCompoundShapeIndex2(IntPtr cShape, int indx);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern IntPtr RemoveChildShapeFromCompoundShapeIndex2(IntPtr cShape, int indx);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+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]
+public static extern IntPtr DuplicateCollisionShape2(IntPtr sim, IntPtr srcShape, uint id);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool DeleteCollisionShape2(IntPtr world, IntPtr shape);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern int GetBodyType2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern IntPtr CreateBodyFromShape2(IntPtr sim, IntPtr shape, uint id, Vector3 pos, Quaternion rot);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern IntPtr CreateBodyWithDefaultMotionState2(IntPtr shape, uint id, Vector3 pos, Quaternion rot);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern IntPtr CreateGhostFromShape2(IntPtr sim, IntPtr shape, uint id, Vector3 pos, Quaternion rot);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void DestroyObject2(IntPtr sim, IntPtr obj);
+
+// =====================================================================================
+// Terrain creation and helper routines
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+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, 
+                                            [MarshalAs(UnmanagedType.LPArray)] float[] heightMap,
+                                            float scaleFactor, float collisionMargin);
+
+// =====================================================================================
+// Constraint creation and helper routines
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern IntPtr Create6DofConstraint2(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 Create6DofConstraintToPoint2(IntPtr world, IntPtr obj1, IntPtr obj2,
+                    Vector3 joinPoint,
+                    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]
+public static extern void SetConstraintNumSolverIterations2(IntPtr constrain, float iterations);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool SetFrames2(IntPtr constrain,
+                Vector3 frameA, Quaternion frameArot, Vector3 frameB, Quaternion frameBrot);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool SetLinearLimits2(IntPtr constrain, Vector3 low, Vector3 hi);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool SetAngularLimits2(IntPtr constrain, Vector3 low, Vector3 hi);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool UseFrameOffset2(IntPtr constrain, float enable);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool TranslationalLimitMotor2(IntPtr constrain, float enable, float targetVel, float maxMotorForce);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool SetBreakingImpulseThreshold2(IntPtr constrain, float threshold);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool CalculateTransforms2(IntPtr constrain);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool SetConstraintParam2(IntPtr constrain, ConstraintParams paramIndex, float value, ConstraintParamAxis axis);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool DestroyConstraint2(IntPtr world, IntPtr constrain);
+
+// =====================================================================================
+// btCollisionWorld entries
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void UpdateSingleAabb2(IntPtr world, IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void UpdateAabbs2(IntPtr world);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool GetForceUpdateAllAabbs2(IntPtr world);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void SetForceUpdateAllAabbs2(IntPtr world, bool force);
+
+// =====================================================================================
+// btDynamicsWorld entries
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool AddObjectToWorld2(IntPtr world, IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool RemoveObjectFromWorld2(IntPtr world, IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool AddConstraintToWorld2(IntPtr world, IntPtr constrain, bool disableCollisionsBetweenLinkedObjects);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool RemoveConstraintFromWorld2(IntPtr world, IntPtr constrain);
+// =====================================================================================
+// btCollisionObject entries
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern Vector3 GetAnisotripicFriction2(IntPtr constrain);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern Vector3 SetAnisotripicFriction2(IntPtr constrain, Vector3 frict);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool HasAnisotripicFriction2(IntPtr constrain);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void SetContactProcessingThreshold2(IntPtr obj, float val);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern float GetContactProcessingThreshold2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool IsStaticObject2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool IsKinematicObject2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool IsStaticOrKinematicObject2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool HasContactResponse2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void SetCollisionShape2(IntPtr sim, IntPtr obj, IntPtr shape);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern IntPtr GetCollisionShape2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern int GetActivationState2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void SetActivationState2(IntPtr obj, int state);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void SetDeactivationTime2(IntPtr obj, float dtime);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern float GetDeactivationTime2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void ForceActivationState2(IntPtr obj, ActivationState state);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void Activate2(IntPtr obj, bool forceActivation);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool IsActive2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void SetRestitution2(IntPtr obj, float val);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern float GetRestitution2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void SetFriction2(IntPtr obj, float val);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern float GetFriction2(IntPtr obj);
+
+    /* Haven't defined the type 'Transform'
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern Transform GetWorldTransform2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void setWorldTransform2(IntPtr obj, Transform trans);
+     */
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern Vector3 GetPosition2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern Quaternion GetOrientation2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void SetTranslation2(IntPtr obj, Vector3 position, Quaternion rotation);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern IntPtr GetBroadphaseHandle2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void SetBroadphaseHandle2(IntPtr obj, IntPtr handle);
+
+    /*
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern Transform GetInterpolationWorldTransform2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void SetInterpolationWorldTransform2(IntPtr obj, Transform trans);
+     */
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void SetInterpolationLinearVelocity2(IntPtr obj, Vector3 vel);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void SetInterpolationAngularVelocity2(IntPtr obj, Vector3 vel);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void SetInterpolationVelocity2(IntPtr obj, Vector3 linearVel, Vector3 angularVel);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern float GetHitFraction2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void SetHitFraction2(IntPtr obj, float val);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern CollisionFlags GetCollisionFlags2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern CollisionFlags SetCollisionFlags2(IntPtr obj, CollisionFlags flags);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern CollisionFlags AddToCollisionFlags2(IntPtr obj, CollisionFlags flags);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern CollisionFlags RemoveFromCollisionFlags2(IntPtr obj, CollisionFlags flags);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern float GetCcdMotionThreshold2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void SetCcdMotionThreshold2(IntPtr obj, float val);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern float GetCcdSweptSphereRadius2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void SetCcdSweptSphereRadius2(IntPtr obj, float val);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern IntPtr GetUserPointer2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void SetUserPointer2(IntPtr obj, IntPtr val);
+
+// =====================================================================================
+// btRigidBody entries
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void ApplyGravity2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void SetGravity2(IntPtr obj, Vector3 val);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern Vector3 GetGravity2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void SetDamping2(IntPtr obj, float lin_damping, float ang_damping);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void SetLinearDamping2(IntPtr obj, float lin_damping);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void SetAngularDamping2(IntPtr obj, float ang_damping);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern float GetLinearDamping2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern float GetAngularDamping2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern float GetLinearSleepingThreshold2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern float GetAngularSleepingThreshold2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void ApplyDamping2(IntPtr obj, float timeStep);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void SetMassProps2(IntPtr obj, float mass, Vector3 inertia);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern Vector3 GetLinearFactor2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void SetLinearFactor2(IntPtr obj, Vector3 factor);
+
+    /*
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void SetCenterOfMassTransform2(IntPtr obj, Transform trans);
+     */
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void SetCenterOfMassByPosRot2(IntPtr obj, Vector3 pos, Quaternion rot);
+
+// Add a force to the object as if its mass is one.
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void ApplyCentralForce2(IntPtr obj, Vector3 force);
+
+// Set the force being applied to the object as if its mass is one.
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void SetObjectForce2(IntPtr obj, Vector3 force);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern Vector3 GetTotalForce2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern Vector3 GetTotalTorque2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern Vector3 GetInvInertiaDiagLocal2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void SetInvInertiaDiagLocal2(IntPtr obj, Vector3 inert);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void SetSleepingThresholds2(IntPtr obj, float lin_threshold, float ang_threshold);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void ApplyTorque2(IntPtr obj, Vector3 torque);
+
+// Apply force at the given point. Will add torque to the object.
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void ApplyForce2(IntPtr obj, Vector3 force, Vector3 pos);
+
+// Apply impulse to the object. Same as "ApplycentralForce" but force scaled by object's mass.
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void ApplyCentralImpulse2(IntPtr obj, Vector3 imp);
+
+// Apply impulse to the object's torque. Force is scaled by object's mass.
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void ApplyTorqueImpulse2(IntPtr obj, Vector3 imp);
+
+// Apply impulse at the point given. For is scaled by object's mass and effects both linear and angular forces.
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void ApplyImpulse2(IntPtr obj, Vector3 imp, Vector3 pos);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void ClearForces2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void ClearAllForces2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void UpdateInertiaTensor2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern Vector3 GetCenterOfMassPosition2(IntPtr obj);
+
+    /*
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern Transform GetCenterOfMassTransform2(IntPtr obj);
+     */
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern Vector3 GetLinearVelocity2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern Vector3 GetAngularVelocity2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void SetLinearVelocity2(IntPtr obj, Vector3 val);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void SetAngularVelocity2(IntPtr obj, Vector3 angularVelocity);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern Vector3 GetVelocityInLocalPoint2(IntPtr obj, Vector3 pos);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void Translate2(IntPtr obj, Vector3 trans);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void UpdateDeactivation2(IntPtr obj, float timeStep);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool WantsSleeping2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void SetAngularFactor2(IntPtr obj, float factor);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void SetAngularFactorV2(IntPtr obj, Vector3 factor);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern Vector3 GetAngularFactor2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool IsInWorld2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void AddConstraintRef2(IntPtr obj, IntPtr constrain);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void RemoveConstraintRef2(IntPtr obj, IntPtr constrain);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern IntPtr GetConstraintRef2(IntPtr obj, int index);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern int GetNumConstraintRefs2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool SetCollisionGroupMask2(IntPtr body, uint filter, uint mask);
+
+// =====================================================================================
+// btCollisionShape entries
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern float GetAngularMotionDisc2(IntPtr shape);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern float GetContactBreakingThreshold2(IntPtr shape, float defaultFactor);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool IsPolyhedral2(IntPtr shape);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool IsConvex2d2(IntPtr shape);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool IsConvex2(IntPtr shape);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool IsNonMoving2(IntPtr shape);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool IsConcave2(IntPtr shape);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool IsCompound2(IntPtr shape);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool IsSoftBody2(IntPtr shape);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool IsInfinite2(IntPtr shape);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void SetLocalScaling2(IntPtr shape, Vector3 scale);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern Vector3 GetLocalScaling2(IntPtr shape);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern Vector3 CalculateLocalInertia2(IntPtr shape, float mass);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern int GetShapeType2(IntPtr shape);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void SetMargin2(IntPtr shape, float val);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern float GetMargin2(IntPtr shape);
+
+// =====================================================================================
+// Debugging
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void DumpRigidBody2(IntPtr sim, IntPtr collisionObject);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void DumpCollisionShape2(IntPtr sim, IntPtr collisionShape);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void DumpMapInfo2(IntPtr sim, IntPtr manInfo);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void DumpConstraint2(IntPtr sim, IntPtr constrain);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void DumpActivationInfo2(IntPtr sim);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+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
new file mode 100755
index 0000000..6fc10e9
--- /dev/null
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSAPIXNA.cs
@@ -0,0 +1,2311 @@
+/*
+ * 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.IO;
+using System.Runtime.InteropServices;
+using System.Text;
+
+using OpenSim.Framework;
+
+using OpenMetaverse;
+
+using BulletXNA;
+using BulletXNA.LinearMath;
+using BulletXNA.BulletCollision;
+using BulletXNA.BulletDynamics;
+using BulletXNA.BulletCollision.CollisionDispatch;
+
+namespace OpenSim.Region.Physics.BulletSPlugin
+{
+public sealed class BSAPIXNA : BSAPITemplate
+{
+private sealed class BulletWorldXNA : BulletWorld
+{
+    public DiscreteDynamicsWorld world;
+    public BulletWorldXNA(uint id, BSScene physScene, DiscreteDynamicsWorld xx)
+        : base(id, physScene)
+    {
+        world = xx;
+    }
+}
+
+private sealed class BulletBodyXNA : BulletBody
+{
+    public CollisionObject body;
+    public RigidBody rigidBody { get { return RigidBody.Upcast(body); } }
+
+    public BulletBodyXNA(uint id, CollisionObject xx)
+        : base(id)
+    {
+        body = xx;
+    }
+    public override bool HasPhysicalBody
+    {
+        get { return body != null; }
+    }
+    public override void Clear()
+    {
+        body = null;
+    }
+    public override string AddrString
+    {
+        get { return "XNARigidBody"; }
+    }
+}
+
+private sealed class BulletShapeXNA : BulletShape
+{
+    public CollisionShape shape;
+    public BulletShapeXNA(CollisionShape xx, BSPhysicsShapeType typ) 
+        : base()
+    {
+        shape = xx;
+        type = typ;
+    }
+    public override bool HasPhysicalShape
+    {
+        get { return shape != null; }
+    }
+    public override void Clear()
+    {
+        shape = null;
+    }
+    public override BulletShape Clone()
+    {
+        return new BulletShapeXNA(shape, type);
+    }
+    public override bool ReferenceSame(BulletShape other)
+    {
+        BulletShapeXNA otheru = other as BulletShapeXNA;
+        return (otheru != null) && (this.shape == otheru.shape);
+
+    }
+    public override string AddrString
+    {
+        get { return "XNACollisionShape"; }
+    }
+}
+private sealed class BulletConstraintXNA : BulletConstraint
+{
+    public TypedConstraint constrain;
+    public BulletConstraintXNA(TypedConstraint xx) : base()
+    {
+        constrain = xx;
+    }
+
+    public override void Clear()
+    {
+        constrain = null;
+    }
+    public override bool HasPhysicalConstraint { get { return constrain != null; } }
+
+    // Used for log messages for a unique display of the memory/object allocated to this instance
+    public override string AddrString
+    {
+        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; }
+
+    public override string BulletEngineName { get { return "BulletXNA"; } }
+    public override string BulletEngineVersion { get; protected set; }
+
+    public BSAPIXNA(string paramName, BSScene physScene)
+    {
+        PhysicsScene = physScene;
+    }
+
+    /// <summary>
+    /// 
+    /// </summary>
+    /// <param name="p"></param>
+    /// <param name="p_2"></param>
+    public override bool RemoveObjectFromWorld(BulletWorld pWorld, BulletBody pBody)
+    {
+        DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
+        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
+        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 pWorld, BulletConstraint pConstraint, bool pDisableCollisionsBetweenLinkedObjects)
+    {
+        DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
+        TypedConstraint constraint = (pConstraint as BulletConstraintXNA).constrain;
+        world.AddConstraint(constraint, pDisableCollisionsBetweenLinkedObjects);
+        
+        return true;
+
+    }
+
+    public override bool RemoveConstraintFromWorld(BulletWorld pWorld, BulletConstraint pConstraint)
+    {
+        DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
+        TypedConstraint constraint = (pConstraint as BulletConstraintXNA).constrain;
+        world.RemoveConstraint(constraint);
+        return true;
+    }
+
+    public override void SetRestitution(BulletBody pCollisionObject, float pRestitution)
+    {
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).body;
+        collisionObject.SetRestitution(pRestitution);
+    }
+
+    public override int GetShapeType(BulletShape pShape)
+    {
+        CollisionShape shape = (pShape as BulletShapeXNA).shape;
+        return (int)shape.GetShapeType();
+    }
+    public override void SetMargin(BulletShape pShape, float pMargin)
+    {
+        CollisionShape shape = (pShape as BulletShapeXNA).shape;
+        shape.SetMargin(pMargin);
+    }
+
+    public override float GetMargin(BulletShape pShape)
+    {
+        CollisionShape shape = (pShape as BulletShapeXNA).shape;
+        return shape.GetMargin();
+    }
+
+    public override void SetLocalScaling(BulletShape pShape, Vector3 pScale)
+    {
+        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 pCollisionObject, float contactprocessingthreshold)
+    {
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
+        collisionObject.SetContactProcessingThreshold(contactprocessingthreshold);
+    }
+
+    public override void SetCcdMotionThreshold(BulletBody pCollisionObject, float pccdMotionThreashold)
+    {
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).body;
+        collisionObject.SetCcdMotionThreshold(pccdMotionThreashold);
+    }
+
+    public override void SetCcdSweptSphereRadius(BulletBody pCollisionObject, float pCcdSweptSphereRadius)
+    {
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).body;
+        collisionObject.SetCcdSweptSphereRadius(pCcdSweptSphereRadius);
+    }
+
+    public override void SetAngularFactorV(BulletBody pBody, Vector3 pAngularFactor)
+    {
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
+        body.SetAngularFactor(new IndexedVector3(pAngularFactor.X, pAngularFactor.Y, pAngularFactor.Z));
+    }
+
+    public override CollisionFlags AddToCollisionFlags(BulletBody pCollisionObject, CollisionFlags pcollisionFlags)
+    {
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).body;
+        CollisionFlags existingcollisionFlags = (CollisionFlags)(uint)collisionObject.GetCollisionFlags();
+        existingcollisionFlags |= pcollisionFlags;
+        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.
+        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);
+
+        //if (body.GetBroadphaseHandle() != null)
+        //    world.UpdateSingleAabb(body);
+        return true;
+    }
+
+    public override void ForceActivationState(BulletBody pCollisionObject, ActivationState pActivationState)
+    {
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).body;
+        collisionObject.ForceActivationState((BulletXNA.BulletCollision.ActivationState)(uint)pActivationState);
+    }
+
+    public override void UpdateSingleAabb(BulletWorld pWorld, BulletBody pCollisionObject)
+    {
+        DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).body;
+        world.UpdateSingleAabb(collisionObject);
+    }
+
+    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 void SetForceUpdateAllAabbs(BulletWorld pWorld, bool pForce)
+    {
+        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 pCollisionObject)
+    {
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).body;
+        IndexedVector3 zeroVector = new IndexedVector3(0, 0, 0);
+        collisionObject.SetInterpolationLinearVelocity(ref zeroVector);
+        collisionObject.SetInterpolationAngularVelocity(ref zeroVector);
+        IndexedMatrix bodytransform = collisionObject.GetWorldTransform();
+
+        collisionObject.SetInterpolationWorldTransform(ref bodytransform);
+
+        if (collisionObject is RigidBody)
+        {
+            RigidBody rigidbody = collisionObject as RigidBody;
+            rigidbody.SetLinearVelocity(zeroVector);
+            rigidbody.SetAngularVelocity(zeroVector);
+            rigidbody.ClearForces();
+        }
+    }
+
+    public override void SetInterpolationAngularVelocity(BulletBody pCollisionObject, Vector3 pVector3)
+    {
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
+        IndexedVector3 vec = new IndexedVector3(pVector3.X, pVector3.Y, pVector3.Z);
+        collisionObject.SetInterpolationAngularVelocity(ref vec);
+    }
+
+    public override void SetAngularVelocity(BulletBody pBody, Vector3 pVector3)
+    {
+        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 = (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 = (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 = (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 = (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 = (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 = (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);
+    }
+
+    public override void ClearForces(BulletBody pBody)
+    {
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
+        body.ClearForces();
+    }
+
+    public override void SetTranslation(BulletBody pCollisionObject, Vector3 _position, Quaternion _orientation)
+    {
+        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;
+        collisionObject.SetWorldTransform(mat);
+        
+    }
+
+    public override Vector3 GetPosition(BulletBody pCollisionObject)
+    {
+        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 = (pShape as BulletShapeXNA).shape;
+        IndexedVector3 inertia = IndexedVector3.Zero;
+        shape.CalculateLocalInertia(pphysMass, out inertia);
+        return new Vector3(inertia.X, inertia.Y, inertia.Z);
+    }
+
+    public override void SetMassProps(BulletBody pBody, float pphysMass, Vector3 plocalInertia)
+    {
+        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 = (pBody as BulletBodyXNA).rigidBody;
+        IndexedVector3 force = new IndexedVector3(_force.X, _force.Y, _force.Z);
+        body.SetTotalForce(ref force);
+    }
+
+    public override void SetFriction(BulletBody pCollisionObject, float _currentFriction)
+    {
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).body;
+        collisionObject.SetFriction(_currentFriction);
+    }
+
+    public override void SetLinearVelocity(BulletBody pBody, Vector3 _velocity)
+    {
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
+        IndexedVector3 velocity = new IndexedVector3(_velocity.X, _velocity.Y, _velocity.Z);
+        body.SetLinearVelocity(velocity);
+    }
+
+    public override void Activate(BulletBody pCollisionObject, bool pforceactivation)
+    {
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
+        collisionObject.Activate(pforceactivation);
+        
+    }
+
+    public override Quaternion GetOrientation(BulletBody pCollisionObject)
+    {
+        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 pCollisionObject, CollisionFlags pcollisionFlags)
+    {
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).body;
+        CollisionFlags existingcollisionFlags = (CollisionFlags)(uint)collisionObject.GetCollisionFlags();
+        existingcollisionFlags &= ~pcollisionFlags;
+        collisionObject.SetCollisionFlags((BulletXNA.BulletCollision.CollisionFlags)(uint)existingcollisionFlags);
+        return (CollisionFlags)(uint)existingcollisionFlags;
+    }
+
+    public override float GetCcdMotionThreshold(BulletBody pCollisionObject)
+    {
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
+        return collisionObject.GetCcdSquareMotionThreshold();
+    }
+
+    public override float GetCcdSweptSphereRadius(BulletBody pCollisionObject)
+    {
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
+        return collisionObject.GetCcdSweptSphereRadius();
+        
+    }
+
+    public override IntPtr GetUserPointer(BulletBody pCollisionObject)
+    {
+        CollisionObject shape = (pCollisionObject as BulletBodyXNA).body;
+        return (IntPtr)shape.GetUserPointer();
+    }
+
+    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 = (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 = (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 = (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);
+        constraint.SetLinearUpperLimit(highlimit);
+        return true;
+    }
+
+    public override bool SetAngularLimits(BulletConstraint pConstraint, Vector3 low, Vector3 high)
+    {
+        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);
+        constraint.SetAngularUpperLimit(highlimit);
+        return true;
+    }
+
+    public override void SetConstraintNumSolverIterations(BulletConstraint pConstraint, float cnt)
+    {
+        Generic6DofConstraint constraint = (pConstraint as BulletConstraintXNA).constrain as Generic6DofConstraint;
+        constraint.SetOverrideNumSolverIterations((int)cnt);
+    }
+
+    public override bool CalculateTransforms(BulletConstraint pConstraint)
+    {
+        Generic6DofConstraint constraint = (pConstraint as BulletConstraintXNA).constrain as Generic6DofConstraint;
+        constraint.CalculateTransforms();
+        return true;
+    }
+
+    public override void SetConstraintEnable(BulletConstraint pConstraint, float p_2)
+    {
+        Generic6DofConstraint constraint = (pConstraint as BulletConstraintXNA).constrain as Generic6DofConstraint;
+        constraint.SetEnabled((p_2 == 0) ? false : true);
+    }
+
+
+    public override BulletConstraint Create6DofConstraint(BulletWorld pWorld, BulletBody pBody1, BulletBody pBody2, 
+                                    Vector3 pframe1, Quaternion pframe1rot, Vector3 pframe2, Quaternion pframe2rot, 
+                                    bool puseLinearReferenceFrameA, bool pdisableCollisionsBetweenLinkedBodies)
+
+    {
+        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);
+        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;
+
+        Generic6DofConstraint consttr = new Generic6DofConstraint(body1, body2, ref frame1, ref frame2,
+                                                                  puseLinearReferenceFrameA);
+        consttr.CalculateTransforms();
+        world.AddConstraint(consttr,pdisableCollisionsBetweenLinkedBodies);
+
+        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>
+    /// <param name="pBody2"></param>
+    /// <param name="pjoinPoint"></param>
+    /// <param name="puseLinearReferenceFrameA"></param>
+    /// <param name="pdisableCollisionsBetweenLinkedBodies"></param>
+    /// <returns></returns>
+    public override BulletConstraint Create6DofConstraintToPoint(BulletWorld pWorld, BulletBody pBody1, BulletBody pBody2, Vector3 pjoinPoint, bool puseLinearReferenceFrameA, bool pdisableCollisionsBetweenLinkedBodies)
+    {
+        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));
+
+        IndexedVector3 joinPoint = new IndexedVector3(pjoinPoint.X, pjoinPoint.Y, pjoinPoint.Z);
+        IndexedMatrix mat = IndexedMatrix.Identity;
+        mat._origin = new IndexedVector3(pjoinPoint.X, pjoinPoint.Y, pjoinPoint.Z);
+        frame1._origin = body1.GetWorldTransform().Inverse()*joinPoint;
+        frame2._origin = body2.GetWorldTransform().Inverse()*joinPoint;
+
+        Generic6DofConstraint consttr = new Generic6DofConstraint(body1, body2, ref frame1, ref frame2, puseLinearReferenceFrameA);
+        consttr.CalculateTransforms();
+        world.AddConstraint(consttr, pdisableCollisionsBetweenLinkedBodies);
+
+        return new BulletConstraintXNA(consttr);
+    }
+    //SetFrames(m_constraint.ptr, frameA, frameArot, frameB, frameBrot);
+    public override bool SetFrames(BulletConstraint pConstraint, Vector3 pframe1, Quaternion pframe1rot, Vector3 pframe2, Quaternion pframe2rot)
+    {
+        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);
+        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;
+        constraint.SetFrames(ref frame1, ref frame2);
+        return true;
+    }
+
+    public override Vector3 GetLinearVelocity(BulletBody pBody)
+    {
+        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 = (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 = (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 pCollisionObject, Vector3 trans)
+    {
+        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 = (pBody as BulletBodyXNA).rigidBody;
+        body.UpdateDeactivation(timeStep);
+    }
+
+    public override bool WantsSleeping(BulletBody pBody)
+    {
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
+        return body.WantsSleeping();
+    }
+
+    public override void SetAngularFactor(BulletBody pBody, float factor)
+    {
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
+        body.SetAngularFactor(factor);
+    }
+
+    public override Vector3 GetAngularFactor(BulletBody pBody)
+    {
+        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 pCollisionObject)
+    {
+        DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).body;
+        return world.IsInWorld(collisionObject);
+    }
+
+    public override void AddConstraintRef(BulletBody pBody, BulletConstraint pConstraint)
+    {
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
+        TypedConstraint constrain = (pConstraint as BulletConstraintXNA).constrain;
+        body.AddConstraintRef(constrain);
+    }
+
+    public override void RemoveConstraintRef(BulletBody pBody, BulletConstraint pConstraint)
+    {
+        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 = (pBody as BulletBodyXNA).rigidBody;
+        return new BulletConstraintXNA(body.GetConstraintRef(index));
+    }
+
+    public override int GetNumConstraintRefs(BulletBody pBody)
+    {
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
+        return body.GetNumConstraintRefs();
+    }
+
+    public override void SetInterpolationLinearVelocity(BulletBody pCollisionObject, Vector3 VehicleVelocity)
+    {
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
+        IndexedVector3 velocity = new IndexedVector3(VehicleVelocity.X, VehicleVelocity.Y, VehicleVelocity.Z);
+        collisionObject.SetInterpolationLinearVelocity(ref velocity);
+    }
+
+    public override bool UseFrameOffset(BulletConstraint pConstraint, float onOff)
+    {
+        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 = (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 = (pBody as BulletBodyXNA).rigidBody;
+        float lineardamping = body.GetLinearDamping();
+        body.SetDamping(lineardamping, angularDamping);
+
+    }
+
+    public override void UpdateInertiaTensor(BulletBody pBody)
+    {
+        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 = (pCompoundShape as BulletShapeXNA).shape as CompoundShape;
+        shape.RecalculateLocalAabb();
+    }
+
+    //BulletSimAPI.GetCollisionFlags(PhysBody.ptr)
+    public override CollisionFlags GetCollisionFlags(BulletBody pCollisionObject)
+    {
+        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 = (pBody as BulletBodyXNA).rigidBody;
+        body.SetDamping(pLinear, pAngular);
+    }
+    //PhysBody.ptr, PhysicsScene.Params.deactivationTime);
+    public override void SetDeactivationTime(BulletBody pCollisionObject, float 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 = (pBody as BulletBodyXNA).rigidBody;
+        body.SetSleepingThresholds(plinearSleepingThreshold, pangularSleepingThreshold);
+    }
+
+    public override CollisionObjectTypes GetBodyType(BulletBody pCollisionObject)
+    {
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).body;
+        return (CollisionObjectTypes)(int) collisionObject.GetInternalType();
+    }
+
+    public override void ApplyGravity(BulletBody pBody)
+    {
+
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
+        body.ApplyGravity();
+    }
+
+    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 pBody, float lin_damping)
+    {
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
+        float angularDamping = body.GetAngularDamping();
+        body.SetDamping(lin_damping, angularDamping); 
+    }
+
+    public override float GetLinearDamping(BulletBody pBody)
+    {
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
+        return body.GetLinearDamping();
+    }
+
+    public override float GetAngularDamping(BulletBody pBody)
+    {
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
+        return body.GetAngularDamping();
+    }
+
+    public override float GetLinearSleepingThreshold(BulletBody pBody)
+    {
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
+        return body.GetLinearSleepingThreshold();
+    }
+
+    public override void ApplyDamping(BulletBody pBody, float timeStep)
+    {
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
+        body.ApplyDamping(timeStep);
+    }
+
+    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 pBody, Vector3 factor)
+    {
+        RigidBody body = (pBody as BulletBodyXNA).rigidBody;
+        body.SetLinearFactor(new IndexedVector3(factor.X, factor.Y, factor.Z));
+    }
+
+    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 = (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 = (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 = (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 = (pBody as BulletBodyXNA).rigidBody;
+        IndexedVector3 fSum = new IndexedVector3(pfSum.X, pfSum.Y, pfSum.Z);
+        body.ApplyTorqueImpulse(ref fSum);
+    }
+
+    public override void DestroyObject(BulletWorld pWorld, BulletBody pBody)
+    {
+        DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
+        CollisionObject co = (pBody as BulletBodyXNA).rigidBody;
+        RigidBody bo = co as RigidBody;
+        if (bo == null)
+        {
+            
+            if (world.IsInWorld(co))
+            {
+                world.RemoveCollisionObject(co);
+            }
+        }
+        else
+        {
+            
+            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 Shutdown(BulletWorld pWorld)
+    {
+        DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
+        world.Cleanup();
+    }
+
+    public override BulletShape DuplicateCollisionShape(BulletWorld pWorld, BulletShape pShape, uint id)
+    {
+        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 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 = (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 = (pShape as BulletShapeXNA).shape;
+        //UpdateSingleAabb(world, shape);
+        // TODO: Feed Update array into null
+        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)
+    {
+
+        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 = (pShape as BulletShapeXNA).shape;
+
+        // TODO: Feed Update array into null
+        RigidBody body = new RigidBody(0, new DefaultMotionState( mat, IndexedMatrix.Identity), shape, IndexedVector3.Zero);
+        body.SetWorldTransform(mat);
+        body.SetUserPointer(pLocalID);
+        return new BulletBodyXNA(pLocalID, body);
+    }
+    //(m_mapInfo.terrainBody.ptr, CollisionFlags.CF_STATIC_OBJECT);
+    public override CollisionFlags SetCollisionFlags(BulletBody pCollisionObject, CollisionFlags collisionFlags)
+    {
+        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 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 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; }
+    public override bool IsActive(BulletBody pBody) { /* TODO */ return false; }
+    public override float GetRestitution(BulletBody pBody) { /* TODO */ return 0f; }
+    public override float GetFriction(BulletBody pBody) { /* TODO */ return 0f; }
+    public override void SetInterpolationVelocity(BulletBody pBody, Vector3 linearVel, Vector3 angularVel) { /* TODO */ }
+    public override float GetHitFraction(BulletBody pBody) { /* TODO */ return 0f; }
+
+    //(m_mapInfo.terrainBody.ptr, PhysicsScene.Params.terrainHitFraction);
+    public override void SetHitFraction(BulletBody pCollisionObject, float 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 = (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);
+        capsuleShapeZ.SetLocalScaling(ref scale);
+
+        return new BulletShapeXNA(capsuleShapeZ, BSPhysicsShapeType.SHAPE_CAPSULE); ;
+    }
+
+    public override BulletWorld Initialize(Vector3 maxPosition, ConfigurationParameters parms,
+                                            int maxCollisions, ref CollisionDesc[] collisionArray,
+                                            int maxUpdates, ref EntityProperties[] updateArray
+                                            )
+    {
+
+        UpdatedObjects = updateArray;
+        UpdatedCollisions = collisionArray;
+        /* TODO */
+        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 DiscreteDynamicsWorld Initialize2(Vector3 worldExtent, 
+                        ConfigurationParameters[] o,
+                        int mMaxCollisionsPerFrame, ref CollisionDesc[] collisionArray,
+                        int mMaxUpdatesPerFrame, ref EntityProperties[] updateArray, 
+                        object mDebugLogCallbackHandle)
+    {
+        CollisionWorld.WorldData.ParamData p = new CollisionWorld.WorldData.ParamData();
+
+        p.angularDamping = BSParam.AngularDamping;
+        p.defaultFriction = o[0].defaultFriction;
+        p.defaultFriction = o[0].defaultFriction;
+        p.defaultDensity = o[0].defaultDensity;
+        p.defaultRestitution = o[0].defaultRestitution;
+        p.collisionMargin = o[0].collisionMargin;
+        p.gravity = o[0].gravity;
+
+        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;
+        p.shouldForceUpdateAllAabbs = o[0].shouldForceUpdateAllAabbs;
+        p.shouldRandomizeSolverOrder = o[0].shouldRandomizeSolverOrder;
+        p.shouldSplitSimulationIslands = o[0].shouldSplitSimulationIslands;
+        p.shouldEnableFrictionCaching = o[0].shouldEnableFrictionCaching;
+        p.numberOfSolverIterations = o[0].numberOfSolverIterations;
+
+        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);
+
+
+        if (p.maxPersistantManifoldPoolSize > 0)
+            cci.m_persistentManifoldPoolSize = (int)p.maxPersistantManifoldPoolSize;
+        if (p.shouldDisableContactPoolDynamicAllocation !=0)
+            m_dispatcher.SetDispatcherFlags(DispatcherFlags.CD_DISABLE_CONTACTPOOL_DYNAMIC_ALLOCATION);
+        //if (p.maxCollisionAlgorithmPoolSize >0 )
+
+        DbvtBroadphase m_broadphase = new DbvtBroadphase();
+        //IndexedVector3 aabbMin = new IndexedVector3(0, 0, 0);
+        //IndexedVector3 aabbMax = new IndexedVector3(256, 256, 256);
+
+        //AxisSweep3Internal m_broadphase2 = new AxisSweep3Internal(ref aabbMin, ref aabbMax, Convert.ToInt32(0xfffe), 0xffff, ushort.MaxValue/2, null, true);
+        m_broadphase.GetOverlappingPairCache().SetInternalGhostPairCallback(new GhostPairCallback());
+
+        SequentialImpulseConstraintSolver m_solver = new SequentialImpulseConstraintSolver();
+
+        DiscreteDynamicsWorld world = new DiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_solver, cci);
+
+        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;
+        if (p.shouldRandomizeSolverOrder != 0)
+            world.GetSolverInfo().m_solverMode |= SolverMode.SOLVER_RANDMIZE_ORDER;
+
+        world.GetSimulationIslandManager().SetSplitIslands(p.shouldSplitSimulationIslands != 0);
+        //world.GetDispatchInfo().m_enableSatConvex Not implemented in C# port
+
+        if (p.shouldEnableFrictionCaching != 0)
+            world.GetSolverInfo().m_solverMode |= SolverMode.SOLVER_ENABLE_FRICTION_DIRECTION_CACHING;
+
+        if (p.numberOfSolverIterations > 0)
+            world.GetSolverInfo().m_numIterations = (int) p.numberOfSolverIterations;
+
+
+        world.GetSolverInfo().m_damping = world.WorldSettings.Params.linearDamping;
+        world.GetSolverInfo().m_restitution = world.WorldSettings.Params.defaultRestitution;
+        world.GetSolverInfo().m_globalCfm = 0.0f;
+        world.GetSolverInfo().m_tau = 0.6f;
+        world.GetSolverInfo().m_friction = 0.3f;
+        world.GetSolverInfo().m_maxErrorReduction = 20f;
+        world.GetSolverInfo().m_numIterations = 10;
+        world.GetSolverInfo().m_erp = 0.2f;
+        world.GetSolverInfo().m_erp2 = 0.1f;
+        world.GetSolverInfo().m_sor = 1.0f;
+        world.GetSolverInfo().m_splitImpulse = false;
+        world.GetSolverInfo().m_splitImpulsePenetrationThreshold = -0.02f;
+        world.GetSolverInfo().m_linearSlop = 0.0f;
+        world.GetSolverInfo().m_warmstartingFactor = 0.85f;
+        world.GetSolverInfo().m_restingContactRestitutionThreshold = 2;
+        world.SetForceUpdateAllAabbs(true);
+
+        //BSParam.TerrainImplementation = 0;
+        world.SetGravity(new IndexedVector3(0,0,p.gravity));
+
+        return world;
+    }
+    //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 = (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);
+            constrain.SetParam((BulletXNA.BulletDynamics.ConstraintParams) (int) paramIndex, paramvalue, 1);
+            constrain.SetParam((BulletXNA.BulletDynamics.ConstraintParams) (int) paramIndex, paramvalue, 2);
+        }
+        if (axis == ConstraintParamAxis.AXIS_ANGULAR_ALL || axis == ConstraintParamAxis.AXIS_ALL)
+        {
+            constrain.SetParam((BulletXNA.BulletDynamics.ConstraintParams)(int)paramIndex, paramvalue, 3);
+            constrain.SetParam((BulletXNA.BulletDynamics.ConstraintParams)(int)paramIndex, paramvalue, 4);
+            constrain.SetParam((BulletXNA.BulletDynamics.ConstraintParams)(int)paramIndex, paramvalue, 5);
+        }
+        if (axis == ConstraintParamAxis.AXIS_LINEAR_ALL)
+        {
+            constrain.SetParam((BulletXNA.BulletDynamics.ConstraintParams)(int)paramIndex, paramvalue, (int)axis);
+        }
+        return true;
+    }
+
+    public override bool PushUpdate(BulletBody pCollisionObject)
+    {
+        bool ret = false;
+        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
+        RigidBody rb = collisionObject as RigidBody;
+        if (rb != null)
+        {
+            SimMotionState sms = rb.GetMotionState() as SimMotionState;
+            if (sms != null)
+            {
+                IndexedMatrix wt = IndexedMatrix.Identity;
+                sms.GetWorldTransform(out wt);
+                sms.SetWorldTransform(ref wt, true);
+                ret = true;
+            }
+        }
+        return ret;
+        
+    }
+
+    public override float GetAngularMotionDisc(BulletShape pShape)
+    {
+        CollisionShape shape = (pShape as BulletShapeXNA).shape;
+        return shape.GetAngularMotionDisc();
+    }
+    public override float GetContactBreakingThreshold(BulletShape pShape, float defaultFactor)
+    {
+        CollisionShape shape = (pShape as BulletShapeXNA).shape;
+        return shape.GetContactBreakingThreshold(defaultFactor);
+    }
+    public override bool IsCompound(BulletShape pShape)
+    {
+        CollisionShape shape = (pShape as BulletShapeXNA).shape;
+        return shape.IsCompound();
+    }
+    public override bool IsSoftBody(BulletShape pShape)
+    {
+        CollisionShape shape = (pShape as BulletShapeXNA).shape;
+        return shape.IsSoftBody();
+    }
+    public override bool IsPolyhedral(BulletShape pShape)
+    {
+        CollisionShape shape = (pShape as BulletShapeXNA).shape;
+        return shape.IsPolyhedral();
+    }
+    public override bool IsConvex2d(BulletShape pShape)
+    {
+        CollisionShape shape = (pShape as BulletShapeXNA).shape;
+        return shape.IsConvex2d();
+    }
+    public override bool IsConvex(BulletShape pShape)
+    {
+        CollisionShape shape = (pShape as BulletShapeXNA).shape;
+        return shape.IsConvex();
+    }
+    public override bool IsNonMoving(BulletShape pShape)
+    {
+        CollisionShape shape = (pShape as BulletShapeXNA).shape;
+        return shape.IsNonMoving();
+    }
+    public override bool IsConcave(BulletShape pShape)
+    {
+        CollisionShape shape = (pShape as BulletShapeXNA).shape;
+        return shape.IsConcave();
+    }
+    public override bool IsInfinite(BulletShape pShape)
+    {
+        CollisionShape shape = (pShape as BulletShapeXNA).shape;
+        return shape.IsInfinite();
+    }
+    public override bool IsNativeShape(BulletShape pShape)
+    {
+        CollisionShape shape = (pShape as BulletShapeXNA).shape;
+        bool ret;
+        switch (shape.GetShapeType())
+        {
+            case BroadphaseNativeTypes.BOX_SHAPE_PROXYTYPE:
+                case BroadphaseNativeTypes.CONE_SHAPE_PROXYTYPE:
+                case BroadphaseNativeTypes.SPHERE_SHAPE_PROXYTYPE:
+                case BroadphaseNativeTypes.CYLINDER_SHAPE_PROXYTYPE:
+                ret = true;
+                break;
+            default:
+                ret = false;
+                break;
+        }
+        return ret;
+    }
+
+    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 = (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 = (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 pCollisionObject, BulletShape pShape)
+    {
+        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()));
+    }
+
+    //(PhysicsScene.World.ptr, nativeShapeData)
+    public override BulletShape BuildNativeShape(BulletWorld pWorld, ShapeData pShapeData)
+    {
+        DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
+        CollisionShape shape = null;
+        switch (pShapeData.Type)
+        {
+            case BSPhysicsShapeType.SHAPE_BOX:
+                shape = new BoxShape(new IndexedVector3(0.5f,0.5f,0.5f));
+                break;
+            case BSPhysicsShapeType.SHAPE_CONE:
+                shape = new ConeShapeZ(0.5f, 1.0f);
+                break;
+            case BSPhysicsShapeType.SHAPE_CYLINDER:
+                shape = new CylinderShapeZ(new IndexedVector3(0.5f, 0.5f, 0.5f));
+                break;
+            case BSPhysicsShapeType.SHAPE_SPHERE:
+                shape = new SphereShape(0.5f);
+                break;
+
+        }
+        if (shape != null)
+        {
+            IndexedVector3 scaling = new IndexedVector3(pShapeData.Scale.X, pShapeData.Scale.Y, pShapeData.Scale.Z);
+            shape.SetMargin(world.WorldSettings.Params.collisionMargin);
+            shape.SetLocalScaling(ref scaling);
+
+        }
+        return new BulletShapeXNA(shape, pShapeData.Type);
+    }
+    //PhysicsScene.World.ptr, false
+    public override BulletShape CreateCompoundShape(BulletWorld pWorld, bool enableDynamicAabbTree)
+    {
+        return new BulletShapeXNA(new CompoundShape(enableDynamicAabbTree), BSPhysicsShapeType.SHAPE_COMPOUND);
+    }
+
+    public override int GetNumberOfCompoundChildren(BulletShape pCompoundShape)
+    {
+        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 = (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));
+        compoundshape.AddChildShape(ref relativeTransform, addshape);
+
+    }
+
+    public override BulletShape RemoveChildShapeFromCompoundShapeIndex(BulletShape pCShape, int pii)
+    {
+        CompoundShape compoundshape = (pCShape as BulletShapeXNA).shape as CompoundShape;
+        CollisionShape ret = null;
+        ret = compoundshape.GetChildShape(pii);
+        compoundshape.RemoveChildShapeByIndex(pii);
+        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 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)
+    {
+        StaticPlaneShape m_planeshape = new StaticPlaneShape(new IndexedVector3(0,0,1),(int)pheight );
+        m_planeshape.SetMargin(pcollisionMargin);
+        m_planeshape.SetUserPointer(pLocalId);
+        return new BulletShapeXNA(m_planeshape, BSPhysicsShapeType.SHAPE_GROUNDPLANE);
+    }
+
+    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 = (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);
+    }
+
+    public override BulletShape CreateHullShape(BulletWorld pWorld, int pHullCount, float[] pConvHulls)
+    {
+        DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
+        CompoundShape compoundshape = new CompoundShape(false);
+        
+        compoundshape.SetMargin(world.WorldSettings.Params.collisionMargin);
+        int ii = 1;
+
+        for (int i = 0; i < pHullCount; i++)
+        {
+            int vertexCount = (int) pConvHulls[ii];
+
+            IndexedVector3 centroid = new IndexedVector3(pConvHulls[ii + 1], pConvHulls[ii + 2], pConvHulls[ii + 3]);
+            IndexedMatrix childTrans = IndexedMatrix.Identity;
+            childTrans._origin = centroid;
+
+            List<IndexedVector3> virts = new List<IndexedVector3>();
+            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);
+            convexShape.SetMargin(world.WorldSettings.Params.collisionMargin);
+            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 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 = (pWorld as BulletWorldXNA).world;
+        IndexedMesh mesh = new IndexedMesh();
+        mesh.m_indexType = PHY_ScalarType.PHY_INTEGER;
+        mesh.m_numTriangles = pIndicesCount/3;
+        mesh.m_numVertices = pVerticesCount;
+        mesh.m_triangleIndexBase = indicesarr;
+        mesh.m_vertexBase = vertices;
+        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);
+        meshShape.SetMargin(world.WorldSettings.Params.collisionMargin);
+       // world.UpdateSingleAabb(meshShape);
+        return new BulletShapeXNA(meshShape, BSPhysicsShapeType.SHAPE_MESH);
+
+    }
+    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);
+        IndexedMesh mesh = new IndexedMesh();
+
+        mesh.m_indexType = PHY_ScalarType.PHY_INTEGER;
+        mesh.m_numTriangles = pIndicesCount / 3;
+        mesh.m_numVertices = pVerticesCount;
+        mesh.m_triangleIndexBase = indices;
+        mesh.m_vertexBase = vertices;
+        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);
+
+
+
+        for (int i = 0; i < pVerticesCount; i++)
+        {
+
+            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");
+        }
+
+        sw.Close();
+    }
+    public static void DumpRaw(int[] indices, float[] vertices, int pIndicesCount, int pVerticesCount)
+    {
+
+        String fileName = "objTest6.raw";
+        String completePath = System.IO.Path.Combine(Util.configDir(), fileName);
+        StreamWriter sw = new StreamWriter(completePath);
+        IndexedMesh mesh = new IndexedMesh();
+
+        mesh.m_indexType = PHY_ScalarType.PHY_INTEGER;
+        mesh.m_numTriangles = pIndicesCount / 3;
+        mesh.m_numVertices = pVerticesCount;
+        mesh.m_triangleIndexBase = indices;
+        mesh.m_vertexBase = vertices;
+        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);
+
+
+        sw.WriteLine("Indices");
+        sw.WriteLine(string.Format("int[] indices = new int[{0}];",pIndicesCount));
+        for (int iter = 0; iter < indices.Length; iter++)
+        {
+            sw.WriteLine(string.Format("indices[{0}]={1};",iter,indices[iter]));
+        }
+        sw.WriteLine("VerticesFloats");
+        sw.WriteLine(string.Format("float[] vertices = new float[{0}];", pVerticesCount));
+        for (int iter = 0; iter < vertices.Length; iter++)
+        {
+            sw.WriteLine(string.Format("Vertices[{0}]={1};", iter, vertices[iter].ToString("0.0000")));
+        }
+
+            // for (int i = 0; i < pVerticesCount; i++)
+            // {
+            //
+            //     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");
+            //}
+
+            sw.Close();
+    }
+
+    public override BulletShape CreateTerrainShape(uint id, Vector3 size, float minHeight, float maxHeight, float[] heightMap, 
+                                                                float scaleFactor, float collisionMargin)
+    {
+        const int upAxis = 2;
+        HeightfieldTerrainShape terrainShape = new HeightfieldTerrainShape((int)size.X, (int)size.Y,
+                                                                           heightMap,  scaleFactor,
+                                                                           minHeight, maxHeight, upAxis,
+                                                                            false);
+        terrainShape.SetMargin(collisionMargin + 0.5f);
+        terrainShape.SetUseDiamondSubdivision(true);
+        terrainShape.SetUserPointer(id);
+        return new BulletShapeXNA(terrainShape, BSPhysicsShapeType.SHAPE_TERRAIN);
+    }
+
+    public override bool TranslationalLimitMotor(BulletConstraint pConstraint, float ponOff, float targetVelocity, float maxMotorForce)
+    {
+        TypedConstraint tconstrain = (pConstraint as BulletConstraintXNA).constrain;
+        bool onOff = ponOff != 0;
+        bool ret = false;
+
+        switch (tconstrain.GetConstraintType())
+        {
+            case TypedConstraintType.D6_CONSTRAINT_TYPE:
+                Generic6DofConstraint constrain = tconstrain as Generic6DofConstraint;
+                constrain.GetTranslationalLimitMotor().m_enableMotor[0] = onOff;
+                constrain.GetTranslationalLimitMotor().m_targetVelocity[0] = targetVelocity;
+                constrain.GetTranslationalLimitMotor().m_maxMotorForce[0] = maxMotorForce;
+                ret = true;
+                break;
+        }
+
+
+        return ret;
+
+    }
+
+    public override int PhysicsStep(BulletWorld world, float timeStep, int maxSubSteps, float fixedTimeStep,
+                        out int updatedEntityCount, out int collidersCount)
+    {
+        /* TODO */
+        updatedEntityCount = 0;
+        collidersCount = 0;
+        
+
+        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 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, m_maxCollisions, m_maxUpdatesPerFrame);
+        return epic;
+    }
+
+    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;
+        
+
+
+
+
+
+        LastCollisionDesc=0;
+       
+        updatedEntityCount = 0;
+        collidersCount = 0;
+        
+
+        if (pWorld is BulletWorldXNA)
+        {
+            DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
+
+            world.LastCollisionDesc = 0;
+            world.LastEntityProperty = 0;
+            numSimSteps = world.StepSimulation(timeStep, m_maxSubSteps, m_fixedTimeStep);
+            int updates = 0;
+
+            PersistentManifold contactManifold;
+            CollisionObject objA;
+            CollisionObject objB;
+            ManifoldPoint manifoldPoint;
+            PairCachingGhostObject pairCachingGhostObject;
+
+            m_collisionsThisFrame = 0;
+            int numManifolds = world.GetDispatcher().GetNumManifolds();
+            for (int j = 0; j < numManifolds; j++)
+            {
+                contactManifold = world.GetDispatcher().GetManifoldByIndexInternal(j);
+                int numContacts = contactManifold.GetNumContacts();
+                if (numContacts == 0)
+                    continue;
+
+                objA = contactManifold.GetBody0() as CollisionObject;
+                objB = contactManifold.GetBody1() as CollisionObject;
+
+                manifoldPoint = contactManifold.GetContactPoint(0);
+                //IndexedVector3 contactPoint = manifoldPoint.GetPositionWorldOnB();
+               // IndexedVector3 contactNormal = -manifoldPoint.m_normalWorldOnB; // make relative to A
+
+                RecordCollision(this, objA, objB, manifoldPoint.GetPositionWorldOnB(), -manifoldPoint.m_normalWorldOnB, manifoldPoint.GetDistance());
+                m_collisionsThisFrame ++;
+                if (m_collisionsThisFrame >= 9999999)
+                    break;
+
+
+            }
+
+            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;
+            
+            
+            //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;
+
+        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)
+        {
+            return;
+        }
+        uint idA = (uint)objA.GetUserPointer();
+        uint idB = (uint)objB.GetUserPointer();
+        if (idA > idB)
+        {
+            uint temp = idA;
+            idA = idB;
+            idB = temp;
+            contactNormal = -contactNormal;
+        }
+
+        //ulong collisionID = ((ulong) idA << 32) | idB;
+
+        CollisionDesc cDesc = new CollisionDesc()
+                                            {
+                                                aID = idA,
+                                                bID = idB,
+                                                point = new Vector3(contact.X,contact.Y,contact.Z),
+                                                normal = new Vector3(contactNormal.X,contactNormal.Y,contactNormal.Z),
+                                                penetration = penetration
+
+                                            };
+        if (world.LastCollisionDesc < world.UpdatedCollisions.Length)
+            world.UpdatedCollisions[world.LastCollisionDesc++] = (cDesc);
+        m_collisionsThisFrame++;
+
+
+    }
+    private static EntityProperties GetDebugProperties(BulletWorld pWorld, BulletBody pCollisionObject)
+    {
+        EntityProperties ent = new EntityProperties();
+        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)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);
+        ent.RotationalVelocity = new Vector3(AngularVelocity.X, AngularVelocity.Y, AngularVelocity.Z);
+        return ent;
+    }
+
+    public override bool UpdateParameter(BulletWorld world, uint localID, String parm, float value) { /* TODO */
+        return false; }
+
+    public override Vector3 GetLocalScaling(BulletShape pShape)
+    {
+        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 = (pWorld as BulletWorldXNA).world;
+        if (world != null)
+        {
+            if (NotMe is BulletBodyXNA && NotMe.HasPhysicalBody)
+            {
+                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 = 
+                                            new ClosestNotMeRayResultCallback(rOrigin, rEnd, AvoidBody)
+                    )
+                {
+                    world.RayTest(ref rOrigin, ref rEnd, rayCallback);
+                    if (rayCallback.HasHit())
+                    {
+                        IndexedVector3 hitLocation = rayCallback.m_hitPointWorld;
+                    }
+                    return rayCallback.HasHit();
+                }
+           }
+        }
+        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/BSApiTemplate.cs b/OpenSim/Region/Physics/BulletSPlugin/BSApiTemplate.cs
new file mode 100644
index 0000000..5765b0d
--- /dev/null
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSApiTemplate.cs
@@ -0,0 +1,683 @@
+/*
+ * 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.
+ */
+using System;
+using System.Collections.Generic;
+using System.Runtime.InteropServices;
+using System.Security;
+using System.Text;
+using OpenMetaverse;
+
+namespace OpenSim.Region.Physics.BulletSPlugin {
+
+    // Constraint type values as defined by Bullet
+public enum ConstraintType : int
+{
+        POINT2POINT_CONSTRAINT_TYPE = 3,
+        HINGE_CONSTRAINT_TYPE,
+        CONETWIST_CONSTRAINT_TYPE,
+        D6_CONSTRAINT_TYPE,
+        SLIDER_CONSTRAINT_TYPE,
+        CONTACT_CONSTRAINT_TYPE,
+        D6_SPRING_CONSTRAINT_TYPE,
+        MAX_CONSTRAINT_TYPE
+}
+
+// ===============================================================================
+[StructLayout(LayoutKind.Sequential)]
+public struct ConvexHull
+{
+        Vector3 Offset;
+        int VertexCount;
+        Vector3[] Vertices;
+}
+public enum BSPhysicsShapeType
+{
+        SHAPE_UNKNOWN   = 0,
+        SHAPE_CAPSULE   = 1,
+        SHAPE_BOX       = 2,
+        SHAPE_CONE      = 3,
+        SHAPE_CYLINDER  = 4,
+        SHAPE_SPHERE    = 5,
+        SHAPE_MESH      = 6,
+        SHAPE_HULL      = 7,
+    // following defined by BulletSim
+        SHAPE_GROUNDPLANE  = 20,
+        SHAPE_TERRAIN   = 21,
+        SHAPE_COMPOUND  = 22,
+        SHAPE_HEIGHTMAP = 23,
+    SHAPE_AVATAR    = 24,
+};
+
+// The native shapes have predefined shape hash keys
+public enum FixedShapeKey : ulong
+{
+    KEY_NONE        = 0,
+    KEY_BOX         = 1,
+    KEY_SPHERE      = 2,
+    KEY_CONE        = 3,
+    KEY_CYLINDER    = 4,
+    KEY_CAPSULE     = 5,
+    KEY_AVATAR      = 6,
+}
+
+[StructLayout(LayoutKind.Sequential)]
+public struct ShapeData
+{
+    public UInt32 ID;
+    public BSPhysicsShapeType Type;
+    public Vector3 Position;
+    public Quaternion Rotation;
+    public Vector3 Velocity;
+    public Vector3 Scale;
+    public float Mass;
+    public float Buoyancy;
+    public System.UInt64 HullKey;
+    public System.UInt64 MeshKey;
+    public float Friction;
+    public float Restitution;
+    public float Collidable;    // true of things bump into this
+    public float Static;        // true if a static object. Otherwise gravity, etc.
+    public float Solid;         // true if object cannot be passed through
+    public Vector3 Size;
+
+    // note that bools are passed as floats since bool size changes by language and architecture
+    public const float numericTrue = 1f;
+    public const float numericFalse = 0f;
+}
+[StructLayout(LayoutKind.Sequential)]
+public struct SweepHit
+{
+    public UInt32 ID;
+    public float Fraction;
+    public Vector3 Normal;
+    public Vector3 Point;
+}
+[StructLayout(LayoutKind.Sequential)]
+public struct RaycastHit
+{
+    public UInt32 ID;
+    public float Fraction;
+    public Vector3 Normal;
+}
+[StructLayout(LayoutKind.Sequential)]
+public struct CollisionDesc
+{
+    public UInt32 aID;
+    public UInt32 bID;
+    public Vector3 point;
+    public Vector3 normal;
+    public float penetration;
+}
+[StructLayout(LayoutKind.Sequential)]
+public struct EntityProperties
+{
+    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
+// NOTE: adding the X causes compile breaks if used. These are unused symbols
+//      that can be removed from both here and the unmanaged definition of this structure.
+[StructLayout(LayoutKind.Sequential)]
+public struct ConfigurationParameters
+{
+    public float defaultFriction;
+    public float defaultDensity;
+    public float defaultRestitution;
+    public float collisionMargin;
+    public float gravity;
+
+        public float maxPersistantManifoldPoolSize;
+        public float maxCollisionAlgorithmPoolSize;
+        public float shouldDisableContactPoolDynamicAllocation;
+        public float shouldForceUpdateAllAabbs;
+        public float shouldRandomizeSolverOrder;
+        public float shouldSplitSimulationIslands;
+        public float shouldEnableFrictionCaching;
+        public float numberOfSolverIterations;
+    public float useSingleSidedMeshes;
+        public float globalContactBreakingThreshold;
+
+    public float physicsLoggingFrames;
+
+    public const float numericTrue = 1f;
+    public const float numericFalse = 0f;
+}
+
+
+// The states a bullet collision object can have
+public enum ActivationState : uint
+{
+    ACTIVE_TAG = 1,
+    ISLAND_SLEEPING,
+    WANTS_DEACTIVATION,
+    DISABLE_DEACTIVATION,
+    DISABLE_SIMULATION,
+}
+
+public enum CollisionObjectTypes : int
+{
+    CO_COLLISION_OBJECT             = 1 << 0,
+    CO_RIGID_BODY                   = 1 << 1,
+    CO_GHOST_OBJECT                 = 1 << 2,
+    CO_SOFT_BODY                    = 1 << 3,
+    CO_HF_FLUID                     = 1 << 4,
+    CO_USER_TYPE                    = 1 << 5,
+}
+
+// Values used by Bullet and BulletSim to control object properties.
+// Bullet's "CollisionFlags" has more to do with operations on the
+//    object (if collisions happen, if gravity effects it, ...).
+public enum CollisionFlags : uint
+{
+    CF_STATIC_OBJECT                 = 1 << 0,
+    CF_KINEMATIC_OBJECT              = 1 << 1,
+    CF_NO_CONTACT_RESPONSE           = 1 << 2,
+    CF_CUSTOM_MATERIAL_CALLBACK      = 1 << 3,
+    CF_CHARACTER_OBJECT              = 1 << 4,
+    CF_DISABLE_VISUALIZE_OBJECT      = 1 << 5,
+    CF_DISABLE_SPU_COLLISION_PROCESS = 1 << 6,
+    // Following used by BulletSim to control collisions and updates
+    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
+};
+
+// Values f collisions groups and masks
+public enum CollisionFilterGroups : uint
+{
+    // Don't use the bit definitions!!  Define the use in a
+    //   filter/mask definition below. This way collision interactions
+    //   are more easily found and debugged.
+    BNoneGroup              = 0,
+    BDefaultGroup           = 1 << 0,   // 0001
+    BStaticGroup            = 1 << 1,   // 0002
+    BKinematicGroup         = 1 << 2,   // 0004
+    BDebrisGroup            = 1 << 3,   // 0008
+    BSensorTrigger          = 1 << 4,   // 0010
+    BCharacterGroup         = 1 << 5,   // 0020
+    BAllGroup               = 0x000FFFFF,
+    // Filter groups defined by BulletSim
+    BGroundPlaneGroup       = 1 << 10,  // 0400
+    BTerrainGroup           = 1 << 11,  // 0800
+    BRaycastGroup           = 1 << 12,  // 1000
+    BSolidGroup             = 1 << 13,  // 2000
+    // BLinksetGroup        = xx  // a linkset proper is either static or dynamic
+    BLinksetChildGroup      = 1 << 14,  // 4000
+};
+
+// CFM controls the 'hardness' of the constraint. 0=fixed, 0..1=violatable. Default=0
+// ERP controls amount of correction per tick. Usable range=0.1..0.8. Default=0.2.
+public enum ConstraintParams : int
+{
+    BT_CONSTRAINT_ERP = 1,  // this one is not used in Bullet as of 20120730
+    BT_CONSTRAINT_STOP_ERP,
+    BT_CONSTRAINT_CFM,
+    BT_CONSTRAINT_STOP_CFM,
+};
+public enum ConstraintParamAxis : int
+{
+    AXIS_LINEAR_X = 0,
+    AXIS_LINEAR_Y,
+    AXIS_LINEAR_Z,
+    AXIS_ANGULAR_X,
+    AXIS_ANGULAR_Y,
+    AXIS_ANGULAR_Z,
+    AXIS_LINEAR_ALL = 20,    // these last three added by BulletSim so we don't have to do zillions of calls
+    AXIS_ANGULAR_ALL,
+    AXIS_ALL
+};
+
+public abstract class BSAPITemplate
+{
+// Returns the name of the underlying Bullet engine
+public abstract string BulletEngineName { get; }
+public abstract string BulletEngineVersion { get; protected set;} 
+
+// Initialization and simulation
+public abstract BulletWorld Initialize(Vector3 maxPosition, ConfigurationParameters parms,
+                                                                                        int maxCollisions,  ref CollisionDesc[] collisionArray,
+                                                                                        int maxUpdates, ref EntityProperties[] updateArray
+                                            );
+
+public abstract int PhysicsStep(BulletWorld world, float timeStep, int maxSubSteps, float fixedTimeStep,
+                        out int updatedEntityCount, out int collidersCount);
+
+public abstract bool UpdateParameter(BulletWorld world, UInt32 localID, String parm, float value);
+
+public abstract void Shutdown(BulletWorld sim);
+
+public abstract bool PushUpdate(BulletBody obj);
+
+// =====================================================================================
+// Mesh, hull, shape and body creation helper routines
+public abstract BulletShape CreateMeshShape(BulletWorld world,
+                int indicesCount, int[] indices,
+                int verticesCount, float[] vertices );
+
+public abstract BulletShape CreateHullShape(BulletWorld world,
+                int hullCount, float[] hulls);
+
+public abstract BulletShape BuildHullShapeFromMesh(BulletWorld world, BulletShape meshShape);
+
+public abstract BulletShape BuildNativeShape(BulletWorld world, ShapeData shapeData);
+
+public abstract bool IsNativeShape(BulletShape shape);
+
+public abstract void SetShapeCollisionMargin(BulletShape shape, float margin);
+
+public abstract BulletShape BuildCapsuleShape(BulletWorld world, float radius, float height, Vector3 scale);
+
+public abstract BulletShape CreateCompoundShape(BulletWorld sim, bool enableDynamicAabbTree);
+
+public abstract int GetNumberOfCompoundChildren(BulletShape cShape);
+
+public abstract void AddChildShapeToCompoundShape(BulletShape cShape, BulletShape addShape, Vector3 pos, Quaternion rot);
+
+public abstract BulletShape GetChildShapeFromCompoundShapeIndex(BulletShape cShape, int indx);
+
+public abstract BulletShape RemoveChildShapeFromCompoundShapeIndex(BulletShape cShape, int indx);
+
+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, UInt32 id);
+
+public abstract bool DeleteCollisionShape(BulletWorld world, BulletShape shape);
+
+public abstract CollisionObjectTypes GetBodyType(BulletBody obj);
+
+public abstract BulletBody CreateBodyFromShape(BulletWorld sim, BulletShape shape, UInt32 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, UInt32 id, Vector3 pos, Quaternion rot);
+
+public abstract void DestroyObject(BulletWorld sim, BulletBody obj);
+
+// =====================================================================================
+public abstract BulletShape CreateGroundPlaneShape(UInt32 id, float height, float collisionMargin);
+
+public abstract BulletShape CreateTerrainShape(UInt32 id, Vector3 size, float minHeight, float maxHeight, float[] heightMap, 
+                                                                float scaleFactor, float collisionMargin);
+
+// =====================================================================================
+// Constraint creation and helper routines
+public abstract BulletConstraint Create6DofConstraint(BulletWorld world, BulletBody obj1, BulletBody obj2,
+                    Vector3 frame1loc, Quaternion frame1rot,
+                    Vector3 frame2loc, Quaternion frame2rot,
+                    bool useLinearReferenceFrameA, bool disableCollisionsBetweenLinkedBodies);
+
+public abstract BulletConstraint Create6DofConstraintToPoint(BulletWorld world, BulletBody obj1, BulletBody obj2,
+                    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);
+
+public abstract bool SetFrames(BulletConstraint constrain,
+                Vector3 frameA, Quaternion frameArot, Vector3 frameB, Quaternion frameBrot);
+
+public abstract bool SetLinearLimits(BulletConstraint constrain, Vector3 low, Vector3 hi);
+
+public abstract bool SetAngularLimits(BulletConstraint constrain, Vector3 low, Vector3 hi);
+
+public abstract bool UseFrameOffset(BulletConstraint constrain, float enable);
+
+public abstract bool TranslationalLimitMotor(BulletConstraint constrain, float enable, float targetVel, float maxMotorForce);
+
+public abstract bool SetBreakingImpulseThreshold(BulletConstraint constrain, float threshold);
+
+public abstract bool CalculateTransforms(BulletConstraint constrain);
+
+public abstract bool SetConstraintParam(BulletConstraint constrain, ConstraintParams paramIndex, float value, ConstraintParamAxis axis);
+
+public abstract bool DestroyConstraint(BulletWorld world, BulletConstraint constrain);
+
+// =====================================================================================
+// btCollisionWorld entries
+public abstract void UpdateSingleAabb(BulletWorld world, BulletBody obj);
+
+public abstract void UpdateAabbs(BulletWorld world);
+
+public abstract bool GetForceUpdateAllAabbs(BulletWorld world);
+
+public abstract void SetForceUpdateAllAabbs(BulletWorld world, bool force);
+
+// =====================================================================================
+// btDynamicsWorld entries
+// public abstract bool AddObjectToWorld(BulletWorld world, BulletBody obj, Vector3 pos, Quaternion rot);
+public abstract bool AddObjectToWorld(BulletWorld world, BulletBody obj);
+
+public abstract bool RemoveObjectFromWorld(BulletWorld world, BulletBody obj);
+
+public abstract bool AddConstraintToWorld(BulletWorld world, BulletConstraint constrain, bool disableCollisionsBetweenLinkedObjects);
+
+public abstract bool RemoveConstraintFromWorld(BulletWorld world, BulletConstraint constrain);
+// =====================================================================================
+// btCollisionObject entries
+public abstract Vector3 GetAnisotripicFriction(BulletConstraint constrain);
+
+public abstract Vector3 SetAnisotripicFriction(BulletConstraint constrain, Vector3 frict);
+
+public abstract bool HasAnisotripicFriction(BulletConstraint constrain);
+
+public abstract void SetContactProcessingThreshold(BulletBody obj, float val);
+
+public abstract float GetContactProcessingThreshold(BulletBody obj);
+
+public abstract bool IsStaticObject(BulletBody obj);
+
+public abstract bool IsKinematicObject(BulletBody obj);
+
+public abstract bool IsStaticOrKinematicObject(BulletBody obj);
+
+public abstract bool HasContactResponse(BulletBody obj);
+
+public abstract void SetCollisionShape(BulletWorld sim, BulletBody obj, BulletShape shape);
+
+public abstract BulletShape GetCollisionShape(BulletBody obj);
+
+public abstract int GetActivationState(BulletBody obj);
+
+public abstract void SetActivationState(BulletBody obj, int state);
+
+public abstract void SetDeactivationTime(BulletBody obj, float dtime);
+
+public abstract float GetDeactivationTime(BulletBody obj);
+
+public abstract void ForceActivationState(BulletBody obj, ActivationState state);
+
+public abstract void Activate(BulletBody obj, bool forceActivation);
+
+public abstract bool IsActive(BulletBody obj);
+
+public abstract void SetRestitution(BulletBody obj, float val);
+
+public abstract float GetRestitution(BulletBody obj);
+
+public abstract void SetFriction(BulletBody obj, float val);
+
+public abstract float GetFriction(BulletBody obj);
+
+public abstract Vector3 GetPosition(BulletBody obj);
+
+public abstract Quaternion GetOrientation(BulletBody obj);
+
+public abstract void SetTranslation(BulletBody obj, Vector3 position, Quaternion rotation);
+
+// public abstract IntPtr GetBroadphaseHandle(BulletBody obj);
+
+// public abstract void SetBroadphaseHandle(BulletBody obj, IntPtr handle);
+
+public abstract void SetInterpolationLinearVelocity(BulletBody obj, Vector3 vel);
+
+public abstract void SetInterpolationAngularVelocity(BulletBody obj, Vector3 vel);
+
+public abstract void SetInterpolationVelocity(BulletBody obj, Vector3 linearVel, Vector3 angularVel);
+
+public abstract float GetHitFraction(BulletBody obj);
+
+public abstract void SetHitFraction(BulletBody obj, float val);
+
+public abstract CollisionFlags GetCollisionFlags(BulletBody obj);
+
+public abstract CollisionFlags SetCollisionFlags(BulletBody obj, CollisionFlags flags);
+
+public abstract CollisionFlags AddToCollisionFlags(BulletBody obj, CollisionFlags flags);
+
+public abstract CollisionFlags RemoveFromCollisionFlags(BulletBody obj, CollisionFlags flags);
+
+public abstract float GetCcdMotionThreshold(BulletBody obj);
+
+public abstract void SetCcdMotionThreshold(BulletBody obj, float val);
+
+public abstract float GetCcdSweptSphereRadius(BulletBody obj);
+
+public abstract void SetCcdSweptSphereRadius(BulletBody obj, float val);
+
+public abstract IntPtr GetUserPointer(BulletBody obj);
+
+public abstract void SetUserPointer(BulletBody obj, IntPtr val);
+
+// =====================================================================================
+// btRigidBody entries
+public abstract void ApplyGravity(BulletBody obj);
+
+public abstract void SetGravity(BulletBody obj, Vector3 val);
+
+public abstract Vector3 GetGravity(BulletBody obj);
+
+public abstract void SetDamping(BulletBody obj, float lin_damping, float ang_damping);
+
+public abstract void SetLinearDamping(BulletBody obj, float lin_damping);
+
+public abstract void SetAngularDamping(BulletBody obj, float ang_damping);
+
+public abstract float GetLinearDamping(BulletBody obj);
+
+public abstract float GetAngularDamping(BulletBody obj);
+
+public abstract float GetLinearSleepingThreshold(BulletBody obj);
+
+public abstract void ApplyDamping(BulletBody obj, float timeStep);
+
+public abstract void SetMassProps(BulletBody obj, float mass, Vector3 inertia);
+
+public abstract Vector3 GetLinearFactor(BulletBody obj);
+
+public abstract void SetLinearFactor(BulletBody obj, Vector3 factor);
+
+public abstract void SetCenterOfMassByPosRot(BulletBody obj, Vector3 pos, Quaternion rot);
+
+// Add a force to the object as if its mass is one.
+public abstract void ApplyCentralForce(BulletBody obj, Vector3 force);
+
+// Set the force being applied to the object as if its mass is one.
+public abstract void SetObjectForce(BulletBody obj, Vector3 force);
+
+public abstract Vector3 GetTotalForce(BulletBody obj);
+
+public abstract Vector3 GetTotalTorque(BulletBody obj);
+
+public abstract Vector3 GetInvInertiaDiagLocal(BulletBody obj);
+
+public abstract void SetInvInertiaDiagLocal(BulletBody obj, Vector3 inert);
+
+public abstract void SetSleepingThresholds(BulletBody obj, float lin_threshold, float ang_threshold);
+
+public abstract void ApplyTorque(BulletBody obj, Vector3 torque);
+
+// Apply force at the given point. Will add torque to the object.
+public abstract void ApplyForce(BulletBody obj, Vector3 force, Vector3 pos);
+
+// Apply impulse to the object. Same as "ApplycentralForce" but force scaled by object's mass.
+public abstract void ApplyCentralImpulse(BulletBody obj, Vector3 imp);
+
+// Apply impulse to the object's torque. Force is scaled by object's mass.
+public abstract 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.
+public abstract void ApplyImpulse(BulletBody obj, Vector3 imp, Vector3 pos);
+
+public abstract void ClearForces(BulletBody obj);
+
+public abstract void ClearAllForces(BulletBody obj);
+
+public abstract void UpdateInertiaTensor(BulletBody obj);
+
+public abstract Vector3 GetLinearVelocity(BulletBody obj);
+
+public abstract Vector3 GetAngularVelocity(BulletBody obj);
+
+public abstract void SetLinearVelocity(BulletBody obj, Vector3 val);
+
+public abstract void SetAngularVelocity(BulletBody obj, Vector3 angularVelocity);
+
+public abstract Vector3 GetVelocityInLocalPoint(BulletBody obj, Vector3 pos);
+
+public abstract void Translate(BulletBody obj, Vector3 trans);
+
+public abstract void UpdateDeactivation(BulletBody obj, float timeStep);
+
+public abstract bool WantsSleeping(BulletBody obj);
+
+public abstract void SetAngularFactor(BulletBody obj, float factor);
+
+public abstract void SetAngularFactorV(BulletBody obj, Vector3 factor);
+
+public abstract Vector3 GetAngularFactor(BulletBody obj);
+
+public abstract bool IsInWorld(BulletWorld world, BulletBody obj);
+
+public abstract void AddConstraintRef(BulletBody obj, BulletConstraint constrain);
+
+public abstract void RemoveConstraintRef(BulletBody obj, BulletConstraint constrain);
+
+public abstract BulletConstraint GetConstraintRef(BulletBody obj, int index);
+
+public abstract int GetNumConstraintRefs(BulletBody obj);
+
+public abstract bool SetCollisionGroupMask(BulletBody body, UInt32 filter, UInt32 mask);
+
+// =====================================================================================
+// btCollisionShape entries
+
+public abstract float GetAngularMotionDisc(BulletShape shape);
+
+public abstract float GetContactBreakingThreshold(BulletShape shape, float defaultFactor);
+
+public abstract bool IsPolyhedral(BulletShape shape);
+
+public abstract bool IsConvex2d(BulletShape shape);
+
+public abstract bool IsConvex(BulletShape shape);
+
+public abstract bool IsNonMoving(BulletShape shape);
+
+public abstract bool IsConcave(BulletShape shape);
+
+public abstract bool IsCompound(BulletShape shape);
+
+public abstract bool IsSoftBody(BulletShape shape);
+
+public abstract bool IsInfinite(BulletShape shape);
+
+public abstract void SetLocalScaling(BulletShape shape, Vector3 scale);
+
+public abstract Vector3 GetLocalScaling(BulletShape shape);
+
+public abstract Vector3 CalculateLocalInertia(BulletShape shape, float mass);
+
+public abstract int GetShapeType(BulletShape shape);
+
+public abstract void SetMargin(BulletShape shape, float val);
+
+public abstract float GetMargin(BulletShape shape);
+
+// =====================================================================================
+// Debugging
+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 virtual void DumpActivationInfo(BulletWorld sim) { }
+
+public virtual void DumpAllInfo(BulletWorld sim) { }
+
+public virtual void DumpPhysicsStatistics(BulletWorld sim) { }
+
+public virtual void ResetBroadphasePool(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 4c195e1..90c2d9c 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
@@ -45,7 +45,6 @@ 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;
@@ -58,16 +57,12 @@ 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 OMV.Vector3 _appliedVelocity;   // the last velocity applied to the avatar
-    private float _currentFriction;         // the friction currently being used (changed by setVelocity).
+    private BSVMotor _velocityMotor;
 
     private OMV.Vector3 _PIDTarget;
     private bool _usePID;
@@ -83,34 +78,36 @@ public sealed class BSCharacter : BSPhysObject
         _physicsActorType = (int)ActorTypes.Agent;
         _position = pos;
 
-        // Old versions of ScenePresence passed only the height. If width and/or depth are zero,
-        //     replace with the default values.
-        _size = size;
-        if (_size.X == 0f) _size.X = PhysicsScene.Params.avatarCapsuleDepth;
-        if (_size.Y == 0f) _size.Y = PhysicsScene.Params.avatarCapsuleWidth;
-
         _flying = isFlying;
         _orientation = OMV.Quaternion.Identity;
         _velocity = OMV.Vector3.Zero;
-        _appliedVelocity = OMV.Vector3.Zero;
         _buoyancy = ComputeBuoyancyFromFlying(isFlying);
-        _currentFriction = PhysicsScene.Params.avatarStandingFriction;
-        _avatarDensity = PhysicsScene.Params.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.
+        _size = size;
+        if (_size.X == 0f) _size.X = BSParam.AvatarCapsuleDepth;
+        if (_size.Y == 0f) _size.Y = BSParam.AvatarCapsuleWidth;
 
-        // The dimensions of the avatar capsule are kept in the scale.
+        // The dimensions of the physical capsule are kept in the scale.
         // Physics creates a unit capsule which is scaled by the physics engine.
-        ComputeAvatarScale(_size);
+        Scale = ComputeAvatarScale(_size);
         // set _avatarVolume and _mass based on capsule size, _density and Scale
         ComputeAvatarVolumeAndMass();
+
+        SetupMovementMotor();
+
         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 create at taint time
+        // do actual creation in taint time
         PhysicsScene.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, null, null);
+            PhysicsScene.Shapes.GetBodyAndShape(true, PhysicsScene.World, this);
 
             SetPhysicalProperties();
         });
@@ -120,54 +117,216 @@ public sealed class BSCharacter : BSPhysObject
     // called when this character is being destroyed and the resources should be released
     public override void Destroy()
     {
+        base.Destroy();
+
         DetailLog("{0},BSCharacter.Destroy", LocalID);
         PhysicsScene.TaintedObject("BSCharacter.destroy", delegate()
         {
-            PhysicsScene.Shapes.DereferenceBody(PhysBody, true, null);
-            PhysicsScene.Shapes.DereferenceShape(PhysShape, true, null);
+            PhysicsScene.Shapes.DereferenceBody(PhysBody, null /* bodyCallback */);
+            PhysBody.Clear();
+            PhysicsScene.Shapes.DereferenceShape(PhysShape, null /* bodyCallback */);
+            PhysShape.Clear();
         });
     }
 
     private void SetPhysicalProperties()
     {
-        BulletSimAPI.RemoveObjectFromWorld2(PhysicsScene.World.ptr, PhysBody.ptr);
+        PhysicsScene.PE.RemoveObjectFromWorld(PhysicsScene.World, PhysBody);
 
         ZeroMotion(true);
         ForcePosition = _position;
-        // Set the velocity and compute the proper friction
+
+        // Set the velocity
+        _velocityMotor.Reset();
+        _velocityMotor.SetTarget(_velocity);
+        _velocityMotor.SetCurrent(_velocity);
         ForceVelocity = _velocity;
 
         // 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;
 
-        BulletSimAPI.SetRestitution2(PhysBody.ptr, PhysicsScene.Params.avatarRestitution);
-        BulletSimAPI.SetMargin2(PhysShape.ptr, PhysicsScene.Params.collisionMargin);
-        BulletSimAPI.SetLocalScaling2(PhysShape.ptr, Scale);
-        BulletSimAPI.SetContactProcessingThreshold2(PhysBody.ptr, PhysicsScene.Params.contactProcessingThreshold);
-        if (PhysicsScene.Params.ccdMotionThreshold > 0f)
+        PhysicsScene.PE.SetRestitution(PhysBody, BSParam.AvatarRestitution);
+        PhysicsScene.PE.SetMargin(PhysShape, PhysicsScene.Params.collisionMargin);
+        PhysicsScene.PE.SetLocalScaling(PhysShape, Scale);
+        PhysicsScene.PE.SetContactProcessingThreshold(PhysBody, BSParam.ContactProcessingThreshold);
+        if (BSParam.CcdMotionThreshold > 0f)
         {
-            BulletSimAPI.SetCcdMotionThreshold2(PhysBody.ptr, PhysicsScene.Params.ccdMotionThreshold);
-            BulletSimAPI.SetCcdSweptSphereRadius2(PhysBody.ptr, PhysicsScene.Params.ccdSweptSphereRadius);
+            PhysicsScene.PE.SetCcdMotionThreshold(PhysBody, BSParam.CcdMotionThreshold);
+            PhysicsScene.PE.SetCcdSweptSphereRadius(PhysBody, BSParam.CcdSweptSphereRadius);
         }
 
-        UpdatePhysicalMassProperties(RawMass);
+        UpdatePhysicalMassProperties(RawMass, false);
 
         // Make so capsule does not fall over
-        BulletSimAPI.SetAngularFactorV2(PhysBody.ptr, OMV.Vector3.Zero);
+        PhysicsScene.PE.SetAngularFactorV(PhysBody, OMV.Vector3.Zero);
 
-        BulletSimAPI.AddToCollisionFlags2(PhysBody.ptr, CollisionFlags.CF_CHARACTER_OBJECT);
+        PhysicsScene.PE.AddToCollisionFlags(PhysBody, CollisionFlags.CF_CHARACTER_OBJECT);
 
-        BulletSimAPI.AddObjectToWorld2(PhysicsScene.World.ptr, PhysBody.ptr);
+        PhysicsScene.PE.AddObjectToWorld(PhysicsScene.World, PhysBody);
 
-        // BulletSimAPI.ForceActivationState2(BSBody.ptr, ActivationState.ACTIVE_TAG);
-        BulletSimAPI.ForceActivationState2(PhysBody.ptr, ActivationState.DISABLE_DEACTIVATION);
-        BulletSimAPI.UpdateSingleAabb2(PhysicsScene.World.ptr, PhysBody.ptr);
+        // PhysicsScene.PE.ForceActivationState(PhysBody, ActivationState.ACTIVE_TAG);
+        PhysicsScene.PE.ForceActivationState(PhysBody, ActivationState.DISABLE_DEACTIVATION);
+        PhysicsScene.PE.UpdateSingleAabb(PhysicsScene.World, PhysBody);
 
         // Do this after the object has been added to the world
-        BulletSimAPI.SetCollisionFilterMask2(PhysBody.ptr,
-                        (uint)CollisionFilterGroups.AvatarFilter,
-                        (uint)CollisionFilterGroups.AvatarMask);
+        PhysBody.collisionType = CollisionType.Avatar;
+        PhysBody.ApplyCollisionMask(PhysicsScene);
+    }
+
+    // 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()
+    {
+        // 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.
+
+            // 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.
+
+            _velocityMotor.Step(timeStep);
+
+            // If we're not supposed to be moving, make sure things are zero.
+            if (_velocityMotor.ErrorIsZero() && _velocityMotor.TargetValue == OMV.Vector3.Zero)
+            {
+                // The avatar shouldn't be moving
+                _velocityMotor.Zero();
+
+                if (IsColliding)
+                {
+                    // If we are colliding with a stationary object, presume we're standing and don't move around
+                    if (!ColliderIsMoving)
+                    {
+                        DetailLog("{0},BSCharacter.MoveMotor,collidingWithStationary,zeroingMotion", LocalID);
+                        ZeroMotion(true /* inTaintTime */);
+                    }
+
+                    // Standing has more friction on the ground
+                    if (Friction != BSParam.AvatarStandingFriction)
+                    {
+                        Friction = BSParam.AvatarStandingFriction;
+                        PhysicsScene.PE.SetFriction(PhysBody, Friction);
+                    }
+                }
+                else
+                {
+                    if (Flying)
+                    {
+                        // Flying and not collising and velocity nearly zero.
+                        ZeroMotion(true /* inTaintTime */);
+                    }
+                }
+
+                DetailLog("{0},BSCharacter.MoveMotor,taint,stopping,target={1},colliding={2}", LocalID, _velocityMotor.TargetValue, IsColliding);
+            }
+            else
+            {
+                // Supposed to be moving.
+                OMV.Vector3 stepVelocity = _velocityMotor.CurrentValue;
+
+                if (Friction != BSParam.AvatarFriction)
+                {
+                    // Probably starting up walking. Set friction to moving friction.
+                    Friction = BSParam.AvatarFriction;
+                    PhysicsScene.PE.SetFriction(PhysBody, Friction);
+                }
+
+                // If falling, we keep the world's downward vector no matter what the other axis specify.
+                // The check for _velocity.Z < 0 makes jumping work (temporary upward force).
+                if (!Flying && !IsColliding)
+                {
+                    if (_velocity.Z < 0)
+                        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;
+
+                // Should we check for move force being small and forcing velocity to zero?
+
+                // Add special movement force to allow avatars to walk up stepped surfaces.
+                moveForce += WalkUpStairs();
+
+                DetailLog("{0},BSCharacter.MoveMotor,move,stepVel={1},vel={2},mass={3},moveForce={4}", LocalID, stepVelocity, _velocity, Mass, moveForce);
+                PhysicsScene.PE.ApplyCentralImpulse(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;
+
+        // This test is done if moving forward, not flying and is colliding with something.
+        // DetailLog("{0},BSCharacter.WalkUpStairs,IsColliding={1},flying={2},targSpeed={3},collisions={4}",
+        //                 LocalID, IsColliding, Flying, TargetSpeed, CollisionsLastTick.Count);
+        if (IsColliding && !Flying && TargetVelocitySpeed > 0.1f /* && ForwardSpeed < 0.1f */)
+        {
+            // The range near the character's feet where we will consider stairs
+            float nearFeetHeightMin = RawPosition.Z - (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
+            foreach (KeyValuePair<uint, ContactPoint> kvp in CollisionsLastTick.m_objCollisionList)
+            {
+                // Don't care about collisions with the terrain
+                if (kvp.Key > PhysicsScene.TerrainManager.HighestTerrainID)
+                {
+                    OMV.Vector3 touchPosition = kvp.Value.Position;
+                    // DetailLog("{0},BSCharacter.WalkUpStairs,min={1},max={2},touch={3}",
+                    //                 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 * RawOrientation;
+                        OMV.Vector3 touchNormal = OMV.Vector3.Normalize(kvp.Value.SurfaceNormal);
+                        float diff = Math.Abs(OMV.Vector3.Distance(directionFacing, touchNormal));
+                        if (diff < BSParam.AvatarStepApproachFactor)
+                        {
+                            // Found the stairs contact point. Push up a little to raise the character.
+                            float upForce = (touchPosition.Z - nearFeetHeightMin) * Mass * BSParam.AvatarStepForceFactor;
+                            ret = new OMV.Vector3(0f, 0f, upForce);
+
+                            // Also move the avatar up for the new height
+                            OMV.Vector3 displacement = new OMV.Vector3(0f, 0f, BSParam.AvatarStepHeight / 2f);
+                            ForcePosition = RawPosition + displacement;
+                        }
+                        DetailLog("{0},BSCharacter.WalkUpStairs,touchPos={1},nearFeetMin={2},faceDir={3},norm={4},diff={5},ret={6}",
+                                LocalID, touchPosition, nearFeetHeightMin, directionFacing, touchNormal, diff, ret);
+                    }
+                }
+            }
+        }
+
+        return ret;
     }
 
     public override void RequestPhysicsterseUpdate()
@@ -185,24 +344,31 @@ public sealed class BSCharacter : BSPhysObject
         }
 
         set {
-            // When an avatar's size is set, only the height is changed.
             _size = value;
-            ComputeAvatarScale(_size);
+            // Old versions of ScenePresence passed only the height. If width and/or depth are zero,
+            //     replace with the default values.
+            if (_size.X == 0f) _size.X = BSParam.AvatarCapsuleDepth;
+            if (_size.Y == 0f) _size.Y = BSParam.AvatarCapsuleWidth;
+
+            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()
             {
-                BulletSimAPI.SetLocalScaling2(PhysShape.ptr, Scale);
-                UpdatePhysicalMassProperties(RawMass);
+                if (PhysBody.HasPhysicalBody && PhysShape.HasPhysicalShape)
+                {
+                    PhysicsScene.PE.SetLocalScaling(PhysShape, Scale);
+                    UpdatePhysicalMassProperties(RawMass, true);
+                    // Make sure this change appears as a property update event
+                    PhysicsScene.PE.PushUpdate(PhysBody);
+                }
             });
 
         }
     }
 
-    public override OMV.Vector3 Scale { get; set; }
-
     public override PrimitiveBaseShape Shape
     {
         set { BaseShape = value; }
@@ -219,6 +385,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; }
@@ -236,7 +406,8 @@ public sealed class BSCharacter : BSPhysObject
         // Zero some other properties directly into the physics engine
         PhysicsScene.TaintedObject(inTaintTime, "BSCharacter.ZeroMotion", delegate()
         {
-            BulletSimAPI.ClearAllForces2(PhysBody.ptr);
+            if (PhysBody.HasPhysicalBody)
+                PhysicsScene.PE.ClearAllForces(PhysBody);
         });
     }
     public override void ZeroAngularMotion(bool inTaintTime)
@@ -245,10 +416,13 @@ public sealed class BSCharacter : BSPhysObject
 
         PhysicsScene.TaintedObject(inTaintTime, "BSCharacter.ZeroMotion", delegate()
         {
-            BulletSimAPI.SetInterpolationAngularVelocity2(PhysBody.ptr, OMV.Vector3.Zero);
-            BulletSimAPI.SetAngularVelocity2(PhysBody.ptr, OMV.Vector3.Zero);
-            // The next also get rid of applied linear force but the linear velocity is untouched.
-            BulletSimAPI.ClearForces2(PhysBody.ptr);
+            if (PhysBody.HasPhysicalBody)
+            {
+                PhysicsScene.PE.SetInterpolationAngularVelocity(PhysBody, OMV.Vector3.Zero);
+                PhysicsScene.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);
+            }
         });
     }
 
@@ -263,29 +437,31 @@ public sealed class BSCharacter : BSPhysObject
     public override OMV.Vector3 Position {
         get {
             // Don't refetch the position because this function is called a zillion times
-            // _position = BulletSimAPI.GetObjectPosition2(Scene.World.ptr, LocalID);
+            // _position = PhysicsScene.PE.GetObjectPosition(Scene.World, LocalID);
             return _position;
         }
         set {
             _position = value;
-            PositionSanityCheck();
 
             PhysicsScene.TaintedObject("BSCharacter.setPosition", delegate()
             {
                 DetailLog("{0},BSCharacter.SetPosition,taint,pos={1},orient={2}", LocalID, _position, _orientation);
-                BulletSimAPI.SetTranslation2(PhysBody.ptr, _position, _orientation);
+                PositionSanityCheck();
+                ForcePosition = _position;
             });
         }
     }
     public override OMV.Vector3 ForcePosition {
         get {
-            _position = BulletSimAPI.GetPosition2(PhysBody.ptr);
+            _position = PhysicsScene.PE.GetPosition(PhysBody);
             return _position;
         }
         set {
             _position = value;
-            PositionSanityCheck();
-            BulletSimAPI.SetTranslation2(PhysBody.ptr, _position, _orientation);
+            if (PhysBody.HasPhysicalBody)
+            {
+                PhysicsScene.PE.SetTranslation(PhysBody, _position, _orientation);
+            }
         }
     }
 
@@ -297,17 +473,28 @@ public sealed class BSCharacter : BSPhysObject
     {
         bool ret = false;
 
+        // TODO: check for out of bounds
+        if (!PhysicsScene.TerrainManager.IsWithinKnownTerrain(RawPosition))
+        {
+            // The character is out of the known/simulated area.
+            // 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 = PhysicsScene.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 = PhysicsScene.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.GetWaterLevelAtXYZ(_position);
+            float waterHeight = PhysicsScene.TerrainManager.GetWaterLevelAtXYZ(_position);
             if (Position.Z < waterHeight)
             {
                 _position.Z = waterHeight;
@@ -315,7 +502,6 @@ public sealed class BSCharacter : BSPhysObject
             }
         }
 
-        // TODO: check for out of bounds
         return ret;
     }
 
@@ -332,7 +518,7 @@ public sealed class BSCharacter : BSPhysObject
             PhysicsScene.TaintedObject(inTaintTime, "BSCharacter.PositionSanityCheck", delegate()
             {
                 DetailLog("{0},BSCharacter.PositionSanityCheck,taint,pos={1},orient={2}", LocalID, _position, _orientation);
-                BulletSimAPI.SetTranslation2(PhysBody.ptr, _position, _orientation);
+                ForcePosition = _position;
             });
             ret = true;
         }
@@ -345,10 +531,10 @@ public sealed class BSCharacter : BSPhysObject
     public override float RawMass { 
         get {return _mass; }
     }
-    public override void UpdatePhysicalMassProperties(float physMass)
+    public override void UpdatePhysicalMassProperties(float physMass, bool inWorld)
     {
-        OMV.Vector3 localInertia = BulletSimAPI.CalculateLocalInertia2(PhysShape.ptr, physMass);
-        BulletSimAPI.SetMassProps2(PhysBody.ptr, physMass, localInertia);
+        OMV.Vector3 localInertia = PhysicsScene.PE.CalculateLocalInertia(PhysShape, physMass);
+        PhysicsScene.PE.SetMassProps(PhysBody, physMass, localInertia);
     }
 
     public override OMV.Vector3 Force {
@@ -359,7 +545,8 @@ public sealed class BSCharacter : BSPhysObject
             PhysicsScene.TaintedObject("BSCharacter.SetForce", delegate()
             {
                 DetailLog("{0},BSCharacter.setForce,taint,force={1}", LocalID, _force);
-                BulletSimAPI.SetObjectForce2(PhysBody.ptr, _force);
+                if (PhysBody.HasPhysicalBody)
+                    PhysicsScene.PE.SetObjectForce(PhysBody, _force);
             });
         }
     }
@@ -376,6 +563,37 @@ public sealed class BSCharacter : BSPhysObject
 
     public override OMV.Vector3 GeometricCenter { get { return OMV.Vector3.Zero; } }
     public override OMV.Vector3 CenterOfMass { get { return OMV.Vector3.Zero; } }
+
+    // Sets the target in the motor. This starts the changing of the avatar's velocity.
+    public override OMV.Vector3 TargetVelocity
+    {
+        get
+        {
+            return m_targetVelocity;
+        }
+        set
+        {
+            DetailLog("{0},BSCharacter.setTargetVelocity,call,vel={1}", LocalID, value);
+            m_targetVelocity = value;
+            OMV.Vector3 targetVel = value;
+            if (_setAlwaysRun)
+                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;
+            });
+        }
+    }
+    public override OMV.Vector3 RawVelocity
+    {
+        get { return _velocity; }
+        set { _velocity = value; }
+    }
+    // Directly setting velocity means this is what the user really wants now.
     public override OMV.Vector3 Velocity {
         get { return _velocity; }
         set {
@@ -383,6 +601,11 @@ public sealed class BSCharacter : BSPhysObject
             // m_log.DebugFormat("{0}: set velocity = {1}", LogHeader, _velocity);
             PhysicsScene.TaintedObject("BSCharacter.setVelocity", delegate()
             {
+                _velocityMotor.Reset();
+                _velocityMotor.SetCurrent(_velocity);
+                _velocityMotor.SetTarget(_velocity);
+                _velocityMotor.Enabled = false;
+
                 DetailLog("{0},BSCharacter.setVelocity,taint,vel={1}", LocalID, _velocity);
                 ForceVelocity = _velocity;
             });
@@ -391,30 +614,11 @@ public sealed class BSCharacter : BSPhysObject
     public override OMV.Vector3 ForceVelocity {
         get { return _velocity; }
         set {
-            // 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 != PhysicsScene.Params.avatarStandingFriction)
-                {
-                    _currentFriction = PhysicsScene.Params.avatarStandingFriction;
-                    BulletSimAPI.SetFriction2(PhysBody.ptr, _currentFriction);
-                }
-            }
-            else
-            {
-                if (_currentFriction != PhysicsScene.Params.avatarFriction)
-                {
-                    _currentFriction = PhysicsScene.Params.avatarFriction;
-                    BulletSimAPI.SetFriction2(PhysBody.ptr, _currentFriction);
-                }
-            }
-            _velocity = value;
-            // Remember the set velocity so we can suppress the reduction by friction, ...
-            _appliedVelocity = value;
+            PhysicsScene.AssertInTaintTime("BSCharacter.ForceVelocity");
 
-            BulletSimAPI.SetLinearVelocity2(PhysBody.ptr, _velocity);
-            BulletSimAPI.Activate2(PhysBody.ptr, true);
+            _velocity = value;
+            PhysicsScene.PE.SetLinearVelocity(PhysBody, _velocity);
+            PhysicsScene.PE.Activate(PhysBody, true);
         }
     }
     public override OMV.Vector3 Torque {
@@ -439,13 +643,16 @@ public sealed class BSCharacter : BSPhysObject
     public override OMV.Quaternion Orientation {
         get { return _orientation; }
         set {
-            _orientation = value;
-            // m_log.DebugFormat("{0}: set orientation to {1}", LogHeader, _orientation);
-            PhysicsScene.TaintedObject("BSCharacter.setOrientation", delegate()
+            // Orientation is set zillions of times when an avatar is walking. It's like
+            //      the viewer doesn't trust us.
+            if (_orientation != value)
             {
-                // _position = BulletSimAPI.GetPosition2(BSBody.ptr);
-                BulletSimAPI.SetTranslation2(PhysBody.ptr, _position, _orientation);
-            });
+                _orientation = value;
+                PhysicsScene.TaintedObject("BSCharacter.setOrientation", delegate()
+                {
+                    ForceOrientation = _orientation;
+                });
+            }
         }
     }
     // Go directly to Bullet to get/set the value.
@@ -453,13 +660,17 @@ public sealed class BSCharacter : BSPhysObject
     {
         get
         {
-            _orientation = BulletSimAPI.GetOrientation2(PhysBody.ptr);
+            _orientation = PhysicsScene.PE.GetOrientation(PhysBody);
             return _orientation;
         }
         set
         {
             _orientation = value;
-            BulletSimAPI.SetTranslation2(PhysBody.ptr, _position, _orientation);
+            if (PhysBody.HasPhysicalBody)
+            {
+                // _position = PhysicsScene.PE.GetPosition(BSBody);
+                PhysicsScene.PE.SetTranslation(PhysBody, _position, _orientation);
+            }
         }
     }
     public override int PhysicsActorType {
@@ -478,10 +689,14 @@ 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 {
             _flying = value;
+
             // simulate flying by changing the effect of gravity
             Buoyancy = ComputeBuoyancyFromFlying(_flying);
         }
@@ -500,27 +715,18 @@ public sealed class BSCharacter : BSPhysObject
         get { return _throttleUpdates; }
         set { _throttleUpdates = value; }
     }
-    public override bool IsColliding {
-        get { return (CollidingStep == PhysicsScene.SimulationStep); }
-        set { _isColliding = value; }
-    }
-    public override bool CollidingGround {
-        get { return (CollidingGroundStep == PhysicsScene.SimulationStep); }
-        set { CollidingGround = value; }
-    }
-    public override bool CollidingObj {
-        get { return _collidingObj; }
-        set { _collidingObj = value; }
-    }
     public override bool FloatOnWater {
         set {
             _floatOnWater = value;
             PhysicsScene.TaintedObject("BSCharacter.setFloatOnWater", delegate()
             {
-                if (_floatOnWater)
-                    CurrentCollisionFlags = BulletSimAPI.AddToCollisionFlags2(PhysBody.ptr, CollisionFlags.BS_FLOATS_ON_WATER);
-                else
-                    CurrentCollisionFlags = BulletSimAPI.RemoveFromCollisionFlags2(PhysBody.ptr, CollisionFlags.BS_FLOATS_ON_WATER);
+                if (PhysBody.HasPhysicalBody)
+                {
+                    if (_floatOnWater)
+                        CurrentCollisionFlags = PhysicsScene.PE.AddToCollisionFlags(PhysBody, CollisionFlags.BS_FLOATS_ON_WATER);
+                    else
+                        CurrentCollisionFlags = PhysicsScene.PE.RemoveFromCollisionFlags(PhysBody, CollisionFlags.BS_FLOATS_ON_WATER);
+                }
             });
         }
     }
@@ -549,11 +755,16 @@ public sealed class BSCharacter : BSPhysObject
     }
     public override float ForceBuoyancy {
         get { return _buoyancy; }
-        set { _buoyancy = value;
+        set { 
+            PhysicsScene.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);
-            BulletSimAPI.SetGravity2(PhysBody.ptr, new OMV.Vector3(0f, 0f, grav));
+            float  grav = BSParam.Gravity * (1f - _buoyancy);
+            Gravity = new OMV.Vector3(0f, 0f, grav);
+            if (PhysBody.HasPhysicalBody)
+                PhysicsScene.PE.SetGravity(PhysBody, Gravity);
         }
     }
 
@@ -589,24 +800,33 @@ public sealed class BSCharacter : BSPhysObject
     public override float APIDStrength { set { return; } }
     public override float APIDDamping { set { return; } }
 
-    public override void AddForce(OMV.Vector3 force, bool pushforce) {
+    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;
+        AddForce(addForce, pushforce, false);
+    }
+    private void AddForce(OMV.Vector3 force, bool pushforce, bool inTaintTime) {
         if (force.IsFinite())
         {
-            _force.X += force.X;
-            _force.Y += force.Y;
-            _force.Z += force.Z;
-            // m_log.DebugFormat("{0}: AddForce. adding={1}, newForce={2}", LogHeader, force, _force);
-            PhysicsScene.TaintedObject("BSCharacter.AddForce", delegate()
+            OMV.Vector3 addForce = Util.ClampV(force, BSParam.MaxAddForceMagnitude);
+            // DetailLog("{0},BSCharacter.addForce,call,force={1}", LocalID, addForce);
+
+            PhysicsScene.TaintedObject(inTaintTime, "BSCharacter.AddForce", delegate()
             {
-                DetailLog("{0},BSCharacter.setAddForce,taint,addedForce={1}", LocalID, _force);
-                BulletSimAPI.SetObjectForce2(PhysBody.ptr, _force);
+                // 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);
+                }
             });
         }
         else
         {
-            m_log.ErrorFormat("{0}: Got a NaN force applied to a Character", LogHeader);
+            m_log.WarnFormat("{0}: Got a NaN force applied to a character. LocalID={1}", LogHeader, LocalID);
+            return;
         }
-        //m_lastUpdateSent = false;
     }
 
     public override void AddAngularForce(OMV.Vector3 force, bool pushforce) {
@@ -614,24 +834,31 @@ public sealed class BSCharacter : BSPhysObject
     public override void SetMomentum(OMV.Vector3 momentum) {
     }
 
-    private void ComputeAvatarScale(OMV.Vector3 size)
+    private OMV.Vector3 ComputeAvatarScale(OMV.Vector3 size)
     {
-        // The 'size' given by the simulator is the mid-point of the avatar
-        //    and X and Y are unspecified.
-
-        OMV.Vector3 newScale = size;
-        // newScale.X = PhysicsScene.Params.avatarCapsuleWidth;
-        // newScale.Y = PhysicsScene.Params.avatarCapsuleDepth;
-
-        // From the total height, remove the capsule half spheres that are at each end
-        // The 1.15f came from ODE. Not sure what this factors in.
-        // newScale.Z = (size.Z * 1.15f) - (newScale.X + newScale.Y);
+        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
+        //     shape to be the size desired.
+        // So, when creating the scale for the avatar height, we take the passed height
+        //     (size.Z) and remove the caps.
+        // Another oddity of the Bullet capsule implementation is that it presumes the Y
+        //     dimension is the radius of the capsule. Even though some of the code allows
+        //     for a asymmetrical capsule, other parts of the code presume it is cylindrical.
+
+        // Scale is multiplier of radius with one of "0.5"
+        newScale.X = size.X / 2f;
+        newScale.Y = size.Y / 2f;
 
         // The total scale height is the central cylindar plus the caps on the two ends.
-        newScale.Z = size.Z + (Math.Min(size.X, size.Y) * 2f);
+        newScale.Z = (size.Z + (Math.Min(size.X, size.Y) * 2)) / 2f;
+        // If smaller than the endcaps, just fake like we're almost that small
+        if (newScale.Z < 0)
+            newScale.Z = 0.1f;
 
-        // Convert diameters to radii and height to half height -- the way Bullet expects it.
-        Scale = newScale / 2f;
+        return newScale;
     }
 
     // set _avatarVolume and _mass based on capsule size, _density and Scale
@@ -639,16 +866,16 @@ public sealed class BSCharacter : BSPhysObject
     {
         _avatarVolume = (float)(
                         Math.PI
-                        * Scale.X
-                        * Scale.Y    // the area of capsule cylinder
-                        * Scale.Z    // times height of capsule cylinder
+                        * Size.X / 2f
+                        * Size.Y / 2f    // the area of capsule cylinder
+                        * Size.Z         // times height of capsule cylinder
                       + 1.33333333f
                         * Math.PI
-                        * Scale.X
-                        * Math.Min(Scale.X, Scale.Y)
-                        * Scale.Y    // plus the volume of the capsule end caps
+                        * Size.X / 2f
+                        * 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
@@ -657,27 +884,30 @@ public sealed class BSCharacter : BSPhysObject
     {
         _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(_velocity, 0.1f))
+            _velocity = 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;
 
-        if (entprop.Velocity != LastEntityProperties.Velocity)
-        {
-            // Changes in the velocity are suppressed in avatars.
-            // That's just the way they are defined.
-            OMV.Vector3 avVel = new OMV.Vector3(_appliedVelocity.X, _appliedVelocity.Y, entprop.Velocity.Z);
-            _velocity = avVel;
-            BulletSimAPI.SetLinearVelocity2(PhysBody.ptr, avVel);
-        }
-
         // Tell the linkset about value changes
-        Linkset.UpdateProperties(this);
+        // Linkset.UpdateProperties(UpdatedProperties.EntPropUpdates, this);
 
         // Avatars don't report their changes the usual way. Changes are checked for in the heartbeat loop.
         // base.RequestPhysicsterseUpdate();
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSConstraint.cs b/OpenSim/Region/Physics/BulletSPlugin/BSConstraint.cs
index 65fac00..b813974 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSConstraint.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSConstraint.cs
@@ -36,7 +36,8 @@ public abstract class BSConstraint : IDisposable
 {
     private static string LogHeader = "[BULLETSIM CONSTRAINT]";
 
-    protected BulletSim m_world;
+    protected BulletWorld m_world;
+    protected BSScene PhysicsScene;
     protected BulletBody m_body1;
     protected BulletBody m_body2;
     protected BulletConstraint m_constraint;
@@ -48,8 +49,10 @@ public abstract class BSConstraint : IDisposable
     public abstract ConstraintType Type { get; }
     public bool IsEnabled { get { return m_enabled; } }
 
-    public BSConstraint()
+    public BSConstraint(BulletWorld world)
     {
+        m_world = world;
+        PhysicsScene = m_world.physicsScene;
     }
 
     public virtual void Dispose()
@@ -57,15 +60,15 @@ public abstract class BSConstraint : IDisposable
         if (m_enabled)
         {
             m_enabled = false;
-            if (m_constraint.ptr != IntPtr.Zero)
+            if (m_constraint.HasPhysicalConstraint)
             {
-                bool success = BulletSimAPI.DestroyConstraint2(m_world.ptr, m_constraint.ptr);
+                bool success = PhysicsScene.PE.DestroyConstraint(m_world, m_constraint);
                 m_world.physicsScene.DetailLog("{0},BSConstraint.Dispose,taint,id1={1},body1={2},id2={3},body2={4},success={5}",
                                     BSScene.DetailLogZero,
-                                    m_body1.ID, m_body1.ptr.ToString("X"),
-                                    m_body2.ID, m_body2.ptr.ToString("X"),
+                                    m_body1.ID, m_body1.AddrString,
+                                    m_body2.ID, m_body2.AddrString,
                                     success);
-                m_constraint.ptr = System.IntPtr.Zero;
+                m_constraint.Clear();
             }
         }
     }
@@ -74,7 +77,7 @@ public abstract class BSConstraint : IDisposable
     {
         bool ret = false;
         if (m_enabled)
-            ret = BulletSimAPI.SetLinearLimits2(m_constraint.ptr, low, high);
+            ret = PhysicsScene.PE.SetLinearLimits(m_constraint, low, high);
         return ret;
     }
 
@@ -82,7 +85,7 @@ public abstract class BSConstraint : IDisposable
     {
         bool ret = false;
         if (m_enabled)
-            ret = BulletSimAPI.SetAngularLimits2(m_constraint.ptr, low, high);
+            ret = PhysicsScene.PE.SetAngularLimits(m_constraint, low, high);
         return ret;
     }
 
@@ -91,7 +94,7 @@ public abstract class BSConstraint : IDisposable
         bool ret = false;
         if (m_enabled)
         {
-            BulletSimAPI.SetConstraintNumSolverIterations2(m_constraint.ptr, cnt);
+            PhysicsScene.PE.SetConstraintNumSolverIterations(m_constraint, cnt);
             ret = true;
         }
         return ret;
@@ -103,7 +106,7 @@ public abstract class BSConstraint : IDisposable
         if (m_enabled)
         {
             // Recompute the internal transforms
-            BulletSimAPI.CalculateTransforms2(m_constraint.ptr);
+            PhysicsScene.PE.CalculateTransforms(m_constraint);
             ret = true;
         }
         return ret;
@@ -122,7 +125,7 @@ public abstract class BSConstraint : IDisposable
                 // Setting an object's mass to zero (making it static like when it's selected)
                 //     automatically disables the constraints.
                 // If the link is enabled, be sure to set the constraint itself to enabled.
-                BulletSimAPI.SetConstraintEnable2(m_constraint.ptr, m_world.physicsScene.NumericBool(true));
+                PhysicsScene.PE.SetConstraintEnable(m_constraint, BSParam.NumericBool(true));
             }
             else
             {
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSConstraint6Dof.cs b/OpenSim/Region/Physics/BulletSPlugin/BSConstraint6Dof.cs
index 23ef052..476a0e5 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSConstraint6Dof.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSConstraint6Dof.cs
@@ -39,51 +39,50 @@ public sealed class BSConstraint6Dof : BSConstraint
     public override ConstraintType Type { get { return ConstraintType.D6_CONSTRAINT_TYPE; } }
 
     // Create a btGeneric6DofConstraint
-    public BSConstraint6Dof(BulletSim world, BulletBody obj1, BulletBody obj2,
+    public BSConstraint6Dof(BulletWorld world, BulletBody obj1, BulletBody obj2,
                     Vector3 frame1, Quaternion frame1rot,
                     Vector3 frame2, Quaternion frame2rot,
                     bool useLinearReferenceFrameA, bool disableCollisionsBetweenLinkedBodies)
+        : base(world)
     {
-        m_world = world;
         m_body1 = obj1;
         m_body2 = obj2;
-        m_constraint = new BulletConstraint(
-                            BulletSimAPI.Create6DofConstraint2(m_world.ptr, m_body1.ptr, m_body2.ptr,
+        m_constraint = PhysicsScene.PE.Create6DofConstraint(m_world, m_body1, m_body2,
                                 frame1, frame1rot,
                                 frame2, frame2rot,
-                                useLinearReferenceFrameA, disableCollisionsBetweenLinkedBodies));
+                                useLinearReferenceFrameA, disableCollisionsBetweenLinkedBodies);
         m_enabled = true;
         world.physicsScene.DetailLog("{0},BS6DofConstraint,createFrame,wID={1}, rID={2}, rBody={3}, cID={4}, cBody={5}",
                             BSScene.DetailLogZero, world.worldID,
-                            obj1.ID, obj1.ptr.ToString("X"), obj2.ID, obj2.ptr.ToString("X"));
+                            obj1.ID, obj1.AddrString, obj2.ID, obj2.AddrString);
     }
 
-    public BSConstraint6Dof(BulletSim world, BulletBody obj1, BulletBody obj2,
+    // 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)
+        : base(world)
     {
-        m_world = world;
         m_body1 = obj1;
         m_body2 = obj2;
-        if (obj1.ptr == IntPtr.Zero || obj2.ptr == IntPtr.Zero)
+        if (!obj1.HasPhysicalBody || !obj2.HasPhysicalBody)
         {
             world.physicsScene.DetailLog("{0},BS6DOFConstraint,badBodyPtr,wID={1}, rID={2}, rBody={3}, cID={4}, cBody={5}",
                             BSScene.DetailLogZero, world.worldID,
-                            obj1.ID, obj1.ptr.ToString("X"), obj2.ID, obj2.ptr.ToString("X"));
+                            obj1.ID, obj1.AddrString, obj2.ID, obj2.AddrString);
             world.physicsScene.Logger.ErrorFormat("{0} Attempt to build 6DOF constraint with missing bodies: wID={1}, rID={2}, rBody={3}, cID={4}, cBody={5}",
-                            LogHeader, world.worldID, obj1.ID, obj1.ptr.ToString("X"), obj2.ID, obj2.ptr.ToString("X"));
+                            LogHeader, world.worldID, obj1.ID, obj1.AddrString, obj2.ID, obj2.AddrString);
             m_enabled = false;
         }
         else
         {
-            m_constraint = new BulletConstraint(
-                                BulletSimAPI.Create6DofConstraintToPoint2(m_world.ptr, m_body1.ptr, m_body2.ptr,
+            m_constraint = PhysicsScene.PE.Create6DofConstraintToPoint(m_world, m_body1, m_body2,
                                     joinPoint,
-                                    useLinearReferenceFrameA, disableCollisionsBetweenLinkedBodies));
-            world.physicsScene.DetailLog("{0},BS6DofConstraint,createMidPoint,wID={1}, csrt={2}, rID={3}, rBody={4}, cID={5}, cBody={6}",
-                                BSScene.DetailLogZero, world.worldID, m_constraint.ptr.ToString("X"),
-                                obj1.ID, obj1.ptr.ToString("X"), obj2.ID, obj2.ptr.ToString("X"));
-            if (m_constraint.ptr == IntPtr.Zero)
+                                    useLinearReferenceFrameA, disableCollisionsBetweenLinkedBodies);
+            PhysicsScene.DetailLog("{0},BS6DofConstraint,createMidPoint,wID={1}, csrt={2}, rID={3}, rBody={4}, cID={5}, cBody={6}",
+                                BSScene.DetailLogZero, world.worldID, m_constraint.AddrString,
+                                obj1.ID, obj1.AddrString, obj2.ID, obj2.AddrString);
+            if (!m_constraint.HasPhysicalConstraint)
             {
                 world.physicsScene.Logger.ErrorFormat("{0} Failed creation of 6Dof constraint. rootID={1}, childID={2}",
                                 LogHeader, obj1.ID, obj2.ID);
@@ -96,12 +95,27 @@ 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 useLinearReferenceFrameA, 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,
+                                useLinearReferenceFrameA, 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;
         if (m_enabled)
         {
-            BulletSimAPI.SetFrames2(m_constraint.ptr, frameA, frameArot, frameB, frameBrot);
+            PhysicsScene.PE.SetFrames(m_constraint, frameA, frameArot, frameB, frameBrot);
             ret = true;
         }
         return ret;
@@ -112,9 +126,9 @@ public sealed class BSConstraint6Dof : BSConstraint
         bool ret = false;
         if (m_enabled)
         {
-            BulletSimAPI.SetConstraintParam2(m_constraint.ptr, ConstraintParams.BT_CONSTRAINT_STOP_CFM, cfm, ConstraintParamAxis.AXIS_ALL);
-            BulletSimAPI.SetConstraintParam2(m_constraint.ptr, ConstraintParams.BT_CONSTRAINT_STOP_ERP, erp, ConstraintParamAxis.AXIS_ALL);
-            BulletSimAPI.SetConstraintParam2(m_constraint.ptr, ConstraintParams.BT_CONSTRAINT_CFM, cfm, ConstraintParamAxis.AXIS_ALL);
+            PhysicsScene.PE.SetConstraintParam(m_constraint, ConstraintParams.BT_CONSTRAINT_STOP_CFM, cfm, ConstraintParamAxis.AXIS_ALL);
+            PhysicsScene.PE.SetConstraintParam(m_constraint, ConstraintParams.BT_CONSTRAINT_STOP_ERP, erp, ConstraintParamAxis.AXIS_ALL);
+            PhysicsScene.PE.SetConstraintParam(m_constraint, ConstraintParams.BT_CONSTRAINT_CFM, cfm, ConstraintParamAxis.AXIS_ALL);
             ret = true;
         }
         return ret;
@@ -125,7 +139,7 @@ public sealed class BSConstraint6Dof : BSConstraint
         bool ret = false;
         float onOff = useOffset ? ConfigurationParameters.numericTrue : ConfigurationParameters.numericFalse;
         if (m_enabled)
-            ret = BulletSimAPI.UseFrameOffset2(m_constraint.ptr, onOff);
+            ret = PhysicsScene.PE.UseFrameOffset(m_constraint, onOff);
         return ret;
     }
 
@@ -135,7 +149,7 @@ public sealed class BSConstraint6Dof : BSConstraint
         float onOff = enable ? ConfigurationParameters.numericTrue : ConfigurationParameters.numericFalse;
         if (m_enabled)
         {
-            ret = BulletSimAPI.TranslationalLimitMotor2(m_constraint.ptr, onOff, targetVelocity, maxMotorForce);
+            ret = PhysicsScene.PE.TranslationalLimitMotor(m_constraint, onOff, targetVelocity, maxMotorForce);
             m_world.physicsScene.DetailLog("{0},BS6DOFConstraint,TransLimitMotor,enable={1},vel={2},maxForce={3}",
                             BSScene.DetailLogZero, enable, targetVelocity, maxMotorForce);
         }
@@ -146,7 +160,7 @@ public sealed class BSConstraint6Dof : BSConstraint
     {
         bool ret = false;
         if (m_enabled)
-            ret = BulletSimAPI.SetBreakingImpulseThreshold2(m_constraint.ptr, threshold);
+            ret = PhysicsScene.PE.SetBreakingImpulseThreshold(m_constraint, threshold);
         return ret;
     }
 }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSConstraintCollection.cs b/OpenSim/Region/Physics/BulletSPlugin/BSConstraintCollection.cs
index a9fd826..5c8d94e 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSConstraintCollection.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSConstraintCollection.cs
@@ -41,9 +41,9 @@ public sealed class BSConstraintCollection : IDisposable
     delegate bool ConstraintAction(BSConstraint constrain);
 
     private List<BSConstraint> m_constraints;
-    private BulletSim m_world;
+    private BulletWorld m_world;
 
-    public BSConstraintCollection(BulletSim world)
+    public BSConstraintCollection(BulletWorld world)
     {
         m_world = world;
         m_constraints = new List<BSConstraint>();
@@ -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 ed3ffa7..7714a03 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSConstraintHinge.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSConstraintHinge.cs
@@ -36,19 +36,17 @@ public sealed class BSConstraintHinge : BSConstraint
 {
     public override ConstraintType Type { get { return ConstraintType.HINGE_CONSTRAINT_TYPE; } }
 
-    public BSConstraintHinge(BulletSim world, BulletBody obj1, BulletBody obj2,
+    public BSConstraintHinge(BulletWorld world, BulletBody obj1, BulletBody obj2,
                     Vector3 pivotInA, Vector3 pivotInB,
                     Vector3 axisInA, Vector3 axisInB,
                     bool useLinearReferenceFrameA, bool disableCollisionsBetweenLinkedBodies)
+        : base(world)
     {
-        m_world = world;
         m_body1 = obj1;
         m_body2 = obj2;
-        m_constraint = new BulletConstraint(
-                            BulletSimAPI.CreateHingeConstraint2(m_world.ptr, m_body1.ptr, m_body2.ptr,
-                                pivotInA, pivotInB,
-                                axisInA, axisInB,
-                                useLinearReferenceFrameA, disableCollisionsBetweenLinkedBodies));
+        m_constraint = PhysicsScene.PE.CreateHingeConstraint(world, obj1, obj2,
+                                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 dbc9039..65df741 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
@@ -24,28 +24,16 @@
  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  *
-
-/* RA: June 14, 2011. Copied from ODEDynamics.cs and converted to
- * call the BulletSim system.
- */
-/* Revised Aug, Sept 2009 by Kitto Flora. ODEDynamics.cs replaces
- * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised:
- * ODEPrim.cs contains methods dealing with Prim editing, Prim
- * characteristics and Kinetic motion.
- * ODEDynamics.cs contains methods dealing with Prim Physical motion
- * (dynamics) and the associated settings. Old Linear and angular
- * motors for dynamic motion have been replace with  MoveLinear()
- * and MoveAngular(); 'Physical' is used only to switch ODE dynamic
- * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_<other> is to
- * switch between 'VEHICLE' parameter use and general dynamics
- * settings use.
+ * 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 log4net;
 using OpenMetaverse;
 using OpenSim.Framework;
 using OpenSim.Region.Physics.Manager;
@@ -80,10 +68,10 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         private Quaternion m_referenceFrame = Quaternion.Identity;
 
         // Linear properties
+        private BSVMotor m_linearMotor = new BSVMotor("LinearMotor");
         private Vector3 m_linearMotorDirection = Vector3.Zero;          // velocity requested by LSL, decayed by time
         private Vector3 m_linearMotorOffset = Vector3.Zero;             // the point of force can be offset from the center
         private Vector3 m_linearMotorDirectionLASTSET = Vector3.Zero;   // velocity requested by LSL
-        private Vector3 m_newVelocity = Vector3.Zero;                   // velocity computed to be applied to body
         private Vector3 m_linearFrictionTimescale = Vector3.Zero;
         private float m_linearMotorDecayTimescale = 0;
         private float m_linearMotorTimescale = 0;
@@ -93,16 +81,18 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         // private Vector3 m_linearMotorOffset = Vector3.Zero;
 
         //Angular properties
+        private BSVMotor m_angularMotor = new BSVMotor("AngularMotor");
         private Vector3 m_angularMotorDirection = Vector3.Zero;         // angular velocity requested by LSL motor
         // private int m_angularMotorApply = 0;                            // application frame counter
         private Vector3 m_angularMotorVelocity = Vector3.Zero;          // current angular motor velocity
         private float m_angularMotorTimescale = 0;                      // motor angular velocity ramp up rate
         private float m_angularMotorDecayTimescale = 0;                 // motor angular velocity decay rate
         private Vector3 m_angularFrictionTimescale = Vector3.Zero;      // body angular velocity  decay rate
-        private Vector3 m_lastAngularVelocity = Vector3.Zero;           // what was last applied to body
+        private Vector3 m_lastAngularVelocity = Vector3.Zero;
         private Vector3 m_lastVertAttractor = Vector3.Zero;             // what VA was last applied to body
 
         //Deflection properties
+        private BSVMotor m_angularDeflectionMotor = new BSVMotor("AngularDeflection");
         private float m_angularDeflectionEfficiency = 0;
         private float m_angularDeflectionTimescale = 0;
         private float m_linearDeflectionEfficiency = 0;
@@ -114,33 +104,68 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         private float m_bankingTimescale = 0;
 
         //Hover and Buoyancy properties
+        private BSVMotor m_hoverMotor = new BSVMotor("Hover");
         private float m_VhoverHeight = 0f;
         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 float m_verticalAttractionEfficiency = 1.0f;        // damped
-        private float m_verticalAttractionTimescale = 500f;         // Timescale > 300  means no vert attractor.
+        private BSVMotor m_verticalAttractionMotor = new BSVMotor("VerticalAttraction");
+        private float m_verticalAttractionEfficiency = 1.0f; // damped
+        private float m_verticalAttractionCutoff = 500f;     // per the documentation
+        // Timescale > cutoff  means no vert attractor.
+        private float m_verticalAttractionTimescale = 510f;
+
+        // Just some recomputed constants:
+        static readonly float PIOverFour = ((float)Math.PI) / 4f;
+        static readonly float PIOverTwo = ((float)Math.PI) / 2f;
+
+        // 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)
         {
             PhysicsScene = myScene;
             Prim = myPrim;
             Type = Vehicle.TYPE_NONE;
+            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.VehicleDebuggingEnabled)
+            {
+                enableAngularVerticalAttraction = true;
+                enableAngularDeflection = false;
+                enableAngularBanking = false;
+            }
         }
 
         // Return 'true' if this vehicle is doing vehicle things
         public bool IsActive
         {
-            get { return Type != Vehicle.TYPE_NONE; }
+            get { return (Type != Vehicle.TYPE_NONE && Prim.IsPhysicallyActive); }
         }
 
-        internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue)
+        // 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); }
+        }
+
+        #region Vehicle parameter setting
+        public void ProcessFloatVehicleParam(Vehicle pParam, float pValue)
         {
             VDetailLog("{0},ProcessFloatVehicleParam,param={1},val={2}", Prim.LocalID, pParam, pValue);
             switch (pParam)
@@ -152,13 +177,15 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     m_angularDeflectionTimescale = Math.Max(pValue, 0.01f);
                     break;
                 case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE:
-                    m_angularMotorDecayTimescale = Math.Max(pValue, 0.01f);
+                    m_angularMotorDecayTimescale = ClampInRange(0.01f, pValue, 120);
+                    m_angularMotor.TargetValueDecayTimeScale = m_angularMotorDecayTimescale;
                     break;
                 case Vehicle.ANGULAR_MOTOR_TIMESCALE:
                     m_angularMotorTimescale = Math.Max(pValue, 0.01f);
+                    m_angularMotor.TimeScale = m_angularMotorTimescale;
                     break;
                 case Vehicle.BANKING_EFFICIENCY:
-                    m_bankingEfficiency = Math.Max(-1f, Math.Min(pValue, 1f));
+                    m_bankingEfficiency = ClampInRange(-1f, pValue, 1f);
                     break;
                 case Vehicle.BANKING_MIX:
                     m_bankingMix = Math.Max(pValue, 0.01f);
@@ -167,10 +194,11 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     m_bankingTimescale = Math.Max(pValue, 0.01f);
                     break;
                 case Vehicle.BUOYANCY:
-                    m_VehicleBuoyancy = Math.Max(-1f, Math.Min(pValue, 1f));
+                    m_VehicleBuoyancy = ClampInRange(-1f, pValue, 1f);
+                    m_VehicleGravity = Prim.ComputeGravity(m_VehicleBuoyancy);
                     break;
                 case Vehicle.HOVER_EFFICIENCY:
-                    m_VhoverEfficiency = Math.Max(0f, Math.Min(pValue, 1f));
+                    m_VhoverEfficiency = ClampInRange(0f, pValue, 1f);
                     break;
                 case Vehicle.HOVER_HEIGHT:
                     m_VhoverHeight = pValue;
@@ -185,33 +213,41 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     m_linearDeflectionTimescale = Math.Max(pValue, 0.01f);
                     break;
                 case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE:
-                    m_linearMotorDecayTimescale = Math.Max(pValue, 0.01f);
+                    m_linearMotorDecayTimescale = ClampInRange(0.01f, pValue, 120);
+                    m_linearMotor.TargetValueDecayTimeScale = m_linearMotorDecayTimescale;
                     break;
                 case Vehicle.LINEAR_MOTOR_TIMESCALE:
                     m_linearMotorTimescale = Math.Max(pValue, 0.01f);
+                    m_linearMotor.TimeScale = m_linearMotorTimescale;
                     break;
                 case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY:
-                    m_verticalAttractionEfficiency = Math.Max(0.1f, Math.Min(pValue, 1f));
+                    m_verticalAttractionEfficiency = ClampInRange(0.1f, pValue, 1f);
+                    m_verticalAttractionMotor.Efficiency = m_verticalAttractionEfficiency;
                     break;
                 case Vehicle.VERTICAL_ATTRACTION_TIMESCALE:
                     m_verticalAttractionTimescale = Math.Max(pValue, 0.01f);
+                    m_verticalAttractionMotor.TimeScale = m_verticalAttractionTimescale;
                     break;
 
                 // These are vector properties but the engine lets you use a single float value to
                 // 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_angularMotorApply = 100;
+                    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);
                     m_linearMotorDirectionLASTSET = new Vector3(pValue, pValue, pValue);
+                    m_linearMotor.SetTarget(m_linearMotorDirection);
                     break;
                 case Vehicle.LINEAR_MOTOR_OFFSET:
                     m_linearMotorOffset = new Vector3(pValue, pValue, pValue);
@@ -227,21 +263,25 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             {
                 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
-                    pValue.X = Math.Max(-12.56f, Math.Min(pValue.X, 12.56f));
-                    pValue.Y = Math.Max(-12.56f, Math.Min(pValue.Y, 12.56f));
-                    pValue.Z = Math.Max(-12.56f, Math.Min(pValue.Z, 12.56f));
+                    pValue.X = ClampInRange(-12.56f, pValue.X, 12.56f);
+                    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_angularMotorApply = 100;
+                    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);
                     m_linearMotorDirectionLASTSET = new Vector3(pValue.X, pValue.Y, pValue.Z);
+                    m_linearMotor.SetTarget(m_linearMotorDirection);
                     break;
                 case Vehicle.LINEAR_MOTOR_OFFSET:
                     m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z);
@@ -281,7 +321,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             }
         }
 
-        internal void ProcessTypeChange(Vehicle pType)
+        public void ProcessTypeChange(Vehicle pType)
         {
             VDetailLog("{0},ProcessTypeChange,type={1}", Prim.LocalID, pType);
             // Set Defaults For Type
@@ -303,7 +343,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     m_VhoverEfficiency = 0;
                     m_VhoverTimescale = 0;
                     m_VehicleBuoyancy = 0;
-                    
+
                     m_linearDeflectionEfficiency = 1;
                     m_linearDeflectionTimescale = 1;
 
@@ -319,6 +359,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
 
                     m_referenceFrame = Quaternion.Identity;
                     m_flags = (VehicleFlag)0;
+
                     break;
 
                 case Vehicle.TYPE_SLED:
@@ -351,10 +392,14 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     m_bankingMix = 1;
 
                     m_referenceFrame = Quaternion.Identity;
-                    m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.LIMIT_MOTOR_UP);
-                    m_flags &=
-                         ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
-                           VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
+                    m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY
+                                | VehicleFlag.HOVER_TERRAIN_ONLY
+                                | VehicleFlag.HOVER_GLOBAL_HEIGHT
+                                | VehicleFlag.HOVER_UP_ONLY);
+                    m_flags |= (VehicleFlag.NO_DEFLECTION_UP
+                            | VehicleFlag.LIMIT_ROLL_ONLY
+                            | VehicleFlag.LIMIT_MOTOR_UP);
+
                     break;
                 case Vehicle.TYPE_CAR:
                     m_linearMotorDirection = Vector3.Zero;
@@ -498,6 +543,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     m_bankingEfficiency = 0;
                     m_bankingMix = 0.7f;
                     m_bankingTimescale = 5;
+
                     m_referenceFrame = Quaternion.Identity;
 
                     m_referenceFrame = Quaternion.Identity;
@@ -510,152 +556,467 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                                     | VehicleFlag.HOVER_GLOBAL_HEIGHT);
                     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_angularMotor = new BSVMotor("AngularMotor", m_angularMotorTimescale,
+                                m_angularMotorDecayTimescale, m_angularFrictionTimescale,
+                                1f);
+            m_angularMotor.PhysicsScene = 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)
+             */
+        }
+        #endregion // Vehicle parameter setting
+
+        public void Refresh()
+        {
+            // If asking for a refresh, reset the physical parameters before the next simulation step.
+            PhysicsScene.PostTaintObject("BSDynamics.Refresh", Prim.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)
             {
-                // Friction effects are handled by this vehicle code
-                BulletSimAPI.SetFriction2(Prim.PhysBody.ptr, 0f);
-                BulletSimAPI.SetHitFraction2(Prim.PhysBody.ptr, 0f);
-
-                // BulletSimAPI.SetAngularDamping2(Prim.PhysBody.ptr, 0.8f);
-
-                VDetailLog("{0},BSDynamics.Refresh,zeroingFriction and adding damping", Prim.LocalID);
+                // Remember the mass so we don't have to fetch it every step
+                m_vehicleMass = Prim.TotalMass;
+
+                // Friction affects are handled by this vehicle code
+                PhysicsScene.PE.SetFriction(Prim.PhysBody, BSParam.VehicleFriction);
+                PhysicsScene.PE.SetRestitution(Prim.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.
+                PhysicsScene.PE.SetAngularDamping(Prim.PhysBody, BSParam.VehicleAngularDamping);
+                PhysicsScene.PE.SetLinearFactor(Prim.PhysBody, BSParam.VehicleLinearFactor);
+                PhysicsScene.PE.SetAngularFactorV(Prim.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);
+
+                Prim.Inertia = PhysicsScene.PE.CalculateLocalInertia(Prim.PhysShape, m_vehicleMass);
+                PhysicsScene.PE.SetMassProps(Prim.PhysBody, m_vehicleMass, Prim.Inertia);
+                PhysicsScene.PE.UpdateInertiaTensor(Prim.PhysBody);
+
+                // Set the gravity for the vehicle depending on the buoyancy
+                // TODO: what should be done if prim and vehicle buoyancy differ?
+                m_VehicleGravity = Prim.ComputeGravity(m_VehicleBuoyancy);
+                // The actual vehicle gravity is set to zero in Bullet so we can do all the application of same.
+                PhysicsScene.PE.SetGravity(Prim.PhysBody, Vector3.Zero);
+
+                VDetailLog("{0},BSDynamics.SetPhysicalParameters,mass={1},inert={2},vehGrav={3},aDamp={4},frict={5},rest={6},lFact={7},aFact={8}",
+                        Prim.LocalID, m_vehicleMass, Prim.Inertia, m_VehicleGravity,
+                        BSParam.VehicleAngularDamping, BSParam.VehicleFriction, BSParam.VehicleRestitution,
+                        BSParam.VehicleLinearFactor, BSParam.VehicleAngularFactor
+                        );
+            }
+            else
+            {
+                if (Prim.PhysBody.HasPhysicalBody)
+                    PhysicsScene.PE.RemoveFromCollisionFlags(Prim.PhysBody, CollisionFlags.BS_VEHICLE_COLLISIONS);
             }
         }
 
         public bool RemoveBodyDependencies(BSPhysObject prim)
         {
-            // If active, we need to add our properties back when the body is rebuilt.
+            Refresh();
             return IsActive;
         }
 
-        public void RestoreBodyDependencies(BSPhysObject prim)
+        #region Known vehicle value functions
+        // Vehicle physical parameters that we buffer from constant getting and setting.
+        // The "m_known*" values are unknown until they are fetched and the m_knownHas flag is set.
+        //      Changing is remembered and the parameter is stored back into the physics engine only if updated.
+        //      This does two things: 1) saves continuious calls into unmanaged code, and
+        //      2) signals when a physics property update must happen back to the simulator
+        //      to update values modified for the vehicle.
+        private int m_knownChanged;
+        private int m_knownHas;
+        private float m_knownTerrainHeight;
+        private float m_knownWaterLevel;
+        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_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
+        public void PushKnownChanged()
+        {
+            if (m_knownChanged != 0)
+            {
+                if ((m_knownChanged & m_knownChangedPosition) != 0)
+                    Prim.ForcePosition = m_knownPosition;
+
+                if ((m_knownChanged & m_knownChangedOrientation) != 0)
+                    Prim.ForceOrientation = m_knownOrientation;
+
+                if ((m_knownChanged & m_knownChangedVelocity) != 0)
+                {
+                    Prim.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 /*pushForce*/, true /*inTaintTime*/);
+
+                if ((m_knownChanged & m_knownChangedForceImpulse) != 0)
+                    Prim.AddForceImpulse((Vector3)m_knownForceImpulse, false /*pushforce*/, true /*inTaintTime*/);
+
+                if ((m_knownChanged & m_knownChangedRotationalVelocity) != 0)
+                {
+                    Prim.ForceRotationalVelocity = m_knownRotationalVelocity;
+                    // PhysicsScene.PE.SetInterpolationAngularVelocity(Prim.PhysBody, m_knownRotationalVelocity);
+                }
+
+                if ((m_knownChanged & m_knownChangedRotationalImpulse) != 0)
+                    Prim.ApplyTorqueImpulse((Vector3)m_knownRotationalImpulse, true /*inTaintTime*/);
+
+                if ((m_knownChanged & m_knownChangedRotationalForce) != 0)
+                {
+                    Prim.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_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 (Prim.LocalID != prim.LocalID)
+            if ((m_knownHas & m_knownChangedTerrainHeight) == 0 || pos != lastRememberedHeightPos)
             {
-                // 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;
+                lastRememberedHeightPos = pos;
+                m_knownTerrainHeight = Prim.PhysicsScene.TerrainManager.GetTerrainHeightAtXYZ(pos);
+                m_knownHas |= m_knownChangedTerrainHeight;
+            }
+            return m_knownTerrainHeight;
+        }
+
+        // Since the computation of water level can be a little involved, this routine
+        //    is used ot fetch the level only once for each vehicle simulation step.
+        private float GetWaterLevel(Vector3 pos)
+        {
+            if ((m_knownHas & m_knownChangedWaterLevel) == 0)
+            {
+                m_knownWaterLevel = Prim.PhysicsScene.TerrainManager.GetWaterLevelAtXYZ(pos);
+                m_knownHas |= m_knownChangedWaterLevel;
+            }
+            return (float)m_knownWaterLevel;
+        }
+
+        private Vector3 VehiclePosition
+        {
+            get
+            {
+                if ((m_knownHas & m_knownChangedPosition) == 0)
+                {
+                    m_knownPosition = Prim.ForcePosition;
+                    m_knownHas |= m_knownChangedPosition;
+                }
+                return m_knownPosition;
+            }
+            set
+            {
+                m_knownPosition = value;
+                m_knownChanged |= m_knownChangedPosition;
+                m_knownHas |= m_knownChangedPosition;
             }
-            Refresh();
         }
 
+        private Quaternion VehicleOrientation
+        {
+            get
+            {
+                if ((m_knownHas & m_knownChangedOrientation) == 0)
+                {
+                    m_knownOrientation = Prim.ForceOrientation;
+                    m_knownHas |= m_knownChangedOrientation;
+                }
+                return m_knownOrientation;
+            }
+            set
+            {
+                m_knownOrientation = value;
+                m_knownChanged |= m_knownChangedOrientation;
+                m_knownHas |= m_knownChangedOrientation;
+            }
+        }
+
+        private Vector3 VehicleVelocity
+        {
+            get
+            {
+                if ((m_knownHas & m_knownChangedVelocity) == 0)
+                {
+                    m_knownVelocity = Prim.ForceVelocity;
+                    m_knownHas |= m_knownChangedVelocity;
+                }
+                return m_knownVelocity;
+            }
+            set
+            {
+                m_knownVelocity = value;
+                m_knownChanged |= m_knownChangedVelocity;
+                m_knownHas |= m_knownChangedVelocity;
+            }
+        }
+
+        private void VehicleAddForce(Vector3 pForce)
+        {
+            if ((m_knownHas & m_knownChangedForce) == 0)
+            {
+                m_knownForce = Vector3.Zero;
+                m_knownHas |= m_knownChangedForce;
+            }
+            m_knownForce += pForce;
+            m_knownChanged |= 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
+        {
+            get
+            {
+                if ((m_knownHas & m_knownChangedRotationalVelocity) == 0)
+                {
+                    m_knownRotationalVelocity = Prim.ForceRotationalVelocity;
+                    m_knownHas |= m_knownChangedRotationalVelocity;
+                }
+                return (Vector3)m_knownRotationalVelocity;
+            }
+            set
+            {
+                m_knownRotationalVelocity = value;
+                m_knownChanged |= m_knownChangedRotationalVelocity;
+                m_knownHas |= m_knownChangedRotationalVelocity;
+            }
+        }
+        private void VehicleAddAngularForce(Vector3 aForce)
+        {
+            if ((m_knownHas & m_knownChangedRotationalForce) == 0)
+            {
+                m_knownRotationalForce = Vector3.Zero;
+            }
+            m_knownRotationalForce += aForce;
+            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
+        {
+            get
+            {
+                if ((m_knownHas & m_knownChangedForwardVelocity) == 0)
+                {
+                    m_knownForwardVelocity = VehicleVelocity * Quaternion.Inverse(Quaternion.Normalize(VehicleOrientation));
+                    m_knownHas |= m_knownChangedForwardVelocity;
+                }
+                return m_knownForwardVelocity;
+            }
+        }
+        private float VehicleForwardSpeed
+        {
+            get
+            {
+                return VehicleForwardVelocity.X;
+            }
+        }
+
+        #endregion // Known vehicle value functions
+
         // One step of the vehicle properties for the next 'pTimestep' seconds.
         internal void Step(float pTimestep)
         {
             if (!IsActive) return;
 
-            // DEBUG
-            // Because Bullet does apply forces to the vehicle, our last computed
-            //     linear and angular velocities are not what is happening now.
-            // Vector3 externalAngularVelocity = Prim.ForceRotationalVelocity - m_lastAngularVelocity;
-            // m_lastAngularVelocity += (externalAngularVelocity * 0.5f) * pTimestep;
-            // m_lastAngularVelocity = Prim.ForceRotationalVelocity;   // DEBUG: account for what Bullet did last time
-            // m_lastLinearVelocityVector = Prim.ForceVelocity * Quaternion.Inverse(Prim.ForceOrientation);        // DEBUG:
-            // END DEBUG
-
-            m_vehicleMass = Prim.Linkset.LinksetMass;
+            ForgetKnownVehicleProperties();
 
             MoveLinear(pTimestep);
-            // Commented out for debug
             MoveAngular(pTimestep);
-            // Prim.ApplyTorqueImpulse(-Prim.RotationalVelocity * m_vehicleMass, false);    // DEBUG DEBUG
-            // Prim.ForceRotationalVelocity = -Prim.RotationalVelocity;    // DEBUG DEBUG
 
             LimitRotation(pTimestep);
 
             // remember the position so next step we can limit absolute movement effects
-            m_lastPositionVector = Prim.ForcePosition;
-
-            VDetailLog("{0},BSDynamics.Step,frict={1},grav={2},inertia={3},mass={4}",        // DEBUG DEBUG
-                                    Prim.LocalID, 
-                                    BulletSimAPI.GetFriction2(Prim.PhysBody.ptr),
-                                    BulletSimAPI.GetGravity2(Prim.PhysBody.ptr),
-                                    Prim.Inertia,
-                                    m_vehicleMass
-                                    );
-            VDetailLog("{0},BSDynamics.Step,done,pos={1},force={2},velocity={3},angvel={4}",
-                    Prim.LocalID, Prim.ForcePosition, Prim.Force, Prim.ForceVelocity, Prim.RotationalVelocity);
-        }// end Step
-
-        // Apply the effect of the linear motor.
-        // Also does hover and float.
+            m_lastPositionVector = VehiclePosition;
+
+            // If we forced the changing of some vehicle parameters, update the values and
+            //      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);
+
+            VDetailLog("{0},BSDynamics.Step,done,pos={1}, force={2},velocity={3},angvel={4}",
+                    Prim.LocalID, VehiclePosition, m_knownForce, VehicleVelocity, VehicleRotationalVelocity);
+        }
+
+        // Called after the simulation step
+        internal void PostStep(float pTimestep)
+        {
+            if (!IsActive) return;
+
+            if (PhysicsScene.VehiclePhysicalLoggingEnabled)
+                PhysicsScene.PE.DumpRigidBody(PhysicsScene.World, Prim.PhysBody);
+        }
+
+        // Apply the effect of the linear motor and other linear motions (like hover and float).
         private void MoveLinear(float pTimestep)
         {
-            // m_linearMotorDirection is the target direction we are moving relative to the vehicle coordinates
-            // m_lastLinearVelocityVector is the current speed we are moving in that direction
-            if (m_linearMotorDirection.LengthSquared() > 0.001f)
-            {
-                Vector3 origDir = m_linearMotorDirection;       // DEBUG
-                Vector3 origVel = m_lastLinearVelocityVector;   // DEBUG
-                // DEBUG: the vehicle velocity rotated to be relative to vehicle coordinates for comparison
-                Vector3 vehicleVelocity = Prim.ForceVelocity * Quaternion.Inverse(Prim.ForceOrientation);   // DEBUG
+            ComputeLinearVelocity(pTimestep);
 
-                // Add (desiredVelocity - lastAppliedVelocity) / howLongItShouldTakeToComplete
-                Vector3 addAmount = (m_linearMotorDirection - m_lastLinearVelocityVector)/(m_linearMotorTimescale) * pTimestep;
-                m_lastLinearVelocityVector += addAmount;
+            ComputeLinearTerrainHeightCorrection(pTimestep);
 
-                float decayFactor = (1.0f / m_linearMotorDecayTimescale) * pTimestep;
-                m_linearMotorDirection *= (1f - decayFactor);
+            ComputeLinearHover(pTimestep);
 
-                // Rotate new object velocity from vehicle relative to world coordinates
-                m_newVelocity = m_lastLinearVelocityVector * Prim.ForceOrientation;
+            ComputeLinearBlockingEndPoint(pTimestep);
 
-                // Apply friction for next time
-                Vector3 frictionFactor = (Vector3.One / m_linearFrictionTimescale) * pTimestep;
-                m_lastLinearVelocityVector *= (Vector3.One - frictionFactor);
+            ComputeLinearMotorUp(pTimestep);
 
-                VDetailLog("{0},MoveLinear,nonZero,origlmDir={1},origlvVel={2},vehVel={3},add={4},decay={5},frict={6},lmDir={7},lvVec={8},newVel={9}",
-                                Prim.LocalID, origDir, origVel, vehicleVelocity, addAmount, decayFactor, frictionFactor, 
-                                m_linearMotorDirection, m_lastLinearVelocityVector, m_newVelocity);
-            }
-            else
+            ApplyGravity(pTimestep);
+
+            // If not changing some axis, reduce out velocity
+            if ((m_flags & (VehicleFlag.NO_X | VehicleFlag.NO_Y | VehicleFlag.NO_Z)) != 0)
             {
-                // if what remains of direction is very small, zero it.
-                m_linearMotorDirection = Vector3.Zero;
-                m_lastLinearVelocityVector = Vector3.Zero;
-                m_newVelocity = Vector3.Zero;
+                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;
+            }
 
-                VDetailLog("{0},MoveLinear,zeroed", Prim.LocalID);
+            // ==================================================================
+            // Clamp high or low velocities
+            float newVelocityLengthSq = VehicleVelocity.LengthSquared();
+            if (newVelocityLengthSq > BSParam.VehicleMaxLinearVelocitySquared)
+            {
+                Vector3 origVelW = VehicleVelocity;         // DEBUG DEBUG
+                VehicleVelocity /= VehicleVelocity.Length();
+                VehicleVelocity *= BSParam.VehicleMaxLinearVelocity;
+                VDetailLog("{0},  MoveLinear,clampMax,origVelW={1},lenSq={2},maxVelSq={3},,newVelW={4}", 
+                            Prim.LocalID, origVelW, newVelocityLengthSq, BSParam.VehicleMaxLinearVelocitySquared, VehicleVelocity);
             }
+            else if (newVelocityLengthSq < 0.001f)
+                VehicleVelocity = Vector3.Zero;
 
-            // m_newVelocity is velocity computed from linear motor in world coordinates
+            VDetailLog("{0},  MoveLinear,done,isColl={1},newVel={2}", Prim.LocalID, Prim.IsColliding, VehicleVelocity );
 
-            // Gravity and Buoyancy
-            // There is some gravity, make a gravity force vector that is applied after object velocity.
-            // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g;
-            Vector3 grav = Prim.PhysicsScene.DefaultGravity * (1f - m_VehicleBuoyancy);
+        } // end MoveLinear()
 
-            /*
-             * RA: Not sure why one would do this unless we are hoping external forces are doing gravity, ...
-            // Preserve the current Z velocity
-            Vector3 vel_now = m_prim.Velocity;
-            m_dir.Z = vel_now.Z;        // Preserve the accumulated falling velocity
-             */
+        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);
+
+            // 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)
+            {
+                if (linearMotorVelocityW.Z > 0f)
+                    linearMotorVelocityW.Z = 0f;
+            }
+
+            // Add this correction to the velocity to make it faster/slower.
+            VehicleVelocity += linearMotorVelocityW;
 
-            Vector3 pos = Prim.ForcePosition;
-//            Vector3 accel = new Vector3(-(m_dir.X - m_lastLinearVelocityVector.X / 0.1f), -(m_dir.Y - m_lastLinearVelocityVector.Y / 0.1f), m_dir.Z - m_lastLinearVelocityVector.Z / 0.1f);
+            VDetailLog("{0},  MoveLinear,velocity,origVelW={1},velV={2},correctV={3},correctW={4},newVelW={5}",
+                        Prim.LocalID, origVelW, currentVelV, linearMotorCorrectionV, linearMotorVelocityW, VehicleVelocity);
+        }
 
+        public void ComputeLinearTerrainHeightCorrection(float pTimestep)
+        {
             // If below the terrain, move us above the ground a little.
-            float terrainHeight = Prim.PhysicsScene.TerrainManager.GetTerrainHeightAtXYZ(pos);
-            // Taking the rotated size doesn't work here because m_prim.Size is the size of the root prim and not the linkset.
-            //     TODO: Add a m_prim.LinkSet.Size similar to m_prim.LinkSet.Mass.
-            // Vector3 rotatedSize = m_prim.Size * m_prim.ForceOrientation;
-            // if (rotatedSize.Z < terrainHeight)
-            if (pos.Z < terrainHeight)
+            // TODO: Consider taking the rotated size of the object or possibly casting a ray.
+            if (VehiclePosition.Z < GetTerrainHeight(VehiclePosition))
             {
-                pos.Z = terrainHeight + 2;
-                Prim.ForcePosition = pos;
-                VDetailLog("{0},MoveLinear,terrainHeight,terrainHeight={1},pos={2}", Prim.LocalID, terrainHeight, pos);
+                // 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);
             }
+        }
 
-            // Check if hovering
+        public void ComputeLinearHover(float pTimestep)
+        {
             // 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)
@@ -663,11 +1024,11 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 // We should hover, get the target height
                 if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) != 0)
                 {
-                    m_VhoverTargetHeight = Prim.PhysicsScene.GetWaterLevelAtXYZ(pos) + m_VhoverHeight;
+                    m_VhoverTargetHeight = GetWaterLevel(VehiclePosition) + m_VhoverHeight;
                 }
                 if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0)
                 {
-                    m_VhoverTargetHeight = terrainHeight + m_VhoverHeight;
+                    m_VhoverTargetHeight = GetTerrainHeight(VehiclePosition) + m_VhoverHeight;
                 }
                 if ((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) != 0)
                 {
@@ -677,45 +1038,63 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 if ((m_flags & VehicleFlag.HOVER_UP_ONLY) != 0)
                 {
                     // If body is already heigher, use its height as target height
-                    if (pos.Z > m_VhoverTargetHeight)
-                        m_VhoverTargetHeight = pos.Z;
+                    if (VehiclePosition.Z > m_VhoverTargetHeight)
+                        m_VhoverTargetHeight = VehiclePosition.Z;
                 }
+                
                 if ((m_flags & VehicleFlag.LOCK_HOVER_HEIGHT) != 0)
                 {
-                    if (Math.Abs(pos.Z - m_VhoverTargetHeight) > 0.2f)
+                    if (Math.Abs(VehiclePosition.Z - m_VhoverTargetHeight) > 0.2f)
                     {
+                        Vector3 pos = VehiclePosition;
                         pos.Z = m_VhoverTargetHeight;
-                        Prim.ForcePosition = pos;
+                        VehiclePosition = pos;
+
+                        VDetailLog("{0},  MoveLinear,hover,pos={1},lockHoverHeight", Prim.LocalID, pos);
                     }
                 }
                 else
                 {
-                    float verticalError = pos.Z - m_VhoverTargetHeight;
-                    // RA: where does the 50 come from?
-                    float verticalCorrectionVelocity = pTimestep * ((verticalError * 50.0f) / m_VhoverTimescale);
-                    // Replace Vertical speed with correction figure if significant
-                    if (Math.Abs(verticalError) > 0.01f)
-                    {
-                        m_newVelocity.Z += verticalCorrectionVelocity;
-                        //KF: m_VhoverEfficiency is not yet implemented
-                    }
-                    else if (verticalError < -0.01)
-                    {
-                        m_newVelocity.Z -= verticalCorrectionVelocity;
-                    }
-                    else
-                    {
-                        m_newVelocity.Z = 0f;
-                    }
+                    // Error is positive if below the target and negative if above.
+                    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
+                    VehicleAddForceImpulse(verticalCorrection);
+                     */
+
+                    VDetailLog("{0},  MoveLinear,hover,pos={1},eff={2},hoverTS={3},height={4},target={5},err={6},corr={7}",
+                                    Prim.LocalID, VehiclePosition, m_VhoverEfficiency,
+                                    m_VhoverTimescale, m_VhoverHeight, m_VhoverTargetHeight,
+                                    verticalError, verticalCorrection);
                 }
 
-                VDetailLog("{0},MoveLinear,hover,pos={1},dir={2},height={3},target={4}", Prim.LocalID, pos, m_newVelocity, m_VhoverHeight, m_VhoverTargetHeight);
             }
+        }
 
+        public bool ComputeLinearBlockingEndPoint(float pTimestep)
+        {
+            bool changed = false;
+
+            Vector3 pos = VehiclePosition;
             Vector3 posChange = pos - m_lastPositionVector;
             if (m_BlockingEndPoint != Vector3.Zero)
             {
-                bool changed = false;
                 if (pos.X >= (m_BlockingEndPoint.X - (float)1))
                 {
                     pos.X -= posChange.X + 1;
@@ -743,233 +1122,119 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 }
                 if (changed)
                 {
-                    Prim.ForcePosition = pos;
-                    VDetailLog("{0},MoveLinear,blockingEndPoint,block={1},origPos={2},pos={3}",
+                    VehiclePosition = pos;
+                    VDetailLog("{0},  MoveLinear,blockingEndPoint,block={1},origPos={2},pos={3}",
                                 Prim.LocalID, m_BlockingEndPoint, posChange, pos);
                 }
             }
+            return changed;
+        }
 
-            #region downForce
-            Vector3 downForce = Vector3.Zero;
-
+        // From http://wiki.secondlife.com/wiki/LlSetVehicleFlags :
+        //    Prevent ground vehicles from motoring into the sky. This flag has a subtle effect when
+        //    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. 
+        // 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 void ComputeLinearMotorUp(float pTimestep)
+        {
             if ((m_flags & (VehicleFlag.LIMIT_MOTOR_UP)) != 0)
             {
-                // If the vehicle is motoring into the sky, get it going back down.
-                // Is this an angular force or both linear and angular??
-                float distanceAboveGround = pos.Z - terrainHeight;
-                if (distanceAboveGround > 2f)
+                // 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) * pTimestep);
                     // downForce = new Vector3(0, 0, -distanceAboveGround / m_bankingTimescale);
-                    downForce = new Vector3(0, 0, -distanceAboveGround);
+                    VehicleVelocity += new Vector3(0, 0, -distanceAboveGround);
                 }
-                // TODO: this calculation is all wrong. From the description at
+                // 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.
-                VDetailLog("{0},MoveLinear,limitMotorUp,distAbove={1},downForce={2}", 
-                                    Prim.LocalID, distanceAboveGround, downForce);
-            }
-            #endregion // downForce
-
-            // If not changing some axis, reduce out velocity
-            if ((m_flags & (VehicleFlag.NO_X)) != 0)
-                m_newVelocity.X = 0;
-            if ((m_flags & (VehicleFlag.NO_Y)) != 0)
-                m_newVelocity.Y = 0;
-            if ((m_flags & (VehicleFlag.NO_Z)) != 0)
-                m_newVelocity.Z = 0;
-
-            // Clamp REALLY high or low velocities
-            if (m_newVelocity.LengthSquared() > 1e6f)
-            {
-                m_newVelocity /= m_newVelocity.Length();
-                m_newVelocity *= 1000f;
+                // TODO: add interaction with banking.
+                VDetailLog("{0},  MoveLinear,limitMotorUp,distAbove={1},colliding={2},ret={3}",
+                                Prim.LocalID, distanceAboveGround, Prim.IsColliding, 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 (!Prim.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}",
+                                    Prim.LocalID, Prim.IsColliding, upVelocity, VehicleVelocity);
+                }
             }
-            else if (m_newVelocity.LengthSquared() < 1e-6f)
-                m_newVelocity = Vector3.Zero;
+        }
 
-            // Stuff new linear velocity into the vehicle
-            Prim.ForceVelocity = m_newVelocity;
-            // Prim.ApplyForceImpulse((m_newVelocity - Prim.Velocity) * m_vehicleMass, false);    // DEBUG DEBUG
+        private void ApplyGravity(float pTimeStep)
+        {
+            Vector3 appliedGravity = m_VehicleGravity * m_vehicleMass;
 
-            Vector3 totalDownForce = downForce + grav;
-            if (totalDownForce != Vector3.Zero)
-            {
-                Prim.AddForce(totalDownForce * m_vehicleMass, false);
-                // Prim.ApplyForceImpulse(totalDownForce * m_vehicleMass, false);
-            }
+            // Hack to reduce downward force if the vehicle is probably sitting on the ground
+            if (Prim.IsColliding && IsGroundVehicle)
+                appliedGravity *= BSParam.VehicleGroundGravityFudge;
 
-            VDetailLog("{0},MoveLinear,done,lmDir={1},lmVel={2},newVel={3},primVel={4},totalDown={5}",
-                    Prim.LocalID, m_linearMotorDirection, m_lastLinearVelocityVector, m_newVelocity, Prim.Velocity, totalDownForce);
+            VehicleAddForce(appliedGravity);
 
-        } // end MoveLinear()
+            VDetailLog("{0},  MoveLinear,applyGravity,vehGrav={1},collid={2},fudge={3},mass={4},appliedForce={3}", 
+                            Prim.LocalID, m_VehicleGravity,
+                            Prim.IsColliding, BSParam.VehicleGroundGravityFudge, m_vehicleMass, appliedGravity);
+        }
 
         // =======================================================================
+        // =======================================================================
         // Apply the effect of the angular motor.
+        // The 'contribution' is how much angular correction velocity each function wants.
+        //     All the contributions are added together and the resulting velocity is
+        //     set directly on the vehicle.
         private void MoveAngular(float pTimestep)
         {
-            // m_angularMotorDirection         // angular velocity requested by LSL motor
-            // m_angularMotorApply             // application frame counter
-            // m_angularMotorVelocity          // current angular motor velocity (ramps up and down)
-            // m_angularMotorTimescale         // motor angular velocity ramp up rate
-            // m_angularMotorDecayTimescale    // motor angular velocity decay rate
-            // m_angularFrictionTimescale      // body angular velocity  decay rate
-            // m_lastAngularVelocity           // what was last applied to body
-
-            if (m_angularMotorDirection.LengthSquared() > 0.0001)
-            {
-                Vector3 origVel = m_angularMotorVelocity;
-                Vector3 origDir = m_angularMotorDirection;
-
-                //       new velocity    +=                      error                       /  (  time to get there   / step interval)
-                //                           requested direction   - current vehicle direction
-                m_angularMotorVelocity += (m_angularMotorDirection - m_angularMotorVelocity) /  (m_angularMotorTimescale / pTimestep);
-                // decay requested direction
-                m_angularMotorDirection *= (1.0f - (pTimestep * 1.0f/m_angularMotorDecayTimescale));
+            ComputeAngularTurning(pTimestep);
 
-                VDetailLog("{0},MoveAngular,angularMotorApply,angTScale={1},timeStep={2},origvel={3},origDir={4},vel={5}",
-                        Prim.LocalID, m_angularMotorTimescale, pTimestep, origVel, origDir, m_angularMotorVelocity);
-            }
-            else
-            {
-                m_angularMotorVelocity = Vector3.Zero;
-            }
+            ComputeAngularVerticalAttraction();
 
-            #region Vertical attactor
+            ComputeAngularDeflection();
 
-            Vector3 vertattr = Vector3.Zero;
-            Vector3 deflection = Vector3.Zero;
-            Vector3 banking = Vector3.Zero;
+            ComputeAngularBanking();
 
-            // If vertical attaction timescale is reasonable and we applied an angular force last time...
-            if (m_verticalAttractionTimescale < 300 && m_lastAngularVelocity != Vector3.Zero)
+            // ==================================================================
+            if (VehicleRotationalVelocity.ApproxEquals(Vector3.Zero, 0.0001f))
             {
-                float VAservo = pTimestep * 0.2f / m_verticalAttractionTimescale;
-                if (Prim.IsColliding)
-                    VAservo = pTimestep * 0.05f / (m_verticalAttractionTimescale);
-
-                VAservo *= (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency);
-
-                // Create a vector of the vehicle "up" in world coordinates
-                Vector3 verticalError = Vector3.UnitZ * Prim.ForceOrientation;
-                // verticalError.X and .Y are the World error amounts. They are 0 when there is no
-                //      error (Vehicle Body is 'vertical'), and .Z will be 1. As the body leans to its
-                //      side |.X| will increase to 1 and .Z fall to 0. As body inverts |.X| will fall
-                //      and .Z will go // negative. Similar for tilt and |.Y|. .X and .Y must be
-                //      modulated to prevent a stable inverted body.
-
-                // Error is 0 (no error) to +/- 2 (max error)
-                if (verticalError.Z < 0.0f)
-                {
-                    verticalError.X = 2.0f - verticalError.X;
-                    verticalError.Y = 2.0f - verticalError.Y;
-                }
-                // scale it by VAservo (timestep and timescale)
-                verticalError = verticalError * VAservo;
-
-                // As the body rotates around the X axis, then verticalError.Y increases; Rotated around Y
-                //     then .X increases, so change  Body angular velocity  X based on Y, and Y based on X.
-                //     Z is not changed.
-                vertattr.X =    verticalError.Y;
-                vertattr.Y =  - verticalError.X;
-                vertattr.Z = 0f;
-
-                // scaling appears better usingsquare-law
-                Vector3 angularVelocity = Prim.ForceRotationalVelocity;
-                float bounce = 1.0f - (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency);
-                vertattr.X += bounce * angularVelocity.X;
-                vertattr.Y += bounce * angularVelocity.Y;
-
-                VDetailLog("{0},MoveAngular,verticalAttraction,VAservo={1},effic={2},verticalError={3},bounce={4},vertattr={5}",
-                            Prim.LocalID, VAservo, m_verticalAttractionEfficiency, verticalError, bounce, vertattr);
-
-            }
-            #endregion // Vertical attactor
-
-            #region Deflection
-
-            if (m_angularDeflectionEfficiency != 0)
-            {
-                // Compute a scaled vector that points in the preferred axis (X direction)
-                Vector3 scaledDefaultDirection =
-                    new Vector3((pTimestep * 10 * (m_angularDeflectionEfficiency / m_angularDeflectionTimescale)), 0, 0);
-                // Adding the current vehicle orientation and reference frame displaces the orientation to the frame.
-                // Rotate the scaled default axix relative to the actual vehicle direction giving where it should point.
-                Vector3 preferredAxisOfMotion = scaledDefaultDirection * Quaternion.Add(Prim.ForceOrientation, m_referenceFrame);
-
-                // Scale by efficiency and timescale
-                deflection = (preferredAxisOfMotion * (m_angularDeflectionEfficiency) / m_angularDeflectionTimescale) * pTimestep;
-
-                VDetailLog("{0},MoveAngular,Deflection,perfAxis={1},deflection={2}", 
-                                        Prim.LocalID, preferredAxisOfMotion, deflection);
-                // This deflection computation is not correct.
-                deflection = Vector3.Zero;
+                // The vehicle is not adding anything angular wise.
+                VehicleRotationalVelocity = Vector3.Zero;
+                VDetailLog("{0},  MoveAngular,done,zero", Prim.LocalID);
             }
-
-            #endregion
-
-            #region Banking
-
-            if (m_bankingEfficiency != 0)
+            else
             {
-                Vector3 dir = Vector3.One * Prim.ForceOrientation;
-                float mult = (m_bankingMix*m_bankingMix)*-1*(m_bankingMix < 0 ? -1 : 1);
-                    //Changes which way it banks in and out of turns
-
-                //Use the square of the efficiency, as it looks much more how SL banking works
-                float effSquared = (m_bankingEfficiency*m_bankingEfficiency);
-                if (m_bankingEfficiency < 0)
-                    effSquared *= -1; //Keep the negative!
-
-                float mix = Math.Abs(m_bankingMix);
-                if (m_angularMotorVelocity.X == 0)
-                {
-                    /*if (!parent.Orientation.ApproxEquals(this.m_referenceFrame, 0.25f))
-                    {
-                        Vector3 axisAngle;
-                        float angle;
-                        parent.Orientation.GetAxisAngle(out axisAngle, out angle);
-                        Vector3 rotatedVel = parent.Velocity * parent.Orientation;
-                        if ((rotatedVel.X < 0 && axisAngle.Y > 0) || (rotatedVel.X > 0 && axisAngle.Y < 0))
-                            m_angularMotorVelocity.X += (effSquared * (mult * mix)) * (1f) * 10;
-                        else
-                            m_angularMotorVelocity.X += (effSquared * (mult * mix)) * (-1f) * 10;
-                    }*/
-                }
-                else
-                    banking.Z += (effSquared*(mult*mix))*(m_angularMotorVelocity.X) * 4;
-                if (!Prim.IsColliding && Math.Abs(m_angularMotorVelocity.X) > mix)
-                    //If they are colliding, we probably shouldn't shove the prim around... probably
-                {
-                    float angVelZ = m_angularMotorVelocity.X*-1;
-                    /*if(angVelZ > mix)
-                        angVelZ = mix;
-                    else if(angVelZ < -mix)
-                        angVelZ = -mix;*/
-                    //This controls how fast and how far the banking occurs
-                    Vector3 bankingRot = new Vector3(angVelZ*(effSquared*mult), 0, 0);
-                    if (bankingRot.X > 3)
-                        bankingRot.X = 3;
-                    else if (bankingRot.X < -3)
-                        bankingRot.X = -3;
-                    bankingRot *= Prim.ForceOrientation;
-                    banking += bankingRot;
-                }
-                m_angularMotorVelocity.X *= m_bankingEfficiency == 1 ? 0.0f : 1 - m_bankingEfficiency;
-                VDetailLog("{0},MoveAngular,Banking,bEff={1},angMotVel={2},banking={3}", 
-                                Prim.LocalID, m_bankingEfficiency, m_angularMotorVelocity, banking);
+                VDetailLog("{0},  MoveAngular,done,nonZero,angVel={1}", Prim.LocalID, VehicleRotationalVelocity);
             }
 
-            #endregion
-
-            m_lastVertAttractor = vertattr;
-
-            // Sum velocities
-            m_lastAngularVelocity = m_angularMotorVelocity + vertattr + banking + deflection;
-
-            #region Linear Motor Offset
-
+            // ==================================================================
             //Offset section
             if (m_linearMotorOffset != Vector3.Zero)
             {
@@ -985,8 +1250,8 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 //
                 // The torque created is the linear velocity crossed with the offset
 
-                // NOTE: this computation does should be in the linear section
-                //    because there we know the impulse being applied.
+                // TODO: this computation should be in the linear section
+                //    because that is where we know the impulse being applied.
                 Vector3 torqueFromOffset = Vector3.Zero;
                 // torqueFromOffset = Vector3.Cross(m_linearMotorOffset, appliedImpulse);
                 if (float.IsNaN(torqueFromOffset.X))
@@ -995,47 +1260,258 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     torqueFromOffset.Y = 0;
                 if (float.IsNaN(torqueFromOffset.Z))
                     torqueFromOffset.Z = 0;
-                torqueFromOffset *= m_vehicleMass;
-                Prim.ApplyTorqueImpulse(torqueFromOffset, true);
-                VDetailLog("{0},BSDynamic.MoveAngular,motorOffset,applyTorqueImpulse={1}", Prim.LocalID, torqueFromOffset);
+
+                VehicleAddAngularForce(torqueFromOffset * m_vehicleMass);
+                VDetailLog("{0},  BSDynamic.MoveAngular,motorOffset,applyTorqueImpulse={1}", Prim.LocalID, torqueFromOffset);
             }
 
-            #endregion
+        }
+
+        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;
+          //  }
+
+            VehicleRotationalVelocity += angularMotorContributionV * VehicleOrientation;
+            VDetailLog("{0},  MoveAngular,angularTurning,angularMotorContrib={1}", Prim.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
+        //      the world z-axis (a.k.a. "up"). To take advantage of this feature you would set the
+        //      VEHICLE_VERTICAL_ATTRACTION_TIMESCALE to control the period of the spring frequency,
+        //      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 void ComputeAngularVerticalAttraction()
+        {
 
-            if ((m_flags & (VehicleFlag.NO_DEFLECTION_UP)) != 0)
+            // If vertical attaction timescale is reasonable
+            if (enableAngularVerticalAttraction && m_verticalAttractionTimescale < m_verticalAttractionCutoff)
             {
-                m_lastAngularVelocity.X = 0;
-                m_lastAngularVelocity.Y = 0;
-                VDetailLog("{0},MoveAngular,noDeflectionUp,lastAngular={1}", Prim.LocalID, m_lastAngularVelocity);
+                // 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}",
+                                Prim.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.Normalize(Vector3.UnitZ * VehicleOrientation);
+
+                // If vertical attraction correction is needed, the vector that was pointing up (UnitZ)
+                //    is now:
+                //    leaning to one side: rotated around the X axis with the Y value going
+                //        from zero (nearly straight up) to one (completely to the side)) or
+                //    leaning front-to-back: rotated around the Y axis with the value of X being between
+                //         zero and one.
+                // The value of Z is how far the rotation is off with 1 meaning none and 0 being 90 degrees.
+
+                // 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.
+                vertContributionV.X = (float)Math.Asin(verticalError.Y);
+                // (Tilt forward (positive X) needs to tilt back (rotate negative) around Y axis.)
+                vertContributionV.Y = -(float)Math.Asin(verticalError.X);
+
+                // If verticalError.Z is negative, the vehicle is upside down. Add additional push.
+                if (verticalError.Z < 0f)
+                {
+                    vertContributionV.X += Math.Sign(vertContributionV.X) * PIOverFour;
+                    // vertContribution.Y -= PIOverFour;
+                }
+
+                // 'vertContrbution' is now the necessary angular correction to correct tilt in one second.
+                //     Correction happens over a number of seconds.
+                Vector3 unscaledContribVerticalErrorV = vertContributionV;     // DEBUG DEBUG
+
+                // The correction happens over the user's time period
+                vertContributionV /= m_verticalAttractionTimescale;
+
+                // 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);
+                */
             }
+        }
+
+        // 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 void ComputeAngularDeflection()
+        {
+            // 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_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f))
+            if (enableAngularDeflection && m_angularDeflectionEfficiency != 0 && VehicleForwardSpeed > 0.2)
             {
-                m_lastAngularVelocity = Vector3.Zero; // Reduce small value to zero.
-                Prim.ZeroAngularMotion(true);
-                VDetailLog("{0},MoveAngular,zeroAngularMotion,lastAngular={1}", Prim.LocalID, m_lastAngularVelocity);
+                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();
+
+                // The difference between what is and what should be.
+                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;
+
+                // ret = m_angularDeflectionCorrectionMotor(1f, deflectionError);
+
+                // Scale the correction by recovery timescale and efficiency
+                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, deflectContributionV);
+                VDetailLog("{0},  MoveAngular,Deflection,fwdSpd={1},defEff={2},defTS={3}",
+                    Prim.LocalID, VehicleForwardSpeed, m_angularDeflectionEfficiency, m_angularDeflectionTimescale);
             }
-            else
+        }
+
+        // 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
+        //      function. The way banking works is this: a rotation around the vehicle's roll-axis will
+        //      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. 
+        //          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?). 
+        //           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
+        //                  it to turn while banking, however video-game motorcycles are often configured
+        //                  to turn in place when at a dead stop--because they are often easier to control
+        //                  that way using the limited interface of the keyboard or game controller. The
+        //                  VEHICLE_BANKING_MIX enables combinations of both realistic and non-realistic
+        //                  banking by functioning as a slider between a banking that is correspondingly
+        //                  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. 
+        //      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 void ComputeAngularBanking()
+        {
+            if (enableAngularBanking && m_bankingEfficiency != 0 && m_verticalAttractionTimescale < m_verticalAttractionCutoff)
             {
-                // Apply to the body.
-                // The above calculates the absolute angular velocity needed. Angular velocity is massless.
-                // Since we are stuffing the angular velocity directly into the object, the computed
-                //     velocity needs to be scaled by the timestep.
-                Vector3 applyAngularForce = ((m_lastAngularVelocity * pTimestep) - Prim.ForceRotationalVelocity);
-                Prim.ForceRotationalVelocity = applyAngularForce;
-
-                // Decay the angular movement for next time
-                Vector3 decayamount = (Vector3.One / m_angularFrictionTimescale) * pTimestep;
-                m_lastAngularVelocity *= Vector3.One - decayamount;
-
-                VDetailLog("{0},MoveAngular,done,newRotVel={1},decay={2},lastAngular={3}", 
-                                    Prim.LocalID, applyAngularForce, decayamount, m_lastAngularVelocity);
+                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 yawAngle = m_angularMotorDirection.X * m_bankingEfficiency;
+                //        actual error  =       static turn error            +           dynamic turn error
+                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?
+                //     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
+                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;
+
+                //VehicleRotationalVelocity += bankingContributionV * VehicleOrientation;
+                VehicleRotationalVelocity += bankingContributionV;
+                
+
+                VDetailLog("{0},  MoveAngular,Banking,rollComp={1},speed={2},rollComp={3},yAng={4},mYAng={5},ret={6}",
+                            Prim.LocalID, rollComponents, VehicleForwardSpeed, rollComponents, yawAngle, mixedYawAngle, bankingContributionV);
             }
-        } //end MoveAngular
+        }
 
+        // This is from previous instantiations of XXXDynamics.cs.
+        // Applies roll reference frame.
+        // TODO: is this the right way to separate the code to do this operation?
+        //    Should this be in MoveAngular()?
         internal void LimitRotation(float timestep)
         {
-            Quaternion rotq = Prim.ForceOrientation;
+            Quaternion rotq = VehicleOrientation;
             Quaternion m_rot = rotq;
             if (m_RollreferenceFrame != Quaternion.Identity)
             {
@@ -1063,12 +1539,18 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             }
             if (rotq != m_rot)
             {
-                Prim.ForceOrientation = m_rot;
-                VDetailLog("{0},LimitRotation,done,orig={1},new={2}", Prim.LocalID, rotq, m_rot);
+                VehicleOrientation = m_rot;
+                VDetailLog("{0},  LimitRotation,done,orig={1},new={2}", Prim.LocalID, rotq, m_rot);
             }
 
         }
 
+        private float ClampInRange(float low, float val, float high)
+        {
+            return Math.Max(low, Math.Min(val, high));
+            // return Utils.Clamp(val, low, high);
+        }
+
         // Invoke the detailed logger and output something if it's enabled.
         private void VDetailLog(string msg, params Object[] args)
         {
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
index 0df4310..4ece1eb 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
@@ -32,6 +32,15 @@ using OMV = OpenMetaverse;
 
 namespace OpenSim.Region.Physics.BulletSPlugin
 {
+
+// A BSPrim can get individual information about its linkedness attached
+//    to it through an instance of a subclass of LinksetInfo.
+// Each type of linkset will define the information needed for its type.
+public abstract class BSLinksetInfo
+{
+    public virtual void Clear() { }
+}
+
 public abstract class BSLinkset
 {
     // private static string LogHeader = "[BULLETSIM LINKSET]";
@@ -43,11 +52,11 @@ 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;
 
-        switch ((int)physScene.Params.linksetImplementation)
+        switch ((int)BSParam.LinksetImplementation)
         {
             case (int)LinksetImplementation.Constraint:
                 ret = new BSLinksetConstraints(physScene, parent);
@@ -62,10 +71,14 @@ 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; }
 
@@ -73,7 +86,7 @@ public abstract class BSLinkset
     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
@@ -82,27 +95,13 @@ public abstract class BSLinkset
 
     // 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)
+    public virtual BSPhysicsShapeType PreferredPhysicalShape(BSPrimLinkable requestor)
     {
         return BSPhysicsShapeType.SHAPE_UNKNOWN;
     }
 
-    // Linksets move around the children so the linkset might need to compute the child position
-    public virtual OMV.Vector3 Position(BSPhysObject member)
-        { return member.RawPosition; }
-    public virtual OMV.Quaternion Orientation(BSPhysObject member)
-        { return member.RawOrientation; }
-    // TODO: does this need to be done for Velocity and RotationalVelocityy?
-    
     // We keep the prim's mass in the linkset structure since it could be dependent on other prims
-    protected float m_mass;
-    public float LinksetMass
-    {
-        get
-        {
-            return m_mass;
-        }
-    }
+    public float LinksetMass { get; protected set; }
 
     public virtual bool LinksetIsColliding { get { return false; } }
 
@@ -116,7 +115,7 @@ public abstract class BSLinkset
         get { return ComputeLinksetGeometricCenter(); }
     }
 
-    protected void Initialize(BSScene scene, BSPhysObject parent)
+    protected BSLinkset(BSScene scene, BSPrimLinkable parent)
     {
         // A simple linkset of one (no children)
         LinksetID = m_nextLinksetID++;
@@ -125,22 +124,25 @@ public abstract class BSLinkset
             m_nextLinksetID = 1;
         PhysicsScene = scene;
         LinksetRoot = parent;
-        m_children = new HashSet<BSPhysObject>();
-        m_mass = parent.RawMass;
+        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)
         {
             // Don't add the root to its own linkset
             if (!IsRoot(child))
                 AddChildToLinkset(child);
-            m_mass = ComputeLinksetMass();
+            LinksetMass = ComputeLinksetMass();
         }
         return this;
     }
@@ -149,7 +151,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,7 +161,7 @@ public abstract class BSLinkset
                 return this;
             }
             RemoveChildFromLinkset(child);
-            m_mass = ComputeLinksetMass();
+            LinksetMass = ComputeLinksetMass();
         }
 
         // The child is down to a linkset of just itself
@@ -167,7 +169,7 @@ public abstract class BSLinkset
     }
 
     // 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);
     }
@@ -178,14 +180,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)
                 {
@@ -200,14 +202,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;
@@ -218,16 +220,23 @@ 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 abstract void Refresh(BSPhysObject requestor);
+    public virtual void Refresh(BSPrimLinkable requestor)
+    {
+        LinksetMass = ComputeLinksetMass();
+    }
+
+    // Flag denoting the linkset is in the process of being rebuilt.
+    // Used to know not the schedule a rebuild in the middle of a rebuild.
+    protected bool Rebuilding { get; set; }
 
     // The object is going dynamic (physical). Do any setup necessary
     //     for a dynamic linkset.
@@ -235,30 +244,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);
+    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 RemoveBodyDependencies(BSPrimLinkable child);
 
     // ================================================================
     protected virtual float ComputeLinksetMass()
@@ -268,7 +273,7 @@ public abstract class BSLinkset
         {
             lock (m_linksetActivityLock)
             {
-                foreach (BSPhysObject bp in m_children)
+                foreach (BSPrimLinkable bp in m_children)
                 {
                     mass += bp.RawMass;
                 }
@@ -277,6 +282,7 @@ public abstract class BSLinkset
         return mass;
     }
 
+    // Computes linkset's center of mass in world coordinates.
     protected virtual OMV.Vector3 ComputeLinksetCenterOfMass()
     {
         OMV.Vector3 com;
@@ -285,7 +291,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;
@@ -304,9 +310,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);
         }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs
index b9c2cf9..e05562a 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs
@@ -28,22 +28,80 @@ using System;
 using System.Collections.Generic;
 using System.Text;
 
+using OpenSim.Framework;
+
 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 int Index;
+    public OMV.Vector3 OffsetFromRoot;
+    public OMV.Vector3 OffsetFromCenterOfMass;
+    public OMV.Quaternion OffsetRot;
+    public BSLinksetCompoundInfo(int indx, OMV.Vector3 p, OMV.Quaternion r)
+    {
+        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()
+    {
+        Index = 0;
+        OffsetFromRoot = OMV.Vector3.Zero;
+        OffsetFromCenterOfMass = OMV.Vector3.Zero;
+        OffsetRot = OMV.Quaternion.Identity;
+    }
+    public override string ToString()
+    {
+        StringBuilder buff = new StringBuilder();
+        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)
+    public BSLinksetCompound(BSScene scene, BSPrimLinkable parent)
+        : base(scene, parent)
     {
-        base.Initialize(scene, parent);
     }
 
     // For compound implimented linksets, if there are children, use compound shape for the root.
-    public override BSPhysicsShapeType PreferredPhysicalShape(BSPhysObject requestor)
+    public override BSPhysicsShapeType PreferredPhysicalShape(BSPrimLinkable requestor)
     { 
+        // Returning 'unknown' means we don't have a preference.
         BSPhysicsShapeType ret = BSPhysicsShapeType.SHAPE_UNKNOWN;
         if (IsRoot(requestor) && HasAnyChildren)
         {
@@ -55,41 +113,57 @@ public sealed class BSLinksetCompound : BSLinkset
 
     // When physical properties are changed the linkset needs to recalculate
     //   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)
     {
-        // External request for Refresh (from BSPrim) is not necessary
-        // InternalRefresh(requestor);
+        base.Refresh(requestor);
+
+        // Something changed so do the rebuilding thing
+        // ScheduleRebuild();
     }
 
-    private void InternalRefresh(BSPhysObject requestor)
+    // Schedule a refresh to happen after all the other taint processing.
+    private void ScheduleRebuild(BSPrimLinkable requestor)
     {
-        DetailLog("{0},BSLinksetCompound.Refresh,schedulingRefresh,requestor={1}", LinksetRoot.LocalID, requestor.LocalID);
-        // Queue to happen after all the other taint processing
-        PhysicsScene.PostTaintObject("BSLinksetCompound.Refresh", requestor.LocalID, delegate()
+        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)
         {
-            if (IsRoot(requestor) && HasAnyChildren)
-                RecomputeLinksetCompound();
-        });
+            PhysicsScene.PostTaintObject("BSLinksetCompound.ScheduleRebuild", LinksetRoot.LocalID, delegate()
+            {
+                if (HasAnyChildren)
+                    RecomputeLinksetCompound();
+            });
+        }
     }
 
-    // The object is going dynamic (physical). Do any setup necessary
-    //     for a dynamic linkset.
+    // The object is going dynamic (physical). Do any setup necessary for a dynamic linkset.
     // Only the state of the passed object can be modified. The rest of the linkset
     //     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));
-        if (!IsRoot(child))
+        if (IsRoot(child))
+        {
+            // The root is going dynamic. Rebuild the linkset so parts and mass get computed properly.
+            ScheduleRebuild(LinksetRoot);
+        }
+        else
         {
-            // Physical children are removed from the world as the shape ofthe root compound
+            // The origional prims are removed from the world as the shape of the root compound
             //     shape takes over.
-            BulletSimAPI.AddToCollisionFlags2(child.PhysBody.ptr, CollisionFlags.CF_NO_CONTACT_RESPONSE);
-            BulletSimAPI.ForceActivationState2(child.PhysBody.ptr, ActivationState.DISABLE_SIMULATION);
+            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;
@@ -100,73 +174,181 @@ public sealed class BSLinksetCompound : 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)
     {
         bool ret = false;
         DetailLog("{0},BSLinksetCompound.MakeStatic,call,IsRoot={1}", child.LocalID, IsRoot(child));
-        if (!IsRoot(child))
+        if (IsRoot(child))
+        {
+            ScheduleRebuild(LinksetRoot);
+        }
+        else
         {
             // The non-physical children can come back to life.
-            BulletSimAPI.RemoveFromCollisionFlags2(child.PhysBody.ptr, CollisionFlags.CF_NO_CONTACT_RESPONSE);
-            // Don't force activation so setting of DISABLE_SIMULATION can stay.
-            BulletSimAPI.Activate2(child.PhysBody.ptr, false);
+            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;
     }
 
-    // Called at taint-time!!
-    public override void UpdateProperties(BSPhysObject updated)
-    {
-        // Nothing to do for constraints on property updates
-    }
-
-    // The children move around in relationship to the root.
-    // Just grab the current values of wherever it is right now.
-    public override OMV.Vector3 Position(BSPhysObject member)
+    // '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)
     {
-        return BulletSimAPI.GetPosition2(member.PhysBody.ptr);
-    }
+        // 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
+        //    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) && PhysicsScene.TerrainManager.IsWithinKnownTerrain(LinksetRoot.RawPosition))
+        {
+            // 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 && PhysicsScene.PE.IsCompound(LinksetRoot.PhysShape))
+                {
+                    // 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 = PhysicsScene.PE.GetNumberOfCompoundChildren(LinksetRoot.PhysShape);
+                    if (updated.LinksetChildIndex < numLinksetChildren)
+                    {
+                        BulletShape linksetChildShape = PhysicsScene.PE.GetChildShapeFromCompoundShapeIndex(LinksetRoot.PhysShape, updated.LinksetChildIndex);
+                        if (linksetChildShape.HasPhysicalShape)
+                        {
+                            // Found the child shape within the compound shape
+                            PhysicsScene.PE.UpdateChildTransform(LinksetRoot.PhysShape, 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
 
-    public override OMV.Quaternion Orientation(BSPhysObject member)
-    {
-        return BulletSimAPI.GetOrientation2(member.PhysBody.ptr);
+                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.
     // Returns 'true' of something was actually removed and would need restoring
     // Called at taint-time!!
-    public override bool RemoveBodyDependencies(BSPrim child)
+    public override bool RemoveBodyDependencies(BSPrimLinkable child)
     {
         bool ret = false;
 
         DetailLog("{0},BSLinksetCompound.RemoveBodyDependencies,refreshIfChild,rID={1},rBody={2},isRoot={3}",
-                        child.LocalID, LinksetRoot.LocalID, LinksetRoot.PhysBody.ptr.ToString("X"), IsRoot(child));
+                        child.LocalID, LinksetRoot.LocalID, LinksetRoot.PhysBody, IsRoot(child));
 
         if (!IsRoot(child))
         {
-            // Cause the current shape to be freed and the new one to be built.
-            InternalRefresh(LinksetRoot);
-            ret = true;
+            // Because it is a convenient time, recompute child world position and rotation based on
+            //    its position in the linkset.
+            RecomputeChildWorldPosition(child, true /* inTaintTime */);
+            child.LinksetInfo = null;
         }
 
+        // Cannot schedule a refresh/rebuild here because this routine is called when
+        //     the linkset is being rebuilt.
+        // InternalRefresh(LinksetRoot);
+
         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(BSPrimLinkable child, bool inTaintTime)
     {
-        // The Refresh operation queued by RemoveBodyDependencies() will build any missing constraints.
+        // For the moment (20130201), disable this computation (converting the child physical addr back to
+        //    a region address) until we have a good handle on center-of-mass offsets and what the physics
+        //    engine moving a child actually means.
+        // The simulator keeps track of where children should be as the linkset moves. Setting
+        //    the pos/rot here does not effect that knowledge as there is no good way for the
+        //    physics engine to send the simulator an update for a child.
+
+        /*
+        BSLinksetCompoundInfo lci = child.LinksetInfo as BSLinksetCompoundInfo;
+        if (lci != null)
+        {
+            if (inTaintTime)
+            {
+                OMV.Vector3 oldPos = child.RawPosition;
+                child.ForcePosition = LinksetRoot.RawPosition + lci.OffsetFromRoot;
+                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.OffsetFromRoot;
+                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))
         {
@@ -174,24 +356,28 @@ public sealed class BSLinksetCompound : BSLinkset
 
             DetailLog("{0},BSLinksetCompound.AddChildToLinkset,call,child={1}", LinksetRoot.LocalID, child.LocalID);
 
-            // Cause constraints and assorted properties to be recomputed before the next simulation step.
-            InternalRefresh(LinksetRoot);
+            // Rebuild the compound shape with the new child shape included
+            ScheduleRebuild(child);
         }
         return;
     }
 
     // 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)
+    // Safe to call even if the child is not really in the linkset.
+    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}",
                             child.LocalID,
-                            LinksetRoot.LocalID, LinksetRoot.PhysBody.ptr.ToString("X"),
-                            child.LocalID, child.PhysBody.ptr.ToString("X"));
+                            LinksetRoot.LocalID, LinksetRoot.PhysBody.AddrString,
+                            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)
@@ -201,8 +387,8 @@ public sealed class BSLinksetCompound : BSLinkset
             }
             else
             {
-                // Schedule a rebuild of the linkset  before the next simulation tick.
-                InternalRefresh(LinksetRoot);
+                // Rebuild the compound shape with the child removed
+                ScheduleRebuild(LinksetRoot);
             }
         }
         return;
@@ -213,63 +399,116 @@ 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 disableCOM = true;     // DEBUG DEBUG: disable until we get this debugged
     private void RecomputeLinksetCompound()
     {
-        // Cause the root shape to be rebuilt as a compound object with just the root in it
-        LinksetRoot.ForceBodyShapeRebuild(true);
+        try
+        {
+            // Suppress rebuilding while rebuilding. (We know rebuilding is on only one thread.)
+            Rebuilding = true;
 
-        DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,start,rBody={1},rShape={2},numChildren={3}",
-                        LinksetRoot.LocalID, LinksetRoot.PhysBody, LinksetRoot.PhysShape, NumberOfChildren);
+            // Cause the root shape to be rebuilt as a compound object with just the root in it
+            LinksetRoot.ForceBodyShapeRebuild(true /* inTaintTime */);
 
-        // Add a shape for each of the other children in the linkset
-        ForEachMember(delegate(BSPhysObject cPrim)
-        {
-            if (!IsRoot(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 = LinksetRoot.RawPosition;
+            if (!disableCOM)                             // DEBUG DEBUG
             {
-                // 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;
+                // Compute a center-of-mass in world coordinates.
+                centerOfMassW = ComputeLinksetCenterOfMass();
+            }
+
+            OMV.Quaternion invRootOrientation = OMV.Quaternion.Inverse(LinksetRoot.RawOrientation);
+
+            // 'centerDisplacement' is the value to subtract from children to give physical offset position
+            OMV.Vector3 centerDisplacement = (centerOfMassW - LinksetRoot.RawPosition) * invRootOrientation;
+            LinksetRoot.SetEffectiveCenterOfMassW(centerDisplacement);
 
-                DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,addMemberToShape,mID={1},mShape={2},dispPos={3},dispRot={4}",
-                    LinksetRoot.LocalID, cPrim.LocalID, cPrim.PhysShape, displacementPos, displacementRot);
+            // This causes the physical position of the root prim to be offset to accomodate for the displacements
+            LinksetRoot.ForcePosition = LinksetRoot.RawPosition;
 
-                if (cPrim.PhysShape.isNativeShape)
+            // Update the local transform for the root child shape so it is offset from the <0,0,0> which is COM
+            PhysicsScene.PE.UpdateChildTransform(LinksetRoot.PhysShape, 0 /* childIndex */,
+                                                -centerDisplacement,
+                                                OMV.Quaternion.Identity, // LinksetRoot.RawOrientation,
+                                                false /* shouldRecalculateLocalAabb (is done later after linkset built) */);
+
+            DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,COM,com={1},rootPos={2},centerDisp={3}",
+                                    LinksetRoot.LocalID, centerOfMassW, LinksetRoot.RawPosition, centerDisplacement);
+
+            DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,start,rBody={1},rShape={2},numChildren={3}",
+                            LinksetRoot.LocalID, LinksetRoot.PhysBody, LinksetRoot.PhysShape, NumberOfChildren);
+
+            // Add a shape for each of the other children in the linkset
+            int memberIndex = 1;
+            ForEachMember(delegate(BSPrimLinkable cPrim)
+            {
+                if (IsRoot(cPrim))
                 {
-                    // Native shapes are not shared so we need to create a new one.
-                    // A mesh or hull is created because scale is not available on a native shape.
-                    //     (TODO: Bullet does have a btScaledCollisionShape. Can that be used?)
-                    BulletShape saveShape = cPrim.PhysShape;
-                    cPrim.PhysShape.ptr = IntPtr.Zero;  // Don't let the create free the child's shape
-                    PhysicsScene.Shapes.CreateGeomMeshOrHull(cPrim, null);
-                    BulletShape newShape = cPrim.PhysShape;
-                    cPrim.PhysShape = saveShape;
-                    BulletSimAPI.AddChildShapeToCompoundShape2(LinksetRoot.PhysShape.ptr, newShape.ptr, displacementPos, displacementRot);
+                    cPrim.LinksetChildIndex = 0;
                 }
                 else
                 {
-                    // For the shared shapes (meshes and hulls), just use the shape in the child.
-                    if (PhysicsScene.Shapes.ReferenceShape(cPrim.PhysShape))
+                    cPrim.LinksetChildIndex = memberIndex;
+
+                    if (cPrim.PhysShape.isNativeShape)
                     {
-                        PhysicsScene.Logger.ErrorFormat("{0} Rebuilt sharable shape when building linkset! Region={1}, primID={2}, shape={3}",
-                                            LogHeader, PhysicsScene.RegionName, cPrim.LocalID, cPrim.PhysShape);
+                        // A native shape is turned 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 scale 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.CreateGeomMeshOrHull(cPrim, null);
+                        BulletShape newShape = cPrim.PhysShape;
+                        cPrim.PhysShape = saveShape;
+
+                        OMV.Vector3 offsetPos = (cPrim.RawPosition - LinksetRoot.RawPosition) * invRootOrientation - centerDisplacement;
+                        OMV.Quaternion offsetRot = cPrim.RawOrientation * invRootOrientation;
+                        PhysicsScene.PE.AddChildShapeToCompoundShape(LinksetRoot.PhysShape, newShape, offsetPos, offsetRot);
+                        DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,addNative,indx={1},rShape={2},cShape={3},offPos={4},offRot={5}",
+                                    LinksetRoot.LocalID, memberIndex, LinksetRoot.PhysShape, newShape, offsetPos, offsetRot);
                     }
-                    BulletSimAPI.AddChildShapeToCompoundShape2(LinksetRoot.PhysShape.ptr, cPrim.PhysShape.ptr, displacementPos, displacementRot);
-                }
-            }
-            return false;   // 'false' says to move onto the next child in the list
-        });
+                    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);
+                        }
+                        OMV.Vector3 offsetPos = (cPrim.RawPosition - LinksetRoot.RawPosition) * invRootOrientation - centerDisplacement;
+                        OMV.Quaternion offsetRot = cPrim.RawOrientation * invRootOrientation;
+                        PhysicsScene.PE.AddChildShapeToCompoundShape(LinksetRoot.PhysShape, cPrim.PhysShape, offsetPos, offsetRot);
+                        DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,addNonNative,indx={1},rShape={2},cShape={3},offPos={4},offRot={5}",
+                                    LinksetRoot.LocalID, memberIndex, LinksetRoot.PhysShape, cPrim.PhysShape, offsetPos, offsetRot);
 
-        // With all of the linkset packed into the root prim, it has the mass of everyone.
-        float linksetMass = LinksetMass;
-        LinksetRoot.UpdatePhysicalMassProperties(linksetMass);
+                    }
+                    memberIndex++;
+                }
+                return false;   // 'false' says to move onto the next child in the list
+            });
 
-        BulletSimAPI.RecalculateCompoundShapeLocalAabb2(LinksetRoot.PhysShape.ptr);
+            // With all of the linkset packed into the root prim, it has the mass of everyone.
+            LinksetMass = ComputeLinksetMass();
+            LinksetRoot.UpdatePhysicalMassProperties(LinksetMass, true);
 
-        // DEBUG: see of inter-linkset collisions are causing problems for constraint linksets.
-        // BulletSimAPI.SetCollisionFilterMask2(LinksetRoot.BSBody.ptr, 
-        //                     (uint)CollisionFilterGroups.LinksetFilter, (uint)CollisionFilterGroups.LinksetMask);
+            // Enable the physical position updator to return the position and rotation of the root shape
+            PhysicsScene.PE.AddToCollisionFlags(LinksetRoot.PhysBody, CollisionFlags.BS_RETURN_ROOT_COMPOUND_SHAPE);
+        }
+        finally
+        {
+            Rebuilding = false;
+        }
 
+        // See that the Aabb surrounds the new shape
+        PhysicsScene.PE.RecalculateCompoundShapeLocalAabb(LinksetRoot.PhysShape);
     }
 }
 }
\ No newline at end of file
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs
index c855fda..6d252ca 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs
@@ -36,23 +36,27 @@ public sealed class BSLinksetConstraints : BSLinkset
 {
     // private static string LogHeader = "[BULLETSIM LINKSET CONSTRAINTS]";
 
-    public BSLinksetConstraints(BSScene scene, BSPhysObject parent)
+    public BSLinksetConstraints(BSScene scene, BSPrimLinkable parent) : base(scene, parent)
     {
-        base.Initialize(scene, parent);
     }
 
     // When physical properties are changed the linkset needs to recalculate
     //   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)
     {
-        // Queue to happen after all the other taint processing
-        PhysicsScene.PostTaintObject("BSLinksetContraints.Refresh", requestor.LocalID, delegate()
-            {
-                if (HasAnyChildren && IsRoot(requestor))
-                    RecomputeLinksetConstraints();
-            });
+        base.Refresh(requestor);
+
+        if (HasAnyChildren && IsRoot(requestor))
+        {
+            // Queue to happen after all the other taint processing
+            PhysicsScene.PostTaintObject("BSLinksetContraints.Refresh", requestor.LocalID, delegate()
+                {
+                    if (HasAnyChildren && IsRoot(requestor))
+                        RecomputeLinksetConstraints();
+                });
+        }
     }
 
     // The object is going dynamic (physical). Do any setup necessary
@@ -61,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;
@@ -72,41 +76,29 @@ 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)
+    public override void UpdateProperties(UpdatedProperties whichUpdated, BSPrimLinkable pObj)
     {
         // Nothing to do for constraints on property updates
     }
 
-    // The children of the linkset are moved around by the constraints.
-    // Just grab the current values of wherever it is right now.
-    public override OMV.Vector3 Position(BSPhysObject member)
-    {
-        return BulletSimAPI.GetPosition2(member.PhysBody.ptr);
-    }
-
-    public override OMV.Quaternion Orientation(BSPhysObject member)
-    {
-        return BulletSimAPI.GetOrientation2(member.PhysBody.ptr);
-    }
-
     // Routine called when rebuilding the body of some member of the linkset.
     // Destroy all the constraints have have been made to root and set
     //     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 RemoveBodyDependencies(BSPrimLinkable child)
     {
         bool ret = false;
 
         DetailLog("{0},BSLinksetConstraint.RemoveBodyDependencies,removeChildrenForRoot,rID={1},rBody={2}",
-                                    child.LocalID, LinksetRoot.LocalID, LinksetRoot.PhysBody.ptr.ToString("X"));
+                                    child.LocalID, LinksetRoot.LocalID, LinksetRoot.PhysBody.AddrString);
 
         lock (m_linksetActivityLock)
         {
@@ -118,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))
         {
@@ -146,17 +130,17 @@ 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.ptr.ToString("X"),
-                            childx.LocalID, childx.PhysBody.ptr.ToString("X"));
+                            rootx.LocalID, rootx.PhysBody.AddrString,
+                            childx.LocalID, childx.PhysBody.AddrString);
 
             PhysicsScene.TaintedObject("BSLinksetConstraints.RemoveChildFromLinkset", delegate()
             {
@@ -175,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,8 +179,8 @@ public sealed class BSLinksetConstraints : BSLinkset
 
         DetailLog("{0},BSLinksetConstraint.BuildConstraint,taint,root={1},rBody={2},child={3},cBody={4},rLoc={5},cLoc={6},midLoc={7}",
                                         rootPrim.LocalID,
-                                        rootPrim.LocalID, rootPrim.PhysBody.ptr.ToString("X"),
-                                        childPrim.LocalID, childPrim.PhysBody.ptr.ToString("X"),
+                                        rootPrim.LocalID, rootPrim.PhysBody.AddrString,
+                                        childPrim.LocalID, childPrim.PhysBody.AddrString,
                                         rootPrim.Position, childPrim.Position, midPoint);
 
         // create a constraint that allows no freedom of movement between the two objects
@@ -239,14 +223,14 @@ public sealed class BSLinksetConstraints : BSLinkset
         constrain.SetAngularLimits(OMV.Vector3.Zero, OMV.Vector3.Zero);
 
         // tweek the constraint to increase stability
-        constrain.UseFrameOffset(PhysicsScene.BoolNumeric(PhysicsScene.Params.linkConstraintUseFrameOffset));
-        constrain.TranslationalLimitMotor(PhysicsScene.BoolNumeric(PhysicsScene.Params.linkConstraintEnableTransMotor),
-                        PhysicsScene.Params.linkConstraintTransMotorMaxVel,
-                        PhysicsScene.Params.linkConstraintTransMotorMaxForce);
-        constrain.SetCFMAndERP(PhysicsScene.Params.linkConstraintCFM, PhysicsScene.Params.linkConstraintERP);
-        if (PhysicsScene.Params.linkConstraintSolverIterations != 0f)
+        constrain.UseFrameOffset(BSParam.LinkConstraintUseFrameOffset);
+        constrain.TranslationalLimitMotor(BSParam.LinkConstraintEnableTransMotor,
+                        BSParam.LinkConstraintTransMotorMaxVel,
+                        BSParam.LinkConstraintTransMotorMaxForce);
+        constrain.SetCFMAndERP(BSParam.LinkConstraintCFM, BSParam.LinkConstraintERP);
+        if (BSParam.LinkConstraintSolverIterations != 0f)
         {
-            constrain.SetSolverIterations(PhysicsScene.Params.linkConstraintSolverIterations);
+            constrain.SetSolverIterations(BSParam.LinkConstraintSolverIterations);
         }
         return constrain;
     }
@@ -255,19 +239,19 @@ 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}",
                             rootPrim.LocalID,
-                            rootPrim.LocalID, rootPrim.PhysBody.ptr.ToString("X"),
-                            childPrim.LocalID, childPrim.PhysBody.ptr.ToString("X"));
+                            rootPrim.LocalID, rootPrim.PhysBody.AddrString,
+                            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))
         {
             // Make the child refresh its location
-            BulletSimAPI.PushUpdate2(childPrim.PhysBody.ptr);
+            PhysicsScene.PE.PushUpdate(childPrim.PhysBody);
             ret = true;
         }
 
@@ -277,7 +261,7 @@ 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);
 
@@ -292,20 +276,17 @@ public sealed class BSLinksetConstraints : BSLinkset
     private void RecomputeLinksetConstraints()
     {
         float linksetMass = LinksetMass;
-        LinksetRoot.UpdatePhysicalMassProperties(linksetMass);
+        LinksetRoot.UpdatePhysicalMassProperties(linksetMass, true);
 
-        // DEBUG: see of inter-linkset collisions are causing problems
-        // BulletSimAPI.SetCollisionFilterMask2(LinksetRoot.BSBody.ptr, 
-        //                     (uint)CollisionFilterGroups.LinksetFilter, (uint)CollisionFilterGroups.LinksetMask);
         DetailLog("{0},BSLinksetConstraint.RecomputeLinksetConstraints,set,rBody={1},linksetMass={2}",
-                            LinksetRoot.LocalID, LinksetRoot.PhysBody.ptr.ToString("X"), linksetMass);
+                            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.
             // (Also do the mass stuff before recomputing the constraint so mass is not zero.)
-            child.UpdatePhysicalMassProperties(linksetMass);
+            child.UpdatePhysicalMassProperties(linksetMass, true);
 
             BSConstraint constrain;
             if (!PhysicsScene.Constraints.TryGetConstraint(LinksetRoot.PhysBody, child.PhysBody, out constrain))
@@ -315,11 +296,7 @@ public sealed class BSLinksetConstraints : BSLinkset
             }
             constrain.RecomputeConstraintVariables(linksetMass);
 
-            // DEBUG: see of inter-linkset collisions are causing problems
-            // BulletSimAPI.SetCollisionFilterMask2(child.BSBody.ptr, 
-            //                 (uint)CollisionFilterGroups.LinksetFilter, (uint)CollisionFilterGroups.LinksetMask);
-
-            // BulletSimAPI.DumpConstraint2(PhysicsScene.World.ptr, constrain.Constraint.ptr);    // DEBUG DEBUG
+            // PhysicsScene.PE.DumpConstraint(PhysicsScene.World, constrain.Constraint);    // DEBUG DEBUG
         }
 
     }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSMaterials.cs b/OpenSim/Region/Physics/BulletSPlugin/BSMaterials.cs
new file mode 100755
index 0000000..ee77d6e
--- /dev/null
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSMaterials.cs
@@ -0,0 +1,203 @@
+/*
+ * Copyright (c) Contributors, http://opensimulator.org/
+ * See CONTRIBUTORS.TXT for a full list of copyright holders.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyrightD
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the OpenSimulator Project nor the
+ *       names of its contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+using System;
+using System.Collections.Generic;
+using System.Text;
+using System.Reflection;
+using Nini.Config;
+
+namespace OpenSim.Region.Physics.BulletSPlugin
+{
+
+public struct MaterialAttributes
+{
+    // Material type values that correspond with definitions for LSL
+    public enum Material : int
+    {
+        Stone = 0,
+        Metal,
+        Glass,
+        Wood,
+        Flesh,
+        Plastic,
+        Rubber,
+        Light,
+        // Hereafter are BulletSim additions
+        Avatar,
+        NumberOfTypes   // the count of types in the enum.
+    }
+
+    // Names must be in the order of the above enum.
+    // These names must coorespond to the lower case field names in the MaterialAttributes
+    //     structure as reflection is used to select the field to put the value in.
+    public static readonly string[] MaterialAttribs = { "Density", "Friction", "Restitution"};
+
+    public MaterialAttributes(string t, float d, float f, float r)
+    {
+        type = t;
+        density = d;
+        friction = f;
+        restitution = r;
+    }
+    public string type;
+    public float density;
+    public float friction;
+    public float restitution;
+}
+
+public static class BSMaterials
+{
+    // Attributes for each material type
+    private static readonly MaterialAttributes[] Attributes;
+
+    // Map of material name to material type code
+    public static readonly Dictionary<string, MaterialAttributes.Material> MaterialMap;
+
+    static BSMaterials()
+    {
+        // Attribute sets for both the non-physical and physical instances of materials.
+        Attributes = new MaterialAttributes[(int)MaterialAttributes.Material.NumberOfTypes * 2];
+
+        // Map of name to type code.
+        MaterialMap = new Dictionary<string, MaterialAttributes.Material>();
+        MaterialMap.Add("Stone", MaterialAttributes.Material.Stone);
+        MaterialMap.Add("Metal", MaterialAttributes.Material.Metal);
+        MaterialMap.Add("Glass", MaterialAttributes.Material.Glass);
+        MaterialMap.Add("Wood", MaterialAttributes.Material.Wood);
+        MaterialMap.Add("Flesh", MaterialAttributes.Material.Flesh);
+        MaterialMap.Add("Plastic", MaterialAttributes.Material.Plastic);
+        MaterialMap.Add("Rubber", MaterialAttributes.Material.Rubber);
+        MaterialMap.Add("Light", MaterialAttributes.Material.Light);
+        MaterialMap.Add("Avatar", MaterialAttributes.Material.Avatar);
+    }
+
+    // This is where all the default material attributes are defined.
+    public static void InitializeFromDefaults(ConfigurationParameters parms)
+    {
+        // Values from http://wiki.secondlife.com/wiki/PRIM_MATERIAL
+        float dDensity = parms.defaultDensity;
+        float dFriction = parms.defaultFriction;
+        float dRestitution = parms.defaultRestitution;
+        Attributes[(int)MaterialAttributes.Material.Stone] =
+                    new MaterialAttributes("stone",dDensity, 0.8f, 0.4f);
+        Attributes[(int)MaterialAttributes.Material.Metal] =
+                    new MaterialAttributes("metal",dDensity, 0.3f, 0.4f);
+        Attributes[(int)MaterialAttributes.Material.Glass] =
+                    new MaterialAttributes("glass",dDensity, 0.2f, 0.7f);
+        Attributes[(int)MaterialAttributes.Material.Wood] =
+                    new MaterialAttributes("wood",dDensity, 0.6f, 0.5f);
+        Attributes[(int)MaterialAttributes.Material.Flesh] =
+                    new MaterialAttributes("flesh",dDensity, 0.9f, 0.3f);
+        Attributes[(int)MaterialAttributes.Material.Plastic] =
+                    new MaterialAttributes("plastic",dDensity, 0.4f, 0.7f);
+        Attributes[(int)MaterialAttributes.Material.Rubber] =
+                    new MaterialAttributes("rubber",dDensity, 0.9f, 0.9f);
+        Attributes[(int)MaterialAttributes.Material.Light] =
+                    new MaterialAttributes("light",dDensity, dFriction, dRestitution);
+        Attributes[(int)MaterialAttributes.Material.Avatar] =
+                    new MaterialAttributes("avatar",3.5f, 0.2f, 0f);
+
+        Attributes[(int)MaterialAttributes.Material.Stone + (int)MaterialAttributes.Material.NumberOfTypes] =
+                    new MaterialAttributes("stonePhysical",dDensity, 0.8f, 0.4f);
+        Attributes[(int)MaterialAttributes.Material.Metal + (int)MaterialAttributes.Material.NumberOfTypes] =
+                    new MaterialAttributes("metalPhysical",dDensity, 0.3f, 0.4f);
+        Attributes[(int)MaterialAttributes.Material.Glass + (int)MaterialAttributes.Material.NumberOfTypes] =
+                    new MaterialAttributes("glassPhysical",dDensity, 0.2f, 0.7f);
+        Attributes[(int)MaterialAttributes.Material.Wood + (int)MaterialAttributes.Material.NumberOfTypes] =
+                    new MaterialAttributes("woodPhysical",dDensity, 0.6f, 0.5f);
+        Attributes[(int)MaterialAttributes.Material.Flesh + (int)MaterialAttributes.Material.NumberOfTypes] =
+                    new MaterialAttributes("fleshPhysical",dDensity, 0.9f, 0.3f);
+        Attributes[(int)MaterialAttributes.Material.Plastic + (int)MaterialAttributes.Material.NumberOfTypes] =
+                    new MaterialAttributes("plasticPhysical",dDensity, 0.4f, 0.7f);
+        Attributes[(int)MaterialAttributes.Material.Rubber + (int)MaterialAttributes.Material.NumberOfTypes] =
+                    new MaterialAttributes("rubberPhysical",dDensity, 0.9f, 0.9f);
+        Attributes[(int)MaterialAttributes.Material.Light + (int)MaterialAttributes.Material.NumberOfTypes] =
+                    new MaterialAttributes("lightPhysical",dDensity, dFriction, dRestitution);
+        Attributes[(int)MaterialAttributes.Material.Avatar + (int)MaterialAttributes.Material.NumberOfTypes] =
+                    new MaterialAttributes("avatarPhysical",3.5f, 0.2f, 0f);
+    }
+
+    // Under the [BulletSim] section, one can change the individual material
+    //    attribute values. The format of the configuration parameter is:
+    //        <materialName><Attribute>["Physical"] = floatValue
+    //    For instance:
+    //        [BulletSim]
+    //             StoneFriction = 0.2
+    //             FleshRestitutionPhysical = 0.8
+    // Materials can have different parameters for their static and
+    //    physical instantiations. When setting the non-physical value,
+    //    both values are changed. Setting the physical value only changes
+    //    the physical value.
+    public static void InitializefromParameters(IConfig pConfig)
+    {
+        foreach (KeyValuePair<string, MaterialAttributes.Material> kvp in MaterialMap)
+        {
+            string matName = kvp.Key;
+            foreach (string attribName in MaterialAttributes.MaterialAttribs)
+            {
+                string paramName = matName + attribName;
+                if (pConfig.Contains(paramName))
+                {
+                    float paramValue = pConfig.GetFloat(paramName);
+                    SetAttributeValue((int)kvp.Value, attribName, paramValue);
+                    // set the physical value also
+                    SetAttributeValue((int)kvp.Value + (int)MaterialAttributes.Material.NumberOfTypes, attribName, paramValue);
+                }
+                paramName += "Physical";
+                if (pConfig.Contains(paramName))
+                {
+                    float paramValue = pConfig.GetFloat(paramName);
+                    SetAttributeValue((int)kvp.Value + (int)MaterialAttributes.Material.NumberOfTypes, attribName, paramValue);
+                }
+            }
+        }
+    }
+
+    // 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;
+        }
+    }
+
+    // Given a material type, return a structure of attributes.
+    public static MaterialAttributes GetAttributes(MaterialAttributes.Material type, bool isPhysical)
+    {
+        int ind = (int)type;
+        if (isPhysical) ind += (int)MaterialAttributes.Material.NumberOfTypes;
+        return Attributes[ind];
+    }
+}
+}
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs b/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs
index bc6e4c4..9501e2d 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs
@@ -1,104 +1,486 @@
-using System;

-using System.Collections.Generic;

-using System.Text;

-using OpenMetaverse;

-

-namespace OpenSim.Region.Physics.BulletSPlugin

-{

-public abstract class BSMotor

-{

-    public virtual void Reset() { }

-    public virtual void Zero() { }

-}

-// Can all the incremental stepping be replaced with motor classes?

-public class BSVMotor : BSMotor

-{

-    public Vector3 FrameOfReference { get; set; }

-    public Vector3 Offset { get; set; }

-

-    public float TimeScale { get; set; }

-    public float TargetValueDecayTimeScale { get; set; }

-    public Vector3 CurrentValueReductionTimescale { get; set; }

-    public float Efficiency { get; set; }

-

-    public Vector3 TargetValue { get; private set; }

-    public Vector3 CurrentValue { get; private set; }

-

-

-

-    BSVMotor(float timeScale, float decayTimeScale, Vector3 frictionTimeScale, float efficiency)

-    {

-        TimeScale = timeScale;

-        TargetValueDecayTimeScale = decayTimeScale;

-        CurrentValueReductionTimescale = frictionTimeScale;

-        Efficiency = efficiency;

-    }

-    public void SetCurrent(Vector3 current)

-    {

-        CurrentValue = current;

-    }

-    public void SetTarget(Vector3 target)

-    {

-        TargetValue = target;

-    }

-    public Vector3 Step(float timeStep)

-    {

-        if (CurrentValue.LengthSquared() > 0.001f)

-        {

-            // Vector3 origDir = Target;       // DEBUG

-            // Vector3 origVel = CurrentValue;   // DEBUG

-

-            // Add (desiredVelocity - currentAppliedVelocity) / howLongItShouldTakeToComplete

-            Vector3 addAmount = (TargetValue - CurrentValue)/(TargetValue) * timeStep;

-            CurrentValue += addAmount;

-

-            float decayFactor = (1.0f / TargetValueDecayTimeScale) * timeStep;

-            TargetValue *= (1f - decayFactor);

-

-            Vector3 frictionFactor = (Vector3.One / CurrentValueReductionTimescale) * timeStep;

-            CurrentValue *= (Vector3.One - frictionFactor);

-        }

-        else

-        {

-            // if what remains of direction is very small, zero it.

-            TargetValue = Vector3.Zero;

-            CurrentValue = Vector3.Zero;

-

-            // VDetailLog("{0},MoveLinear,zeroed", Prim.LocalID);

-        }

-        return CurrentValue;

-    }

-}

-

-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 float Target { get; private set; }

-    public float CurrentValue { get; private set; }

-

-    BSFMotor(float timeScale, float decayTimescale, float friction, float efficiency)

-    {

-    }

-    public void SetCurrent(float target)

-    {

-    }

-    public void SetTarget(float target)

-    {

-    }

-    public float Step(float timeStep)

-    {

-        return 0f;

-    }

-}

-public class BSPIDMotor : BSMotor

-{

-    // TODO: write and use this one

-    BSPIDMotor()

-    {

-    }

-}

-}

+/*
+ * 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.
+ *
+ */
+using System;
+using System.Collections.Generic;
+using System.Text;
+using OpenMetaverse;
+using OpenSim.Framework;
+
+namespace OpenSim.Region.Physics.BulletSPlugin
+{
+public abstract class BSMotor
+{
+    // Timescales and other things can be turned off by setting them to 'infinite'.
+    public const float Infinite = 12345.6f;
+    public readonly static Vector3 InfiniteVector = new Vector3(BSMotor.Infinite, BSMotor.Infinite, BSMotor.Infinite);
+
+    public BSMotor(string useName)
+    {
+        UseName = useName;
+        PhysicsScene = null;
+        Enabled = true;
+    }
+    public virtual bool Enabled { get; set; }
+    public virtual void Reset() { }
+    public virtual void Zero() { }
+    public virtual void GenerateTestOutput(float timeStep) { }
+
+    // A name passed at motor creation for easily identifyable debugging messages.
+    public string UseName { get; private set; }
+
+    // Used only for outputting debug information. Might not be set so check for null.
+    public BSScene PhysicsScene { get; set; }
+    protected void MDetailLog(string msg, params Object[] parms)
+    {
+        if (PhysicsScene != null)
+        {
+            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.
+// 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.
+// 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,
+//    CurrentValue is X and TargetValue is Y. As the motor is stepped, new
+//    values of CurrentValue are returned that approach the TargetValue.
+// The feature of decaying TargetValue is so vehicles will eventually
+//    come to a stop rather than run forever. This can be disabled by
+//    setting TargetValueDecayTimescale to 'infinite'.
+// The change from CurrentValue to TargetValue is linear over TimeScale seconds.
+public class BSVMotor : BSMotor
+{
+    // public Vector3 FrameOfReference { get; set; }
+    // public Vector3 Offset { get; set; }
+
+    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; }
+
+    public virtual Vector3 TargetValue { get; protected set; }
+    public virtual Vector3 CurrentValue { get; protected set; }
+    public virtual Vector3 LastError { get; protected set; }
+
+    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)
+        : base(useName)
+    {
+        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) 
+        : this(useName)
+    {
+        TimeScale = timeScale;
+        TargetValueDecayTimeScale = decayTimeScale;
+        FrictionTimescale = frictionTimeScale;
+        Efficiency = efficiency;
+        CurrentValue = TargetValue = Vector3.Zero;
+    }
+    public void SetCurrent(Vector3 current)
+    {
+        CurrentValue = current;
+    }
+    public void SetTarget(Vector3 target)
+    {
+        TargetValue = target;
+    }
+    public override void Zero()
+    {
+        base.Zero();
+        CurrentValue = TargetValue = Vector3.Zero;
+    }
+
+    // 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;
+
+        Vector3 origTarget = TargetValue;       // DEBUG
+        Vector3 origCurrVal = CurrentValue;     // DEBUG
+
+        Vector3 correction = Vector3.Zero;
+        Vector3 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);
+            }
+
+            // 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);
+        }
+        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},currTgt={4},currCurr={5}",
+                        BSScene.DetailLogZero, UseName, origCurrVal, origTarget, TargetValue, 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 StepError(float timeStep, Vector3 error)
+    {
+        if (!Enabled) return Vector3.Zero;
+
+        Vector3 returnCorrection = Vector3.Zero;
+        if (!ErrorIsZero(error))
+        {
+            // correction =  error / secondsItShouldTakeToCorrect
+            Vector3 correctionAmount;
+            if (TimeScale == 0f || TimeScale == BSMotor.Infinite)
+                correctionAmount = error * timeStep;
+            else
+                correctionAmount = error / TimeScale * timeStep;
+
+            returnCorrection = correctionAmount;
+            MDetailLog("{0},  BSVMotor.Step,nonZero,{1},timeStep={2},timeScale={3},err={4},corr={5}",
+                                    BSScene.DetailLogZero, UseName, timeStep, TimeScale, error, correctionAmount);
+        }
+        return returnCorrection;
+    }
+
+    // The user sets all the parameters and calls this which outputs values until error is zero.
+    public override void GenerateTestOutput(float timeStep)
+    {
+        // 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}",
+                                BSScene.DetailLogZero, UseName,
+                                TimeScale, TargetValueDecayTimeScale, FrictionTimescale, Efficiency, 
+                                CurrentValue, TargetValue);
+
+        LastError = BSMotor.InfiniteVector;
+        while (maxOutput-- > 0 && !LastError.ApproxEquals(Vector3.Zero, ErrorZeroThreshold))
+        {
+            Vector3 lastStep = Step(timeStep);
+            MDetailLog("{0},BSVMotor.Test,{1},cur={2},tgt={3},lastError={4},lastStep={5}",
+                            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);
+    }
+}
+
+// ============================================================================
+// ============================================================================
+public class BSFMotor : BSMotor
+{
+    public virtual float TimeScale { get; set; }
+    public virtual float TargetValueDecayTimeScale { get; set; }
+    public virtual float FrictionTimescale { get; set; }
+    public virtual float Efficiency { get; set; }
+
+    public virtual float ErrorZeroThreshold { get; set; }
+
+    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 friction, float efficiency)
+        : base(useName)
+    {
+        TimeScale = TargetValueDecayTimeScale = BSMotor.Infinite;
+        Efficiency = 1f;
+        FrictionTimescale = BSMotor.Infinite;
+        CurrentValue = TargetValue = 0f;
+        ErrorZeroThreshold = 0.01f;
+    }
+    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)
+    {
+        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);
+            }
+
+            // The amount we can correct the error is reduced by the friction
+            float frictionFactor = 0f;
+            if (FrictionTimescale != BSMotor.Infinite)
+            {
+                // frictionFactor = (Vector3.One / FrictionTimescale) * timeStep;
+                // Individual friction components can be 'infinite' so compute each separately.
+                frictionFactor = 1f / FrictionTimescale;
+                frictionFactor *= timeStep;
+                CurrentValue *= (1f - frictionFactor);
+            }
+
+            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},frictTS={4},frictFact={5},tgt={6},curr={7}",
+                                BSScene.DetailLogZero, UseName,
+                                TargetValueDecayTimeScale, decayFactor, FrictionTimescale, frictionFactor,
+                                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},frictTS={5}>",
+            UseName, CurrentValue, TargetValue, LastError, TargetValueDecayTimeScale, FrictionTimescale);
+    }
+
+}
+
+// ============================================================================
+// ============================================================================
+// 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
+{
+    // Larger makes more overshoot, smaller means converge quicker. Range of 0.1 to 10.
+    public Vector3 proportionFactor { get; set; }
+    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;
+    public float EfficiencyLow = 4.0f;
+
+    // Running integration of the error
+    Vector3 RunningIntegration { get; set; }
+
+    public BSPIDVMotor(string useName)
+        : base(useName)
+    {
+        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;
+    }
+
+    public override void Zero()
+    {
+        base.Zero();
+    }
+
+    public override float Efficiency
+    {
+        get { return base.Efficiency; }
+        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);
+        }
+    }
+
+    // Advance the PID computation on this error.
+    public override Vector3 StepError(float timeStep, Vector3 error)
+    {
+        if (!Enabled) return Vector3.Zero;
+
+        // Add up the error so we can integrate over the accumulated errors
+        RunningIntegration += error * timeStep;
+
+        // A simple derivitive is the rate of change from the last error.
+        Vector3 derivitive = (error - LastError) * timeStep;
+        LastError = error;
+
+        // 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
new file mode 100755
index 0000000..385ed9e
--- /dev/null
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSParam.cs
@@ -0,0 +1,815 @@
+/*
+ * Copyright (c) Contributors, http://opensimulator.org/
+ * See CONTRIBUTORS.TXT for a full list of copyright holders.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyrightD
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the OpenSimulator Project nor the
+ *       names of its contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+using System;
+using System.Collections.Generic;
+using System.Text;
+
+using OpenSim.Region.Physics.Manager;
+
+using OpenMetaverse;
+using Nini.Config;
+
+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; }
+    public static float DeactivationTime { get; private set; }
+    public static float LinearSleepingThreshold { get; private set; }
+    public static float AngularSleepingThreshold { get; private set; }
+        public static float CcdMotionThreshold { get; private set; }
+        public static float CcdSweptSphereRadius { get; private set; }
+    public static float ContactProcessingThreshold { get; private set; }
+
+    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 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; }
+    public static float AvatarAlwaysRunFactor { get; private set; }
+    public static float AvatarDensity { get; private set; }
+    public static float AvatarRestitution { get; private set; }
+    public static float AvatarCapsuleWidth { get; private set; }
+    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; }
+
+    // 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 VehicleDebuggingEnabled { 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; }
+
+    // Linkset implementation parameters
+    public static float LinksetImplementation { 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; }
+    public static float LinkConstraintCFM { get; private set; }
+    public static float LinkConstraintSolverIterations { get; private set; }
+
+    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.
+    public const float MinGravityZ = -1f;
+    public const float MaxGravityZ = 28f;
+    public const float MinFriction = 0f;
+    public const float MaxFriction = 255f;
+    public const float MinDensity = 0.01f;
+    public const float MaxDensity = 22587f;
+    public const float MinRestitution = 0f;
+    public const float MaxRestitution = 1f;
+
+    // =====================================================================================
+    // =====================================================================================
+
+    // 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 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)
+        {
+            defaultValue = pDefault;
+            setter = pSetter;
+            getter = pGetter;
+            objectSet = pObjSetter;
+        }
+        /* Wish I could simplify using this definition but CLR doesn't store references so closure around delegates of references won't work
+        public ParameterDefn(string pName, string pDesc, T pDefault, ref T loc)
+            : base(pName, pDesc)
+        {
+            defaultValue = pDefault;
+            setter = (s, v) => { loc = v; };
+            getter = (s) => { return loc; };
+            objectSet = 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);
+        }
+    }
+
+    // List of all of the externally visible parameters.
+    // For each parameter, this table maps a text name to getter and setters.
+    // To add a new externally referencable/settable parameter, add the paramter storage
+    //    location somewhere in the program and make an entry in this table with the
+    //    getters and setters.
+    // It is easiest to find an existing definition and copy it.
+    //
+    // 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
+    //    -- 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
+    //    v = value (appropriate type)
+    private static ParameterDefnBase[] ParameterDefinitions =
+    {
+        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,
+            (s) => { return ShouldRemoveZeroWidthTriangles; },
+            (s,v) => { ShouldRemoveZeroWidthTriangles = v; } ),
+
+        new ParameterDefn<int>("CrossingFailuresBeforeOutOfBounds", "How forgiving we are about getting into adjactent regions",
+            5,
+            (s) => { return CrossingFailuresBeforeOutOfBounds; },
+            (s,v) => { CrossingFailuresBeforeOutOfBounds = v; } ),
+        new ParameterDefn<float>("UpdateVelocityChangeThreshold", "Change in updated velocity required before reporting change to simulator",
+            0.1f,
+            (s) => { return UpdateVelocityChangeThreshold; },
+            (s,v) => { UpdateVelocityChangeThreshold = v; } ),
+
+        new ParameterDefn<float>("MeshLevelOfDetail", "Level of detail to render meshes (32, 16, 8 or 4. 32=most detailed)",
+            32f,
+            (s) => { return MeshLOD; },
+            (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) => { return MeshMegaPrimThreshold; },
+            (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) => { return SculptLOD; },
+            (s,v) => { SculptLOD = v; } ),
+
+        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) => { return s.m_fixedTimeStep; },
+            (s,v) => { s.m_fixedTimeStep = v; } ),
+        new ParameterDefn<float>("NominalFrameRate", "The base frame rate we claim",
+            55f,
+            (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) => { return MinimumObjectMass; },
+            (s,v) => { MinimumObjectMass = v; } ),
+        new ParameterDefn<float>("MaxObjectMass", "Maximum object mass (10000.01)",
+            10000.01f,
+            (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,
+            (s) => { return DensityScaleFactor; },
+            (s,v) => { DensityScaleFactor = v; } ),
+
+        new ParameterDefn<float>("PID_D", "Derivitive factor for motion smoothing",
+            2200f,
+            (s) => { return (float)PID_D; },
+            (s,v) => { PID_D = v; } ),
+        new ParameterDefn<float>("PID_P", "Parameteric factor for motion smoothing",
+            900f,
+            (s) => { return (float)PID_P; },
+            (s,v) => { PID_P = v; } ),
+
+        new ParameterDefn<float>("DefaultFriction", "Friction factor used on new objects",
+            0.2f,
+            (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) => { return DefaultDensity; },
+            (s,v) => { DefaultDensity = v; s.UnmanagedParams[0].defaultDensity = v; } ),
+        new ParameterDefn<float>("DefaultRestitution", "Bouncyness of an object" ,
+            0f,
+            (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) => { 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) => { 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<float>("LinearDamping", "Factor to damp linear movement per second (0.0 - 1.0)",
+            0f,
+            (s) => { return LinearDamping; },
+            (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) => { return AngularDamping; },
+            (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) => { return DeactivationTime; },
+            (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) => { return LinearSleepingThreshold; },
+            (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) => { return AngularSleepingThreshold; },
+            (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,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,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,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,
+            (s) => { return TerrainImplementation; },
+            (s,v) => { TerrainImplementation = v; } ),
+        new ParameterDefn<int>("TerrainMeshMagnification", "Number of times the 256x256 heightmap is multiplied to create the terrain mesh" ,
+            2,
+            (s) => { return TerrainMeshMagnification; },
+            (s,v) => { TerrainMeshMagnification = v; } ),
+        new ParameterDefn<float>("TerrainFriction", "Factor to reduce movement against terrain surface" ,
+            0.3f,
+            (s) => { return TerrainFriction; },
+            (s,v) => { TerrainFriction = v;  /* TODO: set on real terrain */} ),
+        new ParameterDefn<float>("TerrainHitFraction", "Distance to measure hit collisions" ,
+            0.8f,
+            (s) => { return TerrainHitFraction; },
+            (s,v) => { TerrainHitFraction = v; /* TODO: set on real terrain */ } ),
+        new ParameterDefn<float>("TerrainRestitution", "Bouncyness" ,
+            0f,
+            (s) => { return TerrainRestitution; },
+            (s,v) => { TerrainRestitution = v;  /* TODO: set on real terrain */ } ),
+        new ParameterDefn<float>("TerrainContactProcessingThreshold", "Distance from terrain to stop processing collisions" ,
+            0.0f,
+            (s) => { return TerrainContactProcessingThreshold; },
+            (s,v) => { TerrainContactProcessingThreshold = v;  /* TODO: set on real terrain */ } ),
+        new ParameterDefn<float>("TerrainCollisionMargin", "Margin where collision checking starts" ,
+            0.08f,
+            (s) => { return TerrainCollisionMargin; },
+            (s,v) => { TerrainCollisionMargin = v;  /* TODO: set on real terrain */ } ),
+
+        new ParameterDefn<float>("AvatarFriction", "Factor to reduce movement against an avatar. Changed on avatar recreation.",
+            0.2f,
+            (s) => { return AvatarFriction; },
+            (s,v) => { AvatarFriction = v; } ),
+        new ParameterDefn<float>("AvatarStandingFriction", "Avatar friction when standing. Changed on avatar recreation.",
+            0.95f,
+            (s) => { return AvatarStandingFriction; },
+            (s,v) => { AvatarStandingFriction = v; } ),
+        new ParameterDefn<float>("AvatarAlwaysRunFactor", "Speed multiplier if avatar is set to always run",
+            1.3f,
+            (s) => { return AvatarAlwaysRunFactor; },
+            (s,v) => { AvatarAlwaysRunFactor = v; } ),
+        new ParameterDefn<float>("AvatarDensity", "Density of an avatar. Changed on avatar recreation.",
+            3.5f,
+            (s) => { return AvatarDensity; },
+            (s,v) => { AvatarDensity = v; } ),
+        new ParameterDefn<float>("AvatarRestitution", "Bouncyness. Changed on avatar recreation.",
+            0f,
+            (s) => { return AvatarRestitution; },
+            (s,v) => { AvatarRestitution = v; } ),
+        new ParameterDefn<float>("AvatarCapsuleWidth", "The distance between the sides of the avatar capsule",
+            0.6f,
+            (s) => { return AvatarCapsuleWidth; },
+            (s,v) => { AvatarCapsuleWidth = v; } ),
+        new ParameterDefn<float>("AvatarCapsuleDepth", "The distance between the front and back of the avatar capsule",
+            0.45f,
+            (s) => { return AvatarCapsuleDepth; },
+            (s,v) => { AvatarCapsuleDepth = v; } ),
+        new ParameterDefn<float>("AvatarCapsuleHeight", "Default height of space around avatar",
+            1.5f,
+            (s) => { return AvatarCapsuleHeight; },
+            (s,v) => { AvatarCapsuleHeight = v; } ),
+            new ParameterDefn<float>("AvatarContactProcessingThreshold", "Distance from capsule to check for collisions",
+            0.1f,
+            (s) => { return AvatarContactProcessingThreshold; },
+            (s,v) => { AvatarContactProcessingThreshold = v; } ),
+            new ParameterDefn<float>("AvatarBelowGroundUpCorrectionMeters", "Meters to move avatar up if it seems to be below ground",
+            1.0f,
+            (s) => { return AvatarBelowGroundUpCorrectionMeters; },
+            (s,v) => { AvatarBelowGroundUpCorrectionMeters = v; } ),
+            new ParameterDefn<float>("AvatarStepHeight", "Height of a step obstacle to consider step correction",
+            0.3f,
+            (s) => { return AvatarStepHeight; },
+            (s,v) => { AvatarStepHeight = v; } ),
+            new ParameterDefn<float>("AvatarStepApproachFactor", "Factor to control angle of approach to step (0=straight on)",
+            0.6f,
+            (s) => { return AvatarStepApproachFactor; },
+            (s,v) => { AvatarStepApproachFactor = v; } ),
+            new ParameterDefn<float>("AvatarStepForceFactor", "Controls the amount of force up applied to step up onto a step",
+            2.0f,
+            (s) => { return AvatarStepForceFactor; },
+            (s,v) => { AvatarStepForceFactor = v; } ),
+
+        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,
+            (s) => { return VehicleAngularDamping; },
+            (s,v) => { VehicleAngularDamping = v; } ),
+        new ParameterDefn<Vector3>("VehicleLinearFactor", "Fraction of physical linear changes applied to vehicle (<0,0,0> to <1,1,1>)",
+            new Vector3(1f, 1f, 1f),
+            (s) => { return VehicleLinearFactor; },
+            (s,v) => { VehicleLinearFactor = v; } ),
+        new ParameterDefn<Vector3>("VehicleAngularFactor", "Fraction of physical angular changes applied to vehicle (<0,0,0> to <1,1,1>)",
+            new Vector3(1f, 1f, 1f),
+            (s) => { return VehicleAngularFactor; },
+            (s,v) => { VehicleAngularFactor = v; } ),
+        new ParameterDefn<float>("VehicleFriction", "Friction of vehicle on the ground (0.0 - 1.0)",
+            0.0f,
+            (s) => { return VehicleFriction; },
+            (s,v) => { VehicleFriction = v; } ),
+        new ParameterDefn<float>("VehicleRestitution", "Bouncyness factor for vehicles (0.0 - 1.0)",
+            0.0f,
+            (s) => { return VehicleRestitution; },
+            (s,v) => { VehicleRestitution = v; } ),
+        new ParameterDefn<float>("VehicleGroundGravityFudge", "Factor to multiply gravity if a ground vehicle is probably on the ground (0.0 - 1.0)",
+            0.2f,
+            (s) => { return VehicleGroundGravityFudge; },
+            (s,v) => { VehicleGroundGravityFudge = v; } ),
+        new ParameterDefn<float>("VehicleAngularBankingTimescaleFudge", "Factor to multiple angular banking timescale. Tune to increase realism.",
+            60.0f,
+            (s) => { return VehicleAngularBankingTimescaleFudge; },
+            (s,v) => { VehicleAngularBankingTimescaleFudge = v; } ),
+        new ParameterDefn<bool>("VehicleDebuggingEnable", "Turn on/off vehicle debugging",
+            false,
+            (s) => { return VehicleDebuggingEnabled; },
+            (s,v) => { VehicleDebuggingEnabled = v; } ),
+
+            new ParameterDefn<float>("MaxPersistantManifoldPoolSize", "Number of manifolds pooled (0 means default of 4096)",
+            0f,
+            (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) => { 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) => { 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,
+            (s) => { return CSHullMaxDepthSplit; },
+            (s,v) => { CSHullMaxDepthSplit = v; } ),
+            new ParameterDefn<int>("CSHullMaxDepthSplitForSimpleShapes", "CS impl: max depth setting for simple prim shapes",
+            2,
+            (s) => { return CSHullMaxDepthSplitForSimpleShapes; },
+            (s,v) => { CSHullMaxDepthSplitForSimpleShapes = v; } ),
+            new ParameterDefn<float>("CSHullConcavityThresholdPercent", "CS impl: concavity threshold percent (0-20)",
+            5f,
+            (s) => { return CSHullConcavityThresholdPercent; },
+            (s,v) => { CSHullConcavityThresholdPercent = v; } ),
+            new ParameterDefn<float>("CSHullVolumeConservationThresholdPercent", "percent volume conservation to collapse hulls (0-30)",
+            5f,
+            (s) => { return CSHullVolumeConservationThresholdPercent; },
+            (s,v) => { CSHullVolumeConservationThresholdPercent = v; } ),
+            new ParameterDefn<int>("CSHullMaxVertices", "CS impl: maximum number of vertices in output hulls. Keep < 50.",
+            32,
+            (s) => { return CSHullMaxVertices; },
+            (s,v) => { CSHullMaxVertices = v; } ),
+            new ParameterDefn<float>("CSHullMaxSkinWidth", "CS impl: skin width to apply to output hulls.",
+            0,
+            (s) => { return CSHullMaxSkinWidth; },
+            (s,v) => { CSHullMaxSkinWidth = v; } ),
+
+            new ParameterDefn<float>("LinksetImplementation", "Type of linkset implementation (0=Constraint, 1=Compound, 2=Manual)",
+            (float)BSLinkset.LinksetImplementation.Compound,
+            (s) => { return LinksetImplementation; },
+            (s,v) => { LinksetImplementation = v; } ),
+            new ParameterDefn<bool>("LinkConstraintUseFrameOffset", "For linksets built with constraints, enable frame offsetFor linksets built with constraints, enable frame offset.",
+            false,
+            (s) => { return LinkConstraintUseFrameOffset; },
+            (s,v) => { LinkConstraintUseFrameOffset = v; } ),
+            new ParameterDefn<bool>("LinkConstraintEnableTransMotor", "Whether to enable translational motor on linkset constraints",
+            true,
+            (s) => { return LinkConstraintEnableTransMotor; },
+            (s,v) => { LinkConstraintEnableTransMotor = v; } ),
+            new ParameterDefn<float>("LinkConstraintTransMotorMaxVel", "Maximum velocity to be applied by translational motor in linkset constraints",
+            5.0f,
+            (s) => { return LinkConstraintTransMotorMaxVel; },
+            (s,v) => { LinkConstraintTransMotorMaxVel = v; } ),
+            new ParameterDefn<float>("LinkConstraintTransMotorMaxForce", "Maximum force to be applied by translational motor in linkset constraints",
+            0.1f,
+            (s) => { return LinkConstraintTransMotorMaxForce; },
+            (s,v) => { LinkConstraintTransMotorMaxForce = v; } ),
+            new ParameterDefn<float>("LinkConstraintCFM", "Amount constraint can be violated. 0=no violation, 1=infinite. Default=0.1",
+            0.1f,
+            (s) => { return LinkConstraintCFM; },
+            (s,v) => { LinkConstraintCFM = v; } ),
+            new ParameterDefn<float>("LinkConstraintERP", "Amount constraint is corrected each tick. 0=none, 1=all. Default = 0.2",
+            0.1f,
+            (s) => { return LinkConstraintERP; },
+            (s,v) => { LinkConstraintERP = v; } ),
+            new ParameterDefn<float>("LinkConstraintSolverIterations", "Number of solver iterations when computing constraint. (0 = Bullet default)",
+            40,
+            (s) => { return LinkConstraintSolverIterations; },
+            (s,v) => { LinkConstraintSolverIterations = v; } ),
+
+        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) => { return 0f; },
+            (s,v) => { BSParam.ResetBroadphasePoolTainted(s, v); } ),
+        new ParameterDefn<float>("ResetConstraintSolver", "Setting this is any value resets the constraint solver",
+            0f,
+            (s) => { return 0f; },
+            (s,v) => { BSParam.ResetConstraintSolverTainted(s, v); } ),
+    };
+
+    // Convert a boolean to our numeric true and false values
+    public static float NumericBool(bool b)
+    {
+        return (b ? ConfigurationParameters.numericTrue : ConfigurationParameters.numericFalse);
+    }
+
+    // Convert numeric true and false values to a boolean
+    public static bool BoolNumeric(float b)
+    {
+        return (b == ConfigurationParameters.numericTrue ? true : false);
+    }
+
+    // Search through the parameter definitions and return the matching
+    //    ParameterDefn structure.
+    // Case does not matter as names are compared after converting to lower case.
+    // Returns 'false' if the parameter is not found.
+    internal static bool TryGetParameter(string paramName, out ParameterDefnBase defn)
+    {
+        bool ret = false;
+        ParameterDefnBase foundDefn = null;
+        string pName = paramName.ToLower();
+
+        foreach (ParameterDefnBase parm in ParameterDefinitions)
+        {
+            if (pName == parm.name.ToLower())
+            {
+                foundDefn = parm;
+                ret = true;
+                break;
+            }
+        }
+        defn = foundDefn;
+        return ret;
+    }
+
+    // Pass through the settable parameters and set the default values
+    internal static void SetParameterDefaultValues(BSScene physicsScene)
+    {
+        foreach (ParameterDefnBase parm in ParameterDefinitions)
+        {
+            parm.AssignDefault(physicsScene);
+        }
+    }
+
+    // Get user set values out of the ini file.
+    internal static void SetParameterConfigurationValues(BSScene physicsScene, IConfig cfg)
+    {
+        foreach (ParameterDefnBase parm in ParameterDefinitions)
+        {
+            parm.SetValue(physicsScene, cfg.GetString(parm.name, parm.GetValue(physicsScene)));
+        }
+    }
+
+    internal static PhysParameterEntry[] SettableParameters = new PhysParameterEntry[1];
+
+    // This creates an array in the correct format for returning the list of
+    //    parameters. This is used by the 'list' option of the 'physics' command.
+    internal static void BuildParameterTable()
+    {
+        if (SettableParameters.Length < ParameterDefinitions.Length)
+        {
+            List<PhysParameterEntry> entries = new List<PhysParameterEntry>();
+            for (int ii = 0; ii < ParameterDefinitions.Length; ii++)
+            {
+                ParameterDefnBase pd = ParameterDefinitions[ii];
+                entries.Add(new PhysParameterEntry(pd.name, pd.desc));
+            }
+
+            // 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 f6a890e..6bb88c7 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
@@ -45,6 +45,26 @@ namespace OpenSim.Region.Physics.BulletSPlugin
  *      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.
  */
+
+/*
+ * As of 20121221, the following are the call sequences (going down) for different script physical functions:
+ * llApplyImpulse       llApplyRotImpulse           llSetTorque             llSetForce
+ * 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              
+ */
+
+// 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()
@@ -55,15 +75,40 @@ public abstract class BSPhysObject : PhysicsActor
         PhysicsScene = parentScene;
         LocalID = localID;
         PhysObjectName = name;
+        Name = name;    // PhysicsActor also has the name of the object. Someday consolidate.
         TypeName = typeName;
 
-        Linkset = BSLinkset.Factory(PhysicsScene, this);
-        LastAssetBuildFailed = false;
+        // Initialize variables kept in base.
+        GravModifier = 1.0f;
+        Gravity = new OMV.Vector3(0f, 0f, BSParam.Gravity);
+
+        // We don't have any physical representation yet.
+        PhysBody = new BulletBody(localID);
+        PhysShape = new BulletShape();
+
+        PrimAssetState = PrimAssetCondition.Unknown;
+
+        // Default material type. Also sets Friction, Restitution and Density.
+        SetMaterial((int)MaterialAttributes.Material.Wood);
 
         CollisionCollection = new CollisionEventUpdate();
+        CollisionsLastTick = CollisionCollection;
         SubscribedEventsMs = 0;
         CollidingStep = 0;
         CollidingGroundStep = 0;
+        CollisionAccumulation = 0;
+        ColliderIsMoving = false;
+        CollisionScore = 0;
+
+        // All axis free.
+        LockedAxis = LockedAxisFree;
+    }
+
+    // Tell the object to clean up.
+    public virtual void Destroy()
+    {
+        UnRegisterAllPreStepActions();
+        UnRegisterAllPostStepActions();
     }
 
     public BSScene PhysicsScene { get; protected set; }
@@ -71,13 +116,15 @@ public abstract class BSPhysObject : PhysicsActor
     public string PhysObjectName { get; protected set; }
     public string TypeName { get; protected set; }
 
-    public BSLinkset Linkset { get; set; }
 
     // Return the object mass without calculating it or having side effects
     public abstract float RawMass { get; }
     // Set the raw mass but also update physical mass properties (inertia, ...)
-    public abstract void UpdatePhysicalMassProperties(float mass);
+    // '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; }
 
@@ -86,12 +133,17 @@ public abstract class BSPhysObject : PhysicsActor
     // Reference to the physical shape (btCollisionShape) of this object
     public BulletShape 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
@@ -105,29 +157,46 @@ public abstract class BSPhysObject : PhysicsActor
     public EntityProperties CurrentEntityProperties { get; set; }
     public EntityProperties LastEntityProperties { get; set; }
 
-    public abstract OMV.Vector3 Scale { 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);
 
-    // Tell the object to clean up.
-    public abstract void Destroy();
-
     public abstract OMV.Vector3 RawPosition { get; set; }
     public abstract OMV.Vector3 ForcePosition { get; set; }
 
     public abstract OMV.Quaternion RawOrientation { get; set; }
     public abstract OMV.Quaternion ForceOrientation { get; set; }
 
+    public abstract OMV.Vector3 RawVelocity { get; set; }
     public abstract OMV.Vector3 ForceVelocity { get; set; }
 
     public abstract OMV.Vector3 ForceRotationalVelocity { get; set; }
@@ -136,6 +205,32 @@ public abstract class BSPhysObject : PhysicsActor
 
     public virtual bool ForceBodyShapeRebuild(bool inTaintTime) { return false; }
 
+    // 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).
+    public OMV.Vector3? UserSetCenterOfMass { get; set; }
+
+    public OMV.Vector3 LockedAxis { get; set; } // zero means locked. one means free.
+    public readonly OMV.Vector3 LockedAxisFree = new OMV.Vector3(1f, 1f, 1f);  // All axis are free
+
     #region Collisions
 
     // Requested number of milliseconds between collision events. Zero means disabled.
@@ -146,38 +241,82 @@ public abstract class BSPhysObject : PhysicsActor
     protected long CollidingStep { get; set; }
     // The simulation step that last had a collision with the ground
     protected long CollidingGroundStep { get; set; }
+    // The simulation step that last collided with an object
+    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)
+    protected bool ColliderIsMoving;
+
+    // Count of collisions for this object
+    protected long CollisionAccumulation { get; set; }
+
+    public override bool IsColliding {
+        get { return (CollidingStep == PhysicsScene.SimulationStep); }
+        set {
+            if (value)
+                CollidingStep = PhysicsScene.SimulationStep;
+            else
+                CollidingStep = 0;
+            }
+    }
+    public override bool CollidingGround {
+        get { return (CollidingGroundStep == PhysicsScene.SimulationStep); }
+        set
+        {
+            if (value)
+                CollidingGroundStep = PhysicsScene.SimulationStep;
+            else
+                CollidingGroundStep = 0;
+        }
+    }
+    public override bool CollidingObj {
+        get { return (CollidingObjectStep == PhysicsScene.SimulationStep); }
+        set { 
+            if (value)
+                CollidingObjectStep = PhysicsScene.SimulationStep;
+            else
+                CollidingObjectStep = 0;
+        }
+    }
 
     // The collisions that have been collected this tick
     protected CollisionEventUpdate CollisionCollection;
+    // Remember collisions from last tick for fancy collision based actions
+    //     (like a BSCharacter walking up stairs).
+    protected CollisionEventUpdate CollisionsLastTick;
 
     // 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)
     {
         bool ret = false;
 
-        // The following lines make IsColliding() and IsCollidingGround() work
+        // The following lines make IsColliding(), CollidingGround() and CollidingObj work
         CollidingStep = PhysicsScene.SimulationStep;
         if (collidingWith <= PhysicsScene.TerrainManager.HighestTerrainID)
         {
             CollidingGroundStep = PhysicsScene.SimulationStep;
         }
-
-        // prims in the same linkset cannot collide with each other
-        if (collidee != null && (this.Linkset.LinksetID == collidee.Linkset.LinksetID))
+        else
         {
-            return ret;
+            CollidingObjectStep = PhysicsScene.SimulationStep;
         }
 
-        // if someone has subscribed for collision events....
+        CollisionAccumulation++;
+
+        // For movement tests, remember if we are colliding with an object that is moving.
+        ColliderIsMoving = collidee != null ? (collidee.RawVelocity != OMV.Vector3.Zero) : false;
+
+        // 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;
         }
@@ -191,8 +330,9 @@ 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 && CollisionsLastTick.Count != 0);
 
         // throttle the collisions to the number of milliseconds specified in the subscription
         if (force || (PhysicsScene.SimulationNowTime >= NextCollisionOkTime))
@@ -207,11 +347,16 @@ 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);
 
-            // The collisionCollection structure is passed around in the simulator.
+            // Remember the collisions from this tick for some collision specific processing.
+            CollisionsLastTick = 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, 
+            //    a race condition is created for the other users of this instance.
             CollisionCollection = new CollisionEventUpdate();
         }
         return ret;
@@ -229,7 +374,8 @@ public abstract class BSPhysObject : PhysicsActor
 
             PhysicsScene.TaintedObject(TypeName+".SubscribeEvents", delegate()
             {
-                CurrentCollisionFlags = BulletSimAPI.AddToCollisionFlags2(PhysBody.ptr, CollisionFlags.BS_SUBSCRIBE_COLLISION_EVENTS);
+                if (PhysBody.HasPhysicalBody)
+                    CurrentCollisionFlags = PhysicsScene.PE.AddToCollisionFlags(PhysBody, CollisionFlags.BS_SUBSCRIBE_COLLISION_EVENTS);
             });
         }
         else
@@ -243,21 +389,187 @@ public abstract class BSPhysObject : PhysicsActor
         SubscribedEventsMs = 0;
         PhysicsScene.TaintedObject(TypeName+".UnSubscribeEvents", delegate()
         {
-            CurrentCollisionFlags = BulletSimAPI.RemoveFromCollisionFlags2(PhysBody.ptr, CollisionFlags.BS_SUBSCRIBE_COLLISION_EVENTS);
+            // 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);
         });
     }
     // 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 = PhysicsScene.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> RegisteredPrestepActions = new Dictionary<string, BSScene.PreStepAction>();
+    private Dictionary<string, BSScene.PostStepAction> RegisteredPoststepActions = new Dictionary<string, BSScene.PostStepAction>();
+    protected void RegisterPreStepAction(string op, uint id, BSScene.PreStepAction actn)
+    {
+        string identifier = op + "-" + id.ToString();
+
+        lock (RegisteredPrestepActions)
+        {
+            // Clean out any existing action
+            UnRegisterPreStepAction(op, id);
+            RegisteredPrestepActions[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.
+    // Returns 'true' if an action was actually removed
+    protected bool UnRegisterPreStepAction(string op, uint id)
+    {
+        string identifier = op + "-" + id.ToString();
+        bool removed = false;
+        lock (RegisteredPrestepActions)
+        {
+            if (RegisteredPrestepActions.ContainsKey(identifier))
+            {
+                PhysicsScene.BeforeStep -= RegisteredPrestepActions[identifier];
+                RegisteredPrestepActions.Remove(identifier);
+                removed = true;
+            }
+        }
+        DetailLog("{0},BSPhysObject.UnRegisterPreStepAction,id={1},removed={2}", LocalID, identifier, removed);
+        return removed;
+    }
+
+    protected void UnRegisterAllPreStepActions()
+    {
+        lock (RegisteredPrestepActions)
+        {
+            foreach (KeyValuePair<string, BSScene.PreStepAction> kvp in RegisteredPrestepActions)
+            {
+                PhysicsScene.BeforeStep -= kvp.Value;
+            }
+            RegisteredPrestepActions.Clear();
+        }
+        DetailLog("{0},BSPhysObject.UnRegisterAllPreStepActions,", LocalID);
+    }
+    
+    protected void RegisterPostStepAction(string op, uint id, BSScene.PostStepAction actn)
+    {
+        string identifier = op + "-" + id.ToString();
+
+        lock (RegisteredPoststepActions)
+        {
+            // Clean out any existing action
+            UnRegisterPostStepAction(op, id);
+            RegisteredPoststepActions[identifier] = actn;
+            PhysicsScene.AfterStep += actn;
+        }
+        DetailLog("{0},BSPhysObject.RegisterPostStepAction,id={1}", LocalID, identifier);
+    }
+
+    // Unregister a pre step action. Safe to call if the action has not been registered.
+    // Returns 'true' if an action was actually removed.
+    protected bool UnRegisterPostStepAction(string op, uint id)
+    {
+        string identifier = op + "-" + id.ToString();
+        bool removed = false;
+        lock (RegisteredPoststepActions)
+        {
+            if (RegisteredPoststepActions.ContainsKey(identifier))
+            {
+                PhysicsScene.AfterStep -= RegisteredPoststepActions[identifier];
+                RegisteredPoststepActions.Remove(identifier);
+                removed = true;
+            }
+        }
+        DetailLog("{0},BSPhysObject.UnRegisterPostStepAction,id={1},removed={2}", LocalID, identifier, removed);
+        return removed;
+    }
+
+    protected void UnRegisterAllPostStepActions()
+    {
+        lock (RegisteredPoststepActions)
+        {
+            foreach (KeyValuePair<string, BSScene.PostStepAction> kvp in RegisteredPoststepActions)
+            {
+                PhysicsScene.AfterStep -= kvp.Value;
+            }
+            RegisteredPoststepActions.Clear();
+        }
+        DetailLog("{0},BSPhysObject.UnRegisterAllPostStepActions,", LocalID);
+    }
+
+    // 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)
+    {
+        PreUpdatePropertyAction actions = OnPreUpdateProperty;
+        if (actions != null)
+            actions(ref entprop);
+    }
+
+    private Dictionary<string, PreUpdatePropertyAction> RegisteredPreUpdatePropertyActions = new Dictionary<string, PreUpdatePropertyAction>();
+    public void RegisterPreUpdatePropertyAction(string identifier, PreUpdatePropertyAction actn)
+    {
+        lock (RegisteredPreUpdatePropertyActions)
+        {
+            // Clean out any existing action
+            UnRegisterPreUpdatePropertyAction(identifier);
+            RegisteredPreUpdatePropertyActions[identifier] = actn;
+            OnPreUpdateProperty += actn;
+        }
+        DetailLog("{0},BSPhysObject.RegisterPreUpdatePropertyAction,id={1}", LocalID, identifier);
+    }
+    public bool UnRegisterPreUpdatePropertyAction(string identifier)
+    {
+        bool removed = false;
+        lock (RegisteredPreUpdatePropertyActions)
+        {
+            if (RegisteredPreUpdatePropertyActions.ContainsKey(identifier))
+            {
+                OnPreUpdateProperty -= RegisteredPreUpdatePropertyActions[identifier];
+                RegisteredPreUpdatePropertyActions.Remove(identifier);
+                removed = true;
+            }
+        }
+        DetailLog("{0},BSPhysObject.UnRegisterPreUpdatePropertyAction,id={1},removed={2}", LocalID, identifier, removed);
+        return removed;
+    }
+    public void UnRegisterAllPreUpdatePropertyActions()
+    {
+        lock (RegisteredPreUpdatePropertyActions)
+        {
+            foreach (KeyValuePair<string, PreUpdatePropertyAction> kvp in RegisteredPreUpdatePropertyActions)
+            {
+                OnPreUpdateProperty -= kvp.Value;
+            }
+            RegisteredPreUpdatePropertyActions.Clear();
+        }
+        DetailLog("{0},BSPhysObject.UnRegisterAllPreUpdatePropertyAction,", LocalID);
+    }
+
+    #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);
     }
+
 }
 }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPlugin.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPlugin.cs
index 20f5180..9442854 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPlugin.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPlugin.cs
@@ -59,13 +59,7 @@ public class BSPlugin : IPhysicsPlugin
     {
         if (_mScene == null)
         {
-            if (Util.IsWindows())
-                Util.LoadArchSpecificWindowsDll("BulletSim.dll");
-            // If not Windows, loading is performed by the
-            // Mono loader as specified in
-            // "bin/Physics/OpenSim.Region.Physics.BulletSPlugin.dll.config".
-
-            _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 2b3fa25..6a5461a 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
@@ -39,51 +39,49 @@ 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]";
 
     // _size is what the user passed. Scale is what we pass to the physics engine with the mesh.
-    // Often Scale is unity because the meshmerizer will apply _size when creating the mesh.
     private OMV.Vector3 _size;  // the multiplier for each mesh dimension as passed by the user
 
     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; }
 
+    public BSDynamics VehicleController { get; private set; }
+
+    private BSVMotor _targetMotor;
     private OMV.Vector3 _PIDTarget;
-    private bool _usePID;
     private float _PIDTau;
-    private bool _useHoverPID;
+
+    private BSFMotor _hoverMotor;
     private float _PIDHoverHeight;
     private PIDHoverType _PIDHoverType;
-    private float _PIDHoverTao;
+    private float _PIDHoverTau;
 
     public BSPrim(uint localID, String primName, BSScene parent_scene, OMV.Vector3 pos, OMV.Vector3 size,
                        OMV.Quaternion rotation, PrimitiveBaseShape pbs, bool pisPhysical)
@@ -93,31 +91,29 @@ public sealed class BSPrim : BSPhysObject
         _physicsActorType = (int)ActorTypes.Prim;
         _position = pos;
         _size = size;
-        Scale = size;   // the scale will be set by CreateGeom depending on object type
+        Scale = size;   // prims are the size the user wants them to be (different for BSCharactes).
         _orientation = rotation;
-        _buoyancy = 1f;
+        _buoyancy = 0f;
         _velocity = OMV.Vector3.Zero;
         _rotationalVelocity = OMV.Vector3.Zero;
         BaseShape = pbs;
         _isPhysical = pisPhysical;
         _isVolumeDetect = false;
-        _friction = PhysicsScene.Params.defaultFriction;  // TODO: compute based on object material
-        _density = PhysicsScene.Params.defaultDensity;    // TODO: compute based on object material
-        _restitution = PhysicsScene.Params.defaultRestitution;
-        _vehicle = new BSDynamics(PhysicsScene, this);            // add vehicleness
-        _mass = CalculateMass();
 
-        // No body or shape yet
-        PhysBody = new BulletBody(LocalID, IntPtr.Zero);
-        PhysShape = new BulletShape(IntPtr.Zero);
+        VehicleController = new BSDynamics(PhysicsScene, this);            // add vehicleness
+
+        _mass = CalculateMass();
 
         DetailLog("{0},BSPrim.constructor,call", LocalID);
         // do the actual object creation at taint time
         PhysicsScene.TaintedObject("BSPrim.create", delegate()
         {
+            // Make sure the object is being created with some sanity.
+            ExtremeSanityCheck(true /* inTaintTime */);
+
             CreateGeomAndObject(true);
 
-            CurrentCollisionFlags = BulletSimAPI.GetCollisionFlags2(PhysBody.ptr);
+            CurrentCollisionFlags = PhysicsScene.PE.GetCollisionFlags(PhysBody);
         });
     }
 
@@ -125,15 +121,7 @@ public sealed class BSPrim : BSPhysObject
     public override void Destroy()
     {
         // m_log.DebugFormat("{0}: Destroy, id={1}", LogHeader, LocalID);
-
-        // 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);
+        base.Destroy();
 
         // Undo any vehicle properties
         this.VehicleType = (int)Vehicle.TYPE_NONE;
@@ -142,8 +130,10 @@ public sealed class BSPrim : BSPhysObject
         {
             DetailLog("{0},BSPrim.Destroy,taint,", LocalID);
             // If there are physical body and shape, release my use of same.
-            PhysicsScene.Shapes.DereferenceBody(PhysBody, true, null);
-            PhysicsScene.Shapes.DereferenceShape(PhysShape, true, null);
+            PhysicsScene.Shapes.DereferenceBody(PhysBody, null);
+            PhysBody.Clear();
+            PhysicsScene.Shapes.DereferenceShape(PhysShape, null);
+            PhysShape.Clear();
         });
     }
 
@@ -157,31 +147,37 @@ public sealed class BSPrim : BSPhysObject
             // We presume the scale and size are the same. If scale must be changed for
             //     the physical shape, that is done when the geometry is built.
             _size = value;
+            Scale = _size;
             ForceBodyShapeRebuild(false);
         }
     }
-    // Scale is what we set in the physics engine. It is different than 'size' in that
-    //     'size' can be encorporated into the mesh. In that case, the scale is <1,1,1>.
-    public override OMV.Vector3 Scale { get; set; }
 
     public override PrimitiveBaseShape Shape {
         set {
             BaseShape = value;
+            PrimAssetState = PrimAssetCondition.Unknown;
             ForceBodyShapeRebuild(false);
         }
     }
-    // Whatever the linkset wants is what I want.
+    // 'unknown' says to choose the best type
     public override BSPhysicsShapeType PreferredPhysicalShape
-        { get { return Linkset.PreferredPhysicalShape(this); } }
+        { get { return BSPhysicsShapeType.SHAPE_UNKNOWN; } }
 
     public override bool ForceBodyShapeRebuild(bool inTaintTime)
     {
-        LastAssetBuildFailed = false;
-        PhysicsScene.TaintedObject(inTaintTime, "BSPrim.ForceBodyShapeRebuild", delegate()
+        if (inTaintTime)
         {
             _mass = CalculateMass();   // changing the shape changes the mass
             CreateGeomAndObject(true);
-        });
+        }
+        else
+        {
+            PhysicsScene.TaintedObject("BSPrim.ForceBodyShapeRebuild", delegate()
+            {
+                _mass = CalculateMass();   // changing the shape changes the mass
+                CreateGeomAndObject(true);
+            });
+        }
         return true;
     }
     public override bool Grabbed {
@@ -189,46 +185,44 @@ public sealed class BSPrim : BSPhysObject
         }
     }
     public override bool Selected {
-        set {
-            _isSelected = value;
-            PhysicsScene.TaintedObject("BSPrim.setSelected", delegate()
+        set
+        {
+            if (value != _isSelected)
             {
-                DetailLog("{0},BSPrim.selected,taint,selected={1}", LocalID, _isSelected);
-                SetObjectDynamic(false);
-            });
+                _isSelected = value;
+                PhysicsScene.TaintedObject("BSPrim.setSelected", delegate()
+                {
+                    DetailLog("{0},BSPrim.selected,taint,selected={1}", LocalID, _isSelected);
+                    SetObjectDynamic(false);
+                });
+            }
         }
     }
-    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.
@@ -244,7 +238,8 @@ public sealed class BSPrim : BSPhysObject
         // Zero some other properties in the physics engine
         PhysicsScene.TaintedObject(inTaintTime, "BSPrim.ZeroMotion", delegate()
         {
-            BulletSimAPI.ClearAllForces2(PhysBody.ptr);
+            if (PhysBody.HasPhysicalBody)
+                PhysicsScene.PE.ClearAllForces(PhysBody);
         });
     }
     public override void ZeroAngularMotion(bool inTaintTime)
@@ -253,16 +248,107 @@ public sealed class BSPrim : BSPhysObject
         // Zero some other properties in the physics engine
         PhysicsScene.TaintedObject(inTaintTime, "BSPrim.ZeroMotion", delegate()
         {
-            BulletSimAPI.SetInterpolationAngularVelocity2(PhysBody.ptr, OMV.Vector3.Zero);
-            BulletSimAPI.SetAngularVelocity2(PhysBody.ptr, OMV.Vector3.Zero);
+            // DetailLog("{0},BSPrim.ZeroAngularMotion,call,rotVel={1}", LocalID, _rotationalVelocity);
+            if (PhysBody.HasPhysicalBody)
+            {
+                PhysicsScene.PE.SetInterpolationAngularVelocity(PhysBody, _rotationalVelocity);
+                PhysicsScene.PE.SetAngularVelocity(PhysBody, _rotationalVelocity);
+            }
         });
     }
 
+    bool TryExperimentalLockAxisCode = false;
+    BSConstraint LockAxisConstraint = null;
     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 = new OMV.Vector3(1f, 1f, 1f);
+        if (axis.X != 1) locking.X = 0f;
+        if (axis.Y != 1) locking.Y = 0f;
+        if (axis.Z != 1) locking.Z = 0f;
+        LockedAxis = locking;
+
+        if (TryExperimentalLockAxisCode && LockedAxis != LockedAxisFree)
+        {
+            // 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
+
+            PhysicsScene.TaintedObject("BSPrim.LockAngularMotion", delegate()
+            {
+                CleanUpLockAxisPhysicals(true /* inTaintTime */);
+
+                BSConstraint6Dof axisConstrainer = new BSConstraint6Dof(PhysicsScene.World, PhysBody, 
+                                    OMV.Vector3.Zero, OMV.Quaternion.Inverse(RawOrientation),
+                                    true /* useLinearReferenceFrameB */, true /* disableCollisionsBetweenLinkedBodies */);
+                LockAxisConstraint = axisConstrainer;
+                PhysicsScene.Constraints.AddConstraint(LockAxisConstraint);
+
+                // The constraint is tied to the world and oriented to the prim.
+
+                // Free to move linearly
+                OMV.Vector3 linearLow = OMV.Vector3.Zero;
+                OMV.Vector3 linearHigh = PhysicsScene.TerrainManager.DefaultRegionSize;
+                axisConstrainer.SetLinearLimits(linearLow, linearHigh);
+
+                // Angular with some axis locked
+                float f2PI = (float)Math.PI * 2f;
+                OMV.Vector3 angularLow = new OMV.Vector3(-f2PI, -f2PI, -f2PI);
+                OMV.Vector3 angularHigh = new OMV.Vector3(f2PI, f2PI, f2PI);
+                if (LockedAxis.X != 1f)
+                {
+                    angularLow.X = 0f;
+                    angularHigh.X = 0f;
+                }
+                if (LockedAxis.Y != 1f)
+                {
+                    angularLow.Y = 0f;
+                    angularHigh.Y = 0f;
+                }
+                if (LockedAxis.Z != 1f)
+                {
+                    angularLow.Z = 0f;
+                    angularHigh.Z = 0f;
+                }
+                axisConstrainer.SetAngularLimits(angularLow, angularHigh);
+
+                DetailLog("{0},BSPrim.LockAngularMotion,create,linLow={1},linHi={2},angLow={3},angHi={4}",
+                                            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(RawMass);
+            });
+        }
+        else
+        {
+            // Everything seems unlocked
+            CleanUpLockAxisPhysicals(false /* inTaintTime */);
+        }
+
         return;
     }
+    // Get rid of any constraint built for LockAxis
+    // Most often the constraint is removed when the constraint collection is cleaned for this prim.
+    private void CleanUpLockAxisPhysicals(bool inTaintTime)
+    {
+        if (LockAxisConstraint != null)
+        {
+            PhysicsScene.TaintedObject(inTaintTime, "BSPrim.CleanUpLockAxisPhysicals", delegate()
+            {
+                if (LockAxisConstraint != null)
+                {
+                    PhysicsScene.Constraints.RemoveAndDestroyConstraint(LockAxisConstraint);
+                    LockAxisConstraint = null;
+                    DetailLog("{0},BSPrim.CleanUpLockAxisPhysicals,destroyingConstraint", LocalID);
+                }
+            });
+        }
+    }
 
     public override OMV.Vector3 RawPosition
     {
@@ -271,41 +357,41 @@ public sealed class BSPrim : BSPhysObject
     }
     public override OMV.Vector3 Position {
         get {
-            // child prims move around based on their parent. Need to get the latest location
-            if (!Linkset.IsRoot(this))
-                _position = Linkset.Position(this);
-
             // don't do the GetObjectPosition for root elements because this function is called a zillion times.
-            // _position = BulletSimAPI.GetObjectPosition2(PhysicsScene.World.ptr, BSBody.ptr);
+            // _position = ForcePosition;
             return _position;
         }
         set {
             // If the position must be forced into the physics engine, use ForcePosition.
+            // All positions are given in world positions.
             if (_position == value)
             {
+                DetailLog("{0},BSPrim.setPosition,call,positionNotChanging,pos={1},orient={2}", LocalID, _position, _orientation);
                 return;
             }
             _position = value;
-            // TODO: what does it mean to set the position of a child prim?? Rebuild the constraint?
             PositionSanityCheck(false);
+
             PhysicsScene.TaintedObject("BSPrim.setPosition", delegate()
             {
-                // DetailLog("{0},BSPrim.SetPosition,taint,pos={1},orient={2}", LocalID, _position, _orientation);
-                BulletSimAPI.SetTranslation2(PhysBody.ptr, _position, _orientation);
-                ActivateIfPhysical(false);
+                DetailLog("{0},BSPrim.SetPosition,taint,pos={1},orient={2}", LocalID, _position, _orientation);
+                ForcePosition = _position;
             });
         }
     }
+
     public override OMV.Vector3 ForcePosition {
         get {
-            _position = BulletSimAPI.GetPosition2(PhysBody.ptr);
+            _position = PhysicsScene.PE.GetPosition(PhysBody);
             return _position;
         }
         set {
             _position = value;
-            // PositionSanityCheck();   // Don't do this! Causes a loop and caller should know better.
-            BulletSimAPI.SetTranslation2(PhysBody.ptr, _position, _orientation);
-            ActivateIfPhysical(false);
+            if (PhysBody.HasPhysicalBody)
+            {
+                PhysicsScene.PE.SetTranslation(PhysBody, _position, _orientation);
+                ActivateIfPhysical(false);
+            }
         }
     }
 
@@ -316,119 +402,231 @@ public sealed class BSPrim : BSPhysObject
     {
         bool ret = false;
 
-        float terrainHeight = PhysicsScene.TerrainManager.GetTerrainHeightAtXYZ(_position);
+        // We don't care where non-physical items are placed
+        if (!IsPhysicallyActive)
+            return ret;
+
+        if (!PhysicsScene.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
+            //     the time, we just ignore the position.
+            return ret;
+        }
+
+        float terrainHeight = PhysicsScene.TerrainManager.GetTerrainHeightAtXYZ(RawPosition);
         OMV.Vector3 upForce = OMV.Vector3.Zero;
-        if (Position.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 - Position.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.GetWaterLevelAtXYZ(_position);
+            float waterHeight = PhysicsScene.TerrainManager.GetWaterLevelAtXYZ(_position);
             // TODO: a floating motor so object will bob in the water
-            if (Math.Abs(Position.Z - waterHeight) > 0.1f)
+            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 - Position.Z) * 1f;
+                upForce.Z = (waterHeight - RawPosition.Z) * 1f;
+
+                // 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);
                 ret = true;
             }
         }
 
-        // TODO: check for out of bounds
+        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;
 
-        // The above code computes a force to apply to correct any out-of-bounds problems. Apply same.
-        if (ret)
+        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)
         {
-            PhysicsScene.TaintedObject(inTaintTime, "BSPrim.PositionSanityCheck:belowTerrain", delegate()
-            {
-                // Apply upforce and overcome gravity.
-                ForceVelocity = ForceVelocity + upForce - PhysicsScene.DefaultGravity;
-            });
+            _position = new OMV.Vector3(10, 10, 50);
+            ZeroMotion(inTaintTime);
+            ret = true;
+        }
+        if (_velocity.LengthSquared() > BSParam.MaxLinearVelocity)
+        {
+            _velocity = Util.ClampV(_velocity, BSParam.MaxLinearVelocity);
+            ret = true;
         }
+        if (_rotationalVelocity.LengthSquared() > BSParam.MaxAngularVelocitySquared)
+        {
+            _rotationalVelocity = Util.ClampV(_rotationalVelocity, BSParam.MaxAngularVelocity);
+            ret = true;
+        }
+
         return ret;
     }
 
     // Return the effective mass of the object.
-    // If there are multiple items in the linkset, add them together for the root
+        // The definition of this call is to return the mass of the prim.
+        // If the simulator cares about the mass of the linkset, it will sum it itself.
     public override float Mass
     {
-        get
-        {
-            return Linkset.LinksetMass;
-            // 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 { 
         get { return _mass; }
     }
     // Set the physical mass to the passed mass.
     // Note that this does not change _mass!
-    public override void UpdatePhysicalMassProperties(float physMass)
+    public override void UpdatePhysicalMassProperties(float physMass, bool inWorld)
     {
-        if (IsStatic)
+        if (PhysBody.HasPhysicalBody && PhysShape.HasPhysicalShape)
         {
-            Inertia = OMV.Vector3.Zero;
-            BulletSimAPI.SetMassProps2(PhysBody.ptr, 0f, Inertia);
-            BulletSimAPI.UpdateInertiaTensor2(PhysBody.ptr);
+            if (IsStatic)
+            {
+                PhysicsScene.PE.SetGravity(PhysBody, PhysicsScene.DefaultGravity);
+                Inertia = OMV.Vector3.Zero;
+                PhysicsScene.PE.SetMassProps(PhysBody, 0f, Inertia);
+                PhysicsScene.PE.UpdateInertiaTensor(PhysBody);
+            }
+            else
+            {
+                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);
+                }
+
+                // The computation of mass props requires gravity to be set on the object.
+                Gravity = ComputeGravity(Buoyancy);
+                PhysicsScene.PE.SetGravity(PhysBody, Gravity);
+
+                Inertia = PhysicsScene.PE.CalculateLocalInertia(PhysShape, physMass);
+                PhysicsScene.PE.SetMassProps(PhysBody, physMass, Inertia);
+                PhysicsScene.PE.UpdateInertiaTensor(PhysBody);
+
+                DetailLog("{0},BSPrim.UpdateMassProperties,mass={1},localInertia={2},grav={3},inWorld={4}",
+                                            LocalID, physMass, Inertia, Gravity, inWorld);
+
+                if (inWorld)
+                {
+                    AddObjectToPhysicalWorld();
+                }
+            }
         }
-        else
+    }
+
+    // Return what gravity should be set to this very moment
+    public OMV.Vector3 ComputeGravity(float buoyancy)
+    {
+        OMV.Vector3 ret = PhysicsScene.DefaultGravity;
+
+        if (!IsStatic)
         {
-            Inertia = BulletSimAPI.CalculateLocalInertia2(PhysShape.ptr, physMass);
-            BulletSimAPI.SetMassProps2(PhysBody.ptr, physMass, Inertia);
-            BulletSimAPI.UpdateInertiaTensor2(PhysBody.ptr);
-            // center of mass is at the zero of the object
-            // DEBUG DEBUG BulletSimAPI.SetCenterOfMassByPosRot2(PhysBody.ptr, ForcePosition, ForceOrientation);
-            DetailLog("{0},BSPrim.UpdateMassProperties,mass={1},localInertia={2}", LocalID, physMass, Inertia);
+            ret *= (1f - buoyancy);
+            ret *= GravModifier;
         }
+
+        return ret;
     }
 
     // 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; }
         set {
             _force = value;
-            PhysicsScene.TaintedObject("BSPrim.setForce", delegate()
+            if (_force != OMV.Vector3.Zero)
             {
-                // DetailLog("{0},BSPrim.setForce,taint,force={1}", LocalID, _force);
-                BulletSimAPI.SetObjectForce2(PhysBody.ptr, _force);
-            });
+                // 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)
+                    {
+                        if (!IsPhysicallyActive || _force == OMV.Vector3.Zero)
+                        {
+                            UnRegisterPreStepAction("BSPrim.setForce", LocalID);
+                            return;
+                        }
+
+                        DetailLog("{0},BSPrim.setForce,preStep,force={1}", LocalID, _force);
+                        if (PhysBody.HasPhysicalBody)
+                        {
+                            PhysicsScene.PE.ApplyCentralForce(PhysBody, _force);
+                            ActivateIfPhysical(false);
+                        }
+                    }
+                );
+            }
+            else
+            {
+                UnRegisterPreStepAction("BSPrim.setForce", LocalID);
+            }
         }
     }
 
     public override int VehicleType {
         get {
-            return (int)_vehicle.Type;   // if we are a vehicle, return that type
+            return (int)VehicleController.Type;   // if we are a vehicle, return that type
         }
         set {
             Vehicle type = (Vehicle)value;
 
-            // Tell the scene about the vehicle so it will get processing each frame.
-            PhysicsScene.VehicleInSceneTypeChanged(this, type);
-
             PhysicsScene.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);
+                VehicleController.ProcessTypeChange(type);
                 ActivateIfPhysical(false);
+
+                // If an active vehicle, register the vehicle code to be called before each step
+                if (VehicleController.Type == Vehicle.TYPE_NONE)
+                {
+                    UnRegisterPreStepAction("BSPrim.Vehicle", LocalID);
+                    UnRegisterPostStepAction("BSPrim.Vehicle", LocalID);
+                }
+                else
+                {
+                    RegisterPreStepAction("BSPrim.Vehicle", LocalID, VehicleController.Step);
+                    RegisterPostStepAction("BSPrim.Vehicle", LocalID, VehicleController.PostStep);
+                }
             });
         }
     }
@@ -436,7 +634,7 @@ public sealed class BSPrim : BSPhysObject
     {
         PhysicsScene.TaintedObject("BSPrim.VehicleFloatParam", delegate()
         {
-            _vehicle.ProcessFloatVehicleParam((Vehicle)param, value);
+            VehicleController.ProcessFloatVehicleParam((Vehicle)param, value);
             ActivateIfPhysical(false);
         });
     }
@@ -444,7 +642,7 @@ public sealed class BSPrim : BSPhysObject
     {
         PhysicsScene.TaintedObject("BSPrim.VehicleVectorParam", delegate()
         {
-            _vehicle.ProcessVectorVehicleParam((Vehicle)param, value);
+            VehicleController.ProcessVectorVehicleParam((Vehicle)param, value);
             ActivateIfPhysical(false);
         });
     }
@@ -452,7 +650,7 @@ public sealed class BSPrim : BSPhysObject
     {
         PhysicsScene.TaintedObject("BSPrim.VehicleRotationParam", delegate()
         {
-            _vehicle.ProcessRotationVehicleParam((Vehicle)param, rotation);
+            VehicleController.ProcessRotationVehicleParam((Vehicle)param, rotation);
             ActivateIfPhysical(false);
         });
     }
@@ -460,27 +658,10 @@ public sealed class BSPrim : BSPhysObject
     {
         PhysicsScene.TaintedObject("BSPrim.VehicleFlags", delegate()
         {
-            _vehicle.ProcessVehicleFlags(param, remove);
+            VehicleController.ProcessVehicleFlags(param, remove);
         });
     }
 
-    // Called each simulation step to advance vehicle characteristics.
-    // Called from Scene when doing simulation step so we're in taint processing time.
-    public override void StepVehicle(float timeStep)
-    {
-        if (IsPhysical && _vehicle.IsActive)
-        {
-            _vehicle.Step(timeStep);
-            /* // TEST TEST DEBUG DEBUG -- trying to reduce the extra action of Bullet simulation step
-            PhysicsScene.PostTaintObject("BSPrim.StepVehicles", LocalID, delegate()
-            {
-                // This resets the interpolation values and recomputes the tensor variables
-                BulletSimAPI.SetCenterOfMassByPosRot2(BSBody.ptr, ForcePosition, ForceOrientation);
-            });
-             */
-        }
-    }
-
     // Allows the detection of collisions with inherently non-physical prims. see llVolumeDetect for more
     public override void SetVolumeDetect(int param) {
         bool newValue = (param != 0);
@@ -495,6 +676,81 @@ public sealed class BSPrim : BSPhysObject
         }
         return;
     }
+    public override void SetMaterial(int material)
+    {
+        base.SetMaterial(material);
+        PhysicsScene.TaintedObject("BSPrim.SetMaterial", delegate()
+        {
+            UpdatePhysicalParameters();
+        });
+    }
+    public override float Friction
+    {
+        get { return base.Friction; }
+        set
+        {
+            if (base.Friction != value)
+            {
+                base.Friction = value;
+                PhysicsScene.TaintedObject("BSPrim.setFriction", delegate()
+                {
+                    UpdatePhysicalParameters();
+                });
+            }
+        }
+    }
+    public override float Restitution
+    {
+        get { return base.Restitution; }
+        set
+        {
+            if (base.Restitution != value)
+            {
+                base.Restitution = value;
+                PhysicsScene.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;
+                PhysicsScene.TaintedObject("BSPrim.setDensity", delegate()
+                {
+                    UpdatePhysicalParameters();
+                });
+            }
+        }
+    }
+    public override float GravModifier
+    {
+        get { return base.GravModifier; }
+        set
+        {
+            if (base.GravModifier != value)
+            {
+                base.GravModifier = value;
+                PhysicsScene.TaintedObject("BSPrim.setGravityModifier", delegate()
+                {
+                    UpdatePhysicalParameters();
+                });
+            }
+        }
+    }
+    public override OMV.Vector3 RawVelocity
+    {
+        get { return _velocity; }
+        set { _velocity = value; }
+    }
     public override OMV.Vector3 Velocity {
         get { return _velocity; }
         set {
@@ -502,30 +758,53 @@ public sealed class BSPrim : BSPhysObject
             PhysicsScene.TaintedObject("BSPrim.setVelocity", delegate()
             {
                 // DetailLog("{0},BSPrim.SetVelocity,taint,vel={1}", LocalID, _velocity);
-                BulletSimAPI.SetLinearVelocity2(PhysBody.ptr, _velocity);
+                ForceVelocity = _velocity;
             });
         }
     }
     public override OMV.Vector3 ForceVelocity {
         get { return _velocity; }
         set {
-            _velocity = value;
-            BulletSimAPI.SetLinearVelocity2(PhysBody.ptr, _velocity);
+            PhysicsScene.AssertInTaintTime("BSPrim.ForceVelocity");
+
+            _velocity = Util.ClampV(value, BSParam.MaxLinearVelocity);
+            if (PhysBody.HasPhysicalBody)
+            {
+                DetailLog("{0},BSPrim.ForceVelocity,taint,vel={1}", LocalID, _velocity);
+                PhysicsScene.PE.SetLinearVelocity(PhysBody, _velocity);
+                ActivateIfPhysical(false);
+            }
         }
     }
     public override OMV.Vector3 Torque {
         get { return _torque; }
         set {
             _torque = value;
-            AddAngularForce(_torque, false, false);
+            if (_torque != OMV.Vector3.Zero)
+            {
+                // 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 (!IsPhysicallyActive || _torque == OMV.Vector3.Zero)
+                        {
+                            UnRegisterPreStepAction("BSPrim.setTorque", LocalID);
+                            return;
+                        }
+
+                        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;
-        }
-    }
     public override OMV.Vector3 Acceleration {
         get { return _acceleration; }
         set { _acceleration = value; }
@@ -537,23 +816,16 @@ public sealed class BSPrim : BSPhysObject
     }
     public override OMV.Quaternion Orientation {
         get {
-            // Children move around because tied to parent. Get a fresh value.
-            if (!Linkset.IsRoot(this))
-            {
-                _orientation = Linkset.Orientation(this);
-            }
             return _orientation;
         }
         set {
             if (_orientation == value)
                 return;
             _orientation = value;
-            // TODO: what does it mean if a child in a linkset changes its orientation? Rebuild the constraint?
+
             PhysicsScene.TaintedObject("BSPrim.setOrientation", delegate()
             {
-                // _position = BulletSimAPI.GetObjectPosition2(PhysicsScene.World.ptr, BSBody.ptr);
-                // DetailLog("{0},BSPrim.setOrientation,taint,pos={1},orient={2}", LocalID, _position, _orientation);
-                BulletSimAPI.SetTranslation2(PhysBody.ptr, _position, _orientation);
+                ForceOrientation = _orientation;
             });
         }
     }
@@ -562,13 +834,14 @@ public sealed class BSPrim : BSPhysObject
     {
         get
         {
-            _orientation = BulletSimAPI.GetOrientation2(PhysBody.ptr);
+            _orientation = PhysicsScene.PE.GetOrientation(PhysBody);
             return _orientation;
         }
         set
         {
             _orientation = value;
-            BulletSimAPI.SetTranslation2(PhysBody.ptr, _position, _orientation);
+            if (PhysBody.HasPhysicalBody)
+                PhysicsScene.PE.SetTranslation(PhysBody, _position, _orientation);
         }
     }
     public override int PhysicsActorType {
@@ -583,10 +856,11 @@ public sealed class BSPrim : BSPhysObject
                 _isPhysical = value;
                 PhysicsScene.TaintedObject("BSPrim.setIsPhysical", delegate()
                 {
-                    // DetailLog("{0},setIsPhysical,taint,isPhys={1}", LocalID, _isPhysical);
+                    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);
+
                 });
             }
         }
@@ -604,6 +878,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)
@@ -618,19 +898,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).
-        BulletSimAPI.RemoveObjectFromWorld2(PhysicsScene.World.ptr, PhysBody.ptr);
+        PhysicsScene.PE.RemoveObjectFromWorld(PhysicsScene.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();
+        VehicleController.Refresh();
 
         // Arrange for collision events if the simulator wants them
         EnableCollisions(SubscribedEvents());
@@ -638,25 +923,13 @@ public sealed class BSPrim : BSPhysObject
         // Make solid or not (do things bounce off or pass through this object).
         MakeSolid(IsSolid);
 
-        BulletSimAPI.AddObjectToWorld2(PhysicsScene.World.ptr, PhysBody.ptr);
+        AddObjectToPhysicalWorld();
 
         // Rebuild its shape
-        BulletSimAPI.UpdateSingleAabb2(PhysicsScene.World.ptr, PhysBody.ptr);
-
-        // Collision filter can be set only when the object is in the world
-        if (PhysBody.collisionFilter != 0 || PhysBody.collisionMask != 0)
-        {
-            BulletSimAPI.SetCollisionFilterMask2(PhysBody.ptr, (uint)PhysBody.collisionFilter, (uint)PhysBody.collisionMask);
-        }
-
-        // 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);
+        PhysicsScene.PE.UpdateSingleAabb(PhysicsScene.World, PhysBody);
 
-        DetailLog("{0},BSPrim.UpdatePhysicalParameters,taintExit,static={1},solid={2},mass={3},collide={4},cf={5:X},body={6},shape={7}",
-                        LocalID, IsStatic, IsSolid, _mass, SubscribedEvents(), CurrentCollisionFlags, PhysBody, PhysShape);
+        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);
     }
 
     // "Making dynamic" means changing to and from static.
@@ -664,79 +937,78 @@ 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 = BulletSimAPI.AddToCollisionFlags2(PhysBody.ptr, CollisionFlags.CF_STATIC_OBJECT);
+            CurrentCollisionFlags = PhysicsScene.PE.AddToCollisionFlags(PhysBody, CollisionFlags.CF_STATIC_OBJECT);
             // Stop all movement
             ZeroMotion(true);
-            // Center of mass is at the center of the object
-            // DEBUG DEBUG BulletSimAPI.SetCenterOfMassByPosRot2(Linkset.LinksetRoot.PhysBody.ptr, _position, _orientation);
+
+            // Set various physical properties so other object interact properly
+            PhysicsScene.PE.SetFriction(PhysBody, Friction);
+            PhysicsScene.PE.SetRestitution(PhysBody, Restitution);
+            PhysicsScene.PE.SetContactProcessingThreshold(PhysBody, BSParam.ContactProcessingThreshold);
+
             // Mass is zero which disables a bunch of physics stuff in Bullet
-            UpdatePhysicalMassProperties(0f);
+            UpdatePhysicalMassProperties(0f, false);
             // Set collision detection parameters
-            if (PhysicsScene.Params.ccdMotionThreshold > 0f)
+            if (BSParam.CcdMotionThreshold > 0f)
             {
-                BulletSimAPI.SetCcdMotionThreshold2(PhysBody.ptr, PhysicsScene.Params.ccdMotionThreshold);
-                BulletSimAPI.SetCcdSweptSphereRadius2(PhysBody.ptr, PhysicsScene.Params.ccdSweptSphereRadius);
+                PhysicsScene.PE.SetCcdMotionThreshold(PhysBody, BSParam.CcdMotionThreshold);
+                PhysicsScene.PE.SetCcdSweptSphereRadius(PhysBody, BSParam.CcdSweptSphereRadius);
             }
-            // There can be special things needed for implementing linksets
-            Linkset.MakeStatic(this);
+
             // The activation state is 'disabled' so Bullet will not try to act on it.
-            BulletSimAPI.ForceActivationState2(PhysBody.ptr, ActivationState.DISABLE_SIMULATION);
+            // PhysicsScene.PE.ForceActivationState(PhysBody, ActivationState.DISABLE_SIMULATION);
             // Start it out sleeping and physical actions could wake it up.
-            // BulletSimAPI.ForceActivationState2(BSBody.ptr, ActivationState.ISLAND_SLEEPING);
+            PhysicsScene.PE.ForceActivationState(PhysBody, ActivationState.ISLAND_SLEEPING);
 
-            PhysBody.collisionFilter = CollisionFilterGroups.StaticObjectFilter;
-            PhysBody.collisionMask = CollisionFilterGroups.StaticObjectMask;
+            // This collides like a static object
+            PhysBody.collisionType = CollisionType.Static;
         }
         else
         {
             // Not a Bullet static object
-            CurrentCollisionFlags = BulletSimAPI.RemoveFromCollisionFlags2(PhysBody.ptr, CollisionFlags.CF_STATIC_OBJECT);
+            CurrentCollisionFlags = PhysicsScene.PE.RemoveFromCollisionFlags(PhysBody, CollisionFlags.CF_STATIC_OBJECT);
 
-            // Set various physical properties so internal dynamic properties will get computed correctly as they are set
-            BulletSimAPI.SetFriction2(PhysBody.ptr, PhysicsScene.Params.defaultFriction);
-            BulletSimAPI.SetRestitution2(PhysBody.ptr, PhysicsScene.Params.defaultRestitution);
+            // Set various physical properties so other object interact properly
+            PhysicsScene.PE.SetFriction(PhysBody, Friction);
+            PhysicsScene.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
-            // BulletSimAPI.ClearAllForces2(BSBody.ptr);
+            // PhysicsScene.PE.ClearAllForces(BSBody);
 
             // For good measure, make sure the transform is set through to the motion state
-            BulletSimAPI.SetTranslation2(PhysBody.ptr, _position, _orientation);
-
-            // Center of mass is at the center of the object
-            // DEBUG DEBUG BulletSimAPI.SetCenterOfMassByPosRot2(Linkset.LinksetRoot.PhysBody.ptr, _position, _orientation);
+            ForcePosition = _position;
+            ForceVelocity = _velocity;
+            ForceRotationalVelocity = _rotationalVelocity;
 
             // A dynamic object has mass
-            UpdatePhysicalMassProperties(RawMass);
+            UpdatePhysicalMassProperties(RawMass, false);
 
             // Set collision detection parameters
-            if (PhysicsScene.Params.ccdMotionThreshold > 0f)
+            if (BSParam.CcdMotionThreshold > 0f)
             {
-                BulletSimAPI.SetCcdMotionThreshold2(PhysBody.ptr, PhysicsScene.Params.ccdMotionThreshold);
-                BulletSimAPI.SetCcdSweptSphereRadius2(PhysBody.ptr, PhysicsScene.Params.ccdSweptSphereRadius);
+                PhysicsScene.PE.SetCcdMotionThreshold(PhysBody, BSParam.CcdMotionThreshold);
+                PhysicsScene.PE.SetCcdSweptSphereRadius(PhysBody, BSParam.CcdSweptSphereRadius);
             }
 
             // Various values for simulation limits
-            BulletSimAPI.SetDamping2(PhysBody.ptr, PhysicsScene.Params.linearDamping, PhysicsScene.Params.angularDamping);
-            BulletSimAPI.SetDeactivationTime2(PhysBody.ptr, PhysicsScene.Params.deactivationTime);
-            BulletSimAPI.SetSleepingThresholds2(PhysBody.ptr, PhysicsScene.Params.linearSleepingThreshold, PhysicsScene.Params.angularSleepingThreshold);
-            BulletSimAPI.SetContactProcessingThreshold2(PhysBody.ptr, PhysicsScene.Params.contactProcessingThreshold);
+            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);
 
-            // There might be special things needed for implementing linksets.
-            Linkset.MakeDynamic(this);
+            // 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.
-            BulletSimAPI.ForceActivationState2(PhysBody.ptr, ActivationState.ACTIVE_TAG);
-            // BulletSimAPI.Activate2(BSBody.ptr, true);
-
-            PhysBody.collisionFilter = CollisionFilterGroups.ObjectFilter;
-            PhysBody.collisionMask = CollisionFilterGroups.ObjectMask;
+            PhysicsScene.PE.ForceActivationState(PhysBody, ActivationState.ACTIVE_TAG);
         }
     }
 
@@ -746,7 +1018,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)BulletSimAPI.GetBodyType2(PhysBody.ptr);
+        CollisionObjectTypes bodyType = (CollisionObjectTypes)PhysicsScene.PE.GetBodyType(PhysBody);
         if (makeSolid)
         {
             // Verify the previous code created the correct shape for this type of thing.
@@ -754,7 +1026,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 = BulletSimAPI.RemoveFromCollisionFlags2(PhysBody.ptr, CollisionFlags.CF_NO_CONTACT_RESPONSE);
+            CurrentCollisionFlags = PhysicsScene.PE.RemoveFromCollisionFlags(PhysBody, CollisionFlags.CF_NO_CONTACT_RESPONSE);
         }
         else
         {
@@ -762,9 +1034,10 @@ 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 = BulletSimAPI.AddToCollisionFlags2(PhysBody.ptr, CollisionFlags.CF_NO_CONTACT_RESPONSE);
-            PhysBody.collisionFilter = CollisionFilterGroups.VolumeDetectFilter;
-            PhysBody.collisionMask = CollisionFilterGroups.VolumeDetectMask;
+            CurrentCollisionFlags = PhysicsScene.PE.AddToCollisionFlags(PhysBody, CollisionFlags.CF_NO_CONTACT_RESPONSE);
+
+            // Change collision info from a static object to a ghosty collision object
+            PhysBody.collisionType = CollisionType.VolumeDetect;
         }
     }
 
@@ -773,8 +1046,8 @@ public sealed class BSPrim : BSPhysObject
     // Called in taint-time!!
     private void ActivateIfPhysical(bool forceIt)
     {
-        if (IsPhysical)
-            BulletSimAPI.Activate2(PhysBody.ptr, 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.
@@ -782,11 +1055,27 @@ public sealed class BSPrim : BSPhysObject
     {
         if (wantsCollisionEvents)
         {
-            CurrentCollisionFlags = BulletSimAPI.AddToCollisionFlags2(PhysBody.ptr, CollisionFlags.BS_SUBSCRIBE_COLLISION_EVENTS);
+            CurrentCollisionFlags = PhysicsScene.PE.AddToCollisionFlags(PhysBody, CollisionFlags.BS_SUBSCRIBE_COLLISION_EVENTS);
+        }
+        else
+        {
+            CurrentCollisionFlags = PhysicsScene.PE.RemoveFromCollisionFlags(PhysBody, CollisionFlags.BS_SUBSCRIBE_COLLISION_EVENTS);
+        }
+    }
+
+    // Add me to the physical world.
+    // Object MUST NOT already be in the world.
+    // This routine exists because some assorted properties get mangled by adding to the world.
+    internal void AddObjectToPhysicalWorld()
+    {
+        if (PhysBody.HasPhysicalBody)
+        {
+            PhysicsScene.PE.AddObjectToWorld(PhysicsScene.World, PhysBody);
         }
         else
         {
-            CurrentCollisionFlags = BulletSimAPI.RemoveFromCollisionFlags2(PhysBody.ptr, CollisionFlags.BS_SUBSCRIBE_COLLISION_EVENTS);
+            m_log.ErrorFormat("{0} Attempt to add physical object without body. id={1}", LogHeader, LocalID);
+            DetailLog("{0},BSPrim.AddObjectToPhysicalWorld,addObjectWithoutBody,cType={1}", LocalID, PhysBody.collisionType);
         }
     }
 
@@ -805,18 +1094,6 @@ public sealed class BSPrim : BSPhysObject
         get { return _throttleUpdates; }
         set { _throttleUpdates = value; }
     }
-    public override bool IsColliding {
-        get { return (CollidingStep == PhysicsScene.SimulationStep); }
-        set { _isColliding = value; }
-    }
-    public override bool CollidingGround {
-        get { return (CollidingGroundStep == PhysicsScene.SimulationStep); }
-        set { _collidingGround = value; }
-    }
-    public override bool CollidingObj {
-        get { return _collidingObj; }
-        set { _collidingObj = value; }
-    }
     public bool IsPhantom {
         get {
             // SceneObjectPart removes phantom objects from the physics scene
@@ -831,32 +1108,23 @@ public sealed class BSPrim : BSPhysObject
             PhysicsScene.TaintedObject("BSPrim.setFloatOnWater", delegate()
             {
                 if (_floatOnWater)
-                    CurrentCollisionFlags = BulletSimAPI.AddToCollisionFlags2(PhysBody.ptr, CollisionFlags.BS_FLOATS_ON_WATER);
+                    CurrentCollisionFlags = PhysicsScene.PE.AddToCollisionFlags(PhysBody, CollisionFlags.BS_FLOATS_ON_WATER);
                 else
-                    CurrentCollisionFlags = BulletSimAPI.RemoveFromCollisionFlags2(PhysBody.ptr, CollisionFlags.BS_FLOATS_ON_WATER);
+                    CurrentCollisionFlags = PhysicsScene.PE.RemoveFromCollisionFlags(PhysBody, CollisionFlags.BS_FLOATS_ON_WATER);
             });
         }
     }
     public override OMV.Vector3 RotationalVelocity {
         get {
-            /*
-            OMV.Vector3 pv = OMV.Vector3.Zero;
-            // if close to zero, report zero
-            // This is copied from ODE but I'm not sure why it returns zero but doesn't
-            //    zero the property in the physics engine.
-            if (_rotationalVelocity.ApproxEquals(pv, 0.2f))
-                return pv;
-             */
-
             return _rotationalVelocity;
         }
         set {
             _rotationalVelocity = value;
+            Util.ClampV(_rotationalVelocity, BSParam.MaxAngularVelocity);
             // m_log.DebugFormat("{0}: RotationalVelocity={1}", LogHeader, _rotationalVelocity);
             PhysicsScene.TaintedObject("BSPrim.setRotationalVelocity", delegate()
             {
-                DetailLog("{0},BSPrim.SetRotationalVel,taint,rotvel={1}", LocalID, _rotationalVelocity);
-                BulletSimAPI.SetAngularVelocity2(PhysBody.ptr, _rotationalVelocity);
+                ForceRotationalVelocity = _rotationalVelocity;
             });
         }
     }
@@ -865,8 +1133,14 @@ public sealed class BSPrim : BSPhysObject
             return _rotationalVelocity;
         }
         set {
-            _rotationalVelocity = value;
-            BulletSimAPI.SetAngularVelocity2(PhysBody.ptr, _rotationalVelocity);
+            _rotationalVelocity = Util.ClampV(value, BSParam.MaxAngularVelocity);
+            if (PhysBody.HasPhysicalBody)
+            {
+                DetailLog("{0},BSPrim.ForceRotationalVel,taint,rotvel={1}", LocalID, _rotationalVelocity);
+                PhysicsScene.PE.SetAngularVelocity(PhysBody, _rotationalVelocity);
+                // PhysicsScene.PE.SetInterpolationAngularVelocity(PhysBody, _rotationalVelocity);
+                ActivateIfPhysical(false);
+            }
         }
     }
     public override bool Kinematic {
@@ -890,27 +1164,130 @@ public sealed class BSPrim : BSPhysObject
         set {
             _buoyancy = value;
             // DetailLog("{0},BSPrim.setForceBuoyancy,taint,buoy={1}", LocalID, _buoyancy);
-            // Buoyancy is faked by changing the gravity applied to the object
-            float grav = PhysicsScene.Params.gravity * (1f - _buoyancy);
-            BulletSimAPI.SetGravity2(PhysBody.ptr, new OMV.Vector3(0f, 0f, grav));
+            // Force the recalculation of the various inertia,etc variables in the object
+            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 bool PIDActive {
-        set { _usePID = value; }
+        set
+        {
+            // TODO: add a sanity check -- don't move more than a region or something like that.
+            _PIDTarget = value;
+        }
     }
     public override float PIDTau {
         set { _PIDTau = value; }
     }
+    public override bool PIDActive {
+        set {
+            if (value)
+            {
+                // We're taking over after this.
+                ZeroMotion(true);
+
+                _targetMotor = new BSVMotor("BSPrim.PIDTarget",
+                                            _PIDTau,                    // timeScale
+                                            BSMotor.Infinite,           // decay time scale
+                                            BSMotor.InfiniteVector,     // friction timescale
+                                            1f                          // efficiency
+                );
+                _targetMotor.PhysicsScene = PhysicsScene; // DEBUG DEBUG so motor will output detail log messages.
+                _targetMotor.SetTarget(_PIDTarget);
+                _targetMotor.SetCurrent(RawPosition);
+                /*
+                _targetMotor = new BSPIDVMotor("BSPrim.PIDTarget");
+                _targetMotor.PhysicsScene = PhysicsScene; // DEBUG DEBUG so motor will output detail log messages.
+
+                _targetMotor.SetTarget(_PIDTarget);
+                _targetMotor.SetCurrent(RawPosition);
+                _targetMotor.TimeScale = _PIDTau;
+                _targetMotor.Efficiency = 1f;
+                 */
+
+                RegisterPreStepAction("BSPrim.PIDTarget", LocalID, delegate(float timeStep)
+                {
+                    if (!IsPhysicallyActive)
+                    {
+                        UnRegisterPreStepAction("BSPrim.PIDTarget", LocalID);
+                        return;
+                    }
+
+                    OMV.Vector3 origPosition = RawPosition;     // DEBUG DEBUG (for printout below)
+
+                    // 'movePosition' is where we'd like the prim to be at this moment.
+                    OMV.Vector3 movePosition = RawPosition + _targetMotor.Step(timeStep);
+
+                    // If we are very close to our target, turn off the movement motor.
+                    if (_targetMotor.ErrorIsZero())
+                    {
+                        DetailLog("{0},BSPrim.PIDTarget,zeroMovement,movePos={1},pos={2},mass={3}",
+                                                LocalID, movePosition, RawPosition, Mass);
+                        ForcePosition = _targetMotor.TargetValue;
+                        _targetMotor.Enabled = false;
+                    }
+                    else
+                    {
+                        _position = movePosition;
+                        PositionSanityCheck(true /* intaintTime */);
+                        ForcePosition = _position;
+                    }
+                    DetailLog("{0},BSPrim.PIDTarget,move,fromPos={1},movePos={2}", LocalID, origPosition, movePosition);
+                });
+            }
+            else
+            {
+                // Stop any targetting
+                UnRegisterPreStepAction("BSPrim.PIDTarget", LocalID);
+            }
+        }
+    }
 
     // Used for llSetHoverHeight and maybe vehicle height
     // Hover Height will override MoveTo target's Z
     public override bool PIDHoverActive {
-        set { _useHoverPID = value; }
+        set {
+            if (value)
+            {
+                // Turning the target on
+                _hoverMotor = new BSFMotor("BSPrim.Hover",
+                                            _PIDHoverTau,               // timeScale
+                                            BSMotor.Infinite,           // decay time scale
+                                            BSMotor.Infinite,           // friction timescale
+                                            1f                          // efficiency
+                );
+                _hoverMotor.SetTarget(ComputeCurrentPIDHoverHeight());
+                _hoverMotor.SetCurrent(RawPosition.Z);
+                _hoverMotor.PhysicsScene = PhysicsScene; // DEBUG DEBUG so motor will output detail log messages.
+
+                RegisterPreStepAction("BSPrim.Hover", LocalID, delegate(float timeStep)
+                {
+                    // Don't do hovering while the object is selected.
+                    if (!IsPhysicallyActive)
+                        return;
+
+                    _hoverMotor.SetCurrent(RawPosition.Z);
+                    _hoverMotor.SetTarget(ComputeCurrentPIDHoverHeight());
+                    float targetHeight = _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 - RawPosition.Z) * Mass;
+                    // Undo anything the object thinks it's doing at the moment
+                    moveForce = -RawVelocity.Z * Mass;
+
+                    PhysicsScene.PE.ApplyCentralImpulse(PhysBody, new OMV.Vector3(0f, 0f, moveForce));
+                    DetailLog("{0},BSPrim.Hover,move,targHt={1},moveForce={2},mass={3}", LocalID, targetHeight, moveForce, Mass);
+                });
+            }
+            else
+            {
+                UnRegisterPreStepAction("BSPrim.Hover", LocalID);
+            }
+        }
     }
     public override float PIDHoverHeight {
         set { _PIDHoverHeight = value; }
@@ -919,8 +1296,35 @@ public sealed class BSPrim : BSPhysObject
         set { _PIDHoverType = value; }
     }
     public override float PIDHoverTau {
-        set { _PIDHoverTao = value; }
+        set { _PIDHoverTau = value; }
     }
+    // Based on current position, determine what we should be hovering at now.
+    // Must recompute often. What if we walked offa cliff>
+    private float ComputeCurrentPIDHoverHeight()
+    {
+        float ret = _PIDHoverHeight;
+        float groundHeight = PhysicsScene.TerrainManager.GetTerrainHeightAtXYZ(RawPosition);
+
+        switch (_PIDHoverType)
+        {
+            case PIDHoverType.Ground:
+                ret = groundHeight + _PIDHoverHeight;
+                break;
+            case PIDHoverType.GroundAndWater:
+                float waterHeight = PhysicsScene.TerrainManager.GetWaterLevelAtXYZ(RawPosition);
+                if (groundHeight > waterHeight)
+                {
+                    ret = groundHeight + _PIDHoverHeight;
+                }
+                else
+                {
+                    ret = waterHeight + _PIDHoverHeight;
+                }
+                break;
+        }
+        return ret;
+    }
+
 
     // For RotLookAt
     public override OMV.Quaternion APIDTarget { set { return; } }
@@ -928,54 +1332,73 @@ public sealed class BSPrim : BSPhysObject
     public override float APIDStrength { set { return; } }
     public override float APIDDamping { set { return; } }
 
-    private List<OMV.Vector3> m_accumulatedForces = new List<OMV.Vector3>();
     public override void AddForce(OMV.Vector3 force, bool pushforce) {
-        AddForce(force, pushforce, false);
+        // 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.
+        addForce /= PhysicsScene.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())
-        {
-            // _force += force;
-            lock (m_accumulatedForces)
-                m_accumulatedForces.Add(new OMV.Vector3(force));
-        }
-        else
+        if (IsPhysicallyActive)
         {
-            m_log.WarnFormat("{0}: Got a NaN force applied to a prim. LocalID={1}", LogHeader, LocalID);
-            return;
-        }
-        PhysicsScene.TaintedObject(inTaintTime, "BSPrim.AddForce", delegate()
-        {
-            OMV.Vector3 fSum = OMV.Vector3.Zero;
-            lock (m_accumulatedForces)
+            if (force.IsFinite())
             {
-                // Sum the accumulated additional forces for one big force to apply once.
-                foreach (OMV.Vector3 v in m_accumulatedForces)
+                // DetailLog("{0},BSPrim.addForce,call,force={1}", LocalID, addForce);
+
+                OMV.Vector3 addForce = force;
+                PhysicsScene.TaintedObject(inTaintTime, "BSPrim.AddForce", delegate()
                 {
-                    fSum += v;
-                }
-                m_accumulatedForces.Clear();
+                    // Bullet adds this central force to the total force for this tick
+                    DetailLog("{0},BSPrim.addForce,taint,force={1}", LocalID, addForce);
+                    if (PhysBody.HasPhysicalBody)
+                    {
+                        PhysicsScene.PE.ApplyCentralForce(PhysBody, addForce);
+                        ActivateIfPhysical(false);
+                    }
+                });
             }
-            DetailLog("{0},BSPrim.AddForce,taint,force={1}", LocalID, fSum);
-            if (fSum != OMV.Vector3.Zero)
-                BulletSimAPI.ApplyCentralForce2(PhysBody.ptr, fSum);
-        });
+            else
+            {
+                m_log.WarnFormat("{0}: AddForce: Got a NaN force applied to a prim. LocalID={1}", LogHeader, LocalID);
+                return;
+            }
+        }
     }
 
-    // An impulse force is scaled by the mass of the object.
-    public void ApplyForceImpulse(OMV.Vector3 impulse, bool inTaintTime)
-    {
-        OMV.Vector3 applyImpulse = impulse;
-        PhysicsScene.TaintedObject(inTaintTime, "BSPrim.ApplyForceImpulse", delegate()
+    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)
         {
-            DetailLog("{0},BSPrim.ApplyForceImpulse,taint,tImpulse={1}", LocalID, applyImpulse);
-            BulletSimAPI.ApplyCentralImpulse2(PhysBody.ptr, applyImpulse);
-        });
+            if (impulse.IsFinite())
+            {
+                OMV.Vector3 addImpulse = Util.ClampV(impulse, BSParam.MaxAddForceMagnitude);
+                // DetailLog("{0},BSPrim.addForceImpulse,call,impulse={1}", LocalID, impulse);
+
+                PhysicsScene.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)
+                    {
+                        PhysicsScene.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;
+            }
+        }
     }
 
-    private List<OMV.Vector3> m_accumulatedAngularForces = new List<OMV.Vector3>();
     public override void AddAngularForce(OMV.Vector3 force, bool pushforce) {
         AddAngularForce(force, pushforce, false);
     }
@@ -983,42 +1406,38 @@ public sealed class BSPrim : BSPhysObject
     {
         if (force.IsFinite())
         {
-            // _force += force;
-            lock (m_accumulatedAngularForces)
-                m_accumulatedAngularForces.Add(new OMV.Vector3(force));
+            OMV.Vector3 angForce = force;
+            PhysicsScene.TaintedObject(inTaintTime, "BSPrim.AddAngularForce", delegate()
+            {
+                if (PhysBody.HasPhysicalBody)
+                {
+                    DetailLog("{0},BSPrim.AddAngularForce,taint,angForce={1}", LocalID, angForce);
+                    PhysicsScene.PE.ApplyTorque(PhysBody, angForce);
+                    ActivateIfPhysical(false);
+                }
+            });
         }
         else
         {
             m_log.WarnFormat("{0}: Got a NaN force applied to a prim. LocalID={1}", LogHeader, LocalID);
             return;
         }
-        PhysicsScene.TaintedObject(inTaintTime, "BSPrim.AddAngularForce", delegate()
-        {
-            OMV.Vector3 fSum = OMV.Vector3.Zero;
-            lock (m_accumulatedAngularForces)
-            {
-                // Sum the accumulated additional forces for one big force to apply once.
-                foreach (OMV.Vector3 v in m_accumulatedAngularForces)
-                {
-                    fSum += v;
-                }
-                m_accumulatedAngularForces.Clear();
-            }
-            DetailLog("{0},BSPrim.AddAngularForce,taint,aForce={1}", LocalID, fSum);
-            if (fSum != OMV.Vector3.Zero)
-            {
-                BulletSimAPI.ApplyTorque2(PhysBody.ptr, fSum);
-                _torque = fSum;
-            }
-        });
     }
+
     // A torque impulse.
+    // ApplyTorqueImpulse adds torque directly to the angularVelocity.
+    // AddAngularForce accumulates the force and applied it to the angular velocity all at once.
+    // Computed as: angularVelocity += impulse * inertia;
     public void ApplyTorqueImpulse(OMV.Vector3 impulse, bool inTaintTime)
     {
         OMV.Vector3 applyImpulse = impulse;
         PhysicsScene.TaintedObject(inTaintTime, "BSPrim.ApplyTorqueImpulse", delegate()
         {
-            BulletSimAPI.ApplyTorqueImpulse2(PhysBody.ptr, applyImpulse);
+            if (PhysBody.HasPhysicalBody)
+            {
+                PhysicsScene.PE.ApplyTorqueImpulse(PhysBody, applyImpulse);
+                ActivateIfPhysical(false);
+            }
         });
     }
 
@@ -1301,23 +1720,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();
-            }
-        }
-         */
-
-        if (returnMass <= 0)
-            returnMass = 0.0001f;
+        returnMass = Density * BSParam.DensityScaleFactor * volume;
+        DetailLog("{0},BSPrim.CalculateMass,den={1},vol={2},mass={3}", LocalID, Density, volume, returnMass);
 
-        if (returnMass > PhysicsScene.MaximumObjectMass)
-            returnMass = PhysicsScene.MaximumObjectMass;
+        returnMass = Util.Clamp(returnMass, BSParam.MinimumObjectMass, BSParam.MaximumObjectMass);
 
         return returnMass;
     }// end CalculateMass
@@ -1326,135 +1732,73 @@ public sealed class BSPrim : BSPhysObject
     // Rebuild the geometry and object.
     // This is called when the shape changes so we need to recreate the mesh/hull.
     // Called at taint-time!!!
-    private void CreateGeomAndObject(bool forceRebuild)
+    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.
+        // Updates base.PhysBody and base.PhysShape with the new information.
         // Ignore 'forceRebuild'. This routine makes the right choices and changes of necessary.
-        // Returns 'true' if either the body or the shape was changed.
-        PhysicsScene.Shapes.GetBodyAndShape(false, PhysicsScene.World, this, null, delegate(BulletBody dBody)
+        PhysicsScene.Shapes.GetBodyAndShape(false /*forceRebuild */, PhysicsScene.World, this, null, delegate(BulletBody dBody)
         {
             // Called if the current prim body is about to be destroyed.
             // Remove all the physical dependencies on the old body.
-            // (Maybe someday make the changing of BSShape an event handled by BSLinkset.)
-            needToRestoreLinkset = Linkset.RemoveBodyDependencies(this);
-            needToRestoreVehicle = _vehicle.RemoveBodyDependencies(this);
+            // (Maybe someday make the changing of BSShape an event to be subscribed to by BSLinkset, ...)
+            RemoveBodyDependencies();
         });
 
-        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
+    protected virtual void RemoveBodyDependencies()
+    {
+        VehicleController.RemoveBodyDependencies(this);
     }
 
-    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)
     {
-        /*
-        UpdatedProperties changed = 0;
-        // assign to the local variables so the normal set action does not happen
-        // if (_position != entprop.Position)
-        if (!_position.ApproxEquals(entprop.Position, POSITION_TOLERANCE))
-        {
-            _position = entprop.Position;
-            changed |= UpdatedProperties.Position;
-        }
-        // if (_orientation != entprop.Rotation)
-        if (!_orientation.ApproxEquals(entprop.Rotation, ROTATION_TOLERANCE))
-        {
-            _orientation = entprop.Rotation;
-            changed |= UpdatedProperties.Rotation;
-        }
-        // if (_velocity != entprop.Velocity)
-        if (!_velocity.ApproxEquals(entprop.Velocity, VELOCITY_TOLERANCE))
-        {
-            _velocity = entprop.Velocity;
-            changed |= UpdatedProperties.Velocity;
-        }
-        // if (_acceleration != entprop.Acceleration)
-        if (!_acceleration.ApproxEquals(entprop.Acceleration, ACCELERATION_TOLERANCE))
-        {
-            _acceleration = entprop.Acceleration;
-            changed |= UpdatedProperties.Acceleration;
-        }
-        // if (_rotationalVelocity != entprop.RotationalVelocity)
-        if (!_rotationalVelocity.ApproxEquals(entprop.RotationalVelocity, ROTATIONAL_VELOCITY_TOLERANCE))
-        {
-            _rotationalVelocity = entprop.RotationalVelocity;
-            changed |= UpdatedProperties.RotationalVel;
-        }
-        if (changed != 0)
+        TriggerPreUpdatePropertyAction(ref entprop);
+
+        // A temporary kludge to suppress the rotational effects introduced on vehicles by Bullet
+        // TODO: handle physics introduced by Bullet with computed vehicle physics.
+        if (VehicleController.IsActive)
         {
-            // Only update the position of single objects and linkset roots
-            if (Linkset.IsRoot(this))
-            {
-                base.RequestPhysicsterseUpdate();
-            }
+            entprop.RotationalVelocity = OMV.Vector3.Zero;
         }
-        */
 
-        // Don't check for damping here -- it's done in BulletSim and SceneObjectPart.
+        // DetailLog("{0},BSPrim.UpdateProperties,entry,entprop={1}", LocalID, entprop);   // DEBUG DEBUG
 
-        // Updates only for individual prims and for the root object of a linkset.
-        if (Linkset.IsRoot(this))
-        {
-            // Assign directly to the local variables so the normal set action does not happen
-            _position = entprop.Position;
-            _orientation = entprop.Rotation;
+        // 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(_velocity, BSParam.UpdateVelocityChangeThreshold))
             _velocity = entprop.Velocity;
-            _acceleration = entprop.Acceleration;
-            _rotationalVelocity = entprop.RotationalVelocity;
+        _acceleration = entprop.Acceleration;
+        _rotationalVelocity = entprop.RotationalVelocity;
 
-            // The sanity check can change the velocity and/or position.
-            if (PositionSanityCheck(true))
-            {
-                entprop.Position = _position;
-                entprop.Velocity = _velocity;
-            }
+        // DetailLog("{0},BSPrim.UpdateProperties,afterAssign,entprop={1}", LocalID, entprop);   // DEBUG DEBUG
 
-            // remember the current and last set values
-            LastEntityProperties = CurrentEntityProperties;
-            CurrentEntityProperties = entprop;
+        // The sanity check can change the velocity and/or position.
+        if (PositionSanityCheck(true /* inTaintTime */ ))
+        {
+            entprop.Position = _position;
+            entprop.Velocity = _velocity;
+            entprop.RotationalVelocity = _rotationalVelocity;
+            entprop.Acceleration = _acceleration;
+        }
 
-            OMV.Vector3 direction = OMV.Vector3.UnitX * _orientation;
-            DetailLog("{0},BSPrim.UpdateProperties,call,pos={1},orient={2},dir={3},vel={4},rotVel={5}",
-                    LocalID, _position, _orientation, direction, _velocity, _rotationalVelocity);
+        OMV.Vector3 direction = OMV.Vector3.UnitX * _orientation;   // DEBUG DEBUG DEBUG
+        DetailLog("{0},BSPrim.UpdateProperties,call,entProp={1},dir={2}", LocalID, entprop, direction);
 
-            // BulletSimAPI.DumpRigidBody2(PhysicsScene.World.ptr, BSBody.ptr);   // DEBUG DEBUG DEBUG
+        // remember the current and last set values
+        LastEntityProperties = CurrentEntityProperties;
+        CurrentEntityProperties = entprop;
 
-            base.RequestPhysicsterseUpdate();
-        }
+        base.RequestPhysicsterseUpdate();
             /*
         else
         {
@@ -1464,9 +1808,6 @@ public sealed class BSPrim : BSPhysObject
                     entprop.Acceleration, entprop.RotationalVelocity);
         }
              */
-
-        // The linkset implimentation might want to know about this.
-        Linkset.UpdateProperties(this);
     }
 }
 }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrimDisplaced.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrimDisplaced.cs
new file mode 100755
index 0000000..f1c3b5c
--- /dev/null
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrimDisplaced.cs
@@ -0,0 +1,153 @@
+/*
+ * 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 SetEffectiveCenterOfMassW(Vector3 centerOfMassDisplacement)
+    {
+        Vector3 comDisp;
+        if (UserSetCenterOfMass.HasValue)
+            comDisp = (OMV.Vector3)UserSetCenterOfMass;
+        else
+            comDisp = centerOfMassDisplacement;
+
+        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)
+                base.ForcePosition = value - (PositionDisplacement * RawOrientation);
+            else
+                base.ForcePosition = value;
+        }
+    }
+
+    public override Quaternion ForceOrientation
+    {
+        get { return base.ForceOrientation; }
+        set
+        {
+            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!!!
+            entprop.Position = entprop.Position + (PositionDisplacement * entprop.Rotation);
+            // 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..d65d407
--- /dev/null
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrimLinkable.cs
@@ -0,0 +1,182 @@
+/*
+ * 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(PhysicsScene, this);
+
+        PhysicsScene.TaintedObject("BSPrimLinksetCompound.Refresh", delegate()
+        {
+            Linkset.Refresh(this);
+        });
+    }
+
+    public override void Destroy()
+    {
+        Linkset = Linkset.RemoveMeFromLinkset(this);
+        base.Destroy();
+    }
+
+    public override BSPhysicsShapeType PreferredPhysicalShape
+        { get { return Linkset.PreferredPhysicalShape(this); } }
+
+    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;
+            PhysicsScene.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;
+            PhysicsScene.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 RemoveBodyDependencies()
+    {
+        Linkset.RemoveBodyDependencies(this);
+        base.RemoveBodyDependencies();
+    }
+
+    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);
+        }
+        // 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;
+        }
+        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 27a78d1..e6aefd5 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
@@ -26,6 +26,8 @@
  */
 using System;
 using System.Collections.Generic;
+using System.Linq;
+using System.Reflection;
 using System.Runtime.InteropServices;
 using System.Text;
 using System.Threading;
@@ -38,40 +40,22 @@ using Nini.Config;
 using log4net;
 using OpenMetaverse;
 
-// TODOs for BulletSim (for BSScene, BSPrim, BSCharacter and BulletSim)
-// Test sculpties (verified that they don't work)
-// Compute physics FPS reasonably
-// Based on material, set density and friction
-// Don't use constraints in linksets of non-physical objects. Means having to move children manually.
-// Four states of prim: Physical, regular, phantom and selected. Are we modeling these correctly?
-//     In SL one can set both physical and phantom (gravity, does not effect others, makes collisions with ground)
-//     At the moment, physical and phantom causes object to drop through the terrain
-// Physical phantom objects and related typing (collision options )
-// Check out llVolumeDetect. Must do something for that.
-// Use collision masks for collision with terrain and phantom objects
-// More efficient memory usage when passing hull information from BSPrim to BulletSim
-// Should prim.link() and prim.delink() membership checking happen at taint time?
-// Mesh sharing. Use meshHash to tell if we already have a hull of that shape and only create once.
-// Do attachments need to be handled separately? Need collision events. Do not collide with VolumeDetect
-// Implement LockAngularMotion
-// Decide if clearing forces is the right thing to do when setting position (BulletSim::SetObjectTranslation)
-// Remove mesh and Hull stuff. Use mesh passed to bullet and use convexdecom from bullet.
-// Add PID movement operations. What does ScenePresence.MoveToTarget do?
-// Check terrain size. 128 or 127?
-// Raycast
-//
 namespace OpenSim.Region.Physics.BulletSPlugin
 {
 public sealed class BSScene : PhysicsScene, IPhysicsParameters
 {
-    private static readonly ILog m_log = LogManager.GetLogger(System.Reflection.MethodBase.GetCurrentMethod().DeclaringType);
-    private static readonly string LogHeader = "[BULLETS SCENE]";
+    internal static readonly ILog m_log = LogManager.GetLogger(System.Reflection.MethodBase.GetCurrentMethod().DeclaringType);
+    internal static readonly string LogHeader = "[BULLETS SCENE]";
 
     // The name of the region we're working for.
     public string RegionName { get; private set; }
 
     public string BulletSimVersion = "?";
 
+    // The handle to the underlying managed or unmanaged version of Bullet being used.
+    public string BulletEngineName { get; private set; }
+    public BSAPITemplate PE;
+
     public Dictionary<uint, BSPhysObject> PhysObjects;
     public BSShapeCollection Shapes;
 
@@ -82,32 +66,29 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
     //    every tick so OpenSim will update its animation.
     private HashSet<BSPhysObject> m_avatars = new HashSet<BSPhysObject>();
 
-    // List of all the objects that have vehicle properties and should be called
-    //    to update each physics step.
-    private List<BSPhysObject> m_vehicles = new List<BSPhysObject>();
-
     // let my minuions use my logger
     public ILog Logger { get { return m_log; } }
 
     public IMesher mesher;
-    // Level of Detail values kept as float because that's what the Meshmerizer wants
-    public float MeshLOD { get; private set; }
-    public float MeshMegaPrimLOD { get; private set; }
-    public float MeshMegaPrimThreshold { get; private set; }
-    public float SculptLOD { get; private set; }
-
     public uint WorldID { get; private set; }
-    public BulletSim World { get; private set; }
+    public BulletWorld World { get; private set; }
 
     // All the constraints that have been allocated in this instance.
     public BSConstraintCollection Constraints { get; private set; }
 
     // Simulation parameters
-    private int m_maxSubSteps;
-    private float m_fixedTimeStep;
-    private long m_simulationStep = 0;
+    internal int m_maxSubSteps;
+    internal float m_fixedTimeStep;
+    internal long m_simulationStep = 0;
+    internal float NominalFrameRate { get; set; }
     public long SimulationStep { get { return m_simulationStep; } }
-    private 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 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
@@ -121,31 +102,22 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
     public bool InTaintTime { get; private set; }
 
     // Pinned memory used to pass step information between managed and unmanaged
-    private int m_maxCollisionsPerFrame;
-    private CollisionDesc[] m_collisionArray;
-    private GCHandle m_collisionArrayPinnedHandle;
+    internal int m_maxCollisionsPerFrame;
+    internal CollisionDesc[] m_collisionArray;
 
-    private int m_maxUpdatesPerFrame;
-    private EntityProperties[] m_updateArray;
-    private GCHandle m_updateArrayPinnedHandle;
-
-    public bool ShouldMeshSculptedPrim { get; private set; }   // cause scuplted prims to get meshed
-    public bool ShouldForceSimplePrimMeshing { get; private set; }   // if a cube or sphere, let Bullet do internal shapes
-    public bool ShouldUseHullsForPhysicalObjects { get; private set; }   // 'true' if should create hulls for physical objects
-
-    public float PID_D { get; private set; }    // derivative
-    public float PID_P { get; private set; }    // proportional
+    internal int m_maxUpdatesPerFrame;
+    internal EntityProperties[] m_updateArray;
 
     public const uint TERRAIN_ID = 0;       // OpenSim senses terrain with a localID of zero
     public const uint GROUNDPLANE_ID = 1;
     public const uint CHILDTERRAIN_ID = 2;  // Terrain allocated based on our mega-prim childre start here
 
-    private float m_waterLevel;
+    public float SimpleWaterLevel { get; set; }
     public BSTerrainManager TerrainManager { get; private set; }
 
     public ConfigurationParameters Params
     {
-        get { return m_params[0]; }
+        get { return UnmanagedParams[0]; }
     }
     public Vector3 DefaultGravity
     {
@@ -157,8 +129,6 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
         get { return Params.gravity; }
     }
 
-    public float MaximumObjectMass { get; private set; }
-
     // When functions in the unmanaged code must be called, it is only
     //   done at a known time just before the simulation step. The taint
     //   system saves all these function calls and executes them in
@@ -181,13 +151,7 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
 
     // A pointer to an instance if this structure is passed to the C++ code
     // Used to pass basic configuration values to the unmanaged code.
-    ConfigurationParameters[] m_params;
-    GCHandle m_paramsHandle;
-
-    // Handle to the callback used by the unmanaged code to call into the managed code.
-    // Used for debug logging.
-    // Need to store the handle in a persistant variable so it won't be freed.
-    private BulletSimAPI.DebugLogCallback m_DebugLogCallbackHandle;
+    internal ConfigurationParameters[] UnmanagedParams;
 
     // Sometimes you just have to log everything.
     public Logging.LogWriter PhysicsLogging;
@@ -195,15 +159,24 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
     private string m_physicsLoggingDir;
     private string m_physicsLoggingPrefix;
     private int m_physicsLoggingFileMinutes;
+    private bool m_physicsLoggingDoFlush;
+    private bool m_physicsPhysicalDumpEnabled;
+    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)
@@ -216,17 +189,13 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
         Shapes = new BSShapeCollection(this);
 
         // Allocate pinned memory to pass parameters.
-        m_params = new ConfigurationParameters[1];
-        m_paramsHandle = GCHandle.Alloc(m_params, GCHandleType.Pinned);
+        UnmanagedParams = new ConfigurationParameters[1];
 
         // Set default values for physics parameters plus any overrides from the ini file
         GetInitialParameterValues(config);
 
-        // allocate more pinned memory close to the above in an attempt to get the memory all together
-        m_collisionArray = new CollisionDesc[m_maxCollisionsPerFrame];
-        m_collisionArrayPinnedHandle = GCHandle.Alloc(m_collisionArray, GCHandleType.Pinned);
-        m_updateArray = new EntityProperties[m_maxUpdatesPerFrame];
-        m_updateArrayPinnedHandle = GCHandle.Alloc(m_updateArray, GCHandleType.Pinned);
+        // Get the connection to the physics engine (could be native or one of many DLLs)
+        PE = SelectUnderlyingBulletEngine(BulletEngineName);
 
         // Enable very detailed logging.
         // By creating an empty logger when not logging, the log message invocation code
@@ -234,28 +203,16 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
         if (m_physicsLoggingEnabled)
         {
             PhysicsLogging = new Logging.LogWriter(m_physicsLoggingDir, m_physicsLoggingPrefix, m_physicsLoggingFileMinutes);
+            PhysicsLogging.ErrorLogger = m_log; // for DEBUG. Let's the logger output error messages.
         }
         else
         {
             PhysicsLogging = new Logging.LogWriter();
         }
 
-        // If Debug logging level, enable logging from the unmanaged code
-        m_DebugLogCallbackHandle = null;
-        if (m_log.IsDebugEnabled || PhysicsLogging.Enabled)
-        {
-            m_log.DebugFormat("{0}: Initialize: Setting debug callback for unmanaged code", LogHeader);
-            if (PhysicsLogging.Enabled)
-                // The handle is saved in a variable to make sure it doesn't get freed after this call
-                m_DebugLogCallbackHandle = new BulletSimAPI.DebugLogCallback(BulletLoggerPhysLog);
-            else
-                m_DebugLogCallbackHandle = new BulletSimAPI.DebugLogCallback(BulletLogger);
-        }
-
-        // Get the version of the DLL
-        // TODO: this doesn't work yet. Something wrong with marshaling the returned string.
-        // BulletSimVersion = BulletSimAPI.GetVersion();
-        // m_log.WarnFormat("{0}: BulletSim.dll version='{1}'", LogHeader, BulletSimVersion);
+        // Allocate memory for returning of the updates and collisions from the physics engine
+        m_collisionArray = new CollisionDesc[m_maxCollisionsPerFrame];
+        m_updateArray = new EntityProperties[m_maxUpdatesPerFrame];
 
         // The bounding box for the simulated world. The origin is 0,0,0 unless we're
         //    a child in a mega-region.
@@ -263,18 +220,14 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
         //    area. It tracks active objects no matter where they are.
         Vector3 worldExtent = new Vector3(Constants.RegionSize, Constants.RegionSize, Constants.RegionHeight);
 
-        // m_log.DebugFormat("{0}: Initialize: Calling BulletSimAPI.Initialize.", LogHeader);
-        World = new BulletSim(0, this, BulletSimAPI.Initialize2(worldExtent, m_paramsHandle.AddrOfPinnedObject(),
-                                        m_maxCollisionsPerFrame, m_collisionArrayPinnedHandle.AddrOfPinnedObject(),
-                                        m_maxUpdatesPerFrame, m_updateArrayPinnedHandle.AddrOfPinnedObject(),
-                                        m_DebugLogCallbackHandle));
+        World = PE.Initialize(worldExtent, Params, m_maxCollisionsPerFrame, ref m_collisionArray, m_maxUpdatesPerFrame, ref m_updateArray);
 
         Constraints = new BSConstraintCollection(World);
 
         TerrainManager = new BSTerrainManager(this);
         TerrainManager.CreateInitialGroundPlaneAndTerrain();
 
-        m_log.WarnFormat("{0} Linksets implemented with {1}", LogHeader, (BSLinkset.LinksetImplementation)Params.linksetImplementation);
+        m_log.WarnFormat("{0} Linksets implemented with {1}", LogHeader, (BSLinkset.LinksetImplementation)BSParam.LinksetImplementation);
 
         InTaintTime = false;
         m_initialized = true;
@@ -285,9 +238,9 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
     private void GetInitialParameterValues(IConfigSource config)
     {
         ConfigurationParameters parms = new ConfigurationParameters();
-        m_params[0] = parms;
+        UnmanagedParams[0] = parms;
 
-        SetParameterDefaultValues();
+        BSParam.SetParameterDefaultValues(this);
 
         if (config != null)
         {
@@ -295,19 +248,34 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
             IConfig pConfig = config.Configs["BulletSim"];
             if (pConfig != null)
             {
-                SetParameterConfigurationValues(pConfig);
+                BSParam.SetParameterConfigurationValues(this, pConfig);
+
+                // There are two Bullet implementations to choose from
+                BulletEngineName = pConfig.GetString("BulletEngine", "BulletUnmanaged");
 
                 // Very detailed logging for physics debugging
+                // TODO: the boolean values can be moved to the normal parameter processing.
                 m_physicsLoggingEnabled = pConfig.GetBoolean("PhysicsLoggingEnabled", false);
                 m_physicsLoggingDir = pConfig.GetString("PhysicsLoggingDir", ".");
                 m_physicsLoggingPrefix = pConfig.GetString("PhysicsLoggingPrefix", "physics-%REGIONNAME%-");
                 m_physicsLoggingFileMinutes = pConfig.GetInt("PhysicsLoggingFileMinutes", 5);
+                m_physicsLoggingDoFlush = pConfig.GetBoolean("PhysicsLoggingDoFlush", false);
+                m_physicsPhysicalDumpEnabled = pConfig.GetBoolean("PhysicsPhysicalDumpEnabled", false);
                 // Very detailed logging for vehicle debugging
                 VehicleLoggingEnabled = pConfig.GetBoolean("VehicleLoggingEnabled", false);
+                VehiclePhysicalLoggingEnabled = pConfig.GetBoolean("VehiclePhysicalLoggingEnabled", false);
 
                 // Do any replacements in the parameters
                 m_physicsLoggingPrefix = m_physicsLoggingPrefix.Replace("%REGIONNAME%", RegionName);
             }
+
+            // The material characteristics.
+            BSMaterials.InitializeFromDefaults(Params);
+            if (pConfig != null)
+            {
+                // Let the user add new and interesting material property values.
+                BSMaterials.InitializefromParameters(pConfig);
+            }
         }
     }
 
@@ -326,16 +294,41 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
         return ret;
     }
 
-    // Called directly from unmanaged code so don't do much
-    private void BulletLogger(string msg)
+    // Select the connection to the actual Bullet implementation.
+    // The main engine selection is the engineName up to the first hypen.
+    // So "Bullet-2.80-OpenCL-Intel" specifies the 'bullet' class here and the whole name
+    //     is passed to the engine to do its special selection, etc.
+    private BSAPITemplate SelectUnderlyingBulletEngine(string engineName)
     {
-        m_log.Debug("[BULLETS UNMANAGED]:" + msg);
-    }
+        // For the moment, do a simple switch statement.
+        // Someday do fancyness with looking up the interfaces in the assembly.
+        BSAPITemplate ret = null;
 
-    // Called directly from unmanaged code so don't do much
-    private void BulletLoggerPhysLog(string msg)
-    {
-        DetailLog("[BULLETS UNMANAGED]:" + msg);
+        string selectionName = engineName.ToLower();
+        int hyphenIndex = engineName.IndexOf("-");
+        if (hyphenIndex > 0)
+            selectionName = engineName.ToLower().Substring(0, hyphenIndex - 1);
+
+        switch (selectionName)
+        {
+            case "bulletunmanaged":
+                ret = new BSAPIUnman(engineName, this);
+                break;
+            case "bulletxna":
+                ret = new BSAPIXNA(engineName, this);
+                break;
+        }
+
+        if (ret == null)
+        {
+            m_log.ErrorFormat("{0) COULD NOT SELECT BULLET ENGINE: '[BulletSim]PhysicsEngine' must be either 'BulletUnmanaged-*' or 'BulletXNA-*'", LogHeader);
+        }
+        else
+        {
+            m_log.WarnFormat("{0} Selected bullet engine {1} -> {2}/{3}", LogHeader, engineName, ret.BulletEngineName, ret.BulletEngineVersion);
+        }
+
+        return ret;
     }
 
     public override void Dispose()
@@ -345,8 +338,6 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
         // make sure no stepping happens while we're deleting stuff
         m_initialized = false;
 
-        TerrainManager.ReleaseGroundPlaneAndTerrain();
-
         foreach (KeyValuePair<uint, BSPhysObject> kvp in PhysObjects)
         {
             kvp.Value.Destroy();
@@ -366,8 +357,15 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
             Shapes = null;
         }
 
+        if (TerrainManager != null)
+        {
+            TerrainManager.ReleaseGroundPlaneAndTerrain();
+            TerrainManager.Dispose();
+            TerrainManager = null;
+        }
+
         // Anything left in the unmanaged code should be cleaned out
-        BulletSimAPI.Shutdown2(World.ptr);
+        PE.Shutdown(World);
 
         // Not logging any more
         PhysicsLogging.Close();
@@ -389,12 +387,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;
     }
@@ -410,9 +410,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)
             {
@@ -421,13 +423,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);
@@ -456,9 +463,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;
     }
@@ -474,41 +481,56 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
     // Simulate one timestep
     public override float Simulate(float timeStep)
     {
+        // prevent simulation until we've been initialized
+        if (!m_initialized) return 5.0f;
+
+        LastTimeStep = timeStep;
+
         int updatedEntityCount = 0;
-        IntPtr updatedEntitiesPtr;
         int collidersCount = 0;
-        IntPtr collidersPtr;
 
         int beforeTime = 0;
         int simTime = 0;
 
-        // prevent simulation until we've been initialized
-        if (!m_initialized) return 5.0f;
-
         // update the prim states while we know the physics engine is not busy
         int numTaints = _taintOperations.Count;
+
+        InTaintTime = true; // Only used for debugging so locking is not necessary.
+
         ProcessTaints();
 
-        // Some of the prims operate with special vehicle properties
-        ProcessVehicles(timeStep);
-        ProcessTaints();    // the vehicles might have added taints
+        // 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
+        numTaints += _taintOperations.Count;
+        ProcessTaints();
+
+        InTaintTime = false; // Only used for debugging so locking is not necessary.
+
+        // The following causes the unmanaged code to output ALL the values found in ALL the objects in the world.
+        // Only enable this in a limited test world with few objects.
+        if (m_physicsPhysicalDumpEnabled)
+            PE.DumpAllInfo(World);
 
         // step the physical world one interval
         m_simulationStep++;
         int numSubSteps = 0;
-
         try
         {
-            if (VehicleLoggingEnabled) DumpVehicles();  // DEBUG
-            if (PhysicsLogging.Enabled) beforeTime = Util.EnvironmentTickCount();
+            if (PhysicsLogging.Enabled)
+                beforeTime = Util.EnvironmentTickCount();
 
-            numSubSteps = BulletSimAPI.PhysicsStep2(World.ptr, timeStep, m_maxSubSteps, m_fixedTimeStep,
-                        out updatedEntityCount, out updatedEntitiesPtr, out collidersCount, out collidersPtr);
+            numSubSteps = PE.PhysicsStep(World, timeStep, m_maxSubSteps, m_fixedTimeStep, out updatedEntityCount, out collidersCount);
 
-            if (PhysicsLogging.Enabled) simTime = Util.EnvironmentTickCountSubtract(beforeTime);
-            DetailLog("{0},Simulate,call, frame={1}, nTaints={2}, simTime={3}, substeps={4}, updates={5}, colliders={6}",
-                        DetailLogZero, m_simulationStep, numTaints, simTime, numSubSteps, updatedEntityCount, collidersCount);
-            if (VehicleLoggingEnabled) DumpVehicles();  // DEBUG
+            if (PhysicsLogging.Enabled)
+            {
+                simTime = Util.EnvironmentTickCountSubtract(beforeTime);
+                DetailLog("{0},Simulate,call, frame={1}, nTaints={2}, simTime={3}, substeps={4}, updates={5}, colliders={6}, objWColl={7}",
+                                        DetailLogZero, m_simulationStep, numTaints, simTime, numSubSteps,
+                                        updatedEntityCount, collidersCount, ObjectsWithCollisions.Count);
+            }
         }
         catch (Exception e)
         {
@@ -520,9 +542,10 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
             collidersCount = 0;
         }
 
-        // Don't have to use the pointers passed back since we know it is the same pinned memory we passed in
+        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
+        // Get a value for 'now' so all the collision and update routines don't have to get their own.
         SimulationNowTime = Util.EnvironmentTickCount();
 
         // If there were collisions, process them by sending the event to the prim.
@@ -535,8 +558,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);
             }
         }
 
@@ -562,12 +586,16 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
 
         // Objects that are done colliding are removed from the ObjectsWithCollisions list.
         // Not done above because it is inside an iteration of ObjectWithCollisions.
+        // This complex collision processing is required to create an empty collision
+        //     event call after all real collisions have happened on an object. This enables
+        //     the simulator to generate the 'collision end' event.
         if (ObjectsWithNoMoreCollisions.Count > 0)
         {
             foreach (BSPhysObject po in ObjectsWithNoMoreCollisions)
                 ObjectsWithCollisions.Remove(po);
             ObjectsWithNoMoreCollisions.Clear();
         }
+        // Done with collisions.
 
         // If any of the objects had updated properties, tell the object it has been changed by the physics engine
         if (updatedEntityCount > 0)
@@ -583,17 +611,17 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
             }
         }
 
-        ProcessPostStepTaints();
+        TriggerPostStepEvent(timeStep);
 
-        // This causes the unmanaged code to output ALL the values found in ALL the objects in the world.
+        // The following causes the unmanaged code to output ALL the values found in ALL the objects in the world.
         // Only enable this in a limited test world with few objects.
-        // BulletSimAPI.DumpAllInfo2(World.ptr);    // DEBUG DEBUG DEBUG
+        if (m_physicsPhysicalDumpEnabled)
+            PE.DumpAllInfo(World);
 
         // The physics engine returns the number of milliseconds it simulated this call.
         // These are summed and normalized to one second and divided by 1000 to give the reported physics FPS.
-        // We multiply by 55 to give a recognizable running rate (55 or less).
-        return numSubSteps * m_fixedTimeStep * 1000 * 55;
-        // return timeStep * 1000 * 55;
+        // Multiply by a fixed nominal frame rate to give a rate similar to the simulator (usually 55).
+        return (float)numSubSteps * m_fixedTimeStep * 1000f * NominalFrameRate;
     }
 
     // Something has collided
@@ -639,12 +667,7 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
 
     public override void SetWaterLevel(float baseheight)
     {
-        m_waterLevel = baseheight;
-    }
-    // Someday....
-    public float GetWaterLevelAtXYZ(Vector3 loc)
-    {
-        return m_waterLevel;
+        SimpleWaterLevel = baseheight;
     }
 
     public override void DeleteTerrain()
@@ -675,12 +698,35 @@ 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;  } }
 
     #region Taints
+    // The simulation execution order is:
+    // Simulate()
+    //    DoOneTimeTaints
+    //    TriggerPreStepEvent
+    //    DoOneTimeTaints
+    //    Step()
+    //       ProcessAndSendToSimulatorCollisions
+    //       ProcessAndSendToSimulatorPropertyUpdates
+    //    TriggerPostStepEvent
 
     // Calls to the PhysicsActors can't directly call into the physics engine
     //       because it might be busy. We delay changes to a known time.
@@ -707,58 +753,35 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
             TaintedObject(ident, callback);
     }
 
+    private void TriggerPreStepEvent(float timeStep)
+    {
+        PreStepAction actions = BeforeStep;
+        if (actions != null)
+            actions(timeStep);
+
+    }
+
+    private void TriggerPostStepEvent(float timeStep)
+    {
+        PostStepAction actions = AfterStep;
+        if (actions != null)
+            actions(timeStep);
+
+    }
+
     // When someone tries to change a property on a BSPrim or BSCharacter, the object queues
     // a callback into itself to do the actual property change. That callback is called
     // here just before the physics engine is called to step the simulation.
     public void ProcessTaints()
     {
-        InTaintTime = true; // Only used for debugging so locking is not necessary.
         ProcessRegularTaints();
         ProcessPostTaintTaints();
-        InTaintTime = false;
     }
 
     private void ProcessRegularTaints()
     {
         if (_taintOperations.Count > 0)  // save allocating new list if there is nothing to process
         {
-            /*
-            // Code to limit the number of taints processed per step. Meant to limit step time.
-            // Unsure if a good idea as code assumes that taints are done before the step.
-            int taintCount = m_taintsToProcessPerStep;
-            TaintCallbackEntry oneCallback = new TaintCallbackEntry();
-            while (_taintOperations.Count > 0 && taintCount-- > 0)
-            {
-                bool gotOne = false;
-                lock (_taintLock)
-                {
-                    if (_taintOperations.Count > 0)
-                    {
-                        oneCallback = _taintOperations[0];
-                        _taintOperations.RemoveAt(0);
-                        gotOne = true;
-                    }
-                }
-                if (gotOne)
-                {
-                    try
-                    {
-                        DetailLog("{0},BSScene.ProcessTaints,doTaint,id={1}", DetailLogZero, oneCallback.ident);
-                        oneCallback.callback();
-                    }
-                    catch (Exception e)
-                    {
-                        DetailLog("{0},BSScene.ProcessTaints,doTaintException,id={1}", DetailLogZero, oneCallback.ident); // DEBUG DEBUG DEBUG
-                        m_log.ErrorFormat("{0}: ProcessTaints: {1}: Exception: {2}", LogHeader, oneCallback.ident, e);
-                    }
-                }
-            }
-            if (_taintOperations.Count > 0)
-            {
-                DetailLog("{0},BSScene.ProcessTaints,leftTaintsOnList,numNotProcessed={1}", DetailLogZero, _taintOperations.Count);
-            }
-             */
-
             // swizzle a new list into the list location so we can process what's there
             List<TaintCallbackEntry> oldList;
             lock (_taintLock)
@@ -797,6 +820,7 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
         return;
     }
 
+    // Taints that happen after the normal taint processing but before the simulation step.
     private void ProcessPostTaintTaints()
     {
         if (_postTaintOperations.Count > 0)
@@ -824,45 +848,6 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
         }
     }
 
-    public void PostStepTaintObject(String ident, TaintCallback callback)
-    {
-        if (!m_initialized) return;
-
-        lock (_taintLock)
-        {
-            _postStepOperations.Add(new TaintCallbackEntry(ident, callback));
-        }
-
-        return;
-    }
-
-    private void ProcessPostStepTaints()
-    {
-        if (_postStepOperations.Count > 0)
-        {
-            List<TaintCallbackEntry> oldList;
-            lock (_taintLock)
-            {
-                oldList = _postStepOperations;
-                _postStepOperations = new List<TaintCallbackEntry>();
-            }
-
-            foreach (TaintCallbackEntry tcbe in oldList)
-            {
-                try
-                {
-                    DetailLog("{0},BSScene.ProcessPostStepTaints,doTaint,id={1}", DetailLogZero, tcbe.ident); // DEBUG DEBUG DEBUG
-                    tcbe.callback();
-                }
-                catch (Exception e)
-                {
-                    m_log.ErrorFormat("{0}: ProcessPostStepTaints: {1}: Exception: {2}", LogHeader, tcbe.ident, e);
-                }
-            }
-            oldList.Clear();
-        }
-    }
-
     // Only used for debugging. Does not change state of anything so locking is not necessary.
     public bool AssertInTaintTime(string whereFrom)
     {
@@ -870,517 +855,19 @@ 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();  // Prints the stack into the DEBUG log file.
+            // Util.PrintCallStack(DetailLog);
         }
         return InTaintTime;
     }
 
     #endregion // Taints
 
-    #region Vehicles
-
-    public void VehicleInSceneTypeChanged(BSPrim vehic, Vehicle newType)
-    {
-        RemoveVehiclePrim(vehic);
-        if (newType != Vehicle.TYPE_NONE)
-        {
-           // make it so the scene will call us each tick to do vehicle things
-           AddVehiclePrim(vehic);
-        }
-    }
-
-    // Make so the scene will call this prim for vehicle actions each tick.
-    // Safe to call if prim is already in the vehicle list.
-    public void AddVehiclePrim(BSPrim vehicle)
-    {
-        lock (m_vehicles)
-        {
-            if (!m_vehicles.Contains(vehicle))
-            {
-                m_vehicles.Add(vehicle);
-            }
-        }
-    }
-
-    // Remove a prim from our list of vehicles.
-    // Safe to call if the prim is not in the vehicle list.
-    public void RemoveVehiclePrim(BSPrim vehicle)
-    {
-        lock (m_vehicles)
-        {
-            if (m_vehicles.Contains(vehicle))
-            {
-                m_vehicles.Remove(vehicle);
-            }
-        }
-    }
-
-    // Some prims have extra vehicle actions
-    // Called at taint time!
-    private void ProcessVehicles(float timeStep)
-    {
-        foreach (BSPhysObject pobj in m_vehicles)
-        {
-            pobj.StepVehicle(timeStep);
-        }
-    }
-    #endregion Vehicles
-
-    #region INI and command line parameter processing
-
-    delegate void ParamUser(BSScene scene, IConfig conf, string paramName, float val);
-    delegate float ParamGet(BSScene scene);
-    delegate void ParamSet(BSScene scene, string paramName, uint localID, float val);
-    delegate void SetOnObject(BSScene scene, BSPhysObject obj, float val);
-
-    private struct ParameterDefn
-    {
-        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)
-        {
-            name = n;
-            desc = d;
-            defaultValue = v;
-            userParam = u;
-            getter = g;
-            setter = s;
-            onObject = null;
-        }
-        public ParameterDefn(string n, string d, float v, ParamUser u, ParamGet g, ParamSet s, SetOnObject o)
-        {
-            name = n;
-            desc = d;
-            defaultValue = v;
-            userParam = u;
-            getter = g;
-            setter = s;
-            onObject = o;
-        }
-    }
-
-    // List of all of the externally visible parameters.
-    // For each parameter, this table maps a text name to getter and setters.
-    // To add a new externally referencable/settable parameter, add the paramter storage
-    //    location somewhere in the program and make an entry in this table with the
-    //    getters and setters.
-    // 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:
-    //    -- 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 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
-    //
-    // The single letter parameters for the delegates are:
-    //    s = BSScene
-    //    o = BSPhysObject
-    //    p = string parameter name
-    //    l = localID of referenced object
-    //    v = float value
-    //    cf = parameter configuration class (for fetching values from ini file)
-    private ParameterDefn[] ParameterDefinitions =
-    {
-        new ParameterDefn("MeshSculptedPrim", "Whether to create meshes for sculpties",
-            ConfigurationParameters.numericTrue,
-            (s,cf,p,v) => { s.ShouldMeshSculptedPrim = cf.GetBoolean(p, s.BoolNumeric(v)); },
-            (s) => { return s.NumericBool(s.ShouldMeshSculptedPrim); },
-            (s,p,l,v) => { s.ShouldMeshSculptedPrim = s.BoolNumeric(v); } ),
-        new ParameterDefn("ForceSimplePrimMeshing", "If true, only use primitive meshes for objects",
-            ConfigurationParameters.numericFalse,
-            (s,cf,p,v) => { s.ShouldForceSimplePrimMeshing = cf.GetBoolean(p, s.BoolNumeric(v)); },
-            (s) => { return s.NumericBool(s.ShouldForceSimplePrimMeshing); },
-            (s,p,l,v) => { s.ShouldForceSimplePrimMeshing = s.BoolNumeric(v); } ),
-        new ParameterDefn("UseHullsForPhysicalObjects", "If true, create hulls for physical objects",
-            ConfigurationParameters.numericTrue,
-            (s,cf,p,v) => { s.ShouldUseHullsForPhysicalObjects = cf.GetBoolean(p, s.BoolNumeric(v)); },
-            (s) => { return s.NumericBool(s.ShouldUseHullsForPhysicalObjects); },
-            (s,p,l,v) => { s.ShouldUseHullsForPhysicalObjects = s.BoolNumeric(v); } ),
-
-        new ParameterDefn("MeshLevelOfDetail", "Level of detail to render meshes (32, 16, 8 or 4. 32=most detailed)",
-            8f,
-            (s,cf,p,v) => { s.MeshLOD = (float)cf.GetInt(p, (int)v); },
-            (s) => { return s.MeshLOD; },
-            (s,p,l,v) => { s.MeshLOD = v; } ),
-        new ParameterDefn("MeshLevelOfDetailMegaPrim", "Level of detail to render meshes larger than threshold meters",
-            16f,
-            (s,cf,p,v) => { s.MeshMegaPrimLOD = (float)cf.GetInt(p, (int)v); },
-            (s) => { return s.MeshMegaPrimLOD; },
-            (s,p,l,v) => { s.MeshMegaPrimLOD = v; } ),
-        new ParameterDefn("MeshLevelOfDetailMegaPrimThreshold", "Size (in meters) of a mesh before using MeshMegaPrimLOD",
-            10f,
-            (s,cf,p,v) => { s.MeshMegaPrimThreshold = (float)cf.GetInt(p, (int)v); },
-            (s) => { return s.MeshMegaPrimThreshold; },
-            (s,p,l,v) => { s.MeshMegaPrimThreshold = v; } ),
-        new ParameterDefn("SculptLevelOfDetail", "Level of detail to render sculpties (32, 16, 8 or 4. 32=most detailed)",
-            32f,
-            (s,cf,p,v) => { s.SculptLOD = (float)cf.GetInt(p, (int)v); },
-            (s) => { return s.SculptLOD; },
-            (s,p,l,v) => { s.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)",
-            1f / 60f,
-            (s,cf,p,v) => { s.m_fixedTimeStep = cf.GetFloat(p, v); },
-            (s) => { return (float)s.m_fixedTimeStep; },
-            (s,p,l,v) => { s.m_fixedTimeStep = v; } ),
-        new ParameterDefn("MaxCollisionsPerFrame", "Max collisions returned at end of each frame",
-            2048f,
-            (s,cf,p,v) => { s.m_maxCollisionsPerFrame = cf.GetInt(p, (int)v); },
-            (s) => { return (float)s.m_maxCollisionsPerFrame; },
-            (s,p,l,v) => { s.m_maxCollisionsPerFrame = (int)v; } ),
-        new ParameterDefn("MaxUpdatesPerFrame", "Max updates returned at end of each frame",
-            8000f,
-            (s,cf,p,v) => { s.m_maxUpdatesPerFrame = cf.GetInt(p, (int)v); },
-            (s) => { return (float)s.m_maxUpdatesPerFrame; },
-            (s,p,l,v) => { s.m_maxUpdatesPerFrame = (int)v; } ),
-        new ParameterDefn("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("MaxObjectMass", "Maximum object mass (10000.01)",
-            10000.01f,
-            (s,cf,p,v) => { s.MaximumObjectMass = cf.GetFloat(p, v); },
-            (s) => { return (float)s.MaximumObjectMass; },
-            (s,p,l,v) => { s.MaximumObjectMass = v; } ),
-
-        new ParameterDefn("PID_D", "Derivitive factor for motion smoothing",
-            2200f,
-            (s,cf,p,v) => { s.PID_D = cf.GetFloat(p, v); },
-            (s) => { return (float)s.PID_D; },
-            (s,p,l,v) => { s.PID_D = v; } ),
-        new ParameterDefn("PID_P", "Parameteric factor for motion smoothing",
-            900f,
-            (s,cf,p,v) => { s.PID_P = cf.GetFloat(p, v); },
-            (s) => { return (float)s.PID_P; },
-            (s,p,l,v) => { s.PID_P = v; } ),
-
-        new ParameterDefn("DefaultFriction", "Friction factor used on new objects",
-            0.5f,
-            (s,cf,p,v) => { s.m_params[0].defaultFriction = cf.GetFloat(p, v); },
-            (s) => { return s.m_params[0].defaultFriction; },
-            (s,p,l,v) => { s.m_params[0].defaultFriction = v; } ),
-        new ParameterDefn("DefaultDensity", "Density for new objects" ,
-            10.000006836f,  // Aluminum g/cm3
-            (s,cf,p,v) => { s.m_params[0].defaultDensity = cf.GetFloat(p, v); },
-            (s) => { return s.m_params[0].defaultDensity; },
-            (s,p,l,v) => { s.m_params[0].defaultDensity = v; } ),
-        new ParameterDefn("DefaultRestitution", "Bouncyness of an object" ,
-            0f,
-            (s,cf,p,v) => { s.m_params[0].defaultRestitution = cf.GetFloat(p, v); },
-            (s) => { return s.m_params[0].defaultRestitution; },
-            (s,p,l,v) => { s.m_params[0].defaultRestitution = v; } ),
-        new ParameterDefn("CollisionMargin", "Margin around objects before collisions are calculated (must be zero!)",
-            0f,
-            (s,cf,p,v) => { s.m_params[0].collisionMargin = cf.GetFloat(p, v); },
-            (s) => { return s.m_params[0].collisionMargin; },
-            (s,p,l,v) => { s.m_params[0].collisionMargin = v; } ),
-        new ParameterDefn("Gravity", "Vertical force of gravity (negative means down)",
-            -9.80665f,
-            (s,cf,p,v) => { s.m_params[0].gravity = cf.GetFloat(p, v); },
-            (s) => { return s.m_params[0].gravity; },
-            (s,p,l,v) => { s.UpdateParameterObject(ref s.m_params[0].gravity, p, PhysParameterEntry.APPLY_TO_NONE, v); },
-            (s,o,v) => { BulletSimAPI.SetGravity2(s.World.ptr, new Vector3(0f,0f,v)); } ),
-
-
-        new ParameterDefn("LinearDamping", "Factor to damp linear movement per second (0.0 - 1.0)",
-            0f,
-            (s,cf,p,v) => { s.m_params[0].linearDamping = cf.GetFloat(p, v); },
-            (s) => { return s.m_params[0].linearDamping; },
-            (s,p,l,v) => { s.UpdateParameterObject(ref s.m_params[0].linearDamping, p, l, v); },
-            (s,o,v) => { BulletSimAPI.SetDamping2(o.PhysBody.ptr, v, s.m_params[0].angularDamping); } ),
-        new ParameterDefn("AngularDamping", "Factor to damp angular movement per second (0.0 - 1.0)",
-            0f,
-            (s,cf,p,v) => { s.m_params[0].angularDamping = cf.GetFloat(p, v); },
-            (s) => { return s.m_params[0].angularDamping; },
-            (s,p,l,v) => { s.UpdateParameterObject(ref s.m_params[0].angularDamping, p, l, v); },
-            (s,o,v) => { BulletSimAPI.SetDamping2(o.PhysBody.ptr, s.m_params[0].linearDamping, v); } ),
-        new ParameterDefn("DeactivationTime", "Seconds before considering an object potentially static",
-            0.2f,
-            (s,cf,p,v) => { s.m_params[0].deactivationTime = cf.GetFloat(p, v); },
-            (s) => { return s.m_params[0].deactivationTime; },
-            (s,p,l,v) => { s.UpdateParameterObject(ref s.m_params[0].deactivationTime, p, l, v); },
-            (s,o,v) => { BulletSimAPI.SetDeactivationTime2(o.PhysBody.ptr, v); } ),
-        new ParameterDefn("LinearSleepingThreshold", "Seconds to measure linear movement before considering static",
-            0.8f,
-            (s,cf,p,v) => { s.m_params[0].linearSleepingThreshold = cf.GetFloat(p, v); },
-            (s) => { return s.m_params[0].linearSleepingThreshold; },
-            (s,p,l,v) => { s.UpdateParameterObject(ref s.m_params[0].linearSleepingThreshold, p, l, v); },
-            (s,o,v) => { BulletSimAPI.SetSleepingThresholds2(o.PhysBody.ptr, v, v); } ),
-        new ParameterDefn("AngularSleepingThreshold", "Seconds to measure angular movement before considering static",
-            1.0f,
-            (s,cf,p,v) => { s.m_params[0].angularSleepingThreshold = cf.GetFloat(p, v); },
-            (s) => { return s.m_params[0].angularSleepingThreshold; },
-            (s,p,l,v) => { s.UpdateParameterObject(ref s.m_params[0].angularSleepingThreshold, p, l, v); },
-            (s,o,v) => { BulletSimAPI.SetSleepingThresholds2(o.PhysBody.ptr, v, v); } ),
-        new ParameterDefn("CcdMotionThreshold", "Continuious collision detection threshold (0 means no CCD)" ,
-            0f,     // set to zero to disable
-            (s,cf,p,v) => { s.m_params[0].ccdMotionThreshold = cf.GetFloat(p, v); },
-            (s) => { return s.m_params[0].ccdMotionThreshold; },
-            (s,p,l,v) => { s.UpdateParameterObject(ref s.m_params[0].ccdMotionThreshold, p, l, v); },
-            (s,o,v) => { BulletSimAPI.SetCcdMotionThreshold2(o.PhysBody.ptr, v); } ),
-        new ParameterDefn("CcdSweptSphereRadius", "Continuious collision detection test radius" ,
-            0f,
-            (s,cf,p,v) => { s.m_params[0].ccdSweptSphereRadius = cf.GetFloat(p, v); },
-            (s) => { return s.m_params[0].ccdSweptSphereRadius; },
-            (s,p,l,v) => { s.UpdateParameterObject(ref s.m_params[0].ccdSweptSphereRadius, p, l, v); },
-            (s,o,v) => { BulletSimAPI.SetCcdSweptSphereRadius2(o.PhysBody.ptr, v); } ),
-        new ParameterDefn("ContactProcessingThreshold", "Distance between contacts before doing collision check" ,
-            0.1f,
-            (s,cf,p,v) => { s.m_params[0].contactProcessingThreshold = cf.GetFloat(p, v); },
-            (s) => { return s.m_params[0].contactProcessingThreshold; },
-            (s,p,l,v) => { s.UpdateParameterObject(ref s.m_params[0].contactProcessingThreshold, p, l, v); },
-            (s,o,v) => { BulletSimAPI.SetContactProcessingThreshold2(o.PhysBody.ptr, v); } ),
-
-            new ParameterDefn("TerrainImplementation", "Type of shape to use for terrain (0=heightmap, 1=mesh)",
-            (float)BSTerrainPhys.TerrainImplementation.Mesh,
-            (s,cf,p,v) => { s.m_params[0].terrainImplementation = cf.GetFloat(p,v); },
-            (s) => { return s.m_params[0].terrainImplementation; },
-            (s,p,l,v) => { s.m_params[0].terrainImplementation = v; } ),
-        new ParameterDefn("TerrainFriction", "Factor to reduce movement against terrain surface" ,
-            0.5f,
-            (s,cf,p,v) => { s.m_params[0].terrainFriction = cf.GetFloat(p, v); },
-            (s) => { return s.m_params[0].terrainFriction; },
-            (s,p,l,v) => { s.m_params[0].terrainFriction = v;  /* TODO: set on real terrain */} ),
-        new ParameterDefn("TerrainHitFraction", "Distance to measure hit collisions" ,
-            0.8f,
-            (s,cf,p,v) => { s.m_params[0].terrainHitFraction = cf.GetFloat(p, v); },
-            (s) => { return s.m_params[0].terrainHitFraction; },
-            (s,p,l,v) => { s.m_params[0].terrainHitFraction = v; /* TODO: set on real terrain */ } ),
-        new ParameterDefn("TerrainRestitution", "Bouncyness" ,
-            0f,
-            (s,cf,p,v) => { s.m_params[0].terrainRestitution = cf.GetFloat(p, v); },
-            (s) => { return s.m_params[0].terrainRestitution; },
-            (s,p,l,v) => { s.m_params[0].terrainRestitution = v;  /* 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) => { s.m_params[0].avatarFriction = cf.GetFloat(p, v); },
-            (s) => { return s.m_params[0].avatarFriction; },
-            (s,p,l,v) => { s.UpdateParameterObject(ref s.m_params[0].avatarFriction, p, l, v); } ),
-        new ParameterDefn("AvatarStandingFriction", "Avatar friction when standing. Changed on avatar recreation.",
-            10f,
-            (s,cf,p,v) => { s.m_params[0].avatarStandingFriction = cf.GetFloat(p, v); },
-            (s) => { return s.m_params[0].avatarStandingFriction; },
-            (s,p,l,v) => { s.m_params[0].avatarStandingFriction = v; } ),
-        new ParameterDefn("AvatarDensity", "Density of an avatar. Changed on avatar recreation.",
-            60f,
-            (s,cf,p,v) => { s.m_params[0].avatarDensity = cf.GetFloat(p, v); },
-            (s) => { return s.m_params[0].avatarDensity; },
-            (s,p,l,v) => { s.UpdateParameterObject(ref s.m_params[0].avatarDensity, p, l, v); } ),
-        new ParameterDefn("AvatarRestitution", "Bouncyness. Changed on avatar recreation.",
-            0f,
-            (s,cf,p,v) => { s.m_params[0].avatarRestitution = cf.GetFloat(p, v); },
-            (s) => { return s.m_params[0].avatarRestitution; },
-            (s,p,l,v) => { s.UpdateParameterObject(ref s.m_params[0].avatarRestitution, p, l, v); } ),
-        new ParameterDefn("AvatarCapsuleWidth", "The distance between the sides of the avatar capsule",
-            0.6f,
-            (s,cf,p,v) => { s.m_params[0].avatarCapsuleWidth = cf.GetFloat(p, v); },
-            (s) => { return s.m_params[0].avatarCapsuleWidth; },
-            (s,p,l,v) => { s.UpdateParameterObject(ref s.m_params[0].avatarCapsuleWidth, p, l, v); } ),
-        new ParameterDefn("AvatarCapsuleDepth", "The distance between the front and back of the avatar capsule",
-            0.45f,
-            (s,cf,p,v) => { s.m_params[0].avatarCapsuleDepth = cf.GetFloat(p, v); },
-            (s) => { return s.m_params[0].avatarCapsuleDepth; },
-            (s,p,l,v) => { s.UpdateParameterObject(ref s.m_params[0].avatarCapsuleDepth, p, l, v); } ),
-        new ParameterDefn("AvatarCapsuleHeight", "Default height of space around avatar",
-            1.5f,
-            (s,cf,p,v) => { s.m_params[0].avatarCapsuleHeight = cf.GetFloat(p, v); },
-            (s) => { return s.m_params[0].avatarCapsuleHeight; },
-            (s,p,l,v) => { s.UpdateParameterObject(ref s.m_params[0].avatarCapsuleHeight, p, l, v); } ),
-            new ParameterDefn("AvatarContactProcessingThreshold", "Distance from capsule to check for collisions",
-            0.1f,
-            (s,cf,p,v) => { s.m_params[0].avatarContactProcessingThreshold = cf.GetFloat(p, v); },
-            (s) => { return s.m_params[0].avatarContactProcessingThreshold; },
-            (s,p,l,v) => { s.UpdateParameterObject(ref s.m_params[0].avatarContactProcessingThreshold, p, l, v); } ),
-
-
-            new ParameterDefn("MaxPersistantManifoldPoolSize", "Number of manifolds pooled (0 means default of 4096)",
-            0f,
-            (s,cf,p,v) => { s.m_params[0].maxPersistantManifoldPoolSize = cf.GetFloat(p, v); },
-            (s) => { return s.m_params[0].maxPersistantManifoldPoolSize; },
-            (s,p,l,v) => { s.m_params[0].maxPersistantManifoldPoolSize = v; } ),
-            new ParameterDefn("MaxCollisionAlgorithmPoolSize", "Number of collisions pooled (0 means default of 4096)",
-            0f,
-            (s,cf,p,v) => { s.m_params[0].maxCollisionAlgorithmPoolSize = cf.GetFloat(p, v); },
-            (s) => { return s.m_params[0].maxCollisionAlgorithmPoolSize; },
-            (s,p,l,v) => { s.m_params[0].maxCollisionAlgorithmPoolSize = v; } ),
-            new ParameterDefn("ShouldDisableContactPoolDynamicAllocation", "Enable to allow large changes in object count",
-            ConfigurationParameters.numericFalse,
-            (s,cf,p,v) => { s.m_params[0].shouldDisableContactPoolDynamicAllocation = s.NumericBool(cf.GetBoolean(p, s.BoolNumeric(v))); },
-            (s) => { return s.m_params[0].shouldDisableContactPoolDynamicAllocation; },
-            (s,p,l,v) => { s.m_params[0].shouldDisableContactPoolDynamicAllocation = v; } ),
-            new ParameterDefn("ShouldForceUpdateAllAabbs", "Enable to recomputer AABBs every simulator step",
-            ConfigurationParameters.numericFalse,
-            (s,cf,p,v) => { s.m_params[0].shouldForceUpdateAllAabbs = s.NumericBool(cf.GetBoolean(p, s.BoolNumeric(v))); },
-            (s) => { return s.m_params[0].shouldForceUpdateAllAabbs; },
-            (s,p,l,v) => { s.m_params[0].shouldForceUpdateAllAabbs = v; } ),
-            new ParameterDefn("ShouldRandomizeSolverOrder", "Enable for slightly better stacking interaction",
-            ConfigurationParameters.numericTrue,
-            (s,cf,p,v) => { s.m_params[0].shouldRandomizeSolverOrder = s.NumericBool(cf.GetBoolean(p, s.BoolNumeric(v))); },
-            (s) => { return s.m_params[0].shouldRandomizeSolverOrder; },
-            (s,p,l,v) => { s.m_params[0].shouldRandomizeSolverOrder = v; } ),
-            new ParameterDefn("ShouldSplitSimulationIslands", "Enable splitting active object scanning islands",
-            ConfigurationParameters.numericTrue,
-            (s,cf,p,v) => { s.m_params[0].shouldSplitSimulationIslands = s.NumericBool(cf.GetBoolean(p, s.BoolNumeric(v))); },
-            (s) => { return s.m_params[0].shouldSplitSimulationIslands; },
-            (s,p,l,v) => { s.m_params[0].shouldSplitSimulationIslands = v; } ),
-            new ParameterDefn("ShouldEnableFrictionCaching", "Enable friction computation caching",
-            ConfigurationParameters.numericFalse,
-            (s,cf,p,v) => { s.m_params[0].shouldEnableFrictionCaching = s.NumericBool(cf.GetBoolean(p, s.BoolNumeric(v))); },
-            (s) => { return s.m_params[0].shouldEnableFrictionCaching; },
-            (s,p,l,v) => { s.m_params[0].shouldEnableFrictionCaching = v; } ),
-            new ParameterDefn("NumberOfSolverIterations", "Number of internal iterations (0 means default)",
-            0f,     // zero says use Bullet default
-            (s,cf,p,v) => { s.m_params[0].numberOfSolverIterations = cf.GetFloat(p, v); },
-            (s) => { return s.m_params[0].numberOfSolverIterations; },
-            (s,p,l,v) => { s.m_params[0].numberOfSolverIterations = v; } ),
-
-            new ParameterDefn("LinksetImplementation", "Type of linkset implementation (0=Constraint, 1=Compound, 2=Manual)",
-            (float)BSLinkset.LinksetImplementation.Compound,
-            (s,cf,p,v) => { s.m_params[0].linksetImplementation = cf.GetFloat(p,v); },
-            (s) => { return s.m_params[0].linksetImplementation; },
-            (s,p,l,v) => { s.m_params[0].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) => { s.m_params[0].linkConstraintUseFrameOffset = s.NumericBool(cf.GetBoolean(p, s.BoolNumeric(v))); },
-            (s) => { return s.m_params[0].linkConstraintUseFrameOffset; },
-            (s,p,l,v) => { s.m_params[0].linkConstraintUseFrameOffset = v; } ),
-            new ParameterDefn("LinkConstraintEnableTransMotor", "Whether to enable translational motor on linkset constraints",
-            ConfigurationParameters.numericTrue,
-            (s,cf,p,v) => { s.m_params[0].linkConstraintEnableTransMotor = s.NumericBool(cf.GetBoolean(p, s.BoolNumeric(v))); },
-            (s) => { return s.m_params[0].linkConstraintEnableTransMotor; },
-            (s,p,l,v) => { s.m_params[0].linkConstraintEnableTransMotor = v; } ),
-            new ParameterDefn("LinkConstraintTransMotorMaxVel", "Maximum velocity to be applied by translational motor in linkset constraints",
-            5.0f,
-            (s,cf,p,v) => { s.m_params[0].linkConstraintTransMotorMaxVel = cf.GetFloat(p, v); },
-            (s) => { return s.m_params[0].linkConstraintTransMotorMaxVel; },
-            (s,p,l,v) => { s.m_params[0].linkConstraintTransMotorMaxVel = v; } ),
-            new ParameterDefn("LinkConstraintTransMotorMaxForce", "Maximum force to be applied by translational motor in linkset constraints",
-            0.1f,
-            (s,cf,p,v) => { s.m_params[0].linkConstraintTransMotorMaxForce = cf.GetFloat(p, v); },
-            (s) => { return s.m_params[0].linkConstraintTransMotorMaxForce; },
-            (s,p,l,v) => { s.m_params[0].linkConstraintTransMotorMaxForce = v; } ),
-            new ParameterDefn("LinkConstraintCFM", "Amount constraint can be violated. 0=no violation, 1=infinite. Default=0.1",
-            0.1f,
-            (s,cf,p,v) => { s.m_params[0].linkConstraintCFM = cf.GetFloat(p, v); },
-            (s) => { return s.m_params[0].linkConstraintCFM; },
-            (s,p,l,v) => { s.m_params[0].linkConstraintCFM = v; } ),
-            new ParameterDefn("LinkConstraintERP", "Amount constraint is corrected each tick. 0=none, 1=all. Default = 0.2",
-            0.1f,
-            (s,cf,p,v) => { s.m_params[0].linkConstraintERP = cf.GetFloat(p, v); },
-            (s) => { return s.m_params[0].linkConstraintERP; },
-            (s,p,l,v) => { s.m_params[0].linkConstraintERP = v; } ),
-            new ParameterDefn("LinkConstraintSolverIterations", "Number of solver iterations when computing constraint. (0 = Bullet default)",
-            40,
-            (s,cf,p,v) => { s.m_params[0].linkConstraintSolverIterations = cf.GetFloat(p, v); },
-            (s) => { return s.m_params[0].linkConstraintSolverIterations; },
-            (s,p,l,v) => { s.m_params[0].linkConstraintSolverIterations = v; } ),
-
-        new ParameterDefn("LogPhysicsStatisticsFrames", "Frames between outputting detailed phys stats. (0 is off)",
-            0f,
-            (s,cf,p,v) => { s.m_params[0].physicsLoggingFrames = cf.GetInt(p, (int)v); },
-            (s) => { return (float)s.m_params[0].physicsLoggingFrames; },
-            (s,p,l,v) => { s.m_params[0].physicsLoggingFrames = (int)v; } ),
-    };
-
-    // Convert a boolean to our numeric true and false values
-    public float NumericBool(bool b)
-    {
-        return (b ? ConfigurationParameters.numericTrue : ConfigurationParameters.numericFalse);
-    }
-
-    // Convert numeric true and false values to a boolean
-    public bool BoolNumeric(float b)
-    {
-        return (b == ConfigurationParameters.numericTrue ? true : false);
-    }
-
-    // Search through the parameter definitions and return the matching
-    //    ParameterDefn structure.
-    // Case does not matter as names are compared after converting to lower case.
-    // Returns 'false' if the parameter is not found.
-    private bool TryGetParameter(string paramName, out ParameterDefn defn)
-    {
-        bool ret = false;
-        ParameterDefn foundDefn = new ParameterDefn();
-        string pName = paramName.ToLower();
-
-        foreach (ParameterDefn parm in ParameterDefinitions)
-        {
-            if (pName == parm.name.ToLower())
-            {
-                foundDefn = parm;
-                ret = true;
-                break;
-            }
-        }
-        defn = foundDefn;
-        return ret;
-    }
-
-    // Pass through the settable parameters and set the default values
-    private void SetParameterDefaultValues()
-    {
-        foreach (ParameterDefn parm in ParameterDefinitions)
-        {
-            parm.setter(this, parm.name, PhysParameterEntry.APPLY_TO_NONE, parm.defaultValue);
-        }
-    }
-
-    // Get user set values out of the ini file.
-    private void SetParameterConfigurationValues(IConfig cfg)
-    {
-        foreach (ParameterDefn parm in ParameterDefinitions)
-        {
-            parm.userParam(this, cfg, parm.name, parm.defaultValue);
-        }
-    }
-
-    private PhysParameterEntry[] SettableParameters = new PhysParameterEntry[1];
-
-    // This creates an array in the correct format for returning the list of
-    //    parameters. This is used by the 'list' option of the 'physics' command.
-    private void BuildParameterTable()
-    {
-        if (SettableParameters.Length < ParameterDefinitions.Length)
-        {
-            List<PhysParameterEntry> entries = new List<PhysParameterEntry>();
-            for (int ii = 0; ii < ParameterDefinitions.Length; ii++)
-            {
-                ParameterDefn pd = ParameterDefinitions[ii];
-                entries.Add(new PhysParameterEntry(pd.name, pd.desc));
-            }
-
-            // make the list in alphabetical order for estetic reasons
-            entries.Sort(delegate(PhysParameterEntry ppe1, PhysParameterEntry ppe2)
-            {
-                return ppe1.name.CompareTo(ppe2.name);
-            });
-
-            SettableParameters = entries.ToArray();
-        }
-    }
-
-
     #region IPhysicsParameters
     // Get the list of parameters this physics engine supports
     public PhysParameterEntry[] GetParameterList()
     {
-        BuildParameterTable();
-        return SettableParameters;
+        BSParam.BuildParameterTable();
+        return BSParam.SettableParameters;
     }
 
     // Set parameter on a specific or all instances.
@@ -1389,63 +876,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;
-        ParameterDefn theParam;
-        if (TryGetParameter(parm, out 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
-    private void UpdateParameterObject(ref float defaultLoc, string parm, uint localID, float val)
-    {
-        List<uint> objectIDs = new List<uint>();
-        switch (localID)
-        {
-            case PhysParameterEntry.APPLY_TO_NONE:
-                defaultLoc = 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:
-                defaultLoc = 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() {
-            ParameterDefn thisParam;
-            if (TryGetParameter(xparm, out 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);
                     }
                 }
             }
@@ -1454,14 +943,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;
-        ParameterDefn theParam;
-        if (TryGetParameter(parm, out theParam))
+        BSParam.ParameterDefnBase theParam;
+        if (BSParam.TryGetParameter(parm, out theParam))
         {
-            val = theParam.getter(this);
+            val = theParam.GetValue(this);
             ret = true;
         }
         value = val;
@@ -1470,24 +959,12 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
 
     #endregion IPhysicsParameters
 
-    #endregion Runtime settable parameters
-
-    // Debugging routine for dumping detailed physical information for vehicle prims
-    private void DumpVehicles()
-    {
-        foreach (BSPrim prim in m_vehicles)
-        {
-            BulletSimAPI.DumpRigidBody2(World.ptr, prim.PhysBody.ptr);
-            BulletSimAPI.DumpCollisionShape2(World.ptr, prim.PhysShape.ptr);
-        }
-    }
-
     // Invoke the detailed logger and output something if it's enabled.
     public void DetailLog(string msg, params Object[] args)
     {
         PhysicsLogging.Write(msg, args);
         // Add the Flush() if debugging crashes. Gets all the messages written out.
-        // PhysicsLogging.Flush();
+        if (m_physicsLoggingDoFlush) PhysicsLogging.Flush();
     }
     // Used to fill in the LocalID when there isn't one. It's the correct number of characters.
     public const string DetailLogZero = "0000000000";
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs b/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs
index 892c34b..220fbbc 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs
@@ -45,7 +45,7 @@ public sealed class BSShapeCollection : IDisposable
     // Description of a Mesh
     private struct MeshDesc
     {
-        public IntPtr ptr;
+        public BulletShape shape;
         public int referenceCount;
         public DateTime lastReferenced;
         public UInt64 shapeKey;
@@ -55,7 +55,7 @@ public sealed class BSShapeCollection : IDisposable
     // Meshes and hulls have the same shape hash key but we only need hulls for efficient collision calculations.
     private struct HullDesc
     {
-        public IntPtr ptr;
+        public BulletShape shape;
         public int referenceCount;
         public DateTime lastReferenced;
         public UInt64 shapeKey;
@@ -65,9 +65,16 @@ public sealed class BSShapeCollection : IDisposable
     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;
+        // 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
+        //     statements can be commented/removed.
+        DDetail = true;
     }
 
     public void Dispose()
@@ -91,7 +98,7 @@ public sealed class BSShapeCollection : IDisposable
     //    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, BulletSim sim, BSPhysObject prim,
+    public bool GetBodyAndShape(bool forceRebuild, BulletWorld sim, BSPhysObject prim,
                     ShapeDestructionCallback shapeCallback, BodyDestructionCallback bodyCallback)
     {
         PhysicsScene.AssertInTaintTime("BSShapeCollection.GetBodyAndShape");
@@ -109,8 +116,7 @@ public sealed class BSShapeCollection : IDisposable
             //    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, PhysicsScene.World, bodyCallback);
             ret = newGeom || newBody;
         }
         DetailLog("{0},BSShapeCollection.GetBodyAndShape,taintExit,force={1},ret={2},body={3},shape={4}",
@@ -119,51 +125,52 @@ public sealed class BSShapeCollection : IDisposable
         return ret;
     }
 
+    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)
+    private void ReferenceBody(BulletBody body)
     {
         lock (m_collectionActivityLock)
         {
-            DetailLog("{0},BSShapeCollection.ReferenceBody,newBody,body={1}", body.ID, body);
-            PhysicsScene.TaintedObject(inTaintTime, "BSShapeCollection.ReferenceBody", delegate()
+            if (DDetail) DetailLog("{0},BSShapeCollection.ReferenceBody,newBody,body={1}", body.ID, body);
+            if (!PhysicsScene.PE.IsInWorld(PhysicsScene.World, body))
             {
-                if (!BulletSimAPI.IsInWorld2(body.ptr))
-                {
-                    BulletSimAPI.AddObjectToWorld2(PhysicsScene.World.ptr, body.ptr);
-                    DetailLog("{0},BSShapeCollection.ReferenceBody,addedToWorld,ref={1}", body.ID, 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 )
+    // Called in taint time.
+    public void DereferenceBody(BulletBody body, BodyDestructionCallback bodyCallback )
     {
-        if (body.ptr == IntPtr.Zero)
+        if (!body.HasPhysicalBody)
             return;
 
+        PhysicsScene.AssertInTaintTime("BSShapeCollection.DereferenceBody");
+
         lock (m_collectionActivityLock)
         {
-            PhysicsScene.TaintedObject(inTaintTime, "BSShapeCollection.DereferenceBody", delegate()
-            {
-                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 (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);
 
-                if (BulletSimAPI.IsInWorld2(body.ptr))
-                {
-                    BulletSimAPI.RemoveObjectFromWorld2(PhysicsScene.World.ptr, body.ptr);
-                    DetailLog("{0},BSShapeCollection.DereferenceBody,removingFromWorld. Body={1}", body.ID, 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.
-                BulletSimAPI.SetCollisionShape2(PhysicsScene.World.ptr, body.ptr, IntPtr.Zero);
-                BulletSimAPI.DestroyObject2(PhysicsScene.World.ptr, body.ptr);
-            });
+            // 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);
         }
     }
 
@@ -184,17 +191,17 @@ public sealed class BSShapeCollection : IDisposable
                 {
                     // There is an existing instance of this mesh.
                     meshDesc.referenceCount++;
-                    DetailLog("{0},BSShapeCollection.ReferenceShape,existingMesh,key={1},cnt={2}",
+                    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.ptr = shape.ptr;
+                    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;
-                    DetailLog("{0},BSShapeCollection.ReferenceShape,newMesh,key={1},cnt={2}",
+                    if (DDetail) DetailLog("{0},BSShapeCollection.ReferenceShape,newMesh,key={1},cnt={2}",
                                 BSScene.DetailLogZero, shape.shapeKey.ToString("X"), meshDesc.referenceCount);
                     ret = true;
                 }
@@ -207,16 +214,16 @@ public sealed class BSShapeCollection : IDisposable
                 {
                     // There is an existing instance of this hull.
                     hullDesc.referenceCount++;
-                    DetailLog("{0},BSShapeCollection.ReferenceShape,existingHull,key={1},cnt={2}",
+                    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.ptr = shape.ptr;
+                    hullDesc.shape = shape.Clone();
                     hullDesc.shapeKey = shape.shapeKey;
                     hullDesc.referenceCount = 1;
-                    DetailLog("{0},BSShapeCollection.ReferenceShape,newHull,key={1},cnt={2}",
+                    if (DDetail) DetailLog("{0},BSShapeCollection.ReferenceShape,newHull,key={1},cnt={2}",
                                 BSScene.DetailLogZero, shape.shapeKey.ToString("X"), hullDesc.referenceCount);
                     ret = true;
 
@@ -234,44 +241,43 @@ public sealed class BSShapeCollection : IDisposable
     }
 
     // Release the usage of a shape.
-    public void DereferenceShape(BulletShape shape, bool inTaintTime, ShapeDestructionCallback shapeCallback)
+    public void DereferenceShape(BulletShape shape, ShapeDestructionCallback shapeCallback)
     {
-        if (shape.ptr == IntPtr.Zero)
+        if (!shape.HasPhysicalShape)
             return;
 
-        PhysicsScene.TaintedObject(inTaintTime, "BSShapeCollection.DereferenceShape", delegate()
+        PhysicsScene.AssertInTaintTime("BSShapeCollection.DereferenceShape");
+
+        if (shape.HasPhysicalShape)
         {
-            if (shape.ptr != IntPtr.Zero)
+            if (shape.isNativeShape)
             {
-                if (shape.isNativeShape)
-                {
-                    // Native shapes are not tracked and are released immediately
-                    DetailLog("{0},BSShapeCollection.DereferenceShape,deleteNativeShape,ptr={1},taintTime={2}",
-                                    BSScene.DetailLogZero, shape.ptr.ToString("X"), inTaintTime);
-                    if (shapeCallback != null) shapeCallback(shape);
-                    BulletSimAPI.DeleteCollisionShape2(PhysicsScene.World.ptr, shape.ptr);
-                }
-                else
+                // Native shapes are not tracked and are released immediately
+                if (DDetail) DetailLog("{0},BSShapeCollection.DereferenceShape,deleteNativeShape,ptr={1}",
+                                    BSScene.DetailLogZero, shape.AddrString);
+                if (shapeCallback != null) shapeCallback(shape);
+                PhysicsScene.PE.DeleteCollisionShape(PhysicsScene.World, shape);
+            }
+            else
+            {
+                switch (shape.type)
                 {
-                    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;
-                    }
+                    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
@@ -286,7 +292,7 @@ public sealed class BSShapeCollection : IDisposable
             if (shapeCallback != null) shapeCallback(shape);
             meshDesc.lastReferenced = System.DateTime.Now;
             Meshes[shape.shapeKey] = meshDesc;
-            DetailLog("{0},BSShapeCollection.DereferenceMesh,shape={1},refCnt={2}",
+            if (DDetail) DetailLog("{0},BSShapeCollection.DereferenceMesh,shape={1},refCnt={2}",
                                 BSScene.DetailLogZero, shape, meshDesc.referenceCount);
 
         }
@@ -307,7 +313,7 @@ public sealed class BSShapeCollection : IDisposable
 
             hullDesc.lastReferenced = System.DateTime.Now;
             Hulls[shape.shapeKey] = hullDesc;
-            DetailLog("{0},BSShapeCollection.DereferenceHull,shape={1},refCnt={2}",
+            if (DDetail) DetailLog("{0},BSShapeCollection.DereferenceHull,shape={1},refCnt={2}",
                     BSScene.DetailLogZero, shape, hullDesc.referenceCount);
         }
     }
@@ -320,57 +326,56 @@ public sealed class BSShapeCollection : IDisposable
     // Called at taint-time.
     private void DereferenceCompound(BulletShape shape, ShapeDestructionCallback shapeCallback)
     {
-        if (!BulletSimAPI.IsCompound2(shape.ptr))
+        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.ptr.ToString("X"));
-            DetailLog("{0},BSShapeCollection.DereferenceCompound,notACompoundShape,type={1},ptr={2}",
-                                        BSScene.DetailLogZero, shape.type, shape.ptr.ToString("X"));
+                                        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 = BulletSimAPI.GetNumberOfCompoundChildren2(shape.ptr);
-        DetailLog("{0},BSShapeCollection.DereferenceCompound,shape={1},children={2}", BSScene.DetailLogZero, shape, numChildren);
+        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--)
         {
-            IntPtr childShape = BulletSimAPI.RemoveChildShapeFromCompoundShapeIndex2(shape.ptr, ii);
+            BulletShape childShape = PhysicsScene.PE.RemoveChildShapeFromCompoundShapeIndex(shape, ii);
             DereferenceAnonCollisionShape(childShape);
         }
-        BulletSimAPI.DeleteCollisionShape2(PhysicsScene.World.ptr, shape.ptr);
+        PhysicsScene.PE.DeleteCollisionShape(PhysicsScene.World, shape);
     }
 
     // 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(IntPtr cShape)
+    private void DereferenceAnonCollisionShape(BulletShape shapeInfo)
     {
         MeshDesc meshDesc;
         HullDesc hullDesc;
 
-        BulletShape shapeInfo = new BulletShape(cShape);
-        if (TryGetMeshByPtr(cShape, out meshDesc))
+        if (TryGetMeshByPtr(shapeInfo, out meshDesc))
         {
             shapeInfo.type = BSPhysicsShapeType.SHAPE_MESH;
             shapeInfo.shapeKey = meshDesc.shapeKey;
         }
         else
         {
-            if (TryGetHullByPtr(cShape, out hullDesc))
+            if (TryGetHullByPtr(shapeInfo, out hullDesc))
             {
                 shapeInfo.type = BSPhysicsShapeType.SHAPE_HULL;
                 shapeInfo.shapeKey = hullDesc.shapeKey;
             }
             else
             {
-                if (BulletSimAPI.IsCompound2(cShape))
+                if (PhysicsScene.PE.IsCompound(shapeInfo))
                 {
                     shapeInfo.type = BSPhysicsShapeType.SHAPE_COMPOUND;
                 }
                 else
                 {
-                    if (BulletSimAPI.IsNativeShape2(cShape))
+                    if (PhysicsScene.PE.IsNativeShape(shapeInfo))
                     {
                         shapeInfo.isNativeShape = true;
                         shapeInfo.type = BSPhysicsShapeType.SHAPE_BOX; // (technically, type doesn't matter)
@@ -379,16 +384,16 @@ public sealed class BSShapeCollection : IDisposable
             }
         }
 
-        DetailLog("{0},BSShapeCollection.DereferenceAnonCollisionShape,shape={1}", BSScene.DetailLogZero, shapeInfo);
+        if (DDetail) DetailLog("{0},BSShapeCollection.DereferenceAnonCollisionShape,shape={1}", BSScene.DetailLogZero, shapeInfo);
 
         if (shapeInfo.type != BSPhysicsShapeType.SHAPE_UNKNOWN)
         {
-            DereferenceShape(shapeInfo, true, null);
+            DereferenceShape(shapeInfo, null);
         }
         else
         {
             PhysicsScene.Logger.ErrorFormat("{0} Could not decypher shape type. Region={1}, addr={2}",
-                                    LogHeader, PhysicsScene.RegionName, cShape.ToString("X"));
+                                    LogHeader, PhysicsScene.RegionName, shapeInfo.AddrString);
         }
     }
 
@@ -408,19 +413,18 @@ public sealed class BSShapeCollection : IDisposable
         if (!haveShape && prim.PreferredPhysicalShape == BSPhysicsShapeType.SHAPE_CAPSULE)
         {
             // an avatar capsule is close to a native shape (it is not shared)
-            ret = GetReferenceToNativeShape(prim, BSPhysicsShapeType.SHAPE_CAPSULE,
-                            FixedShapeKey.KEY_CAPSULE, shapeCallback);
-            DetailLog("{0},BSShapeCollection.CreateGeom,avatarCapsule,shape={1}", prim.LocalID, prim.PhysShape);
+            GetReferenceToNativeShape(prim, BSPhysicsShapeType.SHAPE_CAPSULE, FixedShapeKey.KEY_CAPSULE, shapeCallback);
+            if (DDetail) DetailLog("{0},BSShapeCollection.CreateGeom,avatarCapsule,shape={1}", prim.LocalID, prim.PhysShape);
             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 already been created.
+        // 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);
-            DetailLog("{0},BSShapeCollection.CreateGeom,compoundShape,shape={1}", prim.LocalID, prim.PhysShape);
+            if (DDetail) DetailLog("{0},BSShapeCollection.CreateGeom,compoundShape,shape={1}", prim.LocalID, prim.PhysShape);
             haveShape = true;
         }
 
@@ -432,8 +436,9 @@ public sealed class BSShapeCollection : IDisposable
         return ret;
     }
 
-    // Create a mesh/hull shape or a native shape if 'nativeShapePossible' is 'true'.
-    private bool CreateGeomNonSpecial(bool forceRebuild, BSPhysObject prim, ShapeDestructionCallback shapeCallback)
+    // Create a mesh, hull or native shape.
+    // Return 'true' if the prim's shape was changed.
+    public bool CreateGeomNonSpecial(bool forceRebuild, BSPhysObject prim, ShapeDestructionCallback shapeCallback)
     {
         bool ret = false;
         bool haveShape = false;
@@ -442,46 +447,48 @@ public sealed class BSShapeCollection : IDisposable
 
         // If the prim attributes are simple, this could be a simple Bullet native shape
         if (!haveShape
-                && pbs != null
                 && nativeShapePossible
-                && ((pbs.SculptEntry && !PhysicsScene.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) ) )
-        {
-            // It doesn't look like Bullet scales spheres so make sure the scales are all equal
+                && pbs != null
+                && !pbs.SculptEntry
+                && ((pbs.SculptEntry && !BSParam.ShouldMeshSculptedPrim) || PrimHasNoCuts(pbs)) )
+        {
+            // 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);
+
+            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);
+
+            // 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 != prim.Size
+                        || prim.Scale != scaleOfExistingShape
                         || prim.PhysShape.type != BSPhysicsShapeType.SHAPE_SPHERE
                         )
                 {
                     ret = GetReferenceToNativeShape(prim, BSPhysicsShapeType.SHAPE_SPHERE,
                                             FixedShapeKey.KEY_SPHERE, shapeCallback);
-                    DetailLog("{0},BSShapeCollection.CreateGeom,sphere,force={1},shape={2}",
-                                        prim.LocalID, forceRebuild, prim.PhysShape);
                 }
+                if (DDetail) DetailLog("{0},BSShapeCollection.CreateGeom,sphere,force={1},rebuilt={2},shape={3}",
+                                        prim.LocalID, forceRebuild, ret, prim.PhysShape);
             }
             if (!haveShape && pbs.ProfileShape == ProfileShape.Square && pbs.PathCurve == (byte)Extrusion.Straight)
             {
                 haveShape = true;
                 if (forceRebuild
-                        || prim.Scale != prim.Size
+                        || prim.Scale != scaleOfExistingShape
                         || prim.PhysShape.type != BSPhysicsShapeType.SHAPE_BOX
                         )
                 {
                     ret = GetReferenceToNativeShape( prim, BSPhysicsShapeType.SHAPE_BOX,
                                             FixedShapeKey.KEY_BOX, shapeCallback);
-                    DetailLog("{0},BSShapeCollection.CreateGeom,box,force={1},shape={2}",
-                                        prim.LocalID, forceRebuild, prim.PhysShape);
                 }
+                if (DDetail) DetailLog("{0},BSShapeCollection.CreateGeom,box,force={1},rebuilt={2},shape={3}",
+                                        prim.LocalID, forceRebuild, ret, prim.PhysShape);
             }
         }
 
@@ -494,23 +501,36 @@ public sealed class BSShapeCollection : IDisposable
         return ret;
     }
 
+    // return 'true' if this shape description does not include any cutting or twisting.
+    private 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.
     public bool CreateGeomMeshOrHull(BSPhysObject prim, ShapeDestructionCallback shapeCallback)
     {
 
         bool ret = false;
         // Note that if it's a native shape, the check for physical/non-physical is not
         //     made. Native shapes work in either case.
-        if (prim.IsPhysical && PhysicsScene.ShouldUseHullsForPhysicalObjects)
+        if (prim.IsPhysical && BSParam.ShouldUseHullsForPhysicalObjects)
         {
             // Update prim.BSShape to reference a hull of this shape.
-            ret = GetReferenceToHull(prim,shapeCallback);
-            DetailLog("{0},BSShapeCollection.CreateGeom,hull,shape={1},key={2}",
+            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);
-            DetailLog("{0},BSShapeCollection.CreateGeom,mesh,shape={1},key={2}",
+            if (DDetail) DetailLog("{0},BSShapeCollection.CreateGeom,mesh,shape={1},key={2}",
                                     prim.LocalID, prim.PhysShape, prim.PhysShape.shapeKey.ToString("X"));
         }
         return ret;
@@ -523,14 +543,15 @@ public sealed class BSShapeCollection : IDisposable
                             ShapeDestructionCallback shapeCallback)
     {
         // release any previous shape
-        DereferenceShape(prim.PhysShape, true, shapeCallback);
+        DereferenceShape(prim.PhysShape, shapeCallback);
 
         BulletShape newShape = BuildPhysicalNativeShape(prim, shapeType, shapeKey);
 
         // Don't need to do a 'ReferenceShape()' here because native shapes are not shared.
-        DetailLog("{0},BSShapeCollection.AddNativeShapeToPrim,create,newshape={1},scale={2}",
+        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;
     }
@@ -550,20 +571,17 @@ public sealed class BSShapeCollection : IDisposable
 
         if (shapeType == BSPhysicsShapeType.SHAPE_CAPSULE)
         {
-            // The proper scale has been calculated in the prim.
-            newShape = new BulletShape(
-                        BulletSimAPI.BuildCapsuleShape2(PhysicsScene.World.ptr, 1f, 1f, prim.Scale)
-                        , shapeType);
-            DetailLog("{0},BSShapeCollection.BuiletPhysicalNativeShape,capsule,scale={1}", prim.LocalID, prim.Scale);
+
+            newShape = PhysicsScene.PE.BuildCapsuleShape(PhysicsScene.World, 1f, 1f, prim.Scale);
+            if (DDetail) DetailLog("{0},BSShapeCollection.BuildPhysicalNativeShape,capsule,scale={1}", prim.LocalID, prim.Scale);
         }
         else
         {
             // Native shapes are scaled in Bullet so set the scaling to the size
-            prim.Scale = prim.Size;
-            nativeShapeData.Scale = prim.Scale;
-            newShape = new BulletShape(BulletSimAPI.BuildNativeShape2(PhysicsScene.World.ptr, nativeShapeData), shapeType);
+            newShape = PhysicsScene.PE.BuildNativeShape(PhysicsScene.World, nativeShapeData);
+
         }
-        if (newShape.ptr == IntPtr.Zero)
+        if (!newShape.HasPhysicalShape)
         {
             PhysicsScene.Logger.ErrorFormat("{0} BuildPhysicalNativeShape failed. ID={1}, shape={2}",
                                     LogHeader, prim.LocalID, shapeType);
@@ -580,7 +598,7 @@ public sealed class BSShapeCollection : IDisposable
     // Called at taint-time!
     private bool GetReferenceToMesh(BSPhysObject prim, ShapeDestructionCallback shapeCallback)
     {
-        BulletShape newShape = new BulletShape(IntPtr.Zero);
+        BulletShape newShape = new BulletShape();
 
         float lod;
         System.UInt64 newMeshKey = ComputeShapeKey(prim.Size, prim.BaseShape, out lod);
@@ -589,62 +607,96 @@ public sealed class BSShapeCollection : IDisposable
         if (newMeshKey == prim.PhysShape.shapeKey && prim.PhysShape.type == BSPhysicsShapeType.SHAPE_MESH)
             return false;
 
-        DetailLog("{0},BSShapeCollection.GetReferenceToMesh,create,oldKey={1},newKey={2}",
-                                prim.LocalID, prim.PhysShape.shapeKey.ToString("X"), newMeshKey.ToString("X"));
+        if (DDetail) DetailLog("{0},BSShapeCollection.GetReferenceToMesh,create,oldKey={1},newKey={2},size={3},lod={4}",
+                                prim.LocalID, prim.PhysShape.shapeKey.ToString("X"), newMeshKey.ToString("X"), prim.Size, lod);
 
         // Since we're recreating new, get rid of the reference to the previous shape
-        DereferenceShape(prim.PhysShape, true, shapeCallback);
+        DereferenceShape(prim.PhysShape, shapeCallback);
 
-        newShape = CreatePhysicalMesh(prim.PhysObjectName, newMeshKey, prim.BaseShape, prim.Size, lod);
+        newShape = CreatePhysicalMesh(prim, newMeshKey, prim.BaseShape, prim.Size, lod);
         // Take evasive action if the mesh was not constructed.
         newShape = VerifyMeshCreated(newShape, prim);
 
         ReferenceShape(newShape);
 
-        // meshes are already scaled by the meshmerizer
-        prim.Scale = new OMV.Vector3(1f, 1f, 1f);
         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)
+    private BulletShape CreatePhysicalMesh(BSPhysObject prim, System.UInt64 newMeshKey, PrimitiveBaseShape pbs, OMV.Vector3 size, float lod)
     {
-        IMesh meshData = null;
-        IntPtr meshPtr = IntPtr.Zero;
+        BulletShape newShape = new BulletShape();
+
         MeshDesc meshDesc;
         if (Meshes.TryGetValue(newMeshKey, out meshDesc))
         {
             // If the mesh has already been built just use it.
-            meshPtr = meshDesc.ptr;
+            newShape = meshDesc.shape.Clone();
         }
         else
         {
-            // Pass false for physicalness as this creates some sort of bounding box which we don't need
-            meshData = PhysicsScene.mesher.CreateMesh(objName, pbs, size, lod, false);
+            IMesh meshData = PhysicsScene.mesher.CreateMesh(prim.PhysObjectName, pbs, size, lod, 
+                                        true,
+                                        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
+                                        false   // It's NOT for ODE
+                                        );
 
             if (meshData != null)
             {
+
                 int[] indices = meshData.getIndexListAsInt();
-                List<OMV.Vector3> vertices = meshData.getVertexList();
+                int realIndicesIndex = indices.Length;
+                float[] verticesAsFloats = meshData.getVertexListAsFloat();
 
-                float[] verticesAsFloats = new float[vertices.Count * 3];
-                int vi = 0;
-                foreach (OMV.Vector3 vv in vertices)
+                if (BSParam.ShouldRemoveZeroWidthTriangles)
                 {
-                    verticesAsFloats[vi++] = vv.X;
-                    verticesAsFloats[vi++] = vv.Y;
-                    verticesAsFloats[vi++] = vv.Z;
+                    // 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;
+                        }
+                    }
                 }
+                DetailLog("{0},BSShapeCollection.CreatePhysicalMesh,origTri={1},realTri={2},numVerts={3}",
+                            BSScene.DetailLogZero, indices.Length / 3, realIndicesIndex / 3, verticesAsFloats.Length / 3);
 
-                // m_log.DebugFormat("{0}: BSShapeCollection.CreatePhysicalMesh: calling CreateMesh. lid={1}, key={2}, indices={3}, vertices={4}",
-                //                  LogHeader, prim.LocalID, newMeshKey, indices.Length, vertices.Count);
-
-                meshPtr = BulletSimAPI.CreateMeshShape2(PhysicsScene.World.ptr,
-                                    indices.GetLength(0), indices, vertices.Count, verticesAsFloats);
+                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);
+                }
             }
         }
-        BulletShape newShape = new BulletShape(meshPtr, BSPhysicsShapeType.SHAPE_MESH);
         newShape.shapeKey = newMeshKey;
 
         return newShape;
@@ -652,6 +704,7 @@ public sealed class BSShapeCollection : IDisposable
 
     // See that hull shape exists in the physical world and update prim.BSShape.
     // We could be creating the hull because scale changed or whatever.
+    // Return 'true' if a new hull was built. Otherwise, returning a shared hull instance.
     private bool GetReferenceToHull(BSPhysObject prim, ShapeDestructionCallback shapeCallback)
     {
         BulletShape newShape;
@@ -663,19 +716,18 @@ public sealed class BSShapeCollection : IDisposable
         if (newHullKey == prim.PhysShape.shapeKey && prim.PhysShape.type == BSPhysicsShapeType.SHAPE_HULL)
             return false;
 
-        DetailLog("{0},BSShapeCollection.GetReferenceToHull,create,oldKey={1},newKey={2}",
+        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);
+        DereferenceShape(prim.PhysShape, shapeCallback);
 
         newShape = CreatePhysicalHull(prim.PhysObjectName, newHullKey, prim.BaseShape, prim.Size, lod);
+        // It might not have been created if we're waiting for an asset.
         newShape = VerifyMeshCreated(newShape, prim);
 
         ReferenceShape(newShape);
 
-        // hulls are already scaled by the meshmerizer
-        prim.Scale = new OMV.Vector3(1f, 1f, 1f);
         prim.PhysShape = newShape;
         return true;        // 'true' means a new shape has been added to this prim
     }
@@ -684,18 +736,20 @@ public sealed class BSShapeCollection : IDisposable
     private BulletShape CreatePhysicalHull(string objName, System.UInt64 newHullKey, PrimitiveBaseShape pbs, OMV.Vector3 size, float lod)
     {
 
+        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.
-            hullPtr = hullDesc.ptr;
+            // If the hull shape already has been created, just use the one shared instance.
+            newShape = hullDesc.shape.Clone();
         }
         else
         {
-            // Build a new hull in the physical world
-            // Pass false for physicalness as this creates some sort of bounding box which we don't need
-            IMesh meshData = PhysicsScene.mesher.CreateMesh(objName, pbs, size, lod, false);
+            // Build a new hull in the physical world.
+            // Pass true for physicalness as this prevents the creation of bounding box which is not needed
+            IMesh meshData = PhysicsScene.mesher.CreateMesh(objName, pbs, size, lod, true, false, false, false);
             if (meshData != null)
             {
 
@@ -714,15 +768,35 @@ public sealed class BSShapeCollection : IDisposable
                     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 (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);
 
+                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:
@@ -777,14 +851,13 @@ public sealed class BSShapeCollection : IDisposable
                     }
                 }
                 // create the hull data structure in Bullet
-                hullPtr = BulletSimAPI.CreateHullShape2(PhysicsScene.World.ptr, hullCount, convHulls);
+                newShape = PhysicsScene.PE.CreateHullShape(PhysicsScene.World, hullCount, convHulls);
             }
         }
 
-        BulletShape newShape = new BulletShape(hullPtr, BSPhysicsShapeType.SHAPE_HULL);
         newShape.shapeKey = newHullKey;
 
-        return newShape;        // 'true' means a new shape has been added to this prim
+        return newShape;
     }
 
     // Callback from convex hull creater with a newly created hull.
@@ -803,13 +876,12 @@ public sealed class BSShapeCollection : IDisposable
         // Don't need to do this as the shape is freed when the new root shape is created below.
         // DereferenceShape(prim.PhysShape, true, shapeCallback);
 
-        BulletShape cShape = new BulletShape(
-            BulletSimAPI.CreateCompoundShape2(PhysicsScene.World.ptr, false), BSPhysicsShapeType.SHAPE_COMPOUND);
+        BulletShape cShape = PhysicsScene.PE.CreateCompoundShape(PhysicsScene.World, false);
 
         // Create the shape for the root prim and add it to the compound shape. Cannot be a native shape.
         CreateGeomMeshOrHull(prim, shapeCallback);
-        BulletSimAPI.AddChildShapeToCompoundShape2(cShape.ptr, prim.PhysShape.ptr, OMV.Vector3.Zero, OMV.Quaternion.Identity);
-        DetailLog("{0},BSShapeCollection.GetReferenceToCompoundShape,addRootPrim,compShape={1},rootShape={2}",
+        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);
 
         prim.PhysShape = cShape;
@@ -822,14 +894,19 @@ public sealed class BSShapeCollection : IDisposable
     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 = PhysicsScene.MeshLOD;
+        float lod = BSParam.MeshLOD;
+
+        // prims with curvy internal cuts need higher lod
+        if (pbs.HollowShape == HollowShape.Circle)
+            lod = BSParam.MeshCircularLOD;
+
         if (pbs.SculptEntry)
-            lod = PhysicsScene.SculptLOD;
+            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 > PhysicsScene.MeshMegaPrimThreshold)
-            lod = PhysicsScene.MeshMegaPrimLOD;
+        if (maxAxis > BSParam.MeshMegaPrimThreshold)
+            lod = BSParam.MeshMegaPrimLOD;
 
         retLod = lod;
         return pbs.GetMeshKey(size, lod);
@@ -851,51 +928,88 @@ public sealed class BSShapeCollection : IDisposable
     private BulletShape VerifyMeshCreated(BulletShape newShape, BSPhysObject prim)
     {
         // If the shape was successfully created, nothing more to do
-        if (newShape.ptr != IntPtr.Zero)
+        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)
+        // 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.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);
-
-                        });
-                    }
-                });
+            prim.PrimAssetState = BSPhysObject.PrimAssetCondition.Failed;
+            PhysicsScene.Logger.WarnFormat("{0} Fetched asset would not mesh. {1}, texture={2}",
+                                            LogHeader, prim.PhysObjectName, prim.BaseShape.SculptTexture);
         }
         else
         {
-            if (prim.LastAssetBuildFailed)
+            // 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.Logger.ErrorFormat("{0} Mesh failed to fetch asset. lID={1}, texture={2}",
-                                            LogHeader, prim.LocalID, prim.BaseShape.SculptTexture);
+                DetailLog("{0},BSShapeCollection.VerifyMeshCreated,fetchAsset", prim.LocalID);
+                // Multiple requestors will know we're waiting for this asset
+                prim.PrimAssetState = BSPhysObject.PrimAssetCondition.Waiting;
+
+                BSPhysObject xprim = prim;
+                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)
+                            {
+                                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.ForceBodyShapeRebuild(false /* inTaintTime */);
+                                        assetFound = true;
+                                    }
+                                    else
+                                    {
+                                        mismatchIDs = yprim.BaseShape.SculptTexture.ToString() + "/" + asset.ID;
+                                    }
+                                }
+                                if (assetFound)
+                                    yprim.PrimAssetState = BSPhysObject.PrimAssetCondition.Fetched;
+                                else
+                                    yprim.PrimAssetState = BSPhysObject.PrimAssetCondition.Failed;
+                                DetailLog("{0},BSShapeCollection,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);
+                }
             }
         }
 
-        // 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);
+        // While we wait for the mesh defining asset to be loaded, stick in a simple box for the object.
+        BulletShape fillinShape = BuildPhysicalNativeShape(prim, BSPhysicsShapeType.SHAPE_BOX, FixedShapeKey.KEY_BOX);
+        DetailLog("{0},BSShapeCollection.VerifyMeshCreated,boxTempShape", prim.LocalID);
 
         return fillinShape;
     }
@@ -904,49 +1018,45 @@ public sealed class BSShapeCollection : IDisposable
     // 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, BulletSim sim, BulletShape shape,
-                            BodyDestructionCallback bodyCallback)
+    private bool CreateBody(bool forceRebuild, BSPhysObject prim, BulletWorld sim, BodyDestructionCallback bodyCallback)
     {
         bool ret = false;
 
         // the mesh, hull or native shape must have already been created in Bullet
-        bool mustRebuild = (prim.PhysBody.ptr == IntPtr.Zero);
+        bool mustRebuild = !prim.PhysBody.HasPhysicalBody;
 
         // If there is an existing body, verify it's of an acceptable type.
         // If not a solid object, body is a GhostObject. Otherwise a RigidBody.
         if (!mustRebuild)
         {
-            CollisionObjectTypes bodyType = (CollisionObjectTypes)BulletSimAPI.GetBodyType2(prim.PhysBody.ptr);
+            CollisionObjectTypes bodyType = (CollisionObjectTypes)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;
-            IntPtr bodyPtr = IntPtr.Zero;
             if (prim.IsSolid)
             {
-                bodyPtr = BulletSimAPI.CreateBodyFromShape2(sim.ptr, shape.ptr,
-                                        prim.LocalID, prim.RawPosition, prim.RawOrientation);
-                DetailLog("{0},BSShapeCollection.CreateBody,mesh,ptr={1}", prim.LocalID, bodyPtr.ToString("X"));
+                aBody = PhysicsScene.PE.CreateBodyFromShape(sim, prim.PhysShape, prim.LocalID, prim.RawPosition, prim.RawOrientation);
+                if (DDetail) DetailLog("{0},BSShapeCollection.CreateBody,mesh,body={1}", prim.LocalID, aBody);
             }
             else
             {
-                bodyPtr = BulletSimAPI.CreateGhostFromShape2(sim.ptr, shape.ptr,
-                                        prim.LocalID, prim.RawPosition, prim.RawOrientation);
-                DetailLog("{0},BSShapeCollection.CreateBody,ghost,ptr={1}", prim.LocalID, bodyPtr.ToString("X"));
+                aBody = PhysicsScene.PE.CreateGhostFromShape(sim, prim.PhysShape, prim.LocalID, prim.RawPosition, prim.RawOrientation);
+                if (DDetail) DetailLog("{0},BSShapeCollection.CreateBody,ghost,body={1}", prim.LocalID, aBody);
             }
-            aBody = new BulletBody(prim.LocalID, bodyPtr);
 
-            ReferenceBody(aBody, true);
+            ReferenceBody(aBody);
 
             prim.PhysBody = aBody;
 
@@ -956,13 +1066,13 @@ public sealed class BSShapeCollection : IDisposable
         return ret;
     }
 
-    private bool TryGetMeshByPtr(IntPtr addr, out MeshDesc outDesc)
+    private bool TryGetMeshByPtr(BulletShape shape, out MeshDesc outDesc)
     {
         bool ret = false;
         MeshDesc foundDesc = new MeshDesc();
         foreach (MeshDesc md in Meshes.Values)
         {
-            if (md.ptr == addr)
+            if (md.shape.ReferenceSame(shape))
             {
                 foundDesc = md;
                 ret = true;
@@ -974,13 +1084,13 @@ public sealed class BSShapeCollection : IDisposable
         return ret;
     }
 
-    private bool TryGetHullByPtr(IntPtr addr, out HullDesc outDesc)
+    private bool TryGetHullByPtr(BulletShape shape, out HullDesc outDesc)
     {
         bool ret = false;
         HullDesc foundDesc = new HullDesc();
         foreach (HullDesc hd in Hulls.Values)
         {
-            if (hd.ptr == addr)
+            if (hd.shape.ReferenceSame(shape))
             {
                 foundDesc = hd;
                 ret = true;
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSShapes.cs b/OpenSim/Region/Physics/BulletSPlugin/BSShapes.cs
index 96cd55e..ee18379 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSShapes.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSShapes.cs
@@ -27,24 +27,19 @@
 
 using System;
 using System.Collections.Generic;
-using System.Linq;
 using System.Text;
 
+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; }
     public int referenceCount { get; set; }
     public DateTime lastReferenced { get; set; }
 
     public BSShape()
     {
-        ptr = IntPtr.Zero;
-        type = BSPhysicsShapeType.SHAPE_UNKNOWN;
-        key = 0;
         referenceCount = 0;
         lastReferenced = DateTime.Now;
     }
@@ -63,7 +58,7 @@ public abstract class BSShape
         }
 
         // 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.
+        // This isn't too great a hardship since most of the child shapes will have 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
@@ -71,6 +66,14 @@ public abstract class BSShape
             physicsScene.DetailLog("{0},BSShapeCollection.CreateGeom,compoundShape,shape={1}", prim.LocalID, ret);
         }
 
+        // Avatars have their own unique shape
+        if (ret == null  && prim.PreferredPhysicalShape == BSPhysicsShapeType.SHAPE_AVATAR)
+        {
+            // Getting a reference to a compound shape gets you the compound shape with the root prim shape added
+            ret = BSShapeAvatar.GetReference(prim);
+            physicsScene.DetailLog("{0},BSShapeCollection.CreateGeom,avatarShape,shape={1}", prim.LocalID, ret);
+        }
+
         if (ret == null)
             ret = GetShapeReferenceNonSpecial(physicsScene, forceRebuild, prim);
 
@@ -91,15 +94,17 @@ public abstract class BSShape
     // All shapes have a static call to get a reference to the physical shape
     // protected abstract static BSShape GetReference();
 
+    // Returns a string for debugging that uniquily identifies the memory used by this instance
+    public virtual string AddrString
+    {
+        get { return "unknown"; }
+    }
+
     public override string ToString()
     {
         StringBuilder buff = new StringBuilder();
         buff.Append("<p=");
-        buff.Append(ptr.ToString("X"));
-        buff.Append(",s=");
-        buff.Append(type.ToString());
-        buff.Append(",k=");
-        buff.Append(key.ToString("X"));
+        buff.Append(AddrString);
         buff.Append(",c=");
         buff.Append(referenceCount.ToString());
         buff.Append(">");
@@ -126,7 +131,8 @@ public class BSShapeNative : BSShape
                     BSPhysicsShapeType shapeType, FixedShapeKey shapeKey) 
     {
         // Native shapes are not shared and are always built anew.
-        return new BSShapeNative(physicsScene, prim, shapeType, shapeKey);
+        //return new BSShapeNative(physicsScene, prim, shapeType, shapeKey);
+        return null;
     }
 
     private BSShapeNative(BSScene physicsScene, BSPhysObject prim,
@@ -141,14 +147,15 @@ public class BSShapeNative : BSShape
         nativeShapeData.HullKey = (ulong)shapeKey;
 
        
+        /*
         if (shapeType == BSPhysicsShapeType.SHAPE_CAPSULE)
         {
-            ptr = BulletSimAPI.BuildCapsuleShape2(physicsScene.World.ptr, 1f, 1f, prim.Scale);
+            ptr = PhysicsScene.PE.BuildCapsuleShape(physicsScene.World, 1f, 1f, prim.Scale);
             physicsScene.DetailLog("{0},BSShapeCollection.BuiletPhysicalNativeShape,capsule,scale={1}", prim.LocalID, prim.Scale);
         }
         else
         {
-            ptr = BulletSimAPI.BuildNativeShape2(physicsScene.World.ptr, nativeShapeData);
+            ptr = PhysicsScene.PE.BuildNativeShape(physicsScene.World, nativeShapeData);
         }
         if (ptr == IntPtr.Zero)
         {
@@ -157,15 +164,18 @@ public class BSShapeNative : BSShape
         }
         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);
-        BulletSimAPI.DeleteCollisionShape2(physicsScene.World.ptr, ptr);
+        PhysicsScene.PE.DeleteCollisionShape(physicsScene.World, this);
         ptr = IntPtr.Zero;
         // Garbage collection will free up this instance.
+         */
     }
 }
 
@@ -205,4 +215,143 @@ public class BSShapeCompound : BSShape
     }
     public override void Dereference(BSScene physicsScene) { }
 }
+
+public class BSShapeAvatar : BSShape
+{
+    private static string LogHeader = "[BULLETSIM SHAPE AVATAR]";
+    public BSShapeAvatar() : base()
+    {
+    }
+    public static BSShape GetReference(BSPhysObject prim) 
+    { 
+        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 3ca756c..e4fecc3 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSTerrainHeightmap.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSTerrainHeightmap.cs
@@ -44,7 +44,7 @@ public sealed class BSTerrainHeightmap : BSTerrainPhys
 {
     static string LogHeader = "[BULLETSIM TERRAIN HEIGHTMAP]";
 
-    BulletHeightMapInfo m_mapInfo = null;
+    BulletHMapInfo m_mapInfo = null;
 
     // Constructor to build a default, flat heightmap terrain.
     public BSTerrainHeightmap(BSScene physicsScene, Vector3 regionBase, uint id, Vector3 regionSize)
@@ -58,7 +58,7 @@ public sealed class BSTerrainHeightmap : BSTerrainPhys
         {
             initialMap[ii] = BSTerrainManager.HEIGHT_INITIALIZATION;
         }
-        m_mapInfo = new BulletHeightMapInfo(id, initialMap, IntPtr.Zero);
+        m_mapInfo = new BulletHMapInfo(id, initialMap);
         m_mapInfo.minCoords = minTerrainCoords;
         m_mapInfo.maxCoords = maxTerrainCoords;
         m_mapInfo.terrainRegionBase = TerrainBase;
@@ -72,7 +72,7 @@ public sealed class BSTerrainHeightmap : BSTerrainPhys
                                                     Vector3 minCoords, Vector3 maxCoords)
         : base(physicsScene, regionBase, id)
     {
-        m_mapInfo = new BulletHeightMapInfo(id, initialMap, IntPtr.Zero);
+        m_mapInfo = new BulletHMapInfo(id, initialMap);
         m_mapInfo.minCoords = minCoords;
         m_mapInfo.maxCoords = maxCoords;
         m_mapInfo.minZ = minCoords.Z;
@@ -91,13 +91,11 @@ public sealed class BSTerrainHeightmap : BSTerrainPhys
     // Using the information in m_mapInfo, create the physical representation of the heightmap.
     private void BuildHeightmapTerrain()
     {
-        m_mapInfo.Ptr = BulletSimAPI.CreateHeightMapInfo2(PhysicsScene.World.ptr, m_mapInfo.ID,
-                                m_mapInfo.minCoords, m_mapInfo.maxCoords, 
-                                m_mapInfo.heightMap, BSTerrainManager.TERRAIN_COLLISION_MARGIN);
-
         // Create the terrain shape from the mapInfo
-        m_mapInfo.terrainShape = new BulletShape(BulletSimAPI.CreateTerrainShape2(m_mapInfo.Ptr),
-                                                    BSPhysicsShapeType.SHAPE_TERRAIN);
+        m_mapInfo.terrainShape = 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);
+
 
         // The terrain object initial position is at the center of the object
         Vector3 centerPos;
@@ -105,28 +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 = new BulletBody(m_mapInfo.ID,
-                BulletSimAPI.CreateBodyWithDefaultMotionState2(m_mapInfo.terrainShape.ptr,
-                                            m_mapInfo.ID, centerPos, Quaternion.Identity));
+        m_mapInfo.terrainBody = PhysicsScene.PE.CreateBodyWithDefaultMotionState(m_mapInfo.terrainShape, 
+                                m_mapInfo.ID, centerPos, Quaternion.Identity);
 
         // Set current terrain attributes
-        BulletSimAPI.SetFriction2(m_mapInfo.terrainBody.ptr, PhysicsScene.Params.terrainFriction);
-        BulletSimAPI.SetHitFraction2(m_mapInfo.terrainBody.ptr, PhysicsScene.Params.terrainHitFraction);
-        BulletSimAPI.SetRestitution2(m_mapInfo.terrainBody.ptr, PhysicsScene.Params.terrainRestitution);
-        BulletSimAPI.SetCollisionFlags2(m_mapInfo.terrainBody.ptr, CollisionFlags.CF_STATIC_OBJECT);
+        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);
 
         // Return the new terrain to the world of physical objects
-        BulletSimAPI.AddObjectToWorld2(PhysicsScene.World.ptr, m_mapInfo.terrainBody.ptr);
+        PhysicsScene.PE.AddObjectToWorld(PhysicsScene.World, m_mapInfo.terrainBody);
 
         // redo its bounding box now that it is in the world
-        BulletSimAPI.UpdateSingleAabb2(PhysicsScene.World.ptr, m_mapInfo.terrainBody.ptr);
+        PhysicsScene.PE.UpdateSingleAabb(PhysicsScene.World, m_mapInfo.terrainBody);
 
-        BulletSimAPI.SetCollisionFilterMask2(m_mapInfo.terrainBody.ptr,
-                            (uint)CollisionFilterGroups.TerrainFilter,
-                            (uint)CollisionFilterGroups.TerrainMask);
+        m_mapInfo.terrainBody.collisionType = CollisionType.Terrain;
+        m_mapInfo.terrainBody.ApplyCollisionMask(PhysicsScene);
 
         // Make it so the terrain will not move or be considered for movement.
-        BulletSimAPI.ForceActivationState2(m_mapInfo.terrainBody.ptr, ActivationState.DISABLE_SIMULATION);
+        PhysicsScene.PE.ForceActivationState(m_mapInfo.terrainBody, ActivationState.DISABLE_SIMULATION);
 
         return;
     }
@@ -136,19 +132,18 @@ public sealed class BSTerrainHeightmap : BSTerrainPhys
     {
         if (m_mapInfo != null)
         {
-            if (m_mapInfo.terrainBody.ptr != IntPtr.Zero)
+            if (m_mapInfo.terrainBody.HasPhysicalBody)
             {
-                BulletSimAPI.RemoveObjectFromWorld2(PhysicsScene.World.ptr, m_mapInfo.terrainBody.ptr);
+                PhysicsScene.PE.RemoveObjectFromWorld(PhysicsScene.World, m_mapInfo.terrainBody);
                 // Frees both the body and the shape.
-                BulletSimAPI.DestroyObject2(PhysicsScene.World.ptr, m_mapInfo.terrainBody.ptr);
-                BulletSimAPI.ReleaseHeightMapInfo2(m_mapInfo.Ptr);
+                PhysicsScene.PE.DestroyObject(PhysicsScene.World, m_mapInfo.terrainBody);
             }
         }
         m_mapInfo = null;
     }
 
     // The passed position is relative to the base of the region.
-    public override float GetHeightAtXYZ(Vector3 pos)
+    public override float GetTerrainHeightAtXYZ(Vector3 pos)
     {
         float ret = BSTerrainManager.HEIGHT_GETHEIGHT_RET;
 
@@ -166,5 +161,11 @@ public sealed class BSTerrainHeightmap : BSTerrainPhys
         }
         return ret;
     }
+
+    // The passed position is relative to the base of the region.
+    public override float GetWaterLevelAtXYZ(Vector3 pos)
+    {
+        return PhysicsScene.SimpleWaterLevel;
+    }
 }
 }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSTerrainManager.cs b/OpenSim/Region/Physics/BulletSPlugin/BSTerrainManager.cs
index 23fcfd3..b2fb835 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSTerrainManager.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSTerrainManager.cs
@@ -62,11 +62,12 @@ public abstract class BSTerrainPhys : IDisposable
         ID = id;
     }
     public abstract void Dispose();
-    public abstract float GetHeightAtXYZ(Vector3 pos);
+    public abstract float GetTerrainHeightAtXYZ(Vector3 pos);
+    public abstract float GetWaterLevelAtXYZ(Vector3 pos);
 }
 
 // ==========================================================================================
-public sealed class BSTerrainManager
+public sealed class BSTerrainManager : IDisposable
 {
     static string LogHeader = "[BULLETSIM TERRAIN MANAGER]";
 
@@ -75,13 +76,12 @@ public sealed class BSTerrainManager
     public const float HEIGHT_INITIALIZATION = 24.987f;
     public const float HEIGHT_INITIAL_LASTHEIGHT = 24.876f;
     public const float HEIGHT_GETHEIGHT_RET = 24.765f;
+    public const float WATER_HEIGHT_GETHEIGHT_RET = 19.998f;
 
     // If the min and max height are equal, we reduce the min by this
     //    amount to make sure that a bounding box is built for the terrain.
     public const float HEIGHT_EQUAL_FUDGE = 0.2f;
 
-    public const float TERRAIN_COLLISION_MARGIN = 0.0f;
-
     // Until the whole simulator is changed to pass us the region size, we rely on constants.
     public Vector3 DefaultRegionSize = new Vector3(Constants.RegionSize, Constants.RegionSize, Constants.RegionHeight);
 
@@ -122,41 +122,49 @@ public sealed class BSTerrainManager
         MegaRegionParentPhysicsScene = null;
     }
 
+    public void Dispose()
+    {
+        ReleaseGroundPlaneAndTerrain();
+    }
+
     // Create the initial instance of terrain and the underlying ground plane.
     // This is called from the initialization routine so we presume it is
     //    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, PhysicsScene.RegionName);
         // The ground plane is here to catch things that are trying to drop to negative infinity
-        BulletShape groundPlaneShape = new BulletShape(
-                    BulletSimAPI.CreateGroundPlaneShape2(BSScene.GROUNDPLANE_ID, 1f, TERRAIN_COLLISION_MARGIN),
-                    BSPhysicsShapeType.SHAPE_GROUNDPLANE);
-        m_groundPlane = new BulletBody(BSScene.GROUNDPLANE_ID,
-                        BulletSimAPI.CreateBodyWithDefaultMotionState2(groundPlaneShape.ptr, BSScene.GROUNDPLANE_ID,
-                                                            Vector3.Zero, Quaternion.Identity));
-        BulletSimAPI.AddObjectToWorld2(PhysicsScene.World.ptr, m_groundPlane.ptr);
-        BulletSimAPI.UpdateSingleAabb2(PhysicsScene.World.ptr, m_groundPlane.ptr);
+        BulletShape groundPlaneShape = PhysicsScene.PE.CreateGroundPlaneShape(BSScene.GROUNDPLANE_ID, 1f, BSParam.TerrainCollisionMargin);
+        m_groundPlane = 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);
         // Ground plane does not move
-        BulletSimAPI.ForceActivationState2(m_groundPlane.ptr, ActivationState.DISABLE_SIMULATION);
+        PhysicsScene.PE.ForceActivationState(m_groundPlane, ActivationState.DISABLE_SIMULATION);
         // Everything collides with the ground plane.
-        BulletSimAPI.SetCollisionFilterMask2(m_groundPlane.ptr,
-                        (uint)CollisionFilterGroups.GroundPlaneFilter, (uint)CollisionFilterGroups.GroundPlaneMask);
+        m_groundPlane.collisionType = CollisionType.Groundplane;
+        m_groundPlane.ApplyCollisionMask(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);
+        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()
     {
-        if (m_groundPlane.ptr != IntPtr.Zero)
+        DetailLog("{0},BSTerrainManager.ReleaseGroundPlaneAndTerrain,region={1}", BSScene.DetailLogZero, PhysicsScene.RegionName);
+        if (m_groundPlane.HasPhysicalBody)
         {
-            if (BulletSimAPI.RemoveObjectFromWorld2(PhysicsScene.World.ptr, m_groundPlane.ptr))
+            if (PhysicsScene.PE.RemoveObjectFromWorld(PhysicsScene.World, m_groundPlane))
             {
-                BulletSimAPI.DestroyObject2(PhysicsScene.World.ptr, m_groundPlane.ptr);
+                PhysicsScene.PE.DestroyObject(PhysicsScene.World, m_groundPlane);
             }
-            m_groundPlane.ptr = IntPtr.Zero;
+            m_groundPlane.Clear();
         }
 
         ReleaseTerrain();
@@ -165,17 +173,22 @@ public sealed class BSTerrainManager
     // Release all the terrain we have allocated
     public void ReleaseTerrain()
     {
-        foreach (KeyValuePair<Vector3, BSTerrainPhys> kvp in m_terrains)
+        lock (m_terrains)
         {
-            kvp.Value.Dispose();
+            foreach (KeyValuePair<Vector3, BSTerrainPhys> kvp in m_terrains)
+            {
+                kvp.Value.Dispose();
+            }
+            m_terrains.Clear();
         }
-        m_terrains.Clear();
     }
 
     // The simulator wants to set a new heightmap for the terrain.
     public void SetTerrain(float[] heightMap) {
         float[] localHeightMap = heightMap;
-        PhysicsScene.TaintedObject("TerrainManager.SetTerrain", delegate()
+        // 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()
         {
             if (m_worldOffset != Vector3.Zero && MegaRegionParentPhysicsScene != null)
             {
@@ -185,11 +198,16 @@ public sealed class BSTerrainManager
                 //    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);
+                    // This looks really odd but this region is passing its terrain to its mega-region root region
+                    //    and the creation of the terrain must happen on the root region's taint thread and not
+                    //    my taint thread.
+                    ((BSScene)MegaRegionParentPhysicsScene).PostTaintObject("TerrainManager.SetTerrain.Mega-" + m_worldOffset.ToString(), 0, delegate()
+                    {
+                        ((BSScene)MegaRegionParentPhysicsScene).TerrainManager.UpdateTerrain(
+                                        BSScene.CHILDTERRAIN_ID, localHeightMap,
+                                        m_worldOffset, m_worldOffset + DefaultRegionSize, true /* inTaintTime */);
+                    });
                 }
             }
             else
@@ -198,29 +216,30 @@ public sealed class BSTerrainManager
                 DetailLog("{0},SetTerrain.Existing", BSScene.DetailLogZero);
 
                 UpdateTerrain(BSScene.TERRAIN_ID, localHeightMap,
-                                    m_worldOffset, m_worldOffset + DefaultRegionSize, true);
+                                    m_worldOffset, m_worldOffset + DefaultRegionSize, true /* inTaintTime */);
             }
         });
     }
 
-    // If called with no mapInfo for the terrain, this will create a new mapInfo and terrain
+    // 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)
     {
-        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},inTaintTime={4}",
+                            BSScene.DetailLogZero, id, minCoords, maxCoords, inTaintTime);
 
         // Find high and low points of passed heightmap.
         // The min and max passed in is usually the area objects can be in (maximum
         //     object height, for instance). The terrain wants the bounding box for the
-        //     terrain so we replace passed min and max Z with the actual terrain min/max Z.
+        //     terrain so replace passed min and max Z with the actual terrain min/max Z.
         float minZ = float.MaxValue;
         float maxZ = float.MinValue;
         foreach (float height in heightMap)
@@ -238,15 +257,15 @@ public sealed class BSTerrainManager
 
         Vector3 terrainRegionBase = new Vector3(minCoords.X, minCoords.Y, 0f);
 
-        BSTerrainPhys terrainPhys;
-        if (m_terrains.TryGetValue(terrainRegionBase, out terrainPhys))
+        lock (m_terrains)
         {
-            // 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}",
-                            BSScene.DetailLogZero, id, terrainRegionBase, minCoords, minCoords);
-
-            PhysicsScene.TaintedObject(inTaintTime, "BSScene.UpdateTerrain:UpdateExisting", delegate()
+            BSTerrainPhys terrainPhys;
+            if (m_terrains.TryGetValue(terrainRegionBase, out terrainPhys))
             {
+                // There is already a terrain in this spot. Free the old and build the new.
+                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
                 m_terrains.Remove(terrainRegionBase);
                 // Release any physical memory it may be using.
@@ -254,6 +273,7 @@ public sealed class BSTerrainManager
 
                 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);
 
@@ -271,35 +291,24 @@ public sealed class BSTerrainManager
                     // I hate doing this, but just bail
                     return;
                 }
-            });
-        }
-        else
-        {
-            // We don't know about this terrain so either we are creating a new terrain or
-            //    our mega-prim child is giving us a new terrain to add to the phys world
-
-            // if this is a child terrain, calculate a unique terrain id
-            uint newTerrainID = id;
-            if (newTerrainID >= BSScene.CHILDTERRAIN_ID)
-                newTerrainID = ++m_terrainCount;
-
-            float[] heightMapX = heightMap;
-            Vector3 minCoordsX = minCoords;
-            Vector3 maxCoordsX = maxCoords;
+            }
+            else
+            {
+                // We don't know about this terrain so either we are creating a new terrain or
+                //    our mega-prim child is giving us a new terrain to add to the phys world
 
-            DetailLog("{0},UpdateTerrain:NewTerrain,call,id={1}, minC={2}, maxC={3}",
-                            BSScene.DetailLogZero, newTerrainID, minCoords, minCoords);
+                // if this is a child terrain, calculate a unique terrain id
+                uint newTerrainID = id;
+                if (newTerrainID >= BSScene.CHILDTERRAIN_ID)
+                    newTerrainID = ++m_terrainCount;
 
-            // Code that must happen at taint-time
-            PhysicsScene.TaintedObject(inTaintTime, "BSScene.UpdateTerrain:NewTerrain", delegate()
-            {
-                DetailLog("{0},UpdateTerrain:NewTerrain,taint,baseX={1},baseY={2}", 
-                                            BSScene.DetailLogZero, minCoordsX.X, minCoordsX.Y);
+                DetailLog("{0},BSTerrainManager.UpdateTerrain:NewTerrain,taint,newID={1},minCoord={2},maxCoord={3}",
+                                            BSScene.DetailLogZero, newTerrainID, minCoords, minCoords);
                 BSTerrainPhys newTerrainPhys = BuildPhysicalTerrain(terrainRegionBase, id, heightMap, minCoords, maxCoords);
                 m_terrains.Add(terrainRegionBase, newTerrainPhys);
 
                 m_terrainModified = true;
-            });
+            }
         }
     }
 
@@ -308,9 +317,9 @@ public sealed class BSTerrainManager
     {
         PhysicsScene.Logger.DebugFormat("{0} Terrain for {1}/{2} created with {3}", 
                                             LogHeader, PhysicsScene.RegionName, terrainRegionBase, 
-                                            (BSTerrainPhys.TerrainImplementation)PhysicsScene.Params.terrainImplementation);
+                                            (BSTerrainPhys.TerrainImplementation)BSParam.TerrainImplementation);
         BSTerrainPhys newTerrainPhys = null;
-        switch ((int)PhysicsScene.Params.terrainImplementation)
+        switch ((int)BSParam.TerrainImplementation)
         {
             case (int)BSTerrainPhys.TerrainImplementation.Heightmap:
                 newTerrainPhys = new BSTerrainHeightmap(PhysicsScene, terrainRegionBase, id,
@@ -323,14 +332,68 @@ public sealed class BSTerrainManager
             default:
                 PhysicsScene.Logger.ErrorFormat("{0} Bad terrain implementation specified. Type={1}/{2},Region={3}/{4}",
                                             LogHeader, 
-                                            (int)PhysicsScene.Params.terrainImplementation, 
-                                            PhysicsScene.Params.terrainImplementation,
+                                            (int)BSParam.TerrainImplementation, 
+                                            BSParam.TerrainImplementation,
                                             PhysicsScene.RegionName, terrainRegionBase);
                 break;
         }
         return newTerrainPhys;
     }
 
+    // Return 'true' of this position is somewhere in known physical terrain space
+    public bool IsWithinKnownTerrain(Vector3 pos)
+    {
+        Vector3 terrainBaseXYZ;
+        BSTerrainPhys physTerrain;
+        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,
@@ -341,40 +404,125 @@ public sealed class BSTerrainManager
     private float lastHeightTX = 999999f;
     private float lastHeightTY = 999999f;
     private float lastHeight = HEIGHT_INITIAL_LASTHEIGHT;
-    public float GetTerrainHeightAtXYZ(Vector3 loc)
+    public float GetTerrainHeightAtXYZ(Vector3 pos)
     {
-        float tX = loc.X;
-        float tY = loc.Y;
+        float tX = pos.X;
+        float tY = pos.Y;
         // You'd be surprized at the number of times this routine is called
         //    with the same parameters as last time.
-        if (!m_terrainModified && lastHeightTX == tX && lastHeightTY == tY)
+        if (!m_terrainModified && (lastHeightTX == tX) && (lastHeightTY == tY))
             return lastHeight;
+        m_terrainModified = false;
 
         lastHeightTX = tX;
         lastHeightTY = tY;
         float ret = HEIGHT_GETHEIGHT_RET;
 
-        int offsetX = ((int)(tX / (int)DefaultRegionSize.X)) * (int)DefaultRegionSize.X;
-        int offsetY = ((int)(tY / (int)DefaultRegionSize.Y)) * (int)DefaultRegionSize.Y;
-        Vector3 terrainBaseXYZ = new Vector3(offsetX, offsetY, 0f);
-
+        Vector3 terrainBaseXYZ;
         BSTerrainPhys physTerrain;
-        if (m_terrains.TryGetValue(terrainBaseXYZ, out physTerrain))
+        if (GetTerrainPhysicalAtXYZ(pos, out physTerrain, out terrainBaseXYZ))
         {
-            ret = physTerrain.GetHeightAtXYZ(loc - terrainBaseXYZ);
-            DetailLog("{0},BSTerrainManager.GetTerrainHeightAtXYZ,loc={1},base={2},height={3}",
-                                             BSScene.DetailLogZero, loc, terrainBaseXYZ, ret);
+            ret = physTerrain.GetTerrainHeightAtXYZ(pos - terrainBaseXYZ);
         }
         else
         {
             PhysicsScene.Logger.ErrorFormat("{0} GetTerrainHeightAtXY: terrain not found: region={1}, x={2}, y={3}",
                     LogHeader, PhysicsScene.RegionName, tX, tY);
+            DetailLog("{0},BSTerrainManager.GetTerrainHeightAtXYZ,terrainNotFound,pos={1},base={2}",
+                                BSScene.DetailLogZero, pos, terrainBaseXYZ);
         }
-        m_terrainModified = false;
+
         lastHeight = ret;
         return ret;
     }
 
+    public float GetWaterLevelAtXYZ(Vector3 pos)
+    {
+        float ret = WATER_HEIGHT_GETHEIGHT_RET;
+
+        Vector3 terrainBaseXYZ;
+        BSTerrainPhys physTerrain;
+        if (GetTerrainPhysicalAtXYZ(pos, out physTerrain, out terrainBaseXYZ))
+        {
+            ret = physTerrain.GetWaterLevelAtXYZ(pos);
+        }
+        else
+        {
+            PhysicsScene.Logger.ErrorFormat("{0} GetWaterHeightAtXY: terrain not found: pos={1}, terrainBase={2}, height={3}",
+                    LogHeader, PhysicsScene.RegionName, pos, terrainBaseXYZ, ret);
+        }
+        return ret;
+    }
+
+    // Given an address, return 'true' of there is a description of that terrain and output
+    //    the descriptor class and the 'base' fo the addresses therein.
+    private bool GetTerrainPhysicalAtXYZ(Vector3 pos, out BSTerrainPhys outPhysTerrain, out Vector3 outTerrainBase)
+    {
+        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)
+        {
+            ret = m_terrains.TryGetValue(terrainBaseXYZ, out physTerrain);
+        }
+        outTerrainBase = terrainBaseXYZ;
+        outPhysTerrain = physTerrain;
+        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.
     public bool SupportsCombining()
     {
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSTerrainMesh.cs b/OpenSim/Region/Physics/BulletSPlugin/BSTerrainMesh.cs
index dca7150..2ce1513 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSTerrainMesh.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSTerrainMesh.cs
@@ -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(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(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.DetailLog("{0},BSTerrainMesh.create,failedConversionOfHeightmap,id={1}", BSScene.DetailLogZero, ID);
             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;
         }
 
-        m_terrainShape = new BulletShape(BulletSimAPI.CreateMeshShape2(PhysicsScene.World.ptr,
-                                        indicesCount, indices, verticesCount, vertices),
-                                        BSPhysicsShapeType.SHAPE_MESH);
-        if (m_terrainShape.ptr == IntPtr.Zero)
+        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 = PhysicsScene.PE.CreateMeshShape(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);
+            PhysicsScene.DetailLog("{0},BSTerrainMesh.create,failedCreationOfShape,id={1}", BSScene.DetailLogZero, ID);
+            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,49 +120,58 @@ public sealed class BSTerrainMesh : BSTerrainPhys
         Vector3 pos = regionBase;
         Quaternion rot = Quaternion.Identity;
 
-        m_terrainBody = new BulletBody(id, BulletSimAPI.CreateBodyWithDefaultMotionState2( m_terrainShape.ptr, ID, pos, rot));
-        if (m_terrainBody.ptr == IntPtr.Zero)
+        m_terrainBody = 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);
+            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
-        BulletSimAPI.SetFriction2(m_terrainBody.ptr, PhysicsScene.Params.terrainFriction);
-        BulletSimAPI.SetHitFraction2(m_terrainBody.ptr, PhysicsScene.Params.terrainHitFraction);
-        BulletSimAPI.SetRestitution2(m_terrainBody.ptr, PhysicsScene.Params.terrainRestitution);
-        BulletSimAPI.SetCollisionFlags2(m_terrainBody.ptr, CollisionFlags.CF_STATIC_OBJECT);
+        PhysicsScene.PE.SetFriction(m_terrainBody, BSParam.TerrainFriction);
+        PhysicsScene.PE.SetHitFraction(m_terrainBody, BSParam.TerrainHitFraction);
+        PhysicsScene.PE.SetRestitution(m_terrainBody, BSParam.TerrainRestitution);
+        PhysicsScene.PE.SetContactProcessingThreshold(m_terrainBody, BSParam.TerrainContactProcessingThreshold);
+        PhysicsScene.PE.SetCollisionFlags(m_terrainBody, CollisionFlags.CF_STATIC_OBJECT);
 
         // Static objects are not very massive.
-        BulletSimAPI.SetMassProps2(m_terrainBody.ptr, 0f, Vector3.Zero);
+        PhysicsScene.PE.SetMassProps(m_terrainBody, 0f, Vector3.Zero);
 
-        // Return the new terrain to the world of physical objects
-        BulletSimAPI.AddObjectToWorld2(PhysicsScene.World.ptr, m_terrainBody.ptr);
+        // Put the new terrain to the world of physical objects
+        PhysicsScene.PE.AddObjectToWorld(PhysicsScene.World, m_terrainBody);
 
-        // redo its bounding box now that it is in the world
-        BulletSimAPI.UpdateSingleAabb2(PhysicsScene.World.ptr, m_terrainBody.ptr);
+        // Redo its bounding box now that it is in the world
+        PhysicsScene.PE.UpdateSingleAabb(PhysicsScene.World, m_terrainBody);
 
-        BulletSimAPI.SetCollisionFilterMask2(m_terrainBody.ptr,
-                            (uint)CollisionFilterGroups.TerrainFilter,
-                            (uint)CollisionFilterGroups.TerrainMask);
+        m_terrainBody.collisionType = CollisionType.Terrain;
+        m_terrainBody.ApplyCollisionMask(PhysicsScene);
+
+        if (BSParam.UseSingleSidedMeshes)
+        {
+            PhysicsScene.DetailLog("{0},BSTerrainMesh.settingCustomMaterial,id={1}", BSScene.DetailLogZero, id);
+            PhysicsScene.PE.AddToCollisionFlags(m_terrainBody, CollisionFlags.CF_CUSTOM_MATERIAL_CALLBACK);
+        }
 
         // Make it so the terrain will not move or be considered for movement.
-        BulletSimAPI.ForceActivationState2(m_terrainBody.ptr, ActivationState.DISABLE_SIMULATION);
+        PhysicsScene.PE.ForceActivationState(m_terrainBody, ActivationState.DISABLE_SIMULATION);
     }
 
     public override void Dispose()
     {
-        if (m_terrainBody.ptr != IntPtr.Zero)
+        if (m_terrainBody.HasPhysicalBody)
         {
-            BulletSimAPI.RemoveObjectFromWorld2(PhysicsScene.World.ptr, m_terrainBody.ptr);
+            PhysicsScene.PE.RemoveObjectFromWorld(PhysicsScene.World, m_terrainBody);
             // Frees both the body and the shape.
-            BulletSimAPI.DestroyObject2(PhysicsScene.World.ptr, m_terrainBody.ptr);
+            PhysicsScene.PE.DestroyObject(PhysicsScene.World, m_terrainBody);
+            m_terrainBody.Clear();
+            m_terrainShape.Clear();
         }
     }
 
-    public override float GetHeightAtXYZ(Vector3 pos)
+    public override float GetTerrainHeightAtXYZ(Vector3 pos)
     {
         // For the moment use the saved heightmap to get the terrain height.
         // TODO: raycast downward to find the true terrain below the position.
@@ -167,14 +192,17 @@ public sealed class BSTerrainMesh : BSTerrainPhys
         return ret;
     }
 
+    // The passed position is relative to the base of the region.
+    public override float GetWaterLevelAtXYZ(Vector3 pos)
+    {
+        return 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)
     {
@@ -188,43 +216,47 @@ public sealed class BSTerrainMesh : BSTerrainPhys
         // Simple mesh creation which assumes magnification == 1.
         // TODO: do a more general solution that scales, adds new vertices and smoothes the result.
 
+        // Create an array of vertices that is sizeX+1 by sizeY+1 (note the loop
+        //    from zero to <= sizeX). The triangle indices are then generated as two triangles
+        //    per heightmap point. There are sizeX by sizeY 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
         {
-            // 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++)
             {
-                for (int xx = 0; xx <= sizeX; xx++)     // Hint: the "<=" means we got through sizeX + 1 times
+                for (int xx = 0; xx <= sizeX; xx++)     // Hint: the "<=" means we go around sizeX + 1 times
                 {
                     int offset = yy * sizeX + xx;
-                    // Extend the height from the height from the last row or column
+                    // Extend the height with the height from the last row or column
                     if (yy == sizeY) offset -= sizeX;
                     if (xx == sizeX) offset -= 1;
                     float height = heightMap[offset];
-                    vertices[verticesCount + 0] = (float)xx * magX + extentBase.X;
-                    vertices[verticesCount + 1] = (float)yy * magY + extentBase.Y;
+                    minHeight = Math.Min(minHeight, height);
+                    vertices[verticesCount + 0] = (float)xx + extentBase.X;
+                    vertices[verticesCount + 1] = (float)yy + extentBase.Y;
                     vertices[verticesCount + 2] = height + extentBase.Z;
                     verticesCount += 3;
                 }
             }
             verticesCount = verticesCount / 3;
-            physicsScene.DetailLog("{0},BSTerrainMesh.ConvertHeightMapToMesh,completeVerts,verCount={1}", 
-                                            BSScene.DetailLogZero, verticesCount);
 
             for (int yy = 0; yy < sizeY; yy++)
             {
                 for (int xx = 0; xx < sizeX; xx++)
                 {
-                    int offset = yy * sizeX + xx;
+                    int offset = yy * (sizeX + 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;
@@ -235,13 +267,166 @@ public sealed class BSTerrainMesh : BSTerrainPhys
                     indicesCount += 6;
                 }
             }
-            physicsScene.DetailLog("{0},BSTerrainMesh.ConvertHeightMapToMesh,completeIndices,indCount={1}",   // DEEBUG DEBUG DEBUG
-                                                        LogHeader, indicesCount);                        // DEBUG
+
+            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);
+        }
+
+        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)
         {
-            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);
         }
 
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs b/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
deleted file mode 100644
index e60a760..0000000
--- a/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
+++ /dev/null
@@ -1,1015 +0,0 @@
-/*
- * 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.Runtime.InteropServices;
-using System.Security;
-using System.Text;
-using OpenMetaverse;
-
-namespace OpenSim.Region.Physics.BulletSPlugin {
-
-// Classes to allow some type checking for the API
-// These hold pointers to allocated objects in the unmanaged space.
-
-// The physics engine controller class created at initialization
-public struct BulletSim
-{
-    public BulletSim(uint worldId, BSScene bss, IntPtr xx)
-    {
-        ptr = xx;
-        worldID = worldId;
-        physicsScene = bss;
-    }
-    public IntPtr ptr;
-    public uint worldID;
-    // The scene is only in here so very low level routines have a handle to print debug/error messages
-    public BSScene physicsScene;
-}
-
-// An allocated Bullet btRigidBody
-public struct BulletBody
-{
-    public BulletBody(uint id, IntPtr xx)
-    {
-        ID = id;
-        ptr = xx;
-        collisionFilter = 0;
-        collisionMask = 0;
-    }
-    public IntPtr ptr;
-    public uint ID;
-    public CollisionFilterGroups collisionFilter;
-    public CollisionFilterGroups collisionMask;
-    public override string ToString()
-    {
-        StringBuilder buff = new StringBuilder();
-        buff.Append("<id=");
-        buff.Append(ID.ToString());
-        buff.Append(",p=");
-        buff.Append(ptr.ToString("X"));
-        if (collisionFilter != 0 || collisionMask != 0)
-        {
-            buff.Append(",f=");
-            buff.Append(collisionFilter.ToString("X"));
-            buff.Append(",m=");
-            buff.Append(collisionMask.ToString("X"));
-        }
-        buff.Append(">");
-        return buff.ToString();
-    }
-}
-
-public struct BulletShape
-{
-    public BulletShape(IntPtr xx)
-    {
-        ptr = xx;
-        type=BSPhysicsShapeType.SHAPE_UNKNOWN;
-        shapeKey = (System.UInt64)FixedShapeKey.KEY_NONE;
-        isNativeShape = false;
-    }
-    public BulletShape(IntPtr xx, BSPhysicsShapeType typ)
-    {
-        ptr = xx;
-        type = typ;
-        shapeKey = 0;
-        isNativeShape = false;
-    }
-    public IntPtr ptr;
-    public BSPhysicsShapeType type;
-    public System.UInt64 shapeKey;
-    public bool isNativeShape;
-    public override string ToString()
-    {
-        StringBuilder buff = new StringBuilder();
-        buff.Append("<p=");
-        buff.Append(ptr.ToString("X"));
-        buff.Append(",s=");
-        buff.Append(type.ToString());
-        buff.Append(",k=");
-        buff.Append(shapeKey.ToString("X"));
-        buff.Append(",n=");
-        buff.Append(isNativeShape.ToString());
-        buff.Append(">");
-        return buff.ToString();
-    }
-}
-
-    // Constraint type values as defined by Bullet
-public enum ConstraintType : int
-{
-        POINT2POINT_CONSTRAINT_TYPE = 3,
-        HINGE_CONSTRAINT_TYPE,
-        CONETWIST_CONSTRAINT_TYPE,
-        D6_CONSTRAINT_TYPE,
-        SLIDER_CONSTRAINT_TYPE,
-        CONTACT_CONSTRAINT_TYPE,
-        D6_SPRING_CONSTRAINT_TYPE,
-        MAX_CONSTRAINT_TYPE
-}
-
-// An allocated Bullet btConstraint
-public struct BulletConstraint
-{
-    public BulletConstraint(IntPtr xx)
-    {
-        ptr = xx;
-    }
-    public IntPtr ptr;
-}
-
-// An allocated HeightMapThing which holds various heightmap info.
-// Made a class rather than a struct so there would be only one
-//      instance of this and C# will pass around pointers rather
-//      than making copies.
-public class BulletHeightMapInfo
-{
-    public BulletHeightMapInfo(uint id, float[] hm, IntPtr xx) {
-        ID = id;
-        Ptr = xx;
-        heightMap = hm;
-        terrainRegionBase = Vector3.Zero;
-        minCoords = new Vector3(100f, 100f, 25f);
-        maxCoords = new Vector3(101f, 101f, 26f);
-        minZ = maxZ = 0f;
-        sizeX = sizeY = 256f;
-    }
-    public uint ID;
-    public IntPtr Ptr;
-    public float[] heightMap;
-    public Vector3 terrainRegionBase;
-    public Vector3 minCoords;
-    public Vector3 maxCoords;
-    public float sizeX, sizeY;
-    public float minZ, maxZ;
-    public BulletShape terrainShape;
-    public BulletBody terrainBody;
-}
-
-// ===============================================================================
-[StructLayout(LayoutKind.Sequential)]
-public struct ConvexHull
-{
-        Vector3 Offset;
-        int VertexCount;
-        Vector3[] Vertices;
-}
-public enum BSPhysicsShapeType
-{
-        SHAPE_UNKNOWN   = 0,
-        SHAPE_CAPSULE   = 1,
-        SHAPE_BOX       = 2,
-        SHAPE_CONE      = 3,
-        SHAPE_CYLINDER  = 4,
-        SHAPE_SPHERE    = 5,
-        SHAPE_MESH      = 6,
-        SHAPE_HULL      = 7,
-    // following defined by BulletSim
-        SHAPE_GROUNDPLANE  = 20,
-        SHAPE_TERRAIN   = 21,
-        SHAPE_COMPOUND  = 22,
-        SHAPE_HEIGHTMAP = 23,
-};
-
-// The native shapes have predefined shape hash keys
-public enum FixedShapeKey : ulong
-{
-    KEY_NONE        = 0,
-    KEY_BOX         = 1,
-    KEY_SPHERE      = 2,
-    KEY_CONE        = 3,
-    KEY_CYLINDER    = 4,
-    KEY_CAPSULE     = 5,
-}
-
-[StructLayout(LayoutKind.Sequential)]
-public struct ShapeData
-{
-    public uint ID;
-    public BSPhysicsShapeType Type;
-    public Vector3 Position;
-    public Quaternion Rotation;
-    public Vector3 Velocity;
-    public Vector3 Scale;
-    public float Mass;
-    public float Buoyancy;
-    public System.UInt64 HullKey;
-    public System.UInt64 MeshKey;
-    public float Friction;
-    public float Restitution;
-    public float Collidable;    // true of things bump into this
-    public float Static;        // true if a static object. Otherwise gravity, etc.
-    public float Solid;         // true if object cannot be passed through
-    public Vector3 Size;
-
-    // note that bools are passed as floats since bool size changes by language and architecture
-    public const float numericTrue = 1f;
-    public const float numericFalse = 0f;
-}
-[StructLayout(LayoutKind.Sequential)]
-public struct SweepHit
-{
-    public uint ID;
-    public float Fraction;
-    public Vector3 Normal;
-    public Vector3 Point;
-}
-[StructLayout(LayoutKind.Sequential)]
-public struct RaycastHit
-{
-    public uint ID;
-    public float Fraction;
-    public Vector3 Normal;
-}
-[StructLayout(LayoutKind.Sequential)]
-public struct CollisionDesc
-{
-    public uint aID;
-    public uint bID;
-    public Vector3 point;
-    public Vector3 normal;
-}
-[StructLayout(LayoutKind.Sequential)]
-public struct EntityProperties
-{
-    public uint ID;
-    public Vector3 Position;
-    public Quaternion Rotation;
-    public Vector3 Velocity;
-    public Vector3 Acceleration;
-    public Vector3 RotationalVelocity;
-}
-
-// Format of this structure must match the definition in the C++ code
-[StructLayout(LayoutKind.Sequential)]
-public struct ConfigurationParameters
-{
-    public float defaultFriction;
-    public float defaultDensity;
-    public float defaultRestitution;
-    public float collisionMargin;
-    public float gravity;
-
-    public float linearDamping;
-    public float angularDamping;
-    public float deactivationTime;
-    public float linearSleepingThreshold;
-    public float angularSleepingThreshold;
-        public float ccdMotionThreshold;
-        public float ccdSweptSphereRadius;
-    public float contactProcessingThreshold;
-
-    public float terrainImplementation;
-    public float terrainFriction;
-    public float terrainHitFraction;
-    public float terrainRestitution;
-    public float avatarFriction;
-    public float avatarStandingFriction;
-    public float avatarDensity;
-    public float avatarRestitution;
-    public float avatarCapsuleWidth;
-    public float avatarCapsuleDepth;
-    public float avatarCapsuleHeight;
-        public float avatarContactProcessingThreshold;
-
-        public float maxPersistantManifoldPoolSize;
-        public float maxCollisionAlgorithmPoolSize;
-        public float shouldDisableContactPoolDynamicAllocation;
-        public float shouldForceUpdateAllAabbs;
-        public float shouldRandomizeSolverOrder;
-        public float shouldSplitSimulationIslands;
-        public float shouldEnableFrictionCaching;
-        public float numberOfSolverIterations;
-
-    public float linksetImplementation;
-    public float linkConstraintUseFrameOffset;
-    public float linkConstraintEnableTransMotor;
-    public float linkConstraintTransMotorMaxVel;
-    public float linkConstraintTransMotorMaxForce;
-    public float linkConstraintERP;
-    public float linkConstraintCFM;
-    public float linkConstraintSolverIterations;
-
-    public float physicsLoggingFrames;
-
-    public const float numericTrue = 1f;
-    public const float numericFalse = 0f;
-}
-
-
-// The states a bullet collision object can have
-public enum ActivationState : uint
-{
-    ACTIVE_TAG = 1,
-    ISLAND_SLEEPING,
-    WANTS_DEACTIVATION,
-    DISABLE_DEACTIVATION,
-    DISABLE_SIMULATION,
-}
-
-public enum CollisionObjectTypes : int
-{
-    CO_COLLISION_OBJECT             = 1 << 0,
-    CO_RIGID_BODY                   = 1 << 1,
-    CO_GHOST_OBJECT                 = 1 << 2,
-    CO_SOFT_BODY                    = 1 << 3,
-    CO_HF_FLUID                     = 1 << 4,
-    CO_USER_TYPE                    = 1 << 5,
-}
-
-// Values used by Bullet and BulletSim to control object properties.
-// Bullet's "CollisionFlags" has more to do with operations on the
-//    object (if collisions happen, if gravity effects it, ...).
-public enum CollisionFlags : uint
-{
-    CF_STATIC_OBJECT                 = 1 << 0,
-    CF_KINEMATIC_OBJECT              = 1 << 1,
-    CF_NO_CONTACT_RESPONSE           = 1 << 2,
-    CF_CUSTOM_MATERIAL_CALLBACK      = 1 << 3,
-    CF_CHARACTER_OBJECT              = 1 << 4,
-    CF_DISABLE_VISUALIZE_OBJECT      = 1 << 5,
-    CF_DISABLE_SPU_COLLISION_PROCESS = 1 << 6,
-    // Following used by BulletSim to control collisions
-    BS_SUBSCRIBE_COLLISION_EVENTS    = 1 << 10,
-    BS_FLOATS_ON_WATER               = 1 << 11,
-    BS_NONE                          = 0,
-    BS_ALL                           = 0xFFFFFFFF,
-
-    // These are the collision flags switched depending on physical state.
-    // The other flags are used for other things and should not be fooled with.
-    BS_ACTIVE = CF_STATIC_OBJECT
-                | CF_KINEMATIC_OBJECT
-                | CF_NO_CONTACT_RESPONSE
-};
-
-// Values for collisions groups and masks
-public enum CollisionFilterGroups : uint
-{
-    // Don't use the bit definitions!!  Define the use in a
-    //   filter/mask definition below. This way collision interactions
-    //   are more easily debugged.
-    BNoneFilter              = 0,
-    BDefaultFilter           = 1 << 0,
-    BStaticFilter            = 1 << 1,
-    BKinematicFilter         = 1 << 2,
-    BDebrisFilter            = 1 << 3,
-    BSensorTrigger           = 1 << 4,
-    BCharacterFilter         = 1 << 5,
-    BAllFilter               = 0xFFFFFFFF,
-    // Filter groups defined by BulletSim
-    BGroundPlaneFilter       = 1 << 10,
-    BTerrainFilter           = 1 << 11,
-    BRaycastFilter           = 1 << 12,
-    BSolidFilter             = 1 << 13,
-    BLinksetFilter           = 1 << 14,
-
-    // The collsion filters and masked are defined in one place -- don't want them scattered
-    AvatarFilter            = BCharacterFilter,
-    AvatarMask              = BAllFilter,
-    ObjectFilter            = BSolidFilter,
-    ObjectMask              = BAllFilter,
-    StaticObjectFilter      = BStaticFilter,
-    StaticObjectMask        = BAllFilter & ~BStaticFilter,   // static objects don't collide with each other
-    LinksetFilter           = BLinksetFilter,
-    LinksetMask             = BAllFilter & ~BLinksetFilter, // linkset objects don't collide with each other
-    VolumeDetectFilter      = BSensorTrigger,
-    VolumeDetectMask        = ~BSensorTrigger,
-    TerrainFilter           = BTerrainFilter,
-    TerrainMask             = BAllFilter & ~BStaticFilter,  // static objects on the ground don't collide
-    GroundPlaneFilter       = BGroundPlaneFilter,
-    GroundPlaneMask         = BAllFilter
-
-};
-
-// CFM controls the 'hardness' of the constraint. 0=fixed, 0..1=violatable. Default=0
-// ERP controls amount of correction per tick. Usable range=0.1..0.8. Default=0.2.
-public enum ConstraintParams : int
-{
-    BT_CONSTRAINT_ERP = 1,  // this one is not used in Bullet as of 20120730
-    BT_CONSTRAINT_STOP_ERP,
-    BT_CONSTRAINT_CFM,
-    BT_CONSTRAINT_STOP_CFM,
-};
-public enum ConstraintParamAxis : int
-{
-    AXIS_LINEAR_X = 0,
-    AXIS_LINEAR_Y,
-    AXIS_LINEAR_Z,
-    AXIS_ANGULAR_X,
-    AXIS_ANGULAR_Y,
-    AXIS_ANGULAR_Z,
-    AXIS_LINEAR_ALL = 20,    // these last three added by BulletSim so we don't have to do zillions of calls
-    AXIS_ANGULAR_ALL,
-    AXIS_ALL
-};
-
-// ===============================================================================
-static class BulletSimAPI {
-
-// Link back to the managed code for outputting log messages
-[UnmanagedFunctionPointer(CallingConvention.Cdecl)]
-public delegate void DebugLogCallback([MarshalAs(UnmanagedType.LPStr)]string msg);
-
-// ===============================================================================
-// Initialization and simulation
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern IntPtr Initialize2(Vector3 maxPosition, IntPtr parms,
-                                                                                        int maxCollisions,  IntPtr collisionArray,
-                                                                                        int maxUpdates, IntPtr updateArray,
-                                            DebugLogCallback logRoutine);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool UpdateParameter2(IntPtr world, uint localID, String parm, float value);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void SetHeightMap2(IntPtr world, float[] heightmap);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void Shutdown2(IntPtr sim);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern int PhysicsStep2(IntPtr world, float timeStep, int maxSubSteps, float fixedTimeStep,
-                        out int updatedEntityCount,
-                        out IntPtr updatedEntitiesPtr,
-                        out int collidersCount,
-                        out IntPtr collidersPtr);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool PushUpdate2(IntPtr obj);
-
-// =====================================================================================
-// Mesh, hull, shape and body creation helper routines
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern IntPtr CreateMeshShape2(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 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);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern IntPtr BuildNativeShape2(IntPtr world, ShapeData shapeData);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool IsNativeShape2(IntPtr shape);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern IntPtr BuildCapsuleShape2(IntPtr world, float radius, float height, Vector3 scale);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern IntPtr CreateCompoundShape2(IntPtr sim, bool enableDynamicAabbTree);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern int GetNumberOfCompoundChildren2(IntPtr cShape);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void AddChildShapeToCompoundShape2(IntPtr cShape, IntPtr addShape, Vector3 pos, Quaternion rot);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern IntPtr GetChildShapeFromCompoundShapeIndex2(IntPtr cShape, int indx);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern IntPtr RemoveChildShapeFromCompoundShapeIndex2(IntPtr cShape, int indx);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void RemoveChildShapeFromCompoundShape2(IntPtr cShape, IntPtr removeShape);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void RecalculateCompoundShapeLocalAabb2(IntPtr cShape);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern IntPtr DuplicateCollisionShape2(IntPtr sim, IntPtr srcShape, uint id);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern IntPtr CreateBodyFromShapeAndInfo2(IntPtr sim, IntPtr shape, uint id, IntPtr constructionInfo);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool DeleteCollisionShape2(IntPtr world, IntPtr shape);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern int GetBodyType2(IntPtr obj);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern IntPtr CreateBodyFromShape2(IntPtr sim, IntPtr shape, uint id, Vector3 pos, Quaternion rot);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern IntPtr CreateBodyWithDefaultMotionState2(IntPtr shape, uint id, Vector3 pos, Quaternion rot);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern IntPtr CreateGhostFromShape2(IntPtr sim, IntPtr shape, uint id, Vector3 pos, Quaternion rot);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern IntPtr AllocateBodyInfo2(IntPtr obj);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void ReleaseBodyInfo2(IntPtr obj);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void DestroyObject2(IntPtr sim, IntPtr obj);
-
-// =====================================================================================
-// Terrain creation and helper routines
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern IntPtr CreateHeightMapInfo2(IntPtr sim, uint id, Vector3 minCoords, Vector3 maxCoords,
-        [MarshalAs(UnmanagedType.LPArray)] float[] heightMap, float collisionMargin);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern IntPtr FillHeightMapInfo2(IntPtr sim, IntPtr mapInfo, uint id, Vector3 minCoords, Vector3 maxCoords,
-        [MarshalAs(UnmanagedType.LPArray)] float[] heightMap, float collisionMargin);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool ReleaseHeightMapInfo2(IntPtr heightMapInfo);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern IntPtr CreateGroundPlaneShape2(uint id, float height, float collisionMargin);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern IntPtr CreateTerrainShape2(IntPtr mapInfo);
-
-// =====================================================================================
-// Constraint creation and helper routines
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern IntPtr Create6DofConstraint2(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 Create6DofConstraintToPoint2(IntPtr world, IntPtr obj1, IntPtr obj2,
-                    Vector3 joinPoint,
-                    bool useLinearReferenceFrameA, bool disableCollisionsBetweenLinkedBodies);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern IntPtr CreateHingeConstraint2(IntPtr world, IntPtr obj1, IntPtr obj2,
-                    Vector3 pivotinA, Vector3 pivotinB,
-                    Vector3 axisInA, Vector3 axisInB,
-                    bool useLinearReferenceFrameA, bool disableCollisionsBetweenLinkedBodies);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void SetConstraintEnable2(IntPtr constrain, float numericTrueFalse);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void SetConstraintNumSolverIterations2(IntPtr constrain, float iterations);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool SetFrames2(IntPtr constrain,
-                Vector3 frameA, Quaternion frameArot, Vector3 frameB, Quaternion frameBrot);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool SetLinearLimits2(IntPtr constrain, Vector3 low, Vector3 hi);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool SetAngularLimits2(IntPtr constrain, Vector3 low, Vector3 hi);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool UseFrameOffset2(IntPtr constrain, float enable);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool TranslationalLimitMotor2(IntPtr constrain, float enable, float targetVel, float maxMotorForce);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool SetBreakingImpulseThreshold2(IntPtr constrain, float threshold);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool CalculateTransforms2(IntPtr constrain);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool SetConstraintParam2(IntPtr constrain, ConstraintParams paramIndex, float value, ConstraintParamAxis axis);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool DestroyConstraint2(IntPtr world, IntPtr constrain);
-
-// =====================================================================================
-// btCollisionWorld entries
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void UpdateSingleAabb2(IntPtr world, IntPtr obj);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void UpdateAabbs2(IntPtr world);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool GetForceUpdateAllAabbs2(IntPtr world);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void SetForceUpdateAllAabbs2(IntPtr world, bool force);
-
-// =====================================================================================
-// btDynamicsWorld entries
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool AddObjectToWorld2(IntPtr world, IntPtr obj);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool RemoveObjectFromWorld2(IntPtr world, IntPtr obj);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool AddConstraintToWorld2(IntPtr world, IntPtr constrain, bool disableCollisionsBetweenLinkedObjects);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool RemoveConstraintFromWorld2(IntPtr world, IntPtr constrain);
-// =====================================================================================
-// btCollisionObject entries
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern Vector3 GetAnisotripicFriction2(IntPtr constrain);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern Vector3 SetAnisotripicFriction2(IntPtr constrain, Vector3 frict);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool HasAnisotripicFriction2(IntPtr constrain);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void SetContactProcessingThreshold2(IntPtr obj, float val);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern float GetContactProcessingThreshold2(IntPtr obj);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool IsStaticObject2(IntPtr obj);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool IsKinematicObject2(IntPtr obj);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool IsStaticOrKinematicObject2(IntPtr obj);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool HasContactResponse2(IntPtr obj);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void SetCollisionShape2(IntPtr sim, IntPtr obj, IntPtr shape);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern IntPtr GetCollisionShape2(IntPtr obj);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern int GetActivationState2(IntPtr obj);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void SetActivationState2(IntPtr obj, int state);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void SetDeactivationTime2(IntPtr obj, float dtime);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern float GetDeactivationTime2(IntPtr obj);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void ForceActivationState2(IntPtr obj, ActivationState state);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void Activate2(IntPtr obj, bool forceActivation);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool IsActive2(IntPtr obj);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void SetRestitution2(IntPtr obj, float val);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern float GetRestitution2(IntPtr obj);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void SetFriction2(IntPtr obj, float val);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern float GetFriction2(IntPtr obj);
-
-    /* Haven't defined the type 'Transform'
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern Transform GetWorldTransform2(IntPtr obj);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void setWorldTransform2(IntPtr obj, Transform trans);
-     */
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern Vector3 GetPosition2(IntPtr obj);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern Quaternion GetOrientation2(IntPtr obj);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void SetTranslation2(IntPtr obj, Vector3 position, Quaternion rotation);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern IntPtr GetBroadphaseHandle2(IntPtr obj);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void SetBroadphaseHandle2(IntPtr obj, IntPtr handle);
-
-    /*
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern Transform GetInterpolationWorldTransform2(IntPtr obj);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void SetInterpolationWorldTransform2(IntPtr obj, Transform trans);
-     */
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void SetInterpolationLinearVelocity2(IntPtr obj, Vector3 vel);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void SetInterpolationAngularVelocity2(IntPtr obj, Vector3 vel);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void SetInterpolationVelocity2(IntPtr obj, Vector3 linearVel, Vector3 angularVel);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern float GetHitFraction2(IntPtr obj);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void SetHitFraction2(IntPtr obj, float val);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern CollisionFlags GetCollisionFlags2(IntPtr obj);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern CollisionFlags SetCollisionFlags2(IntPtr obj, CollisionFlags flags);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern CollisionFlags AddToCollisionFlags2(IntPtr obj, CollisionFlags flags);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern CollisionFlags RemoveFromCollisionFlags2(IntPtr obj, CollisionFlags flags);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern float GetCcdMotionThreshold2(IntPtr obj);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void SetCcdMotionThreshold2(IntPtr obj, float val);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern float GetCcdSweptSphereRadius2(IntPtr obj);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void SetCcdSweptSphereRadius2(IntPtr obj, float val);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern IntPtr GetUserPointer2(IntPtr obj);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void SetUserPointer2(IntPtr obj, IntPtr val);
-
-// =====================================================================================
-// btRigidBody entries
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void ApplyGravity2(IntPtr obj);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void SetGravity2(IntPtr obj, Vector3 val);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern Vector3 GetGravity2(IntPtr obj);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void SetDamping2(IntPtr obj, float lin_damping, float ang_damping);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void SetLinearDamping2(IntPtr obj, float lin_damping);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void SetAngularDamping2(IntPtr obj, float ang_damping);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern float GetLinearDamping2(IntPtr obj);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern float GetAngularDamping2(IntPtr obj);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern float GetLinearSleepingThreshold2(IntPtr obj);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern float GetAngularSleepingThreshold2(IntPtr obj);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void ApplyDamping2(IntPtr obj, float timeStep);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void SetMassProps2(IntPtr obj, float mass, Vector3 inertia);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern Vector3 GetLinearFactor2(IntPtr obj);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void SetLinearFactor2(IntPtr obj, Vector3 factor);
-
-    /*
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void SetCenterOfMassTransform2(IntPtr obj, Transform trans);
-     */
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void SetCenterOfMassByPosRot2(IntPtr obj, Vector3 pos, Quaternion rot);
-
-// Add a force to the object as if its mass is one.
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void ApplyCentralForce2(IntPtr obj, Vector3 force);
-
-// Set the force being applied to the object as if its mass is one.
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void SetObjectForce2(IntPtr obj, Vector3 force);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern Vector3 GetTotalForce2(IntPtr obj);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern Vector3 GetTotalTorque2(IntPtr obj);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern Vector3 GetInvInertiaDiagLocal2(IntPtr obj);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void SetInvInertiaDiagLocal2(IntPtr obj, Vector3 inert);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void SetSleepingThresholds2(IntPtr obj, float lin_threshold, float ang_threshold);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void ApplyTorque2(IntPtr obj, Vector3 torque);
-
-// Apply force at the given point. Will add torque to the object.
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void ApplyForce2(IntPtr obj, Vector3 force, Vector3 pos);
-
-// Apply impulse to the object. Same as "ApplycentralForce" but force scaled by object's mass.
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void ApplyCentralImpulse2(IntPtr obj, Vector3 imp);
-
-// Apply impulse to the object's torque. Force is scaled by object's mass.
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void ApplyTorqueImpulse2(IntPtr obj, Vector3 imp);
-
-// Apply impulse at the point given. For is scaled by object's mass and effects both linear and angular forces.
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void ApplyImpulse2(IntPtr obj, Vector3 imp, Vector3 pos);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void ClearForces2(IntPtr obj);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void ClearAllForces2(IntPtr obj);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void UpdateInertiaTensor2(IntPtr obj);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern Vector3 GetCenterOfMassPosition2(IntPtr obj);
-
-    /*
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern Transform GetCenterOfMassTransform2(IntPtr obj);
-     */
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern Vector3 GetLinearVelocity2(IntPtr obj);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern Vector3 GetAngularVelocity2(IntPtr obj);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void SetLinearVelocity2(IntPtr obj, Vector3 val);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void SetAngularVelocity2(IntPtr obj, Vector3 angularVelocity);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern Vector3 GetVelocityInLocalPoint2(IntPtr obj, Vector3 pos);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void Translate2(IntPtr obj, Vector3 trans);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void UpdateDeactivation2(IntPtr obj, float timeStep);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool WantsSleeping2(IntPtr obj);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void SetAngularFactor2(IntPtr obj, float factor);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void SetAngularFactorV2(IntPtr obj, Vector3 factor);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern Vector3 GetAngularFactor2(IntPtr obj);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool IsInWorld2(IntPtr obj);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void AddConstraintRef2(IntPtr obj, IntPtr constrain);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void RemoveConstraintRef2(IntPtr obj, IntPtr constrain);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern IntPtr GetConstraintRef2(IntPtr obj, int index);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern int GetNumConstraintRefs2(IntPtr obj);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void SetCollisionFilterMask2(IntPtr body, uint filter, uint mask);
-
-// =====================================================================================
-// btCollisionShape entries
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern float GetAngularMotionDisc2(IntPtr shape);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern float GetContactBreakingThreshold2(IntPtr shape, float defaultFactor);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool IsPolyhedral2(IntPtr shape);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool IsConvex2d2(IntPtr shape);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool IsConvex2(IntPtr shape);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool IsNonMoving2(IntPtr shape);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool IsConcave2(IntPtr shape);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool IsCompound2(IntPtr shape);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool IsSoftBody2(IntPtr shape);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool IsInfinite2(IntPtr shape);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void SetLocalScaling2(IntPtr shape, Vector3 scale);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern Vector3 GetLocalScaling2(IntPtr shape);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern Vector3 CalculateLocalInertia2(IntPtr shape, float mass);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern int GetShapeType2(IntPtr shape);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void SetMargin2(IntPtr shape, float val);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern float GetMargin2(IntPtr shape);
-
-// =====================================================================================
-// Debugging
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void DumpRigidBody2(IntPtr sim, IntPtr collisionObject);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void DumpCollisionShape2(IntPtr sim, IntPtr collisionShape);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void DumpConstraint2(IntPtr sim, IntPtr constrain);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void DumpAllInfo2(IntPtr sim);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void DumpMapInfo2(IntPtr sim, IntPtr manInfo);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void DumpPhysicsStatistics2(IntPtr sim);
-
-}
-}
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BulletSimData.cs b/OpenSim/Region/Physics/BulletSPlugin/BulletSimData.cs
new file mode 100755
index 0000000..8012d91
--- /dev/null
+++ b/OpenSim/Region/Physics/BulletSPlugin/BulletSimData.cs
@@ -0,0 +1,269 @@
+/*
+ * Copyright (c) Contributors, http://opensimulator.org/
+ * See CONTRIBUTORS.TXT for a full list of copyright holders.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyrightD
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the OpenSimulator Project nor the
+ *       names of its contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+using System;
+using System.Collections.Generic;
+using System.Text;
+using OMV = OpenMetaverse;
+
+namespace OpenSim.Region.Physics.BulletSPlugin
+{
+// Classes to allow some type checking for the API
+// These hold pointers to allocated objects in the unmanaged space.
+// These classes are subclassed by the various physical implementations of
+// objects. In particular, there is a version for physical instances in
+// unmanaged memory ("unman") and one for in managed memory ("XNA").
+
+// Currently, the instances of these classes are a reference to a
+// physical representation and this has no releationship to other
+// instances. Someday, refarb the usage of these classes so each instance
+// refers to a particular physical instance and this class controls reference
+// counts and such. This should be done along with adding BSShapes.
+
+public class BulletWorld
+{
+    public BulletWorld(uint worldId, BSScene bss)
+    {
+        worldID = worldId;
+        physicsScene = bss;
+    }
+    public uint worldID;
+    // The scene is only in here so very low level routines have a handle to print debug/error messages
+    public BSScene physicsScene;
+}
+
+// An allocated Bullet btRigidBody
+public class BulletBody
+{
+    public BulletBody(uint id)
+    {
+        ID = id;
+        collisionType = CollisionType.Static;
+    }
+    public uint ID;
+    public CollisionType collisionType;
+
+    public virtual void Clear() { }
+    public virtual bool HasPhysicalBody { get { return false; } }
+
+    // Apply the specificed collision mask into the physical world
+    public virtual bool ApplyCollisionMask(BSScene physicsScene)
+    {
+        // Should assert the body has been added to the physical world.
+        // (The collision masks are stored in the collision proxy cache which only exists for
+        //    a collision body that is in the world.)
+        return physicsScene.PE.SetCollisionGroupMask(this,
+                                BulletSimData.CollisionTypeMasks[collisionType].group,
+                                BulletSimData.CollisionTypeMasks[collisionType].mask);
+    }
+
+    // Used for log messages for a unique display of the memory/object allocated to this instance
+    public virtual string AddrString
+    {
+        get { return "unknown"; }
+    }
+
+    public override string ToString()
+    {
+        StringBuilder buff = new StringBuilder();
+        buff.Append("<id=");
+        buff.Append(ID.ToString());
+        buff.Append(",p=");
+        buff.Append(AddrString);
+        buff.Append(",c=");
+        buff.Append(collisionType);
+        buff.Append(">");
+        return buff.ToString();
+    }
+}
+
+public class BulletShape
+{
+    public BulletShape()
+    {
+        type = BSPhysicsShapeType.SHAPE_UNKNOWN;
+        shapeKey = (System.UInt64)FixedShapeKey.KEY_NONE;
+        isNativeShape = false;
+    }
+    public BSPhysicsShapeType type;
+    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; }
+
+    // Used for log messages for a unique display of the memory/object allocated to this instance
+    public virtual string AddrString
+    {
+        get { 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(shapeKey.ToString("X"));
+        buff.Append(",n=");
+        buff.Append(isNativeShape.ToString());
+        buff.Append(">");
+        return buff.ToString();
+    }
+}
+
+// An allocated Bullet btConstraint
+public class BulletConstraint
+{
+    public BulletConstraint()
+    {
+    }
+    public virtual void Clear() { }
+    public virtual bool HasPhysicalConstraint { get { return false; } }
+
+    // Used for log messages for a unique display of the memory/object allocated to this instance
+    public virtual string AddrString
+    {
+        get { return "unknown"; }
+    }
+}
+
+// An allocated HeightMapThing which holds various heightmap info.
+// Made a class rather than a struct so there would be only one
+//      instance of this and C# will pass around pointers rather
+//      than making copies.
+public class BulletHMapInfo
+{
+    public BulletHMapInfo(uint id, float[] hm) {
+        ID = id;
+        heightMap = hm;
+        terrainRegionBase = OMV.Vector3.Zero;
+        minCoords = new OMV.Vector3(100f, 100f, 25f);
+        maxCoords = new OMV.Vector3(101f, 101f, 26f);
+        minZ = maxZ = 0f;
+        sizeX = sizeY = 256f;
+    }
+    public uint ID;
+    public float[] heightMap;
+    public OMV.Vector3 terrainRegionBase;
+    public OMV.Vector3 minCoords;
+    public OMV.Vector3 maxCoords;
+    public float sizeX, sizeY;
+    public float minZ, maxZ;
+    public BulletShape terrainShape;
+    public BulletBody terrainBody;
+}
+
+// The general class of collsion object.
+public enum CollisionType
+{
+    Avatar,
+    Groundplane,
+    Terrain,
+    Static,
+    Dynamic,
+    VolumeDetect,
+    // Linkset, // A linkset should be either Static or Dynamic
+    LinksetChild,
+    Unknown
+};
+
+// Hold specification of group and mask collision flags for a CollisionType
+public struct CollisionTypeFilterGroup
+{
+    public CollisionTypeFilterGroup(CollisionType t, uint g, uint m)
+    {
+        type = t;
+        group = g;
+        mask = m;
+    }
+    public CollisionType type;
+    public uint group;
+    public uint mask;
+};
+
+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 
+            = new Dictionary<CollisionType, CollisionTypeFilterGroup>()
+{
+    { CollisionType.Avatar, 
+                new CollisionTypeFilterGroup(CollisionType.Avatar, 
+                                (uint)CollisionFilterGroups.BCharacterGroup, 
+                                (uint)CollisionFilterGroups.BAllGroup)
+    },
+    { CollisionType.Groundplane, 
+                new CollisionTypeFilterGroup(CollisionType.Groundplane, 
+                                (uint)CollisionFilterGroups.BGroundPlaneGroup, 
+                                (uint)CollisionFilterGroups.BAllGroup)
+    },
+    { CollisionType.Terrain, 
+                new CollisionTypeFilterGroup(CollisionType.Terrain, 
+                                (uint)CollisionFilterGroups.BTerrainGroup, 
+                                (uint)(CollisionFilterGroups.BAllGroup & ~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, 
+                                (uint)(CollisionFilterGroups.BAllGroup))
+    },
+    { CollisionType.VolumeDetect, 
+                new CollisionTypeFilterGroup(CollisionType.VolumeDetect, 
+                                (uint)CollisionFilterGroups.BSensorTrigger, 
+                                (uint)(~CollisionFilterGroups.BSensorTrigger))
+    },
+    { CollisionType.LinksetChild,
+                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
new file mode 100755
index 0000000..8a15abe
--- /dev/null
+++ b/OpenSim/Region/Physics/BulletSPlugin/BulletSimTODO.txt
@@ -0,0 +1,346 @@
+CURRENT PRIORITIES
+=================================================
+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)
+Lock axis
+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.
+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)
+        avatar capsule rotation completed (NOT DONE - Bullet's capsule shape is not the solution)
+Vehicle script tuning/debugging
+        Avanti speed script
+        Weapon shooter script
+Move material definitions (friction, ...) into simulator.
+Add material densities to the material types.
+Terrain detail: double terrain mesh detail
+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:
+=================================================
+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)
+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.
+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)
+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.
+
+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.
+Scenes with hundred of thousands of static objects take a lot of physics CPU time.
+BSPrim.Force should set a continious force on the prim. The force should be
+        applied each tick.  Some limits?
+Gun sending shooter flying.
+Collision margin (gap between physical objects lying on each other)
+Boundry checking (crashes related to crossing boundry)
+    Add check for border edge position for avatars and objects.
+    Verify the events are created for border crossings.
+Avatar rotation (check out changes to ScenePresence for physical rotation)
+Avatar running (what does phys engine need to do?)
+Small physical objects do not interact correctly
+    Create chain of .5x.5x.1 torui and make all but top physical so to hang.
+        The chain will fall apart and pairs will dance around on ground
+    Chains of 1x1x.2 will stay connected but will dance.
+    Chains above 2x2x.4 are more stable and get stablier as torui get larger.
+Add PID motor for avatar movement (slow to stop, ...)
+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
+        child position. LinksetConstraint acts like it's at taint time!!
+Implement LockAngularMotion -- implements llSetStatus(ROTATE_AXIS_*, T/F)
+Should the different PID factors have non-equal contributions for different
+        values of Efficiency?
+Selecting and deselecting physical objects causes CPU processing time to jump
+        http://www.youtube.com/watch?v=Hjg57fWg8yI&hd=1
+        put thousand physical objects, select and deselect same. CPU time will be large.
+Re-implement buoyancy as a separate force on the object rather than diddling gravity.
+        Register a pre-step event to add the force.
+More efficient memory usage when passing hull information from BSPrim to BulletSim
+Avatar movement motor check for zero or small movement. Somehow suppress small movements
+        when avatar has stopped and is just standing. Simple test for near zero has
+        the problem of preventing starting up (increase from zero) especially when falling.
+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
+        Add LocalID to children shapes in LinksetCompound and create events for individuals
+LinksetCompound: when one of the children changes orientation (like tires
+        turning on a vehicle, the whole compound object is rebuilt. Optimize this
+        so orientation/position of individual children can change without a rebuild.
+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.
+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).
+Eliminate collisions between objects in a linkset. (LinksetConstraint)
+    Have UserPointer point to struct with localID and linksetID?
+    Objects in original linkset still collide with each other?
+
+MORE
+======================================================
+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
+        Are there faster ways?
+        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]
+        Friction: [0, 255]
+        Density: [1, 22587]
+        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.
+Consider moving prim/character body and shape destruction in destroy()
+        to postTimeTime rather than protecting all the potential sets that
+        might have been queued up.
+Remove unused fields from ShapeData (not used in API2)
+Remove unused fields from pinned memory shared parameter block
+        Create parameter variables in BSScene to replace same.
+Breakout code for mesh/hull/compound/native into separate BSShape* classes
+    Standardize access to building and reference code.
+        The skeleton classes are in the sources but are not complete or linked in.
+Make BSBody and BSShape real classes to centralize creation/changin/destruction
+        Convert state and parameter calls from BulletSimAPI direct calls to
+                calls on BSBody and BSShape
+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.
+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?
+Add floating motor for BS_FLOATS_ON_WATER so prim and avatar will
+    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 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
+        Think of something to eliminate one or more of the layers
+
+THREADING
+=================================================
+Do taint action immediately if not actually executing Bullet.
+        Add lock around Bullet execution and just do taint actions if simulation is not happening.
+
+DONE DONE DONE DONE
+=================================================
+Cleanup code in BSDynamics by using motors. (Resolution: started)
+Consider implementing terrain with a mesh rather than heightmap. (Resolution: done)
+    Would have better and adjustable resolution.
+Build terrain mesh so heighmap is height of the center of the square meter.
+    Resolution: NOT DONE: SL and ODE define meter square as being at one corner with one diagional.
+Terrain as mesh. (Resolution: done)
+How are static linksets seen by the physics engine?
+    Resolution: they are not linked in physics. When moved, all the children are repositioned.
+Convert BSCharacter to use all API2 (Resolution: done)
+Avatar pushing difficult (too heavy?)
+Use asset service passed to BulletSim to get sculptie bodies, etc. (Resolution: done)
+Remove old code in DLL (all non-API2 stuff). (Resolution: done)
+Measurements of mega-physical prim performance (with graph) (Resolution: done, email)
+Debug Bullet internal stats output (why is timing all wrong?)
+        Resolution: Bullet stats logging only works with a single instance of Bullet (one region).
+Implement meshes or just verify that they work. (Resolution: they do!)
+Do prim hash codes work for sculpties and meshes? (Resolution: yes)
+Linkset implementation using compound shapes. (Resolution: implemented LinksetCompound)
+    Compound shapes will need the LocalID in the shapes and collision
+    processing to get it from there.
+Light cycle not banking (Resolution: It doesn't. Banking is roll adding yaw.)
+Package Bullet source mods for Bullet internal stats output
+        (Resolution: move code into WorldData.h rather than relying on patches)
+Single prim vehicles don't seem to properly vehiclize.
+        (Resolution: mass was not getting set properly for single prim linksets)
+Add material type linkage and input all the material property definitions.
+        Skeleton classes and table are in the sources but are not filled or used.
+        (Resolution: 
+Neb vehicle taking > 25ms of physics time!!
+        (Resolution: compound linksets were being rebuild WAY too often)
+Avatar height off after unsitting (floats off ground)
+        Editting appearance then moving restores.
+        Must not be initializing height when recreating capsule after unsit.
+        (Resolution: confusion of scale vs size for native objects removed)
+Light cycle falling over when driving (Resolution: implemented angularMotorUp)
+Should vehicle angular/linear movement friction happen after all the components
+        or does it only apply to the basic movement?
+        (Resolution: friction added before returning newly computed motor value.
+        What is expected by some vehicles (turning up friction to moderate speed))
+Tune terrain/object friction to be closer to SL.
+        (Resolution: added material type with friction and resolution)
+Smooth avatar movement with motor (DONE)
+        Should motor update be all at taint-time? (Yes, DONE)
+        Fix avatar slowly sliding when standing (zero motion when stopped) (DONE)
+        (Resolution: added BSVMotor for avatar starting and stopping)
+llApplyImpulse()
+        Compare mass/movement in OS and SL. Calibrate actions.  (DONE)
+        (Resolution: tested on SL and OS. AddForce scales the force for timestep)
+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)
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/OpenSim/Region/Physics/BulletSPlugin/Tests/BasicVehicles.cs b/OpenSim/Region/Physics/BulletSPlugin/Tests/BasicVehicles.cs
new file mode 100755
index 0000000..33232bd
--- /dev/null
+++ b/OpenSim/Region/Physics/BulletSPlugin/Tests/BasicVehicles.cs
@@ -0,0 +1,150 @@
+/*
+ * 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.
+ */
+
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Text;
+
+using NUnit.Framework;
+using log4net;
+
+using OpenSim.Framework;
+using OpenSim.Region.Physics.BulletSPlugin;
+using OpenSim.Region.Physics.Manager;
+using OpenSim.Tests.Common;
+
+using OpenMetaverse;
+
+namespace OpenSim.Region.Physics.BulletSPlugin.Tests
+{
+[TestFixture]
+public class BasicVehicles : OpenSimTestCase
+{
+    // Documentation on attributes: http://www.nunit.org/index.php?p=attributes&r=2.6.1
+    // Documentation on assertions: http://www.nunit.org/index.php?p=assertions&r=2.6.1
+
+    BSScene PhysicsScene { get; set; }
+    BSPrim TestVehicle { get; set; }
+    Vector3 TestVehicleInitPosition { get; set; }
+    float simulationTimeStep = 0.089f;
+
+    [TestFixtureSetUp]
+    public void Init()
+    {
+        Dictionary<string, string> engineParams = new Dictionary<string, string>();
+        PhysicsScene = BulletSimTestsUtil.CreateBasicPhysicsEngine(engineParams);
+
+        PrimitiveBaseShape pbs = PrimitiveBaseShape.CreateSphere();
+        Vector3 pos = new Vector3(100.0f, 100.0f, 0f);
+        pos.Z = PhysicsScene.TerrainManager.GetTerrainHeightAtXYZ(pos) + 2f;
+        TestVehicleInitPosition = pos;
+        Vector3 size = new Vector3(1f, 1f, 1f);
+        pbs.Scale = size;
+        Quaternion rot = Quaternion.Identity;
+        bool isPhys = false;
+        uint localID = 123;
+
+        PhysicsScene.AddPrimShape("testPrim", pbs, pos, size, rot, isPhys, localID);
+        TestVehicle = (BSPrim)PhysicsScene.PhysObjects[localID];
+        // The actual prim shape creation happens at taint time
+        PhysicsScene.ProcessTaints();
+
+    }
+
+    [TestFixtureTearDown]
+    public void TearDown()
+    {
+        if (PhysicsScene != null)
+        {
+            // The Dispose() will also free any physical objects in the scene
+            PhysicsScene.Dispose();
+            PhysicsScene = null;
+        }
+    }
+
+    [TestCase(2f, 0.2f,  0.25f,  0.25f,  0.25f)]
+    [TestCase(2f, 0.2f, -0.25f,  0.25f,  0.25f)]
+    [TestCase(2f, 0.2f,  0.25f, -0.25f,  0.25f)]
+    [TestCase(2f, 0.2f, -0.25f, -0.25f,  0.25f)]
+    // [TestCase(2f, 0.2f, 0.785f,   0.0f,  0.25f) /*, "Leaning 45 degrees to the side" */]
+    // [TestCase(2f, 0.2f, 1.650f,   0.0f,  0.25f) /*, "Leaning more than 90 degrees to the side" */]
+    // [TestCase(2f, 0.2f, 2.750f,   0.0f,  0.25f) /*, "Almost upside down, tipped right" */]
+    // [TestCase(2f, 0.2f,-2.750f,   0.0f,  0.25f) /*, "Almost upside down, tipped left" */]
+    // [TestCase(2f, 0.2f,   0.0f, 0.785f,  0.25f) /*, "Tipped back 45 degrees" */]
+    // [TestCase(2f, 0.2f,   0.0f, 1.650f,  0.25f) /*, "Tipped back more than 90 degrees" */]
+    // [TestCase(2f, 0.2f,   0.0f, 2.750f,  0.25f) /*, "Almost upside down, tipped back" */]
+    // [TestCase(2f, 0.2f,   0.0f,-2.750f,  0.25f) /*, "Almost upside down, tipped forward" */]
+    public void AngularVerticalAttraction(float timeScale, float efficiency, float initRoll, float initPitch, float initYaw)
+    {
+        // Enough simulation steps to cover the timescale the operation should take
+        int simSteps = (int)(timeScale / simulationTimeStep) + 1;
+
+        // Tip the vehicle
+        Quaternion initOrientation = Quaternion.CreateFromEulers(initRoll, initPitch, initYaw);
+        TestVehicle.Orientation = initOrientation;
+
+        TestVehicle.Position = TestVehicleInitPosition;
+
+        // The vehicle controller is not enabled directly (by setting a vehicle type).
+        //    Instead the appropriate values are set and calls are made just the parts of the
+        //    controller we want to exercise. Stepping the physics engine then applies
+        //    the actions of that one feature.
+        TestVehicle.VehicleController.ProcessFloatVehicleParam(Vehicle.VERTICAL_ATTRACTION_EFFICIENCY, efficiency);
+        TestVehicle.VehicleController.ProcessFloatVehicleParam(Vehicle.VERTICAL_ATTRACTION_TIMESCALE, timeScale);
+        TestVehicle.VehicleController.enableAngularVerticalAttraction = true;
+
+        TestVehicle.IsPhysical = true;
+        PhysicsScene.ProcessTaints();
+
+        // Step the simulator a bunch of times and vertical attraction should orient the vehicle up
+        for (int ii = 0; ii < simSteps; ii++)
+        {
+            TestVehicle.VehicleController.ForgetKnownVehicleProperties();
+            TestVehicle.VehicleController.ComputeAngularVerticalAttraction();
+            TestVehicle.VehicleController.PushKnownChanged();
+
+            PhysicsScene.Simulate(simulationTimeStep);
+        }
+
+        TestVehicle.IsPhysical = false;
+        PhysicsScene.ProcessTaints();
+
+        // After these steps, the vehicle should be upright
+        /*
+        float finalRoll, finalPitch, finalYaw;
+        TestVehicle.Orientation.GetEulerAngles(out finalRoll, out finalPitch, out finalYaw);
+        Assert.That(finalRoll, Is.InRange(-0.01f, 0.01f));
+        Assert.That(finalPitch, Is.InRange(-0.01f, 0.01f));
+        Assert.That(finalYaw, Is.InRange(initYaw - 0.1f, initYaw + 0.1f));
+         */
+
+        Vector3 upPointer = Vector3.UnitZ * TestVehicle.Orientation;
+        Assert.That(upPointer.Z, Is.GreaterThan(0.99f));
+    }
+}
+}
\ No newline at end of file
diff --git a/OpenSim/Region/Physics/BulletSPlugin/Tests/BulletSimTests.cs b/OpenSim/Region/Physics/BulletSPlugin/Tests/BulletSimTests.cs
new file mode 100755
index 0000000..35cbc1d
--- /dev/null
+++ b/OpenSim/Region/Physics/BulletSPlugin/Tests/BulletSimTests.cs
@@ -0,0 +1,56 @@
+/*

+ * 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.

+ */

+

+using System;

+using System.Collections.Generic;

+using System.Linq;

+using System.Text;

+

+using NUnit.Framework;

+using log4net;

+

+using OpenSim.Tests.Common;

+

+namespace OpenSim.Region.Physics.BulletSPlugin.Tests

+{

+[TestFixture]

+public class BulletSimTests : OpenSimTestCase

+{

+    // Documentation on attributes: http://www.nunit.org/index.php?p=attributes&r=2.6.1

+    // Documentation on assertions: http://www.nunit.org/index.php?p=assertions&r=2.6.1

+

+    [TestFixtureSetUp]

+    public void Init()

+    {

+    }

+

+    [TestFixtureTearDown]

+    public void TearDown()

+    {

+    }

+}

+}

diff --git a/OpenSim/Region/Physics/BulletSPlugin/Tests/BulletSimTestsUtil.cs b/OpenSim/Region/Physics/BulletSPlugin/Tests/BulletSimTestsUtil.cs
new file mode 100755
index 0000000..28207a4
--- /dev/null
+++ b/OpenSim/Region/Physics/BulletSPlugin/Tests/BulletSimTestsUtil.cs
@@ -0,0 +1,95 @@
+/*
+ * 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.
+ */
+
+using System;
+using System.IO;
+using System.Collections.Generic;
+using System.Linq;
+using System.Text;
+
+using Nini.Config;
+
+using OpenSim.Framework;
+using OpenSim.Region.Physics.BulletSPlugin;
+using OpenSim.Region.Physics.Meshing;
+
+namespace OpenSim.Region.Physics.BulletSPlugin.Tests
+{
+// Utility functions for building up and tearing down the sample physics environments
+public static class BulletSimTestsUtil
+{
+    // 'engineName' is the Bullet engine to use. Either null (for unmanaged), "BulletUnmanaged" or "BulletXNA"
+    // 'params' is a set of keyValue pairs to set in the engine's configuration file (override defaults)
+    //      May be 'null' if there are no overrides.
+    public static BSScene CreateBasicPhysicsEngine(Dictionary<string,string> paramOverrides)
+    {
+        IConfigSource openSimINI = new IniConfigSource();
+        IConfig startupConfig = openSimINI.AddConfig("Startup");
+        startupConfig.Set("physics", "BulletSim");
+        startupConfig.Set("meshing", "Meshmerizer");
+        startupConfig.Set("cacheSculptMaps", "false");  // meshmerizer shouldn't save maps
+
+        IConfig bulletSimConfig = openSimINI.AddConfig("BulletSim");
+        // If the caller cares, specify the bullet engine otherwise it will default to "BulletUnmanaged".
+        // bulletSimConfig.Set("BulletEngine", "BulletUnmanaged");
+        // bulletSimConfig.Set("BulletEngine", "BulletXNA");
+        bulletSimConfig.Set("MeshSculptedPrim", "false");
+        bulletSimConfig.Set("ForceSimplePrimMeshing", "true");
+        if (paramOverrides != null)
+        {
+            foreach (KeyValuePair<string, string> kvp in paramOverrides)
+            {
+                bulletSimConfig.Set(kvp.Key, kvp.Value);
+            }
+        }
+
+        // If a special directory exists, put detailed logging therein.
+        // This allows local testing/debugging without having to worry that the build engine will output logs.
+        if (Directory.Exists("physlogs"))
+        {
+            bulletSimConfig.Set("PhysicsLoggingDir","./physlogs");
+            bulletSimConfig.Set("PhysicsLoggingEnabled","True");
+            bulletSimConfig.Set("PhysicsLoggingDoFlush","True");
+            bulletSimConfig.Set("VehicleLoggingEnabled","True");
+        }
+
+        BSPlugin bsPlugin = new BSPlugin();
+
+        BSScene bsScene = (BSScene)bsPlugin.GetScene("BSTestRegion");
+
+        // Since the asset requestor is not initialized, any mesh or sculptie will be a cube.
+        // In the future, add a fake asset fetcher to get meshes and sculpts.
+        // bsScene.RequestAssetMethod = ???;
+
+        Meshing.Meshmerizer mesher = new Meshmerizer(openSimINI);
+        bsScene.Initialise(mesher, openSimINI);
+
+        return bsScene;
+    }
+
+}
+}
diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
index 8de70ef..ba24aa7 100644
--- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
+++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
@@ -2190,7 +2190,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                         convex = false;
                     try
                     {
-                        _mesh = _parent_scene.mesher.CreateMesh(m_primName, _pbs, _size, (int)LevelOfDetail.High, true,convex,false);
+                        _mesh = _parent_scene.mesher.CreateMesh(m_primName, _pbs, _size, (int)LevelOfDetail.High, true,false,convex,false);
                     }
                     catch
                     {
@@ -2557,7 +2557,7 @@ namespace OpenSim.Region.Physics.OdePlugin
 
                 try
                 {
-                    mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, (int)LevelOfDetail.High, true, convex,false);
+                    mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, (int)LevelOfDetail.High, true, false,convex,false);
                 }
                 catch
                 {
diff --git a/OpenSim/Region/Physics/ConvexDecompositionDotNet/Properties/AssemblyInfo.cs b/OpenSim/Region/Physics/ConvexDecompositionDotNet/Properties/AssemblyInfo.cs
index 5ff945d..f611b9a 100644
--- a/OpenSim/Region/Physics/ConvexDecompositionDotNet/Properties/AssemblyInfo.cs
+++ b/OpenSim/Region/Physics/ConvexDecompositionDotNet/Properties/AssemblyInfo.cs
@@ -32,5 +32,5 @@ using System.Runtime.InteropServices;
 // You can specify all the values or you can default the Build and Revision Numbers 
 // by using the '*' as shown below:
 // [assembly: AssemblyVersion("1.0.*")]
-[assembly: AssemblyVersion("0.7.5.*")]
-[assembly: AssemblyFileVersion("1.0.0.0")]
+[assembly: AssemblyVersion("0.7.6.*")]
+
diff --git a/OpenSim/Region/Physics/Manager/AssemblyInfo.cs b/OpenSim/Region/Physics/Manager/AssemblyInfo.cs
index 36b4235..5da3956 100644
--- a/OpenSim/Region/Physics/Manager/AssemblyInfo.cs
+++ b/OpenSim/Region/Physics/Manager/AssemblyInfo.cs
@@ -55,4 +55,4 @@ using System.Runtime.InteropServices;
 // You can specify all values by your own or you can build default build and revision
 // numbers with the '*' character (the default):
 
-[assembly : AssemblyVersion("0.7.5.*")]
+[assembly : AssemblyVersion("0.7.6.*")]
diff --git a/OpenSim/Region/Physics/Manager/IMesher.cs b/OpenSim/Region/Physics/Manager/IMesher.cs
index ecc2918..5485eb7 100644
--- a/OpenSim/Region/Physics/Manager/IMesher.cs
+++ b/OpenSim/Region/Physics/Manager/IMesher.cs
@@ -37,7 +37,7 @@ namespace OpenSim.Region.Physics.Manager
     {
         IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod);
         IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical);
-        IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool convex, bool forOde);
+        IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool shouldCache, bool convex, bool forOde);
         IMesh GetMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool convex);
         void ReleaseMesh(IMesh mesh);
         void ExpireReleaseMeshs();
@@ -83,6 +83,7 @@ namespace OpenSim.Region.Physics.Manager
         List<Vector3> getVertexList();
         int[] getIndexListAsInt();
         int[] getIndexListAsIntLocked();
+        float[] getVertexListAsFloat();
         float[] getVertexListAsFloatLocked();
         void getIndexListAsPtrToIntArray(out IntPtr indices, out int triStride, out int indexCount);
         void getVertexListAsPtrToFloatArray(out IntPtr vertexList, out int vertexStride, out int vertexCount);
diff --git a/OpenSim/Region/Physics/Manager/IPhysicsParameters.cs b/OpenSim/Region/Physics/Manager/IPhysicsParameters.cs
index b8676ba..31a397c 100755
--- a/OpenSim/Region/Physics/Manager/IPhysicsParameters.cs
+++ b/OpenSim/Region/Physics/Manager/IPhysicsParameters.cs
@@ -60,14 +60,14 @@ namespace OpenSim.Region.Physics.Manager
 
         // Set parameter on a specific or all instances.
         // Return 'false' if not able to set the parameter.
-        bool SetPhysicsParameter(string parm, float value, uint localID);
+        bool SetPhysicsParameter(string parm, string value, uint localID);
 
         // Get parameter.
         // Return 'false' if not able to get the parameter.
-        bool GetPhysicsParameter(string parm, out float value);
+        bool GetPhysicsParameter(string parm, out string value);
 
         // Get parameter from a particular object
         // TODO:
-        // bool GetPhysicsParameter(string parm, out float value, uint localID);
+        // bool GetPhysicsParameter(string parm, out string value, uint localID);
     }
 }
diff --git a/OpenSim/Region/Physics/Manager/PhysicsActor.cs b/OpenSim/Region/Physics/Manager/PhysicsActor.cs
index dcb2c91..7cd364b 100644
--- a/OpenSim/Region/Physics/Manager/PhysicsActor.cs
+++ b/OpenSim/Region/Physics/Manager/PhysicsActor.cs
@@ -349,17 +349,20 @@ namespace OpenSim.Region.Physics.Manager
         }
 
         /// <summary>
-        /// Velocity of this actor.
+        /// The desired velocity of this actor.
         /// </summary>
         /// <remarks>
         /// Setting this provides a target velocity for physics scene updates.
-        /// Getting this returns the velocity calculated by physics scene updates, using factors such as target velocity,
-        /// time to accelerate and collisions.
+        /// Getting this returns the last set target. Fetch Velocity to get the current velocity.
         /// </remarks>
+        protected Vector3 m_targetVelocity;
         public virtual Vector3 TargetVelocity
         {
-            get { return Velocity; }
-            set { Velocity = value; }
+            get { return m_targetVelocity; }
+            set {
+                m_targetVelocity = value;
+                Velocity = m_targetVelocity;
+            }
         }
 
         public abstract Vector3 Velocity { get; set; }
diff --git a/OpenSim/Region/Physics/Manager/PhysicsPluginManager.cs b/OpenSim/Region/Physics/Manager/PhysicsPluginManager.cs
index 8587a2b..8ccfda5 100644
--- a/OpenSim/Region/Physics/Manager/PhysicsPluginManager.cs
+++ b/OpenSim/Region/Physics/Manager/PhysicsPluginManager.cs
@@ -30,7 +30,7 @@ using System.Collections.Generic;
 using System.IO;
 using System.Reflection;
 using Nini.Config;
-using log4net;

+using log4net;
 using OpenSim.Framework;
 
 namespace OpenSim.Region.Physics.Manager
diff --git a/OpenSim/Region/Physics/Manager/PhysicsScene.cs b/OpenSim/Region/Physics/Manager/PhysicsScene.cs
index 57e2d20..20a70b4 100644
--- a/OpenSim/Region/Physics/Manager/PhysicsScene.cs
+++ b/OpenSim/Region/Physics/Manager/PhysicsScene.cs
@@ -47,7 +47,7 @@ namespace OpenSim.Region.Physics.Manager
     public delegate void JointDeactivated(PhysicsJoint joint);
     public delegate void JointErrorMessage(PhysicsJoint joint, string message); // this refers to an "error message due to a problem", not "amount of joint constraint violation"
 
-    public enum RayFilterFlags:ushort
+    public enum RayFilterFlags : ushort
     {
         // the flags
         water = 0x01,
@@ -64,7 +64,7 @@ namespace OpenSim.Region.Physics.Manager
         ClosestHit = 0x8000,
 
         // some combinations
-        LSLPhanton = phantom | volumedtc,
+        LSLPhantom = phantom | volumedtc,
         PrimsNonPhantom = nonphysical | physical,
         PrimsNonPhantomAgents = nonphysical | physical | agent,
 
@@ -97,13 +97,20 @@ namespace OpenSim.Region.Physics.Manager
 //        private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
 
         /// <summary>
-        /// Name of this scene.  Useful in debug messages to distinguish one OdeScene instance from another.
+        /// A unique identifying string for this instance of the physics engine.
+        /// Useful in debug messages to distinguish one OdeScene instance from another.
+        /// Usually set to include the region name that the physics engine is acting for.
         /// </summary>
         public string Name { get; protected set; }
 
+        /// <summary>
+        /// A string identifying the family of this physics engine. Most common values returned
+        /// are "OpenDynamicsEngine" and "BulletSim" but others are possible.
+        /// </summary>
+        public string EngineType { get; protected set; }
+
         // The only thing that should register for this event is the SceneGraph
         // Anything else could cause problems.
-
         public event physicsCrash OnPhysicsCrash;
 
         public static PhysicsScene Null
diff --git a/OpenSim/Region/Physics/Manager/ZeroMesher.cs b/OpenSim/Region/Physics/Manager/ZeroMesher.cs
index 16846e6..80ecf66 100644
--- a/OpenSim/Region/Physics/Manager/ZeroMesher.cs
+++ b/OpenSim/Region/Physics/Manager/ZeroMesher.cs
@@ -64,16 +64,21 @@ namespace OpenSim.Region.Physics.Manager
     {
         public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod)
         {
-            return CreateMesh(primName, primShape, size, lod, false);
+            return CreateMesh(primName, primShape, size, lod, false, false);
         }
 
-        public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool convex,bool forOde)
+        public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool shouldCache, bool convex,bool forOde)
         {
             return CreateMesh(primName, primShape, size, lod, false);
         }
 
         public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical)
         {
+            return CreateMesh(primName, primShape, size, lod, false, false);
+        }
+
+        public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool shouldCache)
+        {
             // Remove the reference to the encoded JPEG2000 data so it can be GCed
             primShape.SculptData = OpenMetaverse.Utils.EmptyBytes;
 
diff --git a/OpenSim/Region/Physics/Meshing/Mesh.cs b/OpenSim/Region/Physics/Meshing/Mesh.cs
index 6970553..e6b32e7 100644
--- a/OpenSim/Region/Physics/Meshing/Mesh.cs
+++ b/OpenSim/Region/Physics/Meshing/Mesh.cs
@@ -226,7 +226,7 @@ namespace OpenSim.Region.Physics.Meshing
             return result;
         }
 
-        private float[] getVertexListAsFloat()
+        public float[] getVertexListAsFloat()
         {
             if (m_vertices == null)
                 throw new NotSupportedException();
diff --git a/OpenSim/Region/Physics/Meshing/Meshmerizer.cs b/OpenSim/Region/Physics/Meshing/Meshmerizer.cs
index f629c4d..d181b78 100644
--- a/OpenSim/Region/Physics/Meshing/Meshmerizer.cs
+++ b/OpenSim/Region/Physics/Meshing/Meshmerizer.cs
@@ -321,6 +321,9 @@ namespace OpenSim.Region.Physics.Meshing
 
             if (primShape.SculptData.Length <= 0)
             {
+                // XXX: At the moment we can not log here since ODEPrim, for instance, ends up triggering this
+                // method twice - once before it has loaded sculpt data from the asset service and once afterwards.
+                // The first time will always call with unloaded SculptData if this needs to be uploaded.
 //                m_log.ErrorFormat("[MESH]: asset data for {0} is zero length", primName);
                 return false;
             }
@@ -699,16 +702,21 @@ namespace OpenSim.Region.Physics.Meshing
 
         public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod)
         {
-            return CreateMesh(primName, primShape, size, lod, false);
+            return CreateMesh(primName, primShape, size, lod, false, true);
         }
 
-        public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool convex, bool forOde)
+        public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool shouldCache, bool convex, bool forOde)
         {
             return CreateMesh(primName, primShape, size, lod, false);
         }
 
         public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical)
         {
+            return CreateMesh(primName, primShape, size, lod, isPhysical, true);
+        }
+
+        public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool shouldCache)
+        {
 #if SPAM
             m_log.DebugFormat("[MESH]: Creating mesh for {0}", primName);
 #endif
@@ -718,9 +726,12 @@ namespace OpenSim.Region.Physics.Meshing
 
             // If this mesh has been created already, return it instead of creating another copy
             // For large regions with 100k+ prims and hundreds of copies of each, this can save a GB or more of memory
-            key = primShape.GetMeshKey(size, lod);
-            if (m_uniqueMeshes.TryGetValue(key, out mesh))
-                return mesh;
+            if (shouldCache)
+            {
+                key = primShape.GetMeshKey(size, lod);
+                if (m_uniqueMeshes.TryGetValue(key, out mesh))
+                    return mesh;
+            }
 
             if (size.X < 0.01f) size.X = 0.01f;
             if (size.Y < 0.01f) size.Y = 0.01f;
@@ -743,7 +754,10 @@ namespace OpenSim.Region.Physics.Meshing
                 // trim the vertex and triangle lists to free up memory
                 mesh.TrimExcess();
 
-                m_uniqueMeshes.Add(key, mesh);
+                if (shouldCache)
+                {
+                    m_uniqueMeshes.Add(key, mesh);
+                }
             }
 
             return mesh;
diff --git a/OpenSim/Region/Physics/Meshing/Properties/AssemblyInfo.cs b/OpenSim/Region/Physics/Meshing/Properties/AssemblyInfo.cs
index 4cc1731..3de061a 100644
--- a/OpenSim/Region/Physics/Meshing/Properties/AssemblyInfo.cs
+++ b/OpenSim/Region/Physics/Meshing/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/OpenSim/Region/Physics/OdePlugin/AssemblyInfo.cs b/OpenSim/Region/Physics/OdePlugin/AssemblyInfo.cs
index 3c4f06a..f477ed1 100644
--- a/OpenSim/Region/Physics/OdePlugin/AssemblyInfo.cs
+++ b/OpenSim/Region/Physics/OdePlugin/AssemblyInfo.cs
@@ -55,4 +55,4 @@ using System.Runtime.InteropServices;
 // You can specify all values by your own or you can build default build and revision
 // numbers with the '*' character (the default):
 
-[assembly : AssemblyVersion("0.7.5.*")]
+[assembly : AssemblyVersion("0.7.6.*")]
diff --git a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
index a59f63f..d09aa62 100644
--- a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
+++ b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
@@ -3367,6 +3367,11 @@ Console.WriteLine(" JointCreateFixed");
                 _pbs.SculptData = new byte[asset.Data.Length];
                 asset.Data.CopyTo(_pbs.SculptData, 0);
 //                m_assetFailed = false;
+
+//                m_log.DebugFormat(
+//                    "[ODE PRIM]: Received mesh/sculpt data asset {0} with {1} bytes for {2} at {3} in {4}", 
+//                    _pbs.SculptTexture, _pbs.SculptData.Length, Name, _position, _parent_scene.Name);
+
                 m_taintshape = true;
                _parent_scene.AddPhysicsActorTaint(this);
             }
diff --git a/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs
index 478dd95..07663b3 100644
--- a/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs
+++ b/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs
@@ -72,7 +72,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                 // http://opensimulator.org/mantis/view.php?id=2750).
                 d.InitODE();
                 
-                m_scene = new OdeScene(sceneIdentifier);
+                m_scene = new OdeScene(GetName(), sceneIdentifier);
             }
 
             return m_scene;
diff --git a/OpenSim/Region/Physics/OdePlugin/OdeScene.cs b/OpenSim/Region/Physics/OdePlugin/OdeScene.cs
index d53bd90..6d7f079 100644
--- a/OpenSim/Region/Physics/OdePlugin/OdeScene.cs
+++ b/OpenSim/Region/Physics/OdePlugin/OdeScene.cs
@@ -526,11 +526,12 @@ namespace OpenSim.Region.Physics.OdePlugin
         /// These settings need to be tweaked 'exactly' right or weird stuff happens.
         /// </summary>
         /// <param value="name">Name of the scene.  Useful in debug messages.</param>
-        public OdeScene(string name)
+        public OdeScene(string engineType, string name)
         {
             m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType.ToString() + "." + name);
 
             Name = name;
+            EngineType = engineType;
 
             nearCallback = near;
             triCallback = TriCallback;
@@ -4095,8 +4096,8 @@ namespace OpenSim.Region.Physics.OdePlugin
             lock (_prims)
             {
                 List<OdePrim> orderedPrims = new List<OdePrim>(_prims);
-                orderedPrims.OrderByDescending(p => p.CollisionScore).Take(25);
-                topColliders = orderedPrims.ToDictionary(p => p.LocalID, p => p.CollisionScore);
+                orderedPrims.OrderByDescending(p => p.CollisionScore);
+                topColliders = orderedPrims.Take(25).ToDictionary(p => p.LocalID, p => p.CollisionScore);
 
                 foreach (OdePrim p in _prims)
                     p.CollisionScore = 0;
diff --git a/OpenSim/Region/Physics/OdePlugin/Tests/ODETestClass.cs b/OpenSim/Region/Physics/OdePlugin/Tests/ODETestClass.cs
index cbc6b95..16404c6 100644
--- a/OpenSim/Region/Physics/OdePlugin/Tests/ODETestClass.cs
+++ b/OpenSim/Region/Physics/OdePlugin/Tests/ODETestClass.cs
@@ -32,13 +32,14 @@ using OpenMetaverse;
 using OpenSim.Framework;
 using OpenSim.Region.Physics.Manager;
 using OpenSim.Region.Physics.OdePlugin;
+using OpenSim.Tests.Common;
 using log4net;
 using System.Reflection;
 
 namespace OpenSim.Region.Physics.OdePlugin.Tests
 {
     [TestFixture]
-    public class ODETestClass
+    public class ODETestClass : OpenSimTestCase
     {
         private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
 
diff --git a/OpenSim/Region/Physics/POSPlugin/AssemblyInfo.cs b/OpenSim/Region/Physics/POSPlugin/AssemblyInfo.cs
index d07df02..4289863 100644
--- a/OpenSim/Region/Physics/POSPlugin/AssemblyInfo.cs
+++ b/OpenSim/Region/Physics/POSPlugin/AssemblyInfo.cs
@@ -55,4 +55,4 @@ using System.Runtime.InteropServices;
 // You can specify all values by your own or you can build default build and revision
 // numbers with the '*' character (the default):
 
-[assembly : AssemblyVersion("0.7.5.*")]
+[assembly : AssemblyVersion("0.7.6.*")]
diff --git a/OpenSim/Region/Physics/POSPlugin/POSPlugin.cs b/OpenSim/Region/Physics/POSPlugin/POSPlugin.cs
index e6b42e6..ed086dd 100644
--- a/OpenSim/Region/Physics/POSPlugin/POSPlugin.cs
+++ b/OpenSim/Region/Physics/POSPlugin/POSPlugin.cs
@@ -49,7 +49,7 @@ namespace OpenSim.Region.Physics.POSPlugin
 
         public PhysicsScene GetScene(string sceneIdentifier)
         {
-            return new POSScene(sceneIdentifier);
+            return new POSScene(GetName(), sceneIdentifier);
         }
 
         public string GetName()
diff --git a/OpenSim/Region/Physics/POSPlugin/POSScene.cs b/OpenSim/Region/Physics/POSPlugin/POSScene.cs
index 2f24a50..d30d482 100644
--- a/OpenSim/Region/Physics/POSPlugin/POSScene.cs
+++ b/OpenSim/Region/Physics/POSPlugin/POSScene.cs
@@ -43,8 +43,10 @@ namespace OpenSim.Region.Physics.POSPlugin
 
         //protected internal string sceneIdentifier;
 
-        public POSScene(String _sceneIdentifier)
+        public POSScene(string engineType, String _sceneIdentifier)
         {
+            EngineType = engineType;
+            Name = EngineType + "/" + _sceneIdentifier;
             //sceneIdentifier = _sceneIdentifier;
         }
 
diff --git a/OpenSim/Region/Physics/UbitMeshing/Mesh.cs b/OpenSim/Region/Physics/UbitMeshing/Mesh.cs
index b67422f..fa06926 100644
--- a/OpenSim/Region/Physics/UbitMeshing/Mesh.cs
+++ b/OpenSim/Region/Physics/UbitMeshing/Mesh.cs
@@ -301,7 +301,7 @@ namespace OpenSim.Region.Physics.Meshing
             return result;
         }
 
-        private float[] getVertexListAsFloat()
+        public float[] getVertexListAsFloat()
         {
             if (m_bdata.m_vertices == null)
                 throw new NotSupportedException();
diff --git a/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs b/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs
index 6e1a105..00cbfbd 100644
--- a/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs
+++ b/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs
@@ -1031,12 +1031,12 @@ namespace OpenSim.Region.Physics.Meshing
 
         public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod)
         {
-            return CreateMesh(primName, primShape, size, lod, false,false,false);
+            return CreateMesh(primName, primShape, size, lod, false,false,false,false);
         }
 
         public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical)
         {
-            return CreateMesh(primName, primShape, size, lod, false,false,false);
+            return CreateMesh(primName, primShape, size, lod, false,false,false,false);
         }
 
         public IMesh GetMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool convex)
@@ -1080,7 +1080,7 @@ namespace OpenSim.Region.Physics.Meshing
 
         private static Vector3 m_MeshUnitSize = new Vector3(1.0f, 1.0f, 1.0f);
         
-        public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool convex, bool forOde)
+        public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool shouldCache, bool convex, bool forOde)
         {
 #if SPAM
             m_log.DebugFormat("[MESH]: Creating mesh for {0}", primName);
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEMeshWorker.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEMeshWorker.cs
index 5030cec..0df71eb 100644
--- a/OpenSim/Region/Physics/UbitOdePlugin/ODEMeshWorker.cs
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEMeshWorker.cs
@@ -448,7 +448,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                 else
                 {
                     repData.meshState = MeshState.needMesh;
-                    mesh = m_mesher.CreateMesh(actor.Name, pbs, size, clod, true, convex, true);
+                    mesh = m_mesher.CreateMesh(actor.Name, pbs, size, clod, true, false, convex, true);
                     if (mesh == null)
                     {
                         repData.meshState = MeshState.MeshFailed;
@@ -513,7 +513,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                     clod = (int)LevelOfDetail.Low;
             }
 
-            mesh = m_mesher.CreateMesh(actor.Name, pbs, size, clod, true, convex, true);
+            mesh = m_mesher.CreateMesh(actor.Name, pbs, size, clod, true, false, convex, true);
    
             if (mesh == null)
             {
@@ -929,4 +929,4 @@ namespace OpenSim.Region.Physics.OdePlugin
                     repData.actor.Name);
         }
     }
-}
\ No newline at end of file
+}
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs
index 4f598ea..2923ccf 100644
--- a/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs
@@ -300,9 +300,9 @@ namespace OpenSim.Region.Physics.OdePlugin
                 ((ProbeSphereCallback)req.callbackMethod)(cresult);
         }
 
-        private const RayFilterFlags FilterActiveSpace = RayFilterFlags.agent | RayFilterFlags.physical | RayFilterFlags.LSLPhanton;
+        private const RayFilterFlags FilterActiveSpace = RayFilterFlags.agent | RayFilterFlags.physical | RayFilterFlags.LSLPhantom;
 //        private const RayFilterFlags FilterStaticSpace = RayFilterFlags.water | RayFilterFlags.land | RayFilterFlags.nonphysical | RayFilterFlags.LSLPhanton;
-        private const RayFilterFlags FilterStaticSpace = RayFilterFlags.water | RayFilterFlags.nonphysical | RayFilterFlags.LSLPhanton;
+        private const RayFilterFlags FilterStaticSpace = RayFilterFlags.water | RayFilterFlags.nonphysical | RayFilterFlags.LSLPhantom;
 
         private void doSpaceRay(ODERayRequest req)
         {
@@ -680,4 +680,4 @@ namespace OpenSim.Region.Physics.OdePlugin
         public RayFilterFlags filter;
         public Quaternion orientation;
     }
-}
\ No newline at end of file
+}