BulletSim: subclass Bullet[World|Body|Shape|Constraint] for unmanaged

to have pointers and managed to have objects.
Initial paste of XNA code. Commented out.

6 files changed, 1806 insertions, 278 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSAPIUnman.cs b/OpenSim/Region/Physics/BulletSPlugin/BSAPIUnman.cs
index 3975776..2c0cb43 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSAPIUnman.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSAPIUnman.cs
@@ -38,6 +38,91 @@ 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;
@@ -89,7 +174,7 @@ public override BulletWorld Initialize(Vector3 maxPosition, ConfigurationParamet
     BulletEngineVersion = "";
 
     // Call the unmanaged code with the buffers and other information
-    return new BulletWorld(0, PhysicsScene, BSAPICPP.Initialize2(maxPosition, m_paramsHandle.AddrOfPinnedObject(),
+    return new BulletWorldUnman(0, PhysicsScene, BSAPICPP.Initialize2(maxPosition, m_paramsHandle.AddrOfPinnedObject(),
                                     maxCollisions, m_collisionArrayPinnedHandle.AddrOfPinnedObject(),
                                     maxUpdates, m_updateArrayPinnedHandle.AddrOfPinnedObject(),
                                     m_DebugLogCallbackHandle));
@@ -111,22 +196,26 @@ private void BulletLoggerPhysLog(string msg)
 public override int PhysicsStep(BulletWorld world, float timeStep, int maxSubSteps, float fixedTimeStep,
                         out int updatedEntityCount, out int collidersCount)
 {
-    return BSAPICPP.PhysicsStep2(world.ptr, timeStep, maxSubSteps, fixedTimeStep, out updatedEntityCount, out collidersCount);
+    BulletWorldUnman worldu = world as BulletWorldUnman;
+    return BSAPICPP.PhysicsStep2(worldu.ptr, timeStep, maxSubSteps, fixedTimeStep, out updatedEntityCount, out collidersCount);
 }
 
-public override void Shutdown(BulletWorld sim)
+public override void Shutdown(BulletWorld world)
 {
-    BSAPICPP.Shutdown2(sim.ptr);
+    BulletWorldUnman worldu = world as BulletWorldUnman;

+    BSAPICPP.Shutdown2(worldu.ptr);
 }
 
 public override bool PushUpdate(BulletBody obj)
 {
-    return BSAPICPP.PushUpdate2(obj.ptr);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.PushUpdate2(bodyu.ptr);
 }
 
 public override bool UpdateParameter(BulletWorld world, uint localID, String parm, float value)
 {
-    return BSAPICPP.UpdateParameter2(world.ptr, localID, parm, value);
+    BulletWorldUnman worldu = world as BulletWorldUnman;
+    return BSAPICPP.UpdateParameter2(worldu.ptr, localID, parm, value);
 }
 
 // =====================================================================================
@@ -135,138 +224,165 @@ public override BulletShape CreateMeshShape(BulletWorld world,
                 int indicesCount, int[] indices,
                 int verticesCount, float[] vertices)
 {
-    return new BulletShape(
-                    BSAPICPP.CreateMeshShape2(world.ptr, indicesCount, indices, verticesCount, 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)
 {
-    return new BulletShape(
-                    BSAPICPP.CreateHullShape2(world.ptr, hullCount, 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)
 {
-    return new BulletShape(
-                    BSAPICPP.BuildHullShapeFromMesh2(world.ptr, meshShape.ptr),
+    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)
+public override BulletShape BuildNativeShape(BulletWorld world, ShapeData shapeData)
 {
-    return new BulletShape(
-                    BSAPICPP.BuildNativeShape2(world.ptr, shapeData), 
-                    shapeData.Type);
+    BulletWorldUnman worldu = world as BulletWorldUnman;

+    return new BulletShapeUnman(BSAPICPP.BuildNativeShape2(worldu.ptr, shapeData), shapeData.Type);
 }
 
 public override bool IsNativeShape(BulletShape shape)
 {
-    if (shape.HasPhysicalShape)
-        return BSAPICPP.IsNativeShape2(shape.ptr);
+    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)
 {
-    if (shape.HasPhysicalShape)
-        BSAPICPP.SetShapeCollisionMargin2(shape.ptr, margin);
+    BulletShapeUnman shapeu = shape as BulletShapeUnman;
+    if (shapeu != null && shapeu.HasPhysicalShape)
+        BSAPICPP.SetShapeCollisionMargin2(shapeu.ptr, margin);
 }
 
 public override BulletShape BuildCapsuleShape(BulletWorld world, float radius, float height, Vector3 scale)
 {
-    return new BulletShape(
-                   BSAPICPP.BuildCapsuleShape2(world.ptr, radius, height, scale),
+    BulletWorldUnman worldu = world as BulletWorldUnman;
+    return new BulletShapeUnman(
+                   BSAPICPP.BuildCapsuleShape2(worldu.ptr, radius, height, scale),
                    BSPhysicsShapeType.SHAPE_CAPSULE);
 }
 
-public override BulletShape CreateCompoundShape(BulletWorld sim, bool enableDynamicAabbTree)
+public override BulletShape CreateCompoundShape(BulletWorld world, bool enableDynamicAabbTree)
 {
-    return new BulletShape(
-                    BSAPICPP.CreateCompoundShape2(sim.ptr, enableDynamicAabbTree),
+    BulletWorldUnman worldu = world as BulletWorldUnman;
+    return new BulletShapeUnman(
+                    BSAPICPP.CreateCompoundShape2(worldu.ptr, enableDynamicAabbTree),
                     BSPhysicsShapeType.SHAPE_COMPOUND);
 
 }
 
 public override int GetNumberOfCompoundChildren(BulletShape shape)
 {
-    if (shape.HasPhysicalShape)
-        return BSAPICPP.GetNumberOfCompoundChildren2(shape.ptr);
+    BulletShapeUnman shapeu = shape as BulletShapeUnman;
+    if (shapeu != null && shapeu.HasPhysicalShape)
+        return BSAPICPP.GetNumberOfCompoundChildren2(shapeu.ptr);
     return 0;
 }
 
-public override void AddChildShapeToCompoundShape(BulletShape cShape, BulletShape addShape, Vector3 pos, Quaternion rot)
+public override void AddChildShapeToCompoundShape(BulletShape shape, BulletShape addShape, Vector3 pos, Quaternion rot)
 {
-    BSAPICPP.AddChildShapeToCompoundShape2(cShape.ptr, addShape.ptr, pos, rot);
+    BulletShapeUnman shapeu = shape as BulletShapeUnman;
+    BulletShapeUnman addShapeu = addShape as BulletShapeUnman;
+    BSAPICPP.AddChildShapeToCompoundShape2(shapeu.ptr, addShapeu.ptr, pos, rot);
 }
 
-public override BulletShape GetChildShapeFromCompoundShapeIndex(BulletShape cShape, int indx)
+public override BulletShape GetChildShapeFromCompoundShapeIndex(BulletShape shape, int indx)
 {
-    return new BulletShape(BSAPICPP.GetChildShapeFromCompoundShapeIndex2(cShape.ptr, indx));
+    BulletShapeUnman shapeu = shape as BulletShapeUnman;
+    return new BulletShapeUnman(BSAPICPP.GetChildShapeFromCompoundShapeIndex2(shapeu.ptr, indx), BSPhysicsShapeType.SHAPE_UNKNOWN);
 }
 
-public override BulletShape RemoveChildShapeFromCompoundShapeIndex(BulletShape cShape, int indx)
+public override BulletShape RemoveChildShapeFromCompoundShapeIndex(BulletShape shape, int indx)
 {
-    return new BulletShape(BSAPICPP.RemoveChildShapeFromCompoundShapeIndex2(cShape.ptr, indx));
+    BulletShapeUnman shapeu = shape as BulletShapeUnman;
+    return new BulletShapeUnman(BSAPICPP.RemoveChildShapeFromCompoundShapeIndex2(shapeu.ptr, indx), BSPhysicsShapeType.SHAPE_UNKNOWN);
 }
 
-public override void RemoveChildShapeFromCompoundShape(BulletShape cShape, BulletShape removeShape)
+public override void RemoveChildShapeFromCompoundShape(BulletShape shape, BulletShape removeShape)
 {
-    BSAPICPP.RemoveChildShapeFromCompoundShape2(cShape.ptr, removeShape.ptr);
+    BulletShapeUnman shapeu = shape as BulletShapeUnman;
+    BulletShapeUnman removeShapeu = removeShape as BulletShapeUnman;
+    BSAPICPP.RemoveChildShapeFromCompoundShape2(shapeu.ptr, removeShapeu.ptr);
 }
 
-public override void RecalculateCompoundShapeLocalAabb(BulletShape cShape)
+public override void RecalculateCompoundShapeLocalAabb(BulletShape shape)
 {
-    BSAPICPP.RecalculateCompoundShapeLocalAabb2(cShape.ptr);
+    BulletShapeUnman shapeu = shape as BulletShapeUnman;
+    BSAPICPP.RecalculateCompoundShapeLocalAabb2(shapeu.ptr);
 }
 
-public override BulletShape DuplicateCollisionShape(BulletWorld sim, BulletShape srcShape, uint id)
+public override BulletShape DuplicateCollisionShape(BulletWorld world, BulletShape srcShape, uint id)
 {
-    return new BulletShape(BSAPICPP.DuplicateCollisionShape2(sim.ptr, srcShape.ptr, id), srcShape.type);
+    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)
 {
-    return BSAPICPP.DeleteCollisionShape2(world.ptr, shape.ptr);
+    BulletWorldUnman worldu = world as BulletWorldUnman;

+    BulletShapeUnman shapeu = shape as BulletShapeUnman;
+    return BSAPICPP.DeleteCollisionShape2(worldu.ptr, shapeu.ptr);
 }
 
 public override int GetBodyType(BulletBody obj)
 {
-    return BSAPICPP.GetBodyType2(obj.ptr);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.GetBodyType2(bodyu.ptr);
 }
 
-public override BulletBody CreateBodyFromShape(BulletWorld sim, BulletShape shape, uint id, Vector3 pos, Quaternion rot)
+public override BulletBody CreateBodyFromShape(BulletWorld world, BulletShape shape, uint id, Vector3 pos, Quaternion rot)
 {
-    return new BulletBody(id, BSAPICPP.CreateBodyFromShape2(sim.ptr, shape.ptr, id, pos, 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)
 {
-    return new BulletBody(id, BSAPICPP.CreateBodyWithDefaultMotionState2(shape.ptr, id, pos, rot));
+    BulletShapeUnman shapeu = shape as BulletShapeUnman;
+    return new BulletBodyUnman(id, BSAPICPP.CreateBodyWithDefaultMotionState2(shapeu.ptr, id, pos, rot));
 }
 
-public override BulletBody CreateGhostFromShape(BulletWorld sim, BulletShape shape, uint id, Vector3 pos, Quaternion rot)
+public override BulletBody CreateGhostFromShape(BulletWorld world, BulletShape shape, uint id, Vector3 pos, Quaternion rot)
 {
-    return new BulletBody(id, BSAPICPP.CreateGhostFromShape2(sim.ptr, shape.ptr, id, pos, 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 sim, BulletBody obj)
+public override void DestroyObject(BulletWorld world, BulletBody obj)
 {
-    BSAPICPP.DestroyObject2(sim.ptr, obj.ptr);
+    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 BulletShape(BSAPICPP.CreateGroundPlaneShape2(id, height, collisionMargin), BSPhysicsShapeType.SHAPE_GROUNDPLANE);
+    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 BulletShape(BSAPICPP.CreateTerrainShape2(id, size, minHeight, maxHeight, heightMap, scaleFactor, collisionMargin),
+    return new BulletShapeUnman(BSAPICPP.CreateTerrainShape2(id, size, minHeight, maxHeight, heightMap, scaleFactor, collisionMargin),
                                                 BSPhysicsShapeType.SHAPE_TERRAIN);
 }
 
@@ -277,7 +393,10 @@ public override BulletConstraint Create6DofConstraint(BulletWorld world, BulletB
                     Vector3 frame2loc, Quaternion frame2rot,
                     bool useLinearReferenceFrameA, bool disableCollisionsBetweenLinkedBodies)
 {
-    return new BulletConstraint(BSAPICPP.Create6DofConstraint2(world.ptr, obj1.ptr, obj2.ptr, frame1loc, frame1rot,
+    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));
 }
 
@@ -285,7 +404,10 @@ public override BulletConstraint Create6DofConstraintToPoint(BulletWorld world,
                     Vector3 joinPoint,
                     bool useLinearReferenceFrameA, bool disableCollisionsBetweenLinkedBodies)
 {
-    return new BulletConstraint(BSAPICPP.Create6DofConstraintToPoint2(world.ptr, obj1.ptr, obj2.ptr,
+    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));
 }
 
@@ -294,560 +416,687 @@ public override BulletConstraint CreateHingeConstraint(BulletWorld world, Bullet
                     Vector3 axisInA, Vector3 axisInB,
                     bool useLinearReferenceFrameA, bool disableCollisionsBetweenLinkedBodies)
 {
-    return new BulletConstraint(BSAPICPP.CreateHingeConstraint2(world.ptr, obj1.ptr, obj2.ptr,
+    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 void SetConstraintEnable(BulletConstraint constrain, float numericTrueFalse)
 {
-    BSAPICPP.SetConstraintEnable2(constrain.ptr, numericTrueFalse);
+    BulletConstraintUnman constrainu = constrain as BulletConstraintUnman;
+    BSAPICPP.SetConstraintEnable2(constrainu.ptr, numericTrueFalse);
 }
 
 public override void SetConstraintNumSolverIterations(BulletConstraint constrain, float iterations)
 {
-    BSAPICPP.SetConstraintNumSolverIterations2(constrain.ptr, 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)
 {
-    return BSAPICPP.SetFrames2(constrain.ptr, frameA, frameArot, frameB, 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)
 {
-    return BSAPICPP.SetLinearLimits2(constrain.ptr, low, hi);
+    BulletConstraintUnman constrainu = constrain as BulletConstraintUnman;
+    return BSAPICPP.SetLinearLimits2(constrainu.ptr, low, hi);
 }
 
 public override bool SetAngularLimits(BulletConstraint constrain, Vector3 low, Vector3 hi)
 {
-    return BSAPICPP.SetAngularLimits2(constrain.ptr, low, hi);
+    BulletConstraintUnman constrainu = constrain as BulletConstraintUnman;
+    return BSAPICPP.SetAngularLimits2(constrainu.ptr, low, hi);
 }
 
 public override bool UseFrameOffset(BulletConstraint constrain, float enable)
 {
-    return BSAPICPP.UseFrameOffset2(constrain.ptr, enable);
+    BulletConstraintUnman constrainu = constrain as BulletConstraintUnman;
+    return BSAPICPP.UseFrameOffset2(constrainu.ptr, enable);
 }
 
 public override bool TranslationalLimitMotor(BulletConstraint constrain, float enable, float targetVel, float maxMotorForce)
 {
-    return BSAPICPP.TranslationalLimitMotor2(constrain.ptr, enable, targetVel, maxMotorForce);
+    BulletConstraintUnman constrainu = constrain as BulletConstraintUnman;
+    return BSAPICPP.TranslationalLimitMotor2(constrainu.ptr, enable, targetVel, maxMotorForce);
 }
 
 public override bool SetBreakingImpulseThreshold(BulletConstraint constrain, float threshold)
 {
-    return BSAPICPP.SetBreakingImpulseThreshold2(constrain.ptr, threshold);
+    BulletConstraintUnman constrainu = constrain as BulletConstraintUnman;
+    return BSAPICPP.SetBreakingImpulseThreshold2(constrainu.ptr, threshold);
 }
 
 public override bool CalculateTransforms(BulletConstraint constrain)
 {
-    return BSAPICPP.CalculateTransforms2(constrain.ptr);
+    BulletConstraintUnman constrainu = constrain as BulletConstraintUnman;
+    return BSAPICPP.CalculateTransforms2(constrainu.ptr);
 }
 
 public override bool SetConstraintParam(BulletConstraint constrain, ConstraintParams paramIndex, float value, ConstraintParamAxis axis)
 {
-    return BSAPICPP.SetConstraintParam2(constrain.ptr, paramIndex, value, axis);
+    BulletConstraintUnman constrainu = constrain as BulletConstraintUnman;
+    return BSAPICPP.SetConstraintParam2(constrainu.ptr, paramIndex, value, axis);
 }
 
 public override bool DestroyConstraint(BulletWorld world, BulletConstraint constrain)
 {
-    return BSAPICPP.DestroyConstraint2(world.ptr, constrain.ptr);
+    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)
-{
-    BSAPICPP.UpdateSingleAabb2(world.ptr, obj.ptr);
+{

+    BulletWorldUnman worldu = world as BulletWorldUnman;
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.UpdateSingleAabb2(worldu.ptr, bodyu.ptr);
 }
 
 public override void UpdateAabbs(BulletWorld world)
 {
-    BSAPICPP.UpdateAabbs2(world.ptr);
+    BulletWorldUnman worldu = world as BulletWorldUnman;

+    BSAPICPP.UpdateAabbs2(worldu.ptr);
 }
 
 public override bool GetForceUpdateAllAabbs(BulletWorld world)
 {
-    return BSAPICPP.GetForceUpdateAllAabbs2(world.ptr);
+    BulletWorldUnman worldu = world as BulletWorldUnman;

+    return BSAPICPP.GetForceUpdateAllAabbs2(worldu.ptr);
 }
 
 public override void SetForceUpdateAllAabbs(BulletWorld world, bool force)
 {
-    BSAPICPP.SetForceUpdateAllAabbs2(world.ptr, force);
+    BulletWorldUnman worldu = world as BulletWorldUnman;

+    BSAPICPP.SetForceUpdateAllAabbs2(worldu.ptr, force);
 }
 
 // =====================================================================================
 // btDynamicsWorld entries
 public override bool AddObjectToWorld(BulletWorld world, BulletBody obj)
 {
-    return BSAPICPP.AddObjectToWorld2(world.ptr, obj.ptr);
+    BulletWorldUnman worldu = world as BulletWorldUnman;

+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.AddObjectToWorld2(worldu.ptr, bodyu.ptr);
 }
 
 public override bool RemoveObjectFromWorld(BulletWorld world, BulletBody obj)
 {
-    return BSAPICPP.RemoveObjectFromWorld2(world.ptr, obj.ptr);
+    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)
 {
-    return BSAPICPP.AddConstraintToWorld2(world.ptr, constrain.ptr, 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)
 {
-    return BSAPICPP.RemoveConstraintFromWorld2(world.ptr, constrain.ptr);
+    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)
 {
-    return BSAPICPP.GetAnisotripicFriction2(constrain.ptr);
+    BulletConstraintUnman constrainu = constrain as BulletConstraintUnman;
+    return BSAPICPP.GetAnisotripicFriction2(constrainu.ptr);
 }
 
 public override Vector3 SetAnisotripicFriction(BulletConstraint constrain, Vector3 frict)
 {
-    return BSAPICPP.SetAnisotripicFriction2(constrain.ptr, frict);
+    BulletConstraintUnman constrainu = constrain as BulletConstraintUnman;
+    return BSAPICPP.SetAnisotripicFriction2(constrainu.ptr, frict);
 }
 
 public override bool HasAnisotripicFriction(BulletConstraint constrain)
 {
-    return BSAPICPP.HasAnisotripicFriction2(constrain.ptr);
+    BulletConstraintUnman constrainu = constrain as BulletConstraintUnman;
+    return BSAPICPP.HasAnisotripicFriction2(constrainu.ptr);
 }
 
 public override void SetContactProcessingThreshold(BulletBody obj, float val)
 {
-    BSAPICPP.SetContactProcessingThreshold2(obj.ptr, val);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.SetContactProcessingThreshold2(bodyu.ptr, val);
 }
 
 public override float GetContactProcessingThreshold(BulletBody obj)
 {
-    return BSAPICPP.GetContactProcessingThreshold2(obj.ptr);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.GetContactProcessingThreshold2(bodyu.ptr);
 }
 
 public override bool IsStaticObject(BulletBody obj)
 {
-    return BSAPICPP.IsStaticObject2(obj.ptr);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.IsStaticObject2(bodyu.ptr);
 }
 
 public override bool IsKinematicObject(BulletBody obj)
 {
-    return BSAPICPP.IsKinematicObject2(obj.ptr);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.IsKinematicObject2(bodyu.ptr);
 }
 
 public override bool IsStaticOrKinematicObject(BulletBody obj)
 {
-    return BSAPICPP.IsStaticOrKinematicObject2(obj.ptr);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.IsStaticOrKinematicObject2(bodyu.ptr);
 }
 
 public override bool HasContactResponse(BulletBody obj)
 {
-    return BSAPICPP.HasContactResponse2(obj.ptr);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.HasContactResponse2(bodyu.ptr);
 }
 
-public override void SetCollisionShape(BulletWorld sim, BulletBody obj, BulletShape shape)
+public override void SetCollisionShape(BulletWorld world, BulletBody obj, BulletShape shape)
 {
-    BSAPICPP.SetCollisionShape2(sim.ptr, obj.ptr, shape.ptr);
+    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)
 {
-    return new BulletShape(BSAPICPP.GetCollisionShape2(obj.ptr));
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return new BulletShapeUnman(BSAPICPP.GetCollisionShape2(bodyu.ptr), BSPhysicsShapeType.SHAPE_UNKNOWN);
 }
 
 public override int GetActivationState(BulletBody obj)
 {
-    return BSAPICPP.GetActivationState2(obj.ptr);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.GetActivationState2(bodyu.ptr);
 }
 
 public override void SetActivationState(BulletBody obj, int state)
 {
-    BSAPICPP.SetActivationState2(obj.ptr, state);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.SetActivationState2(bodyu.ptr, state);
 }
 
 public override void SetDeactivationTime(BulletBody obj, float dtime)
 {
-    BSAPICPP.SetDeactivationTime2(obj.ptr, dtime);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.SetDeactivationTime2(bodyu.ptr, dtime);
 }
 
 public override float GetDeactivationTime(BulletBody obj)
 {
-    return BSAPICPP.GetDeactivationTime2(obj.ptr);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.GetDeactivationTime2(bodyu.ptr);
 }
 
 public override void ForceActivationState(BulletBody obj, ActivationState state)
 {
-    BSAPICPP.ForceActivationState2(obj.ptr, state);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.ForceActivationState2(bodyu.ptr, state);
 }
 
 public override void Activate(BulletBody obj, bool forceActivation)
 {
-    BSAPICPP.Activate2(obj.ptr, forceActivation);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.Activate2(bodyu.ptr, forceActivation);
 }
 
 public override bool IsActive(BulletBody obj)
 {
-    return BSAPICPP.IsActive2(obj.ptr);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.IsActive2(bodyu.ptr);
 }
 
 public override void SetRestitution(BulletBody obj, float val)
 {
-    BSAPICPP.SetRestitution2(obj.ptr, val);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.SetRestitution2(bodyu.ptr, val);
 }
 
 public override float GetRestitution(BulletBody obj)
 {
-    return BSAPICPP.GetRestitution2(obj.ptr);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.GetRestitution2(bodyu.ptr);
 }
 
 public override void SetFriction(BulletBody obj, float val)
 {
-    BSAPICPP.SetFriction2(obj.ptr, val);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.SetFriction2(bodyu.ptr, val);
 }
 
 public override float GetFriction(BulletBody obj)
 {
-    return BSAPICPP.GetFriction2(obj.ptr);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.GetFriction2(bodyu.ptr);
 }
 
 public override Vector3 GetPosition(BulletBody obj)
 {
-    return BSAPICPP.GetPosition2(obj.ptr);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.GetPosition2(bodyu.ptr);
 }
 
 public override Quaternion GetOrientation(BulletBody obj)
 {
-    return BSAPICPP.GetOrientation2(obj.ptr);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.GetOrientation2(bodyu.ptr);
 }
 
 public override void SetTranslation(BulletBody obj, Vector3 position, Quaternion rotation)
 {
-    BSAPICPP.SetTranslation2(obj.ptr, position, rotation);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.SetTranslation2(bodyu.ptr, position, rotation);
 }
 
     /*
 public override IntPtr GetBroadphaseHandle(BulletBody obj)
 {
-    return BSAPICPP.GetBroadphaseHandle2(obj.ptr);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.GetBroadphaseHandle2(bodyu.ptr);
 }
 
 public override void SetBroadphaseHandle(BulletBody obj, IntPtr handle)
 {
-    BSAPICPP.SetUserPointer2(obj.ptr, handle);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.SetUserPointer2(bodyu.ptr, handle);
 }
      */
 
 public override void SetInterpolationLinearVelocity(BulletBody obj, Vector3 vel)
 {
-    BSAPICPP.SetInterpolationLinearVelocity2(obj.ptr, vel);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.SetInterpolationLinearVelocity2(bodyu.ptr, vel);
 }
 
 public override void SetInterpolationAngularVelocity(BulletBody obj, Vector3 vel)
 {
-    BSAPICPP.SetInterpolationAngularVelocity2(obj.ptr, vel);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.SetInterpolationAngularVelocity2(bodyu.ptr, vel);
 }
 
 public override void SetInterpolationVelocity(BulletBody obj, Vector3 linearVel, Vector3 angularVel)
 {
-    BSAPICPP.SetInterpolationVelocity2(obj.ptr, linearVel, angularVel);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.SetInterpolationVelocity2(bodyu.ptr, linearVel, angularVel);
 }
 
 public override float GetHitFraction(BulletBody obj)
 {
-    return BSAPICPP.GetHitFraction2(obj.ptr);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.GetHitFraction2(bodyu.ptr);
 }
 
 public override void SetHitFraction(BulletBody obj, float val)
 {
-    BSAPICPP.SetHitFraction2(obj.ptr, val);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.SetHitFraction2(bodyu.ptr, val);
 }
 
 public override CollisionFlags GetCollisionFlags(BulletBody obj)
 {
-    return BSAPICPP.GetCollisionFlags2(obj.ptr);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.GetCollisionFlags2(bodyu.ptr);
 }
 
 public override CollisionFlags SetCollisionFlags(BulletBody obj, CollisionFlags flags)
 {
-    return BSAPICPP.SetCollisionFlags2(obj.ptr, flags);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.SetCollisionFlags2(bodyu.ptr, flags);
 }
 
 public override CollisionFlags AddToCollisionFlags(BulletBody obj, CollisionFlags flags)
 {
-    return BSAPICPP.AddToCollisionFlags2(obj.ptr, flags);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.AddToCollisionFlags2(bodyu.ptr, flags);
 }
 
 public override CollisionFlags RemoveFromCollisionFlags(BulletBody obj, CollisionFlags flags)
 {
-    return BSAPICPP.RemoveFromCollisionFlags2(obj.ptr, flags);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.RemoveFromCollisionFlags2(bodyu.ptr, flags);
 }
 
 public override float GetCcdMotionThreshold(BulletBody obj)
 {
-    return BSAPICPP.GetCcdMotionThreshold2(obj.ptr);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.GetCcdMotionThreshold2(bodyu.ptr);
 }
 
 
 public override void SetCcdMotionThreshold(BulletBody obj, float val)
 {
-    BSAPICPP.SetCcdMotionThreshold2(obj.ptr, val);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.SetCcdMotionThreshold2(bodyu.ptr, val);
 }
 
 public override float GetCcdSweptSphereRadius(BulletBody obj)
 {
-    return BSAPICPP.GetCcdSweptSphereRadius2(obj.ptr);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.GetCcdSweptSphereRadius2(bodyu.ptr);
 }
 
 public override void SetCcdSweptSphereRadius(BulletBody obj, float val)
 {
-    BSAPICPP.SetCcdSweptSphereRadius2(obj.ptr, val);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.SetCcdSweptSphereRadius2(bodyu.ptr, val);
 }
 
 public override IntPtr GetUserPointer(BulletBody obj)
 {
-    return BSAPICPP.GetUserPointer2(obj.ptr);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.GetUserPointer2(bodyu.ptr);
 }
 
 public override void SetUserPointer(BulletBody obj, IntPtr val)
 {
-    BSAPICPP.SetUserPointer2(obj.ptr, val);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.SetUserPointer2(bodyu.ptr, val);
 }
 
 // =====================================================================================
 // btRigidBody entries
 public override void ApplyGravity(BulletBody obj)
 {
-    BSAPICPP.ApplyGravity2(obj.ptr);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.ApplyGravity2(bodyu.ptr);
 }
 
 public override void SetGravity(BulletBody obj, Vector3 val)
 {
-    BSAPICPP.SetGravity2(obj.ptr, val);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.SetGravity2(bodyu.ptr, val);
 }
 
 public override Vector3 GetGravity(BulletBody obj)
 {
-    return BSAPICPP.GetGravity2(obj.ptr);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.GetGravity2(bodyu.ptr);
 }
 
 public override void SetDamping(BulletBody obj, float lin_damping, float ang_damping)
 {
-    BSAPICPP.SetDamping2(obj.ptr, lin_damping, ang_damping);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.SetDamping2(bodyu.ptr, lin_damping, ang_damping);
 }
 
 public override void SetLinearDamping(BulletBody obj, float lin_damping)
 {
-    BSAPICPP.SetLinearDamping2(obj.ptr, lin_damping);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.SetLinearDamping2(bodyu.ptr, lin_damping);
 }
 
 public override void SetAngularDamping(BulletBody obj, float ang_damping)
 {
-    BSAPICPP.SetAngularDamping2(obj.ptr, ang_damping);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.SetAngularDamping2(bodyu.ptr, ang_damping);
 }
 
 public override float GetLinearDamping(BulletBody obj)
 {
-    return BSAPICPP.GetLinearDamping2(obj.ptr);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.GetLinearDamping2(bodyu.ptr);
 }
 
 public override float GetAngularDamping(BulletBody obj)
 {
-    return BSAPICPP.GetAngularDamping2(obj.ptr);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.GetAngularDamping2(bodyu.ptr);
 }
 
 public override float GetLinearSleepingThreshold(BulletBody obj)
 {
-    return BSAPICPP.GetLinearSleepingThreshold2(obj.ptr);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.GetLinearSleepingThreshold2(bodyu.ptr);
 }
 
 public override void ApplyDamping(BulletBody obj, float timeStep)
 {
-    BSAPICPP.ApplyDamping2(obj.ptr, timeStep);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.ApplyDamping2(bodyu.ptr, timeStep);
 }
 
 public override void SetMassProps(BulletBody obj, float mass, Vector3 inertia)
 {
-    BSAPICPP.SetMassProps2(obj.ptr, mass, inertia);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.SetMassProps2(bodyu.ptr, mass, inertia);
 }
 
 public override Vector3 GetLinearFactor(BulletBody obj)
 {
-    return BSAPICPP.GetLinearFactor2(obj.ptr);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.GetLinearFactor2(bodyu.ptr);
 }
 
 public override void SetLinearFactor(BulletBody obj, Vector3 factor)
 {
-    BSAPICPP.SetLinearFactor2(obj.ptr, factor);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.SetLinearFactor2(bodyu.ptr, factor);
 }
 
 public override void SetCenterOfMassByPosRot(BulletBody obj, Vector3 pos, Quaternion rot)
 {
-    BSAPICPP.SetCenterOfMassByPosRot2(obj.ptr, pos, rot);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.SetCenterOfMassByPosRot2(bodyu.ptr, pos, rot);
 }
 
 // Add a force to the object as if its mass is one.
 public override void ApplyCentralForce(BulletBody obj, Vector3 force)
 {
-    BSAPICPP.ApplyCentralForce2(obj.ptr, 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)
 {
-    BSAPICPP.SetObjectForce2(obj.ptr, force);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.SetObjectForce2(bodyu.ptr, force);
 }
 
 public override Vector3 GetTotalForce(BulletBody obj)
 {
-    return BSAPICPP.GetTotalForce2(obj.ptr);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.GetTotalForce2(bodyu.ptr);
 }
 
 public override Vector3 GetTotalTorque(BulletBody obj)
 {
-    return BSAPICPP.GetTotalTorque2(obj.ptr);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.GetTotalTorque2(bodyu.ptr);
 }
 
 public override Vector3 GetInvInertiaDiagLocal(BulletBody obj)
 {
-    return BSAPICPP.GetInvInertiaDiagLocal2(obj.ptr);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.GetInvInertiaDiagLocal2(bodyu.ptr);
 }
 
 public override void SetInvInertiaDiagLocal(BulletBody obj, Vector3 inert)
 {
-    BSAPICPP.SetInvInertiaDiagLocal2(obj.ptr, inert);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.SetInvInertiaDiagLocal2(bodyu.ptr, inert);
 }
 
 public override void SetSleepingThresholds(BulletBody obj, float lin_threshold, float ang_threshold)
 {
-    BSAPICPP.SetSleepingThresholds2(obj.ptr, lin_threshold, ang_threshold);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.SetSleepingThresholds2(bodyu.ptr, lin_threshold, ang_threshold);
 }
 
 public override void ApplyTorque(BulletBody obj, Vector3 torque)
 {
-    BSAPICPP.ApplyTorque2(obj.ptr, torque);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.ApplyTorque2(bodyu.ptr, torque);
 }
 
 // Apply force at the given point. Will add torque to the object.
 public override void ApplyForce(BulletBody obj, Vector3 force, Vector3 pos)
 {
-    BSAPICPP.ApplyForce2(obj.ptr, force, 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.
 public override void ApplyCentralImpulse(BulletBody obj, Vector3 imp)
 {
-    BSAPICPP.ApplyCentralImpulse2(obj.ptr, 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.
 public override void ApplyTorqueImpulse(BulletBody obj, Vector3 imp)
 {
-    BSAPICPP.ApplyTorqueImpulse2(obj.ptr, 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.
 public override void ApplyImpulse(BulletBody obj, Vector3 imp, Vector3 pos)
 {
-    BSAPICPP.ApplyImpulse2(obj.ptr, imp, pos);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.ApplyImpulse2(bodyu.ptr, imp, pos);
 }
 
 public override void ClearForces(BulletBody obj)
 {
-    BSAPICPP.ClearForces2(obj.ptr);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.ClearForces2(bodyu.ptr);
 }
 
 public override void ClearAllForces(BulletBody obj)
 {
-    BSAPICPP.ClearAllForces2(obj.ptr);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.ClearAllForces2(bodyu.ptr);
 }
 
 public override void UpdateInertiaTensor(BulletBody obj)
 {
-    BSAPICPP.UpdateInertiaTensor2(obj.ptr);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.UpdateInertiaTensor2(bodyu.ptr);
 }
 
 public override Vector3 GetLinearVelocity(BulletBody obj)
 {
-    return BSAPICPP.GetLinearVelocity2(obj.ptr);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.GetLinearVelocity2(bodyu.ptr);
 }
 
 public override Vector3 GetAngularVelocity(BulletBody obj)
 {
-    return BSAPICPP.GetAngularVelocity2(obj.ptr);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.GetAngularVelocity2(bodyu.ptr);
 }
 
 public override void SetLinearVelocity(BulletBody obj, Vector3 vel)
 {
-    BSAPICPP.SetLinearVelocity2(obj.ptr, vel);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.SetLinearVelocity2(bodyu.ptr, vel);
 }
 
 public override void SetAngularVelocity(BulletBody obj, Vector3 angularVelocity)
 {
-    BSAPICPP.SetAngularVelocity2(obj.ptr, angularVelocity);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.SetAngularVelocity2(bodyu.ptr, angularVelocity);
 }
 
 public override Vector3 GetVelocityInLocalPoint(BulletBody obj, Vector3 pos)
 {
-    return BSAPICPP.GetVelocityInLocalPoint2(obj.ptr, pos);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.GetVelocityInLocalPoint2(bodyu.ptr, pos);
 }
 
 public override void Translate(BulletBody obj, Vector3 trans)
 {
-    BSAPICPP.Translate2(obj.ptr, trans);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.Translate2(bodyu.ptr, trans);
 }
 
 public override void UpdateDeactivation(BulletBody obj, float timeStep)
 {
-    BSAPICPP.UpdateDeactivation2(obj.ptr, timeStep);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.UpdateDeactivation2(bodyu.ptr, timeStep);
 }
 
 public override bool WantsSleeping(BulletBody obj)
 {
-    return BSAPICPP.WantsSleeping2(obj.ptr);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.WantsSleeping2(bodyu.ptr);
 }
 
 public override void SetAngularFactor(BulletBody obj, float factor)
 {
-    BSAPICPP.SetAngularFactor2(obj.ptr, factor);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.SetAngularFactor2(bodyu.ptr, factor);
 }
 
 public override void SetAngularFactorV(BulletBody obj, Vector3 factor)
 {
-    BSAPICPP.SetAngularFactorV2(obj.ptr, factor);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BSAPICPP.SetAngularFactorV2(bodyu.ptr, factor);
 }
 
 public override Vector3 GetAngularFactor(BulletBody obj)
 {
-    return BSAPICPP.GetAngularFactor2(obj.ptr);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.GetAngularFactor2(bodyu.ptr);
 }
 
 public override bool IsInWorld(BulletBody obj)
 {
-    return BSAPICPP.IsInWorld2(obj.ptr);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.IsInWorld2(bodyu.ptr);
 }
 
 public override void AddConstraintRef(BulletBody obj, BulletConstraint constrain)
 {
-    BSAPICPP.AddConstraintRef2(obj.ptr, constrain.ptr);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BulletConstraintUnman constrainu = constrain as BulletConstraintUnman;
+    BSAPICPP.AddConstraintRef2(bodyu.ptr, constrainu.ptr);
 }
 
 public override void RemoveConstraintRef(BulletBody obj, BulletConstraint constrain)
 {
-    BSAPICPP.RemoveConstraintRef2(obj.ptr, constrain.ptr);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    BulletConstraintUnman constrainu = constrain as BulletConstraintUnman;
+    BSAPICPP.RemoveConstraintRef2(bodyu.ptr, constrainu.ptr);
 }
 
 public override BulletConstraint GetConstraintRef(BulletBody obj, int index)
 {
-    return new BulletConstraint(BSAPICPP.GetConstraintRef2(obj.ptr, index));
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return new BulletConstraintUnman(BSAPICPP.GetConstraintRef2(bodyu.ptr, index));
 }
 
 public override int GetNumConstraintRefs(BulletBody obj)
 {
-    return BSAPICPP.GetNumConstraintRefs2(obj.ptr);
+    BulletBodyUnman bodyu = obj as BulletBodyUnman;
+    return BSAPICPP.GetNumConstraintRefs2(bodyu.ptr);
 }
 
 public override bool SetCollisionGroupMask(BulletBody body, uint filter, uint mask)
 {
-    return BSAPICPP.SetCollisionGroupMask2(body.ptr, filter, mask);
+    BulletBodyUnman bodyu = body as BulletBodyUnman;
+    return BSAPICPP.SetCollisionGroupMask2(bodyu.ptr, filter, mask);
 }
 
 // =====================================================================================
@@ -855,114 +1104,139 @@ public override bool SetCollisionGroupMask(BulletBody body, uint filter, uint ma
 
 public override float GetAngularMotionDisc(BulletShape shape)
 {
-    return BSAPICPP.GetAngularMotionDisc2(shape.ptr);
+    BulletShapeUnman shapeu = shape as BulletShapeUnman;
+    return BSAPICPP.GetAngularMotionDisc2(shapeu.ptr);
 }
 
 public override float GetContactBreakingThreshold(BulletShape shape, float defaultFactor)
 {
-    return BSAPICPP.GetContactBreakingThreshold2(shape.ptr, defaultFactor);
+    BulletShapeUnman shapeu = shape as BulletShapeUnman;
+    return BSAPICPP.GetContactBreakingThreshold2(shapeu.ptr, defaultFactor);
 }
 
 public override bool IsPolyhedral(BulletShape shape)
 {
-    return BSAPICPP.IsPolyhedral2(shape.ptr);
+    BulletShapeUnman shapeu = shape as BulletShapeUnman;
+    return BSAPICPP.IsPolyhedral2(shapeu.ptr);
 }
 
 public override bool IsConvex2d(BulletShape shape)
 {
-    return BSAPICPP.IsConvex2d2(shape.ptr);
+    BulletShapeUnman shapeu = shape as BulletShapeUnman;
+    return BSAPICPP.IsConvex2d2(shapeu.ptr);
 }
 
 public override bool IsConvex(BulletShape shape)
 {
-    return BSAPICPP.IsConvex2(shape.ptr);
+    BulletShapeUnman shapeu = shape as BulletShapeUnman;
+    return BSAPICPP.IsConvex2(shapeu.ptr);
 }
 
 public override bool IsNonMoving(BulletShape shape)
 {
-    return BSAPICPP.IsNonMoving2(shape.ptr);
+    BulletShapeUnman shapeu = shape as BulletShapeUnman;
+    return BSAPICPP.IsNonMoving2(shapeu.ptr);
 }
 
 public override bool IsConcave(BulletShape shape)
 {
-    return BSAPICPP.IsConcave2(shape.ptr);
+    BulletShapeUnman shapeu = shape as BulletShapeUnman;
+    return BSAPICPP.IsConcave2(shapeu.ptr);
 }
 
 public override bool IsCompound(BulletShape shape)
 {
-    return BSAPICPP.IsCompound2(shape.ptr);
+    BulletShapeUnman shapeu = shape as BulletShapeUnman;
+    return BSAPICPP.IsCompound2(shapeu.ptr);
 }
 
 public override bool IsSoftBody(BulletShape shape)
 {
-    return BSAPICPP.IsSoftBody2(shape.ptr);
+    BulletShapeUnman shapeu = shape as BulletShapeUnman;
+    return BSAPICPP.IsSoftBody2(shapeu.ptr);
 }
 
 public override bool IsInfinite(BulletShape shape)
 {
-    return BSAPICPP.IsInfinite2(shape.ptr);
+    BulletShapeUnman shapeu = shape as BulletShapeUnman;
+    return BSAPICPP.IsInfinite2(shapeu.ptr);
 }
 
 public override void SetLocalScaling(BulletShape shape, Vector3 scale)
 {
-    BSAPICPP.SetLocalScaling2(shape.ptr, scale);
+    BulletShapeUnman shapeu = shape as BulletShapeUnman;
+    BSAPICPP.SetLocalScaling2(shapeu.ptr, scale);
 }
 
 public override Vector3 GetLocalScaling(BulletShape shape)
 {
-    return BSAPICPP.GetLocalScaling2(shape.ptr);
+    BulletShapeUnman shapeu = shape as BulletShapeUnman;
+    return BSAPICPP.GetLocalScaling2(shapeu.ptr);
 }
 
 public override Vector3 CalculateLocalInertia(BulletShape shape, float mass)
 {
-    return BSAPICPP.CalculateLocalInertia2(shape.ptr, mass);
+    BulletShapeUnman shapeu = shape as BulletShapeUnman;
+    return BSAPICPP.CalculateLocalInertia2(shapeu.ptr, mass);
 }
 
 public override int GetShapeType(BulletShape shape)
 {
-    return BSAPICPP.GetShapeType2(shape.ptr);
+    BulletShapeUnman shapeu = shape as BulletShapeUnman;
+    return BSAPICPP.GetShapeType2(shapeu.ptr);
 }
 
 public override void SetMargin(BulletShape shape, float val)
 {
-    BSAPICPP.SetMargin2(shape.ptr, val);
+    BulletShapeUnman shapeu = shape as BulletShapeUnman;
+    BSAPICPP.SetMargin2(shapeu.ptr, val);
 }
 
 public override float GetMargin(BulletShape shape)
 {
-    return BSAPICPP.GetMargin2(shape.ptr);
+    BulletShapeUnman shapeu = shape as BulletShapeUnman;
+    return BSAPICPP.GetMargin2(shapeu.ptr);
 }
 
 // =====================================================================================
 // Debugging
-public override void DumpRigidBody(BulletWorld sim, BulletBody collisionObject)
+public override void DumpRigidBody(BulletWorld world, BulletBody collisionObject)
 {
-    BSAPICPP.DumpRigidBody2(sim.ptr, collisionObject.ptr);
+    BulletWorldUnman worldu = world as BulletWorldUnman;
+    BulletBodyUnman bodyu = collisionObject as BulletBodyUnman;
+    BSAPICPP.DumpRigidBody2(worldu.ptr, bodyu.ptr);
 }
 
-public override void DumpCollisionShape(BulletWorld sim, BulletShape collisionShape)
+public override void DumpCollisionShape(BulletWorld world, BulletShape collisionShape)
 {
-    BSAPICPP.DumpCollisionShape2(sim.ptr, collisionShape.ptr);
+    BulletWorldUnman worldu = world as BulletWorldUnman;
+    BulletShapeUnman shapeu = collisionShape as BulletShapeUnman;
+    BSAPICPP.DumpCollisionShape2(worldu.ptr, shapeu.ptr);
 }
 
-public override void DumpConstraint(BulletWorld sim, BulletConstraint constrain)
+public override void DumpConstraint(BulletWorld world, BulletConstraint constrain)
 {
-    BSAPICPP.DumpConstraint2(sim.ptr, constrain.ptr);
+    BulletWorldUnman worldu = world as BulletWorldUnman;
+    BulletConstraintUnman constrainu = constrain as BulletConstraintUnman;
+    BSAPICPP.DumpConstraint2(worldu.ptr, constrainu.ptr);
 }
 
-public override void DumpActivationInfo(BulletWorld sim)
+public override void DumpActivationInfo(BulletWorld world)
 {
-    BSAPICPP.DumpActivationInfo2(sim.ptr);
+    BulletWorldUnman worldu = world as BulletWorldUnman;
+    BSAPICPP.DumpActivationInfo2(worldu.ptr);
 }
 
-public override void DumpAllInfo(BulletWorld sim)
+public override void DumpAllInfo(BulletWorld world)
 {
-    BSAPICPP.DumpAllInfo2(sim.ptr);
+    BulletWorldUnman worldu = world as BulletWorldUnman;
+    BSAPICPP.DumpAllInfo2(worldu.ptr);
 }
 
-public override void DumpPhysicsStatistics(BulletWorld sim)
+public override void DumpPhysicsStatistics(BulletWorld world)
 {
-    BSAPICPP.DumpPhysicsStatistics2(sim.ptr);
+    BulletWorldUnman worldu = world as BulletWorldUnman;
+    BSAPICPP.DumpPhysicsStatistics2(worldu.ptr);
 }
 
 
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSAPIXNA.cs b/OpenSim/Region/Physics/BulletSPlugin/BSAPIXNA.cs
index 8ed791e..f70ad30 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSAPIXNA.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSAPIXNA.cs
@@ -29,11 +29,1287 @@ using System.Collections.Generic;
 using System.Linq;
 using System.Text;
 
+using BulletXNA;
+using BulletXNA.LinearMath;
+using BulletXNA.BulletCollision;
+using BulletXNA.BulletDynamics;
+using BulletXNA.BulletCollision.CollisionDispatch;
+
+using OpenMetaverse;
+
 namespace OpenSim.Region.Physics.BulletSPlugin
 {
     /*
 public sealed class BSAPIXNA : BSAPITemplate
 {
+    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 RemoveObjectFromWorld2(object pWorld, object pBody)
+    {
+        DiscreteDynamicsWorld world = pWorld as DiscreteDynamicsWorld;
+        RigidBody body = pBody as RigidBody;
+        world.RemoveRigidBody(body);
+        return true;
+    }
+
+    public override void SetRestitution2(object pBody, float pRestitution)
+    {
+        RigidBody body = pBody as RigidBody;
+        body.SetRestitution(pRestitution);
+    }
+
+    public override void SetMargin2(object pShape, float pMargin)
+    {
+        CollisionShape shape = pShape as CollisionShape;
+        shape.SetMargin(pMargin);
+    }
+
+    public override void SetLocalScaling2(object pShape, Vector3 pScale)
+    {
+        CollisionShape shape = pShape as CollisionShape;
+        IndexedVector3 vec = new IndexedVector3(pScale.X, pScale.Y, pScale.Z);
+        shape.SetLocalScaling(ref vec);
+
+    }
+
+    public override void SetContactProcessingThreshold2(object pBody, float contactprocessingthreshold)
+    {
+        RigidBody body = pBody as RigidBody;
+        body.SetContactProcessingThreshold(contactprocessingthreshold);
+    }
+
+    public override void SetCcdMotionThreshold2(object pBody, float pccdMotionThreashold)
+    {
+        RigidBody body = pBody as RigidBody;
+        body.SetCcdMotionThreshold(pccdMotionThreashold);
+    }
+
+    public override void SetCcdSweptSphereRadius2(object pBody, float pCcdSweptSphereRadius)
+    {
+        RigidBody body = pBody as RigidBody;
+        body.SetCcdSweptSphereRadius(pCcdSweptSphereRadius);
+    }
+
+    public override void SetAngularFactorV2(object pBody, Vector3 pAngularFactor)
+    {
+        RigidBody body = pBody as RigidBody;
+        body.SetAngularFactor(new IndexedVector3(pAngularFactor.X, pAngularFactor.Y, pAngularFactor.Z));
+    }
+
+    public override CollisionFlags AddToCollisionFlags2(object pBody, CollisionFlags pcollisionFlags)
+    {
+        CollisionObject body = pBody as CollisionObject;
+        CollisionFlags existingcollisionFlags = (CollisionFlags)(uint)body.GetCollisionFlags();
+        existingcollisionFlags |= pcollisionFlags;
+        body.SetCollisionFlags((BulletXNA.BulletCollision.CollisionFlags)(uint)existingcollisionFlags);
+        return (CollisionFlags) (uint) existingcollisionFlags;
+    }
+
+    public override void AddObjectToWorld2(object pWorld, object pBody)
+    {
+        RigidBody body = pBody as RigidBody;
+        DiscreteDynamicsWorld world = pWorld as DiscreteDynamicsWorld;
+        //if (!(body.GetCollisionShape().GetShapeType() == BroadphaseNativeTypes.STATIC_PLANE_PROXYTYPE && body.GetCollisionShape().GetShapeType() == BroadphaseNativeTypes.TERRAIN_SHAPE_PROXYTYPE))
+
+        world.AddRigidBody(body);
+
+        //if (body.GetBroadphaseHandle() != null)
+        //    world.UpdateSingleAabb(body);
+    }
+
+    public override void AddObjectToWorld2(object pWorld, object pBody, Vector3 _position, Quaternion _orientation)
+    {
+        RigidBody body = pBody as RigidBody;
+        DiscreteDynamicsWorld world = pWorld as DiscreteDynamicsWorld;
+        //if (!(body.GetCollisionShape().GetShapeType() == BroadphaseNativeTypes.STATIC_PLANE_PROXYTYPE && body.GetCollisionShape().GetShapeType() == BroadphaseNativeTypes.TERRAIN_SHAPE_PROXYTYPE))
+
+        world.AddRigidBody(body);
+        IndexedVector3 vposition = new IndexedVector3(_position.X, _position.Y, _position.Z);
+        IndexedQuaternion vquaternion = new IndexedQuaternion(_orientation.X, _orientation.Y, _orientation.Z,
+                                                              _orientation.W);
+        IndexedMatrix mat = IndexedMatrix.CreateFromQuaternion(vquaternion);
+        mat._origin = vposition;
+        body.SetWorldTransform(mat);
+        //if (body.GetBroadphaseHandle() != null)
+        //    world.UpdateSingleAabb(body);
+    }
+
+    public override void ForceActivationState2(object pBody, ActivationState pActivationState)
+    {
+        CollisionObject body = pBody as CollisionObject;
+        body.ForceActivationState((BulletXNA.BulletCollision.ActivationState)(uint)pActivationState);
+    }
+
+    public override void UpdateSingleAabb2(object pWorld, object pBody)
+    {
+        CollisionObject body = pBody as CollisionObject;
+        DiscreteDynamicsWorld world = pWorld as DiscreteDynamicsWorld;
+        world.UpdateSingleAabb(body);
+    }
+
+    public override bool SetCollisionGroupMask2(object pBody, uint pGroup, uint pMask)
+    {
+        RigidBody body = pBody as RigidBody;
+        body.GetBroadphaseHandle().m_collisionFilterGroup = (BulletXNA.BulletCollision.CollisionFilterGroups) pGroup;
+        body.GetBroadphaseHandle().m_collisionFilterGroup = (BulletXNA.BulletCollision.CollisionFilterGroups) pGroup;
+        if ((uint) body.GetBroadphaseHandle().m_collisionFilterGroup == 0)
+            return false;
+        return true;
+    }
+
+    public override void ClearAllForces2(object pBody)
+    {
+        CollisionObject body = pBody as CollisionObject;
+        IndexedVector3 zeroVector = new IndexedVector3(0, 0, 0);
+        body.SetInterpolationLinearVelocity(ref zeroVector);
+        body.SetInterpolationAngularVelocity(ref zeroVector);
+        IndexedMatrix bodytransform = body.GetWorldTransform();
+
+        body.SetInterpolationWorldTransform(ref bodytransform);
+
+        if (body is RigidBody)
+        {
+            RigidBody rigidbody = body as RigidBody;
+            rigidbody.SetLinearVelocity(zeroVector);
+            rigidbody.SetAngularVelocity(zeroVector);
+            rigidbody.ClearForces();
+        }
+    }
+
+    public override void SetInterpolationAngularVelocity2(object pBody, Vector3 pVector3)
+    {
+        RigidBody body = pBody as RigidBody;
+        IndexedVector3 vec = new IndexedVector3(pVector3.X, pVector3.Y, pVector3.Z);
+        body.SetInterpolationAngularVelocity(ref vec);
+    }
+
+    public override void SetAngularVelocity2(object pBody, Vector3 pVector3)
+    {
+        RigidBody body = pBody as RigidBody;
+        IndexedVector3 vec = new IndexedVector3(pVector3.X, pVector3.Y, pVector3.Z);
+        body.SetAngularVelocity(ref vec);
+    }
+
+    public override void ClearForces2(object pBody)
+    {
+        RigidBody body = pBody as RigidBody;
+        body.ClearForces();
+    }
+
+    public override void SetTranslation2(object pBody, Vector3 _position, Quaternion _orientation)
+    {
+        RigidBody body = pBody as RigidBody;
+        IndexedVector3 vposition = new IndexedVector3(_position.X, _position.Y, _position.Z);
+        IndexedQuaternion vquaternion = new IndexedQuaternion(_orientation.X, _orientation.Y, _orientation.Z,
+                                                              _orientation.W);
+        IndexedMatrix mat = IndexedMatrix.CreateFromQuaternion(vquaternion);
+        mat._origin = vposition;
+        body.SetWorldTransform(mat);
+        
+    }
+
+    public override Vector3 GetPosition2(object pBody)
+    {
+        RigidBody body = pBody as RigidBody;
+        IndexedVector3 pos = body.GetInterpolationWorldTransform()._origin;
+        return new Vector3(pos.X, pos.Y, pos.Z);
+    }
+
+    public override Vector3 CalculateLocalInertia2(object pShape, float pphysMass)
+    {
+        CollisionShape shape = pShape as CollisionShape;
+        IndexedVector3 inertia = IndexedVector3.Zero;
+        shape.CalculateLocalInertia(pphysMass, out inertia);
+        return new Vector3(inertia.X, inertia.Y, inertia.Z);
+    }
+
+    public override void SetMassProps2(object pBody, float pphysMass, Vector3 plocalInertia)
+    {
+        RigidBody body = pBody as RigidBody;
+        IndexedVector3 inertia = new IndexedVector3(plocalInertia.X, plocalInertia.Y, plocalInertia.Z);
+        body.SetMassProps(pphysMass, inertia);
+    }
+
+
+    public override void SetObjectForce2(object pBody, Vector3 _force)
+    {
+        RigidBody body = pBody as RigidBody;
+        IndexedVector3 force = new IndexedVector3(_force.X, _force.Y, _force.Z);
+        body.SetTotalForce(ref force);
+    }
+
+    public override void SetFriction2(object pBody, float _currentFriction)
+    {
+        RigidBody body = pBody as RigidBody;
+        body.SetFriction(_currentFriction);
+    }
+
+    public override void SetLinearVelocity2(object pBody, Vector3 _velocity)
+    {
+        RigidBody body = pBody as RigidBody;
+        IndexedVector3 velocity = new IndexedVector3(_velocity.X, _velocity.Y, _velocity.Z);
+        body.SetLinearVelocity(velocity);
+    }
+
+    public override void Activate2(object pBody, bool pforceactivation)
+    {
+        RigidBody body = pBody as RigidBody;
+        body.Activate(pforceactivation);
+        
+    }
+
+    public override Quaternion GetOrientation2(object pBody)
+    {
+        RigidBody body = pBody as RigidBody;
+        IndexedQuaternion mat = body.GetInterpolationWorldTransform().GetRotation();
+        return new Quaternion(mat.X, mat.Y, mat.Z, mat.W);
+    }
+
+    public override CollisionFlags RemoveFromCollisionFlags2(object pBody, CollisionFlags pcollisionFlags)
+    {
+        RigidBody body = pBody as RigidBody;
+        CollisionFlags existingcollisionFlags = (CollisionFlags)(uint)body.GetCollisionFlags();
+        existingcollisionFlags &= ~pcollisionFlags;
+        body.SetCollisionFlags((BulletXNA.BulletCollision.CollisionFlags)(uint)existingcollisionFlags);
+        return (CollisionFlags)(uint)existingcollisionFlags;
+    }
+
+    public override void SetGravity2(object pBody, Vector3 pGravity)
+    {
+        RigidBody body = pBody as RigidBody;
+        IndexedVector3 gravity = new IndexedVector3(pGravity.X, pGravity.Y, pGravity.Z);
+        body.SetGravity(gravity);
+    }
+
+    public override bool DestroyConstraint2(object pBody, object pConstraint)
+    {
+        RigidBody body = pBody as RigidBody;
+        TypedConstraint constraint = pConstraint as TypedConstraint;
+        body.RemoveConstraintRef(constraint);
+        return true;
+    }
+
+    public override bool SetLinearLimits2(object pConstraint, Vector3 low, Vector3 high)
+    {
+        Generic6DofConstraint constraint = pConstraint 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 SetAngularLimits2(object pConstraint, Vector3 low, Vector3 high)
+    {
+        Generic6DofConstraint constraint = pConstraint 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 SetConstraintNumSolverIterations2(object pConstraint, float cnt)
+    {
+        Generic6DofConstraint constraint = pConstraint as Generic6DofConstraint;
+        constraint.SetOverrideNumSolverIterations((int)cnt);
+    }
+
+    public override void CalculateTransforms2(object pConstraint)
+    {
+        Generic6DofConstraint constraint = pConstraint as Generic6DofConstraint;
+        constraint.CalculateTransforms();
+    }
+
+    public override void SetConstraintEnable2(object pConstraint, float p_2)
+    {
+        Generic6DofConstraint constraint = pConstraint as Generic6DofConstraint;
+        constraint.SetEnabled((p_2 == 0) ? false : true);
+    }
+
+
+    //BulletSimAPI.Create6DofConstraint2(m_world.ptr, m_body1.ptr, m_body2.ptr,frame1, frame1rot,frame2, frame2rot,useLinearReferenceFrameA, disableCollisionsBetweenLinkedBodies));
+    public override object Create6DofConstraint2(object pWorld, object pBody1, object pBody2, Vector3 pframe1, Quaternion pframe1rot, Vector3 pframe2, Quaternion pframe2rot, bool puseLinearReferenceFrameA, bool pdisableCollisionsBetweenLinkedBodies)
+
+    {
+         DiscreteDynamicsWorld world = pWorld as DiscreteDynamicsWorld;
+        RigidBody body1 = pBody1 as RigidBody;
+        RigidBody body2 = pBody2 as 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 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 object Create6DofConstraintToPoint2(object pWorld, object pBody1, object pBody2, Vector3 pjoinPoint, bool puseLinearReferenceFrameA, bool pdisableCollisionsBetweenLinkedBodies)
+    {
+        DiscreteDynamicsWorld world = pWorld as DiscreteDynamicsWorld;
+        RigidBody body1 = pBody1 as RigidBody;
+        RigidBody body2 = pBody2 as 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 consttr;
+    }
+    //SetFrames2(m_constraint.ptr, frameA, frameArot, frameB, frameBrot);
+    public override void SetFrames2(object pConstraint, Vector3 pframe1, Quaternion pframe1rot, Vector3 pframe2, Quaternion pframe2rot)
+    {
+        Generic6DofConstraint constraint = pConstraint 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);
+    }
+
+
+    
+
+    public override bool IsInWorld2(object pWorld, object pShapeObj)
+    {
+        DiscreteDynamicsWorld world = pWorld as DiscreteDynamicsWorld;
+        CollisionObject shape = pShapeObj as CollisionObject;
+        return world.IsInWorld(shape);
+    }
+
+    public override void SetInterpolationLinearVelocity2(object pBody, Vector3 VehicleVelocity)
+    {
+        RigidBody body = pBody as RigidBody;
+        IndexedVector3 velocity = new IndexedVector3(VehicleVelocity.X, VehicleVelocity.Y, VehicleVelocity.Z);
+        body.SetInterpolationLinearVelocity(ref velocity);
+    }
+
+    public override bool UseFrameOffset2(object pConstraint, float onOff)
+    {
+        Generic6DofConstraint constraint = pConstraint as Generic6DofConstraint;
+        constraint.SetUseFrameOffset((onOff == 0) ? false : true);
+        return true;
+    }
+    //SetBreakingImpulseThreshold2(m_constraint.ptr, threshold);
+    public override bool SetBreakingImpulseThreshold2(object pConstraint, float threshold)
+    {
+        Generic6DofConstraint constraint = pConstraint as Generic6DofConstraint;
+        constraint.SetBreakingImpulseThreshold(threshold);
+        return true;
+    }
+    //BulletSimAPI.SetAngularDamping2(Prim.PhysBody.ptr, angularDamping);
+    public override void SetAngularDamping2(object pBody, float angularDamping)
+    {
+        RigidBody body = pBody as RigidBody;
+        float lineardamping = body.GetLinearDamping();
+        body.SetDamping(lineardamping, angularDamping);
+
+    }
+
+    public override void UpdateInertiaTensor2(object pBody)
+    {
+        RigidBody body = pBody as RigidBody;
+        body.UpdateInertiaTensor();
+    }
+
+    public override void RecalculateCompoundShapeLocalAabb2( object pCompoundShape)
+    {
+
+        CompoundShape shape = pCompoundShape as CompoundShape;
+        shape.RecalculateLocalAabb();
+    }
+
+    //BulletSimAPI.GetCollisionFlags2(PhysBody.ptr)
+    public override CollisionFlags GetCollisionFlags2(object pBody)
+    {
+        RigidBody body = pBody as RigidBody;
+        uint flags = (uint)body.GetCollisionFlags();
+        return (CollisionFlags) flags;
+    }
+
+    public override void SetDamping2(object pBody, float pLinear, float pAngular)
+    {
+        RigidBody body = pBody as RigidBody;
+        body.SetDamping(pLinear, pAngular);
+    }
+    //PhysBody.ptr, PhysicsScene.Params.deactivationTime);
+    public override void SetDeactivationTime2(object pBody, float pDeactivationTime)
+    {
+        RigidBody body = pBody as RigidBody;
+        body.SetDeactivationTime(pDeactivationTime);
+    }
+    //SetSleepingThresholds2(PhysBody.ptr, PhysicsScene.Params.linearSleepingThreshold, PhysicsScene.Params.angularSleepingThreshold);
+    public override void SetSleepingThresholds2(object pBody, float plinearSleepingThreshold, float pangularSleepingThreshold)
+    {
+        RigidBody body = pBody as RigidBody;
+        body.SetSleepingThresholds(plinearSleepingThreshold, pangularSleepingThreshold);
+    }
+
+    public override CollisionObjectTypes GetBodyType2(object pBody)
+    {
+        RigidBody body = pBody as RigidBody;
+        return (CollisionObjectTypes)(int) body.GetInternalType();
+    }
+
+    //BulletSimAPI.ApplyCentralForce2(PhysBody.ptr, fSum);
+    public override void ApplyCentralForce2(object pBody, Vector3 pfSum)
+    {
+        RigidBody body = pBody as RigidBody;
+        IndexedVector3 fSum = new IndexedVector3(pfSum.X, pfSum.Y, pfSum.Z);
+        body.ApplyCentralForce(ref fSum);
+    }
+    public override void ApplyCentralImpulse2(object pBody, Vector3 pfSum)
+    {
+        RigidBody body = pBody as RigidBody;
+        IndexedVector3 fSum = new IndexedVector3(pfSum.X, pfSum.Y, pfSum.Z);
+        body.ApplyCentralImpulse(ref fSum);
+    }
+    public override void ApplyTorque2(object pBody, Vector3 pfSum)
+    {
+        RigidBody body = pBody as RigidBody;
+        IndexedVector3 fSum = new IndexedVector3(pfSum.X, pfSum.Y, pfSum.Z);
+        body.ApplyTorque(ref fSum);
+    }
+    public override void ApplyTorqueImpulse2(object pBody, Vector3 pfSum)
+    {
+        RigidBody body = pBody as RigidBody;
+        IndexedVector3 fSum = new IndexedVector3(pfSum.X, pfSum.Y, pfSum.Z);
+        body.ApplyTorqueImpulse(ref fSum);
+    }
+
+    public override void DumpRigidBody2(object p, object p_2)
+    {
+        //TODO:
+    }
+
+    public override void DumpCollisionShape2(object p, object p_2)
+    {
+        //TODO:
+    }
+
+    public override void DestroyObject2(object p, object p_2)
+    {
+        //TODO:
+    }
+
+    public override void Shutdown2(object pWorld)
+    {
+        DiscreteDynamicsWorld world = pWorld as DiscreteDynamicsWorld;
+        world.Cleanup();
+    }
+
+    public override void DeleteCollisionShape2(object p, object p_2)
+    {
+        //TODO:
+    }
+    //(sim.ptr, shape.ptr, prim.LocalID, prim.RawPosition, prim.RawOrientation);
+               
+    public override object CreateBodyFromShape2(object pWorld, object pShape, uint pLocalID, Vector3 pRawPosition, Quaternion pRawOrientation)
+    {
+        CollisionWorld world = pWorld as CollisionWorld;
+        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 CollisionShape;
+        //UpdateSingleAabb2(world, shape);
+        // TODO: Feed Update array into null
+        RigidBody body = new RigidBody(0,new SimMotionState(world,pLocalID,mat,null),shape,IndexedVector3.Zero);
+        
+        body.SetUserPointer(pLocalID);
+        return body;
+    }
+
+    
+    public override object CreateBodyWithDefaultMotionState2( object 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 CollisionShape;
+
+        // 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 body;
+    }
+    //(m_mapInfo.terrainBody.ptr, CollisionFlags.CF_STATIC_OBJECT);
+    public override void SetCollisionFlags2(object pBody, CollisionFlags collisionFlags)
+    {
+        RigidBody body = pBody as RigidBody;
+        body.SetCollisionFlags((BulletXNA.BulletCollision.CollisionFlags) (uint) collisionFlags);
+    }
+    //(m_mapInfo.terrainBody.ptr, PhysicsScene.Params.terrainHitFraction);
+    public override void SetHitFraction2(object pBody, float pHitFraction)
+    {
+        RigidBody body = pBody as RigidBody;
+        body.SetHitFraction(pHitFraction);
+    }
+    //BuildCapsuleShape2(physicsScene.World.ptr, 1f, 1f, prim.Scale);
+    public override object BuildCapsuleShape2(object pWorld, float pRadius, float pHeight, Vector3 pScale)
+    {
+        DiscreteDynamicsWorld world = pWorld as DiscreteDynamicsWorld;
+        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 capsuleShapeZ;
+    }
+
+    public static object Initialize2(Vector3 worldExtent, ConfigurationParameters[] o, int mMaxCollisionsPerFrame, ref List<BulletXNA.CollisionDesc> collisionArray, int mMaxUpdatesPerFrame, ref List<BulletXNA.EntityProperties> updateArray, object mDebugLogCallbackHandle)
+    {
+        CollisionWorld.WorldData.ParamData p = new CollisionWorld.WorldData.ParamData();
+
+        p.angularDamping = o[0].XangularDamping;
+        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 = o[0].XlinearDamping;
+        p.angularDamping = o[0].XangularDamping;
+        p.deactivationTime = o[0].XdeactivationTime;
+        p.linearSleepingThreshold = o[0].XlinearSleepingThreshold;
+        p.angularSleepingThreshold = o[0].XangularSleepingThreshold;
+        p.ccdMotionThreshold = o[0].XccdMotionThreshold;
+        p.ccdSweptSphereRadius = o[0].XccdSweptSphereRadius;
+        p.contactProcessingThreshold = o[0].XcontactProcessingThreshold;
+
+        p.terrainImplementation = o[0].XterrainImplementation;
+        p.terrainFriction = o[0].XterrainFriction;
+
+        p.terrainHitFraction = o[0].XterrainHitFraction;
+        p.terrainRestitution = o[0].XterrainRestitution;
+        p.terrainCollisionMargin = o[0].XterrainCollisionMargin;
+
+        p.avatarFriction = o[0].XavatarFriction;
+        p.avatarStandingFriction = o[0].XavatarStandingFriction;
+        p.avatarDensity = o[0].XavatarDensity;
+        p.avatarRestitution = o[0].XavatarRestitution;
+        p.avatarCapsuleWidth = o[0].XavatarCapsuleWidth;
+        p.avatarCapsuleDepth = o[0].XavatarCapsuleDepth;
+        p.avatarCapsuleHeight = o[0].XavatarCapsuleHeight;
+        p.avatarContactProcessingThreshold = o[0].XavatarContactProcessingThreshold;
+       
+        p.vehicleAngularDamping = o[0].XvehicleAngularDamping;
+        
+        p.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 = o[0].XlinksetImplementation;
+        p.linkConstraintUseFrameOffset = o[0].XlinkConstraintUseFrameOffset;
+        p.linkConstraintEnableTransMotor = o[0].XlinkConstraintEnableTransMotor;
+        p.linkConstraintTransMotorMaxVel = o[0].XlinkConstraintTransMotorMaxVel;
+        p.linkConstraintTransMotorMaxForce = o[0].XlinkConstraintTransMotorMaxForce;
+        p.linkConstraintERP = o[0].XlinkConstraintERP;
+        p.linkConstraintCFM = o[0].XlinkConstraintCFM;
+        p.linkConstraintSolverIterations = o[0].XlinkConstraintSolverIterations;
+        p.physicsLoggingFrames = o[0].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.UpdatedObjects = updateArray;
+        world.UpdatedCollisions = collisionArray;
+        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);
+
+
+        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 SetConstraintParam2(object pConstraint, ConstraintParams paramIndex, float paramvalue, ConstraintParamAxis axis)
+    {
+        Generic6DofConstraint constrain = pConstraint 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 PushUpdate2(object pCollisionObject)
+    {
+        bool ret = false;
+        RigidBody rb = pCollisionObject 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 bool IsCompound2(object pShape)
+    {
+        CollisionShape shape = pShape as CollisionShape;
+        return shape.IsCompound();
+    }
+    public override bool IsPloyhedral2(object pShape)
+    {
+        CollisionShape shape = pShape as CollisionShape;
+        return shape.IsPolyhedral();
+    }
+    public override bool IsConvex2d2(object pShape)
+    {
+        CollisionShape shape = pShape as CollisionShape;
+        return shape.IsConvex2d();
+    }
+    public override bool IsConvex2(object pShape)
+    {
+        CollisionShape shape = pShape as CollisionShape;
+        return shape.IsConvex();
+    }
+    public override bool IsNonMoving2(object pShape)
+    {
+        CollisionShape shape = pShape as CollisionShape;
+        return shape.IsNonMoving();
+    }
+    public override bool IsConcave2(object pShape)
+    {
+        CollisionShape shape = pShape as CollisionShape;
+        return shape.IsConcave();
+    }
+    public override bool IsInfinite2(object pShape)
+    {
+        CollisionShape shape = pShape as CollisionShape;
+        return shape.IsInfinite();
+    }
+    public override bool IsNativeShape2(object pShape)
+    {
+        CollisionShape shape = pShape as CollisionShape;
+        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;
+    }
+    //sim.ptr, shape.ptr,prim.LocalID, prim.RawPosition, prim.RawOrientation
+    public override object CreateGhostFromShape2(object pWorld, object pShape, uint pLocalID, Vector3 pRawPosition, Quaternion pRawOrientation)
+    {
+        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 CollisionShape;
+        gObj.SetCollisionShape(shape);
+        gObj.SetUserPointer(pLocalID);
+        // TODO: Add to Special CollisionObjects!
+        return gObj;
+    }
+
+    public static void SetCollisionShape2(object pWorld, object pObj, object pShape)
+    {
+        var world = pWorld as DiscreteDynamicsWorld;
+        var obj = pObj as CollisionObject;
+        var shape = pShape as CollisionShape;
+        obj.SetCollisionShape(shape);
+
+    }
+    //(PhysicsScene.World.ptr, nativeShapeData)
+    public override object BuildNativeShape2(object pWorld, ShapeData pShapeData)
+    {
+        var world = pWorld as DiscreteDynamicsWorld;
+        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 shape;
+    }
+    //PhysicsScene.World.ptr, false
+    public override object CreateCompoundShape2(object pWorld, bool enableDynamicAabbTree)
+    {
+        return new CompoundShape(enableDynamicAabbTree);
+    }
+
+    public override int GetNumberOfCompoundChildren2(object pCompoundShape)
+    {
+        var compoundshape = pCompoundShape as CompoundShape;
+        return compoundshape.GetNumChildShapes();
+    }
+    //LinksetRoot.PhysShape.ptr, newShape.ptr, displacementPos, displacementRot
+    public override void AddChildShapeToCompoundShape2(object pCShape, object paddShape, Vector3 displacementPos, Quaternion displacementRot)
+    {
+        IndexedMatrix relativeTransform = new IndexedMatrix();
+        var compoundshape = pCShape as CompoundShape;
+        var addshape = paddShape as CollisionShape;
+
+        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 object RemoveChildShapeFromCompoundShapeIndex2(object pCShape, int pii)
+    {
+        var compoundshape = pCShape as CompoundShape;
+        CollisionShape ret = null;
+        ret = compoundshape.GetChildShape(pii);
+        compoundshape.RemoveChildShapeByIndex(pii);
+        return ret;
+    }
+
+    public override object CreateGroundPlaneShape2(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 m_planeshape;
+    }
+
+    public override object CreateHingeConstraint2(object pWorld, object pBody1, object ppBody2, Vector3 ppivotInA, Vector3 ppivotInB, Vector3 paxisInA, Vector3 paxisInB, bool puseLinearReferenceFrameA, bool pdisableCollisionsBetweenLinkedBodies)
+    {
+        HingeConstraint constrain = null;
+        var rb1 = pBody1 as RigidBody;
+        var rb2 = ppBody2 as 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);
+            var world = pWorld as DiscreteDynamicsWorld;
+            world.AddConstraint(constrain, pdisableCollisionsBetweenLinkedBodies);
+        }
+        return constrain;
+    }
+
+    public override bool ReleaseHeightMapInfo2(object pMapInfo)
+    {
+        if (pMapInfo != null)
+        {
+            BulletHeightMapInfo mapinfo = pMapInfo as BulletHeightMapInfo;
+            if (mapinfo.heightMap != null)
+                mapinfo.heightMap = null;
+
+
+        }
+        return true;
+    }
+
+    public override object CreateHullShape2(object pWorld, int pHullCount, float[] pConvHulls)
+    {
+        CompoundShape compoundshape = new CompoundShape(false);
+        var world = pWorld as DiscreteDynamicsWorld;
+        
+        
+        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 compoundshape;
+    }
+
+    public override object CreateMeshShape2(object 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);
+        var world = pWorld as DiscreteDynamicsWorld;
+        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 meshShape;
+
+    }
+    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();
+    }
+    //PhysicsScene.World.ptr, m_mapInfo.ID, m_mapInfo.minCoords, m_mapInfo.maxCoords,  m_mapInfo.heightMap, PhysicsScene.Params.terrainCollisionMargin
+    public override object CreateHeightMapInfo2(object pWorld, uint pId, Vector3 pminCoords, Vector3 pmaxCoords, float[] pheightMap, float pCollisionMargin)
+    {
+        BulletHeightMapInfo mapInfo = new BulletHeightMapInfo(pId, pheightMap, null);
+        mapInfo.heightMap = null;
+        mapInfo.minCoords = pminCoords;
+        mapInfo.maxCoords = pmaxCoords;
+        mapInfo.sizeX = (int) (pmaxCoords.X - pminCoords.X);
+        mapInfo.sizeY = (int) (pmaxCoords.Y - pminCoords.Y);
+        mapInfo.ID = pId;
+        mapInfo.minZ = pminCoords.Z;
+        mapInfo.maxZ = pmaxCoords.Z;
+        mapInfo.collisionMargin = pCollisionMargin;
+        if (mapInfo.minZ == mapInfo.maxZ)
+            mapInfo.minZ -= 0.2f;
+        mapInfo.heightMap = pheightMap;
+
+        return mapInfo;
+
+    }
+
+    public override object CreateTerrainShape2(object pMapInfo)
+    {
+        BulletHeightMapInfo mapinfo = pMapInfo as BulletHeightMapInfo;
+        const int upAxis = 2;
+        const float scaleFactor = 1.0f;
+        HeightfieldTerrainShape terrainShape = new HeightfieldTerrainShape((int)mapinfo.sizeX, (int)mapinfo.sizeY,
+                                                                           mapinfo.heightMap,  scaleFactor,
+                                                                           mapinfo.minZ, mapinfo.maxZ, upAxis,
+                                                                            false);
+        terrainShape.SetMargin(mapinfo.collisionMargin + 0.5f);
+        terrainShape.SetUseDiamondSubdivision(true);
+        terrainShape.SetUserPointer(mapinfo.ID);
+        return terrainShape;
+    }
+
+    public override bool TranslationalLimitMotor2(object pConstraint, float ponOff, float targetVelocity, float maxMotorForce)
+    {
+        TypedConstraint tconstrain = pConstraint as TypedConstraint;
+        bool onOff = ponOff != 0;
+        bool ret = false;
+
+        switch (tconstrain.GetConstraintType())
+        {
+            case TypedConstraintType.D6_CONSTRAINT_TYPE:
+                Generic6DofConstraint constrain = pConstraint 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 PhysicsStep2(object pWorld, float timeStep, int m_maxSubSteps, float m_fixedTimeStep, out int updatedEntityCount, out List<BulletXNA.EntityProperties> updatedEntities, out int collidersCount, out List<BulletXNA.CollisionDesc>colliders)
+    {
+        int epic = PhysicsStepint2(pWorld, timeStep, m_maxSubSteps, m_fixedTimeStep, out updatedEntityCount, out updatedEntities,
+                                out collidersCount, out colliders);
+        return epic;
+    }
+
+    private static int PhysicsStepint2(object pWorld,float timeStep, int m_maxSubSteps, float m_fixedTimeStep, out int updatedEntityCount, out List<BulletXNA.EntityProperties> updatedEntities, out int collidersCount, out List<BulletXNA.CollisionDesc> colliders)
+    {
+        int numSimSteps = 0;
+      
+
+        //if (updatedEntities is null)
+        //    updatedEntities = new List<BulletXNA.EntityProperties>();
+
+        //if (colliders is null)
+        //    colliders = new List<BulletXNA.CollisionDesc>();
+        
+
+        if (pWorld is DiscreteDynamicsWorld)
+        {
+            DiscreteDynamicsWorld world = pWorld as DiscreteDynamicsWorld;
+
+            numSimSteps = world.StepSimulation(timeStep, m_maxSubSteps, m_fixedTimeStep);
+            int updates = 0;
+
+            updatedEntityCount = world.UpdatedObjects.Count;
+            updatedEntities = new List<BulletXNA.EntityProperties>(world.UpdatedObjects);
+            updatedEntityCount = updatedEntities.Count;
+            world.UpdatedObjects.Clear();
+            
+
+            collidersCount = world.UpdatedCollisions.Count;
+            colliders = new List<BulletXNA.CollisionDesc>(world.UpdatedCollisions);
+
+            world.UpdatedCollisions.Clear();
+            m_collisionsThisFrame = 0;
+            int numManifolds = world.GetDispatcher().GetNumManifolds();
+            for (int j = 0; j < numManifolds; j++)
+            {
+                PersistentManifold contactManifold = world.GetDispatcher().GetManifoldByIndexInternal(j);
+                int numContacts = contactManifold.GetNumContacts();
+                if (numContacts == 0)
+                    continue;
+
+                CollisionObject objA = contactManifold.GetBody0() as CollisionObject;
+                CollisionObject objB = contactManifold.GetBody1() as CollisionObject;
+
+                ManifoldPoint manifoldPoint = contactManifold.GetContactPoint(0);
+                IndexedVector3 contactPoint = manifoldPoint.GetPositionWorldOnB();
+                IndexedVector3 contactNormal = -manifoldPoint.m_normalWorldOnB; // make relative to A
+
+                RecordCollision(world, objA, objB, contactPoint, contactNormal);
+                m_collisionsThisFrame ++;
+                if (m_collisionsThisFrame >= 9999999)
+                    break;
+
+
+            }
+
+
+        }
+        else
+        {
+            //if (updatedEntities is null)
+            updatedEntities = new List<BulletXNA.EntityProperties>();
+            updatedEntityCount = 0;
+            //if (colliders is null)
+            colliders = new List<BulletXNA.CollisionDesc>();
+            collidersCount = 0;
+        }
+        return numSimSteps;
+    }
+
+    private static void RecordCollision(CollisionWorld world,CollisionObject objA, CollisionObject objB, IndexedVector3 contact, IndexedVector3 norm)
+    {
+       
+        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;
+
+        BulletXNA.CollisionDesc cDesc = new BulletXNA.CollisionDesc()
+                                            {
+                                                aID = idA,
+                                                bID = idB,
+                                                point = contact,
+                                                normal = contactNormal
+                                            };
+        world.UpdatedCollisions.Add(cDesc);
+        m_collisionsThisFrame++;
+
+
+    }
+    private static EntityProperties GetDebugProperties(object pWorld, object pBody)
+    {
+        EntityProperties ent = new EntityProperties();
+        DiscreteDynamicsWorld world = pWorld as DiscreteDynamicsWorld;
+        RigidBody body = pBody as RigidBody;
+        IndexedMatrix transform = body.GetWorldTransform();
+        IndexedVector3 LinearVelocity = body.GetInterpolationLinearVelocity();
+        IndexedVector3 AngularVelocity = body.GetInterpolationAngularVelocity();
+        IndexedQuaternion rotation = transform.GetRotation();
+        ent.Acceleration = Vector3.Zero;
+        ent.ID = (uint)body.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 Vector3 GetLocalScaling2(object pBody)
+    {
+        CollisionShape shape = pBody as CollisionShape;
+        IndexedVector3 scale = shape.GetLocalScaling();
+        return new Vector3(scale.X,scale.Y,scale.Z);
+    }
+
+    public override bool RayCastGround(object pWorld, Vector3 _RayOrigin, float pRayHeight, object NotMe)
+    {
+        DynamicsWorld world = pWorld as DynamicsWorld;
+        if (world != null)
+        {
+            if (NotMe is CollisionObject || NotMe is RigidBody)
+            {
+                CollisionObject AvoidBody = NotMe as CollisionObject;
+                
+                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;
+    }
 }
-     */
+*/
 }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSApiTemplate.cs b/OpenSim/Region/Physics/BulletSPlugin/BSApiTemplate.cs
index 699f055..2350f59 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSApiTemplate.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSApiTemplate.cs
@@ -362,7 +362,7 @@ public abstract void DestroyObject(BulletWorld sim, BulletBody obj);
 // =====================================================================================
 public abstract BulletShape CreateGroundPlaneShape(uint id, float height, float collisionMargin);
 
-public abstract BulletShape CreateTerrainShape(uint id, Vector3 size, float minHeight, float maxHeight, float[] heightMap, 

+public abstract BulletShape CreateTerrainShape(uint id, Vector3 size, float minHeight, float maxHeight, float[] heightMap, 
                                                                 float scaleFactor, float collisionMargin);
 
 // =====================================================================================
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
index 064ce3c..cb8108d 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
@@ -300,7 +300,7 @@ public sealed class BSPrim : BSPhysObject
             // All positions are given in world positions.
             if (_position == value)
             {
-                DetailLog("{0},BSPrim.setPosition,taint,positionNotChanging,pos={1},orient={2}", LocalID, _position, _orientation);
+                DetailLog("{0},BSPrim.setPosition,call,positionNotChanging,pos={1},orient={2}", LocalID, _position, _orientation);
                 return;
             }
             _position = value;
@@ -894,21 +894,26 @@ public sealed class BSPrim : BSPhysObject
     // 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);
-
-            // TODO: Fix this. Total kludge because adding object to world resets its gravity to default.
-            // Replace this when the new AddObjectToWorld function is complete.
-            PhysicsScene.PE.SetGravity(PhysBody, ComputeGravity());
-
-            // Collision filter can be set only when the object is in the world
-            if (!PhysBody.ApplyCollisionMask(PhysicsScene))
-            {
-                m_log.ErrorFormat("{0} Failed setting object collision mask: id={1}", LogHeader, LocalID);
-                DetailLog("{0},BSPrim.UpdatePhysicalParameters,failedSetMaskGroup,cType={1}", LocalID, PhysBody.collisionType);
-            }
+    {

+        if (PhysBody.HasPhysicalBody)

+        {

+            PhysicsScene.PE.AddObjectToWorld(PhysicsScene.World, PhysBody);

+

+            // TODO: Fix this. Total kludge because adding object to world resets its gravity to default.

+            // Replace this when the new AddObjectToWorld function is complete.

+            PhysicsScene.PE.SetGravity(PhysBody, ComputeGravity());

+

+            // Collision filter can be set only when the object is in the world

+            if (!PhysBody.ApplyCollisionMask(PhysicsScene))

+            {

+                m_log.ErrorFormat("{0} Failed setting object collision mask: id={1}", LogHeader, LocalID);

+                DetailLog("{0},BSPrim.UpdatePhysicalParameters,failedSetMaskGroup,cType={1}", LocalID, PhysBody.collisionType);

+            }

+        }

+        else

+        {

+            m_log.ErrorFormat("{0} Attempt to add physical object without body. id={1}", LogHeader, LocalID);

+            DetailLog("{0},BSPrim.UpdatePhysicalParameters,addObjectWithoutBody,cType={1}", LocalID, PhysBody.collisionType);

         }
     }
 
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs b/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs
index cd77581..2b652f5 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;
@@ -173,7 +173,7 @@ public sealed class BSShapeCollection : IDisposable
                 }
 
                 // Zero any reference to the shape so it is not freed when the body is deleted.
-                PhysicsScene.PE.SetCollisionShape(PhysicsScene.World, body, new BulletShape());
+                PhysicsScene.PE.SetCollisionShape(PhysicsScene.World, body, null);
                 PhysicsScene.PE.DestroyObject(PhysicsScene.World, body);
             });
         }
@@ -202,7 +202,7 @@ public sealed class BSShapeCollection : IDisposable
                 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;
@@ -225,7 +225,7 @@ public sealed class BSShapeCollection : IDisposable
                 else
                 {
                     // This is a new reference to a hull
-                    hullDesc.ptr = shape.ptr;
+                    hullDesc.shape = shape.Clone();
                     hullDesc.shapeKey = shape.shapeKey;
                     hullDesc.referenceCount = 1;
                     if (DDetail) DetailLog("{0},BSShapeCollection.ReferenceShape,newHull,key={1},cnt={2}",
@@ -361,15 +361,14 @@ public sealed class BSShapeCollection : IDisposable
         MeshDesc meshDesc;
         HullDesc hullDesc;
 
-        IntPtr cShape = shapeInfo.ptr;
-        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;
@@ -632,7 +631,7 @@ public sealed class BSShapeCollection : IDisposable
         if (Meshes.TryGetValue(newMeshKey, out meshDesc))
         {
             // If the mesh has already been built just use it.
-            newShape = new BulletShape(meshDesc.ptr, BSPhysicsShapeType.SHAPE_MESH);
+            newShape = meshDesc.shape.Clone();
         }
         else
         {
@@ -703,7 +702,7 @@ public sealed class BSShapeCollection : IDisposable
         if (Hulls.TryGetValue(newHullKey, out hullDesc))
         {
             // If the hull shape already is created, just use it.
-            newShape = new BulletShape(hullDesc.ptr, BSPhysicsShapeType.SHAPE_HULL);
+            newShape = hullDesc.shape.Clone();
         }
         else
         {
@@ -965,13 +964,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;
@@ -983,13 +982,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/BulletSimData.cs b/OpenSim/Region/Physics/BulletSPlugin/BulletSimData.cs
index 681d21e..662dd68 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BulletSimData.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BulletSimData.cs
@@ -33,17 +33,23 @@ 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.
 
-// The physics engine controller class created at initialization
 public class BulletWorld
 {
-    public BulletWorld(uint worldId, BSScene bss, IntPtr xx)
+    public BulletWorld(uint worldId, BSScene bss)
     {
-        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;
@@ -52,27 +58,19 @@ public class BulletWorld
 // An allocated Bullet btRigidBody
 public class BulletBody
 {
-    public BulletBody(uint id) : this(id, IntPtr.Zero)
-    {
-    }
-    public BulletBody(uint id, IntPtr xx)
+    public BulletBody(uint id)
     {
         ID = id;
-        ptr = xx;
         collisionType = CollisionType.Static;
     }
-    public IntPtr ptr;
     public uint ID;
     public CollisionType collisionType;
 
-    public void Clear()
-    {
-        ptr = IntPtr.Zero;
-    }
-    public bool HasPhysicalBody { get { return ptr != IntPtr.Zero; } }
+    public virtual void Clear() { }
+    public virtual bool HasPhysicalBody { get { return false; } }
 
     // Apply the specificed collision mask into the physical world
-    public bool ApplyCollisionMask(BSScene physicsScene)
+    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
@@ -83,12 +81,9 @@ public class BulletBody
     }
 
     // Used for log messages for a unique display of the memory/object allocated to this instance
-    public string AddrString
+    public virtual string AddrString
     {
-        get
-        {
-            return ptr.ToString("X");
-        }
+        get { return "unknown"; }
     }
 
     public override string ToString()
@@ -108,38 +103,26 @@ public class BulletBody
 public class BulletShape
 {
     public BulletShape()
-        : this(IntPtr.Zero, BSPhysicsShapeType.SHAPE_UNKNOWN)
-    {
-    }
-    public BulletShape(IntPtr xx)
-        : this(xx, BSPhysicsShapeType.SHAPE_UNKNOWN)
     {
-    }
-    public BulletShape(IntPtr xx, BSPhysicsShapeType typ)
-    {
-        ptr = xx;
-        type = typ;
+        type = BSPhysicsShapeType.SHAPE_UNKNOWN;
         shapeKey = (System.UInt64)FixedShapeKey.KEY_NONE;
         isNativeShape = false;
     }
-    public IntPtr ptr;
     public BSPhysicsShapeType type;
     public System.UInt64 shapeKey;
     public bool isNativeShape;
 
-    public void Clear()
-    {
-        ptr = IntPtr.Zero;
-    }
-    public bool HasPhysicalShape { get { return ptr != IntPtr.Zero; } }
+    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 string AddrString
+    public virtual string AddrString
     {
-        get
-        {
-            return ptr.ToString("X");
-        }
+        get { return "unknown"; }
     }
 
     public override string ToString()
@@ -161,25 +144,16 @@ public class BulletShape
 // An allocated Bullet btConstraint
 public class BulletConstraint
 {
-    public BulletConstraint(IntPtr xx)
-    {
-        ptr = xx;
-    }
-    public IntPtr ptr;
-
-    public void Clear()
+    public BulletConstraint()
     {
-        ptr = IntPtr.Zero;
     }
-    public bool HasPhysicalConstraint { get { return ptr != IntPtr.Zero; } }
+    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 string AddrString
+    public virtual string AddrString
     {
-        get
-        {
-            return ptr.ToString("X");
-        }
+        get { return "unknown"; }
     }
 }