116 files changed, 24079 insertions, 2664 deletions

diff --git a/OpenSim/Region/PhysicsModules/BasicPhysics/AssemblyInfo.cs b/OpenSim/Region/PhysicsModules/BasicPhysics/AssemblyInfo.cs
index 1765ae0..68d6eb9 100644
--- a/OpenSim/Region/PhysicsModules/BasicPhysics/AssemblyInfo.cs
+++ b/OpenSim/Region/PhysicsModules/BasicPhysics/AssemblyInfo.cs
@@ -56,7 +56,7 @@ using Mono.Addins;
 // You can specify all values by your own or you can build default build and revision
 // numbers with the '*' character (the default):
 
-[assembly : AssemblyVersion("0.8.2.*")]
+[assembly : AssemblyVersion(OpenSim.VersionInfo.AssemblyVersionNumber)]
 
 [assembly: Addin("OpenSim.Region.PhysicsModule.BasicPhysics", OpenSim.VersionInfo.VersionNumber)]
 [assembly: AddinDependency("OpenSim.Region.Framework", OpenSim.VersionInfo.VersionNumber)]
diff --git a/OpenSim/Region/PhysicsModules/BasicPhysics/BasicPhysicsActor.cs b/OpenSim/Region/PhysicsModules/BasicPhysics/BasicPhysicsActor.cs
index e7b30ba..67d9207 100644
--- a/OpenSim/Region/PhysicsModules/BasicPhysics/BasicPhysicsActor.cs
+++ b/OpenSim/Region/PhysicsModules/BasicPhysics/BasicPhysicsActor.cs
@@ -211,7 +211,7 @@ namespace OpenSim.Region.PhysicsModule.BasicPhysics
         {
         }
 
-        public override void LockAngularMotion(Vector3 axis)
+        public override void LockAngularMotion(byte axislocks)
         {
         }
 
@@ -254,6 +254,7 @@ namespace OpenSim.Region.PhysicsModule.BasicPhysics
 
         public override bool PIDHoverActive
         {
+            get { return false; }
             set { return; }
         }
 
@@ -267,23 +268,23 @@ namespace OpenSim.Region.PhysicsModule.BasicPhysics
             set { return; }
         }
 
-        public override Quaternion APIDTarget 
-        { 
+        public override Quaternion APIDTarget
+        {
             set { return; }
         }
-        
+
         public override bool APIDActive
-        { 
+        {
             set { return; }
         }
-        
+
         public override float APIDStrength
-        { 
+        {
             set { return; }
         }
-        
+
         public override float APIDDamping
-        { 
+        {
             set { return; }
         }
 
diff --git a/OpenSim/Region/PhysicsModules/BasicPhysics/BasicPhysicsPrim.cs b/OpenSim/Region/PhysicsModules/BasicPhysics/BasicPhysicsPrim.cs
index 5383f1b..58c2581 100644
--- a/OpenSim/Region/PhysicsModules/BasicPhysics/BasicPhysicsPrim.cs
+++ b/OpenSim/Region/PhysicsModules/BasicPhysics/BasicPhysicsPrim.cs
@@ -224,7 +224,7 @@ namespace OpenSim.Region.PhysicsModule.BasicPhysics
         {
         }
 
-        public override void LockAngularMotion(Vector3 axis)
+        public override void LockAngularMotion(byte axislocks)
         {
         }
 
@@ -267,6 +267,7 @@ namespace OpenSim.Region.PhysicsModule.BasicPhysics
 
         public override bool PIDHoverActive
         {
+            get { return false; }
             set { return; }
         }
 
@@ -280,23 +281,23 @@ namespace OpenSim.Region.PhysicsModule.BasicPhysics
             set { return; }
         }
 
-        public override Quaternion APIDTarget 
-        { 
+        public override Quaternion APIDTarget
+        {
             set { return; }
         }
-        
+
         public override bool APIDActive
-        { 
+        {
             set { return; }
         }
-        
+
         public override float APIDStrength
-        { 
+        {
             set { return; }
         }
-        
+
         public override float APIDDamping
-        { 
+        {
             set { return; }
         }
 
diff --git a/OpenSim/Region/PhysicsModules/BasicPhysics/BasicPhysicsScene.cs b/OpenSim/Region/PhysicsModules/BasicPhysics/BasicPhysicsScene.cs
index 20b337a..2a1ae45 100644
--- a/OpenSim/Region/PhysicsModules/BasicPhysics/BasicPhysicsScene.cs
+++ b/OpenSim/Region/PhysicsModules/BasicPhysics/BasicPhysicsScene.cs
@@ -61,6 +61,11 @@ namespace OpenSim.Region.PhysicsModule.BasicPhysics
             get { return "basicphysics"; }
         }
 
+        public string Version
+        {
+            get { return "1.0"; }
+        }
+
         public Type ReplaceableInterface
         {
             get { return null; }
@@ -90,6 +95,7 @@ namespace OpenSim.Region.PhysicsModule.BasicPhysics
 
             EngineType = Name;
             PhysicsSceneName = EngineType + "/" + scene.RegionInfo.RegionName;
+            EngineName = Name + " " + Version;
 
             scene.RegisterModuleInterface<PhysicsScene>(this);
             m_regionExtent = new Vector3(scene.RegionInfo.RegionSizeX, scene.RegionInfo.RegionSizeY, scene.RegionInfo.RegionSizeZ);
@@ -220,7 +226,7 @@ namespace OpenSim.Region.PhysicsModule.BasicPhysics
                 actor.Velocity = actorVelocity;
             }
 
-            return fps;
+            return 1.0f;
         }
 
         public override void GetResults()
diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSAPIUnman.cs b/OpenSim/Region/PhysicsModules/BulletS/BSAPIUnman.cs
index c4a923c..840e453 100644..100755
--- a/OpenSim/Region/PhysicsModules/BulletS/BSAPIUnman.cs
+++ b/OpenSim/Region/PhysicsModules/BulletS/BSAPIUnman.cs
@@ -155,8 +155,8 @@ public BSAPIUnman(string paramName, BSScene physScene)
 
 // Initialization and simulation
 public override BulletWorld Initialize(Vector3 maxPosition, ConfigurationParameters parms,
-                                                                                        int maxCollisions,  ref CollisionDesc[] collisionArray,
-                                                                                        int maxUpdates, ref EntityProperties[] updateArray
+                                            int maxCollisions,  ref CollisionDesc[] collisionArray,
+                                            int maxUpdates, ref EntityProperties[] updateArray
                                             )
 {
     // Pin down the memory that will be used to pass object collisions and updates back from unmanaged code
@@ -1405,6 +1405,19 @@ public override float GetMargin(BulletShape shape)
 }
 
 // =====================================================================================
+// Raycast
+public override SweepHit ConvexSweepTest2(BulletWorld world, BulletBody sweepObject, Vector3 from, Vector3 to, float margin) {
+    BulletWorldUnman worldu = world as BulletWorldUnman;
+    BulletBodyUnman bodyu = sweepObject as BulletBodyUnman;
+    return BSAPICPP.ConvexSweepTest2(worldu.ptr, bodyu.ptr, from, to, margin);
+}
+
+public override RaycastHit RayTest2(BulletWorld world, Vector3 from, Vector3 to, uint filterGroup, uint filterMask) {
+    BulletWorldUnman worldu = world as BulletWorldUnman;
+    return BSAPICPP.RayTest2(worldu.ptr, from, to, filterGroup, filterMask);
+}
+
+// =====================================================================================
 // Debugging
 public override void DumpRigidBody(BulletWorld world, BulletBody collisionObject)
 {
@@ -1472,8 +1485,8 @@ public delegate void DebugLogCallback([MarshalAs(UnmanagedType.LPStr)]string msg
 // Initialization and simulation
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern IntPtr Initialize2(Vector3 maxPosition, IntPtr parms,
-                                                                                        int maxCollisions,  IntPtr collisionArray,
-                                                                                        int maxUpdates, IntPtr updateArray,
+                                            int maxCollisions,  IntPtr collisionArray,
+                                            int maxUpdates, IntPtr updateArray,
                                             DebugLogCallback logRoutine);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
@@ -2084,6 +2097,15 @@ public static extern void SetMargin2(IntPtr shape, float val);
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern float GetMargin2(IntPtr shape);
 
+
+// =====================================================================================
+// Raycast
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern SweepHit ConvexSweepTest2(IntPtr sim, IntPtr obj, Vector3 from, Vector3 to, float margin);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern RaycastHit RayTest2(IntPtr sim, Vector3 from, Vector3 to, uint filterGroup, uint filterMask);
+
 // =====================================================================================
 // Debugging
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSAPIXNA.cs b/OpenSim/Region/PhysicsModules/BulletS/BSAPIXNA.cs
index 887311d..7d58728 100644..100755
--- a/OpenSim/Region/PhysicsModules/BulletS/BSAPIXNA.cs
+++ b/OpenSim/Region/PhysicsModules/BulletS/BSAPIXNA.cs
@@ -815,161 +815,161 @@ private sealed class BulletConstraintXNA : BulletConstraint
     public override bool SliderSetLimits(BulletConstraint pConstraint, int lowerUpper, int linAng, float val)
     {
         SliderConstraint constraint = (pConstraint as BulletConstraintXNA).constrain as SliderConstraint;
-                switch (lowerUpper)
-                {
-                        case SLIDER_LOWER_LIMIT:
-                                switch (linAng)
-                                {
-                                        case SLIDER_LINEAR:
-                                                constraint.SetLowerLinLimit(val);
-                                                break;
-                                        case SLIDER_ANGULAR:
-                                                constraint.SetLowerAngLimit(val);
-                                                break;
-                                }
-                                break;
-                        case SLIDER_UPPER_LIMIT:
-                                switch (linAng)
-                                {
-                                        case SLIDER_LINEAR:
-                                                constraint.SetUpperLinLimit(val);
-                                                break;
-                                        case SLIDER_ANGULAR:
-                                                constraint.SetUpperAngLimit(val);
-                                                break;
-                                }
-                                break;
-                }
+        switch (lowerUpper)
+        {
+            case SLIDER_LOWER_LIMIT:
+                switch (linAng)
+                {
+                    case SLIDER_LINEAR:
+                        constraint.SetLowerLinLimit(val);
+                        break;
+                    case SLIDER_ANGULAR:
+                        constraint.SetLowerAngLimit(val);
+                        break;
+                }
+                break;
+            case SLIDER_UPPER_LIMIT:
+                switch (linAng)
+                {
+                    case SLIDER_LINEAR:
+                        constraint.SetUpperLinLimit(val);
+                        break;
+                    case SLIDER_ANGULAR:
+                        constraint.SetUpperAngLimit(val);
+                        break;
+                }
+                break;
+        }
         return true;
     }
     public override bool SliderSet(BulletConstraint pConstraint, int softRestDamp, int dirLimOrtho, int linAng, float val)
     {
         SliderConstraint constraint = (pConstraint as BulletConstraintXNA).constrain as SliderConstraint;
-                switch (softRestDamp)
-                {
-                        case SLIDER_SET_SOFTNESS:
-                                switch (dirLimOrtho)
-                                {
-                                        case SLIDER_SET_DIRECTION:
-                                                switch (linAng)
-                                                {
-                                                        case SLIDER_LINEAR: constraint.SetSoftnessDirLin(val); break;
-                                                        case SLIDER_ANGULAR: constraint.SetSoftnessDirAng(val); break;
-                                                }
-                                                break;
-                                        case SLIDER_SET_LIMIT:
-                                                switch (linAng)
-                                                {
-                                                        case SLIDER_LINEAR: constraint.SetSoftnessLimLin(val); break;
-                                                        case SLIDER_ANGULAR: constraint.SetSoftnessLimAng(val); break;
-                                                }
-                                                break;
-                                        case SLIDER_SET_ORTHO:
-                                                switch (linAng)
-                                                {
-                                                        case SLIDER_LINEAR: constraint.SetSoftnessOrthoLin(val); break;
-                                                        case SLIDER_ANGULAR: constraint.SetSoftnessOrthoAng(val); break;
-                                                }
-                                                break;
-                                }
-                                break;
-                        case SLIDER_SET_RESTITUTION:
-                                switch (dirLimOrtho)
-                                {
-                                        case SLIDER_SET_DIRECTION:
-                                                switch (linAng)
-                                                {
-                                                        case SLIDER_LINEAR: constraint.SetRestitutionDirLin(val); break;
-                                                        case SLIDER_ANGULAR: constraint.SetRestitutionDirAng(val); break;
-                                                }
-                                                break;
-                                        case SLIDER_SET_LIMIT:
-                                                switch (linAng)
-                                                {
-                                                        case SLIDER_LINEAR: constraint.SetRestitutionLimLin(val); break;
-                                                        case SLIDER_ANGULAR: constraint.SetRestitutionLimAng(val); break;
-                                                }
-                                                break;
-                                        case SLIDER_SET_ORTHO:
-                                                switch (linAng)
-                                                {
-                                                        case SLIDER_LINEAR: constraint.SetRestitutionOrthoLin(val); break;
-                                                        case SLIDER_ANGULAR: constraint.SetRestitutionOrthoAng(val); break;
-                                                }
-                                                break;
-                                }
-                                break;
-                        case SLIDER_SET_DAMPING:
-                                switch (dirLimOrtho)
-                                {
-                                        case SLIDER_SET_DIRECTION:
-                                                switch (linAng)
-                                                {
-                                                        case SLIDER_LINEAR: constraint.SetDampingDirLin(val); break;
-                                                        case SLIDER_ANGULAR: constraint.SetDampingDirAng(val); break;
-                                                }
-                                                break;
-                                        case SLIDER_SET_LIMIT:
-                                                switch (linAng)
-                                                {
-                                                        case SLIDER_LINEAR: constraint.SetDampingLimLin(val); break;
-                                                        case SLIDER_ANGULAR: constraint.SetDampingLimAng(val); break;
-                                                }
-                                                break;
-                                        case SLIDER_SET_ORTHO:
-                                                switch (linAng)
-                                                {
-                                                        case SLIDER_LINEAR: constraint.SetDampingOrthoLin(val); break;
-                                                        case SLIDER_ANGULAR: constraint.SetDampingOrthoAng(val); break;
-                                                }
-                                                break;
-                                }
-                                break;
-                }
+        switch (softRestDamp)
+        {
+            case SLIDER_SET_SOFTNESS:
+                switch (dirLimOrtho)
+                {
+                    case SLIDER_SET_DIRECTION:
+                        switch (linAng)
+                        {
+                            case SLIDER_LINEAR: constraint.SetSoftnessDirLin(val); break;
+                            case SLIDER_ANGULAR: constraint.SetSoftnessDirAng(val); break;
+                        }
+                        break;
+                    case SLIDER_SET_LIMIT:
+                        switch (linAng)
+                        {
+                            case SLIDER_LINEAR: constraint.SetSoftnessLimLin(val); break;
+                            case SLIDER_ANGULAR: constraint.SetSoftnessLimAng(val); break;
+                        }
+                        break;
+                    case SLIDER_SET_ORTHO:
+                        switch (linAng)
+                        {
+                            case SLIDER_LINEAR: constraint.SetSoftnessOrthoLin(val); break;
+                            case SLIDER_ANGULAR: constraint.SetSoftnessOrthoAng(val); break;
+                        }
+                        break;
+                }
+                break;
+            case SLIDER_SET_RESTITUTION:
+                switch (dirLimOrtho)
+                {
+                    case SLIDER_SET_DIRECTION:
+                        switch (linAng)
+                        {
+                            case SLIDER_LINEAR: constraint.SetRestitutionDirLin(val); break;
+                            case SLIDER_ANGULAR: constraint.SetRestitutionDirAng(val); break;
+                        }
+                        break;
+                    case SLIDER_SET_LIMIT:
+                        switch (linAng)
+                        {
+                            case SLIDER_LINEAR: constraint.SetRestitutionLimLin(val); break;
+                            case SLIDER_ANGULAR: constraint.SetRestitutionLimAng(val); break;
+                        }
+                        break;
+                    case SLIDER_SET_ORTHO:
+                        switch (linAng)
+                        {
+                            case SLIDER_LINEAR: constraint.SetRestitutionOrthoLin(val); break;
+                            case SLIDER_ANGULAR: constraint.SetRestitutionOrthoAng(val); break;
+                        }
+                        break;
+                }
+                break;
+            case SLIDER_SET_DAMPING:
+                switch (dirLimOrtho)
+                {
+                    case SLIDER_SET_DIRECTION:
+                        switch (linAng)
+                        {
+                            case SLIDER_LINEAR: constraint.SetDampingDirLin(val); break;
+                            case SLIDER_ANGULAR: constraint.SetDampingDirAng(val); break;
+                        }
+                        break;
+                    case SLIDER_SET_LIMIT:
+                        switch (linAng)
+                        {
+                            case SLIDER_LINEAR: constraint.SetDampingLimLin(val); break;
+                            case SLIDER_ANGULAR: constraint.SetDampingLimAng(val); break;
+                        }
+                        break;
+                    case SLIDER_SET_ORTHO:
+                        switch (linAng)
+                        {
+                            case SLIDER_LINEAR: constraint.SetDampingOrthoLin(val); break;
+                            case SLIDER_ANGULAR: constraint.SetDampingOrthoAng(val); break;
+                        }
+                        break;
+                }
+                break;
+        }
         return true;
     }
     public override bool SliderMotorEnable(BulletConstraint pConstraint, int linAng, float numericTrueFalse)
     {
         SliderConstraint constraint = (pConstraint as BulletConstraintXNA).constrain as SliderConstraint;
-                switch (linAng)
-                {
-                        case SLIDER_LINEAR:
-                                constraint.SetPoweredLinMotor(numericTrueFalse == 0.0 ? false : true);
-                                break;
-                        case SLIDER_ANGULAR:
-                                constraint.SetPoweredAngMotor(numericTrueFalse == 0.0 ? false : true);
-                                break;
-                }
+        switch (linAng)
+        {
+            case SLIDER_LINEAR:
+                constraint.SetPoweredLinMotor(numericTrueFalse == 0.0 ? false : true);
+                break;
+            case SLIDER_ANGULAR:
+                constraint.SetPoweredAngMotor(numericTrueFalse == 0.0 ? false : true);
+                break;
+        }
         return true;
     }
     public override bool SliderMotor(BulletConstraint pConstraint, int forceVel, int linAng, float val)
     {
         SliderConstraint constraint = (pConstraint as BulletConstraintXNA).constrain as SliderConstraint;
-                switch (forceVel)
-                {
-                        case SLIDER_MOTOR_VELOCITY:
-                                switch (linAng)
-                                {
-                                        case SLIDER_LINEAR:
-                                                constraint.SetTargetLinMotorVelocity(val);
-                                                break;
-                                        case SLIDER_ANGULAR:
-                                                constraint.SetTargetAngMotorVelocity(val);
-                                                break;
-                                }
-                                break;
-                        case SLIDER_MAX_MOTOR_FORCE:
-                                switch (linAng)
-                                {
-                                        case SLIDER_LINEAR:
-                                                constraint.SetMaxLinMotorForce(val);
-                                                break;
-                                        case SLIDER_ANGULAR:
-                                                constraint.SetMaxAngMotorForce(val);
-                                                break;
-                                }
-                                break;
-                }
+        switch (forceVel)
+        {
+            case SLIDER_MOTOR_VELOCITY:
+                switch (linAng)
+                {
+                    case SLIDER_LINEAR:
+                        constraint.SetTargetLinMotorVelocity(val);
+                        break;
+                    case SLIDER_ANGULAR:
+                        constraint.SetTargetAngMotorVelocity(val);
+                        break;
+                }
+                break;
+            case SLIDER_MAX_MOTOR_FORCE:
+                switch (linAng)
+                {
+                    case SLIDER_LINEAR:
+                        constraint.SetMaxLinMotorForce(val);
+                        break;
+                    case SLIDER_ANGULAR:
+                        constraint.SetMaxAngMotorForce(val);
+                        break;
+                }
+                break;
+        }
         return true;
     }
 
@@ -1197,20 +1197,20 @@ private sealed class BulletConstraintXNA : BulletConstraint
                                                          };
         /*
             m_mass = mass;
-                        m_motionState =motionState;
-                        m_collisionShape = collisionShape;
-                        m_localInertia = localInertia;
-                        m_linearDamping = 0f;
-                        m_angularDamping = 0f;
-                        m_friction = 0.5f;
-                        m_restitution = 0f;
-                        m_linearSleepingThreshold = 0.8f;
-                        m_angularSleepingThreshold = 1f;
-                        m_additionalDamping = false;
-                        m_additionalDampingFactor = 0.005f;
-                        m_additionalLinearDampingThresholdSqr = 0.01f;
-                        m_additionalAngularDampingThresholdSqr = 0.01f;
-                        m_additionalAngularDampingFactor = 0.01f;
+            m_motionState =motionState;
+            m_collisionShape = collisionShape;
+            m_localInertia = localInertia;
+            m_linearDamping = 0f;
+            m_angularDamping = 0f;
+            m_friction = 0.5f;
+            m_restitution = 0f;
+            m_linearSleepingThreshold = 0.8f;
+            m_angularSleepingThreshold = 1f;
+            m_additionalDamping = false;
+            m_additionalDampingFactor = 0.005f;
+            m_additionalLinearDampingThresholdSqr = 0.01f;
+            m_additionalAngularDampingThresholdSqr = 0.01f;
+            m_additionalAngularDampingFactor = 0.01f;
             m_startWorldTransform = IndexedMatrix.Identity;
         */
         body.SetUserPointer(pLocalID);
@@ -2172,7 +2172,7 @@ private sealed class BulletConstraintXNA : BulletConstraint
     }
 
     public override BulletShape CreateTerrainShape(uint id, Vector3 size, float minHeight, float maxHeight, float[] heightMap,
-                                                                float scaleFactor, float collisionMargin)
+                                float scaleFactor, float collisionMargin)
     {
         const int upAxis = 2;
         HeightfieldTerrainShape terrainShape = new HeightfieldTerrainShape((int)size.X, (int)size.Y,
@@ -2459,6 +2459,14 @@ private sealed class BulletConstraintXNA : BulletConstraint
         }
         return false;
     }
+
+    public override SweepHit ConvexSweepTest2(BulletWorld world, BulletBody obj, Vector3 from, Vector3 to, float margin) {
+        return new SweepHit();
+    }
+
+    public override RaycastHit RayTest2(BulletWorld world, Vector3 from, Vector3 to, uint filterGroup, uint filterMask) {
+        return new RaycastHit();
+    }
 }
 
 
diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSActorAvatarMove.cs b/OpenSim/Region/PhysicsModules/BulletS/BSActorAvatarMove.cs
index 0191893..40c6b98 100644..100755
--- a/OpenSim/Region/PhysicsModules/BulletS/BSActorAvatarMove.cs
+++ b/OpenSim/Region/PhysicsModules/BulletS/BSActorAvatarMove.cs
@@ -47,10 +47,9 @@ public class BSActorAvatarMove : BSActor
     // The amount the step up is applying. Used to smooth stair walking.
     float m_lastStepUp;
 
-    // Jumping happens over several frames. If use applies up force while colliding, start the
-    //    jump and allow the jump to continue for this number of frames.
-    int m_jumpFrames = 0;
-    float m_jumpVelocity = 0f;
+    // There are times the velocity is set but we don't want to inforce stationary until the
+    //    real velocity drops.
+    bool m_waitingForLowVelocityForStationary = false;
 
     public BSActorAvatarMove(BSScene physicsScene, BSPhysObject pObj, string actorName)
         : base(physicsScene, pObj, actorName)
@@ -109,19 +108,23 @@ public class BSActorAvatarMove : BSActor
         {
             if (m_velocityMotor != null)
             {
-//                    if (targ == OMV.Vector3.Zero)
-//                        Util.PrintCallStack();
-//
-//                    Console.WriteLine("SetVelocityAndTarget, {0} {1}", vel, targ);
                 m_velocityMotor.Reset();
                 m_velocityMotor.SetTarget(targ);
                 m_velocityMotor.SetCurrent(vel);
                 m_velocityMotor.Enabled = true;
+                m_physicsScene.DetailLog("{0},BSCharacter.MoveMotor,SetVelocityAndTarget,vel={1}, targ={2}",
+                            m_controllingPrim.LocalID, vel, targ);
+                m_waitingForLowVelocityForStationary = false;
             }
         });
     }
 
-    // If a hover motor has not been created, create one and start the hovering.
+    public void SuppressStationayCheckUntilLowVelocity()
+    {
+        m_waitingForLowVelocityForStationary = true;
+    }
+
+    // If a movement motor has not been created, create one and start the movement
     private void ActivateAvatarMove()
     {
         if (m_velocityMotor == null)
@@ -133,13 +136,14 @@ public class BSActorAvatarMove : BSActor
                                                 1f                          // efficiency
             );
             m_velocityMotor.ErrorZeroThreshold = BSParam.AvatarStopZeroThreshold;
-            // _velocityMotor.PhysicsScene = PhysicsScene; // DEBUG DEBUG so motor will output detail log messages.
+            // m_velocityMotor.PhysicsScene = m_controllingPrim.PhysScene; // DEBUG DEBUG so motor will output detail log messages.
             SetVelocityAndTarget(m_controllingPrim.RawVelocity, m_controllingPrim.TargetVelocity, true /* inTaintTime */);
 
             m_physicsScene.BeforeStep += Mover;
             m_controllingPrim.OnPreUpdateProperty += Process_OnPreUpdateProperty;
 
             m_walkingUpStairs = 0;
+            m_waitingForLowVelocityForStationary = false;
         }
     }
 
@@ -153,7 +157,7 @@ public class BSActorAvatarMove : BSActor
         }
     }
 
-    // Called just before the simulation step. Update the vertical position for hoverness.
+    // Called just before the simulation step.
     private void Mover(float timeStep)
     {
         // Don't do movement while the object is selected.
@@ -187,12 +191,28 @@ public class BSActorAvatarMove : BSActor
 
             if (m_controllingPrim.IsColliding)
             {
-                // If we are colliding with a stationary object, presume we're standing and don't move around
+                // if colliding with something stationary and we're not doing volume detect .
                 if (!m_controllingPrim.ColliderIsMoving && !m_controllingPrim.ColliderIsVolumeDetect)
                 {
-                    m_physicsScene.DetailLog("{0},BSCharacter.MoveMotor,collidingWithStationary,zeroingMotion", m_controllingPrim.LocalID);
-                    m_controllingPrim.IsStationary = true;
-                    m_controllingPrim.ZeroMotion(true /* inTaintTime */);
+                    if (m_waitingForLowVelocityForStationary)
+                    {
+                        // if waiting for velocity to drop and it has finally dropped, we can be stationary
+                        if (m_controllingPrim.RawVelocity.LengthSquared() < BSParam.AvatarStopZeroThresholdSquared)
+                        {
+                            m_waitingForLowVelocityForStationary = false;
+                        }
+                    }
+                    if (!m_waitingForLowVelocityForStationary)
+                    {
+                        m_physicsScene.DetailLog("{0},BSCharacter.MoveMotor,collidingWithStationary,zeroingMotion", m_controllingPrim.LocalID);
+                        m_controllingPrim.IsStationary = true;
+                        m_controllingPrim.ZeroMotion(true /* inTaintTime */);
+                    }
+                    else
+                    {
+                        m_physicsScene.DetailLog("{0},BSCharacter.MoveMotor,waitingForLowVel,rawvel={1}",
+                                    m_controllingPrim.LocalID, m_controllingPrim.RawVelocity.Length());
+                    }
                 }
 
                 // Standing has more friction on the ground
@@ -222,8 +242,8 @@ public class BSActorAvatarMove : BSActor
                 }
             }
 
-            m_physicsScene.DetailLog("{0},BSCharacter.MoveMotor,taint,stopping,target={1},colliding={2}",
-                            m_controllingPrim.LocalID, m_velocityMotor.TargetValue, m_controllingPrim.IsColliding);
+            m_physicsScene.DetailLog("{0},BSCharacter.MoveMotor,taint,stopping,target={1},colliding={2},isStationary={3}",
+                            m_controllingPrim.LocalID, m_velocityMotor.TargetValue, m_controllingPrim.IsColliding,m_controllingPrim.IsStationary);
         }
         else
         {
@@ -237,50 +257,24 @@ public class BSActorAvatarMove : BSActor
                 m_physicsScene.PE.SetFriction(m_controllingPrim.PhysBody, m_controllingPrim.Friction);
             }
 
-            if (!m_controllingPrim.Flying && !m_controllingPrim.IsColliding)
-            {
-                stepVelocity.Z = m_controllingPrim.RawVelocity.Z;
-            }
+            // 'm_velocityMotor is used for walking, flying, and jumping and will thus have the correct values
+            //    for Z. But in come cases it must be over-ridden. Like when falling or jumping.
 
-            // Colliding and not flying with an upward force. The avatar must be trying to jump.
-            if (!m_controllingPrim.Flying && m_controllingPrim.IsColliding && stepVelocity.Z > 0)
-            {
-                // We allow the upward force to happen for this many frames.
-                m_jumpFrames = BSParam.AvatarJumpFrames;
-                m_jumpVelocity = stepVelocity.Z;
-            }
+            float realVelocityZ = m_controllingPrim.RawVelocity.Z;
 
-            // The case where the avatar is not colliding and is not flying is special.
-            // The avatar is either falling or jumping and the user can be applying force to the avatar
-            //     (force in some direction or force up or down).
-            // If the avatar has negative Z velocity and is not colliding, presume we're falling and keep the velocity.
-            // If the user is trying to apply upward force but we're not colliding, assume the avatar
-            //     is trying to jump and don't apply the upward force if not touching the ground any more.
-            if (!m_controllingPrim.Flying && !m_controllingPrim.IsColliding)
+            // If not flying and falling, we over-ride the stepping motor so we can fall to the ground
+            if (!m_controllingPrim.Flying && realVelocityZ < 0)
             {
-                // If upward velocity is being applied, this must be a jump and only allow that to go on so long
-                if (m_jumpFrames > 0)
+                // Can't fall faster than this
+                if (realVelocityZ < BSParam.AvatarTerminalVelocity)
                 {
-                    // Since not touching the ground, only apply upward force for so long.
-                    m_jumpFrames--;
-                    stepVelocity.Z = m_jumpVelocity;
+                    realVelocityZ = BSParam.AvatarTerminalVelocity;
                 }
-                else
-                {
-                    
-                    // Since we're not affected by anything, the avatar must be falling and we do not want that to be too fast.
-                    if (m_controllingPrim.RawVelocity.Z < BSParam.AvatarTerminalVelocity)
-                    {
-                        
-                        stepVelocity.Z = BSParam.AvatarTerminalVelocity;
-                    }
-                    else
-                    {
-                        stepVelocity.Z = m_controllingPrim.RawVelocity.Z;
-                    }
-                }
-                // DetailLog("{0},BSCharacter.MoveMotor,taint,overrideStepZWithWorldZ,stepVel={1}", LocalID, stepVelocity);
+
+                stepVelocity.Z = realVelocityZ;
             }
+            // m_physicsScene.DetailLog("{0},BSCharacter.MoveMotor,DEBUG,motorCurrent={1},realZ={2},flying={3},collid={4},jFrames={5}",
+            //     m_controllingPrim.LocalID, m_velocityMotor.CurrentValue, realVelocityZ, m_controllingPrim.Flying, m_controllingPrim.IsColliding, m_jumpFrames);
 
             //Alicia: Maintain minimum height when flying.
             // SL has a flying effect that keeps the avatar flying above the ground by some margin
@@ -291,6 +285,8 @@ public class BSActorAvatarMove : BSActor
 
                 if( m_controllingPrim.Position.Z < hover_height)
                 {
+                    m_physicsScene.DetailLog("{0},BSCharacter.MoveMotor,addingUpforceForGroundMargin,height={1},hoverHeight={2}",
+                                m_controllingPrim.LocalID, m_controllingPrim.Position.Z, hover_height);
                     stepVelocity.Z += BSParam.AvatarFlyingGroundUpForce;
                 }
             }
diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSActorHover.cs b/OpenSim/Region/PhysicsModules/BulletS/BSActorHover.cs
index 7ff171e..7ff171e 100644..100755
--- a/OpenSim/Region/PhysicsModules/BulletS/BSActorHover.cs
+++ b/OpenSim/Region/PhysicsModules/BulletS/BSActorHover.cs
diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSActorLockAxis.cs b/OpenSim/Region/PhysicsModules/BulletS/BSActorLockAxis.cs
index 78c1b6a..78c1b6a 100644..100755
--- a/OpenSim/Region/PhysicsModules/BulletS/BSActorLockAxis.cs
+++ b/OpenSim/Region/PhysicsModules/BulletS/BSActorLockAxis.cs
diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSActorMoveToTarget.cs b/OpenSim/Region/PhysicsModules/BulletS/BSActorMoveToTarget.cs
index 3db8f2c..f2019ae 100644..100755
--- a/OpenSim/Region/PhysicsModules/BulletS/BSActorMoveToTarget.cs
+++ b/OpenSim/Region/PhysicsModules/BulletS/BSActorMoveToTarget.cs
@@ -205,13 +205,13 @@ public class BSActorMoveToTarget : BSActor
             addedForce = correctionVector / timeStep;
             // Remove the existing velocity (only the moveToTarget force counts)
             addedForce -= m_controllingPrim.RawVelocity;
-            // Overcome gravity. 
+            // Overcome gravity.
             addedForce -= m_controllingPrim.Gravity;
 
             // Add enough force to overcome the mass of the object
             addedForce *= m_controllingPrim.Mass;
 
-            m_controllingPrim.AddForce(addedForce, false /* pushForce */, true /* inTaintTime */);
+            m_controllingPrim.AddForce(true /* inTaintTime */, addedForce);
         }
         m_physicsScene.DetailLog("{0},BSActorMoveToTarget.Mover3,move,fromPos={1},addedForce={2}",
                                         m_controllingPrim.LocalID, origPosition, addedForce);
diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSActorSetForce.cs b/OpenSim/Region/PhysicsModules/BulletS/BSActorSetForce.cs
index ecb4b7f..ecb4b7f 100644..100755
--- a/OpenSim/Region/PhysicsModules/BulletS/BSActorSetForce.cs
+++ b/OpenSim/Region/PhysicsModules/BulletS/BSActorSetForce.cs
diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSActorSetTorque.cs b/OpenSim/Region/PhysicsModules/BulletS/BSActorSetTorque.cs
index a1cf4db..0261bcb 100644..100755
--- a/OpenSim/Region/PhysicsModules/BulletS/BSActorSetTorque.cs
+++ b/OpenSim/Region/PhysicsModules/BulletS/BSActorSetTorque.cs
@@ -127,7 +127,7 @@ public class BSActorSetTorque : BSActor
         m_physicsScene.DetailLog("{0},BSActorSetTorque,preStep,force={1}", m_controllingPrim.LocalID, m_controllingPrim.RawTorque);
         if (m_controllingPrim.PhysBody.HasPhysicalBody)
         {
-            m_controllingPrim.AddAngularForce(m_controllingPrim.RawTorque, false, true);
+            m_controllingPrim.AddAngularForce(true /* inTaintTime */, m_controllingPrim.RawTorque);
             m_controllingPrim.ActivateIfPhysical(false);
         }
 
diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSActors.cs b/OpenSim/Region/PhysicsModules/BulletS/BSActors.cs
index 851347b..851347b 100644..100755
--- a/OpenSim/Region/PhysicsModules/BulletS/BSActors.cs
+++ b/OpenSim/Region/PhysicsModules/BulletS/BSActors.cs
diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSApiTemplate.cs b/OpenSim/Region/PhysicsModules/BulletS/BSApiTemplate.cs
index 7756b10..a288048 100644
--- a/OpenSim/Region/PhysicsModules/BulletS/BSApiTemplate.cs
+++ b/OpenSim/Region/PhysicsModules/BulletS/BSApiTemplate.cs
@@ -36,16 +36,16 @@ namespace OpenSim.Region.PhysicsModule.BulletS {
     // Constraint type values as defined by Bullet
 public enum ConstraintType : int
 {
-        POINT2POINT_CONSTRAINT_TYPE = 3,
-        HINGE_CONSTRAINT_TYPE,
-        CONETWIST_CONSTRAINT_TYPE,
-        D6_CONSTRAINT_TYPE,
-        SLIDER_CONSTRAINT_TYPE,
-        CONTACT_CONSTRAINT_TYPE,
-        D6_SPRING_CONSTRAINT_TYPE,
-        GEAR_CONSTRAINT_TYPE,           // added in Bullet 2.82
-        FIXED_CONSTRAINT_TYPE,          // added in Bullet 2.82
-        MAX_CONSTRAINT_TYPE,            // last type defined by Bullet
+    POINT2POINT_CONSTRAINT_TYPE = 3,
+    HINGE_CONSTRAINT_TYPE,
+    CONETWIST_CONSTRAINT_TYPE,
+    D6_CONSTRAINT_TYPE,
+    SLIDER_CONSTRAINT_TYPE,
+    CONTACT_CONSTRAINT_TYPE,
+    D6_SPRING_CONSTRAINT_TYPE,
+    GEAR_CONSTRAINT_TYPE,           // added in Bullet 2.82
+    FIXED_CONSTRAINT_TYPE,          // added in Bullet 2.82
+    MAX_CONSTRAINT_TYPE,            // last type defined by Bullet
     //
     BS_FIXED_CONSTRAINT_TYPE = 1234    // BulletSim constraint that is fixed and unmoving
 }
@@ -54,25 +54,25 @@ public enum ConstraintType : int
 [StructLayout(LayoutKind.Sequential)]
 public struct ConvexHull
 {
-        Vector3 Offset;
-        int VertexCount;
-        Vector3[] Vertices;
+    Vector3 Offset;
+    int VertexCount;
+    Vector3[] Vertices;
 }
 public enum BSPhysicsShapeType
 {
-        SHAPE_UNKNOWN   = 0,
-        SHAPE_CAPSULE   = 1,
-        SHAPE_BOX       = 2,
-        SHAPE_CONE      = 3,
-        SHAPE_CYLINDER  = 4,
-        SHAPE_SPHERE    = 5,
-        SHAPE_MESH      = 6,
-        SHAPE_HULL      = 7,
+    SHAPE_UNKNOWN   = 0,
+    SHAPE_CAPSULE   = 1,
+    SHAPE_BOX       = 2,
+    SHAPE_CONE      = 3,
+    SHAPE_CYLINDER  = 4,
+    SHAPE_SPHERE    = 5,
+    SHAPE_MESH      = 6,
+    SHAPE_HULL      = 7,
     // following defined by BulletSim
-        SHAPE_GROUNDPLANE  = 20,
-        SHAPE_TERRAIN   = 21,
-        SHAPE_COMPOUND  = 22,
-        SHAPE_HEIGHTMAP = 23,
+    SHAPE_GROUNDPLANE  = 20,
+    SHAPE_TERRAIN   = 21,
+    SHAPE_COMPOUND  = 22,
+    SHAPE_HEIGHTMAP = 23,
     SHAPE_AVATAR    = 24,
     SHAPE_CONVEXHULL= 25,
     SHAPE_GIMPACT   = 26,
@@ -121,6 +121,14 @@ public struct SweepHit
     public float Fraction;
     public Vector3 Normal;
     public Vector3 Point;
+
+    public bool hasHit()
+    {
+        float sum = Fraction
+                + Normal.X + Normal.Y + Normal.Z
+                + Point.X + Point.Y + Point.Z;
+        return (sum != 0) || (ID != 0);
+    }
 }
 [StructLayout(LayoutKind.Sequential)]
 public struct RaycastHit
@@ -128,6 +136,13 @@ public struct RaycastHit
     public UInt32 ID;
     public float Fraction;
     public Vector3 Normal;
+    public Vector3 Point;
+
+    public bool hasHit()
+    {
+        float sum = Normal.X + Normal.Y + Normal.Z + Point.X + Point.Y + Point.Z;
+        return (sum != 0);
+    }
 }
 [StructLayout(LayoutKind.Sequential)]
 public struct CollisionDesc
@@ -180,16 +195,16 @@ public struct ConfigurationParameters
     public float collisionMargin;
     public float gravity;
 
-        public float maxPersistantManifoldPoolSize;
-        public float maxCollisionAlgorithmPoolSize;
-        public float shouldDisableContactPoolDynamicAllocation;
-        public float shouldForceUpdateAllAabbs;
-        public float shouldRandomizeSolverOrder;
-        public float shouldSplitSimulationIslands;
-        public float shouldEnableFrictionCaching;
-        public float numberOfSolverIterations;
+    public float maxPersistantManifoldPoolSize;
+    public float maxCollisionAlgorithmPoolSize;
+    public float shouldDisableContactPoolDynamicAllocation;
+    public float shouldForceUpdateAllAabbs;
+    public float shouldRandomizeSolverOrder;
+    public float shouldSplitSimulationIslands;
+    public float shouldEnableFrictionCaching;
+    public float numberOfSolverIterations;
     public float useSingleSidedMeshes;
-        public float globalContactBreakingThreshold;
+    public float globalContactBreakingThreshold;
 
     public float physicsLoggingFrames;
 
@@ -202,30 +217,30 @@ public struct ConfigurationParameters
 public struct HACDParams
 {
                                             // usual default values
-        public float maxVerticesPerHull;                // 100
-        public float minClusters;                               // 2
-        public float compacityWeight;                   // 0.1
-        public float volumeWeight;                              // 0.0
-        public float concavity;                             // 100
-        public float addExtraDistPoints;                // false
-        public float addNeighboursDistPoints;   // false
-        public float addFacesPoints;                    // false
-        public float shouldAdjustCollisionMargin;       // false
+    public float maxVerticesPerHull;            // 100
+    public float minClusters;                           // 2
+    public float compacityWeight;                       // 0.1
+    public float volumeWeight;                          // 0.0
+    public float concavity;                                 // 100
+    public float addExtraDistPoints;            // false
+    public float addNeighboursDistPoints;       // false
+    public float addFacesPoints;                        // false
+    public float shouldAdjustCollisionMargin;   // false
     // VHACD
-        public float whichHACD;                             // zero if Bullet HACD, non-zero says VHACD
-        // http://kmamou.blogspot.ca/2014/12/v-hacd-20-parameters-description.html
-        public float vHACDresolution;                   // 100,000 max number of voxels generated during voxelization stage
-        public float vHACDdepth;                                // 20 max number of clipping stages
-        public float vHACDconcavity;                    // 0.0025 maximum concavity
-        public float vHACDplaneDownsampling;    // 4 granularity of search for best clipping plane
-        public float vHACDconvexHullDownsampling;       // 4 precision of hull gen process
-        public float vHACDalpha;                                // 0.05 bias toward clipping along symmetry planes
-        public float vHACDbeta;                             // 0.05 bias toward clipping along revolution axis
-        public float vHACDgamma;                                // 0.00125 max concavity when merging
-        public float vHACDpca;                                  // 0 on/off normalizing mesh before decomp
-        public float vHACDmode;                             // 0 0:voxel based, 1: tetrahedron based
-        public float vHACDmaxNumVerticesPerCH;  // 64 max triangles per convex hull
-        public float vHACDminVolumePerCH;               // 0.0001 sampling of generated convex hulls
+    public float whichHACD;                                 // zero if Bullet HACD, non-zero says VHACD
+    // http://kmamou.blogspot.ca/2014/12/v-hacd-20-parameters-description.html
+    public float vHACDresolution;                       // 100,000 max number of voxels generated during voxelization stage
+    public float vHACDdepth;                            // 20 max number of clipping stages
+    public float vHACDconcavity;                        // 0.0025 maximum concavity
+    public float vHACDplaneDownsampling;        // 4 granularity of search for best clipping plane
+    public float vHACDconvexHullDownsampling;   // 4 precision of hull gen process
+    public float vHACDalpha;                            // 0.05 bias toward clipping along symmetry planes
+    public float vHACDbeta;                                 // 0.05 bias toward clipping along revolution axis
+    public float vHACDgamma;                            // 0.00125 max concavity when merging
+    public float vHACDpca;                                      // 0 on/off normalizing mesh before decomp
+    public float vHACDmode;                                 // 0 0:voxel based, 1: tetrahedron based
+    public float vHACDmaxNumVerticesPerCH;      // 64 max triangles per convex hull
+    public float vHACDminVolumePerCH;           // 0.0001 sampling of generated convex hulls
 }
 
 // The states a bullet collision object can have
@@ -322,8 +337,8 @@ public abstract string BulletEngineVersion { get; protected set;}
 
 // Initialization and simulation
 public abstract BulletWorld Initialize(Vector3 maxPosition, ConfigurationParameters parms,
-                                                                                        int maxCollisions,  ref CollisionDesc[] collisionArray,
-                                                                                        int maxUpdates, ref EntityProperties[] updateArray
+                                            int maxCollisions,  ref CollisionDesc[] collisionArray,
+                                            int maxUpdates, ref EntityProperties[] updateArray
                                             );
 
 public abstract int PhysicsStep(BulletWorld world, float timeStep, int maxSubSteps, float fixedTimeStep,
@@ -398,7 +413,7 @@ public abstract void DestroyObject(BulletWorld sim, BulletBody obj);
 public abstract BulletShape CreateGroundPlaneShape(UInt32 id, float height, float collisionMargin);
 
 public abstract BulletShape CreateTerrainShape(UInt32 id, Vector3 size, float minHeight, float maxHeight, float[] heightMap,
-                                                                float scaleFactor, float collisionMargin);
+                                float scaleFactor, float collisionMargin);
 
 // =====================================================================================
 // Constraint creation and helper routines
@@ -742,6 +757,12 @@ public abstract void SetMargin(BulletShape shape, float val);
 public abstract float GetMargin(BulletShape shape);
 
 // =====================================================================================
+// Raycast
+public abstract SweepHit ConvexSweepTest2(BulletWorld world, BulletBody obj, Vector3 from, Vector3 to, float margin);
+
+public abstract RaycastHit RayTest2(BulletWorld world, Vector3 from, Vector3 to, uint filterGroup, uint filterMask);
+
+// =====================================================================================
 // Debugging
 public virtual void DumpRigidBody(BulletWorld sim, BulletBody collisionObject) { }
 
diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSCharacter.cs b/OpenSim/Region/PhysicsModules/BulletS/BSCharacter.cs
index 83fc3a6..d182c34 100644
--- a/OpenSim/Region/PhysicsModules/BulletS/BSCharacter.cs
+++ b/OpenSim/Region/PhysicsModules/BulletS/BSCharacter.cs
@@ -40,7 +40,6 @@ public sealed class BSCharacter : BSPhysObject
     private static readonly string LogHeader = "[BULLETS CHAR]";
 
     // private bool _stopped;
-    private OMV.Vector3 _size;
     private bool _grabbed;
     private bool _selected;
     private float _mass;
@@ -53,19 +52,21 @@ public sealed class BSCharacter : BSPhysObject
     private bool _setAlwaysRun;
     private bool _throttleUpdates;
     private bool _floatOnWater;
-    private OMV.Vector3 _rotationalVelocity;
     private bool _kinematic;
     private float _buoyancy;
 
+    private OMV.Vector3 _size;
+    private float _footOffset;
+
     private BSActorAvatarMove m_moveActor;
     private const string AvatarMoveActorName = "BSCharacter.AvatarMove";
 
     private OMV.Vector3 _PIDTarget;
     private float _PIDTau;
 
-//        public override OMV.Vector3 RawVelocity 
-//        { get { return base.RawVelocity; } 
-//            set { 
+//        public override OMV.Vector3 RawVelocity
+//        { get { return base.RawVelocity; }
+//            set {
 //                if (value != base.RawVelocity)
 //                    Util.PrintCallStack();
 //                Console.WriteLine("Set rawvel to {0}", value);
@@ -76,12 +77,12 @@ public sealed class BSCharacter : BSPhysObject
     public override bool IsIncomplete { get { return false; } }
 
     public BSCharacter(
-            uint localID, String avName, BSScene parent_scene, OMV.Vector3 pos, OMV.Vector3 vel, OMV.Vector3 size, bool isFlying)
+            uint localID, String avName, BSScene parent_scene, OMV.Vector3 pos, OMV.Vector3 vel, OMV.Vector3 size, float footOffset, bool isFlying)
 
             : base(parent_scene, localID, avName, "BSCharacter")
     {
         _physicsActorType = (int)ActorTypes.Agent;
-        RawPosition = pos;        
+        RawPosition = pos;
 
         _flying = isFlying;
         RawOrientation = OMV.Quaternion.Identity;
@@ -89,22 +90,15 @@ public sealed class BSCharacter : BSPhysObject
         _buoyancy = ComputeBuoyancyFromFlying(isFlying);
         Friction = BSParam.AvatarStandingFriction;
         Density = BSParam.AvatarDensity;
+        _isPhysical = true;
 
-        // Old versions of ScenePresence passed only the height. If width and/or depth are zero,
-        //     replace with the default values.
-        _size = size;
-        if (_size.X == 0f) _size.X = BSParam.AvatarCapsuleDepth;
-        if (_size.Y == 0f) _size.Y = BSParam.AvatarCapsuleWidth;
-
-        // The dimensions of the physical capsule are kept in the scale.
-        // Physics creates a unit capsule which is scaled by the physics engine.
-        Scale = ComputeAvatarScale(_size);
-        // set _avatarVolume and _mass based on capsule size, _density and Scale
-        ComputeAvatarVolumeAndMass();
+        // Adjustments for zero X and Y made in Size()
+        // This also computes avatar scale, volume, and mass
+        SetAvatarSize(size, footOffset, true /* initializing */);
 
         DetailLog(
             "{0},BSCharacter.create,call,size={1},scale={2},density={3},volume={4},mass={5},pos={6},vel={7}",
-            LocalID, _size, Scale, Density, _avatarVolume, RawMass, pos, vel);
+            LocalID, Size, Scale, Density, _avatarVolume, RawMass, pos, vel);
 
         // do actual creation in taint time
         PhysScene.TaintedObject(LocalID, "BSCharacter.create", delegate()
@@ -208,45 +202,68 @@ public sealed class BSCharacter : BSPhysObject
     public override OMV.Vector3 Size {
         get
         {
-            // Avatar capsule size is kept in the scale parameter.
             return _size;
         }
 
         set {
-            // This is how much the avatar size is changing. Positive means getting bigger.
-            // The avatar altitude must be adjusted for this change.
-            float heightChange = value.Z - _size.Z;
+            setAvatarSize(value, _footOffset);
+        }
+    }
+
+    // OpenSim 0.9 introduces a common avatar size computation
+    public override void setAvatarSize(OMV.Vector3 size, float feetOffset)
+    {
+        SetAvatarSize(size, feetOffset, false /* initializing */);
+    }
 
-            _size = value;
+    // Internal version that, if initializing, doesn't do all the updating of the physics engine
+    public void SetAvatarSize(OMV.Vector3 size, float feetOffset, bool initializing)
+    {
+        OMV.Vector3 newSize = size;
+        if (newSize.IsFinite())
+        {
             // Old versions of ScenePresence passed only the height. If width and/or depth are zero,
             //     replace with the default values.
-            if (_size.X == 0f) _size.X = BSParam.AvatarCapsuleDepth;
-            if (_size.Y == 0f) _size.Y = BSParam.AvatarCapsuleWidth;
+            if (newSize.X == 0f) newSize.X = BSParam.AvatarCapsuleDepth;
+            if (newSize.Y == 0f) newSize.Y = BSParam.AvatarCapsuleWidth;
 
-            Scale = ComputeAvatarScale(_size);
-            ComputeAvatarVolumeAndMass();
-            DetailLog("{0},BSCharacter.setSize,call,size={1},scale={2},density={3},volume={4},mass={5}",
-                            LocalID, _size, Scale, Density, _avatarVolume, RawMass);
+            if (newSize.X < 0.01f) newSize.X = 0.01f;
+            if (newSize.Y < 0.01f) newSize.Y = 0.01f;
+            if (newSize.Z < 0.01f) newSize.Z = BSParam.AvatarCapsuleHeight;
+        }
+        else
+        {
+            newSize = new OMV.Vector3(BSParam.AvatarCapsuleDepth, BSParam.AvatarCapsuleWidth, BSParam.AvatarCapsuleHeight);
+        }
 
-            PhysScene.TaintedObject(LocalID, "BSCharacter.setSize", delegate()
-            {
-                if (PhysBody.HasPhysicalBody && PhysShape.physShapeInfo.HasPhysicalShape)
-                {
-                    PhysScene.PE.SetLocalScaling(PhysShape.physShapeInfo, Scale);
-                    UpdatePhysicalMassProperties(RawMass, true);
+        // This is how much the avatar size is changing. Positive means getting bigger.
+        // The avatar altitude must be adjusted for this change.
+        float heightChange = newSize.Z - Size.Z;
 
-                    // Adjust the avatar's position to account for the increase/decrease in size
-                    ForcePosition = new OMV.Vector3(RawPosition.X, RawPosition.Y, RawPosition.Z + heightChange / 2f);
+        _size = newSize;
 
-                    // Make sure this change appears as a property update event
-                    PhysScene.PE.PushUpdate(PhysBody);
-                }
-            });
+        Scale = ComputeAvatarScale(Size);
+        ComputeAvatarVolumeAndMass();
+        DetailLog("{0},BSCharacter.setSize,call,size={1},scale={2},density={3},volume={4},mass={5}",
+                        LocalID, _size, Scale, Density, _avatarVolume, RawMass);
 
-        }
+        PhysScene.TaintedObject(LocalID, "BSCharacter.setSize", delegate()
+        {
+            if (PhysBody.HasPhysicalBody && PhysShape.physShapeInfo.HasPhysicalShape)
+            {
+                PhysScene.PE.SetLocalScaling(PhysShape.physShapeInfo, Scale);
+                UpdatePhysicalMassProperties(RawMass, true);
+
+                // Adjust the avatar's position to account for the increase/decrease in size
+                ForcePosition = new OMV.Vector3(RawPosition.X, RawPosition.Y, RawPosition.Z + heightChange / 2f);
+
+                // Make sure this change appears as a property update event
+                PhysScene.PE.PushUpdate(PhysBody);
+            }
+        });
     }
 
-    public override PrimitiveBaseShape Shape
+        public override PrimitiveBaseShape Shape
     {
         set { BaseShape = value; }
     }
@@ -273,7 +290,7 @@ public sealed class BSCharacter : BSPhysObject
     {
         RawVelocity = OMV.Vector3.Zero;
         _acceleration = OMV.Vector3.Zero;
-        _rotationalVelocity = OMV.Vector3.Zero;
+        RawRotationalVelocity = OMV.Vector3.Zero;
 
         // Zero some other properties directly into the physics engine
         PhysScene.TaintedObject(inTaintTime, LocalID, "BSCharacter.ZeroMotion", delegate()
@@ -285,7 +302,7 @@ public sealed class BSCharacter : BSPhysObject
 
     public override void ZeroAngularMotion(bool inTaintTime)
     {
-        _rotationalVelocity = OMV.Vector3.Zero;
+        RawRotationalVelocity = OMV.Vector3.Zero;
 
         PhysScene.TaintedObject(inTaintTime, LocalID, "BSCharacter.ZeroMotion", delegate()
         {
@@ -300,7 +317,7 @@ public sealed class BSCharacter : BSPhysObject
     }
 
 
-    public override void LockAngularMotion(OMV.Vector3 axis) { return; }
+    public override void LockAngularMotion(byte axislocks) { return; }
 
     public override OMV.Vector3 Position {
         get {
@@ -333,7 +350,6 @@ public sealed class BSCharacter : BSPhysObject
         }
     }
 
-
     // Check that the current position is sane and, if not, modify the position to make it so.
     // Check for being below terrain or on water.
     // Returns 'true' of the position was made sane by some action.
@@ -433,6 +449,7 @@ public sealed class BSCharacter : BSPhysObject
     public override OMV.Vector3 GeometricCenter { get { return OMV.Vector3.Zero; } }
     public override OMV.Vector3 CenterOfMass { get { return OMV.Vector3.Zero; } }
 
+    // PhysicsActor.TargetVelocity
     // Sets the target in the motor. This starts the changing of the avatar's velocity.
     public override OMV.Vector3 TargetVelocity
     {
@@ -443,43 +460,51 @@ public sealed class BSCharacter : BSPhysObject
         set
         {
             DetailLog("{0},BSCharacter.setTargetVelocity,call,vel={1}", LocalID, value);
-            m_targetVelocity = value;
             OMV.Vector3 targetVel = value;
-            if (_setAlwaysRun && !_flying)
-                targetVel *= new OMV.Vector3(BSParam.AvatarAlwaysRunFactor, BSParam.AvatarAlwaysRunFactor, 1f);
+            if (!_flying)
+            {
+                if (_setAlwaysRun)
+                    targetVel *= new OMV.Vector3(BSParam.AvatarAlwaysRunFactor, BSParam.AvatarAlwaysRunFactor, 1f);
+                else
+                    if (BSParam.AvatarWalkVelocityFactor != 1f)
+                        targetVel *= new OMV.Vector3(BSParam.AvatarWalkVelocityFactor, BSParam.AvatarWalkVelocityFactor, 1f);
+            }
+            base.m_targetVelocity = targetVel;
 
             if (m_moveActor != null)
-                m_moveActor.SetVelocityAndTarget(RawVelocity, targetVel, false /* inTaintTime */);
+                m_moveActor.SetVelocityAndTarget(RawVelocity, base.m_targetVelocity, false /* inTaintTime */);
         }
     }
     // Directly setting velocity means this is what the user really wants now.
     public override OMV.Vector3 Velocity {
         get { return RawVelocity; }
         set {
-            RawVelocity = value;
-                OMV.Vector3 vel = RawVelocity;
-
-            DetailLog("{0}: set Velocity = {1}", LocalID, value);
-
-            PhysScene.TaintedObject(LocalID, "BSCharacter.setVelocity", delegate()
+            if (m_moveActor != null)
             {
-                if (m_moveActor != null)
-                    m_moveActor.SetVelocityAndTarget(vel, vel, true /* inTaintTime */);
+                // m_moveActor.SetVelocityAndTarget(OMV.Vector3.Zero, OMV.Vector3.Zero, false /* inTaintTime */);
+                m_moveActor.SetVelocityAndTarget(RawVelocity, RawVelocity, false /* inTaintTime */);
+            }
+            base.Velocity = value;
+        }
+    }
 
-                DetailLog("{0},BSCharacter.setVelocity,taint,vel={1}", LocalID, vel);
-                ForceVelocity = vel;
-            });
+    // SetMomentum just sets the velocity without a target. We need to stop the movement actor if a character.
+    public override void SetMomentum(OMV.Vector3 momentum)
+    {
+        if (m_moveActor != null)
+        {
+            // m_moveActor.SetVelocityAndTarget(OMV.Vector3.Zero, OMV.Vector3.Zero, false /* inTaintTime */);
+            m_moveActor.SetVelocityAndTarget(RawVelocity, RawVelocity, false /* inTaintTime */);
         }
+        base.SetMomentum(momentum);
     }
 
     public override OMV.Vector3 ForceVelocity {
         get { return RawVelocity; }
         set {
-            PhysScene.AssertInTaintTime("BSCharacter.ForceVelocity");
-//                Util.PrintCallStack();
-            DetailLog("{0}: set ForceVelocity = {1}", LocalID, value);
+            DetailLog("{0},BSCharacter.ForceVelocity.set={1}", LocalID, value);
 
-            RawVelocity = value;
+            RawVelocity = Util.ClampV(value, BSParam.MaxLinearVelocity);
             PhysScene.PE.SetLinearVelocity(PhysBody, RawVelocity);
             PhysScene.PE.Activate(PhysBody, true);
         }
@@ -600,14 +625,6 @@ public sealed class BSCharacter : BSPhysObject
             });
         }
     }
-    public override OMV.Vector3 RotationalVelocity {
-        get { return _rotationalVelocity; }
-        set { _rotationalVelocity = value; }
-    }
-    public override OMV.Vector3 ForceRotationalVelocity {
-        get { return _rotationalVelocity; }
-        set { _rotationalVelocity = value; }
-    }
     public override bool Kinematic {
         get { return _kinematic; }
         set { _kinematic = value; }
@@ -626,8 +643,6 @@ public sealed class BSCharacter : BSPhysObject
     public override float ForceBuoyancy {
         get { return _buoyancy; }
         set {
-            PhysScene.AssertInTaintTime("BSCharacter.ForceBuoyancy");
-
             _buoyancy = value;
             DetailLog("{0},BSCharacter.setForceBuoyancy,taint,buoy={1}", LocalID, _buoyancy);
             // Buoyancy is faked by changing the gravity applied to the object
@@ -652,14 +667,24 @@ public sealed class BSCharacter : BSPhysObject
     public override void AddForce(OMV.Vector3 force, bool pushforce)
     {
         // Since this force is being applied in only one step, make this a force per second.
-        OMV.Vector3 addForce = force / PhysScene.LastTimeStep;
-        AddForce(addForce, pushforce, false);
+        OMV.Vector3 addForce = force;
+
+        // The interaction of this force with the simulator rate and collision occurance is tricky.
+        // ODE multiplies the force by 100
+        // ubODE multiplies the force by 5.3
+        // BulletSim, after much in-world testing, thinks it gets a similar effect by multiplying mass*0.315f
+        //    This number could be a feature of friction or timing, but it seems to move avatars the same as ubODE
+        addForce *= Mass * BSParam.AvatarAddForcePushFactor;
+
+        DetailLog("{0},BSCharacter.addForce,call,force={1},addForce={2},push={3},mass={4}", LocalID, force, addForce, pushforce, Mass);
+        AddForce(false, addForce);
     }
-    public override void AddForce(OMV.Vector3 force, bool pushforce, bool inTaintTime) {
+
+    public override void AddForce(bool inTaintTime, OMV.Vector3 force) {
         if (force.IsFinite())
         {
             OMV.Vector3 addForce = Util.ClampV(force, BSParam.MaxAddForceMagnitude);
-            // DetailLog("{0},BSCharacter.addForce,call,force={1}", LocalID, addForce);
+            // DetailLog("{0},BSCharacter.addForce,call,force={1},push={2},inTaint={3}", LocalID, addForce, pushforce, inTaintTime);
 
             PhysScene.TaintedObject(inTaintTime, LocalID, "BSCharacter.AddForce", delegate()
             {
@@ -667,7 +692,15 @@ public sealed class BSCharacter : BSPhysObject
                 // DetailLog("{0},BSCharacter.addForce,taint,force={1}", LocalID, addForce);
                 if (PhysBody.HasPhysicalBody)
                 {
+                    // Bullet adds this central force to the total force for this tick.
+                    // Deep down in Bullet:
+                    //      linearVelocity += totalForce / mass * timeStep;
                     PhysScene.PE.ApplyCentralForce(PhysBody, addForce);
+                    PhysScene.PE.Activate(PhysBody, true);
+                }
+                if (m_moveActor != null)
+                {
+                    m_moveActor.SuppressStationayCheckUntilLowVelocity();
                 }
             });
         }
@@ -678,65 +711,74 @@ public sealed class BSCharacter : BSPhysObject
         }
     }
 
-    public override void AddAngularForce(OMV.Vector3 force, bool pushforce, bool inTaintTime) {
-    }
-    public override void SetMomentum(OMV.Vector3 momentum) {
+    public override void AddAngularForce(bool inTaintTime, OMV.Vector3 force) {
     }
 
+    // The avatar's physical shape (whether capsule or cube) is unit sized. BulletSim sets
+    //    the scale of that unit shape to create the avatars full size.
     private OMV.Vector3 ComputeAvatarScale(OMV.Vector3 size)
     {
         OMV.Vector3 newScale = size;
 
-        // Bullet's capsule total height is the "passed height + radius * 2";
-        // The base capsule is 1 unit in diameter and 2 units in height (passed radius=0.5, passed height = 1)
-        // The number we pass in for 'scaling' is the multiplier to get that base
-        //     shape to be the size desired.
-        // So, when creating the scale for the avatar height, we take the passed height
-        //     (size.Z) and remove the caps.
-        // An oddity of the Bullet capsule implementation is that it presumes the Y
-        //     dimension is the radius of the capsule. Even though some of the code allows
-        //     for a asymmetrical capsule, other parts of the code presume it is cylindrical.
+        if (BSParam.AvatarUseBefore09SizeComputation)
+        {
 
-        // Scale is multiplier of radius with one of "0.5"
+            // Bullet's capsule total height is the "passed height + radius * 2";
+            // The base capsule is 1 unit in diameter and 2 units in height (passed radius=0.5, passed height = 1)
+            // The number we pass in for 'scaling' is the multiplier to get that base
+            //     shape to be the size desired.
+            // So, when creating the scale for the avatar height, we take the passed height
+            //     (size.Z) and remove the caps.
+            // An oddity of the Bullet capsule implementation is that it presumes the Y
+            //     dimension is the radius of the capsule. Even though some of the code allows
+            //     for a asymmetrical capsule, other parts of the code presume it is cylindrical.
 
-        float heightAdjust = BSParam.AvatarHeightMidFudge;
-        if (BSParam.AvatarHeightLowFudge != 0f || BSParam.AvatarHeightHighFudge != 0f)
-        {
-            const float AVATAR_LOW = 1.1f;
-            const float AVATAR_MID = 1.775f; // 1.87f
-            const float AVATAR_HI = 2.45f;
-            // An avatar is between 1.1 and 2.45 meters. Midpoint is 1.775m.
-            float midHeightOffset = size.Z - AVATAR_MID;
-            if (midHeightOffset < 0f)
+            // Scale is multiplier of radius with one of "0.5"
+
+            float heightAdjust = BSParam.AvatarHeightMidFudge;
+            if (BSParam.AvatarHeightLowFudge != 0f || BSParam.AvatarHeightHighFudge != 0f)
             {
-                // Small avatar. Add the adjustment based on the distance from midheight
-                heightAdjust += ((-1f * midHeightOffset) / (AVATAR_MID - AVATAR_LOW)) * BSParam.AvatarHeightLowFudge;
+                const float AVATAR_LOW = 1.1f;
+                const float AVATAR_MID = 1.775f; // 1.87f
+                const float AVATAR_HI = 2.45f;
+                // An avatar is between 1.1 and 2.45 meters. Midpoint is 1.775m.
+                float midHeightOffset = size.Z - AVATAR_MID;
+                if (midHeightOffset < 0f)
+                {
+                    // Small avatar. Add the adjustment based on the distance from midheight
+                    heightAdjust += ((-1f * midHeightOffset) / (AVATAR_MID - AVATAR_LOW)) * BSParam.AvatarHeightLowFudge;
+                }
+                else
+                {
+                    // Large avatar. Add the adjustment based on the distance from midheight
+                    heightAdjust += ((midHeightOffset) / (AVATAR_HI - AVATAR_MID)) * BSParam.AvatarHeightHighFudge;
+                }
+            }
+            if (BSParam.AvatarShape == BSShapeCollection.AvatarShapeCapsule)
+            {
+                newScale.X = size.X / 2f;
+                newScale.Y = size.Y / 2f;
+                // The total scale height is the central cylindar plus the caps on the two ends.
+                newScale.Z = (size.Z + (Math.Min(size.X, size.Y) * 2) + heightAdjust) / 2f;
             }
             else
             {
-                // Large avatar. Add the adjustment based on the distance from midheight
-                heightAdjust += ((midHeightOffset) / (AVATAR_HI - AVATAR_MID)) * BSParam.AvatarHeightHighFudge;
+                newScale.Z = size.Z + heightAdjust;
             }
-        }
-        if (BSParam.AvatarShape == BSShapeCollection.AvatarShapeCapsule)
-        {
-            newScale.X = size.X / 2f;
-            newScale.Y = size.Y / 2f;
-            // The total scale height is the central cylindar plus the caps on the two ends.
-            newScale.Z = (size.Z + (Math.Min(size.X, size.Y) * 2) + heightAdjust) / 2f;
+            // m_log.DebugFormat("{0} ComputeAvatarScale: size={1},adj={2},scale={3}", LogHeader, size, heightAdjust, newScale);
+
+            // If smaller than the endcaps, just fake like we're almost that small
+            if (newScale.Z < 0)
+                newScale.Z = 0.1f;
+
+            DetailLog("{0},BSCharacter.ComputeAvatarScale,size={1},lowF={2},midF={3},hiF={4},adj={5},newScale={6}",
+                LocalID, size, BSParam.AvatarHeightLowFudge, BSParam.AvatarHeightMidFudge, BSParam.AvatarHeightHighFudge, heightAdjust, newScale);
         }
         else
         {
-            newScale.Z = size.Z + heightAdjust;
+            newScale.Z = size.Z + _footOffset;
+            DetailLog("{0},BSCharacter.ComputeAvatarScale,using newScale={1}, footOffset={2}", LocalID, newScale, _footOffset);
         }
-        // m_log.DebugFormat("{0} ComputeAvatarScale: size={1},adj={2},scale={3}", LogHeader, size, heightAdjust, newScale);
-
-        // If smaller than the endcaps, just fake like we're almost that small
-        if (newScale.Z < 0)
-            newScale.Z = 0.1f;
-
-        DetailLog("{0},BSCharacter.ComputerAvatarScale,size={1},lowF={2},midF={3},hiF={4},adj={5},newScale={6}",
-            LocalID, size, BSParam.AvatarHeightLowFudge, BSParam.AvatarHeightMidFudge, BSParam.AvatarHeightHighFudge, heightAdjust, newScale);
 
         return newScale;
     }
@@ -744,17 +786,24 @@ public sealed class BSCharacter : BSPhysObject
     // set _avatarVolume and _mass based on capsule size, _density and Scale
     private void ComputeAvatarVolumeAndMass()
     {
-        _avatarVolume = (float)(
-                        Math.PI
-                        * Size.X / 2f
-                        * Size.Y / 2f    // the area of capsule cylinder
-                        * Size.Z         // times height of capsule cylinder
-                      + 1.33333333f
-                        * Math.PI
-                        * Size.X / 2f
-                        * Math.Min(Size.X, Size.Y) / 2
-                        * Size.Y / 2f    // plus the volume of the capsule end caps
-                        );
+        if (BSParam.AvatarShape == BSShapeCollection.AvatarShapeCapsule)
+        {
+            _avatarVolume = (float)(
+                            Math.PI
+                            * Size.X / 2f
+                            * Size.Y / 2f    // the area of capsule cylinder
+                            * Size.Z         // times height of capsule cylinder
+                          + 1.33333333f
+                            * Math.PI
+                            * Size.X / 2f
+                            * Math.Min(Size.X, Size.Y) / 2
+                            * Size.Y / 2f    // plus the volume of the capsule end caps
+                            );
+        }
+        else
+        {
+            _avatarVolume = Size.X * Size.Y * Size.Z;
+        }
         _mass = Density * BSParam.DensityScaleFactor * _avatarVolume;
     }
 
@@ -773,7 +822,7 @@ public sealed class BSCharacter : BSPhysObject
         //    0.001m/s. Bullet introduces a lot of jitter in the velocity which causes many
         //    extra updates.
         //
-        // XXX: Contrary to the above comment, setting an update threshold here above 0.4 actually introduces jitter to 
+        // XXX: Contrary to the above comment, setting an update threshold here above 0.4 actually introduces jitter to
         // avatar movement rather than removes it.  The larger the threshold, the bigger the jitter.
         // This is most noticeable in level flight and can be seen with
         // the "show updates" option in a viewer.  With an update threshold, the RawVelocity cycles between a lower
@@ -787,7 +836,7 @@ public sealed class BSCharacter : BSPhysObject
             RawVelocity = entprop.Velocity;
 
         _acceleration = entprop.Acceleration;
-        _rotationalVelocity = entprop.RotationalVelocity;
+        RawRotationalVelocity = entprop.RotationalVelocity;
 
         // Do some sanity checking for the avatar. Make sure it's above ground and inbounds.
         if (PositionSanityCheck(true))
@@ -807,7 +856,7 @@ public sealed class BSCharacter : BSPhysObject
         // PhysScene.PostUpdate(this);
 
         DetailLog("{0},BSCharacter.UpdateProperties,call,pos={1},orient={2},vel={3},accel={4},rotVel={5}",
-                LocalID, RawPosition, RawOrientation, RawVelocity, _acceleration, _rotationalVelocity);
+                LocalID, RawPosition, RawOrientation, RawVelocity, _acceleration, RawRotationalVelocity);
     }
 }
 }
diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSConstraint.cs b/OpenSim/Region/PhysicsModules/BulletS/BSConstraint.cs
index e42e868..e42e868 100644..100755
--- a/OpenSim/Region/PhysicsModules/BulletS/BSConstraint.cs
+++ b/OpenSim/Region/PhysicsModules/BulletS/BSConstraint.cs
diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSConstraint6Dof.cs b/OpenSim/Region/PhysicsModules/BulletS/BSConstraint6Dof.cs
index 4bcde2b..4bcde2b 100644..100755
--- a/OpenSim/Region/PhysicsModules/BulletS/BSConstraint6Dof.cs
+++ b/OpenSim/Region/PhysicsModules/BulletS/BSConstraint6Dof.cs
diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSConstraintCollection.cs b/OpenSim/Region/PhysicsModules/BulletS/BSConstraintCollection.cs
index 5746ac1..5746ac1 100644..100755
--- a/OpenSim/Region/PhysicsModules/BulletS/BSConstraintCollection.cs
+++ b/OpenSim/Region/PhysicsModules/BulletS/BSConstraintCollection.cs
diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSConstraintConeTwist.cs b/OpenSim/Region/PhysicsModules/BulletS/BSConstraintConeTwist.cs
index e7566a8..e7566a8 100644..100755
--- a/OpenSim/Region/PhysicsModules/BulletS/BSConstraintConeTwist.cs
+++ b/OpenSim/Region/PhysicsModules/BulletS/BSConstraintConeTwist.cs
diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSConstraintHinge.cs b/OpenSim/Region/PhysicsModules/BulletS/BSConstraintHinge.cs
index d20538d..d20538d 100644..100755
--- a/OpenSim/Region/PhysicsModules/BulletS/BSConstraintHinge.cs
+++ b/OpenSim/Region/PhysicsModules/BulletS/BSConstraintHinge.cs
diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSConstraintSlider.cs b/OpenSim/Region/PhysicsModules/BulletS/BSConstraintSlider.cs
index 83d42af..83d42af 100644..100755
--- a/OpenSim/Region/PhysicsModules/BulletS/BSConstraintSlider.cs
+++ b/OpenSim/Region/PhysicsModules/BulletS/BSConstraintSlider.cs
diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSConstraintSpring.cs b/OpenSim/Region/PhysicsModules/BulletS/BSConstraintSpring.cs
index 563a1b1..563a1b1 100644..100755
--- a/OpenSim/Region/PhysicsModules/BulletS/BSConstraintSpring.cs
+++ b/OpenSim/Region/PhysicsModules/BulletS/BSConstraintSpring.cs
diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSDynamics.cs b/OpenSim/Region/PhysicsModules/BulletS/BSDynamics.cs
index 0fc5577..c4ccdbe 100644
--- a/OpenSim/Region/PhysicsModules/BulletS/BSDynamics.cs
+++ b/OpenSim/Region/PhysicsModules/BulletS/BSDynamics.cs
@@ -125,8 +125,8 @@ namespace OpenSim.Region.PhysicsModule.BulletS
 
         // Just some recomputed constants:
 #pragma warning disable 414
-        static readonly float TwoPI = ((float)Math.PI) * 2f; 
-        static readonly float FourPI = ((float)Math.PI) * 4f; 
+        static readonly float TwoPI = ((float)Math.PI) * 2f;
+        static readonly float FourPI = ((float)Math.PI) * 4f;
         static readonly float PIOverFour = ((float)Math.PI) / 4f;
         static readonly float PIOverTwo = ((float)Math.PI) / 2f;
 #pragma warning restore 414
@@ -768,7 +768,7 @@ namespace OpenSim.Region.PhysicsModule.BulletS
                 }
 
                 if ((m_knownChanged & m_knownChangedForce) != 0)
-                    ControllingPrim.AddForce((Vector3)m_knownForce, false /*pushForce*/, true /*inTaintTime*/);
+                    ControllingPrim.AddForce(false /* inTaintTime */, (Vector3)m_knownForce);
 
                 if ((m_knownChanged & m_knownChangedForceImpulse) != 0)
                     ControllingPrim.AddForceImpulse((Vector3)m_knownForceImpulse, false /*pushforce*/, true /*inTaintTime*/);
@@ -784,7 +784,7 @@ namespace OpenSim.Region.PhysicsModule.BulletS
 
                 if ((m_knownChanged & m_knownChangedRotationalForce) != 0)
                 {
-                    ControllingPrim.AddAngularForce((Vector3)m_knownRotationalForce, false /*pushForce*/, true /*inTaintTime*/);
+                    ControllingPrim.AddAngularForce(true /* inTaintTime */, (Vector3)m_knownRotationalForce);
                 }
 
                 // If we set one of the values (ie, the physics engine didn't do it) we must force
@@ -1595,7 +1595,7 @@ namespace OpenSim.Region.PhysicsModule.BulletS
         //     in that direction.
         // TODO: implement reference frame.
         public void ComputeAngularDeflection()
-        {   
+        {
 
             if (BSParam.VehicleEnableAngularDeflection && m_angularDeflectionEfficiency != 0 && VehicleForwardSpeed > 0.2)
             {
diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSLinkset.cs b/OpenSim/Region/PhysicsModules/BulletS/BSLinkset.cs
index 8312239..13c1361 100644..100755
--- a/OpenSim/Region/PhysicsModules/BulletS/BSLinkset.cs
+++ b/OpenSim/Region/PhysicsModules/BulletS/BSLinkset.cs
@@ -257,7 +257,7 @@ public abstract class BSLinkset
     // Return 'true' if linkset processed the collision. 'false' says the linkset didn't have
     //     anything to add for the collision and it should be passed through normal processing.
     // Default processing for a linkset.
-    public virtual bool HandleCollide(uint collidingWith, BSPhysObject collidee,
+    public virtual bool HandleCollide(BSPhysObject collider, BSPhysObject collidee,
                                 OMV.Vector3 contactPoint, OMV.Vector3 contactNormal, float pentrationDepth)
     {
         bool ret = false;
@@ -319,7 +319,7 @@ public abstract class BSLinkset
     public virtual bool AllPartsComplete
     {
         get {
-            bool ret = true; 
+            bool ret = true;
             this.ForEachMember((member) =>
             {
                 if ((!member.IsInitialized) || member.IsIncomplete || member.PrimAssetState == BSPhysObject.PrimAssetCondition.Waiting)
diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSLinksetCompound.cs b/OpenSim/Region/PhysicsModules/BulletS/BSLinksetCompound.cs
index 953ddee..dc390b2 100644..100755
--- a/OpenSim/Region/PhysicsModules/BulletS/BSLinksetCompound.cs
+++ b/OpenSim/Region/PhysicsModules/BulletS/BSLinksetCompound.cs
@@ -450,6 +450,7 @@ public sealed class BSLinksetCompound : BSLinkset
             m_physicsScene.PE.AddObjectToWorld(m_physicsScene.World, LinksetRoot.PhysBody);
             DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,addBody,body={1},shape={2}",
                                         LinksetRoot.LocalID, LinksetRoot.PhysBody, linksetShape);
+            m_physicsScene.PE.ResetBroadphasePool(m_physicsScene.World);    // DEBUG DEBUG
 
             // With all of the linkset packed into the root prim, it has the mass of everyone.
             LinksetMass = ComputeLinksetMass();
diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSLinksetConstraints.cs b/OpenSim/Region/PhysicsModules/BulletS/BSLinksetConstraints.cs
index c4b4c86..c4b4c86 100644..100755
--- a/OpenSim/Region/PhysicsModules/BulletS/BSLinksetConstraints.cs
+++ b/OpenSim/Region/PhysicsModules/BulletS/BSLinksetConstraints.cs
diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSMaterials.cs b/OpenSim/Region/PhysicsModules/BulletS/BSMaterials.cs
index 0e44d03..0e44d03 100644..100755
--- a/OpenSim/Region/PhysicsModules/BulletS/BSMaterials.cs
+++ b/OpenSim/Region/PhysicsModules/BulletS/BSMaterials.cs
diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSMotors.cs b/OpenSim/Region/PhysicsModules/BulletS/BSMotors.cs
index 2faf2d4..2faf2d4 100644..100755
--- a/OpenSim/Region/PhysicsModules/BulletS/BSMotors.cs
+++ b/OpenSim/Region/PhysicsModules/BulletS/BSMotors.cs
diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSParam.cs b/OpenSim/Region/PhysicsModules/BulletS/BSParam.cs
index c296008..495f752 100644..100755
--- a/OpenSim/Region/PhysicsModules/BulletS/BSParam.cs
+++ b/OpenSim/Region/PhysicsModules/BulletS/BSParam.cs
@@ -90,8 +90,8 @@ public static class BSParam
     public static float DeactivationTime { get; private set; }
     public static float LinearSleepingThreshold { get; private set; }
     public static float AngularSleepingThreshold { get; private set; }
-        public static float CcdMotionThreshold { get; private set; }
-        public static float CcdSweptSphereRadius { get; private set; }
+    public static float CcdMotionThreshold { get; private set; }
+    public static float CcdSweptSphereRadius { get; private set; }
     public static float ContactProcessingThreshold { get; private set; }
 
     public static bool ShouldMeshSculptedPrim { get; private set; }   // cause scuplted prims to get meshed
@@ -119,14 +119,14 @@ public static class BSParam
     public static float Gravity { get; private set; }
 
     // Physics Engine operation
-        public static float MaxPersistantManifoldPoolSize { get; private set; }
-        public static float MaxCollisionAlgorithmPoolSize { get; private set; }
-        public static bool ShouldDisableContactPoolDynamicAllocation { get; private set; }
-        public static bool ShouldForceUpdateAllAabbs { get; private set; }
-        public static bool ShouldRandomizeSolverOrder { get; private set; }
-        public static bool ShouldSplitSimulationIslands { get; private set; }
-        public static bool ShouldEnableFrictionCaching { get; private set; }
-        public static float NumberOfSolverIterations { get; private set; }
+    public static float MaxPersistantManifoldPoolSize { get; private set; }
+    public static float MaxCollisionAlgorithmPoolSize { get; private set; }
+    public static bool ShouldDisableContactPoolDynamicAllocation { get; private set; }
+    public static bool ShouldForceUpdateAllAabbs { get; private set; }
+    public static bool ShouldRandomizeSolverOrder { get; private set; }
+    public static bool ShouldSplitSimulationIslands { get; private set; }
+    public static bool ShouldEnableFrictionCaching { get; private set; }
+    public static float NumberOfSolverIterations { get; private set; }
     public static bool UseSingleSidedMeshes { get; private set; }
     public static float GlobalContactBreakingThreshold { get; private set; }
     public static float PhysicsUnmanLoggingFrames { get; private set; }
@@ -135,6 +135,7 @@ public static class BSParam
     public static bool AvatarToAvatarCollisionsByDefault { get; private set; }
     public static float AvatarFriction { get; private set; }
     public static float AvatarStandingFriction { get; private set; }
+    public static float AvatarWalkVelocityFactor { get; private set; }
     public static float AvatarAlwaysRunFactor { get; private set; }
     public static float AvatarDensity { get; private set; }
     public static float AvatarRestitution { get; private set; }
@@ -142,23 +143,26 @@ public static class BSParam
     public static float AvatarCapsuleWidth { get; private set; }
     public static float AvatarCapsuleDepth { get; private set; }
     public static float AvatarCapsuleHeight { get; private set; }
+    public static bool AvatarUseBefore09SizeComputation { get; private set; }
     public static float AvatarHeightLowFudge { get; private set; }
     public static float AvatarHeightMidFudge { get; private set; }
     public static float AvatarHeightHighFudge { get; private set; }
     public static float AvatarFlyingGroundMargin { get; private set; }
     public static float AvatarFlyingGroundUpForce { get; private set; }
     public static float AvatarTerminalVelocity { get; private set; }
-        public static float AvatarContactProcessingThreshold { get; private set; }
+    public static float AvatarContactProcessingThreshold { get; private set; }
+    public static float AvatarAddForcePushFactor { get; private set; }
     public static float AvatarStopZeroThreshold { get; private set; }
-        public static int AvatarJumpFrames { get; private set; }
-        public static float AvatarBelowGroundUpCorrectionMeters { get; private set; }
-        public static float AvatarStepHeight { get; private set; }
-        public static float AvatarStepAngle { get; private set; }
-        public static float AvatarStepGroundFudge { get; private set; }
-        public static float AvatarStepApproachFactor { get; private set; }
-        public static float AvatarStepForceFactor { get; private set; }
-        public static float AvatarStepUpCorrectionFactor { get; private set; }
-        public static int AvatarStepSmoothingSteps { get; private set; }
+    public static float AvatarStopZeroThresholdSquared { get; private set; }
+    public static int AvatarJumpFrames { get; private set; }
+    public static float AvatarBelowGroundUpCorrectionMeters { get; private set; }
+    public static float AvatarStepHeight { get; private set; }
+    public static float AvatarStepAngle { get; private set; }
+    public static float AvatarStepGroundFudge { get; private set; }
+    public static float AvatarStepApproachFactor { get; private set; }
+    public static float AvatarStepForceFactor { get; private set; }
+    public static float AvatarStepUpCorrectionFactor { get; private set; }
+    public static int AvatarStepSmoothingSteps { get; private set; }
 
     // Vehicle parameters
     public static float VehicleMaxLinearVelocity { get; private set; }
@@ -190,31 +194,31 @@ public static class BSParam
     public static float CSHullVolumeConservationThresholdPercent { get; private set; }
     public static int CSHullMaxVertices { get; private set; }
     public static float CSHullMaxSkinWidth { get; private set; }
-        public static float BHullMaxVerticesPerHull { get; private set; }               // 100
-        public static float BHullMinClusters { get; private set; }                              // 2
-        public static float BHullCompacityWeight { get; private set; }                  // 0.1
-        public static float BHullVolumeWeight { get; private set; }                             // 0.0
-        public static float BHullConcavity { get; private set; }                                    // 100
-        public static bool BHullAddExtraDistPoints { get; private set; }                // false
-        public static bool BHullAddNeighboursDistPoints { get; private set; }   // false
-        public static bool BHullAddFacesPoints { get; private set; }                    // false
-        public static bool BHullShouldAdjustCollisionMargin { get; private set; }       // false
-        public static float WhichHACD { get; private set; }                                 // zero if Bullet HACD, non-zero says VHACD
+    public static float BHullMaxVerticesPerHull { get; private set; }           // 100
+    public static float BHullMinClusters { get; private set; }                          // 2
+    public static float BHullCompacityWeight { get; private set; }                      // 0.1
+    public static float BHullVolumeWeight { get; private set; }                         // 0.0
+    public static float BHullConcavity { get; private set; }                                // 100
+    public static bool BHullAddExtraDistPoints { get; private set; }            // false
+    public static bool BHullAddNeighboursDistPoints { get; private set; }       // false
+    public static bool BHullAddFacesPoints { get; private set; }                        // false
+    public static bool BHullShouldAdjustCollisionMargin { get; private set; }   // false
+    public static float WhichHACD { get; private set; }                             // zero if Bullet HACD, non-zero says VHACD
     // Parameters for VHACD 2.0: http://code.google.com/p/v-hacd
     // To enable, set both ShouldUseBulletHACD=true and WhichHACD=1
-        // http://kmamou.blogspot.ca/2014/12/v-hacd-20-parameters-description.html
-        public static float VHACDresolution { get; private set; }                       // 100,000 max number of voxels generated during voxelization stage
-        public static float VHACDdepth { get; private set; }                            // 20 max number of clipping stages
-        public static float VHACDconcavity { get; private set; }                        // 0.0025 maximum concavity
-        public static float VHACDplaneDownsampling { get; private set; }        // 4 granularity of search for best clipping plane
-        public static float VHACDconvexHullDownsampling { get; private set; }   // 4 precision of hull gen process
-        public static float VHACDalpha { get; private set; }                            // 0.05 bias toward clipping along symmetry planes
-        public static float VHACDbeta { get; private set; }                                 // 0.05 bias toward clipping along revolution axis
-        public static float VHACDgamma { get; private set; }                            // 0.00125 max concavity when merging
-        public static float VHACDpca { get; private set; }                                      // 0 on/off normalizing mesh before decomp
-        public static float VHACDmode { get; private set; }                                 // 0 0:voxel based, 1: tetrahedron based
-        public static float VHACDmaxNumVerticesPerCH { get; private set; }      // 64 max triangles per convex hull
-        public static float VHACDminVolumePerCH { get; private set; }           // 0.0001 sampling of generated convex hulls
+    // http://kmamou.blogspot.ca/2014/12/v-hacd-20-parameters-description.html
+    public static float VHACDresolution { get; private set; }                   // 100,000 max number of voxels generated during voxelization stage
+    public static float VHACDdepth { get; private set; }                                // 20 max number of clipping stages
+    public static float VHACDconcavity { get; private set; }                    // 0.0025 maximum concavity
+    public static float VHACDplaneDownsampling { get; private set; }    // 4 granularity of search for best clipping plane
+    public static float VHACDconvexHullDownsampling { get; private set; }       // 4 precision of hull gen process
+    public static float VHACDalpha { get; private set; }                                // 0.05 bias toward clipping along symmetry planes
+    public static float VHACDbeta { get; private set; }                             // 0.05 bias toward clipping along revolution axis
+    public static float VHACDgamma { get; private set; }                                // 0.00125 max concavity when merging
+    public static float VHACDpca { get; private set; }                                  // 0 on/off normalizing mesh before decomp
+    public static float VHACDmode { get; private set; }                             // 0 0:voxel based, 1: tetrahedron based
+    public static float VHACDmaxNumVerticesPerCH { get; private set; }  // 64 max triangles per convex hull
+    public static float VHACDminVolumePerCH { get; private set; }               // 0.0001 sampling of generated convex hulls
 
     // Linkset implementation parameters
     public static float LinksetImplementation { get; private set; }
@@ -227,9 +231,13 @@ public static class BSParam
     public static float LinkConstraintCFM { get; private set; }
     public static float LinkConstraintSolverIterations { get; private set; }
 
+    public static bool UseBulletRaycast { get; private set; }
+
     public static float PID_D { get; private set; }    // derivative
     public static float PID_P { get; private set; }    // proportional
 
+    public static float DebugNumber { get; private set; }   // A console setable number used for debugging
+
     // Various constants that come from that other virtual world that shall not be named.
     public const float MinGravityZ = -1f;
     public const float MaxGravityZ = 28f;
@@ -574,7 +582,7 @@ public static class BSParam
             (s,v) => { ContactProcessingThreshold = v;},
             (s,o) => { s.PE.SetContactProcessingThreshold(o.PhysBody, ContactProcessingThreshold); } ),
 
-            new ParameterDefn<float>("TerrainImplementation", "Type of shape to use for terrain (0=heightmap, 1=mesh)",
+        new ParameterDefn<float>("TerrainImplementation", "Type of shape to use for terrain (0=heightmap, 1=mesh)",
             (float)BSTerrainPhys.TerrainImplementation.Heightmap ),
         new ParameterDefn<int>("TerrainMeshMagnification", "Number of times the 256x256 heightmap is multiplied to create the terrain mesh" ,
             2 ),
@@ -597,11 +605,13 @@ public static class BSParam
             0.2f ),
         new ParameterDefn<float>("AvatarStandingFriction", "Avatar friction when standing. Changed on avatar recreation.",
             0.95f ),
+        new ParameterDefn<float>("AvatarWalkVelocityFactor", "Speed multiplier if avatar is walking",
+            1.0f ),
         new ParameterDefn<float>("AvatarAlwaysRunFactor", "Speed multiplier if avatar is set to always run",
             1.3f ),
             // For historical reasons, density is reported  * 100
         new ParameterDefn<float>("AvatarDensity", "Density of an avatar. Changed on avatar recreation. Scaled times 100.",
-            3500f) ,    // 3.5 * 100
+            350f) ,    // 3.5 * 100
         new ParameterDefn<float>("AvatarRestitution", "Bouncyness. Changed on avatar recreation.",
             0f ),
         new ParameterDefn<int>("AvatarShape", "Code for avatar physical shape: 0:capsule, 1:cube, 2:ovoid, 2:mesh",
@@ -612,6 +622,8 @@ public static class BSParam
             0.45f ),
         new ParameterDefn<float>("AvatarCapsuleHeight", "Default height of space around avatar",
             1.5f ),
+        new ParameterDefn<bool>("AvatarUseBefore09SizeComputation", "Use the old fudge method of computing avatar capsule size",
+            true ),
         new ParameterDefn<float>("AvatarHeightLowFudge", "A fudge factor to make small avatars stand on the ground",
             0f ),
         new ParameterDefn<float>("AvatarHeightMidFudge", "A fudge distance to adjust average sized avatars to be standing on ground",
@@ -624,27 +636,31 @@ public static class BSParam
             2.0f ),
         new ParameterDefn<float>("AvatarTerminalVelocity", "Terminal Velocity of falling avatar",
             -54.0f ),
-            new ParameterDefn<float>("AvatarContactProcessingThreshold", "Distance from capsule to check for collisions",
-            0.1f ),
-            new ParameterDefn<float>("AvatarStopZeroThreshold", "Movement velocity below which avatar is assumed to be stopped",
+        new ParameterDefn<float>("AvatarContactProcessingThreshold", "Distance from capsule to check for collisions",
             0.1f ),
-            new ParameterDefn<float>("AvatarBelowGroundUpCorrectionMeters", "Meters to move avatar up if it seems to be below ground",
+        new ParameterDefn<float>("AvatarAddForcePushFactor", "BSCharacter.AddForce is multiplied by this and mass to be like other physics engines",
+            0.315f ),
+        new ParameterDefn<float>("AvatarStopZeroThreshold", "Movement velocity below which avatar is assumed to be stopped",
+            0.45f,
+            (s) => { return (float)AvatarStopZeroThreshold; },
+            (s,v) => { AvatarStopZeroThreshold = v; AvatarStopZeroThresholdSquared = v * v; } ),
+        new ParameterDefn<float>("AvatarBelowGroundUpCorrectionMeters", "Meters to move avatar up if it seems to be below ground",
             1.0f ),
-            new ParameterDefn<int>("AvatarJumpFrames", "Number of frames to allow jump forces. Changes jump height.",
+        new ParameterDefn<int>("AvatarJumpFrames", "Number of frames to allow jump forces. Changes jump height.",
             4 ),
-            new ParameterDefn<float>("AvatarStepHeight", "Height of a step obstacle to consider step correction",
+        new ParameterDefn<float>("AvatarStepHeight", "Height of a step obstacle to consider step correction",
             0.999f ) ,
-            new ParameterDefn<float>("AvatarStepAngle", "The angle (in radians) for a vertical surface to be considered a step",
+        new ParameterDefn<float>("AvatarStepAngle", "The angle (in radians) for a vertical surface to be considered a step",
             0.3f ) ,
-            new ParameterDefn<float>("AvatarStepGroundFudge", "Fudge factor subtracted from avatar base when comparing collision height",
+        new ParameterDefn<float>("AvatarStepGroundFudge", "Fudge factor subtracted from avatar base when comparing collision height",
             0.1f ) ,
-            new ParameterDefn<float>("AvatarStepApproachFactor", "Factor to control angle of approach to step (0=straight on)",
+        new ParameterDefn<float>("AvatarStepApproachFactor", "Factor to control angle of approach to step (0=straight on)",
             2f ),
-            new ParameterDefn<float>("AvatarStepForceFactor", "Controls the amount of force up applied to step up onto a step",
+        new ParameterDefn<float>("AvatarStepForceFactor", "Controls the amount of force up applied to step up onto a step",
             0f ),
-            new ParameterDefn<float>("AvatarStepUpCorrectionFactor", "Multiplied by height of step collision to create up movement at step",
+        new ParameterDefn<float>("AvatarStepUpCorrectionFactor", "Multiplied by height of step collision to create up movement at step",
             0.8f ),
-            new ParameterDefn<int>("AvatarStepSmoothingSteps", "Number of frames after a step collision that we continue walking up stairs",
+        new ParameterDefn<int>("AvatarStepSmoothingSteps", "Number of frames after a step collision that we continue walking up stairs",
             1 ),
 
         new ParameterDefn<float>("VehicleMaxLinearVelocity", "Maximum velocity magnitude that can be assigned to a vehicle",
@@ -688,130 +704,136 @@ public static class BSParam
         new ParameterDefn<bool>("VehicleEnableAngularBanking", "Turn on/off vehicle angular banking effect",
             true ),
 
-            new ParameterDefn<float>("MaxPersistantManifoldPoolSize", "Number of manifolds pooled (0 means default of 4096)",
+        new ParameterDefn<float>("MaxPersistantManifoldPoolSize", "Number of manifolds pooled (0 means default of 4096)",
             0f,
             (s) => { return MaxPersistantManifoldPoolSize; },
             (s,v) => { MaxPersistantManifoldPoolSize = v; s.UnmanagedParams[0].maxPersistantManifoldPoolSize = v; } ),
-            new ParameterDefn<float>("MaxCollisionAlgorithmPoolSize", "Number of collisions pooled (0 means default of 4096)",
+        new ParameterDefn<float>("MaxCollisionAlgorithmPoolSize", "Number of collisions pooled (0 means default of 4096)",
             0f,
             (s) => { return MaxCollisionAlgorithmPoolSize; },
             (s,v) => { MaxCollisionAlgorithmPoolSize = v; s.UnmanagedParams[0].maxCollisionAlgorithmPoolSize = v; } ),
-            new ParameterDefn<bool>("ShouldDisableContactPoolDynamicAllocation", "Enable to allow large changes in object count",
+        new ParameterDefn<bool>("ShouldDisableContactPoolDynamicAllocation", "Enable to allow large changes in object count",
             false,
             (s) => { return ShouldDisableContactPoolDynamicAllocation; },
             (s,v) => { ShouldDisableContactPoolDynamicAllocation = v;
                         s.UnmanagedParams[0].shouldDisableContactPoolDynamicAllocation = NumericBool(v); } ),
-            new ParameterDefn<bool>("ShouldForceUpdateAllAabbs", "Enable to recomputer AABBs every simulator step",
+        new ParameterDefn<bool>("ShouldForceUpdateAllAabbs", "Enable to recomputer AABBs every simulator step",
             false,
             (s) => { return ShouldForceUpdateAllAabbs; },
             (s,v) => { ShouldForceUpdateAllAabbs = v; s.UnmanagedParams[0].shouldForceUpdateAllAabbs = NumericBool(v); } ),
-            new ParameterDefn<bool>("ShouldRandomizeSolverOrder", "Enable for slightly better stacking interaction",
+        new ParameterDefn<bool>("ShouldRandomizeSolverOrder", "Enable for slightly better stacking interaction",
             true,
             (s) => { return ShouldRandomizeSolverOrder; },
             (s,v) => { ShouldRandomizeSolverOrder = v; s.UnmanagedParams[0].shouldRandomizeSolverOrder = NumericBool(v); } ),
-            new ParameterDefn<bool>("ShouldSplitSimulationIslands", "Enable splitting active object scanning islands",
+        new ParameterDefn<bool>("ShouldSplitSimulationIslands", "Enable splitting active object scanning islands",
             true,
             (s) => { return ShouldSplitSimulationIslands; },
             (s,v) => { ShouldSplitSimulationIslands = v; s.UnmanagedParams[0].shouldSplitSimulationIslands = NumericBool(v); } ),
-            new ParameterDefn<bool>("ShouldEnableFrictionCaching", "Enable friction computation caching",
+        new ParameterDefn<bool>("ShouldEnableFrictionCaching", "Enable friction computation caching",
             true,
             (s) => { return ShouldEnableFrictionCaching; },
             (s,v) => { ShouldEnableFrictionCaching = v; s.UnmanagedParams[0].shouldEnableFrictionCaching = NumericBool(v); } ),
-            new ParameterDefn<float>("NumberOfSolverIterations", "Number of internal iterations (0 means default)",
+        new ParameterDefn<float>("NumberOfSolverIterations", "Number of internal iterations (0 means default)",
             0f,     // zero says use Bullet default
             (s) => { return NumberOfSolverIterations; },
             (s,v) => { NumberOfSolverIterations = v; s.UnmanagedParams[0].numberOfSolverIterations = v; } ),
-            new ParameterDefn<bool>("UseSingleSidedMeshes", "Whether to compute collisions based on single sided meshes.",
+        new ParameterDefn<bool>("UseSingleSidedMeshes", "Whether to compute collisions based on single sided meshes.",
             true,
             (s) => { return UseSingleSidedMeshes; },
             (s,v) => { UseSingleSidedMeshes = v; s.UnmanagedParams[0].useSingleSidedMeshes = NumericBool(v); } ),
-            new ParameterDefn<float>("GlobalContactBreakingThreshold", "Amount of shape radius before breaking a collision contact (0 says Bullet default (0.2))",
+        new ParameterDefn<float>("GlobalContactBreakingThreshold", "Amount of shape radius before breaking a collision contact (0 says Bullet default (0.2))",
             0f,
             (s) => { return GlobalContactBreakingThreshold; },
             (s,v) => { GlobalContactBreakingThreshold = v; s.UnmanagedParams[0].globalContactBreakingThreshold = v; } ),
-            new ParameterDefn<float>("PhysicsUnmanLoggingFrames", "If non-zero, frames between output of detailed unmanaged physics statistics",
+        new ParameterDefn<float>("PhysicsUnmanLoggingFrames", "If non-zero, frames between output of detailed unmanaged physics statistics",
             0f,
             (s) => { return PhysicsUnmanLoggingFrames; },
             (s,v) => { PhysicsUnmanLoggingFrames = v; s.UnmanagedParams[0].physicsLoggingFrames = v; } ),
 
-            new ParameterDefn<int>("CSHullMaxDepthSplit", "CS impl: max depth to split for hull. 1-10 but > 7 is iffy",
+        new ParameterDefn<int>("CSHullMaxDepthSplit", "CS impl: max depth to split for hull. 1-10 but > 7 is iffy",
             7 ),
-            new ParameterDefn<int>("CSHullMaxDepthSplitForSimpleShapes", "CS impl: max depth setting for simple prim shapes",
+        new ParameterDefn<int>("CSHullMaxDepthSplitForSimpleShapes", "CS impl: max depth setting for simple prim shapes",
             2 ),
-            new ParameterDefn<float>("CSHullConcavityThresholdPercent", "CS impl: concavity threshold percent (0-20)",
+        new ParameterDefn<float>("CSHullConcavityThresholdPercent", "CS impl: concavity threshold percent (0-20)",
             5f ),
-            new ParameterDefn<float>("CSHullVolumeConservationThresholdPercent", "percent volume conservation to collapse hulls (0-30)",
+        new ParameterDefn<float>("CSHullVolumeConservationThresholdPercent", "percent volume conservation to collapse hulls (0-30)",
             5f ),
-            new ParameterDefn<int>("CSHullMaxVertices", "CS impl: maximum number of vertices in output hulls. Keep < 50.",
+        new ParameterDefn<int>("CSHullMaxVertices", "CS impl: maximum number of vertices in output hulls. Keep < 50.",
             32 ),
-            new ParameterDefn<float>("CSHullMaxSkinWidth", "CS impl: skin width to apply to output hulls.",
+        new ParameterDefn<float>("CSHullMaxSkinWidth", "CS impl: skin width to apply to output hulls.",
             0f ),
 
-            new ParameterDefn<float>("BHullMaxVerticesPerHull", "Bullet impl: max number of vertices per created hull",
+        new ParameterDefn<float>("BHullMaxVerticesPerHull", "Bullet impl: max number of vertices per created hull",
             200f ),
-            new ParameterDefn<float>("BHullMinClusters", "Bullet impl: minimum number of hulls to create per mesh",
+        new ParameterDefn<float>("BHullMinClusters", "Bullet impl: minimum number of hulls to create per mesh",
             10f ),
-            new ParameterDefn<float>("BHullCompacityWeight", "Bullet impl: weight factor for how compact to make hulls",
+        new ParameterDefn<float>("BHullCompacityWeight", "Bullet impl: weight factor for how compact to make hulls",
             20f ),
-            new ParameterDefn<float>("BHullVolumeWeight", "Bullet impl: weight factor for volume in created hull",
+        new ParameterDefn<float>("BHullVolumeWeight", "Bullet impl: weight factor for volume in created hull",
             0.1f ),
-            new ParameterDefn<float>("BHullConcavity", "Bullet impl: weight factor for how convex a created hull can be",
+        new ParameterDefn<float>("BHullConcavity", "Bullet impl: weight factor for how convex a created hull can be",
             10f ),
-            new ParameterDefn<bool>("BHullAddExtraDistPoints", "Bullet impl: whether to add extra vertices for long distance vectors",
+        new ParameterDefn<bool>("BHullAddExtraDistPoints", "Bullet impl: whether to add extra vertices for long distance vectors",
             true ),
-            new ParameterDefn<bool>("BHullAddNeighboursDistPoints", "Bullet impl: whether to add extra vertices between neighbor hulls",
+        new ParameterDefn<bool>("BHullAddNeighboursDistPoints", "Bullet impl: whether to add extra vertices between neighbor hulls",
             true ),
-            new ParameterDefn<bool>("BHullAddFacesPoints", "Bullet impl: whether to add extra vertices to break up hull faces",
+        new ParameterDefn<bool>("BHullAddFacesPoints", "Bullet impl: whether to add extra vertices to break up hull faces",
             true ),
-            new ParameterDefn<bool>("BHullShouldAdjustCollisionMargin", "Bullet impl: whether to shrink resulting hulls to account for collision margin",
+        new ParameterDefn<bool>("BHullShouldAdjustCollisionMargin", "Bullet impl: whether to shrink resulting hulls to account for collision margin",
             false ),
 
-            new ParameterDefn<float>("WhichHACD", "zero if Bullet HACD, non-zero says VHACD",
+        new ParameterDefn<float>("WhichHACD", "zero if Bullet HACD, non-zero says VHACD",
             0f ),
-            new ParameterDefn<float>("VHACDresolution", "max number of voxels generated during voxelization stage",
+        new ParameterDefn<float>("VHACDresolution", "max number of voxels generated during voxelization stage",
             100000f ),
-            new ParameterDefn<float>("VHACDdepth", "max number of clipping stages",
+        new ParameterDefn<float>("VHACDdepth", "max number of clipping stages",
             20f ),
-            new ParameterDefn<float>("VHACDconcavity", "maximum concavity",
+        new ParameterDefn<float>("VHACDconcavity", "maximum concavity",
             0.0025f ),
-            new ParameterDefn<float>("VHACDplaneDownsampling", "granularity of search for best clipping plane",
+        new ParameterDefn<float>("VHACDplaneDownsampling", "granularity of search for best clipping plane",
             4f ),
-            new ParameterDefn<float>("VHACDconvexHullDownsampling", "precision of hull gen process",
+        new ParameterDefn<float>("VHACDconvexHullDownsampling", "precision of hull gen process",
             4f ),
-            new ParameterDefn<float>("VHACDalpha", "bias toward clipping along symmetry planes",
+        new ParameterDefn<float>("VHACDalpha", "bias toward clipping along symmetry planes",
             0.05f ),
-            new ParameterDefn<float>("VHACDbeta", "bias toward clipping along revolution axis",
+        new ParameterDefn<float>("VHACDbeta", "bias toward clipping along revolution axis",
             0.05f ),
-            new ParameterDefn<float>("VHACDgamma", "max concavity when merging",
+        new ParameterDefn<float>("VHACDgamma", "max concavity when merging",
             0.00125f ),
-            new ParameterDefn<float>("VHACDpca", "on/off normalizing mesh before decomp",
+        new ParameterDefn<float>("VHACDpca", "on/off normalizing mesh before decomp",
             0f ),
-            new ParameterDefn<float>("VHACDmode", "0:voxel based, 1: tetrahedron based",
+        new ParameterDefn<float>("VHACDmode", "0:voxel based, 1: tetrahedron based",
             0f ),
-            new ParameterDefn<float>("VHACDmaxNumVerticesPerCH", "max triangles per convex hull",
+        new ParameterDefn<float>("VHACDmaxNumVerticesPerCH", "max triangles per convex hull",
             64f ),
-            new ParameterDefn<float>("VHACDminVolumePerCH",     "sampling of generated convex hulls",
+        new ParameterDefn<float>("VHACDminVolumePerCH", "sampling of generated convex hulls",
             0.0001f ),
 
-            new ParameterDefn<float>("LinksetImplementation", "Type of linkset implementation (0=Constraint, 1=Compound, 2=Manual)",
+        new ParameterDefn<float>("LinksetImplementation", "Type of linkset implementation (0=Constraint, 1=Compound, 2=Manual)",
             (float)BSLinkset.LinksetImplementation.Compound ),
-            new ParameterDefn<bool>("LinksetOffsetCenterOfMass", "If 'true', compute linkset center-of-mass and offset linkset position to account for same",
+        new ParameterDefn<bool>("LinksetOffsetCenterOfMass", "If 'true', compute linkset center-of-mass and offset linkset position to account for same",
             true ),
-            new ParameterDefn<bool>("LinkConstraintUseFrameOffset", "For linksets built with constraints, enable frame offsetFor linksets built with constraints, enable frame offset.",
+        new ParameterDefn<bool>("LinkConstraintUseFrameOffset", "For linksets built with constraints, enable frame offsetFor linksets built with constraints, enable frame offset.",
             false ),
-            new ParameterDefn<bool>("LinkConstraintEnableTransMotor", "Whether to enable translational motor on linkset constraints",
+        new ParameterDefn<bool>("LinkConstraintEnableTransMotor", "Whether to enable translational motor on linkset constraints",
             true ),
-            new ParameterDefn<float>("LinkConstraintTransMotorMaxVel", "Maximum velocity to be applied by translational motor in linkset constraints",
+        new ParameterDefn<float>("LinkConstraintTransMotorMaxVel", "Maximum velocity to be applied by translational motor in linkset constraints",
             5.0f ),
-            new ParameterDefn<float>("LinkConstraintTransMotorMaxForce", "Maximum force to be applied by translational motor in linkset constraints",
+        new ParameterDefn<float>("LinkConstraintTransMotorMaxForce", "Maximum force to be applied by translational motor in linkset constraints",
             0.1f ),
-            new ParameterDefn<float>("LinkConstraintCFM", "Amount constraint can be violated. 0=no violation, 1=infinite. Default=0.1",
+        new ParameterDefn<float>("LinkConstraintCFM", "Amount constraint can be violated. 0=no violation, 1=infinite. Default=0.1",
             0.1f ),
-            new ParameterDefn<float>("LinkConstraintERP", "Amount constraint is corrected each tick. 0=none, 1=all. Default = 0.2",
+        new ParameterDefn<float>("LinkConstraintERP", "Amount constraint is corrected each tick. 0=none, 1=all. Default = 0.2",
             0.1f ),
-            new ParameterDefn<float>("LinkConstraintSolverIterations", "Number of solver iterations when computing constraint. (0 = Bullet default)",
+        new ParameterDefn<float>("LinkConstraintSolverIterations", "Number of solver iterations when computing constraint. (0 = Bullet default)",
             40 ),
 
+        new ParameterDefn<bool>("UseBulletRaycast", "If 'true', use the raycast function of the Bullet physics engine",
+            true ),
+
+        new ParameterDefn<float>("DebugNumber", "A console setable number sometimes used for debugging",
+            1.0f ),
+
         new ParameterDefn<int>("PhysicsMetricFrames", "Frames between outputting detailed phys metrics. (0 is off)",
             0,
             (s) => { return s.PhysicsMetricDumpFrames; },
@@ -819,7 +841,7 @@ public static class BSParam
         new ParameterDefn<float>("ResetBroadphasePool", "Setting this is any value resets the broadphase collision pool",
             0f,
             (s) => { return 0f; },
-            (s,v) => { BSParam.ResetBroadphasePoolTainted(s, v, false /* inTaintTime */); } ),
+            (s,v) => { BSParam.ResetBroadphasePoolTainted(s, v); } ),
         new ParameterDefn<float>("ResetConstraintSolver", "Setting this is any value resets the constraint solver",
             0f,
             (s) => { return 0f; },
@@ -905,10 +927,10 @@ public static class BSParam
     // =====================================================================
     // There are parameters that, when set, cause things to happen in the physics engine.
     // This causes the broadphase collision cache to be cleared.
-    private static void ResetBroadphasePoolTainted(BSScene pPhysScene, float v, bool inTaintTime)
+    private static void ResetBroadphasePoolTainted(BSScene pPhysScene, float v)
     {
         BSScene physScene = pPhysScene;
-        physScene.TaintedObject(inTaintTime, "BSParam.ResetBroadphasePoolTainted", delegate()
+        physScene.TaintedObject(BSScene.DetailLogZero, "BSParam.ResetBroadphasePoolTainted", delegate()
         {
             physScene.PE.ResetBroadphasePool(physScene.World);
         });
diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSPhysObject.cs b/OpenSim/Region/PhysicsModules/BulletS/BSPhysObject.cs
index da3fc18..6aa24d5 100644..100755
--- a/OpenSim/Region/PhysicsModules/BulletS/BSPhysObject.cs
+++ b/OpenSim/Region/PhysicsModules/BulletS/BSPhysObject.cs
@@ -230,34 +230,97 @@ public abstract class BSPhysObject : PhysicsActor
     // Update the physical location and motion of the object. Called with data from Bullet.
     public abstract void UpdateProperties(EntityProperties entprop);
 
+    // The position value as known by BulletSim. Does not effect the physics engine.
     public virtual OMV.Vector3 RawPosition { get; set; }
+    // Set position in BulletSim and the physics engined to a value immediately. Must be called at taint time.
     public abstract OMV.Vector3 ForcePosition { get; set; }
 
+    // The orientation value as known by BulletSim. Does not effect the physics engine.
     public virtual OMV.Quaternion RawOrientation { get; set; }
+    // Set orientation in BulletSim and the physics engine to a value immediately. Must be called at taint time.
     public abstract OMV.Quaternion ForceOrientation { get; set; }
 
+    // The velocity value as known by BulletSim. Does not effect the physics engine.
     public virtual OMV.Vector3 RawVelocity { get; set; }
+    // Set velocity in BulletSim and the physics engined to a value immediately. Must be called at taint time.
     public abstract OMV.Vector3 ForceVelocity { get; set; }
 
+    // The rotational velocity value as known by BulletSim. Does not effect the physics engine.
+    public OMV.Vector3 RawRotationalVelocity { get; set; }
+
+    // RawForce is a constant force applied to object (see Force { set; } )
     public OMV.Vector3 RawForce { get; set; }
     public OMV.Vector3 RawTorque { get; set; }
+
     public override void AddAngularForce(OMV.Vector3 force, bool pushforce)
     {
-        AddAngularForce(force, pushforce, false);
+        AddAngularForce(false, force);
     }
-    public abstract void AddAngularForce(OMV.Vector3 force, bool pushforce, bool inTaintTime);
-    public abstract void AddForce(OMV.Vector3 force, bool pushforce, bool inTaintTime);
+    public abstract void AddAngularForce(bool inTaintTime, OMV.Vector3 force);
+    public abstract void AddForce(bool inTaintTime, OMV.Vector3 force);
 
-    public abstract OMV.Vector3 ForceRotationalVelocity { get; set; }
+    // PhysicsActor.Velocity
+    public override OMV.Vector3 Velocity
+    {
+        get { return RawVelocity;  }
+        set
+        {
+            // This sets the velocity now. BSCharacter will override to clear target velocity
+            //    before calling this.
+            RawVelocity = value;
+            PhysScene.TaintedObject(LocalID, TypeName + ".SetVelocity", delegate () {
+                // DetailLog("{0},BSPhysObject.Velocity.set,vel={1}", LocalID, RawVelocity);
+                ForceVelocity = RawVelocity;
+            });
+        }
+    }
+
+    // PhysicsActor.SetMomentum
+    // All the physics engines use this as a way of forcing the velocity to something.
+    // BSCharacter overrides this so it can set the target velocity to zero before calling this.
+    public override void SetMomentum(OMV.Vector3 momentum)
+    {
+        this.Velocity = momentum;
+    }
+
+    public override OMV.Vector3 RotationalVelocity {
+        get {
+            return RawRotationalVelocity;
+        }
+        set {
+            RawRotationalVelocity = value;
+            Util.ClampV(RawRotationalVelocity, BSParam.MaxAngularVelocity);
+            // m_log.DebugFormat("{0}: RotationalVelocity={1}", LogHeader, _rotationalVelocity);
+            PhysScene.TaintedObject(LocalID, TypeName + ".setRotationalVelocity", delegate()
+            {
+                ForceRotationalVelocity = RawRotationalVelocity;
+            });
+        }
+    }
+    public OMV.Vector3 ForceRotationalVelocity {
+        get {
+            return RawRotationalVelocity;
+        }
+        set {
+            RawRotationalVelocity = Util.ClampV(value, BSParam.MaxAngularVelocity);
+            if (PhysBody.HasPhysicalBody)
+            {
+                DetailLog("{0},{1}.ForceRotationalVel,taint,rotvel={2}", LocalID, TypeName, RawRotationalVelocity);
+                PhysScene.PE.SetAngularVelocity(PhysBody, RawRotationalVelocity);
+                // PhysicsScene.PE.SetInterpolationAngularVelocity(PhysBody, _rotationalVelocity);
+                ActivateIfPhysical(false);
+            }
+        }
+    }
 
     public abstract float ForceBuoyancy { get; set; }
 
     public virtual bool ForceBodyShapeRebuild(bool inTaintTime) { return false; }
 
-    public override bool PIDActive 
+    public override bool PIDActive
     {
         get { return MoveToTargetActive; }
-        set { MoveToTargetActive = value; } 
+        set { MoveToTargetActive = value; }
     }
 
     public override OMV.Vector3 PIDTarget { set { MoveToTargetTarget = value; } }
@@ -268,7 +331,7 @@ public abstract class BSPhysObject : PhysicsActor
     public float MoveToTargetTau { get; set; }
 
     // Used for llSetHoverHeight and maybe vehicle height. Hover Height will override MoveTo target's Z
-    public override bool PIDHoverActive { set { HoverActive = value; } }
+    public override bool PIDHoverActive {get {return HoverActive;}  set { HoverActive = value; } }
     public override float PIDHoverHeight { set { HoverHeight = value; } }
     public override PIDHoverType PIDHoverType { set { HoverType = value; } }
     public override float PIDHoverTau { set { HoverTau = value; } }
@@ -452,18 +515,24 @@ public abstract class BSPhysObject : PhysicsActor
     private long CollisionsLastTickStep = -1;
 
     // The simulation step is telling this object about a collision.
+    // I'm the 'collider', the thing I'm colliding with is the 'collidee'.
     // Return 'true' if a collision was processed and should be sent up.
     // Return 'false' if this object is not enabled/subscribed/appropriate for or has already seen this collision.
     // Called at taint time from within the Step() function
-    public delegate bool CollideCall(uint collidingWith, BSPhysObject collidee, OMV.Vector3 contactPoint, OMV.Vector3 contactNormal, float pentrationDepth);
-    public virtual bool Collide(uint collidingWith, BSPhysObject collidee,
-                    OMV.Vector3 contactPoint, OMV.Vector3 contactNormal, float pentrationDepth)
+    public virtual bool Collide(BSPhysObject collidee, OMV.Vector3 contactPoint, OMV.Vector3 contactNormal, float pentrationDepth)
     {
         bool ret = false;
 
+        // if 'collidee' is null, that means it is terrain
+        uint collideeLocalID = (collidee == null) ? BSScene.TERRAIN_ID : collidee.LocalID;
+        // All terrain goes by the TERRAIN_ID id when passed up as a collision
+        if (collideeLocalID <= PhysScene.TerrainManager.HighestTerrainID) {
+            collideeLocalID = BSScene.TERRAIN_ID;
+        }
+
         // The following lines make IsColliding(), CollidingGround() and CollidingObj work
         CollidingStep = PhysScene.SimulationStep;
-        if (collidingWith <= PhysScene.TerrainManager.HighestTerrainID)
+        if (collideeLocalID == BSScene.TERRAIN_ID)
         {
             CollidingGroundStep = PhysScene.SimulationStep;
         }
@@ -474,10 +543,13 @@ public abstract class BSPhysObject : PhysicsActor
 
         CollisionAccumulation++;
 
-        // For movement tests, remember if we are colliding with an object that is moving.
-        ColliderIsMoving = collidee != null ? (collidee.RawVelocity != OMV.Vector3.Zero) : false;
+        // For movement tests, if the collider is me, remember if we are colliding with an object that is moving.
+        // Here the 'collider'/'collidee' thing gets messed up. In the larger context, when something is checking
+        //     if the thing it is colliding with is moving, for instance, it asks if the its collider is moving.
+        ColliderIsMoving = collidee != null ? (collidee.RawVelocity != OMV.Vector3.Zero || collidee.RotationalVelocity != OMV.Vector3.Zero) : false;
         ColliderIsVolumeDetect = collidee != null ? (collidee.IsVolumeDetect) : false;
 
+
         // Make a collection of the collisions that happened the last simulation tick.
         // This is different than the collection created for sending up to the simulator as it is cleared every tick.
         if (CollisionsLastTickStep != PhysScene.SimulationStep)
@@ -485,16 +557,29 @@ public abstract class BSPhysObject : PhysicsActor
             CollisionsLastTick = new CollisionEventUpdate();
             CollisionsLastTickStep = PhysScene.SimulationStep;
         }
-        CollisionsLastTick.AddCollider(collidingWith, new ContactPoint(contactPoint, contactNormal, pentrationDepth));
+        CollisionsLastTick.AddCollider(collideeLocalID, new ContactPoint(contactPoint, contactNormal, pentrationDepth));
 
         // If someone has subscribed for collision events log the collision so it will be reported up
         if (SubscribedEvents()) {
+            ContactPoint newContact = new ContactPoint(contactPoint, contactNormal, pentrationDepth);
+
+            // Collision sound requires a velocity to know it should happen. This is a lot of computation for a little used feature.
+            OMV.Vector3 relvel = OMV.Vector3.Zero;
+            if (IsPhysical)
+                relvel = RawVelocity;
+            if (collidee != null && collidee.IsPhysical)
+                relvel -= collidee.RawVelocity;
+            newContact.RelativeSpeed = -OMV.Vector3.Dot(relvel, contactNormal);
+            // DetailLog("{0},{1}.Collision.AddCollider,vel={2},contee.vel={3},relvel={4},relspeed={5}",
+            //         LocalID, TypeName, RawVelocity, (collidee == null ? OMV.Vector3.Zero : collidee.RawVelocity), relvel, newContact.RelativeSpeed);
+
             lock (PhysScene.CollisionLock)
             {
-                CollisionCollection.AddCollider(collidingWith, new ContactPoint(contactPoint, contactNormal, pentrationDepth));
+                CollisionCollection.AddCollider(collideeLocalID, newContact);
             }
-            DetailLog("{0},{1}.Collision.AddCollider,call,with={2},point={3},normal={4},depth={5},colliderMoving={6}",
-                            LocalID, TypeName, collidingWith, contactPoint, contactNormal, pentrationDepth, ColliderIsMoving);
+            DetailLog("{0},{1}.Collision.AddCollider,call,with={2},point={3},normal={4},depth={5},speed={6},colliderMoving={7}",
+                            LocalID, TypeName, collideeLocalID, contactPoint, contactNormal, pentrationDepth,
+                            newContact.RelativeSpeed, ColliderIsMoving);
 
             ret = true;
         }
@@ -555,7 +640,11 @@ public abstract class BSPhysObject : PhysicsActor
             PhysScene.TaintedObject(LocalID, TypeName+".SubscribeEvents", delegate()
             {
                 if (PhysBody.HasPhysicalBody)
+                {
                     CurrentCollisionFlags = PhysScene.PE.AddToCollisionFlags(PhysBody, CollisionFlags.BS_SUBSCRIBE_COLLISION_EVENTS);
+                    DetailLog("{0},{1}.SubscribeEvents,setting collision. ms={2}, collisionFlags={3:x}",
+                            LocalID, TypeName, SubscribedEventsMs, CurrentCollisionFlags);
+                }
             });
         }
         else
diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSPrim.cs b/OpenSim/Region/PhysicsModules/BulletS/BSPrim.cs
index 6f27ac7..f085d70 100644
--- a/OpenSim/Region/PhysicsModules/BulletS/BSPrim.cs
+++ b/OpenSim/Region/PhysicsModules/BulletS/BSPrim.cs
@@ -33,7 +33,7 @@ using log4net;
 using OMV = OpenMetaverse;
 using OpenSim.Framework;
 using OpenSim.Region.PhysicsModules.SharedBase;
-using OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet;
+using OpenSim.Region.PhysicsModules.ConvexDecompositionDotNet;
 
 namespace OpenSim.Region.PhysicsModule.BulletS
 {
@@ -59,7 +59,6 @@ public class BSPrim : BSPhysObject
     private bool _setAlwaysRun;
     private bool _throttleUpdates;
     private bool _floatOnWater;
-    private OMV.Vector3 _rotationalVelocity;
     private bool _kinematic;
     private float _buoyancy;
 
@@ -90,7 +89,7 @@ public class BSPrim : BSPhysObject
         RawOrientation = rotation;
         _buoyancy = 0f;
         RawVelocity = OMV.Vector3.Zero;
-        _rotationalVelocity = OMV.Vector3.Zero;
+        RawRotationalVelocity = OMV.Vector3.Zero;
         BaseShape = pbs;
         _isPhysical = pisPhysical;
         _isVolumeDetect = false;
@@ -256,7 +255,7 @@ public class BSPrim : BSPhysObject
     {
         RawVelocity = OMV.Vector3.Zero;
         _acceleration = OMV.Vector3.Zero;
-        _rotationalVelocity = OMV.Vector3.Zero;
+        RawRotationalVelocity = OMV.Vector3.Zero;
 
         // Zero some other properties in the physics engine
         PhysScene.TaintedObject(inTaintTime, LocalID, "BSPrim.ZeroMotion", delegate()
@@ -267,33 +266,33 @@ public class BSPrim : BSPhysObject
     }
     public override void ZeroAngularMotion(bool inTaintTime)
     {
-        _rotationalVelocity = OMV.Vector3.Zero;
+        RawRotationalVelocity = OMV.Vector3.Zero;
         // Zero some other properties in the physics engine
         PhysScene.TaintedObject(inTaintTime, LocalID, "BSPrim.ZeroMotion", delegate()
         {
             // DetailLog("{0},BSPrim.ZeroAngularMotion,call,rotVel={1}", LocalID, _rotationalVelocity);
             if (PhysBody.HasPhysicalBody)
             {
-                PhysScene.PE.SetInterpolationAngularVelocity(PhysBody, _rotationalVelocity);
-                PhysScene.PE.SetAngularVelocity(PhysBody, _rotationalVelocity);
+                PhysScene.PE.SetInterpolationAngularVelocity(PhysBody, RawRotationalVelocity);
+                PhysScene.PE.SetAngularVelocity(PhysBody, RawRotationalVelocity);
             }
         });
     }
 
-    public override void LockAngularMotion(OMV.Vector3 axis)
+    public override void LockAngularMotion(byte axislocks)
     {
-        DetailLog("{0},BSPrim.LockAngularMotion,call,axis={1}", LocalID, axis);
+        DetailLog("{0},BSPrim.LockAngularMotion,call,axis={1}", LocalID, axislocks);
 
         ApplyAxisLimits(ExtendedPhysics.PHYS_AXIS_UNLOCK_ANGULAR, 0f, 0f);
-        if (axis.X != 1)
+        if ((axislocks & 0x02) != 0)
         {
             ApplyAxisLimits(ExtendedPhysics.PHYS_AXIS_LOCK_ANGULAR_X, 0f, 0f);
         }
-        if (axis.Y != 1)
+        if ((axislocks & 0x04) != 0)
         {
             ApplyAxisLimits(ExtendedPhysics.PHYS_AXIS_LOCK_ANGULAR_Y, 0f, 0f);
         }
-        if (axis.Z != 1)
+        if ((axislocks & 0x08) != 0)
         {
             ApplyAxisLimits(ExtendedPhysics.PHYS_AXIS_LOCK_ANGULAR_Z, 0f, 0f);
         }
@@ -394,7 +393,7 @@ public class BSPrim : BSPhysObject
                 // Apply upforce and overcome gravity.
                 OMV.Vector3 correctionForce = upForce - PhysScene.DefaultGravity;
                 DetailLog("{0},BSPrim.PositionSanityCheck,applyForce,pos={1},upForce={2},correctionForce={3}", LocalID, RawPosition, upForce, correctionForce);
-                AddForce(correctionForce, false, inTaintTime);
+                AddForce(inTaintTime, correctionForce);
                 ret = true;
             }
         }
@@ -426,9 +425,9 @@ public class BSPrim : BSPhysObject
             RawVelocity = Util.ClampV(RawVelocity, BSParam.MaxLinearVelocity);
             ret = true;
         }
-        if (_rotationalVelocity.LengthSquared() > BSParam.MaxAngularVelocitySquared)
+        if (RawRotationalVelocity.LengthSquared() > BSParam.MaxAngularVelocitySquared)
         {
-            _rotationalVelocity = Util.ClampV(_rotationalVelocity, BSParam.MaxAngularVelocity);
+            RawRotationalVelocity = Util.ClampV(RawRotationalVelocity, BSParam.MaxAngularVelocity);
             ret = true;
         }
 
@@ -647,6 +646,59 @@ public class BSPrim : BSPhysObject
         });
     }
 
+    public override void SetVehicle(object pvdata)
+    {
+        PhysScene.TaintedObject(LocalID, "BSPrim.SetVehicle", delegate ()
+        {
+            BSDynamics vehicleActor = GetVehicleActor(true /* createIfNone */);
+            if (vehicleActor != null && (pvdata is VehicleData) )
+            {
+                VehicleData vdata = (VehicleData)pvdata;
+                // vehicleActor.ProcessSetVehicle((VehicleData)vdata);
+
+                vehicleActor.ProcessTypeChange(vdata.m_type);
+                vehicleActor.ProcessVehicleFlags(-1, false);
+                vehicleActor.ProcessVehicleFlags((int)vdata.m_flags, false);
+
+                // Linear properties
+                vehicleActor.ProcessVectorVehicleParam(Vehicle.LINEAR_MOTOR_DIRECTION, vdata.m_linearMotorDirection);
+                vehicleActor.ProcessVectorVehicleParam(Vehicle.LINEAR_FRICTION_TIMESCALE, vdata.m_linearFrictionTimescale);
+                vehicleActor.ProcessFloatVehicleParam(Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE, vdata.m_linearMotorDecayTimescale);
+                vehicleActor.ProcessFloatVehicleParam(Vehicle.LINEAR_MOTOR_TIMESCALE, vdata.m_linearMotorTimescale);
+                vehicleActor.ProcessVectorVehicleParam(Vehicle.LINEAR_MOTOR_OFFSET, vdata.m_linearMotorOffset);
+
+                //Angular properties
+                vehicleActor.ProcessVectorVehicleParam(Vehicle.ANGULAR_MOTOR_DIRECTION, vdata.m_angularMotorDirection);
+                vehicleActor.ProcessFloatVehicleParam(Vehicle.ANGULAR_MOTOR_TIMESCALE, vdata.m_angularMotorTimescale);
+                vehicleActor.ProcessFloatVehicleParam(Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE, vdata.m_angularMotorDecayTimescale);
+                vehicleActor.ProcessVectorVehicleParam(Vehicle.ANGULAR_FRICTION_TIMESCALE, vdata.m_angularFrictionTimescale);
+
+                //Deflection properties
+                vehicleActor.ProcessFloatVehicleParam(Vehicle.ANGULAR_DEFLECTION_EFFICIENCY, vdata.m_angularDeflectionEfficiency);
+                vehicleActor.ProcessFloatVehicleParam(Vehicle.ANGULAR_DEFLECTION_TIMESCALE, vdata.m_angularDeflectionTimescale);
+                vehicleActor.ProcessFloatVehicleParam(Vehicle.LINEAR_DEFLECTION_EFFICIENCY, vdata.m_linearDeflectionEfficiency);
+                vehicleActor.ProcessFloatVehicleParam(Vehicle.LINEAR_DEFLECTION_TIMESCALE, vdata.m_linearDeflectionTimescale);
+
+                //Banking properties
+                vehicleActor.ProcessFloatVehicleParam(Vehicle.BANKING_EFFICIENCY, vdata.m_bankingEfficiency);
+                vehicleActor.ProcessFloatVehicleParam(Vehicle.BANKING_MIX, vdata.m_bankingMix);
+                vehicleActor.ProcessFloatVehicleParam(Vehicle.BANKING_TIMESCALE, vdata.m_bankingTimescale);
+
+                //Hover and Buoyancy properties
+                vehicleActor.ProcessFloatVehicleParam(Vehicle.HOVER_HEIGHT, vdata.m_VhoverHeight);
+                vehicleActor.ProcessFloatVehicleParam(Vehicle.HOVER_EFFICIENCY, vdata.m_VhoverEfficiency);
+                vehicleActor.ProcessFloatVehicleParam(Vehicle.HOVER_TIMESCALE, vdata.m_VhoverTimescale);
+                vehicleActor.ProcessFloatVehicleParam(Vehicle.BUOYANCY, vdata.m_VehicleBuoyancy);
+
+                //Attractor properties
+                vehicleActor.ProcessFloatVehicleParam(Vehicle.VERTICAL_ATTRACTION_EFFICIENCY, vdata.m_verticalAttractionEfficiency);
+                vehicleActor.ProcessFloatVehicleParam(Vehicle.VERTICAL_ATTRACTION_TIMESCALE, vdata.m_verticalAttractionTimescale);
+
+                vehicleActor.ProcessRotationVehicleParam(Vehicle.REFERENCE_FRAME, vdata.m_referenceFrame);
+            }
+        });
+    }
+
     // Allows the detection of collisions with inherently non-physical prims. see llVolumeDetect for more
     public override void SetVolumeDetect(int param) {
         bool newValue = (param != 0);
@@ -735,22 +787,9 @@ public class BSPrim : BSPhysObject
             }
         }
     }
-    public override OMV.Vector3 Velocity {
-        get { return RawVelocity; }
-        set {
-            RawVelocity = value;
-            PhysScene.TaintedObject(LocalID, "BSPrim.setVelocity", delegate()
-            {
-                // DetailLog("{0},BSPrim.SetVelocity,taint,vel={1}", LocalID, RawVelocity);
-                ForceVelocity = RawVelocity;
-            });
-        }
-    }
     public override OMV.Vector3 ForceVelocity {
         get { return RawVelocity; }
         set {
-            PhysScene.AssertInTaintTime("BSPrim.ForceVelocity");
-
             RawVelocity = Util.ClampV(value, BSParam.MaxLinearVelocity);
             if (PhysBody.HasPhysicalBody)
             {
@@ -955,7 +994,7 @@ public class BSPrim : BSPhysObject
             // For good measure, make sure the transform is set through to the motion state
             ForcePosition = RawPosition;
             ForceVelocity = RawVelocity;
-            ForceRotationalVelocity = _rotationalVelocity;
+            ForceRotationalVelocity = RawRotationalVelocity;
 
             // A dynamic object has mass
             UpdatePhysicalMassProperties(RawMass, false);
@@ -1075,35 +1114,6 @@ public class BSPrim : BSPhysObject
             });
         }
     }
-    public override OMV.Vector3 RotationalVelocity {
-        get {
-            return _rotationalVelocity;
-        }
-        set {
-            _rotationalVelocity = value;
-            Util.ClampV(_rotationalVelocity, BSParam.MaxAngularVelocity);
-            // m_log.DebugFormat("{0}: RotationalVelocity={1}", LogHeader, _rotationalVelocity);
-            PhysScene.TaintedObject(LocalID, "BSPrim.setRotationalVelocity", delegate()
-            {
-                ForceRotationalVelocity = _rotationalVelocity;
-            });
-        }
-    }
-    public override OMV.Vector3 ForceRotationalVelocity {
-        get {
-            return _rotationalVelocity;
-        }
-        set {
-            _rotationalVelocity = Util.ClampV(value, BSParam.MaxAngularVelocity);
-            if (PhysBody.HasPhysicalBody)
-            {
-                DetailLog("{0},BSPrim.ForceRotationalVel,taint,rotvel={1}", LocalID, _rotationalVelocity);
-                PhysScene.PE.SetAngularVelocity(PhysBody, _rotationalVelocity);
-                // PhysicsScene.PE.SetInterpolationAngularVelocity(PhysBody, _rotationalVelocity);
-                ActivateIfPhysical(false);
-            }
-        }
-    }
     public override bool Kinematic {
         get { return _kinematic; }
         set { _kinematic = value;
@@ -1132,14 +1142,14 @@ public class BSPrim : BSPhysObject
         }
     }
 
-    public override bool PIDActive 
+    public override bool PIDActive
     {
         get
         {
             return MoveToTargetActive;
         }
 
-        set 
+        set
         {
             MoveToTargetActive = value;
 
@@ -1167,12 +1177,16 @@ public class BSPrim : BSPhysObject
                 // if the actor exists, tell it to refresh its values.
                 actor.Refresh();
             }
-            
+
         }
     }
     // Used for llSetHoverHeight and maybe vehicle height
     // Hover Height will override MoveTo target's Z
     public override bool PIDHoverActive {
+        get
+        {
+        return base.HoverActive;
+        }
         set {
             base.HoverActive = value;
             EnableActor(HoverActive, HoverActorName, delegate()
@@ -1192,14 +1206,18 @@ public class BSPrim : BSPhysObject
         // Per documentation, max force is limited.
         OMV.Vector3 addForce = Util.ClampV(force, BSParam.MaxAddForceMagnitude);
 
-        // Since this force is being applied in only one step, make this a force per second.
-        addForce /= PhysScene.LastTimeStep;
-        AddForce(addForce, pushforce, false /* inTaintTime */);
+        // Push forces seem to be scaled differently (follow pattern in ubODE)
+        if (!pushforce) {
+            // Since this force is being applied in only one step, make this a force per second.
+            addForce /= PhysScene.LastTimeStep;
+        }
+
+        AddForce(false /* inTaintTime */, addForce);
     }
 
     // Applying a force just adds this to the total force on the object.
     // This added force will only last the next simulation tick.
-    public override void AddForce(OMV.Vector3 force, bool pushforce, bool inTaintTime) {
+    public override void AddForce(bool inTaintTime, OMV.Vector3 force) {
         // for an object, doesn't matter if force is a pushforce or not
         if (IsPhysicallyActive)
         {
@@ -1258,7 +1276,7 @@ public class BSPrim : BSPhysObject
     }
 
     // BSPhysObject.AddAngularForce()
-    public override void AddAngularForce(OMV.Vector3 force, bool pushforce, bool inTaintTime)
+    public override void AddAngularForce(bool inTaintTime, OMV.Vector3 force)
     {
         if (force.IsFinite())
         {
@@ -1297,9 +1315,6 @@ public class BSPrim : BSPhysObject
         });
     }
 
-    public override void SetMomentum(OMV.Vector3 momentum) {
-        // DetailLog("{0},BSPrim.SetMomentum,call,mom={1}", LocalID, momentum);
-    }
     #region Mass Calculation
 
     private float CalculateMass()
@@ -1869,7 +1884,7 @@ public class BSPrim : BSPhysObject
         if (entprop.Velocity == OMV.Vector3.Zero || !entprop.Velocity.ApproxEquals(RawVelocity, BSParam.UpdateVelocityChangeThreshold))
             RawVelocity = entprop.Velocity;
         _acceleration = entprop.Acceleration;
-        _rotationalVelocity = entprop.RotationalVelocity;
+        RawRotationalVelocity = entprop.RotationalVelocity;
 
         // DetailLog("{0},BSPrim.UpdateProperties,afterAssign,entprop={1}", LocalID, entprop);   // DEBUG DEBUG
 
@@ -1878,7 +1893,7 @@ public class BSPrim : BSPhysObject
         {
             entprop.Position = RawPosition;
             entprop.Velocity = RawVelocity;
-            entprop.RotationalVelocity = _rotationalVelocity;
+            entprop.RotationalVelocity = RawRotationalVelocity;
             entprop.Acceleration = _acceleration;
         }
 
diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSPrimDisplaced.cs b/OpenSim/Region/PhysicsModules/BulletS/BSPrimDisplaced.cs
index d8ed56b..3f90fc5 100644..100755
--- a/OpenSim/Region/PhysicsModules/BulletS/BSPrimDisplaced.cs
+++ b/OpenSim/Region/PhysicsModules/BulletS/BSPrimDisplaced.cs
@@ -81,7 +81,6 @@ public class BSPrimDisplaced : BSPrim
     // Called at taint time.
     public virtual Vector3 SetEffectiveCenterOfMassDisplacement(Vector3 centerOfMassDisplacement)
     {
-        PhysScene.AssertInTaintTime("BSPrimDisplaced.SetEffectiveCenterOfMassDisplacement");
         Vector3 comDisp;
         if (UserSetCenterOfMassDisplacement.HasValue)
             comDisp = (OMV.Vector3)UserSetCenterOfMassDisplacement;
diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSPrimLinkable.cs b/OpenSim/Region/PhysicsModules/BulletS/BSPrimLinkable.cs
index 55b5da0..563dcfa 100644..100755
--- a/OpenSim/Region/PhysicsModules/BulletS/BSPrimLinkable.cs
+++ b/OpenSim/Region/PhysicsModules/BulletS/BSPrimLinkable.cs
@@ -203,15 +203,14 @@ public class BSPrimLinkable : BSPrimDisplaced
     // Called after a simulation step to post a collision with this object.
     // This returns 'true' if the collision has been queued and the SendCollisions call must
     //     be made at the end of the simulation step.
-    public override bool Collide(uint collidingWith, BSPhysObject collidee,
-                    OMV.Vector3 contactPoint, OMV.Vector3 contactNormal, float pentrationDepth)
+    public override bool Collide(BSPhysObject collidee, OMV.Vector3 contactPoint, OMV.Vector3 contactNormal, float pentrationDepth)
     {
         bool ret = false;
         // Ask the linkset if it wants to handle the collision
-        if (!Linkset.HandleCollide(collidingWith, collidee, contactPoint, contactNormal, pentrationDepth))
+        if (!Linkset.HandleCollide(this, collidee, contactPoint, contactNormal, pentrationDepth))
         {
             // The linkset didn't handle it so pass the collision through normal processing
-            ret = base.Collide(collidingWith, collidee, contactPoint, contactNormal, pentrationDepth);
+            ret = base.Collide(collidee, contactPoint, contactNormal, pentrationDepth);
         }
         return ret;
     }
diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSScene.cs b/OpenSim/Region/PhysicsModules/BulletS/BSScene.cs
index 452ce55..163efaa 100644
--- a/OpenSim/Region/PhysicsModules/BulletS/BSScene.cs
+++ b/OpenSim/Region/PhysicsModules/BulletS/BSScene.cs
@@ -124,9 +124,10 @@ namespace OpenSim.Region.PhysicsModule.BulletS
         // True if initialized and ready to do simulation steps
         private bool m_initialized = false;
 
-        // Flag which is true when processing taints.
-        // Not guaranteed to be correct all the time (don't depend on this) but good for debugging.
-        public bool InTaintTime { get; private set; }
+        // Object locked whenever execution is inside the physics engine
+        public Object PhysicsEngineLock = new object();
+        // Flag that is true when the simulator is active and shouldn't be touched
+        public bool InSimulationTime { get; private set; }
 
         // Pinned memory used to pass step information between managed and unmanaged
         internal int m_maxCollisionsPerFrame;
@@ -212,6 +213,11 @@ namespace OpenSim.Region.PhysicsModule.BulletS
             get { return "BulletSim"; }
         }
 
+        public string Version
+        {
+            get { return "1.0"; }
+        }
+
         public Type ReplaceableInterface
         {
             get { return null; }
@@ -245,6 +251,7 @@ namespace OpenSim.Region.PhysicsModule.BulletS
             EngineType = Name;
             RegionName = scene.RegionInfo.RegionName;
             PhysicsSceneName = EngineType + "/" + RegionName;
+            EngineName = Name + " " + Version;
 
             scene.RegisterModuleInterface<PhysicsScene>(this);
             Vector3 extent = new Vector3(scene.RegionInfo.RegionSizeX, scene.RegionInfo.RegionSizeY, scene.RegionInfo.RegionSizeZ);
@@ -338,19 +345,19 @@ namespace OpenSim.Region.PhysicsModule.BulletS
             // Put some informational messages into the log file.
             m_log.InfoFormat("{0} Linksets implemented with {1}", LogHeader, (BSLinkset.LinksetImplementation)BSParam.LinksetImplementation);
 
-            InTaintTime = false;
+            InSimulationTime = false;
             m_initialized = true;
 
             // If the physics engine runs on its own thread, start same.
             if (BSParam.UseSeparatePhysicsThread)
             {
                 // The physics simulation should happen independently of the heartbeat loop
-                m_physicsThread 
+                m_physicsThread
                     = WorkManager.StartThread(
-                        BulletSPluginPhysicsThread, 
-                        string.Format("{0} ({1})", BulletEngineName, RegionName), 
-                        ThreadPriority.Normal, 
-                        true, 
+                        BulletSPluginPhysicsThread,
+                        string.Format("{0} ({1})", BulletEngineName, RegionName),
+                        ThreadPriority.Normal,
+                        true,
                         true);
             }
         }
@@ -523,13 +530,13 @@ namespace OpenSim.Region.PhysicsModule.BulletS
             return null;
         }
 
-        public override PhysicsActor AddAvatar(uint localID, string avName, Vector3 position, Vector3 velocity, Vector3 size, bool isFlying)
+        public override PhysicsActor AddAvatar(uint localID, string avName, Vector3 position, Vector3 size, float footOffset, bool isFlying)
         {
             // m_log.DebugFormat("{0}: AddAvatar: {1}", LogHeader, avName);
 
             if (!m_initialized) return null;
 
-            BSCharacter actor = new BSCharacter(localID, avName, this, position, velocity, size, isFlying);
+            BSCharacter actor = new BSCharacter(localID, avName, this, position, Vector3.Zero, size, footOffset, isFlying);
             lock (PhysObjects)
                 PhysObjects.Add(localID, actor);
 
@@ -651,49 +658,57 @@ namespace OpenSim.Region.PhysicsModule.BulletS
 
             int beforeTime = Util.EnvironmentTickCount();
             int simTime = 0;
+            int numTaints = 0;
+            int numSubSteps = 0;
 
-            int numTaints = _taintOperations.Count;
-            InTaintTime = true; // Only used for debugging so locking is not necessary.
+            lock (PhysicsEngineLock)
+            {
+                InSimulationTime = true;
+                // update the prim states while we know the physics engine is not busy
+                numTaints += ProcessTaints();
 
-            // update the prim states while we know the physics engine is not busy
-            ProcessTaints();
+                // Some of the physical objects requre individual, pre-step calls
+                //      (vehicles and avatar movement, in particular)
+                TriggerPreStepEvent(timeStep);
 
-            // Some of the physical objects requre individual, pre-step calls
-            //      (vehicles and avatar movement, in particular)
-            TriggerPreStepEvent(timeStep);
+                // the prestep actions might have added taints
+                numTaints += ProcessTaints();
 
-            // the prestep actions might have added taints
-            numTaints += _taintOperations.Count;
-            ProcessTaints();
+                // The following causes the unmanaged code to output ALL the values found in ALL the objects in the world.
+                // Only enable this in a limited test world with few objects.
+                if (m_physicsPhysicalDumpEnabled)
+                    PE.DumpAllInfo(World);
 
-            InTaintTime = false; // Only used for debugging so locking is not necessary.
+                // step the physical world one interval
+                m_simulationStep++;
+                try
+                {
+                    numSubSteps = PE.PhysicsStep(World, timeStep, m_maxSubSteps, m_fixedTimeStep, out updatedEntityCount, out collidersCount);
+                }
+                catch (Exception e)
+                {
+                    m_log.WarnFormat("{0},PhysicsStep Exception: nTaints={1}, substeps={2}, updates={3}, colliders={4}, e={5}",
+                                LogHeader, numTaints, numSubSteps, updatedEntityCount, collidersCount, e);
+                    DetailLog("{0},PhysicsStepException,call, nTaints={1}, substeps={2}, updates={3}, colliders={4}",
+                                DetailLogZero, numTaints, numSubSteps, updatedEntityCount, collidersCount);
+                    updatedEntityCount = 0;
+                    collidersCount = 0;
+                }
 
-            // The following causes the unmanaged code to output ALL the values found in ALL the objects in the world.
-            // Only enable this in a limited test world with few objects.
-            if (m_physicsPhysicalDumpEnabled)
-                PE.DumpAllInfo(World);
+                // Make the physics engine dump useful statistics periodically
+                if (PhysicsMetricDumpFrames != 0 && ((m_simulationStep % PhysicsMetricDumpFrames) == 0))
+                    PE.DumpPhysicsStatistics(World);
 
-            // step the physical world one interval
-            m_simulationStep++;
-            int numSubSteps = 0;
-            try
-            {
-                numSubSteps = PE.PhysicsStep(World, timeStep, m_maxSubSteps, m_fixedTimeStep, out updatedEntityCount, out collidersCount);
+                InSimulationTime = false;
 
-            }
-            catch (Exception e)
-            {
-                m_log.WarnFormat("{0},PhysicsStep Exception: nTaints={1}, substeps={2}, updates={3}, colliders={4}, e={5}",
-                            LogHeader, numTaints, numSubSteps, updatedEntityCount, collidersCount, e);
-                DetailLog("{0},PhysicsStepException,call, nTaints={1}, substeps={2}, updates={3}, colliders={4}",
-                            DetailLogZero, numTaints, numSubSteps, updatedEntityCount, collidersCount);
-                updatedEntityCount = 0;
-                collidersCount = 0;
-            }
+                // Some actors want to know when the simulation step is complete.
+                TriggerPostStepEvent(timeStep);
+
+                // In case there were any parameter updates that happened during the simulation step
+                numTaints += ProcessTaints();
 
-            // Make the physics engine dump useful statistics periodically
-            if (PhysicsMetricDumpFrames != 0 && ((m_simulationStep % PhysicsMetricDumpFrames) == 0))
-                PE.DumpPhysicsStatistics(World);
+                InSimulationTime = false;
+            }
 
             // Get a value for 'now' so all the collision and update routines don't have to get their own.
             SimulationNowTime = Util.EnvironmentTickCount();
@@ -743,9 +758,6 @@ namespace OpenSim.Region.PhysicsModule.BulletS
                 }
             }
 
-            // Some actors want to know when the simulation step is complete.
-            TriggerPostStepEvent(timeStep);
-
             simTime = Util.EnvironmentTickCountSubtract(beforeTime);
             if (PhysicsLogging.Enabled)
             {
@@ -762,7 +774,8 @@ namespace OpenSim.Region.PhysicsModule.BulletS
             // The physics engine returns the number of milliseconds it simulated this call.
             // These are summed and normalized to one second and divided by 1000 to give the reported physics FPS.
             // Multiply by a fixed nominal frame rate to give a rate similar to the simulator (usually 55).
-            m_simulatedTime +=  (float)numSubSteps * m_fixedTimeStep * 1000f * NominalFrameRate;
+//            m_simulatedTime +=  (float)numSubSteps * m_fixedTimeStep * 1000f * NominalFrameRate;
+            m_simulatedTime +=  (float)numSubSteps * m_fixedTimeStep;
         }
 
         // Called by a BSPhysObject to note that it has changed properties and this information
@@ -849,7 +862,7 @@ namespace OpenSim.Region.PhysicsModule.BulletS
 
             // Return the framerate simulated to give the above returned results.
             // (Race condition here but this is just bookkeeping so rare mistakes do not merit a lock).
-            float simTime = m_simulatedTime;
+            float simTime = m_simulatedTime / timeStep;
             m_simulatedTime = 0f;
             return simTime;
         }
@@ -879,7 +892,7 @@ namespace OpenSim.Region.PhysicsModule.BulletS
 
             if (collider.IsInitialized)
             {
-                if (collider.Collide(collidingWith, collidee, collidePoint, collideNormal, penetration))
+                if (collider.Collide(collidee, collidePoint, collideNormal, penetration))
                 {
                     // If a collision was 'good', remember to send it to the simulator
                     lock (CollisionLock)
@@ -948,26 +961,150 @@ namespace OpenSim.Region.PhysicsModule.BulletS
             // m_log.DebugFormat("{0}: DeleteTerrain()", LogHeader);
         }
 
-        // Although no one seems to check this, I do support combining.
-        public override bool SupportsCombining()
+        #endregion // Terrain
+
+        #region Raycast
+
+        public override bool SupportsRayCast()
         {
-            return TerrainManager.SupportsCombining();
+            return BSParam.UseBulletRaycast;
         }
-        // This call says I am a child to region zero in a mega-region. 'pScene' is that
-        //    of region zero, 'offset' is my offset from regions zero's origin, and
-        //    'extents' is the largest XY that is handled in my region.
-        public override void Combine(PhysicsScene pScene, Vector3 offset, Vector3 extents)
+
+        public override bool SupportsRaycastWorldFiltered()
         {
-            TerrainManager.Combine(pScene, offset, extents);
+            return BSParam.UseBulletRaycast;
         }
 
-        // Unhook all the combining that I know about.
-        public override void UnCombine(PhysicsScene pScene)
+
+        /// <summary>
+        /// Queue a raycast against the physics scene.
+        /// The provided callback method will be called when the raycast is complete
+        ///
+        /// Many physics engines don't support collision testing at the same time as
+        /// manipulating the physics scene, so we queue the request up and callback
+        /// a custom method when the raycast is complete.
+        /// This allows physics engines that give an immediate result to callback immediately
+        /// and ones that don't, to callback when it gets a result back.
+        ///      public delegate void RayCallback(List<ContactResult> list);
+        ///
+        /// ODE for example will not allow you to change the scene while collision testing or
+        /// it asserts, 'opteration not valid for locked space'.  This includes adding a ray to the scene.
+        ///
+        /// This is named RayCastWorld to not conflict with modrex's Raycast method.
+        /// </summary>
+        /// <param name="position">Origin of the ray</param>
+        /// <param name="direction">Direction of the ray</param>
+        /// <param name="length">Length of ray in meters</param>
+        /// <param name="retMethod">Method to call when the raycast is complete</param>
+        public override void RaycastWorld(Vector3 position, Vector3 direction, float length, RaycastCallback retMethod)
         {
-            TerrainManager.UnCombine(pScene);
+            if (retMethod != null)
+            {
+                if (BSParam.UseBulletRaycast)
+                {
+                    Vector3 posFrom = position;
+                    Vector3 posTo = Vector3.Normalize(direction) * length + position;
+
+                    TaintedObject(DetailLogZero, "BSScene.RaycastWorld1", delegate ()
+                    {
+                        RaycastHit hitInfo = PE.RayTest2(World, posFrom, posTo, 0xffff, 0xffff);
+                        retMethod(true, hitInfo.Point, hitInfo.ID, hitInfo.Fraction, hitInfo.Normal);
+                    });
+                }
+                else
+                {
+                    retMethod(false, Vector3.Zero, 0, 999999999999f, Vector3.Zero);
+                }
+            }
         }
 
-        #endregion // Terrain
+        public override void RaycastWorld(Vector3 position, Vector3 direction, float length, int count, RayCallback retMethod)
+        {
+            if (retMethod != null)
+            {
+                if (BSParam.UseBulletRaycast)
+                {
+                    List<ContactResult> hitInfo = RaycastWorld(position, direction, length, count);
+                    retMethod(hitInfo);
+                }
+                else
+                {
+                    retMethod(new List<ContactResult>());
+                }
+            }
+        }
+
+        public override List<ContactResult> RaycastWorld(Vector3 position, Vector3 direction, float length, int count)
+        {
+            return (List<ContactResult>)RaycastWorld(position, direction, length, count, RayFilterFlags.All);
+        }
+
+        public override object RaycastWorld(Vector3 position, Vector3 direction, float length, int count, RayFilterFlags filter)
+        {
+            List<ContactResult> ret = new List<ContactResult>();
+            if (BSParam.UseBulletRaycast)
+            {
+                uint collisionFilter = 0;
+                uint collisionMask = 0;
+                if ((filter & RayFilterFlags.land) != 0)
+                {
+                    collisionFilter |= BulletSimData.CollisionTypeMasks[CollisionType.Terrain].group;
+                    collisionMask |= BulletSimData.CollisionTypeMasks[CollisionType.Terrain].mask;
+                }
+                if ((filter & RayFilterFlags.agent) != 0)
+                {
+                    collisionFilter |= BulletSimData.CollisionTypeMasks[CollisionType.Avatar].group;
+                    collisionMask |= BulletSimData.CollisionTypeMasks[CollisionType.Avatar].mask;
+                }
+                if ((filter & RayFilterFlags.nonphysical) != 0)
+                {
+                    collisionFilter |= BulletSimData.CollisionTypeMasks[CollisionType.Static].group;
+                    collisionMask |= BulletSimData.CollisionTypeMasks[CollisionType.Static].mask;
+                }
+                if ((filter & RayFilterFlags.physical) != 0)
+                {
+                    collisionFilter |= BulletSimData.CollisionTypeMasks[CollisionType.Dynamic].group;
+                    collisionMask |= BulletSimData.CollisionTypeMasks[CollisionType.Dynamic].mask;
+                }
+                // if ((filter & RayFilterFlags.phantom) != 0)
+                // {
+                //     collisionFilter |= BulletSimData.CollisionTypeMasks[CollisionType.VolumeDetect].group;
+                //     collisionMask |= BulletSimData.CollisionTypeMasks[CollisionType.VolumeDetect].mask;
+                // }
+                if ((filter & RayFilterFlags.volumedtc) != 0)
+                {
+                    collisionFilter |= BulletSimData.CollisionTypeMasks[CollisionType.VolumeDetect].group;
+                    collisionMask |= BulletSimData.CollisionTypeMasks[CollisionType.VolumeDetect].mask;
+                }
+                DetailLog("{0},RaycastWorld,pos={1},dir={2},len={3},count={4},filter={5},filter={6},mask={7}",
+                        DetailLogZero, position, direction, length, count, filter, collisionFilter, collisionMask);
+                // NOTE: locking ensures the physics engine is not executing.
+                //    The caller might have to wait for the physics engine to finish.
+                lock (PhysicsEngineLock)
+                {
+                    Vector3 posFrom = position;
+                    Vector3 posTo = Vector3.Normalize(direction) * length + position;
+                    DetailLog("{0},RaycastWorld,RayTest2,from={1},to={2}",
+                                DetailLogZero, posFrom, posTo);
+                    RaycastHit hitInfo = PE.RayTest2(World, posFrom, posTo, collisionFilter, collisionMask);
+                    if (hitInfo.hasHit())
+                    {
+                        ContactResult result = new ContactResult();
+                        result.Pos = hitInfo.Point;
+                        result.Normal = hitInfo.Normal;
+                        result.ConsumerID = hitInfo.ID;
+                        result.Depth = hitInfo.Fraction;
+                        ret.Add(result);
+                        DetailLog("{0},RaycastWorld,hit,pos={1},norm={2},depth={3},id={4}",
+                            DetailLogZero, result.Pos, result.Normal, result.Depth, result.ConsumerID);
+                    }
+                }
+            }
+            return ret;
+        }
+
+        #endregion Raycast
+
 
         public override Dictionary<uint, float> GetTopColliders()
         {
@@ -1081,32 +1218,35 @@ namespace OpenSim.Region.PhysicsModule.BulletS
         // Calls to the PhysicsActors can't directly call into the physics engine
         //       because it might be busy. We delay changes to a known time.
         // We rely on C#'s closure to save and restore the context for the delegate.
-        public void TaintedObject(string pOriginator, string pIdent, TaintCallback pCallback)
+        // NOTE: 'inTaintTime' is no longer used. This entry exists so all the calls don't have to be changed.
+        // public void TaintedObject(bool inTaintTime, String pIdent, TaintCallback pCallback)
+        // {
+        //     TaintedObject(BSScene.DetailLogZero, pIdent, pCallback);
+        // }
+        // NOTE: 'inTaintTime' is no longer used. This entry exists so all the calls don't have to be changed.
+        public void TaintedObject(bool inTaintTime, uint pOriginator, String pIdent, TaintCallback pCallback)
         {
-            TaintedObject(false /*inTaintTime*/, pOriginator, pIdent, pCallback);
+            TaintedObject(m_physicsLoggingEnabled ? pOriginator.ToString() : BSScene.DetailLogZero, pIdent, pCallback);
         }
         public void TaintedObject(uint pOriginator, String pIdent, TaintCallback pCallback)
         {
-            TaintedObject(false /*inTaintTime*/, m_physicsLoggingEnabled ? pOriginator.ToString() : BSScene.DetailLogZero, pIdent, pCallback);
-        }
-        public void TaintedObject(bool inTaintTime, String pIdent, TaintCallback pCallback)
-        {
-            TaintedObject(inTaintTime, BSScene.DetailLogZero, pIdent, pCallback);
-        }
-        public void TaintedObject(bool inTaintTime, uint pOriginator, String pIdent, TaintCallback pCallback)
-        {
-            TaintedObject(inTaintTime, m_physicsLoggingEnabled ? pOriginator.ToString() : BSScene.DetailLogZero, pIdent, pCallback);
+            TaintedObject(m_physicsLoggingEnabled ? pOriginator.ToString() : BSScene.DetailLogZero, pIdent, pCallback);
         }
         // Sometimes a potentially tainted operation can be used in and out of taint time.
         // This routine executes the command immediately if in taint-time otherwise it is queued.
-        public void TaintedObject(bool inTaintTime, string pOriginator, string pIdent, TaintCallback pCallback)
+        public void TaintedObject(string pOriginator, string pIdent, TaintCallback pCallback)
         {
             if (!m_initialized) return;
 
-            if (inTaintTime)
+            if (Monitor.TryEnter(PhysicsEngineLock))
+            {
+                // If we can get exclusive access to the physics engine, just do the operation
                 pCallback();
+                Monitor.Exit(PhysicsEngineLock);
+            }
             else
             {
+                // The physics engine is busy, queue the operation
                 lock (_taintLock)
                 {
                     _taintOperations.Add(new TaintCallbackEntry(pOriginator, pIdent, pCallback));
@@ -1133,14 +1273,21 @@ namespace OpenSim.Region.PhysicsModule.BulletS
         // When someone tries to change a property on a BSPrim or BSCharacter, the object queues
         // a callback into itself to do the actual property change. That callback is called
         // here just before the physics engine is called to step the simulation.
-        public void ProcessTaints()
+        // Returns the number of taints processed
+        // NOTE: Called while PhysicsEngineLock is locked
+        public int ProcessTaints()
         {
-            ProcessRegularTaints();
-            ProcessPostTaintTaints();
+            int ret = 0;
+            ret += ProcessRegularTaints();
+            ret += ProcessPostTaintTaints();
+            return ret;
         }
 
-        private void ProcessRegularTaints()
+        // Returns the number of taints processed
+        // NOTE: Called while PhysicsEngineLock is locked
+        private int ProcessRegularTaints()
         {
+            int ret = 0;
             if (m_initialized && _taintOperations.Count > 0)  // save allocating new list if there is nothing to process
             {
                 // swizzle a new list into the list location so we can process what's there
@@ -1157,6 +1304,7 @@ namespace OpenSim.Region.PhysicsModule.BulletS
                     {
                         DetailLog("{0},BSScene.ProcessTaints,doTaint,id={1}", tcbe.originator, tcbe.ident); // DEBUG DEBUG DEBUG
                         tcbe.callback();
+                        ret++;
                     }
                     catch (Exception e)
                     {
@@ -1165,6 +1313,7 @@ namespace OpenSim.Region.PhysicsModule.BulletS
                 }
                 oldList.Clear();
             }
+            return ret;
         }
 
         // Schedule an update to happen after all the regular taints are processed.
@@ -1183,8 +1332,11 @@ namespace OpenSim.Region.PhysicsModule.BulletS
         }
 
         // Taints that happen after the normal taint processing but before the simulation step.
-        private void ProcessPostTaintTaints()
+        // Returns the number of taints processed
+        // NOTE: Called while PhysicsEngineLock is locked
+        private int ProcessPostTaintTaints()
         {
+            int ret = 0;
             if (m_initialized && _postTaintOperations.Count > 0)
             {
                 Dictionary<string, TaintCallbackEntry> oldList;
@@ -1200,6 +1352,7 @@ namespace OpenSim.Region.PhysicsModule.BulletS
                     {
                         DetailLog("{0},BSScene.ProcessPostTaintTaints,doTaint,id={1}", DetailLogZero, kvp.Key); // DEBUG DEBUG DEBUG
                         kvp.Value.callback();
+                        ret++;
                     }
                     catch (Exception e)
                     {
@@ -1208,20 +1361,8 @@ namespace OpenSim.Region.PhysicsModule.BulletS
                 }
                 oldList.Clear();
             }
+            return ret;
         }
-
-        // Only used for debugging. Does not change state of anything so locking is not necessary.
-        public bool AssertInTaintTime(string whereFrom)
-        {
-            if (!InTaintTime)
-            {
-                DetailLog("{0},BSScene.AssertInTaintTime,NOT IN TAINT TIME,Region={1},Where={2}", DetailLogZero, RegionName, whereFrom);
-                m_log.ErrorFormat("{0} NOT IN TAINT TIME!! Region={1}, Where={2}", LogHeader, RegionName, whereFrom);
-                // Util.PrintCallStack(DetailLog);
-            }
-            return InTaintTime;
-        }
-
         #endregion // Taints
 
         #region IPhysicsParameters
diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSShapeCollection.cs b/OpenSim/Region/PhysicsModules/BulletS/BSShapeCollection.cs
index b100273..86bf23f 100644..100755
--- a/OpenSim/Region/PhysicsModules/BulletS/BSShapeCollection.cs
+++ b/OpenSim/Region/PhysicsModules/BulletS/BSShapeCollection.cs
@@ -30,7 +30,6 @@ using System.Text;
 using OMV = OpenMetaverse;
 using OpenSim.Framework;
 using OpenSim.Region.PhysicsModules.SharedBase;
-using OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet;
 
 namespace OpenSim.Region.PhysicsModule.BulletS
 {
@@ -76,8 +75,6 @@ public sealed class BSShapeCollection : IDisposable
     // Called at taint-time.
     public bool GetBodyAndShape(bool forceRebuild, BulletWorld sim, BSPhysObject prim, PhysicalDestructionCallback bodyCallback)
     {
-        m_physicsScene.AssertInTaintTime("BSShapeCollection.GetBodyAndShape");
-
         bool ret = false;
 
         // This lock could probably be pushed down lower but building shouldn't take long
@@ -230,6 +227,8 @@ public sealed class BSShapeCollection : IDisposable
             ret = CreateGeomMeshOrHull(prim, shapeCallback);
         }
 
+        m_physicsScene.PE.ResetBroadphasePool(m_physicsScene.World);    // DEBUG DEBUG
+
         return ret;
     }
 
@@ -345,8 +344,6 @@ public sealed class BSShapeCollection : IDisposable
         if (!body.HasPhysicalBody)
             return;
 
-        m_physicsScene.AssertInTaintTime("BSShapeCollection.DereferenceBody");
-
         lock (m_collectionActivityLock)
         {
             if (DDetail) DetailLog("{0},BSShapeCollection.DereferenceBody,DestroyingBody,body={1}", body.ID, body);
diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSShapes.cs b/OpenSim/Region/PhysicsModules/BulletS/BSShapes.cs
index 086a412..e791b27 100644..100755
--- a/OpenSim/Region/PhysicsModules/BulletS/BSShapes.cs
+++ b/OpenSim/Region/PhysicsModules/BulletS/BSShapes.cs
@@ -31,8 +31,8 @@ using System.Text;
 
 using OpenSim.Framework;
 using OpenSim.Region.PhysicsModules.SharedBase;
-using OpenSim.Region.PhysicsModules.Meshing;
-using OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet;
+using OpenSim.Region.PhysicsModule.Meshing;
+using OpenSim.Region.PhysicsModules.ConvexDecompositionDotNet;
 
 using OMV = OpenMetaverse;
 
@@ -555,7 +555,9 @@ public class BSShapeMesh : BSShape
         {
             meshData = physicsScene.mesher.CreateMesh(prim.PhysObjectName, pbs, size, lod,
                                             false,  // say it is not physical so a bounding box is not built
-                                            false   // do not cache the mesh and do not use previously built versions
+                                            false,   // do not cache the mesh and do not use previously built versions
+                                            false,
+                                            false
                                             );
         }
 
@@ -712,7 +714,7 @@ public class BSShapeHull : BSShape
         lock (physicsScene.mesher)
         {
             // Pass true for physicalness as this prevents the creation of bounding box which is not needed
-            meshData = physicsScene.mesher.CreateMesh(prim.PhysObjectName, pbs, size, lod, true /* isPhysical */, false /* shouldCache */);
+            meshData = physicsScene.mesher.CreateMesh(prim.PhysObjectName, pbs, size, lod, true /* isPhysical */, false /* shouldCache */, false, false);
 
             // If we should use the asset's hull info, fetch it out of the locked mesher
             if (meshData != null && BSParam.ShouldUseAssetHulls)
diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSTerrainHeightmap.cs b/OpenSim/Region/PhysicsModules/BulletS/BSTerrainHeightmap.cs
index 42fc11b..f72ad28 100644..100755
--- a/OpenSim/Region/PhysicsModules/BulletS/BSTerrainHeightmap.cs
+++ b/OpenSim/Region/PhysicsModules/BulletS/BSTerrainHeightmap.cs
@@ -141,14 +141,30 @@ public sealed class BSTerrainHeightmap : BSTerrainPhys
     }
 
     // The passed position is relative to the base of the region.
+    // There are many assumptions herein that the heightmap increment is 1.
     public override float GetTerrainHeightAtXYZ(Vector3 pos)
     {
         float ret = BSTerrainManager.HEIGHT_GETHEIGHT_RET;
 
-        int mapIndex = (int)pos.Y * (int)m_mapInfo.sizeY + (int)pos.X;
-        try
-        {
-            ret = m_mapInfo.heightMap[mapIndex];
+        try {
+            int baseX = (int)pos.X;
+            int baseY = (int)pos.Y;
+            int maxX = (int)m_mapInfo.sizeX;
+            int maxY = (int)m_mapInfo.sizeY;
+            float diffX = pos.X - (float)baseX;
+            float diffY = pos.Y - (float)baseY;
+
+            float mapHeight1 = m_mapInfo.heightMap[baseY * maxY + baseX];
+            float mapHeight2 = m_mapInfo.heightMap[Math.Min(baseY + 1, maxY - 1) * maxY + baseX];
+            float mapHeight3 = m_mapInfo.heightMap[baseY * maxY + Math.Min(baseX + 1, maxX  - 1)];
+            float mapHeight4 = m_mapInfo.heightMap[Math.Min(baseY + 1, maxY - 1) * maxY +  Math.Min(baseX + 1, maxX  - 1)];
+
+            float Xrise = (mapHeight4 - mapHeight3) * diffX;
+            float Yrise = (mapHeight2 - mapHeight1) * diffY;
+
+            ret = mapHeight1 + ((Xrise + Yrise) / 2f);
+            // m_physicsScene.DetailLog("{0},BSTerrainHeightMap,GetTerrainHeightAtXYZ,pos={1},{2}/{3}/{4}/{5},ret={6}",
+            //         BSScene.DetailLogZero, pos, mapHeight1, mapHeight2, mapHeight3, mapHeight4, ret);
         }
         catch
         {
diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSTerrainManager.cs b/OpenSim/Region/PhysicsModules/BulletS/BSTerrainManager.cs
index d11baa6..d11baa6 100644..100755
--- a/OpenSim/Region/PhysicsModules/BulletS/BSTerrainManager.cs
+++ b/OpenSim/Region/PhysicsModules/BulletS/BSTerrainManager.cs
diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSTerrainMesh.cs b/OpenSim/Region/PhysicsModules/BulletS/BSTerrainMesh.cs
index cd59b65..cd59b65 100644..100755
--- a/OpenSim/Region/PhysicsModules/BulletS/BSTerrainMesh.cs
+++ b/OpenSim/Region/PhysicsModules/BulletS/BSTerrainMesh.cs
diff --git a/OpenSim/Region/PhysicsModules/BulletS/BulletSimData.cs b/OpenSim/Region/PhysicsModules/BulletS/BulletSimData.cs
index 3329395..308769b 100644..100755
--- a/OpenSim/Region/PhysicsModules/BulletS/BulletSimData.cs
+++ b/OpenSim/Region/PhysicsModules/BulletS/BulletSimData.cs
@@ -100,6 +100,7 @@ public class BulletBody
     }
 }
 
+// Handle to btCollisionObject - a shape that can be added to a btRidgidBody
 public class BulletShape
 {
     public BulletShape()
diff --git a/OpenSim/Region/PhysicsModules/BulletS/BulletSimTODO.txt b/OpenSim/Region/PhysicsModules/BulletS/BulletSimTODO.txt
index 0453376..0453376 100644..100755
--- a/OpenSim/Region/PhysicsModules/BulletS/BulletSimTODO.txt
+++ b/OpenSim/Region/PhysicsModules/BulletS/BulletSimTODO.txt
diff --git a/OpenSim/Region/PhysicsModules/BulletS/ExtendedPhysics.cs b/OpenSim/Region/PhysicsModules/BulletS/ExtendedPhysics.cs
index 2ba3c5a..ab5d4849 100644..100755
--- a/OpenSim/Region/PhysicsModules/BulletS/ExtendedPhysics.cs
+++ b/OpenSim/Region/PhysicsModules/BulletS/ExtendedPhysics.cs
@@ -136,7 +136,7 @@ namespace OpenSim.Region.PhysicsModule.BulletS
 
                 return;
             }
- 
+
             // Register as LSL functions all the [ScriptInvocation] marked methods.
             Comms.RegisterScriptInvocations(this);
             Comms.RegisterConstants(this);
diff --git a/OpenSim/Region/PhysicsModules/BulletS/Properties/AssemblyInfo.cs b/OpenSim/Region/PhysicsModules/BulletS/Properties/AssemblyInfo.cs
index 698be39..91ce468 100644
--- a/OpenSim/Region/PhysicsModules/BulletS/Properties/AssemblyInfo.cs
+++ b/OpenSim/Region/PhysicsModules/BulletS/Properties/AssemblyInfo.cs
@@ -3,7 +3,7 @@ using System.Runtime.CompilerServices;
 using System.Runtime.InteropServices;
 using Mono.Addins;
 
-// General Information about an assembly is controlled through the following 
+// General Information about an assembly is controlled through the following
 // set of attributes. Change these attribute values to modify the information
 // associated with an assembly.
 [assembly: AssemblyTitle("OpenSim.Region.Physics.BulletSPlugin")]
@@ -15,8 +15,8 @@ using Mono.Addins;
 [assembly: AssemblyTrademark("")]
 [assembly: AssemblyCulture("")]
 
-// Setting ComVisible to false makes the types in this assembly not visible 
-// to COM components.  If you need to access a type in this assembly from 
+// Setting ComVisible to false makes the types in this assembly not visible
+// to COM components.  If you need to access a type in this assembly from
 // COM, set the ComVisible attribute to true on that type.
 [assembly: ComVisible(false)]
 
@@ -26,11 +26,11 @@ using Mono.Addins;
 // Version information for an assembly consists of the following four values:
 //
 //      Major Version
-//      Minor Version 
+//      Minor Version
 //      Build Number
 //      Revision
 //
-[assembly: AssemblyVersion("0.8.3.*")]
+[assembly: AssemblyVersion(OpenSim.VersionInfo.AssemblyVersionNumber)]
 
 [assembly: Addin("OpenSim.Region.PhysicsModule.BulletS", OpenSim.VersionInfo.VersionNumber)]
 [assembly: AddinDependency("OpenSim.Region.Framework", OpenSim.VersionInfo.VersionNumber)]
diff --git a/OpenSim/Region/PhysicsModules/BulletS/Tests/BasicVehicles.cs b/OpenSim/Region/PhysicsModules/BulletS/Tests/BasicVehicles.cs
index 35eba29..35eba29 100644..100755
--- a/OpenSim/Region/PhysicsModules/BulletS/Tests/BasicVehicles.cs
+++ b/OpenSim/Region/PhysicsModules/BulletS/Tests/BasicVehicles.cs
diff --git a/OpenSim/Region/PhysicsModules/BulletS/Tests/BulletSimTests.cs b/OpenSim/Region/PhysicsModules/BulletS/Tests/BulletSimTests.cs
index 0be1f4c..14d0bdc 100644..100755
--- a/OpenSim/Region/PhysicsModules/BulletS/Tests/BulletSimTests.cs
+++ b/OpenSim/Region/PhysicsModules/BulletS/Tests/BulletSimTests.cs
@@ -1,56 +1,56 @@
-/*

- * Copyright (c) Contributors, http://opensimulator.org/

- * See CONTRIBUTORS.TXT for a full list of copyright holders.

- *

- * Redistribution and use in source and binary forms, with or without

- * modification, are permitted provided that the following conditions are met:

- *     * Redistributions of source code must retain the above copyright

- *       notice, this list of conditions and the following disclaimer.

- *     * Redistributions in binary form must reproduce the above copyright

- *       notice, this list of conditions and the following disclaimer in the

- *       documentation and/or other materials provided with the distribution.

- *     * Neither the name of the OpenSimulator Project nor the

- *       names of its contributors may be used to endorse or promote products

- *       derived from this software without specific prior written permission.

- *

- * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY

- * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED

- * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE

- * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY

- * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES

- * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;

- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND

- * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT

- * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS

- * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

- */

-

-using System;

-using System.Collections.Generic;

-using System.Linq;

-using System.Text;

-

-using NUnit.Framework;

-using log4net;

-

-using OpenSim.Tests.Common;

-

-namespace OpenSim.Region.PhysicsModule.BulletS.Tests

-{

-[TestFixture]

-public class BulletSimTests : OpenSimTestCase

-{

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

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

-

-    [TestFixtureSetUp]

-    public void Init()

-    {

-    }

-

-    [TestFixtureTearDown]

-    public void TearDown()

-    {

-    }

-}

-}

+/*
+ * Copyright (c) Contributors, http://opensimulator.org/
+ * See CONTRIBUTORS.TXT for a full list of copyright holders.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the OpenSimulator Project nor the
+ *       names of its contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Text;
+
+using NUnit.Framework;
+using log4net;
+
+using OpenSim.Tests.Common;
+
+namespace OpenSim.Region.PhysicsModule.BulletS.Tests
+{
+[TestFixture]
+public class BulletSimTests : OpenSimTestCase
+{
+    // Documentation on attributes: http://www.nunit.org/index.php?p=attributes&r=2.6.1
+    // Documentation on assertions: http://www.nunit.org/index.php?p=assertions&r=2.6.1
+
+    [TestFixtureSetUp]
+    public void Init()
+    {
+    }
+
+    [TestFixtureTearDown]
+    public void TearDown()
+    {
+    }
+}
+}
diff --git a/OpenSim/Region/PhysicsModules/BulletS/Tests/BulletSimTestsUtil.cs b/OpenSim/Region/PhysicsModules/BulletS/Tests/BulletSimTestsUtil.cs
index 4eeea4d..3e8c1cd 100644..100755
--- a/OpenSim/Region/PhysicsModules/BulletS/Tests/BulletSimTestsUtil.cs
+++ b/OpenSim/Region/PhysicsModules/BulletS/Tests/BulletSimTestsUtil.cs
@@ -34,7 +34,7 @@ using Nini.Config;
 
 using OpenSim.Framework;
 using OpenSim.Region.PhysicsModules.SharedBase;
-using OpenSim.Region.PhysicsModules.Meshing;
+using OpenSim.Region.PhysicsModule.Meshing;
 using OpenSim.Region.Framework.Interfaces;
 
 using OpenMetaverse;
@@ -80,13 +80,13 @@ public static class BulletSimTestsUtil
         }
 
         Vector3 regionExtent = new Vector3(Constants.RegionSize, Constants.RegionSize, Constants.RegionHeight);
-       
+
         RegionInfo info = new RegionInfo();
         info.RegionName = "BSTestRegion";
         info.RegionSizeX = info.RegionSizeY = info.RegionSizeZ = Constants.RegionSize;
         OpenSim.Region.Framework.Scenes.Scene scene = new OpenSim.Region.Framework.Scenes.Scene(info);
 
-        IMesher mesher = new OpenSim.Region.PhysicsModules.Meshing.Meshmerizer();
+        IMesher mesher = new OpenSim.Region.PhysicsModule.Meshing.Meshmerizer();
         INonSharedRegionModule mod = mesher as INonSharedRegionModule;
         mod.Initialise(openSimINI);
         mod.AddRegion(scene);
diff --git a/OpenSim/Region/PhysicsModules/BulletS/Tests/HullCreation.cs b/OpenSim/Region/PhysicsModules/BulletS/Tests/HullCreation.cs
index c0cf19a..c2be568 100644
--- a/OpenSim/Region/PhysicsModules/BulletS/Tests/HullCreation.cs
+++ b/OpenSim/Region/PhysicsModules/BulletS/Tests/HullCreation.cs
@@ -146,7 +146,7 @@ public class HullCreation : OpenSimTestCase
         uint torusLocalID = 125;
         PhysicsScene.AddPrimShape("testTorus", pbs, pos, size, rot, isPhys, torusLocalID);
         BSPrim primTypeTorus = (BSPrim)PhysicsScene.PhysObjects[torusLocalID];
-        
+
         // The actual prim shape creation happens at taint time
         PhysicsScene.ProcessTaints();
 
diff --git a/OpenSim/Region/PhysicsModules/BulletS/Tests/Raycast.cs b/OpenSim/Region/PhysicsModules/BulletS/Tests/Raycast.cs
new file mode 100755
index 0000000..bfa95c1
--- /dev/null
+++ b/OpenSim/Region/PhysicsModules/BulletS/Tests/Raycast.cs
@@ -0,0 +1,124 @@
+/*
+ * Copyright (c) Contributors, http://opensimulator.org/
+ * See CONTRIBUTORS.TXT for a full list of copyright holders.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the OpenSimulator Project nor the
+ *       names of its contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Text;
+
+using NUnit.Framework;
+using log4net;
+
+using OpenSim.Framework;
+using OpenSim.Region.PhysicsModule.BulletS;
+using OpenSim.Region.PhysicsModules.SharedBase;
+using OpenSim.Tests.Common;
+
+using OpenMetaverse;
+
+namespace OpenSim.Region.PhysicsModule.BulletS.Tests
+{
+    [TestFixture]
+    public class BulletSimRaycast : OpenSimTestCase
+    {
+        // Documentation on attributes: http://www.nunit.org/index.php?p=attributes&r=2.6.1
+        // Documentation on assertions: http://www.nunit.org/index.php?p=assertions&r=2.6.1
+
+        BSScene _physicsScene { get; set; }
+        BSPrim _targetSphere { get; set; }
+        Vector3 _targetSpherePosition { get; set; }
+        float _simulationTimeStep = 0.089f;
+
+        uint _targetLocalID = 123;
+
+        [TestFixtureSetUp]
+        public void Init()
+        {
+            Dictionary<string, string> engineParams = new Dictionary<string, string>();
+            engineParams.Add("UseBulletRaycast", "true");
+            _physicsScene = BulletSimTestsUtil.CreateBasicPhysicsEngine(engineParams);
+
+            PrimitiveBaseShape pbs = PrimitiveBaseShape.CreateSphere();
+            Vector3 pos = new Vector3(100.0f, 100.0f, 50f);
+            _targetSpherePosition = pos;
+            Vector3 size = new Vector3(10f, 10f, 10f);
+            pbs.Scale = size;
+            Quaternion rot = Quaternion.Identity;
+            bool isPhys = false;
+
+            _physicsScene.AddPrimShape("TargetSphere", pbs, pos, size, rot, isPhys, _targetLocalID);
+            _targetSphere = (BSPrim)_physicsScene.PhysObjects[_targetLocalID];
+            // The actual prim shape creation happens at taint time
+            _physicsScene.ProcessTaints();
+
+        }
+
+        [TestFixtureTearDown]
+        public void TearDown()
+        {
+            if (_physicsScene != null)
+            {
+                // The Dispose() will also free any physical objects in the scene
+                _physicsScene.Dispose();
+                _physicsScene = null;
+            }
+        }
+
+        // There is a 10x10x10 sphere at <100,100,50>
+        // Shoot rays around the sphere and verify it hits and doesn't hit
+        // TestCase parameters are <x,y,z> of start and <x,y,z> of end and expected result
+        [TestCase(100f, 50f, 50f, 100f, 150f, 50f, true, "Pass through sphere from front")]
+        [TestCase(50f, 100f, 50f, 150f, 100f, 50f, true, "Pass through sphere from side")]
+        [TestCase(50f, 50f, 50f, 150f, 150f, 50f, true, "Pass through sphere diaginally")]
+        [TestCase(100f, 100f, 100f, 100f, 100f, 20f, true, "Pass through sphere from above")]
+        [TestCase(20f, 20f, 50f, 80f, 80f, 50f, false, "Not reach sphere")]
+        [TestCase(50f, 50f, 65f, 150f, 150f, 65f, false, "Passed over sphere")]
+        public void RaycastAroundObject(float fromX, float fromY, float fromZ, float toX, float toY, float toZ, bool expected, string msg) {
+            Vector3 fromPos = new Vector3(fromX, fromY, fromZ);
+            Vector3 toPos = new Vector3(toX, toY, toZ);
+            Vector3 direction = toPos - fromPos;
+            float len = Vector3.Distance(fromPos, toPos);
+
+            List<ContactResult> results = _physicsScene.RaycastWorld(fromPos, direction, len, 1);
+
+            if (expected) {
+                // The  test coordinates should generate a hit
+                Assert.True(results.Count != 0, msg + ": Did not return a hit but expected to.");
+                Assert.True(results.Count == 1, msg + ": Raycast returned not just one hit result.");
+                Assert.True(results[0].ConsumerID == _targetLocalID, msg + ": Raycast returned a collision object other than the target");
+            }
+            else
+            {
+                // The test coordinates should not generate a hit
+                if (results.Count > 0)
+                {
+                    Assert.False(results.Count > 0, msg + ": Returned a hit at " + results[0].Pos.ToString());
+                }
+            }
+        }
+    }
+}
\ No newline at end of file
diff --git a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/CTri.cs b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/CTri.cs
index 7ad689e..bd1f75e 100644
--- a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/CTri.cs
+++ b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/CTri.cs
@@ -1,21 +1,21 @@
 /* The MIT License
- * 
+ *
  * Copyright (c) 2010 Intel Corporation.
  * All rights reserved.
  *
- * Based on the convexdecomposition library from 
+ * Based on the convexdecomposition library from
  * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax.
- * 
+ *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
  * in the Software without restriction, including without limitation the rights
  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
  * copies of the Software, and to permit persons to whom the Software is
  * furnished to do so, subject to the following conditions:
- * 
+ *
  * The above copyright notice and this permission notice shall be included in
  * all copies or substantial portions of the Software.
- * 
+ *
  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
@@ -28,7 +28,7 @@
 using System;
 using System.Collections.Generic;
 
-namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet
+namespace OpenSim.Region.PhysicsModules.ConvexDecompositionDotNet
 {
     public class Wpoint
     {
@@ -246,13 +246,13 @@ namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet
         }
 
         private static float DistToPt(float3 p, float4 plane)
-            {
-                    float x = p.x;
-                    float y = p.y;
-                    float z = p.z;
-                    float d = x*plane.x + y*plane.y + z*plane.z + plane.w;
-                    return d;
-            }
+        {
+            float x = p.x;
+            float y = p.y;
+            float z = p.z;
+            float d = x*plane.x + y*plane.y + z*plane.z + plane.w;
+            return d;
+        }
 
         private static void intersect(float3 p1, float3 p2, ref float3 split, float4 plane)
         {
diff --git a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/Concavity.cs b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/Concavity.cs
index 4140d25..0d22d71 100644
--- a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/Concavity.cs
+++ b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/Concavity.cs
@@ -1,21 +1,21 @@
 /* The MIT License
- * 
+ *
  * Copyright (c) 2010 Intel Corporation.
  * All rights reserved.
  *
- * Based on the convexdecomposition library from 
+ * Based on the convexdecomposition library from
  * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax.
- * 
+ *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
  * in the Software without restriction, including without limitation the rights
  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
  * copies of the Software, and to permit persons to whom the Software is
  * furnished to do so, subject to the following conditions:
- * 
+ *
  * The above copyright notice and this permission notice shall be included in
  * all copies or substantial portions of the Software.
- * 
+ *
  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
@@ -29,7 +29,7 @@ using System;
 using System.Collections.Generic;
 using System.Text;
 
-namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet
+namespace OpenSim.Region.PhysicsModules.ConvexDecompositionDotNet
 {
     public static class Concavity
     {
diff --git a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/ConvexBuilder.cs b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/ConvexBuilder.cs
index 70c3a2b..810e95d 100644
--- a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/ConvexBuilder.cs
+++ b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/ConvexBuilder.cs
@@ -1,21 +1,21 @@
 /* The MIT License
- * 
+ *
  * Copyright (c) 2010 Intel Corporation.
  * All rights reserved.
  *
- * Based on the convexdecomposition library from 
+ * Based on the convexdecomposition library from
  * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax.
- * 
+ *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
  * in the Software without restriction, including without limitation the rights
  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
  * copies of the Software, and to permit persons to whom the Software is
  * furnished to do so, subject to the following conditions:
- * 
+ *
  * The above copyright notice and this permission notice shall be included in
  * all copies or substantial portions of the Software.
- * 
+ *
  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
@@ -29,7 +29,7 @@ using System;
 using System.Collections.Generic;
 using System.Diagnostics;
 
-namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet
+namespace OpenSim.Region.PhysicsModules.ConvexDecompositionDotNet
 {
     public class DecompDesc
     {
diff --git a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/ConvexDecomposition.cs b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/ConvexDecomposition.cs
index 5046bce..a382f9d 100644
--- a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/ConvexDecomposition.cs
+++ b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/ConvexDecomposition.cs
@@ -1,21 +1,21 @@
 /* The MIT License
- * 
+ *
  * Copyright (c) 2010 Intel Corporation.
  * All rights reserved.
  *
- * Based on the convexdecomposition library from 
+ * Based on the convexdecomposition library from
  * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax.
- * 
+ *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
  * in the Software without restriction, including without limitation the rights
  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
  * copies of the Software, and to permit persons to whom the Software is
  * furnished to do so, subject to the following conditions:
- * 
+ *
  * The above copyright notice and this permission notice shall be included in
  * all copies or substantial portions of the Software.
- * 
+ *
  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
@@ -29,7 +29,7 @@ using System;
 using System.Collections.Generic;
 using System.Diagnostics;
 
-namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet
+namespace OpenSim.Region.PhysicsModules.ConvexDecompositionDotNet
 {
     public delegate void ConvexDecompositionCallback(ConvexResult result);
 
diff --git a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/ConvexResult.cs b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/ConvexResult.cs
index 44e3e50..300fed3 100644
--- a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/ConvexResult.cs
+++ b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/ConvexResult.cs
@@ -1,21 +1,21 @@
 /* The MIT License
- * 
+ *
  * Copyright (c) 2010 Intel Corporation.
  * All rights reserved.
  *
- * Based on the convexdecomposition library from 
+ * Based on the convexdecomposition library from
  * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax.
- * 
+ *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
  * in the Software without restriction, including without limitation the rights
  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
  * copies of the Software, and to permit persons to whom the Software is
  * furnished to do so, subject to the following conditions:
- * 
+ *
  * The above copyright notice and this permission notice shall be included in
  * all copies or substantial portions of the Software.
- * 
+ *
  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
@@ -28,7 +28,7 @@
 using System;
 using System.Collections.Generic;
 
-namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet
+namespace OpenSim.Region.PhysicsModules.ConvexDecompositionDotNet
 {
     public class ConvexResult
     {
diff --git a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/HullClasses.cs b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/HullClasses.cs
index 8a0164e..73fb252 100644
--- a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/HullClasses.cs
+++ b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/HullClasses.cs
@@ -1,21 +1,21 @@
 /* The MIT License
- * 
+ *
  * Copyright (c) 2010 Intel Corporation.
  * All rights reserved.
  *
- * Based on the convexdecomposition library from 
+ * Based on the convexdecomposition library from
  * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax.
- * 
+ *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
  * in the Software without restriction, including without limitation the rights
  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
  * copies of the Software, and to permit persons to whom the Software is
  * furnished to do so, subject to the following conditions:
- * 
+ *
  * The above copyright notice and this permission notice shall be included in
  * all copies or substantial portions of the Software.
- * 
+ *
  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
@@ -28,7 +28,7 @@
 using System;
 using System.Collections.Generic;
 
-namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet
+namespace OpenSim.Region.PhysicsModules.ConvexDecompositionDotNet
 {
     public class HullResult
     {
diff --git a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/HullTriangle.cs b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/HullTriangle.cs
index d3f0052..6e73fd5 100644
--- a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/HullTriangle.cs
+++ b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/HullTriangle.cs
@@ -1,21 +1,21 @@
 /* The MIT License
- * 
+ *
  * Copyright (c) 2010 Intel Corporation.
  * All rights reserved.
  *
- * Based on the convexdecomposition library from 
+ * Based on the convexdecomposition library from
  * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax.
- * 
+ *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
  * in the Software without restriction, including without limitation the rights
  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
  * copies of the Software, and to permit persons to whom the Software is
  * furnished to do so, subject to the following conditions:
- * 
+ *
  * The above copyright notice and this permission notice shall be included in
  * all copies or substantial portions of the Software.
- * 
+ *
  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
@@ -29,7 +29,7 @@ using System;
 using System.Collections.Generic;
 using System.Diagnostics;
 
-namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet
+namespace OpenSim.Region.PhysicsModules.ConvexDecompositionDotNet
 {
     public class HullTriangle : int3
     {
diff --git a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/HullUtils.cs b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/HullUtils.cs
index 3903254..cf7161a 100644
--- a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/HullUtils.cs
+++ b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/HullUtils.cs
@@ -1,21 +1,21 @@
 /* The MIT License
- * 
+ *
  * Copyright (c) 2010 Intel Corporation.
  * All rights reserved.
  *
- * Based on the convexdecomposition library from 
+ * Based on the convexdecomposition library from
  * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax.
- * 
+ *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
  * in the Software without restriction, including without limitation the rights
  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
  * copies of the Software, and to permit persons to whom the Software is
  * furnished to do so, subject to the following conditions:
- * 
+ *
  * The above copyright notice and this permission notice shall be included in
  * all copies or substantial portions of the Software.
- * 
+ *
  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
@@ -29,7 +29,7 @@ using System;
 using System.Collections.Generic;
 using System.Diagnostics;
 
-namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet
+namespace OpenSim.Region.PhysicsModules.ConvexDecompositionDotNet
 {
     public static class HullUtils
     {
@@ -763,7 +763,7 @@ namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet
                     }
                     else if ((vertflag[edge0.v].planetest | vertflag[edge1.v].planetest) == (0))
                     {
-                        // both endpoints coplanar 
+                        // both endpoints coplanar
                         // must check a 3rd point to see if UNDER
                         int e2 = e1 + 1;
                         if (e2 >= convex.edges.Count || convex.edges[e2].p != currentplane)
@@ -792,7 +792,7 @@ namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet
                     }
                     else if (vertflag[edge0.v].planetest == (1) && vertflag[edge1.v].planetest == (2))
                     {
-                        // first is under 2nd is over 
+                        // first is under 2nd is over
 
                         edgeflag[e0].undermap = (short)under_edge_count;
                         tmpunderedges[under_edge_count].v = vertflag[edge0.v].undermap;
@@ -815,7 +815,7 @@ namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet
                             vout = vertcountunder++;
                         }
                         under_edge_count++;
-                        /// hmmm something to think about: i might be able to output this edge regarless of 
+                        /// hmmm something to think about: i might be able to output this edge regarless of
                         // wheter or not we know v-in yet.  ok i;ll try this now:
                         tmpunderedges[under_edge_count].v = (byte)vout;
                         tmpunderedges[under_edge_count].p = (byte)underplanescount;
@@ -834,7 +834,7 @@ namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet
                     }
                     else if (vertflag[edge0.v].planetest == (0) && vertflag[edge1.v].planetest == (2))
                     {
-                        // first is coplanar 2nd is over 
+                        // first is coplanar 2nd is over
 
                         edgeflag[e0].undermap = -1;
                         vout = vertflag[edge0.v].undermap;
@@ -858,7 +858,7 @@ namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet
                     }
                     else if (vertflag[edge0.v].planetest == (2) && vertflag[edge1.v].planetest == (1))
                     {
-                        // first is over next is under 
+                        // first is over next is under
                         // new vertex!!!
                         Debug.Assert(vin == -1);
                         if (e0 < edge0.ea)
@@ -901,7 +901,7 @@ namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet
                     }
                     else if (vertflag[edge0.v].planetest == (2) && vertflag[edge1.v].planetest == (0))
                     {
-                        // first is over next is coplanar 
+                        // first is over next is coplanar
 
                         edgeflag[e0].undermap = -1;
                         vin = vertflag[edge1.v].undermap;
@@ -1074,14 +1074,27 @@ namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet
         public static int maxdirfiltered(List<float3> p, int count, float3 dir, byte[] allow)
         {
             //Debug.Assert(count != 0);
-            int m = 0;
-            float currDotm = float3.dot(p[0], dir);
+            int m = -1;
+            float currDotm = 0;
             float currDoti;
 
-            while (allow[m] == 0)
-                m++;
+            for (int i = 0; i < count; i++)
+            {
+                if (allow[i] != 0)
+                {
+                    currDotm = float3.dot(p[i], dir);
+                    m = i;
+                    break;
+               }
+            }
 
-            for (int i = 1; i < count; i++)
+            if(m == -1)
+            {
+                Debug.Assert(false);
+                return m;
+            }
+
+            for (int i = m + 1; i < count; i++)
             {
                 if (allow[i] != 0)
                 {
@@ -1093,7 +1106,8 @@ namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet
                     }
                 }
             }
-            //Debug.Assert(m != -1);
+
+//            Debug.Assert(m != -1);
             return m;
         }
 
@@ -1112,8 +1126,8 @@ namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet
                 {
                     int mb;
                     {
-                        float s = (float)Math.Sin((3.14159264f / 180.0f) * (x));
-                        float c = (float)Math.Cos((3.14159264f / 180.0f) * (x));
+                        float s = (float)Math.Sin(0.01745329f * x);
+                        float c = (float)Math.Cos(0.01745329f * x);
                         mb = maxdirfiltered(p, count, dir + (u * s + v * c) * 0.025f, allow);
                     }
                     if (ma == m && mb == m)
@@ -1126,8 +1140,8 @@ namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet
                         int mc = ma;
                         for (float xx = x - 40.0f; xx <= x; xx += 5.0f)
                         {
-                            float s = (float)Math.Sin((3.14159264f / 180.0f) * (xx));
-                            float c = (float)Math.Cos((3.14159264f / 180.0f) * (xx));
+                            float s = (float)Math.Sin(0.01745329f * xx);
+                            float c = (float)Math.Cos(0.01745329f * xx);
                             int md = maxdirfiltered(p, count, dir + (u * s + v * c) * 0.025f, allow);
                             if (mc == m && md == m)
                             {
@@ -1176,7 +1190,7 @@ namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet
             Debug.Assert(!(p0 == p1 || p0 == p2 || p0 == p3 || p1 == p2 || p1 == p3 || p2 == p3));
             if (float3.dot(verts[p3] - verts[p0], float3.cross(verts[p1] - verts[p0], verts[p2] - verts[p0])) < 0)
             {
-                Swap(ref p2, ref p3);
+                return new int4(p0, p1, p3, p2);
             }
             return new int4(p0, p1, p2, p3);
         }
@@ -1207,12 +1221,12 @@ namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet
             int j;
             float3 bmin = new float3(verts[0]);
             float3 bmax = new float3(verts[0]);
-            List<int> isextreme = new List<int>(verts.Count);
+            byte[] isextreme = new byte[verts.Count];
             byte[] allow = new byte[verts.Count];
             for (j = 0; j < verts.Count; j++)
             {
                 allow[j] = 1;
-                isextreme.Add(0);
+                isextreme[j] = 0;
                 bmin = float3.VectorMin(bmin, verts[j]);
                 bmax = float3.VectorMax(bmax, verts[j]);
             }
@@ -1526,6 +1540,19 @@ namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet
             }
         }
 
+        public static bool ComputeHull(List<float3> vertices, out List<int> indices)
+        {
+            List<HullTriangle> tris = new List<HullTriangle>();
+
+            bool ret = calchull(vertices, out indices, 0, tris);
+            if (ret == false)
+            {
+                indices = new List<int>();
+                return false;
+            }
+            return true;
+        }
+
         private static bool CleanupVertices(List<float3> svertices, out List<float3> vertices, float normalepsilon, out float3 scale)
         {
             const float EPSILON = 0.000001f;
@@ -1609,7 +1636,7 @@ namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet
                 addPoint(ref vcount, vertices, x2, y2, z2);
                 addPoint(ref vcount, vertices, x1, y2, z2);
 
-                return true; // return cube     
+                return true; // return cube
             }
             else
             {
diff --git a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/Plane.cs b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/Plane.cs
index da9ae0c..0f10e5f 100644
--- a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/Plane.cs
+++ b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/Plane.cs
@@ -1,21 +1,21 @@
 /* The MIT License
- * 
+ *
  * Copyright (c) 2010 Intel Corporation.
  * All rights reserved.
  *
- * Based on the convexdecomposition library from 
+ * Based on the convexdecomposition library from
  * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax.
- * 
+ *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
  * in the Software without restriction, including without limitation the rights
  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
  * copies of the Software, and to permit persons to whom the Software is
  * furnished to do so, subject to the following conditions:
- * 
+ *
  * The above copyright notice and this permission notice shall be included in
  * all copies or substantial portions of the Software.
- * 
+ *
  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
@@ -27,7 +27,7 @@
 
 using System;
 
-namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet
+namespace OpenSim.Region.PhysicsModules.ConvexDecompositionDotNet
 {
     public class Plane
     {
diff --git a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/PlaneTri.cs b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/PlaneTri.cs
index 42f7a22..5915c29 100644
--- a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/PlaneTri.cs
+++ b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/PlaneTri.cs
@@ -1,21 +1,21 @@
 /* The MIT License
- * 
+ *
  * Copyright (c) 2010 Intel Corporation.
  * All rights reserved.
  *
- * Based on the convexdecomposition library from 
+ * Based on the convexdecomposition library from
  * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax.
- * 
+ *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
  * in the Software without restriction, including without limitation the rights
  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
  * copies of the Software, and to permit persons to whom the Software is
  * furnished to do so, subject to the following conditions:
- * 
+ *
  * The above copyright notice and this permission notice shall be included in
  * all copies or substantial portions of the Software.
- * 
+ *
  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
@@ -29,7 +29,7 @@ using System;
 using System.Collections.Generic;
 using System.Diagnostics;
 
-namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet
+namespace OpenSim.Region.PhysicsModules.ConvexDecompositionDotNet
 {
     public enum PlaneTriResult : int
     {
diff --git a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/Properties/AssemblyInfo.cs b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/Properties/AssemblyInfo.cs
index b9cd6f5..9b418d6 100644
--- a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/Properties/AssemblyInfo.cs
+++ b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/Properties/AssemblyInfo.cs
@@ -2,7 +2,7 @@
 using System.Runtime.CompilerServices;
 using System.Runtime.InteropServices;
 
-// General Information about an assembly is controlled through the following 
+// General Information about an assembly is controlled through the following
 // set of attributes. Change these attribute values to modify the information
 // associated with an assembly.
 [assembly: AssemblyTitle("ConvexDecompositionDotNet")]
@@ -14,8 +14,8 @@ using System.Runtime.InteropServices;
 [assembly: AssemblyTrademark("")]
 [assembly: AssemblyCulture("")]
 
-// Setting ComVisible to false makes the types in this assembly not visible 
-// to COM components.  If you need to access a type in this assembly from 
+// Setting ComVisible to false makes the types in this assembly not visible
+// to COM components.  If you need to access a type in this assembly from
 // COM, set the ComVisible attribute to true on that type.
 [assembly: ComVisible(false)]
 
@@ -25,12 +25,12 @@ using System.Runtime.InteropServices;
 // Version information for an assembly consists of the following four values:
 //
 //      Major Version
-//      Minor Version 
+//      Minor Version
 //      Build Number
 //      Revision
 //
-// You can specify all the values or you can default the Build and Revision Numbers 
+// You can specify all the values or you can default the Build and Revision Numbers
 // by using the '*' as shown below:
 // [assembly: AssemblyVersion("1.0.*")]
-[assembly: AssemblyVersion("0.8.3.*")]
+[assembly: AssemblyVersion(OpenSim.VersionInfo.AssemblyVersionNumber)]
 
diff --git a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/Quaternion.cs b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/Quaternion.cs
index 045f620..571677d 100644
--- a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/Quaternion.cs
+++ b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/Quaternion.cs
@@ -1,21 +1,21 @@
 /* The MIT License
- * 
+ *
  * Copyright (c) 2010 Intel Corporation.
  * All rights reserved.
  *
- * Based on the convexdecomposition library from 
+ * Based on the convexdecomposition library from
  * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax.
- * 
+ *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
  * in the Software without restriction, including without limitation the rights
  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
  * copies of the Software, and to permit persons to whom the Software is
  * furnished to do so, subject to the following conditions:
- * 
+ *
  * The above copyright notice and this permission notice shall be included in
  * all copies or substantial portions of the Software.
- * 
+ *
  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
@@ -27,7 +27,7 @@
 
 using System;
 
-namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet
+namespace OpenSim.Region.PhysicsModules.ConvexDecompositionDotNet
 {
     public class Quaternion : float4
     {
@@ -126,9 +126,9 @@ namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet
         }
 
         public static Quaternion operator *(Quaternion a, float b)
-            {
-                    return new Quaternion(a.x *b, a.y *b, a.z *b, a.w *b);
-            }
+        {
+            return new Quaternion(a.x *b, a.y *b, a.z *b, a.w *b);
+        }
 
         public static Quaternion normalize(Quaternion a)
         {
diff --git a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/SplitPlane.cs b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/SplitPlane.cs
index 9f56bc5..d8c10c7 100644
--- a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/SplitPlane.cs
+++ b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/SplitPlane.cs
@@ -1,21 +1,21 @@
 /* The MIT License
- * 
+ *
  * Copyright (c) 2010 Intel Corporation.
  * All rights reserved.
  *
- * Based on the convexdecomposition library from 
+ * Based on the convexdecomposition library from
  * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax.
- * 
+ *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
  * in the Software without restriction, including without limitation the rights
  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
  * copies of the Software, and to permit persons to whom the Software is
  * furnished to do so, subject to the following conditions:
- * 
+ *
  * The above copyright notice and this permission notice shall be included in
  * all copies or substantial portions of the Software.
- * 
+ *
  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
@@ -28,7 +28,7 @@
 using System;
 using System.Collections.Generic;
 
-namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet
+namespace OpenSim.Region.PhysicsModules.ConvexDecompositionDotNet
 {
     public class Rect3d
     {
diff --git a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/VertexLookup.cs b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/VertexLookup.cs
index bfe11e5..915bec6 100644
--- a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/VertexLookup.cs
+++ b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/VertexLookup.cs
@@ -1,21 +1,21 @@
 /* The MIT License
- * 
+ *
  * Copyright (c) 2010 Intel Corporation.
  * All rights reserved.
  *
- * Based on the convexdecomposition library from 
+ * Based on the convexdecomposition library from
  * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax.
- * 
+ *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
  * in the Software without restriction, including without limitation the rights
  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
  * copies of the Software, and to permit persons to whom the Software is
  * furnished to do so, subject to the following conditions:
- * 
+ *
  * The above copyright notice and this permission notice shall be included in
  * all copies or substantial portions of the Software.
- * 
+ *
  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
@@ -28,7 +28,7 @@
 using System;
 using System.Collections.Generic;
 
-namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet
+namespace OpenSim.Region.PhysicsModules.ConvexDecompositionDotNet
 {
     public class VertexPool
     {
diff --git a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/float2.cs b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/float2.cs
index e7358c1..c2c8940 100644
--- a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/float2.cs
+++ b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/float2.cs
@@ -1,21 +1,21 @@
 /* The MIT License
- * 
+ *
  * Copyright (c) 2010 Intel Corporation.
  * All rights reserved.
  *
- * Based on the convexdecomposition library from 
+ * Based on the convexdecomposition library from
  * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax.
- * 
+ *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
  * in the Software without restriction, including without limitation the rights
  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
  * copies of the Software, and to permit persons to whom the Software is
  * furnished to do so, subject to the following conditions:
- * 
+ *
  * The above copyright notice and this permission notice shall be included in
  * all copies or substantial portions of the Software.
- * 
+ *
  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
@@ -27,7 +27,7 @@
 
 using System;
 
-namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet
+namespace OpenSim.Region.PhysicsModules.ConvexDecompositionDotNet
 {
     public class float2
     {
diff --git a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/float3.cs b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/float3.cs
index fde9b32..1ea003b 100644
--- a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/float3.cs
+++ b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/float3.cs
@@ -1,21 +1,21 @@
 /* The MIT License
- * 
+ *
  * Copyright (c) 2010 Intel Corporation.
  * All rights reserved.
  *
- * Based on the convexdecomposition library from 
+ * Based on the convexdecomposition library from
  * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax.
- * 
+ *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
  * in the Software without restriction, including without limitation the rights
  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
  * copies of the Software, and to permit persons to whom the Software is
  * furnished to do so, subject to the following conditions:
- * 
+ *
  * The above copyright notice and this permission notice shall be included in
  * all copies or substantial portions of the Software.
- * 
+ *
  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
@@ -27,7 +27,7 @@
 
 using System;
 
-namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet
+namespace OpenSim.Region.PhysicsModules.ConvexDecompositionDotNet
 {
     public class float3 : IEquatable<float3>
     {
diff --git a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/float3x3.cs b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/float3x3.cs
index c420fde..4b6cd5d 100644
--- a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/float3x3.cs
+++ b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/float3x3.cs
@@ -1,21 +1,21 @@
 /* The MIT License
- * 
+ *
  * Copyright (c) 2010 Intel Corporation.
  * All rights reserved.
  *
- * Based on the convexdecomposition library from 
+ * Based on the convexdecomposition library from
  * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax.
- * 
+ *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
  * in the Software without restriction, including without limitation the rights
  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
  * copies of the Software, and to permit persons to whom the Software is
  * furnished to do so, subject to the following conditions:
- * 
+ *
  * The above copyright notice and this permission notice shall be included in
  * all copies or substantial portions of the Software.
- * 
+ *
  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
@@ -29,7 +29,7 @@ using System;
 using System.Collections.Generic;
 using System.Diagnostics;
 
-namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet
+namespace OpenSim.Region.PhysicsModules.ConvexDecompositionDotNet
 {
     public class float3x3
     {
diff --git a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/float4.cs b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/float4.cs
index b2b6fd3..1f2ab02 100644
--- a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/float4.cs
+++ b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/float4.cs
@@ -1,21 +1,21 @@
 /* The MIT License
- * 
+ *
  * Copyright (c) 2010 Intel Corporation.
  * All rights reserved.
  *
- * Based on the convexdecomposition library from 
+ * Based on the convexdecomposition library from
  * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax.
- * 
+ *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
  * in the Software without restriction, including without limitation the rights
  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
  * copies of the Software, and to permit persons to whom the Software is
  * furnished to do so, subject to the following conditions:
- * 
+ *
  * The above copyright notice and this permission notice shall be included in
  * all copies or substantial portions of the Software.
- * 
+ *
  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
@@ -27,7 +27,7 @@
 
 using System;
 
-namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet
+namespace OpenSim.Region.PhysicsModules.ConvexDecompositionDotNet
 {
     public class float4
     {
diff --git a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/float4x4.cs b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/float4x4.cs
index 087eba7..85a8cf1 100644
--- a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/float4x4.cs
+++ b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/float4x4.cs
@@ -1,21 +1,21 @@
 /* The MIT License
- * 
+ *
  * Copyright (c) 2010 Intel Corporation.
  * All rights reserved.
  *
- * Based on the convexdecomposition library from 
+ * Based on the convexdecomposition library from
  * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax.
- * 
+ *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
  * in the Software without restriction, including without limitation the rights
  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
  * copies of the Software, and to permit persons to whom the Software is
  * furnished to do so, subject to the following conditions:
- * 
+ *
  * The above copyright notice and this permission notice shall be included in
  * all copies or substantial portions of the Software.
- * 
+ *
  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
@@ -30,7 +30,7 @@ using System.Collections.Generic;
 using System.Linq;
 using System.Text;
 
-namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet
+namespace OpenSim.Region.PhysicsModules.ConvexDecompositionDotNet
 {
     public class float4x4
     {
@@ -127,88 +127,88 @@ namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet
         }
 
         public static float4x4 Inverse(float4x4 m)
-            {
-                    float4x4 d = new float4x4();
-                    //float dst = d.x.x;
-                    float[] tmp = new float[12]; // temp array for pairs
-                    float[] src = new float[16]; // array of transpose source matrix
-                    float det; // determinant
-                    // transpose matrix
-                    for (int i = 0; i < 4; i++)
-                    {
-                            src[i] = m[i].x;
-                            src[i + 4] = m[i].y;
-                            src[i + 8] = m[i].z;
-                            src[i + 12] = m[i].w;
-                    }
-                    // calculate pairs for first 8 elements (cofactors)
-                    tmp[0] = src[10] * src[15];
-                    tmp[1] = src[11] * src[14];
-                    tmp[2] = src[9] * src[15];
-                    tmp[3] = src[11] * src[13];
-                    tmp[4] = src[9] * src[14];
-                    tmp[5] = src[10] * src[13];
-                    tmp[6] = src[8] * src[15];
-                    tmp[7] = src[11] * src[12];
-                    tmp[8] = src[8] * src[14];
-                    tmp[9] = src[10] * src[12];
-                    tmp[10] = src[8] * src[13];
-                    tmp[11] = src[9] * src[12];
-                    // calculate first 8 elements (cofactors)
-                    d.x.x = tmp[0]*src[5] + tmp[3]*src[6] + tmp[4]*src[7];
-                    d.x.x -= tmp[1]*src[5] + tmp[2]*src[6] + tmp[5]*src[7];
-                    d.x.y = tmp[1]*src[4] + tmp[6]*src[6] + tmp[9]*src[7];
-                    d.x.y -= tmp[0]*src[4] + tmp[7]*src[6] + tmp[8]*src[7];
-                    d.x.z = tmp[2]*src[4] + tmp[7]*src[5] + tmp[10]*src[7];
-                    d.x.z -= tmp[3]*src[4] + tmp[6]*src[5] + tmp[11]*src[7];
-                    d.x.w = tmp[5]*src[4] + tmp[8]*src[5] + tmp[11]*src[6];
-                    d.x.w -= tmp[4]*src[4] + tmp[9]*src[5] + tmp[10]*src[6];
-                    d.y.x = tmp[1]*src[1] + tmp[2]*src[2] + tmp[5]*src[3];
-                    d.y.x -= tmp[0]*src[1] + tmp[3]*src[2] + tmp[4]*src[3];
-                    d.y.y = tmp[0]*src[0] + tmp[7]*src[2] + tmp[8]*src[3];
-                    d.y.y -= tmp[1]*src[0] + tmp[6]*src[2] + tmp[9]*src[3];
-                    d.y.z = tmp[3]*src[0] + tmp[6]*src[1] + tmp[11]*src[3];
-                    d.y.z -= tmp[2]*src[0] + tmp[7]*src[1] + tmp[10]*src[3];
-                    d.y.w = tmp[4]*src[0] + tmp[9]*src[1] + tmp[10]*src[2];
-                    d.y.w -= tmp[5]*src[0] + tmp[8]*src[1] + tmp[11]*src[2];
-                    // calculate pairs for second 8 elements (cofactors)
-                    tmp[0] = src[2]*src[7];
-                    tmp[1] = src[3]*src[6];
-                    tmp[2] = src[1]*src[7];
-                    tmp[3] = src[3]*src[5];
-                    tmp[4] = src[1]*src[6];
-                    tmp[5] = src[2]*src[5];
-                    tmp[6] = src[0]*src[7];
-                    tmp[7] = src[3]*src[4];
-                    tmp[8] = src[0]*src[6];
-                    tmp[9] = src[2]*src[4];
-                    tmp[10] = src[0]*src[5];
-                    tmp[11] = src[1]*src[4];
-                    // calculate second 8 elements (cofactors)
-                    d.z.x = tmp[0]*src[13] + tmp[3]*src[14] + tmp[4]*src[15];
-                    d.z.x -= tmp[1]*src[13] + tmp[2]*src[14] + tmp[5]*src[15];
-                    d.z.y = tmp[1]*src[12] + tmp[6]*src[14] + tmp[9]*src[15];
-                    d.z.y -= tmp[0]*src[12] + tmp[7]*src[14] + tmp[8]*src[15];
-                    d.z.z = tmp[2]*src[12] + tmp[7]*src[13] + tmp[10]*src[15];
-                    d.z.z -= tmp[3]*src[12] + tmp[6]*src[13] + tmp[11]*src[15];
-                    d.z.w = tmp[5]*src[12] + tmp[8]*src[13] + tmp[11]*src[14];
-                    d.z.w-= tmp[4]*src[12] + tmp[9]*src[13] + tmp[10]*src[14];
-                    d.w.x = tmp[2]*src[10] + tmp[5]*src[11] + tmp[1]*src[9];
-                    d.w.x-= tmp[4]*src[11] + tmp[0]*src[9] + tmp[3]*src[10];
-                    d.w.y = tmp[8]*src[11] + tmp[0]*src[8] + tmp[7]*src[10];
-                    d.w.y-= tmp[6]*src[10] + tmp[9]*src[11] + tmp[1]*src[8];
-                    d.w.z = tmp[6]*src[9] + tmp[11]*src[11] + tmp[3]*src[8];
-                    d.w.z-= tmp[10]*src[11] + tmp[2]*src[8] + tmp[7]*src[9];
-                    d.w.w = tmp[10]*src[10] + tmp[4]*src[8] + tmp[9]*src[9];
-                    d.w.w-= tmp[8]*src[9] + tmp[11]*src[10] + tmp[5]*src[8];
-                    // calculate determinant
+        {
+            float4x4 d = new float4x4();
+            //float dst = d.x.x;
+            float[] tmp = new float[12]; // temp array for pairs
+            float[] src = new float[16]; // array of transpose source matrix
+            float det; // determinant
+            // transpose matrix
+            for (int i = 0; i < 4; i++)
+            {
+                src[i] = m[i].x;
+                src[i + 4] = m[i].y;
+                src[i + 8] = m[i].z;
+                src[i + 12] = m[i].w;
+            }
+            // calculate pairs for first 8 elements (cofactors)
+            tmp[0] = src[10] * src[15];
+            tmp[1] = src[11] * src[14];
+            tmp[2] = src[9] * src[15];
+            tmp[3] = src[11] * src[13];
+            tmp[4] = src[9] * src[14];
+            tmp[5] = src[10] * src[13];
+            tmp[6] = src[8] * src[15];
+            tmp[7] = src[11] * src[12];
+            tmp[8] = src[8] * src[14];
+            tmp[9] = src[10] * src[12];
+            tmp[10] = src[8] * src[13];
+            tmp[11] = src[9] * src[12];
+            // calculate first 8 elements (cofactors)
+            d.x.x = tmp[0]*src[5] + tmp[3]*src[6] + tmp[4]*src[7];
+            d.x.x -= tmp[1]*src[5] + tmp[2]*src[6] + tmp[5]*src[7];
+            d.x.y = tmp[1]*src[4] + tmp[6]*src[6] + tmp[9]*src[7];
+            d.x.y -= tmp[0]*src[4] + tmp[7]*src[6] + tmp[8]*src[7];
+            d.x.z = tmp[2]*src[4] + tmp[7]*src[5] + tmp[10]*src[7];
+            d.x.z -= tmp[3]*src[4] + tmp[6]*src[5] + tmp[11]*src[7];
+            d.x.w = tmp[5]*src[4] + tmp[8]*src[5] + tmp[11]*src[6];
+            d.x.w -= tmp[4]*src[4] + tmp[9]*src[5] + tmp[10]*src[6];
+            d.y.x = tmp[1]*src[1] + tmp[2]*src[2] + tmp[5]*src[3];
+            d.y.x -= tmp[0]*src[1] + tmp[3]*src[2] + tmp[4]*src[3];
+            d.y.y = tmp[0]*src[0] + tmp[7]*src[2] + tmp[8]*src[3];
+            d.y.y -= tmp[1]*src[0] + tmp[6]*src[2] + tmp[9]*src[3];
+            d.y.z = tmp[3]*src[0] + tmp[6]*src[1] + tmp[11]*src[3];
+            d.y.z -= tmp[2]*src[0] + tmp[7]*src[1] + tmp[10]*src[3];
+            d.y.w = tmp[4]*src[0] + tmp[9]*src[1] + tmp[10]*src[2];
+            d.y.w -= tmp[5]*src[0] + tmp[8]*src[1] + tmp[11]*src[2];
+            // calculate pairs for second 8 elements (cofactors)
+            tmp[0] = src[2]*src[7];
+            tmp[1] = src[3]*src[6];
+            tmp[2] = src[1]*src[7];
+            tmp[3] = src[3]*src[5];
+            tmp[4] = src[1]*src[6];
+            tmp[5] = src[2]*src[5];
+            tmp[6] = src[0]*src[7];
+            tmp[7] = src[3]*src[4];
+            tmp[8] = src[0]*src[6];
+            tmp[9] = src[2]*src[4];
+            tmp[10] = src[0]*src[5];
+            tmp[11] = src[1]*src[4];
+            // calculate second 8 elements (cofactors)
+            d.z.x = tmp[0]*src[13] + tmp[3]*src[14] + tmp[4]*src[15];
+            d.z.x -= tmp[1]*src[13] + tmp[2]*src[14] + tmp[5]*src[15];
+            d.z.y = tmp[1]*src[12] + tmp[6]*src[14] + tmp[9]*src[15];
+            d.z.y -= tmp[0]*src[12] + tmp[7]*src[14] + tmp[8]*src[15];
+            d.z.z = tmp[2]*src[12] + tmp[7]*src[13] + tmp[10]*src[15];
+            d.z.z -= tmp[3]*src[12] + tmp[6]*src[13] + tmp[11]*src[15];
+            d.z.w = tmp[5]*src[12] + tmp[8]*src[13] + tmp[11]*src[14];
+            d.z.w-= tmp[4]*src[12] + tmp[9]*src[13] + tmp[10]*src[14];
+            d.w.x = tmp[2]*src[10] + tmp[5]*src[11] + tmp[1]*src[9];
+            d.w.x-= tmp[4]*src[11] + tmp[0]*src[9] + tmp[3]*src[10];
+            d.w.y = tmp[8]*src[11] + tmp[0]*src[8] + tmp[7]*src[10];
+            d.w.y-= tmp[6]*src[10] + tmp[9]*src[11] + tmp[1]*src[8];
+            d.w.z = tmp[6]*src[9] + tmp[11]*src[11] + tmp[3]*src[8];
+            d.w.z-= tmp[10]*src[11] + tmp[2]*src[8] + tmp[7]*src[9];
+            d.w.w = tmp[10]*src[10] + tmp[4]*src[8] + tmp[9]*src[9];
+            d.w.w-= tmp[8]*src[9] + tmp[11]*src[10] + tmp[5]*src[8];
+            // calculate determinant
             det = src[0] * d.x.x + src[1] * d.x.y + src[2] * d.x.z + src[3] * d.x.w;
-                    // calculate matrix inverse
-                    det = 1/det;
+            // calculate matrix inverse
+            det = 1/det;
             for (int j = 0; j < 4; j++)
                 d[j] *= det;
-                    return d;
-            }
+            return d;
+        }
 
         public static float4x4 MatrixRigidInverse(float4x4 m)
         {
diff --git a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/int3.cs b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/int3.cs
index 90624eb..e7bb6e7 100644
--- a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/int3.cs
+++ b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/int3.cs
@@ -1,21 +1,21 @@
 /* The MIT License
- * 
+ *
  * Copyright (c) 2010 Intel Corporation.
  * All rights reserved.
  *
- * Based on the convexdecomposition library from 
+ * Based on the convexdecomposition library from
  * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax.
- * 
+ *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
  * in the Software without restriction, including without limitation the rights
  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
  * copies of the Software, and to permit persons to whom the Software is
  * furnished to do so, subject to the following conditions:
- * 
+ *
  * The above copyright notice and this permission notice shall be included in
  * all copies or substantial portions of the Software.
- * 
+ *
  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
@@ -27,7 +27,7 @@
 
 using System;
 
-namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet
+namespace OpenSim.Region.PhysicsModules.ConvexDecompositionDotNet
 {
     public class int3
     {
diff --git a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/int4.cs b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/int4.cs
index e9320c0..9950be2 100644
--- a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/int4.cs
+++ b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/int4.cs
@@ -1,21 +1,21 @@
 /* The MIT License
- * 
+ *
  * Copyright (c) 2010 Intel Corporation.
  * All rights reserved.
  *
- * Based on the convexdecomposition library from 
+ * Based on the convexdecomposition library from
  * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax.
- * 
+ *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
  * in the Software without restriction, including without limitation the rights
  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
  * copies of the Software, and to permit persons to whom the Software is
  * furnished to do so, subject to the following conditions:
- * 
+ *
  * The above copyright notice and this permission notice shall be included in
  * all copies or substantial portions of the Software.
- * 
+ *
  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
@@ -27,7 +27,7 @@
 
 using System;
 
-namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet
+namespace OpenSim.Region.PhysicsModules.ConvexDecompositionDotNet
 {
     public class int4
     {
diff --git a/OpenSim/Region/PhysicsModules/Meshing/Meshmerizer/HelperTypes.cs b/OpenSim/Region/PhysicsModules/Meshing/Meshmerizer/HelperTypes.cs
index 34a925d..a0e3b7f 100644
--- a/OpenSim/Region/PhysicsModules/Meshing/Meshmerizer/HelperTypes.cs
+++ b/OpenSim/Region/PhysicsModules/Meshing/Meshmerizer/HelperTypes.cs
@@ -31,7 +31,7 @@ using System.Diagnostics;
 using System.Globalization;
 using OpenMetaverse;
 using OpenSim.Region.PhysicsModules.SharedBase;
-using OpenSim.Region.PhysicsModules.Meshing;
+using OpenSim.Region.PhysicsModule.Meshing;
 
 public class Vertex : IComparable<Vertex>
 {
diff --git a/OpenSim/Region/PhysicsModules/Meshing/Meshmerizer/Mesh.cs b/OpenSim/Region/PhysicsModules/Meshing/Meshmerizer/Mesh.cs
index bf397ee..42ba37e 100644
--- a/OpenSim/Region/PhysicsModules/Meshing/Meshmerizer/Mesh.cs
+++ b/OpenSim/Region/PhysicsModules/Meshing/Meshmerizer/Mesh.cs
@@ -33,7 +33,7 @@ using OpenSim.Region.PhysicsModules.SharedBase;
 using PrimMesher;
 using OpenMetaverse;
 
-namespace OpenSim.Region.PhysicsModules.Meshing
+namespace OpenSim.Region.PhysicsModule.Meshing
 {
     public class Mesh : IMesh
     {
@@ -46,11 +46,36 @@ namespace OpenSim.Region.PhysicsModules.Meshing
         IntPtr m_indicesPtr = IntPtr.Zero;
         int m_indexCount = 0;
         public float[] m_normals;
+        Vector3 _centroid;
+        int _centroidDiv;
+
+        private class vertexcomp : IEqualityComparer<Vertex>
+        {
+            public bool Equals(Vertex v1, Vertex v2)
+            {
+                if (v1.X == v2.X && v1.Y == v2.Y && v1.Z == v2.Z)
+                    return true;
+                else
+                    return false;
+            }
+            public int GetHashCode(Vertex v)
+            {
+                int a = v.X.GetHashCode();
+                int b = v.Y.GetHashCode();
+                int c = v.Z.GetHashCode();
+                return (a << 16) ^ (b << 8) ^ c;
+            }
+
+        }
 
         public Mesh()
         {
-            m_vertices = new Dictionary<Vertex, int>();
+            vertexcomp vcomp = new vertexcomp();
+
+            m_vertices = new Dictionary<Vertex, int>(vcomp);
             m_triangles = new List<Triangle>();
+            _centroid = Vector3.Zero;
+            _centroidDiv = 0;
         }
 
         public Mesh Clone()
@@ -61,7 +86,8 @@ namespace OpenSim.Region.PhysicsModules.Meshing
             {
                 result.Add(new Triangle(t.v1.Clone(), t.v2.Clone(), t.v3.Clone()));
             }
-
+            result._centroid = _centroid;
+            result._centroidDiv = _centroidDiv;
             return result;
         }
 
@@ -71,15 +97,63 @@ namespace OpenSim.Region.PhysicsModules.Meshing
                 throw new NotSupportedException("Attempt to Add to a pinned Mesh");
             // If a vertex of the triangle is not yet in the vertices list,
             // add it and set its index to the current index count
+            // vertex == seems broken
+            // skip colapsed triangles
+            if ((triangle.v1.X == triangle.v2.X && triangle.v1.Y == triangle.v2.Y && triangle.v1.Z == triangle.v2.Z)
+                || (triangle.v1.X == triangle.v3.X && triangle.v1.Y == triangle.v3.Y && triangle.v1.Z == triangle.v3.Z)
+                || (triangle.v2.X == triangle.v3.X && triangle.v2.Y == triangle.v3.Y && triangle.v2.Z == triangle.v3.Z)
+                )
+            {
+                return;
+            }
+
+            if (m_vertices.Count == 0)
+            {
+                _centroidDiv = 0;
+                _centroid = Vector3.Zero;
+            }
+
             if (!m_vertices.ContainsKey(triangle.v1))
+            {
                 m_vertices[triangle.v1] = m_vertices.Count;
+                _centroid.X += triangle.v1.X;
+                _centroid.Y += triangle.v1.Y;
+                _centroid.Z += triangle.v1.Z;
+                _centroidDiv++;
+            }
             if (!m_vertices.ContainsKey(triangle.v2))
+            {
                 m_vertices[triangle.v2] = m_vertices.Count;
+                _centroid.X += triangle.v2.X;
+                _centroid.Y += triangle.v2.Y;
+                _centroid.Z += triangle.v2.Z;
+                _centroidDiv++;
+            }
             if (!m_vertices.ContainsKey(triangle.v3))
+            {
                 m_vertices[triangle.v3] = m_vertices.Count;
+                _centroid.X += triangle.v3.X;
+                _centroid.Y += triangle.v3.Y;
+                _centroid.Z += triangle.v3.Z;
+                _centroidDiv++;
+            }
             m_triangles.Add(triangle);
         }
 
+        public Vector3 GetCentroid()
+        {
+            if (_centroidDiv > 0)
+                return new Vector3(_centroid.X / _centroidDiv, _centroid.Y / _centroidDiv, _centroid.Z / _centroidDiv);
+            else
+                return Vector3.Zero;
+        }
+
+        // not functional
+        public Vector3 GetOBB()
+        {
+            return new Vector3(0.5f, 0.5f, 0.5f);
+        }
+
         public void CalcNormals()
         {
             int iTriangles = m_triangles.Count;
@@ -185,6 +259,7 @@ namespace OpenSim.Region.PhysicsModules.Meshing
         public void getVertexListAsPtrToFloatArray(out IntPtr vertices, out int vertexStride, out int vertexCount)
         {
             // A vertex is 3 floats
+
             vertexStride = 3 * sizeof(float);
 
             // If there isn't an unmanaged array allocated yet, do it now
@@ -224,7 +299,7 @@ namespace OpenSim.Region.PhysicsModules.Meshing
         {
             if (m_pinnedIndex.IsAllocated)
                 return (int[])(m_pinnedIndex.Target);
-        
+
             int[] result = getIndexListAsInt();
             m_pinnedIndex = GCHandle.Alloc(result, GCHandleType.Pinned);
             // Inform the garbage collector of this unmanaged allocation so it can schedule
@@ -282,7 +357,7 @@ namespace OpenSim.Region.PhysicsModules.Meshing
         {
             if (m_pinnedIndex.IsAllocated || m_pinnedVertexes.IsAllocated || m_indicesPtr != IntPtr.Zero || m_verticesPtr != IntPtr.Zero)
                 throw new NotSupportedException("Attempt to Append to a pinned Mesh");
-        
+
             if (!(newMesh is Mesh))
                 return;
 
@@ -295,7 +370,7 @@ namespace OpenSim.Region.PhysicsModules.Meshing
         {
             if (m_pinnedIndex.IsAllocated || m_pinnedVertexes.IsAllocated || m_indicesPtr != IntPtr.Zero || m_verticesPtr != IntPtr.Zero)
                 throw new NotSupportedException("Attempt to TransformLinear a pinned Mesh");
-        
+
             foreach (Vertex v in m_vertices.Keys)
             {
                 if (v == null)
diff --git a/OpenSim/Region/PhysicsModules/Meshing/Meshmerizer/Meshmerizer.cs b/OpenSim/Region/PhysicsModules/Meshing/Meshmerizer/Meshmerizer.cs
index 3a944d1..0d4b6b9 100644
--- a/OpenSim/Region/PhysicsModules/Meshing/Meshmerizer/Meshmerizer.cs
+++ b/OpenSim/Region/PhysicsModules/Meshing/Meshmerizer/Meshmerizer.cs
@@ -44,7 +44,7 @@ using log4net;
 using Nini.Config;
 using Mono.Addins;
 
-namespace OpenSim.Region.PhysicsModules.Meshing
+namespace OpenSim.Region.PhysicsModule.Meshing
 {
     [Extension(Path = "/OpenSim/RegionModules", NodeName = "RegionModule", Id = "Meshmerizer")]
     public class Meshmerizer : IMesher, INonSharedRegionModule
@@ -66,7 +66,7 @@ namespace OpenSim.Region.PhysicsModules.Meshing
 
         private bool cacheSculptMaps = true;
         private string decodedSculptMapPath = null;
-        private bool useMeshiesPhysicsMesh = false;
+        private bool useMeshiesPhysicsMesh = true;
 
         private float minSizeForComplexMesh = 0.2f; // prims with all dimensions smaller than this will have a bounding box mesh
 
@@ -99,7 +99,7 @@ namespace OpenSim.Region.PhysicsModules.Meshing
 
                     IConfig mesh_config = source.Configs["Mesh"];
 
-                    decodedSculptMapPath = "../caches/" + config.GetString("DecodedSculptMapPath", "j2kDecodeCache");
+                    decodedSculptMapPath = config.GetString("DecodedSculptMapPath", "j2kDecodeCache");
                     cacheSculptMaps = config.GetBoolean("CacheSculptMaps", cacheSculptMaps);
                     if (mesh_config != null)
                     {
@@ -247,13 +247,13 @@ namespace OpenSim.Region.PhysicsModules.Meshing
         private void AddSubMesh(OSDMap subMeshData, Vector3 size, List<Coord> coords, List<Face> faces)
         {
     //                                    Console.WriteLine("subMeshMap for {0} - {1}", primName, Util.GetFormattedXml((OSD)subMeshMap));
-    
+
             // As per http://wiki.secondlife.com/wiki/Mesh/Mesh_Asset_Format, some Mesh Level
             // of Detail Blocks (maps) contain just a NoGeometry key to signal there is no
             // geometry for this submesh.
             if (subMeshData.ContainsKey("NoGeometry") && ((OSDBoolean)subMeshData["NoGeometry"]))
                 return;
-    
+
             OpenMetaverse.Vector3 posMax = ((OSDMap)subMeshData["PositionDomain"])["Max"].AsVector3();
             OpenMetaverse.Vector3 posMin = ((OSDMap)subMeshData["PositionDomain"])["Min"].AsVector3();
             ushort faceIndexOffset = (ushort)coords.Count;
@@ -264,15 +264,15 @@ namespace OpenSim.Region.PhysicsModules.Meshing
                 ushort uX = Utils.BytesToUInt16(posBytes, i);
                 ushort uY = Utils.BytesToUInt16(posBytes, i + 2);
                 ushort uZ = Utils.BytesToUInt16(posBytes, i + 4);
-    
+
                 Coord c = new Coord(
                 Utils.UInt16ToFloat(uX, posMin.X, posMax.X) * size.X,
                 Utils.UInt16ToFloat(uY, posMin.Y, posMax.Y) * size.Y,
                 Utils.UInt16ToFloat(uZ, posMin.Z, posMax.Z) * size.Z);
-    
+
                 coords.Add(c);
             }
-    
+
             byte[] triangleBytes = subMeshData["TriangleList"].AsBinary();
             for (int i = 0; i < triangleBytes.Length; i += 6)
             {
@@ -436,9 +436,9 @@ namespace OpenSim.Region.PhysicsModules.Meshing
                             int convexSize = convexBlock["size"].AsInteger();
 
                             byte[] convexBytes = new byte[convexSize];
-                            
+
                             System.Buffer.BlockCopy(primShape.SculptData, convexOffset, convexBytes, 0, convexSize);
-                            
+
                             try
                             {
                                 convexBlockOsd = DecompressOsd(convexBytes);
@@ -449,7 +449,7 @@ namespace OpenSim.Region.PhysicsModules.Meshing
                                 //return false;
                             }
                         }
-                        
+
                         if (convexBlockOsd != null && convexBlockOsd is OSDMap)
                         {
                             convexBlock = convexBlockOsd as OSDMap;
@@ -762,7 +762,7 @@ namespace OpenSim.Region.PhysicsModules.Meshing
         {
             PrimMesh primMesh;
             coords = new List<Coord>();
-            faces = new List<Face>();            
+            faces = new List<Face>();
 
             float pathShearX = primShape.PathShearX < 128 ? (float)primShape.PathShearX * 0.01f : (float)(primShape.PathShearX - 256) * 0.01f;
             float pathShearY = primShape.PathShearY < 128 ? (float)primShape.PathShearY * 0.01f : (float)(primShape.PathShearY - 256) * 0.01f;
@@ -947,11 +947,21 @@ namespace OpenSim.Region.PhysicsModules.Meshing
             return CreateMesh(primName, primShape, size, lod, false, true);
         }
 
+        public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool shouldCache, bool convex, bool forOde)
+        {
+            return CreateMesh(primName, primShape, size, lod, false);
+        }
+
         public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical)
         {
             return CreateMesh(primName, primShape, size, lod, isPhysical, true);
         }
 
+        public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool convex, bool forOde)
+        {
+            return CreateMesh(primName, primShape, size, lod, isPhysical, true);
+        }
+
         public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool shouldCache)
         {
 #if SPAM
@@ -984,7 +994,7 @@ namespace OpenSim.Region.PhysicsModules.Meshing
                 if ((!isPhysical) && size.X < minSizeForComplexMesh && size.Y < minSizeForComplexMesh && size.Z < minSizeForComplexMesh)
                 {
 #if SPAM
-                m_log.Debug("Meshmerizer: prim " + primName + " has a size of " + size.ToString() + " which is below threshold of " + 
+                m_log.Debug("Meshmerizer: prim " + primName + " has a size of " + size.ToString() + " which is below threshold of " +
                             minSizeForComplexMesh.ToString() + " - creating simple bounding box");
 #endif
                     mesh = CreateBoundingBoxMesh(mesh);
@@ -1005,6 +1015,13 @@ namespace OpenSim.Region.PhysicsModules.Meshing
 
             return mesh;
         }
+        public IMesh GetMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool convex)
+        {
+            return null;
+        }
 
+        public void ReleaseMesh(IMesh imesh) { }
+        public void ExpireReleaseMeshs() { }
+        public void ExpireFileCache() { }
     }
 }
diff --git a/OpenSim/Region/PhysicsModules/Meshing/Meshmerizer/PrimMesher.cs b/OpenSim/Region/PhysicsModules/Meshing/Meshmerizer/PrimMesher.cs
index 4049ee1..fd2b1ea 100644
--- a/OpenSim/Region/PhysicsModules/Meshing/Meshmerizer/PrimMesher.cs
+++ b/OpenSim/Region/PhysicsModules/Meshing/Meshmerizer/PrimMesher.cs
@@ -445,7 +445,7 @@ namespace PrimMesher
             new Angle(1.0f, 1.0f, 0.0f)
         };
 
-        private static Coord[] normals4 = 
+        private static Coord[] normals4 =
         {
             new Coord(0.5f, 0.5f, 0.0f).Normalize(),
             new Coord(-0.5f, 0.5f, 0.0f).Normalize(),
@@ -2066,7 +2066,7 @@ namespace PrimMesher
         /// DEPRICATED - use Extrude(PathType.Linear) instead
         /// Extrudes a profile along a straight line path. Used for prim types box, cylinder, and prism.
         /// </summary>
-        /// 
+        ///
         public void ExtrudeLinear()
         {
             this.Extrude(PathType.Linear);
@@ -2077,7 +2077,7 @@ namespace PrimMesher
         /// DEPRICATED - use Extrude(PathType.Circular) instead
         /// Extrude a profile into a circular path prim mesh. Used for prim types torus, tube, and ring.
         /// </summary>
-        /// 
+        ///
         public void ExtrudeCircular()
         {
             this.Extrude(PathType.Circular);
diff --git a/OpenSim/Region/PhysicsModules/Meshing/Meshmerizer/SculptMap.cs b/OpenSim/Region/PhysicsModules/Meshing/Meshmerizer/SculptMap.cs
index 740424e..01d11f4 100644
--- a/OpenSim/Region/PhysicsModules/Meshing/Meshmerizer/SculptMap.cs
+++ b/OpenSim/Region/PhysicsModules/Meshing/Meshmerizer/SculptMap.cs
@@ -58,28 +58,24 @@ namespace PrimMesher
             if (bmW == 0 || bmH == 0)
                 throw new Exception("SculptMap: bitmap has no data");
 
-            int numLodPixels = lod * 2 * lod * 2;  // (32 * 2)^2  = 64^2 pixels for default sculpt map image
+            int numLodPixels = lod * lod;  // (32 * 2)^2  = 64^2 pixels for default sculpt map image
 
+            bool smallMap = bmW * bmH <= numLodPixels;
             bool needsScaling = false;
 
-            bool smallMap = bmW * bmH <= lod * lod;
-
             width = bmW;
             height = bmH;
-            while (width * height > numLodPixels)
+            while (width * height > numLodPixels * 4)
             {
                 width >>= 1;
                 height >>= 1;
                 needsScaling = true;
             }
 
-
-
             try
             {
                 if (needsScaling)
-                    bm = ScaleImage(bm, width, height,
-                        System.Drawing.Drawing2D.InterpolationMode.NearestNeighbor);
+                    bm = ScaleImage(bm, width, height);
             }
 
             catch (Exception e)
@@ -87,7 +83,7 @@ namespace PrimMesher
                 throw new Exception("Exception in ScaleImage(): e: " + e.ToString());
             }
 
-            if (width * height > lod * lod)
+            if (width * height > numLodPixels)
             {
                 width >>= 1;
                 height >>= 1;
@@ -144,15 +140,17 @@ namespace PrimMesher
             int rowNdx, colNdx;
             int smNdx = 0;
 
+
             for (rowNdx = 0; rowNdx < numRows; rowNdx++)
             {
                 List<Coord> row = new List<Coord>(numCols);
                 for (colNdx = 0; colNdx < numCols; colNdx++)
                 {
+
                     if (mirror)
-                        row.Add(new Coord(-(redBytes[smNdx] * pixScale - 0.5f), (greenBytes[smNdx] * pixScale - 0.5f), blueBytes[smNdx] * pixScale - 0.5f));
+                        row.Add(new Coord(-((float)redBytes[smNdx] * pixScale - 0.5f), ((float)greenBytes[smNdx] * pixScale - 0.5f), (float)blueBytes[smNdx] * pixScale - 0.5f));
                     else
-                        row.Add(new Coord(redBytes[smNdx] * pixScale - 0.5f, greenBytes[smNdx] * pixScale - 0.5f, blueBytes[smNdx] * pixScale - 0.5f));
+                        row.Add(new Coord((float)redBytes[smNdx] * pixScale - 0.5f, (float)greenBytes[smNdx] * pixScale - 0.5f, (float)blueBytes[smNdx] * pixScale - 0.5f));
 
                     ++smNdx;
                 }
@@ -161,23 +159,39 @@ namespace PrimMesher
             return rows;
         }
 
-        private Bitmap ScaleImage(Bitmap srcImage, int destWidth, int destHeight,
-                System.Drawing.Drawing2D.InterpolationMode interpMode)
+        private Bitmap ScaleImage(Bitmap srcImage, int destWidth, int destHeight)
         {
-            Bitmap scaledImage = new Bitmap(srcImage, destWidth, destHeight);
-            scaledImage.SetResolution(96.0f, 96.0f);
 
-            Graphics grPhoto = Graphics.FromImage(scaledImage);
-            grPhoto.InterpolationMode = interpMode;
+            Bitmap scaledImage = new Bitmap(destWidth, destHeight, PixelFormat.Format24bppRgb);
 
-            grPhoto.DrawImage(srcImage,
-                new Rectangle(0, 0, destWidth, destHeight),
-                new Rectangle(0, 0, srcImage.Width, srcImage.Height),
-                GraphicsUnit.Pixel);
+            Color c;
+            float xscale = srcImage.Width / destWidth;
+            float yscale = srcImage.Height / destHeight;
 
-            grPhoto.Dispose();
+            float sy = 0.5f;
+            for (int y = 0; y < destHeight; y++)
+            {
+                float sx = 0.5f;
+                for (int x = 0; x < destWidth; x++)
+                {
+                    try
+                    {
+                        c = srcImage.GetPixel((int)(sx), (int)(sy));
+                        scaledImage.SetPixel(x, y, Color.FromArgb(c.R, c.G, c.B));
+                    }
+                    catch (IndexOutOfRangeException)
+                    {
+                    }
+
+                    sx += xscale;
+                }
+                sy += yscale;
+            }
+            srcImage.Dispose();
             return scaledImage;
         }
+
+    }
+
     }
-}
 #endif
diff --git a/OpenSim/Region/PhysicsModules/Meshing/Properties/AssemblyInfo.cs b/OpenSim/Region/PhysicsModules/Meshing/Properties/AssemblyInfo.cs
index b4bdb5a..5c257e6 100644
--- a/OpenSim/Region/PhysicsModules/Meshing/Properties/AssemblyInfo.cs
+++ b/OpenSim/Region/PhysicsModules/Meshing/Properties/AssemblyInfo.cs
@@ -3,10 +3,10 @@ using System.Runtime.CompilerServices;
 using System.Runtime.InteropServices;
 using Mono.Addins;
 
-// General Information about an assembly is controlled through the following 
+// General Information about an assembly is controlled through the following
 // set of attributes. Change these attribute values to modify the information
 // associated with an assembly.
-[assembly: AssemblyTitle("OpenSim.Region.PhysicsModules.Meshing")]
+[assembly: AssemblyTitle("OpenSim.Region.PhysicsModule.Meshing")]
 [assembly: AssemblyDescription("")]
 [assembly: AssemblyConfiguration("")]
 [assembly: AssemblyCompany("http://opensimulator.org")]
@@ -15,8 +15,8 @@ using Mono.Addins;
 [assembly: AssemblyTrademark("")]
 [assembly: AssemblyCulture("")]
 
-// Setting ComVisible to false makes the types in this assembly not visible 
-// to COM components.  If you need to access a type in this assembly from 
+// Setting ComVisible to false makes the types in this assembly not visible
+// to COM components.  If you need to access a type in this assembly from
 // COM, set the ComVisible attribute to true on that type.
 [assembly: ComVisible(false)]
 
@@ -26,11 +26,11 @@ using Mono.Addins;
 // Version information for an assembly consists of the following four values:
 //
 //      Major Version
-//      Minor Version 
+//      Minor Version
 //      Build Number
 //      Revision
 //
-[assembly: AssemblyVersion("0.8.3.*")]
+[assembly: AssemblyVersion(OpenSim.VersionInfo.AssemblyVersionNumber)]
 
-[assembly: Addin("OpenSim.Region.PhysicsModules.Meshing", OpenSim.VersionInfo.VersionNumber)]
+[assembly: Addin("OpenSim.Region.PhysicsModule.Meshing", OpenSim.VersionInfo.VersionNumber)]
 [assembly: AddinDependency("OpenSim.Region.Framework", OpenSim.VersionInfo.VersionNumber)]
diff --git a/OpenSim/Region/PhysicsModules/Meshing/ZeroMesher.cs b/OpenSim/Region/PhysicsModules/Meshing/ZeroMesher.cs
index 0a3b3a4..dbf4f7e 100644
--- a/OpenSim/Region/PhysicsModules/Meshing/ZeroMesher.cs
+++ b/OpenSim/Region/PhysicsModules/Meshing/ZeroMesher.cs
@@ -47,7 +47,7 @@ using log4net;
  * it's always availabe and thus the default in case of configuration errors
 */
 
-namespace OpenSim.Region.PhysicsModules.Meshing
+namespace OpenSim.Region.PhysicsModule.Meshing
 {
 
     [Extension(Path = "/OpenSim/RegionModules", NodeName = "RegionModule", Id = "ZeroMesher")]
@@ -110,23 +110,36 @@ namespace OpenSim.Region.PhysicsModules.Meshing
         #region IMesher
         public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod)
         {
-            return CreateMesh(primName, primShape, size, lod, false, false);
+            return CreateMesh(primName, primShape, size, lod, false);
         }
 
-        public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical)
+        public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool shouldCache, bool convex, bool forOde)
+        {
+            return CreateMesh(primName, primShape, size, lod, false);
+        }
+
+        public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool convex,bool forOde)
         {
-            return CreateMesh(primName, primShape, size, lod, false, false);
+            return CreateMesh(primName, primShape, size, lod, false);
         }
 
-        public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool shouldCache)
+        public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical)
         {
             // Remove the reference to the encoded JPEG2000 data so it can be GCed
             primShape.SculptData = OpenMetaverse.Utils.EmptyBytes;
 
             return null;
         }
-        #endregion
 
+        public IMesh GetMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool convex)
+        {
+            return null;
+        }
 
+        public void ReleaseMesh(IMesh mesh) { }
+        public void ExpireReleaseMeshs() { }
+        public void ExpireFileCache() { }
+
+        #endregion
     }
 }
diff --git a/OpenSim/Region/PhysicsModules/Ode/AssemblyInfo.cs b/OpenSim/Region/PhysicsModules/Ode/AssemblyInfo.cs
index 7869739..3691d96 100644
--- a/OpenSim/Region/PhysicsModules/Ode/AssemblyInfo.cs
+++ b/OpenSim/Region/PhysicsModules/Ode/AssemblyInfo.cs
@@ -56,7 +56,7 @@ using Mono.Addins;
 // You can specify all values by your own or you can build default build and revision
 // numbers with the '*' character (the default):
 
-[assembly : AssemblyVersion("0.8.2.*")]
+[assembly : AssemblyVersion(OpenSim.VersionInfo.AssemblyVersionNumber)]
 
 [assembly: Addin("OpenSim.Region.PhysicsModule.ODE", OpenSim.VersionInfo.VersionNumber)]
 [assembly: AddinDependency("OpenSim.Region.Framework", OpenSim.VersionInfo.VersionNumber)]
diff --git a/OpenSim/Region/PhysicsModules/Ode/ODEApi.cs b/OpenSim/Region/PhysicsModules/Ode/ODEApi.cs
new file mode 100644
index 0000000..c851b12
--- /dev/null
+++ b/OpenSim/Region/PhysicsModules/Ode/ODEApi.cs
@@ -0,0 +1,2025 @@
+/*
+ * based on:
+ * Ode.NET - .NET bindings for ODE
+ * Jason Perkins (starkos@industriousone.com)
+  *  Licensed under the New BSD
+ *  Part of the OpenDynamicsEngine
+Open Dynamics Engine
+Copyright (c) 2001-2007, Russell L. Smith.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions
+are met:
+
+Redistributions of source code must retain the above copyright notice,
+this list of conditions and the following disclaimer.
+
+Redistributions in binary form must reproduce the above copyright notice,
+this list of conditions and the following disclaimer in the documentation
+and/or other materials provided with the distribution.
+
+Neither the names of ODE's copyright owner nor the names of its
+contributors may be used to endorse or promote products derived from
+this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ * changes by opensim team;
+ * changes by Aurora team http://www.aurora-sim.org/
+ * changes by Ubit Umarov
+ */
+
+using System;
+using System.Runtime.InteropServices;
+using System.Security;
+using OMV = OpenMetaverse;
+
+namespace OpenSim.Region.PhysicsModule.ODE
+{
+//#if dDOUBLE
+// don't see much use in double precision with time steps of 20ms and 10 iterations used on opensim
+// at least we save same memory and memory access time, FPU performance on intel usually is similar
+//      using dReal = System.Double;
+//#else
+    using dReal = System.Single;
+//#endif
+
+    public static class d
+    {
+        public static dReal Infinity = dReal.MaxValue;
+        public static int NTotalBodies = 0;
+        public static int NTotalGeoms = 0;
+
+        public const uint CONTACTS_UNIMPORTANT = 0x80000000;
+
+        #region Flags and Enumerations
+
+        [Flags]
+        public enum AllocateODEDataFlags : uint
+        {
+            BasicData = 0,
+            CollisionData = 0x00000001,
+            All = ~0u
+        }
+
+        [Flags]
+        public enum IniteODEFlags : uint
+        {
+            dInitFlagManualThreadCleanup = 0x00000001
+        }
+
+        [Flags]
+        public enum ContactFlags : int
+        {
+            Mu2 = 0x001,
+            FDir1 = 0x002,
+            Bounce = 0x004,
+            SoftERP = 0x008,
+            SoftCFM = 0x010,
+            Motion1 = 0x020,
+            Motion2 = 0x040,
+            MotionN = 0x080,
+            Slip1 = 0x100,
+            Slip2 = 0x200,
+            Approx0 = 0x0000,
+            Approx1_1 = 0x1000,
+            Approx1_2 = 0x2000,
+            Approx1 = 0x3000
+        }
+
+        public enum GeomClassID : int
+        {
+            SphereClass,
+            BoxClass,
+            CapsuleClass,
+            CylinderClass,
+            PlaneClass,
+            RayClass,
+            ConvexClass,
+            GeomTransformClass,
+            TriMeshClass,
+            HeightfieldClass,
+            FirstSpaceClass,
+            SimpleSpaceClass = FirstSpaceClass,
+            HashSpaceClass,
+            QuadTreeSpaceClass,
+            LastSpaceClass = QuadTreeSpaceClass,
+            ubtTerrainClass,
+            FirstUserClass,
+            LastUserClass = FirstUserClass + MaxUserClasses - 1,
+            NumClasses,
+            MaxUserClasses = 5
+        }
+
+        public enum JointType : int
+        {
+            None,
+            Ball,
+            Hinge,
+            Slider,
+            Contact,
+            Universal,
+            Hinge2,
+            Fixed,
+            Null,
+            AMotor,
+            LMotor,
+            Plane2D
+        }
+
+        public enum JointParam : int
+        {
+            LoStop,
+            HiStop,
+            Vel,
+            FMax,
+            FudgeFactor,
+            Bounce,
+            CFM,
+            StopERP,
+            StopCFM,
+            SuspensionERP,
+            SuspensionCFM,
+            LoStop2 = 256,
+            HiStop2,
+            Vel2,
+            FMax2,
+            FudgeFactor2,
+            Bounce2,
+            CFM2,
+            StopERP2,
+            StopCFM2,
+            SuspensionERP2,
+            SuspensionCFM2,
+            LoStop3 = 512,
+            HiStop3,
+            Vel3,
+            FMax3,
+            FudgeFactor3,
+            Bounce3,
+            CFM3,
+            StopERP3,
+            StopCFM3,
+            SuspensionERP3,
+            SuspensionCFM3
+        }
+
+        public enum dSweepAndPruneAxis : int
+        {
+            XYZ = ((0)|(1<<2)|(2<<4)),
+            XZY = ((0)|(2<<2)|(1<<4)),
+            YXZ = ((1)|(0<<2)|(2<<4)),
+            YZX = ((1)|(2<<2)|(0<<4)),
+            ZXY = ((2)|(0<<2)|(1<<4)),
+            ZYX = ((2)|(1<<2)|(0<<4))
+        }
+
+        #endregion
+
+        #region Callbacks
+
+        [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
+        public delegate int AABBTestFn(IntPtr o1, IntPtr o2, ref AABB aabb);
+
+        [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
+        public delegate int ColliderFn(IntPtr o1, IntPtr o2, int flags, out ContactGeom contact, int skip);
+
+        [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
+        public delegate void GetAABBFn(IntPtr geom, out AABB aabb);
+
+        [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
+        public delegate ColliderFn GetColliderFnFn(int num);
+
+        [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
+        public delegate void GeomDtorFn(IntPtr o);
+
+        [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
+        public delegate dReal HeightfieldGetHeight(IntPtr p_user_data, int x, int z);
+
+        [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
+        public delegate dReal OSTerrainGetHeight(IntPtr p_user_data, int x, int z);
+
+        [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
+        public delegate void NearCallback(IntPtr data, IntPtr geom1, IntPtr geom2);
+
+        [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
+        public delegate int TriCallback(IntPtr trimesh, IntPtr refObject, int triangleIndex);
+
+        [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
+        public delegate int TriArrayCallback(IntPtr trimesh, IntPtr refObject, int[] triangleIndex, int triCount);
+
+        [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
+        public delegate int TriRayCallback(IntPtr trimesh, IntPtr ray, int triangleIndex, dReal u, dReal v);
+
+        #endregion
+
+        #region Structs
+
+        [StructLayout(LayoutKind.Sequential)]
+        public struct AABB
+        {
+            public dReal MinX, MaxX;
+            public dReal MinY, MaxY;
+            public dReal MinZ, MaxZ;
+        }
+
+
+        [StructLayout(LayoutKind.Sequential)]
+        public struct Contact
+        {
+            public SurfaceParameters surface;
+            public ContactGeom geom;
+            public Vector3 fdir1;
+            public static readonly int unmanagedSizeOf = Marshal.SizeOf(typeof(Contact));
+        }
+
+
+        [StructLayout(LayoutKind.Sequential)]
+        public struct ContactGeom
+        {
+
+            public Vector3 pos;
+            public Vector3 normal;
+            public dReal depth;
+            public IntPtr g1;
+            public IntPtr g2;
+            public int side1;
+            public int side2;
+            public static readonly int unmanagedSizeOf = Marshal.SizeOf(typeof(ContactGeom));
+        }
+
+        [StructLayout(LayoutKind.Sequential)]
+        public struct GeomClass
+        {
+            public int bytes;
+            public GetColliderFnFn collider;
+            public GetAABBFn aabb;
+            public AABBTestFn aabb_test;
+            public GeomDtorFn dtor;
+        }
+
+
+        [StructLayout(LayoutKind.Sequential)]
+        public struct JointFeedback
+        {
+            public Vector3 f1;
+            public Vector3 t1;
+            public Vector3 f2;
+            public Vector3 t2;
+        }
+
+
+        [StructLayout(LayoutKind.Sequential)]
+        public struct Mass
+        {
+            public dReal mass;
+            public Vector4 c;
+            public Matrix3 I;
+        }
+
+
+        [StructLayout(LayoutKind.Sequential)]
+        public struct Matrix3
+        {
+            public Matrix3(dReal m00, dReal m10, dReal m20, dReal m01, dReal m11, dReal m21, dReal m02, dReal m12, dReal m22)
+            {
+                M00 = m00;  M10 = m10;  M20 = m20;  _m30 = 0.0f;
+                M01 = m01;  M11 = m11;  M21 = m21;  _m31 = 0.0f;
+                M02 = m02;  M12 = m12;  M22 = m22;  _m32 = 0.0f;
+            }
+            public dReal M00, M10, M20;
+            private dReal _m30;
+            public dReal M01, M11, M21;
+            private dReal _m31;
+            public dReal M02, M12, M22;
+            private dReal _m32;
+        }
+
+        [StructLayout(LayoutKind.Sequential)]
+        public struct Matrix4
+        {
+            public Matrix4(dReal m00, dReal m10, dReal m20, dReal m30,
+                dReal m01, dReal m11, dReal m21, dReal m31,
+                dReal m02, dReal m12, dReal m22, dReal m32,
+                dReal m03, dReal m13, dReal m23, dReal m33)
+            {
+                M00 = m00; M10 = m10; M20 = m20; M30 = m30;
+                M01 = m01; M11 = m11; M21 = m21; M31 = m31;
+                M02 = m02; M12 = m12; M22 = m22; M32 = m32;
+                M03 = m03; M13 = m13; M23 = m23; M33 = m33;
+            }
+            public dReal M00, M10, M20, M30;
+            public dReal M01, M11, M21, M31;
+            public dReal M02, M12, M22, M32;
+            public dReal M03, M13, M23, M33;
+        }
+
+        [StructLayout(LayoutKind.Sequential)]
+        public struct Quaternion
+        {
+            public dReal W, X, Y, Z;
+        }
+
+
+        [StructLayout(LayoutKind.Sequential)]
+        public struct SurfaceParameters
+        {
+            public ContactFlags mode;
+            public dReal mu;
+            public dReal mu2;
+            public dReal bounce;
+            public dReal bounce_vel;
+            public dReal soft_erp;
+            public dReal soft_cfm;
+            public dReal motion1;
+            public dReal motion2;
+            public dReal motionN;
+            public dReal slip1;
+            public dReal slip2;
+        }
+
+
+        [StructLayout(LayoutKind.Sequential)]
+        public struct Vector3
+        {
+            public Vector3(dReal x, dReal y, dReal z)
+            {
+                X = x;  Y = y;  Z = z;  _w = 0.0f;
+            }
+            public dReal X, Y, Z;
+            private dReal _w;
+        }
+
+
+        [StructLayout(LayoutKind.Sequential)]
+        public struct Vector4
+        {
+            public Vector4(dReal x, dReal y, dReal z, dReal w)
+            {
+                X = x;  Y = y;  Z = z;  W = w;
+            }
+            public dReal X, Y, Z, W;
+        }
+
+        #endregion
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dAllocateODEDataForThread"), SuppressUnmanagedCodeSecurity]
+        public static extern int AllocateODEDataForThread(uint ODEInitFlags);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dAreConnected"), SuppressUnmanagedCodeSecurity]
+        public static extern bool AreConnected(IntPtr b1, IntPtr b2);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dAreConnectedExcluding"), SuppressUnmanagedCodeSecurity]
+        public static extern bool AreConnectedExcluding(IntPtr b1, IntPtr b2, JointType joint_type);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddForce"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyAddForce(IntPtr body, dReal fx, dReal fy, dReal fz);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddForceAtPos"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyAddForceAtPos(IntPtr body, dReal fx, dReal fy, dReal fz, dReal px, dReal py, dReal pz);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddForceAtRelPos"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyAddForceAtRelPos(IntPtr body, dReal fx, dReal fy, dReal fz, dReal px, dReal py, dReal pz);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddRelForce"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyAddRelForce(IntPtr body, dReal fx, dReal fy, dReal fz);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddRelForceAtPos"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyAddRelForceAtPos(IntPtr body, dReal fx, dReal fy, dReal fz, dReal px, dReal py, dReal pz);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddRelForceAtRelPos"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyAddRelForceAtRelPos(IntPtr body, dReal fx, dReal fy, dReal fz, dReal px, dReal py, dReal pz);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddRelTorque"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyAddRelTorque(IntPtr body, dReal fx, dReal fy, dReal fz);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddTorque"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyAddTorque(IntPtr body, dReal fx, dReal fy, dReal fz);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyPosition"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyCopyPosition(IntPtr body, out Vector3 pos);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyPosition"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyCopyPosition(IntPtr body, out dReal X);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyQuaternion"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyCopyQuaternion(IntPtr body, out Quaternion quat);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyQuaternion"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyCopyQuaternion(IntPtr body, out dReal X);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyRotation"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyCopyRotation(IntPtr body, out Matrix3 R);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyRotation"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyCopyRotation(IntPtr body, out dReal M00);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCreate"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr BodyiCreate(IntPtr world);
+        public static IntPtr BodyCreate(IntPtr world)
+        {
+            NTotalBodies++;
+            return BodyiCreate(world);
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyDestroy"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyiDestroy(IntPtr body);
+        public static void BodyDestroy(IntPtr body)
+        {
+            NTotalBodies--;
+            BodyiDestroy(body);
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyDisable"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyDisable(IntPtr body);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyEnable"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyEnable(IntPtr body);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableAngularThreshold"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal BodyGetAutoDisableAngularThreshold(IntPtr body);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableFlag"), SuppressUnmanagedCodeSecurity]
+        public static extern bool BodyGetAutoDisableFlag(IntPtr body);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableDefaults"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyGetAutoDisableDefaults(IntPtr body);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableLinearThreshold"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal BodyGetAutoDisableLinearThreshold(IntPtr body);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableSteps"), SuppressUnmanagedCodeSecurity]
+        public static extern int BodyGetAutoDisableSteps(IntPtr body);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableTime"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal BodyGetAutoDisableTime(IntPtr body);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAngularVel"), SuppressUnmanagedCodeSecurity]
+        public extern unsafe static Vector3* BodyGetAngularVelUnsafe(IntPtr body);
+        public static Vector3 BodyGetAngularVel(IntPtr body)
+        {
+            unsafe { return *(BodyGetAngularVelUnsafe(body)); }
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetData"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr BodyGetData(IntPtr body);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetFiniteRotationMode"), SuppressUnmanagedCodeSecurity]
+        public static extern int BodyGetFiniteRotationMode(IntPtr body);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetFiniteRotationAxis"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyGetFiniteRotationAxis(IntPtr body, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetForce"), SuppressUnmanagedCodeSecurity]
+        public extern unsafe static Vector3* BodyGetForceUnsafe(IntPtr body);
+        public static Vector3 BodyGetForce(IntPtr body)
+        {
+            unsafe { return *(BodyGetForceUnsafe(body)); }
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetGravityMode"), SuppressUnmanagedCodeSecurity]
+        public static extern bool BodyGetGravityMode(IntPtr body);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetGyroscopicMode"), SuppressUnmanagedCodeSecurity]
+        public static extern int BodyGetGyroscopicMode(IntPtr body);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetJoint"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr BodyGetJoint(IntPtr body, int index);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetLinearVel"), SuppressUnmanagedCodeSecurity]
+        public extern unsafe static Vector3* BodyGetLinearVelUnsafe(IntPtr body);
+        public static Vector3 BodyGetLinearVel(IntPtr body)
+        {
+            unsafe { return *(BodyGetLinearVelUnsafe(body)); }
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetMass"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyGetMass(IntPtr body, out Mass mass);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetNumJoints"), SuppressUnmanagedCodeSecurity]
+        public static extern int BodyGetNumJoints(IntPtr body);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetPointVel"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyGetPointVel(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetPosition"), SuppressUnmanagedCodeSecurity]
+        public extern unsafe static Vector3* BodyGetPositionUnsafe(IntPtr body);
+        public static Vector3 BodyGetPosition(IntPtr body)
+        {
+            unsafe { return *(BodyGetPositionUnsafe(body)); }
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetPosRelPoint"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyGetPosRelPoint(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetQuaternion"), SuppressUnmanagedCodeSecurity]
+        public extern unsafe static Quaternion* BodyGetQuaternionUnsafe(IntPtr body);
+        public static Quaternion BodyGetQuaternion(IntPtr body)
+        {
+            unsafe { return *(BodyGetQuaternionUnsafe(body)); }
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetRelPointPos"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyGetRelPointPos(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetRelPointVel"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyGetRelPointVel(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetRotation"), SuppressUnmanagedCodeSecurity]
+        public extern unsafe static Matrix3* BodyGetRotationUnsafe(IntPtr body);
+        public static Matrix3 BodyGetRotation(IntPtr body)
+        {
+            unsafe { return *(BodyGetRotationUnsafe(body)); }
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetTorque"), SuppressUnmanagedCodeSecurity]
+        public extern unsafe static Vector3* BodyGetTorqueUnsafe(IntPtr body);
+        public static Vector3 BodyGetTorque(IntPtr body)
+        {
+            unsafe { return *(BodyGetTorqueUnsafe(body)); }
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetWorld"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr BodyGetWorld(IntPtr body);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetFirstGeom"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr BodyGetFirstGeom(IntPtr body);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetNextGeom"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr dBodyGetNextGeom(IntPtr Geom);
+
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyIsEnabled"), SuppressUnmanagedCodeSecurity]
+        public static extern bool BodyIsEnabled(IntPtr body);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAngularVel"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetAngularVel(IntPtr body, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableAngularThreshold"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetAutoDisableAngularThreshold(IntPtr body, dReal angular_threshold);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableDefaults"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetAutoDisableDefaults(IntPtr body);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableFlag"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetAutoDisableFlag(IntPtr body, bool do_auto_disable);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableLinearThreshold"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetAutoDisableLinearThreshold(IntPtr body, dReal linear_threshold);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableSteps"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetAutoDisableSteps(IntPtr body, int steps);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableTime"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetAutoDisableTime(IntPtr body, dReal time);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetData"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetData(IntPtr body, IntPtr data);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetFiniteRotationMode"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetFiniteRotationMode(IntPtr body, int mode);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetFiniteRotationAxis"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetFiniteRotationAxis(IntPtr body, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetLinearDamping"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetLinearDamping(IntPtr body, dReal scale);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAngularDamping"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetAngularDamping(IntPtr body, dReal scale);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetLinearDamping"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal BodyGetLinearDamping(IntPtr body);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAngularDamping"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal BodyGetAngularDamping(IntPtr body);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAngularDamping"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetDamping(IntPtr body, dReal linear_scale, dReal angular_scale);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAngularDampingThreshold"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetAngularDampingThreshold(IntPtr body, dReal threshold);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetLinearDampingThreshold"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetLinearDampingThreshold(IntPtr body, dReal threshold);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetLinearDampingThreshold"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal BodyGetLinearDampingThreshold(IntPtr body);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAngularDampingThreshold"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal BodyGetAngularDampingThreshold(IntPtr body);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetForce"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetForce(IntPtr body, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetGravityMode"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetGravityMode(IntPtr body, bool mode);
+
+        /// <summary>
+        /// Sets the Gyroscopic term status on the body specified.
+        /// </summary>
+        /// <param name="body">Pointer to body</param>
+        /// <param name="enabled">NonZero enabled, Zero disabled</param>
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetGyroscopicMode"), SuppressUnmanagedCodeSecurity]
+        public static extern void dBodySetGyroscopicMode(IntPtr body, int enabled);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetLinearVel"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetLinearVel(IntPtr body, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetMass"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetMass(IntPtr body, ref Mass mass);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetPosition"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetPosition(IntPtr body, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetQuaternion"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetQuaternion(IntPtr body, ref Quaternion q);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetQuaternion"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetQuaternion(IntPtr body, ref dReal w);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetRotation"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetRotation(IntPtr body, ref Matrix3 R);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetRotation"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetRotation(IntPtr body, ref dReal M00);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetTorque"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetTorque(IntPtr body, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyVectorFromWorld"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyVectorFromWorld(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyVectorToWorld"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyVectorToWorld(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBoxBox"), SuppressUnmanagedCodeSecurity]
+        public static extern void BoxBox(ref Vector3 p1, ref Matrix3 R1,
+            ref Vector3 side1, ref Vector3 p2,
+            ref Matrix3 R2, ref Vector3 side2,
+            ref Vector3 normal, out dReal depth, out int return_code,
+            int maxc, out ContactGeom contact, int skip);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBoxTouchesBox"), SuppressUnmanagedCodeSecurity]
+        public static extern void BoxTouchesBox(ref Vector3 _p1, ref Matrix3 R1,
+            ref Vector3 side1, ref Vector3 _p2,
+            ref Matrix3 R2, ref Vector3 side2);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCleanupODEAllDataForThread"), SuppressUnmanagedCodeSecurity]
+        public static extern void CleanupODEAllDataForThread();
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dClosestLineSegmentPoints"), SuppressUnmanagedCodeSecurity]
+        public static extern void ClosestLineSegmentPoints(ref Vector3 a1, ref Vector3 a2,
+            ref Vector3 b1, ref Vector3 b2,
+            ref Vector3 cp1, ref Vector3 cp2);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCloseODE"), SuppressUnmanagedCodeSecurity]
+        public static extern void CloseODE();
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCollide"), SuppressUnmanagedCodeSecurity]
+        public static extern int Collide(IntPtr o1, IntPtr o2, int flags, [In, Out] ContactGeom[] contact, int skip);
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCollide"), SuppressUnmanagedCodeSecurity]
+        public static extern int CollidePtr(IntPtr o1, IntPtr o2, int flags, IntPtr contactgeomarray, int skip);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dConnectingJoint"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr ConnectingJoint(IntPtr j1, IntPtr j2);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateBox"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr CreateiBox(IntPtr space, dReal lx, dReal ly, dReal lz);
+        public static IntPtr CreateBox(IntPtr space, dReal lx, dReal ly, dReal lz)
+        {
+            NTotalGeoms++;
+            return CreateiBox(space, lx, ly, lz);
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateCapsule"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr CreateiCapsule(IntPtr space, dReal radius, dReal length);
+        public static IntPtr CreateCapsule(IntPtr space, dReal radius, dReal length)
+        {
+            NTotalGeoms++;
+            return CreateiCapsule(space, radius, length);
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateConvex"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr CreateiConvex(IntPtr space, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons);
+        public static IntPtr CreateConvex(IntPtr space, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons)
+        {
+            NTotalGeoms++;
+            return CreateiConvex(space, planes, planeCount, points, pointCount, polygons);
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateCylinder"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr CreateiCylinder(IntPtr space, dReal radius, dReal length);
+        public static IntPtr CreateCylinder(IntPtr space, dReal radius, dReal length)
+        {
+            NTotalGeoms++;
+            return CreateiCylinder(space, radius, length);
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateHeightfield"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr CreateiHeightfield(IntPtr space, IntPtr data, int bPlaceable);
+        public static IntPtr CreateHeightfield(IntPtr space, IntPtr data, int bPlaceable)
+        {
+            NTotalGeoms++;
+            return CreateiHeightfield(space, data, bPlaceable);
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateOSTerrain"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr CreateiOSTerrain(IntPtr space, IntPtr data, int bPlaceable);
+        public static IntPtr CreateOSTerrain(IntPtr space, IntPtr data, int bPlaceable)
+        {
+            NTotalGeoms++;
+            return CreateiOSTerrain(space, data, bPlaceable);
+        }
+
+
+
+
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateGeom"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr CreateiGeom(int classnum);
+        public static IntPtr CreateGeom(int classnum)
+        {
+            NTotalGeoms++;
+            return CreateiGeom(classnum);
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateGeomClass"), SuppressUnmanagedCodeSecurity]
+        public static extern int CreateGeomClass(ref GeomClass classptr);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateGeomTransform"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr CreateGeomTransform(IntPtr space);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreatePlane"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr CreateiPlane(IntPtr space, dReal a, dReal b, dReal c, dReal d);
+        public static IntPtr CreatePlane(IntPtr space, dReal a, dReal b, dReal c, dReal d)
+        {
+            NTotalGeoms++;
+            return CreateiPlane(space, a, b, c, d);
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateRay"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr CreateiRay(IntPtr space, dReal length);
+        public static IntPtr CreateRay(IntPtr space, dReal length)
+        {
+            NTotalGeoms++;
+            return CreateiRay(space, length);
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateSphere"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr CreateiSphere(IntPtr space, dReal radius);
+        public static IntPtr CreateSphere(IntPtr space, dReal radius)
+        {
+            NTotalGeoms++;
+            return CreateiSphere(space, radius);
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateTriMesh"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr CreateiTriMesh(IntPtr space, IntPtr data,
+            TriCallback callback, TriArrayCallback arrayCallback, TriRayCallback rayCallback);
+        public static IntPtr CreateTriMesh(IntPtr space, IntPtr data,
+            TriCallback callback, TriArrayCallback arrayCallback, TriRayCallback rayCallback)
+        {
+            NTotalGeoms++;
+            return CreateiTriMesh(space, data, callback, arrayCallback, rayCallback);
+        }
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dDot"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal Dot(ref dReal X0, ref dReal X1, int n);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dDQfromW"), SuppressUnmanagedCodeSecurity]
+        public static extern void DQfromW(dReal[] dq, ref Vector3 w, ref Quaternion q);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dFactorCholesky"), SuppressUnmanagedCodeSecurity]
+        public static extern int FactorCholesky(ref dReal A00, int n);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dFactorLDLT"), SuppressUnmanagedCodeSecurity]
+        public static extern void FactorLDLT(ref dReal A, out dReal d, int n, int nskip);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomBoxGetLengths"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomBoxGetLengths(IntPtr geom, out Vector3 len);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomBoxGetLengths"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomBoxGetLengths(IntPtr geom, out dReal x);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomBoxPointDepth"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal GeomBoxPointDepth(IntPtr geom, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomBoxSetLengths"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomBoxSetLengths(IntPtr geom, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCapsuleGetParams"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomCapsuleGetParams(IntPtr geom, out dReal radius, out dReal length);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCapsulePointDepth"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal GeomCapsulePointDepth(IntPtr geom, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCapsuleSetParams"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomCapsuleSetParams(IntPtr geom, dReal radius, dReal length);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomClearOffset"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomClearOffset(IntPtr geom);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyOffsetPosition"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr GeomCopyOffsetPosition(IntPtr geom, ref Vector3 pos);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyOffsetPosition"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr GeomCopyOffsetPosition(IntPtr geom, ref dReal X);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetOffsetQuaternion"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomCopyOffsetQuaternion(IntPtr geom, ref Quaternion Q);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetOffsetQuaternion"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomCopyOffsetQuaternion(IntPtr geom, ref dReal X);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyOffsetRotation"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr GeomCopyOffsetRotation(IntPtr geom, ref Matrix3 R);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyOffsetRotation"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr GeomCopyOffsetRotation(IntPtr geom, ref dReal M00);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyPosition"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomCopyPosition(IntPtr geom, out Vector3 pos);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyPosition"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomCopyPosition(IntPtr geom, out dReal X);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyRotation"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomCopyRotation(IntPtr geom, out Matrix3 R);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyRotation"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomCopyRotation(IntPtr geom, out dReal M00);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCylinderGetParams"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomCylinderGetParams(IntPtr geom, out dReal radius, out dReal length);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCylinderSetParams"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomCylinderSetParams(IntPtr geom, dReal radius, dReal length);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomDestroy"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomiDestroy(IntPtr geom);
+        public static void GeomDestroy(IntPtr geom)
+        {
+            NTotalGeoms--;
+            GeomiDestroy(geom);
+        }
+
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomDisable"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomDisable(IntPtr geom);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomEnable"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomEnable(IntPtr geom);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetAABB"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomGetAABB(IntPtr geom, out AABB aabb);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetAABB"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomGetAABB(IntPtr geom, out dReal minX);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetBody"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr GeomGetBody(IntPtr geom);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetCategoryBits"), SuppressUnmanagedCodeSecurity]
+        public static extern uint GeomGetCategoryBits(IntPtr geom);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetClassData"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr GeomGetClassData(IntPtr geom);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetCollideBits"), SuppressUnmanagedCodeSecurity]
+        public static extern uint GeomGetCollideBits(IntPtr geom);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetClass"), SuppressUnmanagedCodeSecurity]
+        public static extern GeomClassID GeomGetClass(IntPtr geom);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetData"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr GeomGetData(IntPtr geom);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetOffsetPosition"), SuppressUnmanagedCodeSecurity]
+        public extern unsafe static Vector3* GeomGetOffsetPositionUnsafe(IntPtr geom);
+        public static Vector3 GeomGetOffsetPosition(IntPtr geom)
+        {
+            unsafe { return *(GeomGetOffsetPositionUnsafe(geom)); }
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetOffsetRotation"), SuppressUnmanagedCodeSecurity]
+        public extern unsafe static Matrix3* GeomGetOffsetRotationUnsafe(IntPtr geom);
+        public static Matrix3 GeomGetOffsetRotation(IntPtr geom)
+        {
+            unsafe { return *(GeomGetOffsetRotationUnsafe(geom)); }
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetPosition"), SuppressUnmanagedCodeSecurity]
+        public extern unsafe static Vector3* GeomGetPositionUnsafe(IntPtr geom);
+        public static Vector3 GeomGetPosition(IntPtr geom)
+        {
+            unsafe { return *(GeomGetPositionUnsafe(geom)); }
+        }
+        public static OMV.Vector3 GeomGetPositionOMV(IntPtr geom)
+        {
+            Vector3 vtmp = GeomGetPosition(geom);
+            return new OMV.Vector3(vtmp.X, vtmp.Y, vtmp.Z);
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetQuaternion"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomCopyQuaternion(IntPtr geom, out Quaternion q);
+        public static OMV.Quaternion GeomGetQuaternionOMV(IntPtr geom)
+        {
+            Quaternion qtmp;
+            GeomCopyQuaternion(geom, out qtmp);
+            return new OMV.Quaternion(qtmp.X, qtmp.Y, qtmp.Z, qtmp.W);
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetQuaternion"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomCopyQuaternion(IntPtr geom, out dReal X);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetRotation"), SuppressUnmanagedCodeSecurity]
+        public extern unsafe static Matrix3* GeomGetRotationUnsafe(IntPtr geom);
+        public static Matrix3 GeomGetRotation(IntPtr geom)
+        {
+            unsafe { return *(GeomGetRotationUnsafe(geom)); }
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetSpace"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr GeomGetSpace(IntPtr geom);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildByte"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomHeightfieldDataBuildByte(IntPtr d, byte[] pHeightData, int bCopyHeightData,
+                dReal width, dReal depth, int widthSamples, int depthSamples,
+                dReal scale, dReal offset, dReal thickness, int bWrap);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildByte"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomHeightfieldDataBuildByte(IntPtr d, IntPtr pHeightData, int bCopyHeightData,
+                dReal width, dReal depth, int widthSamples, int depthSamples,
+                dReal scale, dReal offset, dReal thickness,     int bWrap);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildCallback"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomHeightfieldDataBuildCallback(IntPtr d, IntPtr pUserData, HeightfieldGetHeight pCallback,
+                dReal width, dReal depth, int widthSamples, int depthSamples,
+                dReal scale, dReal offset, dReal thickness, int bWrap);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildShort"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomHeightfieldDataBuildShort(IntPtr d, ushort[] pHeightData, int bCopyHeightData,
+                dReal width, dReal depth, int widthSamples, int depthSamples,
+                dReal scale, dReal offset, dReal thickness, int bWrap);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildShort"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomHeightfieldDataBuildShort(IntPtr d, short[] pHeightData, int bCopyHeightData,
+                dReal width, dReal depth, int widthSamples, int depthSamples,
+                dReal scale, dReal offset, dReal thickness, int bWrap);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildShort"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomHeightfieldDataBuildShort(IntPtr d, IntPtr pHeightData, int bCopyHeightData,
+                dReal width, dReal depth, int widthSamples, int depthSamples,
+                dReal scale, dReal offset, dReal thickness, int bWrap);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildSingle"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomHeightfieldDataBuildSingle(IntPtr d, float[] pHeightData, int bCopyHeightData,
+                dReal width, dReal depth, int widthSamples, int depthSamples,
+                dReal scale, dReal offset, dReal thickness, int bWrap);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildSingle"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomHeightfieldDataBuildSingle(IntPtr d, IntPtr pHeightData, int bCopyHeightData,
+                dReal width, dReal depth, int widthSamples, int depthSamples,
+                dReal scale, dReal offset, dReal thickness, int bWrap);
+
+
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildDouble"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomHeightfieldDataBuildDouble(IntPtr d, double[] pHeightData, int bCopyHeightData,
+                dReal width, dReal depth, int widthSamples, int depthSamples,
+                dReal scale, dReal offset, dReal thickness, int bWrap);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildDouble"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomHeightfieldDataBuildDouble(IntPtr d, IntPtr pHeightData, int bCopyHeightData,
+                dReal width, dReal depth, int widthSamples, int depthSamples,
+                dReal scale, dReal offset, dReal thickness, int bWrap);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataCreate"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr GeomHeightfieldDataCreate();
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataDestroy"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomHeightfieldDataDestroy(IntPtr d);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataSetBounds"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomHeightfieldDataSetBounds(IntPtr d, dReal minHeight, dReal maxHeight);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldGetHeightfieldData"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr GeomHeightfieldGetHeightfieldData(IntPtr g);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldSetHeightfieldData"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomHeightfieldSetHeightfieldData(IntPtr g, IntPtr d);
+
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomUbitTerrainDataBuild"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomOSTerrainDataBuild(IntPtr d, float[] pHeightData, int bCopyHeightData,
+                dReal sampleSize, int widthSamples, int depthSamples,
+                dReal offset, dReal thickness, int bWrap);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomOSTerrainDataBuild"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomOSTerrainDataBuild(IntPtr d, IntPtr pHeightData, int bCopyHeightData,
+                dReal sampleSize, int widthSamples, int depthSamples,
+                dReal thickness, int bWrap);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomOSTerrainDataCreate"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr GeomOSTerrainDataCreate();
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomOSTerrainDataDestroy"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomOSTerrainDataDestroy(IntPtr d);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomOSTerrainDataSetBounds"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomOSTerrainDataSetBounds(IntPtr d, dReal minHeight, dReal maxHeight);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomOSTerrainGetHeightfieldData"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr GeomOSTerrainGetHeightfieldData(IntPtr g);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomOSTerrainSetHeightfieldData"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomOSTerrainSetHeightfieldData(IntPtr g, IntPtr d);
+
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomIsEnabled"), SuppressUnmanagedCodeSecurity]
+        public static extern bool GeomIsEnabled(IntPtr geom);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomIsOffset"), SuppressUnmanagedCodeSecurity]
+        public static extern bool GeomIsOffset(IntPtr geom);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomIsSpace"), SuppressUnmanagedCodeSecurity]
+        public static extern bool GeomIsSpace(IntPtr geom);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomPlaneGetParams"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomPlaneGetParams(IntPtr geom, ref Vector4 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomPlaneGetParams"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomPlaneGetParams(IntPtr geom, ref dReal A);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomPlanePointDepth"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal GeomPlanePointDepth(IntPtr geom, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomPlaneSetParams"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomPlaneSetParams(IntPtr plane, dReal a, dReal b, dReal c, dReal d);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGet"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomRayGet(IntPtr ray, ref Vector3 start, ref Vector3 dir);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGet"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomRayGet(IntPtr ray, ref dReal startX, ref dReal dirX);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGetClosestHit"), SuppressUnmanagedCodeSecurity]
+        public static extern int GeomRayGetClosestHit(IntPtr ray);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGetLength"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal GeomRayGetLength(IntPtr ray);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGetParams"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal GeomRayGetParams(IntPtr g, out int firstContact, out int backfaceCull);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRaySet"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomRaySet(IntPtr ray, dReal px, dReal py, dReal pz, dReal dx, dReal dy, dReal dz);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRaySetClosestHit"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomRaySetClosestHit(IntPtr ray, int closestHit);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRaySetLength"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomRaySetLength(IntPtr ray, dReal length);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRaySetParams"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomRaySetParams(IntPtr ray, int firstContact, int backfaceCull);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetBody"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomSetBody(IntPtr geom, IntPtr body);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetCategoryBits"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomSetCategoryBits(IntPtr geom, uint bits);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetCollideBits"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomSetCollideBits(IntPtr geom, uint bits);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetConvex"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr GeomSetConvex(IntPtr geom, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetData"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomSetData(IntPtr geom, IntPtr data);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetPosition"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomSetOffsetPosition(IntPtr geom, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetQuaternion"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomSetOffsetQuaternion(IntPtr geom, ref Quaternion Q);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetQuaternion"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomSetOffsetQuaternion(IntPtr geom, ref dReal X);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetRotation"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomSetOffsetRotation(IntPtr geom, ref Matrix3 R);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetRotation"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomSetOffsetRotation(IntPtr geom, ref dReal M00);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldPosition"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomSetOffsetWorldPosition(IntPtr geom, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldQuaternion"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomSetOffsetWorldQuaternion(IntPtr geom, ref Quaternion Q);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldQuaternion"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomSetOffsetWorldQuaternion(IntPtr geom, ref dReal X);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldRotation"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomSetOffsetWorldRotation(IntPtr geom, ref Matrix3 R);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldRotation"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomSetOffsetWorldRotation(IntPtr geom, ref dReal M00);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetPosition"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomSetPosition(IntPtr geom, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetQuaternion"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomSetQuaternion(IntPtr geom, ref Quaternion quat);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetQuaternion"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomSetQuaternion(IntPtr geom, ref dReal w);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetRotation"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomSetRotation(IntPtr geom, ref Matrix3 R);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetRotation"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomSetRotation(IntPtr geom, ref dReal M00);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSphereGetRadius"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal GeomSphereGetRadius(IntPtr geom);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSpherePointDepth"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal GeomSpherePointDepth(IntPtr geom, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSphereSetRadius"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomSphereSetRadius(IntPtr geom, dReal radius);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformGetCleanup"), SuppressUnmanagedCodeSecurity]
+        public static extern int GeomTransformGetCleanup(IntPtr geom);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformGetGeom"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr GeomTransformGetGeom(IntPtr geom);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformGetInfo"), SuppressUnmanagedCodeSecurity]
+        public static extern int GeomTransformGetInfo(IntPtr geom);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformSetCleanup"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTransformSetCleanup(IntPtr geom, int mode);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformSetGeom"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTransformSetGeom(IntPtr geom, IntPtr obj);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformSetInfo"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTransformSetInfo(IntPtr geom, int info);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildDouble"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTriMeshDataBuildDouble(IntPtr d,
+            double[] vertices, int vertexStride, int vertexCount,
+            int[] indices, int indexCount, int triStride);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildDouble"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTriMeshDataBuildDouble(IntPtr d,
+            IntPtr vertices, int vertexStride, int vertexCount,
+            IntPtr indices, int indexCount, int triStride);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildDouble1"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTriMeshDataBuildDouble1(IntPtr d,
+            double[] vertices, int vertexStride, int vertexCount,
+            int[] indices, int indexCount, int triStride,
+            double[] normals);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildDouble1"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTriMeshDataBuildDouble(IntPtr d,
+            IntPtr vertices, int vertexStride, int vertexCount,
+            IntPtr indices, int indexCount, int triStride,
+            IntPtr normals);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSimple"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTriMeshDataBuildSingle(IntPtr d,
+            dReal[] vertices, int vertexStride, int vertexCount,
+            int[] indices, int indexCount, int triStride);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSimple"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTriMeshDataBuildSingle(IntPtr d,
+            IntPtr vertices, int vertexStride, int vertexCount,
+            IntPtr indices, int indexCount, int triStride);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSimple1"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTriMeshDataBuildSingle1(IntPtr d,
+            dReal[] vertices, int vertexStride, int vertexCount,
+            int[] indices, int indexCount, int triStride,
+            dReal[] normals);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSimple1"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTriMeshDataBuildSingle1(IntPtr d,
+            IntPtr vertices, int vertexStride, int vertexCount,
+            IntPtr indices, int indexCount, int triStride,
+            IntPtr normals);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSingle"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTriMeshDataBuildSimple(IntPtr d,
+            float[] vertices, int vertexStride, int vertexCount,
+            int[] indices, int indexCount, int triStride);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSingle"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTriMeshDataBuildSimple(IntPtr d,
+            IntPtr vertices, int vertexStride, int vertexCount,
+            IntPtr indices, int indexCount, int triStride);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSingle1"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTriMeshDataBuildSimple1(IntPtr d,
+            float[] vertices, int vertexStride, int vertexCount,
+            int[] indices, int indexCount, int triStride,
+            float[] normals);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSingle1"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTriMeshDataBuildSimple1(IntPtr d,
+            IntPtr vertices, int vertexStride, int vertexCount,
+            IntPtr indices, int indexCount, int triStride,
+            IntPtr normals);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshClearTCCache"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTriMeshClearTCCache(IntPtr g);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataCreate"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr GeomTriMeshDataCreate();
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataDestroy"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTriMeshDataDestroy(IntPtr d);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataGet"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr GeomTriMeshDataGet(IntPtr d, int data_id);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataPreprocess"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTriMeshDataPreprocess(IntPtr d);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataSet"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTriMeshDataSet(IntPtr d, int data_id, IntPtr in_data);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataUpdate"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTriMeshDataUpdate(IntPtr d);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshEnableTC"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTriMeshEnableTC(IntPtr g, int geomClass, bool enable);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetArrayCallback"), SuppressUnmanagedCodeSecurity]
+        public static extern TriArrayCallback GeomTriMeshGetArrayCallback(IntPtr g);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetCallback"), SuppressUnmanagedCodeSecurity]
+        public static extern TriCallback GeomTriMeshGetCallback(IntPtr g);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetData"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr GeomTriMeshGetData(IntPtr g);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetLastTransform"), SuppressUnmanagedCodeSecurity]
+        public extern unsafe static Matrix4* GeomTriMeshGetLastTransformUnsafe(IntPtr geom);
+        public static Matrix4 GeomTriMeshGetLastTransform(IntPtr geom)
+        {
+            unsafe { return *(GeomTriMeshGetLastTransformUnsafe(geom)); }
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetPoint"), SuppressUnmanagedCodeSecurity]
+        public extern static void GeomTriMeshGetPoint(IntPtr g, int index, dReal u, dReal v, ref Vector3 outVec);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetRayCallback"), SuppressUnmanagedCodeSecurity]
+        public static extern TriRayCallback GeomTriMeshGetRayCallback(IntPtr g);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetTriangle"), SuppressUnmanagedCodeSecurity]
+        public extern static void GeomTriMeshGetTriangle(IntPtr g, int index, ref Vector3 v0, ref Vector3 v1, ref Vector3 v2);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetTriangleCount"), SuppressUnmanagedCodeSecurity]
+        public extern static int GeomTriMeshGetTriangleCount(IntPtr g);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetTriMeshDataID"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr GeomTriMeshGetTriMeshDataID(IntPtr g);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshIsTCEnabled"), SuppressUnmanagedCodeSecurity]
+        public static extern bool GeomTriMeshIsTCEnabled(IntPtr g, int geomClass);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetArrayCallback"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTriMeshSetArrayCallback(IntPtr g, TriArrayCallback arrayCallback);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetCallback"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTriMeshSetCallback(IntPtr g, TriCallback callback);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetData"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTriMeshSetData(IntPtr g, IntPtr data);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetLastTransform"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTriMeshSetLastTransform(IntPtr g, ref Matrix4 last_trans);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetLastTransform"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTriMeshSetLastTransform(IntPtr g, ref dReal M00);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetRayCallback"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTriMeshSetRayCallback(IntPtr g, TriRayCallback callback);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGetConfiguration"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr iGetConfiguration();
+
+        public static string GetConfiguration()
+        {
+            IntPtr ptr = iGetConfiguration();
+            string s = Marshal.PtrToStringAnsi(ptr);
+            return s;
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dHashSpaceCreate"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr HashSpaceCreate(IntPtr space);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dHashSpaceGetLevels"), SuppressUnmanagedCodeSecurity]
+        public static extern void HashSpaceGetLevels(IntPtr space, out int minlevel, out int maxlevel);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dHashSpaceSetLevels"), SuppressUnmanagedCodeSecurity]
+        public static extern void HashSpaceSetLevels(IntPtr space, int minlevel, int maxlevel);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dInfiniteAABB"), SuppressUnmanagedCodeSecurity]
+        public static extern void InfiniteAABB(IntPtr geom, out AABB aabb);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dInitODE"), SuppressUnmanagedCodeSecurity]
+        public static extern void InitODE();
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dInitODE2"), SuppressUnmanagedCodeSecurity]
+        public static extern int InitODE2(uint ODEInitFlags);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dIsPositiveDefinite"), SuppressUnmanagedCodeSecurity]
+        public static extern int IsPositiveDefinite(ref dReal A, int n);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dInvertPDMatrix"), SuppressUnmanagedCodeSecurity]
+        public static extern int InvertPDMatrix(ref dReal A, out dReal Ainv, int n);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddAMotorTorques"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointAddAMotorTorques(IntPtr joint, dReal torque1, dReal torque2, dReal torque3);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddHingeTorque"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointAddHingeTorque(IntPtr joint, dReal torque);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddHinge2Torque"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointAddHinge2Torques(IntPtr joint, dReal torque1, dReal torque2);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddPRTorque"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointAddPRTorque(IntPtr joint, dReal torque);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddUniversalTorque"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointAddUniversalTorques(IntPtr joint, dReal torque1, dReal torque2);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddSliderForce"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointAddSliderForce(IntPtr joint, dReal force);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAttach"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointAttach(IntPtr joint, IntPtr body1, IntPtr body2);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateAMotor"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr JointCreateAMotor(IntPtr world, IntPtr group);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateBall"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr JointCreateBall(IntPtr world, IntPtr group);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateContact"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr JointCreateContact(IntPtr world, IntPtr group, ref Contact contact);
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateContact"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr JointCreateContactPtr(IntPtr world, IntPtr group, IntPtr contact);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateFixed"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr JointCreateFixed(IntPtr world, IntPtr group);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateHinge"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr JointCreateHinge(IntPtr world, IntPtr group);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateHinge2"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr JointCreateHinge2(IntPtr world, IntPtr group);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateLMotor"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr JointCreateLMotor(IntPtr world, IntPtr group);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateNull"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr JointCreateNull(IntPtr world, IntPtr group);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreatePR"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr JointCreatePR(IntPtr world, IntPtr group);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreatePlane2D"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr JointCreatePlane2D(IntPtr world, IntPtr group);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateSlider"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr JointCreateSlider(IntPtr world, IntPtr group);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateUniversal"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr JointCreateUniversal(IntPtr world, IntPtr group);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointDestroy"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointDestroy(IntPtr j);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorAngle"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal JointGetAMotorAngle(IntPtr j, int anum);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorAngleRate"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal JointGetAMotorAngleRate(IntPtr j, int anum);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorAxis"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointGetAMotorAxis(IntPtr j, int anum, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorAxisRel"), SuppressUnmanagedCodeSecurity]
+        public static extern int JointGetAMotorAxisRel(IntPtr j, int anum);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorMode"), SuppressUnmanagedCodeSecurity]
+        public static extern int JointGetAMotorMode(IntPtr j);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorNumAxes"), SuppressUnmanagedCodeSecurity]
+        public static extern int JointGetAMotorNumAxes(IntPtr j);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorParam"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal JointGetAMotorParam(IntPtr j, int parameter);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetBallAnchor"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointGetBallAnchor(IntPtr j, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetBallAnchor2"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointGetBallAnchor2(IntPtr j, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetBody"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr JointGetBody(IntPtr j);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetData"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr JointGetData(IntPtr j);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetFeedback"), SuppressUnmanagedCodeSecurity]
+        public extern unsafe static JointFeedback* JointGetFeedbackUnsafe(IntPtr j);
+        public static JointFeedback JointGetFeedback(IntPtr j)
+        {
+            unsafe { return *(JointGetFeedbackUnsafe(j)); }
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAnchor"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointGetHingeAnchor(IntPtr j, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAngle"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal JointGetHingeAngle(IntPtr j);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAngleRate"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal JointGetHingeAngleRate(IntPtr j);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAxis"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointGetHingeAxis(IntPtr j, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeParam"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal JointGetHingeParam(IntPtr j, int parameter);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Angle1"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal JointGetHinge2Angle1(IntPtr j);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Angle1Rate"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal JointGetHinge2Angle1Rate(IntPtr j);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Angle2Rate"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal JointGetHinge2Angle2Rate(IntPtr j);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAnchor2"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointGetHingeAnchor2(IntPtr j, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Anchor"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointGetHinge2Anchor(IntPtr j, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Anchor2"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointGetHinge2Anchor2(IntPtr j, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Axis1"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointGetHinge2Axis1(IntPtr j, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Axis2"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointGetHinge2Axis2(IntPtr j, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Param"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal JointGetHinge2Param(IntPtr j, int parameter);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetLMotorAxis"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointGetLMotorAxis(IntPtr j, int anum, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetLMotorNumAxes"), SuppressUnmanagedCodeSecurity]
+        public static extern int JointGetLMotorNumAxes(IntPtr j);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetLMotorParam"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal JointGetLMotorParam(IntPtr j, int parameter);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRAnchor"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointGetPRAnchor(IntPtr j, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRAxis1"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointGetPRAxis1(IntPtr j, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRAxis2"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointGetPRAxis2(IntPtr j, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRParam"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal JointGetPRParam(IntPtr j, int parameter);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRPosition"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal JointGetPRPosition(IntPtr j);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRPositionRate"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal JointGetPRPositionRate(IntPtr j);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetSliderAxis"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointGetSliderAxis(IntPtr j, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetSliderParam"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal JointGetSliderParam(IntPtr j, int parameter);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetSliderPosition"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal JointGetSliderPosition(IntPtr j);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetSliderPositionRate"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal JointGetSliderPositionRate(IntPtr j);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetType"), SuppressUnmanagedCodeSecurity]
+        public static extern JointType JointGetType(IntPtr j);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAnchor"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointGetUniversalAnchor(IntPtr j, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAnchor2"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointGetUniversalAnchor2(IntPtr j, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngle1"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal JointGetUniversalAngle1(IntPtr j);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngle1Rate"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal JointGetUniversalAngle1Rate(IntPtr j);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngle2"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal JointGetUniversalAngle2(IntPtr j);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngle2Rate"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal JointGetUniversalAngle2Rate(IntPtr j);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngles"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointGetUniversalAngles(IntPtr j, out dReal angle1, out dReal angle2);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAxis1"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointGetUniversalAxis1(IntPtr j, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAxis2"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointGetUniversalAxis2(IntPtr j, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalParam"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal JointGetUniversalParam(IntPtr j, int parameter);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGroupCreate"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr JointGroupCreate(int max_size);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGroupDestroy"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointGroupDestroy(IntPtr group);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGroupEmpty"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointGroupEmpty(IntPtr group);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorAngle"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetAMotorAngle(IntPtr j, int anum, dReal angle);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorAxis"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetAMotorAxis(IntPtr j, int anum, int rel, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorMode"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetAMotorMode(IntPtr j, int mode);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorNumAxes"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetAMotorNumAxes(IntPtr group, int num);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorParam"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetAMotorParam(IntPtr group, int parameter, dReal value);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetBallAnchor"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetBallAnchor(IntPtr j, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetBallAnchor2"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetBallAnchor2(IntPtr j, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetData"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetData(IntPtr j, IntPtr data);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetFeedback"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetFeedback(IntPtr j, out JointFeedback feedback);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetFixed"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetFixed(IntPtr j);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHingeAnchor"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetHingeAnchor(IntPtr j, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHingeAnchorDelta"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetHingeAnchorDelta(IntPtr j, dReal x, dReal y, dReal z, dReal ax, dReal ay, dReal az);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHingeAxis"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetHingeAxis(IntPtr j, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHingeParam"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetHingeParam(IntPtr j, int parameter, dReal value);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHinge2Anchor"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetHinge2Anchor(IntPtr j, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHinge2Axis1"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetHinge2Axis1(IntPtr j, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHinge2Axis2"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetHinge2Axis2(IntPtr j, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHinge2Param"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetHinge2Param(IntPtr j, int parameter, dReal value);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetLMotorAxis"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetLMotorAxis(IntPtr j, int anum, int rel, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetLMotorNumAxes"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetLMotorNumAxes(IntPtr j, int num);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetLMotorParam"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetLMotorParam(IntPtr j, int parameter, dReal value);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPlane2DAngleParam"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetPlane2DAngleParam(IntPtr j, int parameter, dReal value);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPlane2DXParam"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetPlane2DXParam(IntPtr j, int parameter, dReal value);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPlane2DYParam"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetPlane2DYParam(IntPtr j, int parameter, dReal value);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPRAnchor"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetPRAnchor(IntPtr j, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPRAxis1"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetPRAxis1(IntPtr j, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPRAxis2"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetPRAxis2(IntPtr j, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPRParam"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetPRParam(IntPtr j, int parameter, dReal value);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetSliderAxis"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetSliderAxis(IntPtr j, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetSliderAxisDelta"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetSliderAxisDelta(IntPtr j, dReal x, dReal y, dReal z, dReal ax, dReal ay, dReal az);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetSliderParam"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetSliderParam(IntPtr j, int parameter, dReal value);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetUniversalAnchor"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetUniversalAnchor(IntPtr j, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetUniversalAxis1"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetUniversalAxis1(IntPtr j, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetUniversalAxis2"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetUniversalAxis2(IntPtr j, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetUniversalParam"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetUniversalParam(IntPtr j, int parameter, dReal value);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dLDLTAddTL"), SuppressUnmanagedCodeSecurity]
+        public static extern void LDLTAddTL(ref dReal L, ref dReal d, ref dReal a, int n, int nskip);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassAdd"), SuppressUnmanagedCodeSecurity]
+        public static extern void MassAdd(ref Mass a, ref Mass b);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassAdjust"), SuppressUnmanagedCodeSecurity]
+        public static extern void MassAdjust(ref Mass m, dReal newmass);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassCheck"), SuppressUnmanagedCodeSecurity]
+        public static extern bool MassCheck(ref Mass m);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassRotate"), SuppressUnmanagedCodeSecurity]
+        public static extern void MassRotate(ref Mass mass, ref Matrix3 R);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassRotate"), SuppressUnmanagedCodeSecurity]
+        public static extern void MassRotate(ref Mass mass, ref dReal M00);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetBox"), SuppressUnmanagedCodeSecurity]
+        public static extern void MassSetBox(out Mass mass, dReal density, dReal lx, dReal ly, dReal lz);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetBoxTotal"), SuppressUnmanagedCodeSecurity]
+        public static extern void MassSetBoxTotal(out Mass mass, dReal total_mass, dReal lx, dReal ly, dReal lz);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetCapsule"), SuppressUnmanagedCodeSecurity]
+        public static extern void MassSetCapsule(out Mass mass, dReal density, int direction, dReal radius, dReal length);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetCapsuleTotal"), SuppressUnmanagedCodeSecurity]
+        public static extern void MassSetCapsuleTotal(out Mass mass, dReal total_mass, int direction, dReal radius, dReal length);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetCylinder"), SuppressUnmanagedCodeSecurity]
+        public static extern void MassSetCylinder(out Mass mass, dReal density, int direction, dReal radius, dReal length);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetCylinderTotal"), SuppressUnmanagedCodeSecurity]
+        public static extern void MassSetCylinderTotal(out Mass mass, dReal total_mass, int direction, dReal radius, dReal length);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetParameters"), SuppressUnmanagedCodeSecurity]
+        public static extern void MassSetParameters(out Mass mass, dReal themass,
+             dReal cgx, dReal cgy, dReal cgz,
+             dReal i11, dReal i22, dReal i33,
+             dReal i12, dReal i13, dReal i23);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetSphere"), SuppressUnmanagedCodeSecurity]
+        public static extern void MassSetSphere(out Mass mass, dReal density, dReal radius);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetSphereTotal"), SuppressUnmanagedCodeSecurity]
+        public static extern void dMassSetSphereTotal(out Mass mass, dReal total_mass, dReal radius);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetTrimesh"), SuppressUnmanagedCodeSecurity]
+        public static extern void MassSetTrimesh(out Mass mass, dReal density, IntPtr g);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetZero"), SuppressUnmanagedCodeSecurity]
+        public static extern void MassSetZero(out Mass mass);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassTranslate"), SuppressUnmanagedCodeSecurity]
+        public static extern void MassTranslate(ref Mass mass, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMultiply0"), SuppressUnmanagedCodeSecurity]
+        public static extern void Multiply0(out dReal A00, ref dReal B00, ref dReal C00, int p, int q, int r);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMultiply0"), SuppressUnmanagedCodeSecurity]
+        private static extern void MultiplyiM3V3(out Vector3 vout, ref Matrix3 matrix, ref Vector3 vect,int p, int q, int r);
+        public static void MultiplyM3V3(out Vector3 outvector, ref Matrix3 matrix, ref Vector3 invector)
+        {
+            MultiplyiM3V3(out outvector, ref matrix, ref invector, 3, 3, 1);
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMultiply1"), SuppressUnmanagedCodeSecurity]
+        public static extern void Multiply1(out dReal A00, ref dReal B00, ref dReal C00, int p, int q, int r);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMultiply2"), SuppressUnmanagedCodeSecurity]
+        public static extern void Multiply2(out dReal A00, ref dReal B00, ref dReal C00, int p, int q, int r);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQFromAxisAndAngle"), SuppressUnmanagedCodeSecurity]
+        public static extern void QFromAxisAndAngle(out Quaternion q, dReal ax, dReal ay, dReal az, dReal angle);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQfromR"), SuppressUnmanagedCodeSecurity]
+        public static extern void QfromR(out Quaternion q, ref Matrix3 R);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQMultiply0"), SuppressUnmanagedCodeSecurity]
+        public static extern void QMultiply0(out Quaternion qa, ref Quaternion qb, ref Quaternion qc);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQMultiply1"), SuppressUnmanagedCodeSecurity]
+        public static extern void QMultiply1(out Quaternion qa, ref Quaternion qb, ref Quaternion qc);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQMultiply2"), SuppressUnmanagedCodeSecurity]
+        public static extern void QMultiply2(out Quaternion qa, ref Quaternion qb, ref Quaternion qc);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQMultiply3"), SuppressUnmanagedCodeSecurity]
+        public static extern void QMultiply3(out Quaternion qa, ref Quaternion qb, ref Quaternion qc);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQSetIdentity"), SuppressUnmanagedCodeSecurity]
+        public static extern void QSetIdentity(out Quaternion q);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQuadTreeSpaceCreate"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr QuadTreeSpaceCreate(IntPtr space, ref Vector3 center, ref Vector3 extents, int depth);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQuadTreeSpaceCreate"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr QuadTreeSpaceCreate(IntPtr space, ref dReal centerX, ref dReal extentsX, int depth);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRandReal"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal RandReal();
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRFrom2Axes"), SuppressUnmanagedCodeSecurity]
+        public static extern void RFrom2Axes(out Matrix3 R, dReal ax, dReal ay, dReal az, dReal bx, dReal by, dReal bz);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRFromAxisAndAngle"), SuppressUnmanagedCodeSecurity]
+        public static extern void RFromAxisAndAngle(out Matrix3 R, dReal x, dReal y, dReal z, dReal angle);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRFromEulerAngles"), SuppressUnmanagedCodeSecurity]
+        public static extern void RFromEulerAngles(out Matrix3 R, dReal phi, dReal theta, dReal psi);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRfromQ"), SuppressUnmanagedCodeSecurity]
+        public static extern void RfromQ(out Matrix3 R, ref Quaternion q);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRFromZAxis"), SuppressUnmanagedCodeSecurity]
+        public static extern void RFromZAxis(out Matrix3 R, dReal ax, dReal ay, dReal az);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRSetIdentity"), SuppressUnmanagedCodeSecurity]
+        public static extern void RSetIdentity(out Matrix3 R);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSetValue"), SuppressUnmanagedCodeSecurity]
+        public static extern void SetValue(out dReal a, int n);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSetZero"), SuppressUnmanagedCodeSecurity]
+        public static extern void SetZero(out dReal a, int n);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSimpleSpaceCreate"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr SimpleSpaceCreate(IntPtr space);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSolveCholesky"), SuppressUnmanagedCodeSecurity]
+        public static extern void SolveCholesky(ref dReal L, out dReal b, int n);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSolveL1"), SuppressUnmanagedCodeSecurity]
+        public static extern void SolveL1(ref dReal L, out dReal b, int n, int nskip);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSolveL1T"), SuppressUnmanagedCodeSecurity]
+        public static extern void SolveL1T(ref dReal L, out dReal b, int n, int nskip);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSolveLDLT"), SuppressUnmanagedCodeSecurity]
+        public static extern void SolveLDLT(ref dReal L, ref dReal d, out dReal b, int n, int nskip);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceAdd"), SuppressUnmanagedCodeSecurity]
+        public static extern void SpaceAdd(IntPtr space, IntPtr geom);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceLockQuery"), SuppressUnmanagedCodeSecurity]
+        public static extern bool SpaceLockQuery(IntPtr space);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceClean"), SuppressUnmanagedCodeSecurity]
+        public static extern void SpaceClean(IntPtr space);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceCollide"), SuppressUnmanagedCodeSecurity]
+        public static extern void SpaceCollide(IntPtr space, IntPtr data, NearCallback callback);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceCollide2"), SuppressUnmanagedCodeSecurity]
+        public static extern void SpaceCollide2(IntPtr space1, IntPtr space2, IntPtr data, NearCallback callback);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceDestroy"), SuppressUnmanagedCodeSecurity]
+        public static extern void SpaceDestroy(IntPtr space);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceGetCleanup"), SuppressUnmanagedCodeSecurity]
+        public static extern bool SpaceGetCleanup(IntPtr space);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceGetNumGeoms"), SuppressUnmanagedCodeSecurity]
+        public static extern int SpaceGetNumGeoms(IntPtr space);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceGetGeom"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr SpaceGetGeom(IntPtr space, int i);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceGetSublevel"), SuppressUnmanagedCodeSecurity]
+        public static extern int SpaceGetSublevel(IntPtr space);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceQuery"), SuppressUnmanagedCodeSecurity]
+        public static extern bool SpaceQuery(IntPtr space, IntPtr geom);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceRemove"), SuppressUnmanagedCodeSecurity]
+        public static extern void SpaceRemove(IntPtr space, IntPtr geom);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceSetCleanup"), SuppressUnmanagedCodeSecurity]
+        public static extern void SpaceSetCleanup(IntPtr space, bool mode);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceSetSublevel"), SuppressUnmanagedCodeSecurity]
+        public static extern void SpaceSetSublevel(IntPtr space, int sublevel);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSweepAndPruneSpaceCreate"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr SweepAndPruneSpaceCreate(IntPtr space, int AxisOrder);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dVectorScale"), SuppressUnmanagedCodeSecurity]
+        public static extern void VectorScale(out dReal a, ref dReal d, int n);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldCreate"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr WorldCreate();
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldDestroy"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldDestroy(IntPtr world);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableAverageSamplesCount"), SuppressUnmanagedCodeSecurity]
+        public static extern int WorldGetAutoDisableAverageSamplesCount(IntPtr world);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableAngularThreshold"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal WorldGetAutoDisableAngularThreshold(IntPtr world);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableFlag"), SuppressUnmanagedCodeSecurity]
+        public static extern bool WorldGetAutoDisableFlag(IntPtr world);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableLinearThreshold"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal WorldGetAutoDisableLinearThreshold(IntPtr world);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableSteps"), SuppressUnmanagedCodeSecurity]
+        public static extern int WorldGetAutoDisableSteps(IntPtr world);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableTime"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal WorldGetAutoDisableTime(IntPtr world);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoEnableDepthSF1"), SuppressUnmanagedCodeSecurity]
+        public static extern int WorldGetAutoEnableDepthSF1(IntPtr world);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetCFM"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal WorldGetCFM(IntPtr world);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetERP"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal WorldGetERP(IntPtr world);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetGravity"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldGetGravity(IntPtr world, out Vector3 gravity);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetGravity"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldGetGravity(IntPtr world, out dReal X);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetContactMaxCorrectingVel"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal WorldGetContactMaxCorrectingVel(IntPtr world);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetContactSurfaceLayer"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal WorldGetContactSurfaceLayer(IntPtr world);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAngularDamping"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal WorldGetAngularDamping(IntPtr world);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAngularDampingThreshold"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal WorldGetAngularDampingThreshold(IntPtr world);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetLinearDamping"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal WorldGetLinearDamping(IntPtr world);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetLinearDampingThreshold"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal WorldGetLinearDampingThreshold(IntPtr world);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetQuickStepNumIterations"), SuppressUnmanagedCodeSecurity]
+        public static extern int WorldGetQuickStepNumIterations(IntPtr world);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetQuickStepW"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal WorldGetQuickStepW(IntPtr world);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetMaxAngularSpeed"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal WorldGetMaxAngularSpeed(IntPtr world);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldImpulseToForce"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldImpulseToForce(IntPtr world, dReal stepsize, dReal ix, dReal iy, dReal iz, out Vector3 force);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldImpulseToForce"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldImpulseToForce(IntPtr world, dReal stepsize, dReal ix, dReal iy, dReal iz, out dReal forceX);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldQuickStep"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldQuickStep(IntPtr world, dReal stepsize);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAngularDamping"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldSetAngularDamping(IntPtr world, dReal scale);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAngularDampingThreshold"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldSetAngularDampingThreshold(IntPtr world, dReal threshold);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableAngularThreshold"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldSetAutoDisableAngularThreshold(IntPtr world, dReal angular_threshold);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableAverageSamplesCount"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldSetAutoDisableAverageSamplesCount(IntPtr world, int average_samples_count);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableFlag"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldSetAutoDisableFlag(IntPtr world, bool do_auto_disable);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableLinearThreshold"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldSetAutoDisableLinearThreshold(IntPtr world, dReal linear_threshold);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableSteps"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldSetAutoDisableSteps(IntPtr world, int steps);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableTime"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldSetAutoDisableTime(IntPtr world, dReal time);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoEnableDepthSF1"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldSetAutoEnableDepthSF1(IntPtr world, int autoEnableDepth);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetCFM"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldSetCFM(IntPtr world, dReal cfm);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetContactMaxCorrectingVel"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldSetContactMaxCorrectingVel(IntPtr world, dReal vel);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetContactSurfaceLayer"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldSetContactSurfaceLayer(IntPtr world, dReal depth);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetDamping"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldSetDamping(IntPtr world, dReal linear_scale, dReal angular_scale);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetERP"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldSetERP(IntPtr world, dReal erp);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetGravity"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldSetGravity(IntPtr world, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetLinearDamping"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldSetLinearDamping(IntPtr world, dReal scale);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetLinearDampingThreshold"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldSetLinearDampingThreshold(IntPtr world, dReal threshold);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetQuickStepNumIterations"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldSetQuickStepNumIterations(IntPtr world, int num);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetQuickStepW"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldSetQuickStepW(IntPtr world, dReal over_relaxation);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetMaxAngularSpeed"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldSetMaxAngularSpeed(IntPtr world, dReal max_speed);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldStep"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldStep(IntPtr world, dReal stepsize);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldStepFast1"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldStepFast1(IntPtr world, dReal stepsize, int maxiterations);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldExportDIF"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldExportDIF(IntPtr world, string filename, bool append, string prefix);
+    }
+}
diff --git a/OpenSim/Region/PhysicsModules/Ode/ODECharacter.cs b/OpenSim/Region/PhysicsModules/Ode/ODECharacter.cs
index b35c299..98bfd1c 100644
--- a/OpenSim/Region/PhysicsModules/Ode/ODECharacter.cs
+++ b/OpenSim/Region/PhysicsModules/Ode/ODECharacter.cs
@@ -29,7 +29,6 @@ using System;
 using System.Collections.Generic;
 using System.Reflection;
 using OpenMetaverse;
-using Ode.NET;
 using OpenSim.Framework;
 using OpenSim.Region.PhysicsModules.SharedBase;
 using log4net;
@@ -150,7 +149,7 @@ namespace OpenSim.Region.PhysicsModule.ODE
         /// Collision geometry
         /// </summary>
         internal IntPtr Shell { get; private set; }
-        
+
         private IntPtr Amotor = IntPtr.Zero;
         private d.Mass ShellMass;
 
@@ -238,7 +237,7 @@ namespace OpenSim.Region.PhysicsModule.ODE
             m_tainted_isPhysical = true; // new tainted status: need to create ODE information
 
             _parent_scene.AddPhysicsActorTaint(this);
-            
+
             Name = avName;
         }
 
@@ -280,7 +279,7 @@ namespace OpenSim.Region.PhysicsModule.ODE
 
         public override bool IsPhysical
         {
-            get { return false; }
+            get { return m_isPhysical; }
             set { return; }
         }
 
@@ -462,7 +461,7 @@ namespace OpenSim.Region.PhysicsModule.ODE
                             value.Z = _parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
                         }
 
-                        m_taintPosition = value;                        
+                        m_taintPosition = value;
                         _parent_scene.AddPhysicsActorTaint(this);
                     }
                     else
@@ -595,7 +594,7 @@ namespace OpenSim.Region.PhysicsModule.ODE
 
         public override void delink() {}
 
-        public override void LockAngularMotion(Vector3 axis) {}
+        public override void LockAngularMotion(byte axislocks) {}
 
 //      This code is very useful. Written by DanX0r. We're just not using it right now.
 //      Commented out to prevent a warning.
@@ -797,7 +796,7 @@ namespace OpenSim.Region.PhysicsModule.ODE
         internal void Move(List<OdeCharacter> defects)
         {
             //  no lock; for now it's only called from within Simulate()
-            
+
             // If the PID Controller isn't active then we set our force
             // calculating base velocity to the current position
 
@@ -812,7 +811,7 @@ namespace OpenSim.Region.PhysicsModule.ODE
 
             d.Vector3 localpos = d.BodyGetPosition(Body);
             Vector3 localPos = new Vector3(localpos.X, localpos.Y, localpos.Z);
-            
+
             if (!localPos.IsFinite())
             {
                 m_log.WarnFormat(
@@ -1105,8 +1104,8 @@ namespace OpenSim.Region.PhysicsModule.ODE
 //          lock (OdeScene.UniversalColliderSyncObject)
             Shell = d.CreateCapsule(_parent_scene.space, CAPSULE_RADIUS, CAPSULE_LENGTH);
 
-            d.GeomSetCategoryBits(Shell, (int)m_collisionCategories);
-            d.GeomSetCollideBits(Shell, (int)m_collisionFlags);
+            d.GeomSetCategoryBits(Shell, (uint)m_collisionCategories);
+            d.GeomSetCollideBits(Shell, (uint)m_collisionFlags);
 
             d.MassSetCapsuleTotal(out ShellMass, m_mass, 2, CAPSULE_RADIUS, CAPSULE_LENGTH);
             Body = d.BodyCreate(_parent_scene.world);
@@ -1249,18 +1248,18 @@ namespace OpenSim.Region.PhysicsModule.ODE
         }
 
         public override Vector3 PIDTarget { set { return; } }
-        public override bool PIDActive 
-        { 
+        public override bool PIDActive
+        {
             get { return false; }
-            set { return; } 
+            set { return; }
         }
         public override float PIDTau { set { return; } }
 
         public override float PIDHoverHeight { set { return; } }
-        public override bool PIDHoverActive { set { return; } }
+        public override bool PIDHoverActive {get {return false;} set { return; } }
         public override PIDHoverType PIDHoverType { set { return; } }
         public override float PIDHoverTau { set { return; } }
-        
+
         public override Quaternion APIDTarget{ set { return; } }
 
         public override bool APIDActive{ set { return; } }
@@ -1291,7 +1290,7 @@ namespace OpenSim.Region.PhysicsModule.ODE
             m_eventsubscription = 0;
         }
 
-        internal void AddCollisionEvent(uint CollidedWith, ContactPoint contact)
+        public override void AddCollisionEvent(uint CollidedWith, ContactPoint contact)
         {
             if (m_eventsubscription > 0)
             {
@@ -1370,7 +1369,7 @@ namespace OpenSim.Region.PhysicsModule.ODE
 //                    m_log.DebugFormat(
 //                        "[ODE CHARACTER]: Changing capsule size from {0} to {1} for {2}",
 //                        CAPSULE_LENGTH, m_tainted_CAPSULE_LENGTH, Name);
-                    
+
                     m_pidControllerActive = true;
 
                     // no lock needed on _parent_scene.OdeLock because we are called from within the thread lock in OdePlugin's simulate()
diff --git a/OpenSim/Region/PhysicsModules/Ode/ODEDynamics.cs b/OpenSim/Region/PhysicsModules/Ode/ODEDynamics.cs
index 8f8e2bd..7e95d7f 100644
--- a/OpenSim/Region/PhysicsModules/Ode/ODEDynamics.cs
+++ b/OpenSim/Region/PhysicsModules/Ode/ODEDynamics.cs
@@ -44,10 +44,10 @@ using System.Reflection;
 using System.Runtime.InteropServices;
 using log4net;
 using OpenMetaverse;
-using Ode.NET;
 using OpenSim.Framework;
 using OpenSim.Region.PhysicsModules.SharedBase;
 
+
 namespace OpenSim.Region.PhysicsModule.ODE
 {
     public class ODEDynamics
@@ -607,6 +607,13 @@ namespace OpenSim.Region.PhysicsModule.ODE
             m_body = pBody;
         }
 
+        internal void Stop()
+        {
+            m_lastLinearVelocityVector = Vector3.Zero;
+            m_lastAngularVelocity = Vector3.Zero;
+            m_lastPositionVector = d.BodyGetPosition(Body);
+        }
+
         internal void Step(float pTimestep,  OdeScene pParentScene)
         {
             if (m_body == IntPtr.Zero || m_type == Vehicle.TYPE_NONE)
@@ -907,7 +914,7 @@ namespace OpenSim.Region.PhysicsModule.ODE
 
             // Sum velocities
             m_lastAngularVelocity = m_angularMotorVelocity + vertattr; // + bank + deflection
-            
+
             if ((m_flags & (VehicleFlag.NO_DEFLECTION_UP)) != 0)
             {
                 m_lastAngularVelocity.X = 0;
diff --git a/OpenSim/Region/PhysicsModules/Ode/ODEModule.cs b/OpenSim/Region/PhysicsModules/Ode/ODEModule.cs
new file mode 100644
index 0000000..22fc84d
--- /dev/null
+++ b/OpenSim/Region/PhysicsModules/Ode/ODEModule.cs
@@ -0,0 +1,89 @@
+using System;
+using System.Reflection;
+using log4net;
+using Nini.Config;
+using Mono.Addins;
+using OpenSim.Framework;
+using OpenSim.Region.Framework.Scenes;
+using OpenSim.Region.Framework.Interfaces;
+
+namespace OpenSim.Region.PhysicsModule.ODE
+{
+    [Extension(Path = "/OpenSim/RegionModules", NodeName = "RegionModule", Id = "ODEPhysicsScene")]
+    public class OdeModule : INonSharedRegionModule
+    {
+        private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
+
+        private bool m_Enabled = false;
+        private IConfigSource m_config;
+        private OdeScene  m_scene;
+
+       #region INonSharedRegionModule
+
+        public string Name
+        {
+            get { return "OpenDynamicsEngine"; }
+        }
+
+        public string Version
+        {
+            get { return "1.0"; }
+        }
+
+        public Type ReplaceableInterface
+        {
+            get { return null; }
+        }
+
+        public void Initialise(IConfigSource source)
+        {
+            IConfig config = source.Configs["Startup"];
+            if (config != null)
+            {
+                string physics = config.GetString("physics", string.Empty);
+                if (physics == Name)
+                {
+                    m_config = source;
+                    m_Enabled = true;
+                }
+            }
+        }
+
+        public void Close()
+        {
+        }
+
+        public void AddRegion(Scene scene)
+        {
+            if (!m_Enabled)
+                return;
+
+            if (Util.IsWindows())
+                Util.LoadArchSpecificWindowsDll("ode.dll");
+
+            // Initializing ODE only when a scene is created allows alternative ODE plugins to co-habit (according to
+            // http://opensimulator.org/mantis/view.php?id=2750).
+            d.InitODE();
+
+            m_scene = new OdeScene(scene, m_config, Name, Version);
+        }
+
+        public void RemoveRegion(Scene scene)
+        {
+            if (!m_Enabled || m_scene == null)
+                return;
+
+            m_scene.Dispose();
+            m_scene = null;
+        }
+
+        public void RegionLoaded(Scene scene)
+        {
+            if (!m_Enabled || m_scene == null)
+                return;
+
+            m_scene.RegionLoaded();
+        }
+        #endregion
+    }
+}
diff --git a/OpenSim/Region/PhysicsModules/Ode/ODEPrim.cs b/OpenSim/Region/PhysicsModules/Ode/ODEPrim.cs
index 0b9c45f..8934330 100644
--- a/OpenSim/Region/PhysicsModules/Ode/ODEPrim.cs
+++ b/OpenSim/Region/PhysicsModules/Ode/ODEPrim.cs
@@ -33,7 +33,7 @@
  * ODEDynamics.cs contains methods dealing with Prim Physical motion
  * (dynamics) and the associated settings. Old Linear and angular
  * motors for dynamic motion have been replace with  MoveLinear()
- * and MoveAngular(); 'Physical' is used only to switch ODE dynamic 
+ * and MoveAngular(); 'Physical' is used only to switch ODE dynamic
  * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_<other> is to
  * switch between 'VEHICLE' parameter use and general dynamics
  * settings use.
@@ -48,7 +48,6 @@ using System.Runtime.InteropServices;
 using System.Threading;
 using log4net;
 using OpenMetaverse;
-using Ode.NET;
 using OpenSim.Framework;
 using OpenSim.Region.PhysicsModules.SharedBase;
 
@@ -83,8 +82,15 @@ namespace OpenSim.Region.PhysicsModule.ODE
             set
             {
                 m_isphysical = value;
-                if (!m_isphysical) // Zero the remembered last velocity
+                if (!m_isphysical)
+                {
+                    _zeroFlag = true; // Zero the remembered last velocity
                     m_lastVelocity = Vector3.Zero;
+                    _acceleration = Vector3.Zero;
+                    _velocity = Vector3.Zero;
+                    m_taintVelocity = Vector3.Zero;
+                    m_rotationalVelocity = Vector3.Zero;
+                }
             }
         }
 
@@ -104,10 +110,12 @@ namespace OpenSim.Region.PhysicsModule.ODE
         private Vector3 m_taintVelocity;
         private Vector3 m_taintTorque;
         private Quaternion m_taintrot;
-        private Vector3 m_angularlock = Vector3.One;
-        private Vector3 m_taintAngularLock = Vector3.One;
+
         private IntPtr Amotor = IntPtr.Zero;
 
+        private byte m_taintAngularLock = 0;
+        private byte m_angularlock = 0;
+
         private bool m_assetFailed = false;
 
         private Vector3 m_PIDTarget;
@@ -125,7 +133,7 @@ namespace OpenSim.Region.PhysicsModule.ODE
         private float m_targetHoverHeight;
         private float m_groundHeight;
         private float m_waterHeight;
-        private float m_buoyancy;                //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle. 
+        private float m_buoyancy;                //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle.
 
         // private float m_tensor = 5f;
         private int body_autodisable_frames = 20;
@@ -162,7 +170,7 @@ namespace OpenSim.Region.PhysicsModule.ODE
 
         private PrimitiveBaseShape _pbs;
         private OdeScene _parent_scene;
-        
+
         /// <summary>
         /// The physics space which contains prim geometries
         /// </summary>
@@ -195,7 +203,6 @@ namespace OpenSim.Region.PhysicsModule.ODE
         public int m_interpenetrationcount { get; private set; }
         internal float m_collisionscore;
         public int m_roundsUnderMotionThreshold { get; private set; }
-        private int m_crossingfailures;
 
         public bool outofBounds { get; private set; }
         private float m_density = 10.000006836f; // Aluminum g/cm3;
@@ -350,12 +357,12 @@ namespace OpenSim.Region.PhysicsModule.ODE
             if (m_assetFailed)
             {
                 d.GeomSetCategoryBits(prim_geom, 0);
-                d.GeomSetCollideBits(prim_geom, BadMeshAssetCollideBits);
+                d.GeomSetCollideBits(prim_geom, (uint)BadMeshAssetCollideBits);
             }
             else
             {
-                d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
-                d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
+                d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories);
+                d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags);
             }
 
             _parent_scene.geom_name_map[prim_geom] = Name;
@@ -423,7 +430,7 @@ namespace OpenSim.Region.PhysicsModule.ODE
                 if (m_assetFailed)
                 {
                     d.GeomSetCategoryBits(prim_geom, 0);
-                    d.GeomSetCollideBits(prim_geom, BadMeshAssetCollideBits);
+                    d.GeomSetCollideBits(prim_geom, (uint)BadMeshAssetCollideBits);
                 }
                 else
                 {
@@ -431,12 +438,12 @@ namespace OpenSim.Region.PhysicsModule.ODE
                     m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
                 }
 
-                d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
-                d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
+                d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories);
+                d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags);
 
                 d.BodySetAutoDisableFlag(Body, true);
                 d.BodySetAutoDisableSteps(Body, body_autodisable_frames);
-                
+
                 // disconnect from world gravity so we can apply buoyancy
                 d.BodySetGravityMode (Body, false);
 
@@ -445,7 +452,7 @@ namespace OpenSim.Region.PhysicsModule.ODE
                 m_disabled = false;
 
                 // The body doesn't already have a finite rotation mode set here
-                if ((!m_angularlock.ApproxEquals(Vector3.One, 0.0f)) && _parent == null)
+                if (m_angularlock != 0 && _parent == null)
                 {
                     createAMotor(m_angularlock);
                 }
@@ -467,8 +474,8 @@ namespace OpenSim.Region.PhysicsModule.ODE
 
             float returnMass = 0;
             float hollowAmount = (float)_pbs.ProfileHollow * 2.0e-5f;
-            float hollowVolume = hollowAmount * hollowAmount; 
-            
+            float hollowVolume = hollowAmount * hollowAmount;
+
             switch (_pbs.ProfileShape)
             {
                 case ProfileShape.Square:
@@ -504,16 +511,16 @@ namespace OpenSim.Region.PhysicsModule.ODE
 
                     else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
                         {
-                        //a tube 
+                        //a tube
 
                         volume *= 0.78539816339e-2f * (float)(200 - _pbs.PathScaleX);
                         tmp= 1.0f -2.0e-2f * (float)(200 - _pbs.PathScaleY);
                         volume -= volume*tmp*tmp;
-                        
+
                         if (hollowAmount > 0.0)
                             {
                             hollowVolume *= hollowAmount;
-                            
+
                             switch (_pbs.HollowShape)
                                 {
                                 case HollowShape.Square:
@@ -572,7 +579,7 @@ namespace OpenSim.Region.PhysicsModule.ODE
                         volume *= 0.61685027506808491367715568749226e-2f * (float)(200 - _pbs.PathScaleX);
                         tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY);
                         volume *= (1.0f - tmp * tmp);
-                        
+
                         if (hollowAmount > 0.0)
                             {
 
@@ -816,8 +823,8 @@ namespace OpenSim.Region.PhysicsModule.ODE
                         }
                         else
                         {
-                            d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
-                            d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
+                            d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories);
+                            d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags);
                         }
 
                         d.BodyDestroy(Body);
@@ -838,7 +845,7 @@ namespace OpenSim.Region.PhysicsModule.ODE
                 else
                 {
                     _parent_scene.DeactivatePrim(this);
-                    
+
                     m_collisionCategories &= ~CollisionCategories.Body;
                     m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land);
 
@@ -850,8 +857,8 @@ namespace OpenSim.Region.PhysicsModule.ODE
                     else
                     {
 
-                        d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
-                        d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
+                        d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories);
+                        d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags);
                     }
 
                     Body = IntPtr.Zero;
@@ -909,7 +916,7 @@ namespace OpenSim.Region.PhysicsModule.ODE
                 else
                 {
                     _triMeshData = d.GeomTriMeshDataCreate();
-    
+
                     d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride);
                     d.GeomTriMeshDataPreprocess(_triMeshData);
                     m_MeshToTriMeshMap[mesh] = _triMeshData;
@@ -919,7 +926,7 @@ namespace OpenSim.Region.PhysicsModule.ODE
 //            _parent_scene.waitForSpaceUnlock(m_targetSpace);
             try
             {
-                SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, parent_scene.triCallback, null, null));
+                SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, null, null, null));
             }
             catch (AccessViolationException)
             {
@@ -967,7 +974,7 @@ Console.WriteLine("ZProcessTaints for " + Name);
                     rotate();
                 }
             }
-        
+
             if (m_taintPhysics != IsPhysical && !(m_taintparent != _parent))
                 changePhysicsStatus();
 
@@ -1001,7 +1008,7 @@ Console.WriteLine("ZProcessTaints for " + Name);
             if (m_taintCollidesWater != m_collidesWater)
                 changefloatonwater();
 
-            if (!m_angularlock.ApproxEquals(m_taintAngularLock,0f))
+            if (m_taintAngularLock != m_angularlock)
                 changeAngularLock();
         }
 
@@ -1017,10 +1024,8 @@ Console.WriteLine("ZProcessTaints for " + Name);
                 //If we have a parent then we're not authorative here
                 if (_parent == null)
                 {
-                    if (!m_taintAngularLock.ApproxEquals(Vector3.One, 0f))
+                    if (m_taintAngularLock != 0)
                     {
-                        //d.BodySetFiniteRotationMode(Body, 0);
-                        //d.BodySetFiniteRotationAxis(Body,m_taintAngularLock.X,m_taintAngularLock.Y,m_taintAngularLock.Z);
                         createAMotor(m_taintAngularLock);
                     }
                     else
@@ -1034,7 +1039,6 @@ Console.WriteLine("ZProcessTaints for " + Name);
                 }
             }
 
-            // Store this for later in case we get turned into a separate body
             m_angularlock = m_taintAngularLock;
         }
 
@@ -1070,7 +1074,7 @@ Console.WriteLine("ZProcessTaints for " + Name);
             else if (_parent != null && m_taintparent == null)
             {
 //Console.WriteLine("  changelink B");
-            
+
                 if (_parent is OdePrim)
                 {
                     OdePrim obj = (OdePrim)_parent;
@@ -1078,16 +1082,16 @@ Console.WriteLine("ZProcessTaints for " + Name);
                     childPrim = false;
                     //_parent = null;
                 }
-                
+
                 /*
                     if (Body != (IntPtr)0 && _linkJointGroup != (IntPtr)0)
                     d.JointGroupDestroy(_linkJointGroup);
-                        
+
                     _linkJointGroup = (IntPtr)0;
                     m_linkJoint = (IntPtr)0;
                 */
             }
- 
+
             _parent = m_taintparent;
             m_taintPhysics = IsPhysical;
         }
@@ -1145,12 +1149,12 @@ Console.WriteLine("ZProcessTaints for " + Name);
                     if (prm.m_assetFailed)
                     {
                         d.GeomSetCategoryBits(prm.prim_geom, 0);
-                        d.GeomSetCollideBits(prm.prim_geom, prm.BadMeshAssetCollideBits);
+                        d.GeomSetCollideBits(prm.prim_geom, (uint)prm.BadMeshAssetCollideBits);
                     }
                     else
                     {
-                        d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories);
-                        d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags);
+                        d.GeomSetCategoryBits(prm.prim_geom, (uint)prm.m_collisionCategories);
+                        d.GeomSetCollideBits(prm.prim_geom, (uint)prm.m_collisionFlags);
                     }
 
                     d.Quaternion quat = new d.Quaternion();
@@ -1194,14 +1198,14 @@ Console.WriteLine("ZProcessTaints for " + Name);
                 if (m_assetFailed)
                 {
                     d.GeomSetCategoryBits(prim_geom, 0);
-                    d.GeomSetCollideBits(prim_geom, BadMeshAssetCollideBits);
+                    d.GeomSetCollideBits(prim_geom, (uint)BadMeshAssetCollideBits);
                 }
                 else
                 {
                     //Console.WriteLine("GeomSetCategoryBits 2: " + prim_geom + " - " + (int)m_collisionCategories + " for " + Name);
-                    d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
+                    d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories);
                     //Console.WriteLine(" Post GeomSetCategoryBits 2");
-                    d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
+                    d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags);
                 }
 
                 d.Quaternion quat2 = new d.Quaternion();
@@ -1230,7 +1234,8 @@ Console.WriteLine("ZProcessTaints for " + Name);
                 m_disabled = false;
 
                 // The body doesn't already have a finite rotation mode set here
-                if ((!m_angularlock.ApproxEquals(Vector3.One, 0f)) && _parent == null)
+                // or remove
+                if (_parent == null)
                 {
                     createAMotor(m_angularlock);
                 }
@@ -1340,7 +1345,7 @@ Console.WriteLine("ZProcessTaints for " + Name);
                 // in between the disabling and the collision properties setting
                 // which would wake the physical body up from a soft disabling and potentially cause it to fall
                 // through the ground.
-                
+
                 // NOTE FOR JOINTS: this doesn't always work for jointed assemblies because if you select
                 // just one part of the assembly, the rest of the assembly is non-selected and still simulating,
                 // so that causes the selected part to wake up and continue moving.
@@ -1354,7 +1359,7 @@ Console.WriteLine("ZProcessTaints for " + Name);
                 // e.g. we select 100 prims that are connected by joints. non-atomically, the first 50 are
                 // selected and disabled. then, due to a thread switch, the selection processing is
                 // interrupted and the physics engine continues to simulate, so the last 50 items, whose
-                // selection was not yet processed, continues to simulate. this wakes up ALL of the 
+                // selection was not yet processed, continues to simulate. this wakes up ALL of the
                 // first 50 again. then the last 50 are disabled. then the first 50, which were just woken
                 // up, start simulating again, which in turn wakes up the last 50.
 
@@ -1370,8 +1375,8 @@ Console.WriteLine("ZProcessTaints for " + Name);
                 }
                 else
                 {
-                    d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
-                    d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
+                    d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories);
+                    d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags);
                 }
 
                 if (IsPhysical)
@@ -1396,12 +1401,12 @@ Console.WriteLine("ZProcessTaints for " + Name);
                 if (m_assetFailed)
                 {
                     d.GeomSetCategoryBits(prim_geom, 0);
-                    d.GeomSetCollideBits(prim_geom, BadMeshAssetCollideBits);
+                    d.GeomSetCollideBits(prim_geom, (uint)BadMeshAssetCollideBits);
                 }
                 else
                 {
-                    d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
-                    d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
+                    d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories);
+                    d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags);
                 }
 
                 if (IsPhysical)
@@ -1560,7 +1565,7 @@ Console.WriteLine("CreateGeom:");
         private void changeadd()
         {
 //            m_log.DebugFormat("[ODE PRIM]: Adding prim {0}", Name);
-            
+
             int[] iprimspaceArrItem = _parent_scene.calculateSpaceArrayItemFromPos(_position);
             IntPtr targetspace = _parent_scene.calculateSpaceForGeom(_position);
 
@@ -1686,6 +1691,9 @@ Console.WriteLine(" JointCreateFixed");
             float fy = 0;
             float fz = 0;
 
+            if (outofBounds)
+                return;
+
             if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim)        // KF: Only move root prims.
             {
                 if (m_vehicle.Type != Vehicle.TYPE_NONE)
@@ -1697,31 +1705,16 @@ Console.WriteLine(" JointCreateFixed");
                 {
 //Console.WriteLine("Move " +  Name);
                     if (!d.BodyIsEnabled (Body))  d.BodyEnable (Body); // KF add 161009
-                    // NON-'VEHICLES' are dealt with here
-//                    if (d.BodyIsEnabled(Body) && !m_angularlock.ApproxEquals(Vector3.Zero, 0.003f))
-//                    {
-//                        d.Vector3 avel2 = d.BodyGetAngularVel(Body);
-//                        /*
-//                        if (m_angularlock.X == 1)
-//                            avel2.X = 0;
-//                        if (m_angularlock.Y == 1)
-//                            avel2.Y = 0;
-//                        if (m_angularlock.Z == 1)
-//                            avel2.Z = 0;
-//                        d.BodySetAngularVel(Body, avel2.X, avel2.Y, avel2.Z);
-//                         */
-//                    }
-                    //float PID_P = 900.0f;
 
                     float m_mass = CalculateMass();
 
 //                    fz = 0f;
                     //m_log.Info(m_collisionFlags.ToString());
 
-                    
+
                     //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle.
                     // would come from SceneObjectPart.cs, public void SetBuoyancy(float fvalue) , PhysActor.Buoyancy = fvalue; ??
-                    // m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up 
+                    // m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up
                     // gravityz multiplier = 1 - m_buoyancy
                     fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass;
 
@@ -1736,7 +1729,7 @@ Console.WriteLine(" JointCreateFixed");
                         fz = 0f;
 
                         //  no lock; for now it's only called from within Simulate()
-    
+
                         // If the PID Controller isn't active then we set our force
                         // calculating base velocity to the current position
 
@@ -1745,7 +1738,7 @@ Console.WriteLine(" JointCreateFixed");
                             //PID_G = PID_G / m_PIDTau;
                             m_PIDTau = 1;
                         }
-    
+
                         if ((PID_G - m_PIDTau) <= 0)
                         {
                             PID_G = m_PIDTau + 1;
@@ -1768,7 +1761,7 @@ Console.WriteLine(" JointCreateFixed");
                         if (_target_velocity.ApproxEquals(Vector3.Zero,0.1f))
                         {
                             //  keep track of where we stopped.  No more slippin' & slidin'
-    
+
                             // We only want to deactivate the PID Controller if we think we want to have our surrogate
                             // react to the physics scene by moving it's position.
                             // Avatar to Avatar collisions
@@ -1789,7 +1782,7 @@ Console.WriteLine(" JointCreateFixed");
                             // We're flying and colliding with something
                             fx = ((_target_velocity.X) - vel.X) * (PID_D);
                             fy = ((_target_velocity.Y) - vel.Y) * (PID_D);
-    
+
                             // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P;
 
                             fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
@@ -1800,7 +1793,7 @@ Console.WriteLine(" JointCreateFixed");
                     if (m_useHoverPID && !PIDActive)
                     {
 //Console.WriteLine("Hover " +  Name);
-                    
+
                         // If we're using the PID controller, then we have no gravity
                         fz = (-1 * _parent_scene.gravityz) * m_mass;
 
@@ -1857,7 +1850,7 @@ Console.WriteLine(" JointCreateFixed");
                         if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f))
                         {
                             //  keep track of where we stopped.  No more slippin' & slidin'
-    
+
                             // We only want to deactivate the PID Controller if we think we want to have our surrogate
                             // react to the physics scene by moving it's position.
                             // Avatar to Avatar collisions
@@ -1895,7 +1888,7 @@ Console.WriteLine(" JointCreateFixed");
                         {
                             // A physical body at rest on a surface will auto-disable after a while,
                             // this appears to re-enable it incase the surface it is upon vanishes,
-                            // and the body should fall again. 
+                            // and the body should fall again.
                             d.BodySetLinearVel(Body, 0f, 0f, 0f);
                             d.BodySetForce(Body, 0, 0, 0);
                             enableBodySoft();
@@ -1904,7 +1897,7 @@ Console.WriteLine(" JointCreateFixed");
                         // 35x10 = 350n times the mass per second applied maximum.
                         float nmax = 35f * m_mass;
                         float nmin = -35f * m_mass;
-                    
+
                         if (fx > nmax)
                             fx = nmax;
                         if (fx < nmin)
@@ -1923,7 +1916,7 @@ Console.WriteLine(" JointCreateFixed");
               //  _zeroPosition = d.BodyGetPosition(Body);
                 return;
 //Console.WriteLine("Nothing " +  Name);
-               
+
             }
         }
 
@@ -1940,8 +1933,8 @@ Console.WriteLine(" JointCreateFixed");
                 d.BodySetQuaternion(Body, ref myrot);
                 if (IsPhysical)
                 {
-                    if (!m_angularlock.ApproxEquals(Vector3.One, 0f))
-                        createAMotor(m_angularlock);
+                    // create or remove locks
+                    createAMotor(m_angularlock);
                 }
             }
             else
@@ -1949,7 +1942,7 @@ Console.WriteLine(" JointCreateFixed");
                 // daughter prim, do Geom set
                 d.GeomSetQuaternion(prim_geom, ref myrot);
             }
-            
+
             resetCollisionAccounting();
             m_taintrot = _orientation;
         }
@@ -2087,7 +2080,7 @@ Console.WriteLine(" JointCreateFixed");
                     else
                         m_assetFailed = false;
                 }
-                    
+
             }
 
             CreateGeom(m_targetSpace, mesh);
@@ -2140,10 +2133,10 @@ Console.WriteLine(" JointCreateFixed");
             }
 
             if (m_assetFailed)
-                d.GeomSetCollideBits(prim_geom, BadMeshAssetCollideBits);
+                d.GeomSetCollideBits(prim_geom, (uint)BadMeshAssetCollideBits);
             else
 
-                d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
+                d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags);
         }
         /// <summary>
         /// Change prim in response to a shape taint.
@@ -2319,7 +2312,7 @@ Console.WriteLine(" JointCreateFixed");
                         }
                         d.BodyEnable(Body);
                         d.BodyAddTorque(Body, iforce.X, iforce.Y, iforce.Z);
-                        
+
                     }
                     m_angularforcelist.Clear();
                 }
@@ -2349,7 +2342,7 @@ Console.WriteLine(" JointCreateFixed");
                         d.BodySetLinearVel(Body, m_taintVelocity.X, m_taintVelocity.Y, m_taintVelocity.Z);
                     }
                 }
-                
+
                 //resetCollisionAccounting();
             }
 
@@ -2659,16 +2652,46 @@ Console.WriteLine(" JointCreateFixed");
 
         public override void CrossingFailure()
         {
-            m_crossingfailures++;
-            if (m_crossingfailures > _parent_scene.geomCrossingFailuresBeforeOutofbounds)
-            {
-                base.RaiseOutOfBounds(_position);
-                return;
-            }
-            else if (m_crossingfailures == _parent_scene.geomCrossingFailuresBeforeOutofbounds)
+            /*
+                        m_crossingfailures++;
+                        if (m_crossingfailures > _parent_scene.geomCrossingFailuresBeforeOutofbounds)
+                        {
+                            base.RaiseOutOfBounds(_position);
+                            return;
+                        }
+                        else if (m_crossingfailures == _parent_scene.geomCrossingFailuresBeforeOutofbounds)
+                        {
+                            m_log.Warn("[PHYSICS]: Too many crossing failures for: " + Name);
+                        }
+            */
+
+            d.AllocateODEDataForThread(0U);
+
+            _position.X = Util.Clip(_position.X, 0.5f, _parent_scene.WorldExtents.X - 0.5f);
+            _position.Y = Util.Clip(_position.Y, 0.5f, _parent_scene.WorldExtents.Y - 0.5f);
+            _position.Z = Util.Clip(_position.Z + 0.2f, -100f, 50000f);
+
+            m_lastposition = _position;
+            _velocity.X = 0;
+            _velocity.Y = 0;
+            _velocity.Z = 0;
+
+            m_lastVelocity = _velocity;
+
+            if (Body != IntPtr.Zero)
             {
-                m_log.Warn("[PHYSICS]: Too many crossing failures for: " + Name);
+                d.BodySetLinearVel(Body, 0, 0, 0); // stop it
+                d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
             }
+
+            if(m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE)
+                m_vehicle.Stop(); // this also updates vehicle last position from the body position
+
+            enableBodySoft();
+
+            outofBounds = false;
+            base.RequestPhysicsterseUpdate();
+
         }
 
         public override float Buoyancy
@@ -2687,32 +2710,23 @@ Console.WriteLine(" JointCreateFixed");
             m_taintparent = null;
         }
 
-        public override void LockAngularMotion(Vector3 axis)
+        public override void LockAngularMotion(byte axislocks)
         {
-            // reverse the zero/non zero values for ODE.
-            if (axis.IsFinite())
-            {
-                axis.X = (axis.X > 0) ? 1f : 0f;
-                axis.Y = (axis.Y > 0) ? 1f : 0f;
-                axis.Z = (axis.Z > 0) ? 1f : 0f;
-                m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z);
-                m_taintAngularLock = axis;
-            }
-            else
-            {
-                m_log.WarnFormat("[PHYSICS]: Got NaN locking axis from Scene on Object {0}", Name);
-            }
+            // m_log.DebugFormat("[axislocks]: {0}", axislocks);
+            m_taintAngularLock = axislocks;
         }
 
         internal void UpdatePositionAndVelocity()
         {
             //  no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit!
+            if (outofBounds)
+                return;
             if (_parent == null)
             {
                 Vector3 pv = Vector3.Zero;
                 bool lastZeroFlag = _zeroFlag;
                 float m_minvelocity = 0;
-                if (Body != (IntPtr)0) // FIXME -> or if it is a joint
+                if (Body != IntPtr.Zero) // FIXME -> or if it is a joint
                 {
                     d.Vector3 vec = d.BodyGetPosition(Body);
                     d.Quaternion ori = d.BodyGetQuaternion(Body);
@@ -2723,12 +2737,6 @@ Console.WriteLine(" JointCreateFixed");
                     Vector3 l_position = Vector3.Zero;
                     Quaternion l_orientation = Quaternion.Identity;
 
-                    //  kluge to keep things in bounds.  ODE lets dead avatars drift away (they should be removed!)
-                    //if (vec.X < 0.0f) { vec.X = 0.0f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); }
-                    //if (vec.Y < 0.0f) { vec.Y = 0.0f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); }
-                    //if (vec.X > 255.95f) { vec.X = 255.95f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); }
-                    //if (vec.Y > 255.95f) { vec.Y = 255.95f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); }
-
                     m_lastposition = _position;
                     m_lastorientation = _orientation;
 
@@ -2740,26 +2748,6 @@ Console.WriteLine(" JointCreateFixed");
                     l_orientation.Z = ori.Z;
                     l_orientation.W = ori.W;
 
-                    if (l_position.X > ((int)_parent_scene.WorldExtents.X - 0.05f) || l_position.X < 0f || l_position.Y > ((int)_parent_scene.WorldExtents.Y - 0.05f) || l_position.Y < 0f)
-                    {
-                        //base.RaiseOutOfBounds(l_position);
-
-                        if (m_crossingfailures < _parent_scene.geomCrossingFailuresBeforeOutofbounds)
-                        {
-                            _position = l_position;
-                            //_parent_scene.remActivePrim(this);
-                            if (_parent == null)
-                                base.RequestPhysicsterseUpdate();
-                            return;
-                        }
-                        else
-                        {
-                            if (_parent == null)
-                                base.RaiseOutOfBounds(l_position);
-                            return;
-                        }
-                    }
-
                     if (l_position.Z < 0)
                     {
                         // This is so prim that get lost underground don't fall forever and suck up
@@ -2769,8 +2757,6 @@ Console.WriteLine(" JointCreateFixed");
                         // It's a hack and will generate a console message if it fails.
 
                         //IsPhysical = false;
-                        if (_parent == null)
-                            base.RaiseOutOfBounds(_position);
 
                         _acceleration.X = 0;
                         _acceleration.Y = 0;
@@ -2784,16 +2770,65 @@ Console.WriteLine(" JointCreateFixed");
                         m_rotationalVelocity.Z = 0;
 
                         if (_parent == null)
+                            base.RaiseOutOfBounds(_position);
+
+                        if (_parent == null)
                             base.RequestPhysicsterseUpdate();
 
                         m_throttleUpdates = false;
                         throttleCounter = 0;
                         _zeroFlag = true;
                         //outofBounds = true;
+                        return;
+                    }
+
+                    if (l_position.X > ((int)_parent_scene.WorldExtents.X - 0.05f) || l_position.X < 0f || l_position.Y > ((int)_parent_scene.WorldExtents.Y - 0.05f) || l_position.Y < 0f)
+                    {
+                        //base.RaiseOutOfBounds(l_position);
+                        /*
+                                                if (m_crossingfailures < _parent_scene.geomCrossingFailuresBeforeOutofbounds)
+                                                {
+                                                    _position = l_position;
+                                                    //_parent_scene.remActivePrim(this);
+                                                    if (_parent == null)
+                                                        base.RequestPhysicsterseUpdate();
+                                                    return;
+                                                }
+                                                else
+                                                {
+                                                    if (_parent == null)
+                                                        base.RaiseOutOfBounds(l_position);
+                                                    return;
+                                                }
+                        */
+                        outofBounds = true;
+                        // part near the border on outside
+                        if (l_position.X < 0)
+                            Util.Clamp(l_position.X, -0.1f, -2f);
+                        else
+                            Util.Clamp(l_position.X, _parent_scene.WorldExtents.X + 0.1f, _parent_scene.WorldExtents.X + 2f);
+                        if (l_position.Y < 0)
+                            Util.Clamp(l_position.Y, -0.1f, -2f);
+                        else
+                            Util.Clamp(l_position.Y, _parent_scene.WorldExtents.Y + 0.1f, _parent_scene.WorldExtents.Y + 2f);
+
+                        d.BodySetPosition(Body, l_position.X, l_position.Y, l_position.Z);
+
+                        // stop it
+                        d.BodySetAngularVel(Body, 0, 0, 0);
+                        d.BodySetLinearVel(Body, 0, 0, 0);
+                        disableBodySoft();
+
+                        _position = l_position;
+                        // tell framework to fix it
+                        if (_parent == null)
+                            base.RequestPhysicsterseUpdate();
+                        return;
                     }
 
+
                     //float Adiff = 1.0f - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation));
-//Console.WriteLine("Adiff " + Name + " = " + Adiff);
+                    //Console.WriteLine("Adiff " + Name + " = " + Adiff);
                     if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02)
                         && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02)
                         && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02)
@@ -2864,12 +2899,12 @@ Console.WriteLine(" JointCreateFixed");
                         _acceleration = ((_velocity - m_lastVelocity) / 0.1f);
                         _acceleration = new Vector3(_velocity.X - m_lastVelocity.X / 0.1f, _velocity.Y - m_lastVelocity.Y / 0.1f, _velocity.Z - m_lastVelocity.Z / 0.1f);
                         //m_log.Info("[PHYSICS]: V1: " + _velocity + " V2: " + m_lastVelocity + " Acceleration: " + _acceleration.ToString());
-                       
-                        // Note here that linearvelocity is affecting angular velocity...  so I'm guessing this is a vehicle specific thing... 
-                        // it does make sense to do this for tiny little instabilities with physical prim, however 0.5m/frame is fairly large. 
+
+                        // Note here that linearvelocity is affecting angular velocity...  so I'm guessing this is a vehicle specific thing...
+                        // it does make sense to do this for tiny little instabilities with physical prim, however 0.5m/frame is fairly large.
                         // reducing this to 0.02m/frame seems to help the angular rubberbanding quite a bit, however, to make sure it doesn't affect elevators and vehicles
                         // adding these logical exclusion situations to maintain this where I think it was intended to be.
-                        if (m_throttleUpdates || PIDActive || (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) || (Amotor != IntPtr.Zero)) 
+                        if (m_throttleUpdates || PIDActive || (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) || (Amotor != IntPtr.Zero))
                         {
                             m_minvelocity = 0.5f;
                         }
@@ -2938,8 +2973,8 @@ Console.WriteLine(" JointCreateFixed");
         {
         }
 
-        public override Vector3 PIDTarget 
-        { 
+        public override Vector3 PIDTarget
+        {
             set
             {
                 if (value.IsFinite())
@@ -2948,16 +2983,17 @@ Console.WriteLine(" JointCreateFixed");
                 }
                 else
                     m_log.WarnFormat("[PHYSICS]: Got NaN PIDTarget from Scene on Object {0}", Name);
-            } 
+            }
         }
+
         public override bool PIDActive { get; set; }
         public override float PIDTau { set { m_PIDTau = value; } }
 
         public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } }
-        public override bool PIDHoverActive { set { m_useHoverPID = value; } }
+        public override bool PIDHoverActive { get { return m_useHoverPID;} set { m_useHoverPID = value; } }
         public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } }
         public override float PIDHoverTau { set { m_PIDHoverTau = value; } }
-        
+
         public override Quaternion APIDTarget{ set { return; } }
 
         public override bool APIDActive{ set { return; } }
@@ -2966,7 +3002,7 @@ Console.WriteLine(" JointCreateFixed");
 
         public override float APIDDamping{ set { return; } }
 
-        private void createAMotor(Vector3 axis)
+        private void createAMotor(byte axislock)
         {
             if (Body == IntPtr.Zero)
                 return;
@@ -2977,150 +3013,105 @@ Console.WriteLine(" JointCreateFixed");
                 Amotor = IntPtr.Zero;
             }
 
-            float axisnum = 3;
-
-            axisnum = (axisnum - (axis.X + axis.Y + axis.Z));
-
-            // PhysicsVector totalSize = new PhysicsVector(_size.X, _size.Y, _size.Z);
-
-            
-            // Inverse Inertia Matrix, set the X, Y, and/r Z inertia to 0 then invert it again.
-            d.Mass objMass;
-            d.MassSetZero(out objMass);
-            DMassCopy(ref pMass, ref objMass);
-
-            //m_log.DebugFormat("1-{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, ", objMass.I.M00, objMass.I.M01, objMass.I.M02, objMass.I.M10, objMass.I.M11, objMass.I.M12, objMass.I.M20, objMass.I.M21, objMass.I.M22);
-
-            Matrix4 dMassMat = FromDMass(objMass);
-
-            Matrix4 mathmat = Inverse(dMassMat);
-
-            /*
-            //m_log.DebugFormat("2-{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, ", mathmat[0, 0], mathmat[0, 1], mathmat[0, 2], mathmat[1, 0], mathmat[1, 1], mathmat[1, 2], mathmat[2, 0], mathmat[2, 1], mathmat[2, 2]);
-
-            mathmat = Inverse(mathmat);
-
-
-            objMass = FromMatrix4(mathmat, ref objMass);
-            //m_log.DebugFormat("3-{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, ", objMass.I.M00, objMass.I.M01, objMass.I.M02, objMass.I.M10, objMass.I.M11, objMass.I.M12, objMass.I.M20, objMass.I.M21, objMass.I.M22);
-
-            mathmat = Inverse(mathmat);
-            */
-            if (axis.X == 0)
-            {
-                mathmat.M33 = 50.0000001f;
-                //objMass.I.M22 = 0;
-            }
-            if (axis.Y == 0)
-            {
-                mathmat.M22 = 50.0000001f;
-                //objMass.I.M11 = 0;
-            }
-            if (axis.Z == 0)
-            {
-                mathmat.M11 = 50.0000001f;
-                //objMass.I.M00 = 0;
-            }
-            
-            
+            if(axislock == 0)
+                return;
 
-            mathmat = Inverse(mathmat);
-            objMass = FromMatrix4(mathmat, ref objMass);
-            //m_log.DebugFormat("4-{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, ", objMass.I.M00, objMass.I.M01, objMass.I.M02, objMass.I.M10, objMass.I.M11, objMass.I.M12, objMass.I.M20, objMass.I.M21, objMass.I.M22);
-           
-            //return;
-            if (d.MassCheck(ref objMass))
-            {
-                d.BodySetMass(Body, ref objMass);
-            }
-            else
-            {
-                //m_log.Debug("[PHYSICS]: Mass invalid, ignoring");
-            }
+            int axisnum = 0;
+            bool axisX = false;
+            bool axisY = false;
+            bool axisZ = false;
+            if((axislock & 0x02) != 0)
+                {
+                axisnum++;
+                axisX = true;
+                }
+            if((axislock & 0x04) != 0)
+                {
+                axisnum++;
+                axisY = true;
+                }
+            if((axislock & 0x08) != 0)
+                {
+                axisnum++;
+                axisZ = true;
+                }
 
-            if (axisnum <= 0)
+            if(axisnum == 0)
                 return;
-            // int dAMotorEuler = 1;
+            // stop it
+            d.BodySetTorque(Body, 0, 0, 0);
+            d.BodySetAngularVel(Body, 0, 0, 0);
 
             Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero);
             d.JointAttach(Amotor, Body, IntPtr.Zero);
+
             d.JointSetAMotorMode(Amotor, 0);
 
-            d.JointSetAMotorNumAxes(Amotor,(int)axisnum);
-            int i = 0;
+            d.JointSetAMotorNumAxes(Amotor, axisnum);
 
-            if (axis.X == 0)
-            {
-                d.JointSetAMotorAxis(Amotor, i, 0, 1, 0, 0);
-                i++;
-            }
+            // get current orientation to lock
 
-            if (axis.Y == 0)
-            {
-                d.JointSetAMotorAxis(Amotor, i, 0, 0, 1, 0);
-                i++;
-            }
+            d.Quaternion dcur = d.BodyGetQuaternion(Body);
+            Quaternion curr; // crap convertion between identical things
+            curr.X = dcur.X;
+            curr.Y = dcur.Y;
+            curr.Z = dcur.Z;
+            curr.W = dcur.W;
+            Vector3 ax;
 
-            if (axis.Z == 0)
-            {
-                d.JointSetAMotorAxis(Amotor, i, 0, 0, 0, 1);
+            int i = 0;
+            int j = 0;
+            if (axisX)
+            {
+                ax = (new Vector3(1, 0, 0)) * curr; // rotate world X to current local X
+                d.JointSetAMotorAxis(Amotor, 0, 0, ax.X, ax.Y, ax.Z);
+                d.JointSetAMotorAngle(Amotor, 0, 0);
+                d.JointSetAMotorParam(Amotor, (int)d.JointParam.LoStop, 0f);
+                d.JointSetAMotorParam(Amotor, (int)d.JointParam.HiStop, 0f);
+                d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel, 0);
+                d.JointSetAMotorParam(Amotor, (int)d.JointParam.FudgeFactor, 0.0001f);
+                d.JointSetAMotorParam(Amotor, (int)d.JointParam.Bounce, 0f);
+                d.JointSetAMotorParam(Amotor, (int)d.JointParam.CFM, 0f);
+                d.JointSetAMotorParam(Amotor, (int)d.JointParam.FMax, 5e8f);
+                d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopCFM, 0f);
+                d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopERP, 0.8f);
+                i++;
+                j = 256; // move to next axis set
+            }
+
+            if (axisY)
+            {
+                ax = (new Vector3(0, 1, 0)) * curr;
+                d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z);
+                d.JointSetAMotorAngle(Amotor, i, 0);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, 0f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Vel, 0);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.CFM, 0f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopCFM, 0f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f);
                 i++;
+                j += 256;
             }
 
-            for (int j = 0; j < (int)axisnum; j++)
+            if (axisZ)
             {
-                //d.JointSetAMotorAngle(Amotor, j, 0);
+                ax = (new Vector3(0, 0, 1)) * curr;
+                d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z);
+                d.JointSetAMotorAngle(Amotor, i, 0);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, 0f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Vel, 0);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.CFM, 0f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopCFM, 0f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f);
             }
-
-            //d.JointSetAMotorAngle(Amotor, 1, 0);
-            //d.JointSetAMotorAngle(Amotor, 2, 0);
-
-            // These lowstops and high stops are effectively (no wiggle room)
-            d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, -0f);
-            d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -0f);
-            d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, -0f);
-            d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0f);
-            d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0f);
-            d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0f);
-            //d.JointSetAMotorParam(Amotor, (int) dParam.Vel, 9000f);
-            d.JointSetAMotorParam(Amotor, (int)dParam.FudgeFactor, 0f);
-            d.JointSetAMotorParam(Amotor, (int)dParam.FMax, Mass * 50f);//
-        }
-
-        private Matrix4 FromDMass(d.Mass pMass)
-        {
-            Matrix4 obj;
-            obj.M11 = pMass.I.M00;
-            obj.M12 = pMass.I.M01;
-            obj.M13 = pMass.I.M02;
-            obj.M14 = 0;
-            obj.M21 = pMass.I.M10;
-            obj.M22 = pMass.I.M11;
-            obj.M23 = pMass.I.M12;
-            obj.M24 = 0;
-            obj.M31 = pMass.I.M20;
-            obj.M32 = pMass.I.M21;
-            obj.M33 = pMass.I.M22;
-            obj.M34 = 0;
-            obj.M41 = 0;
-            obj.M42 = 0;
-            obj.M43 = 0;
-            obj.M44 = 1;
-            return obj;
-        }
-
-        private d.Mass FromMatrix4(Matrix4 pMat, ref d.Mass obj)
-        {
-            obj.I.M00 = pMat[0, 0];
-            obj.I.M01 = pMat[0, 1];
-            obj.I.M02 = pMat[0, 2];
-            obj.I.M10 = pMat[1, 0];
-            obj.I.M11 = pMat[1, 1];
-            obj.I.M12 = pMat[1, 2];
-            obj.I.M20 = pMat[2, 0];
-            obj.I.M21 = pMat[2, 1];
-            obj.I.M22 = pMat[2, 2];
-            return obj;
         }
 
         public override void SubscribeEvents(int ms)
@@ -3135,7 +3126,7 @@ Console.WriteLine(" JointCreateFixed");
             m_eventsubscription = 0;
         }
 
-        public void AddCollisionEvent(uint CollidedWith, ContactPoint contact)
+        public override void AddCollisionEvent(uint CollidedWith, ContactPoint contact)
         {
             CollisionEventsThisFrame.AddCollider(CollidedWith, contact);
         }
@@ -3165,173 +3156,6 @@ Console.WriteLine(" JointCreateFixed");
             return false;
         }
 
-        public static Matrix4 Inverse(Matrix4 pMat)
-        {
-            if (determinant3x3(pMat) == 0)
-            {
-                return Matrix4.Identity; // should probably throw an error.  singluar matrix inverse not possible
-            }
-
-            return (Adjoint(pMat) / determinant3x3(pMat));
-        }
-
-        public static Matrix4 Adjoint(Matrix4 pMat)
-        {
-            Matrix4 adjointMatrix = new Matrix4();
-            for (int i=0; i<4; i++)
-            {
-                for (int j=0; j<4; j++)
-                {
-                    Matrix4SetValue(ref adjointMatrix, i, j, (float)(Math.Pow(-1, i + j) * (determinant3x3(Minor(pMat, i, j)))));
-                }
-            }
-
-            adjointMatrix = Transpose(adjointMatrix);
-            return adjointMatrix;
-        }
-
-        public static Matrix4 Minor(Matrix4 matrix, int iRow, int iCol)
-        {
-            Matrix4 minor = new Matrix4();
-            int m = 0, n = 0;
-            for (int i = 0; i < 4; i++)
-            {
-                if (i == iRow)
-                    continue;
-                n = 0;
-                for (int j = 0; j < 4; j++)
-                {
-                    if (j == iCol)
-                        continue;
-                    Matrix4SetValue(ref minor, m,n, matrix[i, j]);
-                    n++;
-                }
-                m++;
-            }
-
-            return minor;
-        }
-
-        public static Matrix4 Transpose(Matrix4 pMat)
-        {
-            Matrix4 transposeMatrix = new Matrix4();
-            for (int i = 0; i < 4; i++)
-                for (int j = 0; j < 4; j++)
-                    Matrix4SetValue(ref transposeMatrix, i, j, pMat[j, i]);
-            return transposeMatrix;
-        }
-
-        public static void Matrix4SetValue(ref Matrix4 pMat, int r, int c, float val)
-        {
-            switch (r)
-            {
-                case 0:
-                    switch (c)
-                    {
-                        case 0:
-                            pMat.M11 = val;
-                            break;
-                        case 1:
-                            pMat.M12 = val;
-                            break;
-                        case 2:
-                            pMat.M13 = val;
-                            break;
-                        case 3:
-                            pMat.M14 = val;
-                            break;
-                    }
-
-                    break;
-                case 1:
-                    switch (c)
-                    {
-                        case 0:
-                            pMat.M21 = val;
-                            break;
-                        case 1:
-                            pMat.M22 = val;
-                            break;
-                        case 2:
-                            pMat.M23 = val;
-                            break;
-                        case 3:
-                            pMat.M24 = val;
-                            break;
-                    }
-
-                    break;
-                case 2:
-                    switch (c)
-                    {
-                        case 0:
-                            pMat.M31 = val;
-                            break;
-                        case 1:
-                            pMat.M32 = val;
-                            break;
-                        case 2:
-                            pMat.M33 = val;
-                            break;
-                        case 3:
-                            pMat.M34 = val;
-                            break;
-                    }
-
-                    break;
-                case 3:
-                    switch (c)
-                    {
-                        case 0:
-                            pMat.M41 = val;
-                            break;
-                        case 1:
-                            pMat.M42 = val;
-                            break;
-                        case 2:
-                            pMat.M43 = val;
-                            break;
-                        case 3:
-                            pMat.M44 = val;
-                            break;
-                    }
-
-                    break;
-            }
-        }
-
-        private static float determinant3x3(Matrix4 pMat)
-        {
-            float det = 0;
-            float diag1 = pMat[0, 0]*pMat[1, 1]*pMat[2, 2];
-            float diag2 = pMat[0, 1]*pMat[2, 1]*pMat[2, 0];
-            float diag3 = pMat[0, 2]*pMat[1, 0]*pMat[2, 1];
-            float diag4 = pMat[2, 0]*pMat[1, 1]*pMat[0, 2];
-            float diag5 = pMat[2, 1]*pMat[1, 2]*pMat[0, 0];
-            float diag6 = pMat[2, 2]*pMat[1, 0]*pMat[0, 1];
-
-            det = diag1 + diag2 + diag3 - (diag4 + diag5 + diag6);
-            return det;
-        }
-        
-        private static void DMassCopy(ref d.Mass src, ref d.Mass dst)
-        {
-            dst.c.W = src.c.W;
-            dst.c.X = src.c.X;
-            dst.c.Y = src.c.Y;
-            dst.c.Z = src.c.Z;
-            dst.mass = src.mass;
-            dst.I.M00 = src.I.M00;
-            dst.I.M01 = src.I.M01;
-            dst.I.M02 = src.I.M02;
-            dst.I.M10 = src.I.M10;
-            dst.I.M11 = src.I.M11;
-            dst.I.M12 = src.I.M12;
-            dst.I.M20 = src.I.M20;
-            dst.I.M21 = src.I.M21;
-            dst.I.M22 = src.I.M22;
-        }
-
         public override void SetMaterial(int pMaterial)
         {
             m_material = pMaterial;
@@ -3365,7 +3189,7 @@ Console.WriteLine(" JointCreateFixed");
 //                m_assetFailed = false;
 
 //                m_log.DebugFormat(
-//                    "[ODE PRIM]: Received mesh/sculpt data asset {0} with {1} bytes for {2} at {3} in {4}", 
+//                    "[ODE PRIM]: Received mesh/sculpt data asset {0} with {1} bytes for {2} at {3} in {4}",
 //                    _pbs.SculptTexture, _pbs.SculptData.Length, Name, _position, _parent_scene.Name);
 
                 m_taintshape = true;
@@ -3377,6 +3201,6 @@ Console.WriteLine(" JointCreateFixed");
                     "[ODE PRIM]: Could not get mesh/sculpt asset {0} for {1} at {2} in {3}",
                     _pbs.SculptTexture, Name, _position, _parent_scene.PhysicsSceneName);
             }
-        }          
+        }
     }
 }
\ No newline at end of file
diff --git a/OpenSim/Region/PhysicsModules/Ode/ODERayCastRequestManager.cs b/OpenSim/Region/PhysicsModules/Ode/ODERayCastRequestManager.cs
index 8d610f7..78dd7de 100644
--- a/OpenSim/Region/PhysicsModules/Ode/ODERayCastRequestManager.cs
+++ b/OpenSim/Region/PhysicsModules/Ode/ODERayCastRequestManager.cs
@@ -32,7 +32,6 @@ using System.Runtime.InteropServices;
 using System.Text;
 using OpenMetaverse;
 using OpenSim.Region.PhysicsModules.SharedBase;
-using Ode.NET;
 using log4net;
 
 namespace OpenSim.Region.PhysicsModule.ODE
@@ -76,7 +75,7 @@ namespace OpenSim.Region.PhysicsModule.ODE
         {
             m_scene = pScene;
             nearCallback = near;
-            
+
         }
 
         /// <summary>
@@ -137,7 +136,7 @@ namespace OpenSim.Region.PhysicsModule.ODE
                     ODERayCastRequest[] reqs = m_PendingRequests.ToArray();
                     for (int i = 0; i < reqs.Length; i++)
                     {
-                        if (reqs[i].callbackMethod != null) // quick optimization here, don't raycast 
+                        if (reqs[i].callbackMethod != null) // quick optimization here, don't raycast
                             RayCast(reqs[i]);               // if there isn't anyone to send results
                     }
 
@@ -152,7 +151,7 @@ namespace OpenSim.Region.PhysicsModule.ODE
                     ODERayRequest[] reqs = m_PendingRayRequests.ToArray();
                     for (int i = 0; i < reqs.Length; i++)
                     {
-                        if (reqs[i].callbackMethod != null) // quick optimization here, don't raycast 
+                        if (reqs[i].callbackMethod != null) // quick optimization here, don't raycast
                             RayCast(reqs[i]);               // if there isn't anyone to send results
                     }
 
@@ -176,11 +175,11 @@ namespace OpenSim.Region.PhysicsModule.ODE
             // this value may still be too large, depending on machine configuration
             // of maximum stack
             float len = req.length;
-            if (len > 250f)
-                len = 250f;
+            if (len > 100f)
+                len = 100f;
 
             // Create the ray
-            IntPtr ray = d.CreateRay(m_scene.space, req.length);
+            IntPtr ray = d.CreateRay(m_scene.space, len);
             d.GeomRaySet(ray, req.Origin.X, req.Origin.Y, req.Origin.Z, req.Normal.X, req.Normal.Y, req.Normal.Z);
 
             // Collide test
@@ -225,8 +224,13 @@ namespace OpenSim.Region.PhysicsModule.ODE
         /// <param name="req"></param>
         private void RayCast(ODERayRequest req)
         {
+            // limit ray length or collisions will take all avaiable stack space
+            float len = req.length;
+            if (len > 100f)
+                len = 100f;
+
             // Create the ray
-            IntPtr ray = d.CreateRay(m_scene.space, req.length);
+            IntPtr ray = d.CreateRay(m_scene.space, len);
             d.GeomRaySet(ray, req.Origin.X, req.Origin.Y, req.Origin.Z, req.Normal.X, req.Normal.Y, req.Normal.Z);
 
             // Collide test
@@ -243,83 +247,22 @@ namespace OpenSim.Region.PhysicsModule.ODE
                     req.callbackMethod(m_contactResults);
             }
         }
-        
+
         // This is the standard Near.   Uses space AABBs to speed up detection.
         private void near(IntPtr space, IntPtr g1, IntPtr g2)
         {
 
-            //Don't test against heightfield Geom, or you'll be sorry!
-            
-            /*
-             terminate called after throwing an instance of 'std::bad_alloc'
-                  what():  std::bad_alloc
-                Stacktrace:
-                 
-                  at (wrapper managed-to-native) Ode.NET.d.Collide (intptr,intptr,int,Ode.NET.d/ContactGeom[],int) <0x00004>
-                  at (wrapper managed-to-native) Ode.NET.d.Collide (intptr,intptr,int,Ode.NET.d/ContactGeom[],int) <0xffffffff>
-                  at OpenSim.Region.Physics.OdePlugin.ODERayCastRequestManager.near (intptr,intptr,intptr) <0x00280>
-                  at (wrapper native-to-managed) OpenSim.Region.Physics.OdePlugin.ODERayCastRequestManager.near (intptr,intptr,intptr) <0xfff
-                fffff>
-                  at (wrapper managed-to-native) Ode.NET.d.SpaceCollide2 (intptr,intptr,intptr,Ode.NET.d/NearCallback) <0x00004>
-                  at (wrapper managed-to-native) Ode.NET.d.SpaceCollide2 (intptr,intptr,intptr,Ode.NET.d/NearCallback) <0xffffffff>
-                  at OpenSim.Region.Physics.OdePlugin.ODERayCastRequestManager.RayCast (OpenSim.Region.Physics.OdePlugin.ODERayCastRequest) <
-                0x00114>
-                  at OpenSim.Region.Physics.OdePlugin.ODERayCastRequestManager.ProcessQueuedRequests () <0x000eb>
-                  at OpenSim.Region.Physics.OdePlugin.OdeScene.Simulate (single) <0x017e6>
-                  at OpenSim.Region.Framework.Scenes.SceneGraph.UpdatePhysics (double) <0x00042>
-                  at OpenSim.Region.Framework.Scenes.Scene.Update () <0x0039e>
-                  at OpenSim.Region.Framework.Scenes.Scene.Heartbeat (object) <0x00019>
-                  at (wrapper runtime-invoke) object.runtime_invoke_void__this___object (object,intptr,intptr,intptr) <0xffffffff>
-                 
-                Native stacktrace:
-                 
-                        mono [0x80d2a42]
-                        [0xb7f5840c]
-                        /lib/i686/cmov/libc.so.6(abort+0x188) [0xb7d1a018]
-                        /usr/lib/libstdc++.so.6(_ZN9__gnu_cxx27__verbose_terminate_handlerEv+0x158) [0xb45fc988]
-                        /usr/lib/libstdc++.so.6 [0xb45fa865]
-                        /usr/lib/libstdc++.so.6 [0xb45fa8a2]
-                        /usr/lib/libstdc++.so.6 [0xb45fa9da]
-                        /usr/lib/libstdc++.so.6(_Znwj+0x83) [0xb45fb033]
-                        /usr/lib/libstdc++.so.6(_Znaj+0x1d) [0xb45fb11d]
-                        libode.so(_ZN13dxHeightfield23dCollideHeightfieldZoneEiiiiP6dxGeomiiP12dContactGeomi+0xd04) [0xb46678e4]
-                        libode.so(_Z19dCollideHeightfieldP6dxGeomS0_iP12dContactGeomi+0x54b) [0xb466832b]
-                        libode.so(dCollide+0x102) [0xb46571b2]
-                        [0x95cfdec9]
-                        [0x8ea07fe1]
-                        [0xab260146]
-                        libode.so [0xb465a5c4]
-                        libode.so(_ZN11dxHashSpace8collide2EPvP6dxGeomPFvS0_S2_S2_E+0x75) [0xb465bcf5]
-                        libode.so(dSpaceCollide2+0x177) [0xb465ac67]
-                        [0x95cf978e]
-                        [0x8ea07945]
-                        [0x95cf2bbc]
-                        [0xab2787e7]
-                        [0xab419fb3]
-                        [0xab416657]
-                        [0xab415bda]
-                        [0xb609b08e]
-                        mono(mono_runtime_delegate_invoke+0x34) [0x8192534]
-                        mono [0x81a2f0f]
-                        mono [0x81d28b6]
-                        mono [0x81ea2c6]
-                        /lib/i686/cmov/libpthread.so.0 [0xb7e744c0]
-                        /lib/i686/cmov/libc.so.6(clone+0x5e) [0xb7dcd6de]
-             */
-
-            // Exclude heightfield geom
-
             if (g1 == IntPtr.Zero || g2 == IntPtr.Zero)
                 return;
-            if (d.GeomGetClass(g1) == d.GeomClassID.HeightfieldClass || d.GeomGetClass(g2) == d.GeomClassID.HeightfieldClass)
-                return;
+//            if (d.GeomGetClass(g1) == d.GeomClassID.HeightfieldClass || d.GeomGetClass(g2) == d.GeomClassID.HeightfieldClass)
+//                return;
 
             // Raytest against AABBs of spaces first, then dig into the spaces it hits for actual geoms.
             if (d.GeomIsSpace(g1) || d.GeomIsSpace(g2))
             {
                 if (g1 == IntPtr.Zero || g2 == IntPtr.Zero)
                     return;
-                
+
                 // Separating static prim geometry spaces.
                 // We'll be calling near recursivly if one
                 // of them is a space to find all of the
@@ -347,13 +290,13 @@ namespace OpenSim.Region.PhysicsModule.ODE
             int count = 0;
             try
             {
-                
+
                 if (g1 == g2)
                     return; // Can't collide with yourself
 
                 lock (contacts)
                 {
-                    count = d.Collide(g1, g2, contacts.GetLength(0), contacts, d.ContactGeom.SizeOf);
+                    count = d.Collide(g1, g2, contacts.GetLength(0), contacts, d.ContactGeom.unmanagedSizeOf);
                 }
             }
             catch (SEHException)
@@ -383,7 +326,7 @@ namespace OpenSim.Region.PhysicsModule.ODE
                     if (p1 is OdePrim)
                     {
                         ContactResult collisionresult = new ContactResult();
-                    
+
                         collisionresult.ConsumerID = p1.LocalID;
                         collisionresult.Pos = new Vector3(contacts[i].pos.X, contacts[i].pos.Y, contacts[i].pos.Z);
                         collisionresult.Depth = contacts[i].depth;
@@ -438,4 +381,4 @@ namespace OpenSim.Region.PhysicsModule.ODE
         public float length;
         public RayCallback callbackMethod;
     }
-}
\ No newline at end of file
+}
diff --git a/OpenSim/Region/PhysicsModules/Ode/OdePhysicsJoint.cs b/OpenSim/Region/PhysicsModules/Ode/OdePhysicsJoint.cs
index 2eb7ba6..e2578be 100644
--- a/OpenSim/Region/PhysicsModules/Ode/OdePhysicsJoint.cs
+++ b/OpenSim/Region/PhysicsModules/Ode/OdePhysicsJoint.cs
@@ -27,10 +27,8 @@
 
 using System;
 using OpenMetaverse;
-using Ode.NET;
 using OpenSim.Framework;
 using OpenSim.Region.PhysicsModules.SharedBase;
-using OpenSim.Region.PhysicsModule.ODE;
 
 namespace OpenSim.Region.PhysicsModule.ODE
 {
diff --git a/OpenSim/Region/PhysicsModules/Ode/OdeScene.cs b/OpenSim/Region/PhysicsModules/Ode/OdeScene.cs
index 8cc7f28..d15568e 100644
--- a/OpenSim/Region/PhysicsModules/Ode/OdeScene.cs
+++ b/OpenSim/Region/PhysicsModules/Ode/OdeScene.cs
@@ -31,31 +31,24 @@
 // or application thread stack may just blowup
 // see RayCast(ODERayCastRequest req)
 
-//#define USE_DRAWSTUFF
-//#define SPAM
-
 using System;
 using System.Collections.Generic;
 using System.Diagnostics;
 using System.IO;
 using System.Linq;
 using System.Reflection;
+using System.Runtime.ExceptionServices;
 using System.Runtime.InteropServices;
 using System.Threading;
 using log4net;
 using Nini.Config;
 using Mono.Addins;
-using Ode.NET;
 using OpenMetaverse;
-#if USE_DRAWSTUFF
-using Drawstuff.NET;
-#endif 
 using OpenSim.Framework;
 using OpenSim.Region.PhysicsModules.SharedBase;
 using OpenSim.Region.Framework.Scenes;
 using OpenSim.Region.Framework.Interfaces;
 
-
 namespace OpenSim.Region.PhysicsModule.ODE
 {
     public enum StatusIndicators : int
@@ -65,16 +58,6 @@ namespace OpenSim.Region.PhysicsModule.ODE
         End = 2
     }
 
-//    public struct sCollisionData
-//    {
-//        public uint ColliderLocalId;
-//        public uint CollidedWithLocalId;
-//        public int NumberOfCollisions;
-//        public int CollisionType;
-//        public int StatusIndicator;
-//        public int lastframe;
-//    }
-
     [Flags]
     public enum CollisionCategories : int
     {
@@ -111,11 +94,9 @@ namespace OpenSim.Region.PhysicsModule.ODE
         Rubber = 6
     }
 
-    [Extension(Path = "/OpenSim/RegionModules", NodeName = "RegionModule", Id = "ODEPhysicsScene")]
-    public class OdeScene : PhysicsScene, INonSharedRegionModule
+    public class OdeScene : PhysicsScene
     {
         private readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType.ToString());
-        private bool m_Enabled = false;
 
         // private Dictionary<string, sCollisionData> m_storedCollisions = new Dictionary<string, sCollisionData>();
 
@@ -144,6 +125,7 @@ namespace OpenSim.Region.PhysicsModule.ODE
         /// .../opensim/bin/libode-x86_64.so(_Z12dCollideCCTLP6dxGeomS0_iP12dContactGeomi+0x92) [0x7f03b44bcf82]
         /// </remarks>
         internal static Object UniversalColliderSyncObject = new Object();
+        internal static Object SimulationLock = new Object();
 
         /// <summary>
         /// Is stats collecting enabled for this ODE scene?
@@ -276,8 +258,6 @@ namespace OpenSim.Region.PhysicsModule.ODE
 
         private Random fluidRandomizer = new Random(Environment.TickCount);
 
-        public bool m_suportCombine = true;
-
         private uint m_regionWidth = Constants.RegionSize;
         private uint m_regionHeight = Constants.RegionSize;
 
@@ -293,11 +273,8 @@ namespace OpenSim.Region.PhysicsModule.ODE
 
         private float contactsurfacelayer = 0.001f;
 
-        private int worldHashspaceLow = -4;
-        private int worldHashspaceHigh = 128;
-
-        private int smallHashspaceLow = -4;
-        private int smallHashspaceHigh = 66;
+        private int HashspaceLow = -5;
+        private int HashspaceHigh = 12;
 
         private float waterlevel = 0f;
         private int framecount = 0;
@@ -335,7 +312,6 @@ namespace OpenSim.Region.PhysicsModule.ODE
         public bool IsAvCapsuleTilted { get; private set; }
 
         private float avDensity = 80f;
-//        private float avHeightFudgeFactor = 0.52f;
         private float avMovementDivisorWalk = 1.3f;
         private float avMovementDivisorRun = 0.8f;
         private float minimumGroundFlightOffset = 3f;
@@ -356,13 +332,8 @@ namespace OpenSim.Region.PhysicsModule.ODE
         public float bodyPIDD = 35f;
         public float bodyPIDG = 25;
 
-        public int geomCrossingFailuresBeforeOutofbounds = 5;
-
-        public float bodyMotorJointMaxforceTensor = 2;
-
         public int bodyFramesAutoDisable = 20;
 
-        private float[] _watermap;
         private bool m_filterCollisions = true;
 
         private d.NearCallback nearCallback;
@@ -473,31 +444,21 @@ namespace OpenSim.Region.PhysicsModule.ODE
         private d.Contact WaterContact;
         private d.Contact[,] m_materialContacts;
 
-//Ckrinke: Comment out until used. We declare it, initialize it, but do not use it
-//Ckrinke        private int m_randomizeWater = 200;
         private int m_physicsiterations = 10;
         private const float m_SkipFramesAtms = 0.40f; // Drop frames gracefully at a 400 ms lag
         private readonly PhysicsActor PANull = new NullPhysicsActor();
-//        private float step_time = 0.0f;
-//Ckrinke: Comment out until used. We declare it, initialize it, but do not use it
-//Ckrinke        private int ms = 0;
+        private float step_time = 0.0f;
         public IntPtr world;
-        //private bool returncollisions = false;
-        // private uint obj1LocalID = 0;
         private uint obj2LocalID = 0;
-        //private int ctype = 0;
         private OdeCharacter cc1;
         private OdePrim cp1;
         private OdeCharacter cc2;
         private OdePrim cp2;
         private int p1ExpectedPoints = 0;
         private int p2ExpectedPoints = 0;
-        //private int cStartStop = 0;
-        //private string cDictKey = "";
 
         public IntPtr space;
 
-        //private IntPtr tmpSpace;
         // split static geometry collision handling into spaces of 30 meters
         public IntPtr[,] staticPrimspace;
 
@@ -535,88 +496,37 @@ namespace OpenSim.Region.PhysicsModule.ODE
 
         private ODERayCastRequestManager m_rayCastManager;
 
+        public Scene m_frameWorkScene = null;
 
-        #region INonSharedRegionModule
-        public string Name
-        {
-            get { return "OpenDynamicsEngine"; }
-        }
-
-        public Type ReplaceableInterface
-        {
-            get { return null; }
-        }
-
-        public void Initialise(IConfigSource source)
-        {
-            // TODO: Move this out of Startup
-            IConfig config = source.Configs["Startup"];
-            if (config != null)
-            {
-                string physics = config.GetString("physics", string.Empty);
-                if (physics == Name)
-                {
-                    m_Enabled = true;
-                    m_config = source;
-
-                    // We do this so that OpenSimulator on Windows loads the correct native ODE library depending on whether
-                    // it's running as a 32-bit process or a 64-bit one.  By invoking LoadLibary here, later DLLImports
-                    // will find it already loaded later on.
-                    //
-                    // This isn't necessary for other platforms (e.g. Mac OSX and Linux) since the DLL used can be
-                    // controlled in Ode.NET.dll.config
-                    if (Util.IsWindows())
-                        Util.LoadArchSpecificWindowsDll("ode.dll");
-
-                    // Initializing ODE only when a scene is created allows alternative ODE plugins to co-habit (according to
-                    // http://opensimulator.org/mantis/view.php?id=2750).
-                    d.InitODE();
-
-                }
-            }
-
-        }
-
-        public void Close()
-        {
-        }
-
-        public void AddRegion(Scene scene)
+        public OdeScene(Scene pscene, IConfigSource psourceconfig, string pname, string pversion)
         {
-            if (!m_Enabled)
-                return;
+            m_config = psourceconfig;
+            m_frameWorkScene = pscene;
 
-            EngineType = Name;
-            PhysicsSceneName = EngineType + "/" + scene.RegionInfo.RegionName;
+            EngineType = pname;
+            PhysicsSceneName = EngineType + "/" + pscene.RegionInfo.RegionName;
+            EngineName = pname + " " + pversion;
 
-            scene.RegisterModuleInterface<PhysicsScene>(this);
-            Vector3 extent = new Vector3(scene.RegionInfo.RegionSizeX, scene.RegionInfo.RegionSizeY, scene.RegionInfo.RegionSizeZ);
+            pscene.RegisterModuleInterface<PhysicsScene>(this);
+            Vector3 extent = new Vector3(pscene.RegionInfo.RegionSizeX, pscene.RegionInfo.RegionSizeY, pscene.RegionInfo.RegionSizeZ);
             Initialise(extent);
             InitialiseFromConfig(m_config);
 
             // This may not be that good since terrain may not be avaiable at this point
-            base.Initialise(scene.PhysicsRequestAsset, 
-                (scene.Heightmap != null ? scene.Heightmap.GetFloatsSerialised() : new float[(int)(extent.X * extent.Y)]), 
-                (float)scene.RegionInfo.RegionSettings.WaterHeight);
+            base.Initialise(pscene.PhysicsRequestAsset,
+                (pscene.Heightmap != null ? pscene.Heightmap.GetFloatsSerialised() : new float[(int)(extent.X * extent.Y)]),
+                (float)pscene.RegionInfo.RegionSettings.WaterHeight);
 
         }
 
-        public void RemoveRegion(Scene scene)
+         public void RegionLoaded()
         {
-            if (!m_Enabled)
-                return;
-        }
-
-        public void RegionLoaded(Scene scene)
-        {
-            if (!m_Enabled)
-                return;
-
-            mesher = scene.RequestModuleInterface<IMesher>();
+            mesher = m_frameWorkScene.RequestModuleInterface<IMesher>();
             if (mesher == null)
                 m_log.WarnFormat("[ODE SCENE]: No mesher in {0}. Things will not work well.", PhysicsSceneName);
+
+            m_frameWorkScene.PhysicsEnabled = true;
         }
-        #endregion
 
         /// <summary>
         /// Initiailizes the scene
@@ -625,16 +535,12 @@ namespace OpenSim.Region.PhysicsModule.ODE
         /// </summary>
         private void Initialise(Vector3 regionExtent)
         {
-                        WorldExtents.X = regionExtent.X;
+            WorldExtents.X = regionExtent.X;
             m_regionWidth = (uint)regionExtent.X;
             WorldExtents.Y = regionExtent.Y;
             m_regionHeight = (uint)regionExtent.Y;
-                        
-            m_suportCombine = false;
-                        
+
             nearCallback = near;
-            triCallback = TriCallback;
-            triArrayCallback = TriArrayCallback;
             m_rayCastManager = new ODERayCastRequestManager(this);
 
             // Create the world and the first space
@@ -644,33 +550,8 @@ namespace OpenSim.Region.PhysicsModule.ODE
             contactgroup = d.JointGroupCreate(0);
 
             d.WorldSetAutoDisableFlag(world, false);
-
-            #if USE_DRAWSTUFF
-            Thread viewthread = new Thread(new ParameterizedThreadStart(startvisualization));
-            viewthread.Start();
-            #endif
-
- //           _watermap = new float[258 * 258];
-
-            // Zero out the prim spaces array (we split our space into smaller spaces so
-            // we can hit test less.
         }
 
-#if USE_DRAWSTUFF
-        public void startvisualization(object o)
-        {
-            ds.Functions fn;
-            fn.version = ds.VERSION;
-            fn.start = new ds.CallbackFunction(start);
-            fn.step = new ds.CallbackFunction(step);
-            fn.command = new ds.CallbackFunction(command);
-            fn.stop = null;
-            fn.path_to_textures = "./textures";
-            string[] args = new string[0];
-            ds.SimulationLoop(args.Length, args, 352, 288, ref fn);
-        }
-#endif
-
         // Initialize from configs
         private void InitialiseFromConfig(IConfigSource config)
         {
@@ -679,18 +560,9 @@ namespace OpenSim.Region.PhysicsModule.ODE
             m_config = config;
             // Defaults
 
-            if (Environment.OSVersion.Platform == PlatformID.Unix)
-            {
-                avPIDD = 3200.0f;
-                avPIDP = 1400.0f;
-                avStandupTensor = 2000000f;
-            }
-            else
-            {
-                avPIDD = 2200.0f;
-                avPIDP = 900.0f;
-                avStandupTensor = 550000f;
-            }
+            avPIDD = 2200.0f;
+            avPIDP = 900.0f;
+            avStandupTensor = 550000f;
 
             int contactsPerCollision = 80;
 
@@ -714,12 +586,10 @@ namespace OpenSim.Region.PhysicsModule.ODE
                             avatarTerminalVelocity, AvatarTerminalVelocity);
                     }
 
-                    worldHashspaceLow = physicsconfig.GetInt("world_hashspace_size_low", -4);
-                    worldHashspaceHigh = physicsconfig.GetInt("world_hashspace_size_high", 128);
+                    HashspaceLow = physicsconfig.GetInt("world_hashspace_level_low", -5);
+                    HashspaceHigh = physicsconfig.GetInt("world_hashspace_level_high", 12);
 
                     metersInSpace = physicsconfig.GetFloat("meters_in_small_space", 29.9f);
-                    smallHashspaceLow = physicsconfig.GetInt("small_hashspace_size_low", -4);
-                    smallHashspaceHigh = physicsconfig.GetInt("small_hashspace_size_high", 66);
 
                     contactsurfacelayer = physicsconfig.GetFloat("world_contact_surface_layer", 0.001f);
 
@@ -738,7 +608,7 @@ namespace OpenSim.Region.PhysicsModule.ODE
                     mAvatarObjectContactBounce = physicsconfig.GetFloat("m_avatarobjectcontact_bounce", 0.1f);
 
                     ODE_STEPSIZE = physicsconfig.GetFloat("world_stepsize", ODE_STEPSIZE);
-                    m_physicsiterations = physicsconfig.GetInt("world_internal_steps_without_collisions", 10);
+                    m_physicsiterations = physicsconfig.GetInt("world_solver_iterations", 10);
 
                     avDensity = physicsconfig.GetFloat("av_density", 80f);
 //                    avHeightFudgeFactor = physicsconfig.GetFloat("av_height_fudge_factor", 0.52f);
@@ -754,7 +624,6 @@ namespace OpenSim.Region.PhysicsModule.ODE
 
                     geomContactPointsStartthrottle = physicsconfig.GetInt("geom_contactpoints_start_throttling", 5);
                     geomUpdatesPerThrottledUpdate = physicsconfig.GetInt("geom_updates_before_throttled_update", 15);
-                    geomCrossingFailuresBeforeOutofbounds = physicsconfig.GetInt("geom_crossing_failures_before_outofbounds", 5);
 
                     geomDefaultDensity = physicsconfig.GetFloat("geometry_default_density", 10.000006836f);
                     bodyFramesAutoDisable = physicsconfig.GetInt("body_frames_auto_disable", 20);
@@ -767,29 +636,16 @@ namespace OpenSim.Region.PhysicsModule.ODE
                     meshSculptLOD = physicsconfig.GetFloat("mesh_lod", 32f);
                     MeshSculptphysicalLOD = physicsconfig.GetFloat("mesh_physical_lod", 16f);
                     m_filterCollisions = physicsconfig.GetBoolean("filter_collisions", false);
-                    
-                    
 
-                    if (Environment.OSVersion.Platform == PlatformID.Unix)
-                    {
-                        avPIDD = physicsconfig.GetFloat("av_pid_derivative_linux", 2200.0f);
-                        avPIDP = physicsconfig.GetFloat("av_pid_proportional_linux", 900.0f);
-                        avStandupTensor = physicsconfig.GetFloat("av_capsule_standup_tensor_linux", 550000f);
-                        bodyMotorJointMaxforceTensor = physicsconfig.GetFloat("body_motor_joint_maxforce_tensor_linux", 5f);
-                    }
-                    else
-                    {
-                        avPIDD = physicsconfig.GetFloat("av_pid_derivative_win", 2200.0f);
-                        avPIDP = physicsconfig.GetFloat("av_pid_proportional_win", 900.0f);
-                        avStandupTensor = physicsconfig.GetFloat("av_capsule_standup_tensor_win", 550000f);
-                        bodyMotorJointMaxforceTensor = physicsconfig.GetFloat("body_motor_joint_maxforce_tensor_win", 5f);
-                    }
+                    avPIDD = physicsconfig.GetFloat("av_pid_derivative", 2200.0f);
+                    avPIDP = physicsconfig.GetFloat("av_pid_proportional", 900.0f);
+                    avStandupTensor = physicsconfig.GetFloat("av_capsule_standup_tensor", 550000f);
 
                     physics_logging = physicsconfig.GetBoolean("physics_logging", false);
                     physics_logging_interval = physicsconfig.GetInt("physics_logging_interval", 0);
                     physics_logging_append_existing_logfile = physicsconfig.GetBoolean("physics_logging_append_existing_logfile", false);
 
-                    m_NINJA_physics_joints_enabled = physicsconfig.GetBoolean("use_NINJA_physics_joints", false);
+//                    m_NINJA_physics_joints_enabled = physicsconfig.GetBoolean("use_NINJA_physics_joints", false);
                     minimumGroundFlightOffset = physicsconfig.GetFloat("minimum_ground_flight_offset", 3f);
                     maximumMassObject = physicsconfig.GetFloat("maximum_mass_object", 10000.01f);
                 }
@@ -812,17 +668,15 @@ namespace OpenSim.Region.PhysicsModule.ODE
             if (spaceGridMaxY > 24)
             {
                 spaceGridMaxY = 24;
-                spacesPerMeterY = spaceGridMaxY / WorldExtents.Y ;
+                spacesPerMeterY = spaceGridMaxY / WorldExtents.Y;
             }
 
-            staticPrimspace = new IntPtr[spaceGridMaxX, spaceGridMaxY]; 
+            staticPrimspace = new IntPtr[spaceGridMaxX, spaceGridMaxY];
 
             // make this index limits
             spaceGridMaxX--;
             spaceGridMaxY--;
 
-
-
             // Centeral contact friction and bounce
             // ckrinke 11/10/08 Enabling soft_erp but not soft_cfm until I figure out why
             // an avatar falls through in Z but not in X or Y when walking on a prim.
@@ -984,7 +838,7 @@ namespace OpenSim.Region.PhysicsModule.ODE
             m_materialContacts[(int)Material.Rubber, 1].surface.soft_cfm = 0.010f;
             m_materialContacts[(int)Material.Rubber, 1].surface.soft_erp = 0.010f;
 
-            d.HashSpaceSetLevels(space, worldHashspaceLow, worldHashspaceHigh);
+            d.HashSpaceSetLevels(space, HashspaceLow, HashspaceHigh);
 
             // Set the gravity,, don't disable things automatically (we set it explicitly on some things)
 
@@ -997,9 +851,6 @@ namespace OpenSim.Region.PhysicsModule.ODE
             d.WorldSetLinearDampingThreshold(world, 256f);
             d.WorldSetMaxAngularSpeed(world, 256f);
 
-            // Set how many steps we go without running collision testing
-            // This is in addition to the step size.
-            // Essentially Steps * m_physicsiterations
             d.WorldSetQuickStepNumIterations(world, m_physicsiterations);
             //d.WorldSetContactMaxCorrectingVel(world, 1000.0f);
 
@@ -1014,22 +865,6 @@ namespace OpenSim.Region.PhysicsModule.ODE
             _worldInitialized = true;
         }
 
-//        internal void waitForSpaceUnlock(IntPtr space)
-//        {
-//            //if (space != IntPtr.Zero)
-//                //while (d.SpaceLockQuery(space)) { } // Wait and do nothing
-//        }
-
-//        /// <summary>
-//        /// Debug space message for printing the space that a prim/avatar is in.
-//        /// </summary>
-//        /// <param name="pos"></param>
-//        /// <returns>Returns which split up space the given position is in.</returns>
-//        public string whichspaceamIin(Vector3 pos)
-//        {
-//            return calculateSpaceForGeom(pos).ToString();
-//        }
-
         #region Collision Detection
 
         /// <summary>
@@ -1042,7 +877,7 @@ namespace OpenSim.Region.PhysicsModule.ODE
         /// <param name='contactsArray'></param>
         /// <param name='contactGeomSize'></param>
         private int CollideGeoms(
-            IntPtr geom1, IntPtr geom2, int maxContacts, Ode.NET.d.ContactGeom[] contactsArray, int contactGeomSize)
+            IntPtr geom1, IntPtr geom2, int maxContacts, d.ContactGeom[] contactsArray, int contactGeomSize)
         {
             int count;
 
@@ -1113,7 +948,7 @@ namespace OpenSim.Region.PhysicsModule.ODE
             {
                 if (g1 == IntPtr.Zero || g2 == IntPtr.Zero)
                     return;
-                
+
                 // Separating static prim geometry spaces.
                 // We'll be calling near recursivly if one
                 // of them is a space to find all of the
@@ -1155,12 +990,6 @@ namespace OpenSim.Region.PhysicsModule.ODE
                 name2 = "null";
             }
 
-            //if (id == d.GeomClassId.TriMeshClass)
-            //{
-                //               m_log.InfoFormat("near: A collision was detected between {1} and {2}", 0, name1, name2);
-                //m_log.Debug("near: A collision was detected between {1} and {2}", 0, name1, name2);
-            //}
-
             // Figure out how many contact points we have
             int count = 0;
 
@@ -1175,7 +1004,7 @@ namespace OpenSim.Region.PhysicsModule.ODE
                 if (b1 != IntPtr.Zero && b2 != IntPtr.Zero && d.AreConnectedExcluding(b1, b2, d.JointType.Contact))
                     return;
 
-                count = CollideGeoms(g1, g2, contacts.Length, contacts, d.ContactGeom.SizeOf);
+                count = CollideGeoms(g1, g2, contacts.Length, contacts, d.ContactGeom.unmanagedSizeOf);
 
                 // All code after this is only relevant if we have any collisions
                 if (count <= 0)
@@ -1198,10 +1027,10 @@ namespace OpenSim.Region.PhysicsModule.ODE
 
             PhysicsActor p1;
             PhysicsActor p2;
-            
+
             p1ExpectedPoints = 0;
             p2ExpectedPoints = 0;
-            
+
             if (!actor_name_map.TryGetValue(g1, out p1))
             {
                 p1 = PANull;
@@ -1238,7 +1067,7 @@ namespace OpenSim.Region.PhysicsModule.ODE
                 IntPtr joint;
                 // If we're colliding with terrain, use 'TerrainContact' instead of contact.
                 // allows us to have different settings
-                
+
                 // We only need to test p2 for 'jump crouch purposes'
                 if (p2 is OdeCharacter && p1.PhysicsActorType == (int)ActorTypes.Prim)
                 {
@@ -1252,7 +1081,7 @@ namespace OpenSim.Region.PhysicsModule.ODE
                 {
                     p2.IsColliding = true;
                 }
-                
+
                 //if ((framecount % m_returncollisions) == 0)
 
                 switch (p1.PhysicsActorType)
@@ -1279,110 +1108,9 @@ namespace OpenSim.Region.PhysicsModule.ODE
                 // we don't want prim or avatar to explode
 
                 #region InterPenetration Handling - Unintended physics explosions
-# region disabled code1
 
                 if (curContact.depth >= 0.08f)
                 {
-                    //This is disabled at the moment only because it needs more tweaking
-                    //It will eventually be uncommented
-                    /*
-                    if (contact.depth >= 1.00f)
-                    {
-                        //m_log.Debug("[PHYSICS]: " + contact.depth.ToString());
-                    }
-
-                    //If you interpenetrate a prim with an agent
-                    if ((p2.PhysicsActorType == (int) ActorTypes.Agent &&
-                         p1.PhysicsActorType == (int) ActorTypes.Prim) ||
-                        (p1.PhysicsActorType == (int) ActorTypes.Agent &&
-                         p2.PhysicsActorType == (int) ActorTypes.Prim))
-                    {
-                        
-                        //contact.depth = contact.depth * 4.15f;
-                        /*
-                        if (p2.PhysicsActorType == (int) ActorTypes.Agent)
-                        {
-                            p2.CollidingObj = true;
-                            contact.depth = 0.003f;
-                            p2.Velocity = p2.Velocity + new PhysicsVector(0, 0, 2.5f);
-                            OdeCharacter character = (OdeCharacter) p2;
-                            character.SetPidStatus(true);
-                            contact.pos = new d.Vector3(contact.pos.X + (p1.Size.X / 2), contact.pos.Y + (p1.Size.Y / 2), contact.pos.Z + (p1.Size.Z / 2));
-
-                        }
-                        else
-                        {
-
-                            //contact.depth = 0.0000000f;
-                        }
-                        if (p1.PhysicsActorType == (int) ActorTypes.Agent)
-                        {
-
-                            p1.CollidingObj = true;
-                            contact.depth = 0.003f;
-                            p1.Velocity = p1.Velocity + new PhysicsVector(0, 0, 2.5f);
-                            contact.pos = new d.Vector3(contact.pos.X + (p2.Size.X / 2), contact.pos.Y + (p2.Size.Y / 2), contact.pos.Z + (p2.Size.Z / 2));
-                            OdeCharacter character = (OdeCharacter)p1;
-                            character.SetPidStatus(true);
-                        }
-                        else
-                        {
-
-                            //contact.depth = 0.0000000f;
-                        }
-                          
-                        
-                     
-                    }
-*/
-                    // If you interpenetrate a prim with another prim
-                /*
-                    if (p1.PhysicsActorType == (int) ActorTypes.Prim && p2.PhysicsActorType == (int) ActorTypes.Prim)
-                    {
-                        #region disabledcode2
-                        //OdePrim op1 = (OdePrim)p1;
-                        //OdePrim op2 = (OdePrim)p2;
-                        //op1.m_collisionscore++;
-                        //op2.m_collisionscore++;
-
-                        //if (op1.m_collisionscore > 8000 || op2.m_collisionscore > 8000)
-                        //{
-                            //op1.m_taintdisable = true;
-                            //AddPhysicsActorTaint(p1);
-                            //op2.m_taintdisable = true;
-                            //AddPhysicsActorTaint(p2);
-                        //}
-
-                        //if (contact.depth >= 0.25f)
-                        //{
-                            // Don't collide, one or both prim will expld.
-
-                            //op1.m_interpenetrationcount++;
-                            //op2.m_interpenetrationcount++;
-                            //interpenetrations_before_disable = 200;
-                            //if (op1.m_interpenetrationcount >= interpenetrations_before_disable)
-                            //{
-                                //op1.m_taintdisable = true;
-                                //AddPhysicsActorTaint(p1);
-                            //}
-                            //if (op2.m_interpenetrationcount >= interpenetrations_before_disable)
-                            //{
-                               // op2.m_taintdisable = true;
-                                //AddPhysicsActorTaint(p2);
-                            //}
-
-                            //contact.depth = contact.depth / 8f;
-                            //contact.normal = new d.Vector3(0, 0, 1);
-                        //}
-                        //if (op1.m_disabled || op2.m_disabled)
-                        //{
-                            //Manually disabled objects stay disabled
-                            //contact.depth = 0f;
-                        //}
-                        #endregion
-                    }
-                    */
-#endregion
                     if (curContact.depth >= 1.00f)
                     {
                         //m_log.Info("[P]: " + contact.depth.ToString());
@@ -1432,7 +1160,7 @@ namespace OpenSim.Region.PhysicsModule.ODE
 
                 // Logic for collision handling
                 // Note, that if *all* contacts are skipped (VolumeDetect)
-                // The prim still detects (and forwards) collision events but 
+                // The prim still detects (and forwards) collision events but
                 // appears to be phantom for the world
                 Boolean skipThisContact = false;
 
@@ -1507,10 +1235,10 @@ namespace OpenSim.Region.PhysicsModule.ODE
                                         material = ((OdePrim) p2).m_material;
                                         p2ExpectedPoints = ((OdePrim)p2).ExpectedCollisionContacts;
                                     }
-                                   
+
                                     // Unnessesary because p1 is defined above
                                     //if (p1 is OdePrim)
-                                    // { 
+                                    // {
                                     //     p1ExpectedPoints = ((OdePrim)p1).ExpectedCollisionContacts;
                                     // }
                                     //m_log.DebugFormat("Material: {0}", material);
@@ -1622,7 +1350,7 @@ namespace OpenSim.Region.PhysicsModule.ODE
                                 material = ((OdePrim)p2).m_material;
                                 p2ExpectedPoints = ((OdePrim)p2).ExpectedCollisionContacts;
                             }
-                            
+
                             //m_log.DebugFormat("Material: {0}", material);
                             m_materialContacts[material, 0].geom = curContact;
 
@@ -1642,7 +1370,7 @@ namespace OpenSim.Region.PhysicsModule.ODE
                 }
 
                 collision_accounting_events(p1, p2, maxDepthContact);
-                
+
                 if (count > ((p1ExpectedPoints + p2ExpectedPoints) * 0.25) + (geomContactPointsStartthrottle))
                 {
                     // If there are more then 3 contact points, it's likely
@@ -1678,31 +1406,13 @@ namespace OpenSim.Region.PhysicsModule.ODE
                     {
                         if (Math.Abs(contact.depth - contactGeom.depth) < 0.052f)
                         {
-                            //contactGeom.depth *= .00005f;
-                           //m_log.DebugFormat("[Collsion]: Depth {0}", Math.Abs(contact.depth - contactGeom.depth));
-                            // m_log.DebugFormat("[Collision]: <{0},{1},{2}>", Math.Abs(contactGeom.normal.X - contact.normal.X), Math.Abs(contactGeom.normal.Y - contact.normal.Y), Math.Abs(contactGeom.normal.Z - contact.normal.Z));
-                            result = true;
-                            break;
+                             result = true;
+                             break;
                         }
-//                        else
-//                        {
-//                            //m_log.DebugFormat("[Collsion]: Depth {0}", Math.Abs(contact.depth - contactGeom.depth));
-//                        }
                     }
-//                    else
-//                    {
-//                        //m_log.DebugFormat("[Collision]: <{0},{1},{2}>", Math.Abs(contactGeom.normal.X - contact.normal.X), Math.Abs(contactGeom.normal.Y - contact.normal.Y), Math.Abs(contactGeom.normal.Z - contact.normal.Z));
-//                        //int i = 0;
-//                    }
-                } 
+                }
                 else if (at == ActorTypes.Prim)
                 {
-                    //d.AABB aabb1 = new d.AABB();
-                    //d.AABB aabb2 = new d.AABB();
-
-                    //d.GeomGetAABB(contactGeom.g2, out aabb2);
-                    //d.GeomGetAABB(contactGeom.g1, out aabb1);
-                    //aabb1.
                     if (((Math.Abs(contactGeom.normal.X - contact.normal.X) < 1.026f) && (Math.Abs(contactGeom.normal.Y - contact.normal.Y) < 0.303f) && (Math.Abs(contactGeom.normal.Z - contact.normal.Z) < 0.065f)))
                     {
                         if (contactGeom.normal.X == contact.normal.X && contactGeom.normal.Y == contact.normal.Y && contactGeom.normal.Z == contact.normal.Z)
@@ -1729,9 +1439,28 @@ namespace OpenSim.Region.PhysicsModule.ODE
             obj2LocalID = 0;
             //ctype = 0;
             //cStartStop = 0;
-            if (!p2.SubscribedEvents() && !p1.SubscribedEvents())
+//            if (!p2.SubscribedEvents() && !p1.SubscribedEvents())
+//                return;
+            bool p1events = p1.SubscribedEvents();
+            bool p2events = p2.SubscribedEvents();
+
+            if (p1.IsVolumeDtc)
+                p2events = false;
+            if (p2.IsVolumeDtc)
+                p1events = false;
+
+            if (!p2events && !p1events)
                 return;
 
+            Vector3 vel = Vector3.Zero;
+            if (p2 != null && p2.IsPhysical)
+                vel = p2.Velocity;
+
+            if (p1 != null && p1.IsPhysical)
+                vel -= p1.Velocity;
+
+            contact.RelativeSpeed = Vector3.Dot(vel, contact.SurfaceNormal);
+
             switch ((ActorTypes)p2.PhysicsActorType)
             {
                 case ActorTypes.Agent:
@@ -1744,14 +1473,6 @@ namespace OpenSim.Region.PhysicsModule.ODE
                             cc1 = (OdeCharacter)p1;
                             obj2LocalID = cc1.LocalID;
                             cc1.AddCollisionEvent(cc2.LocalID, contact);
-                            //ctype = (int)CollisionCategories.Character;
-
-                            //if (cc1.CollidingObj)
-                            //cStartStop = (int)StatusIndicators.Generic;
-                            //else
-                            //cStartStop = (int)StatusIndicators.Start;
-
-                            //returncollisions = true;
                             break;
 
                         case ActorTypes.Prim:
@@ -1761,21 +1482,11 @@ namespace OpenSim.Region.PhysicsModule.ODE
                                 obj2LocalID = cp1.LocalID;
                                 cp1.AddCollisionEvent(cc2.LocalID, contact);
                             }
-                            //ctype = (int)CollisionCategories.Geom;
-
-                            //if (cp1.CollidingObj)
-                            //cStartStop = (int)StatusIndicators.Generic;
-                            //else
-                            //cStartStop = (int)StatusIndicators.Start;
-
-                            //returncollisions = true;
                             break;
 
                         case ActorTypes.Ground:
                         case ActorTypes.Unknown:
                             obj2LocalID = 0;
-                            //ctype = (int)CollisionCategories.Land;
-                            //returncollisions = true;
                             break;
                     }
 
@@ -1797,13 +1508,6 @@ namespace OpenSim.Region.PhysicsModule.ODE
                                     cc1 = (OdeCharacter) p1;
                                     obj2LocalID = cc1.LocalID;
                                     cc1.AddCollisionEvent(cp2.LocalID, contact);
-                                    //ctype = (int)CollisionCategories.Character;
-
-                                    //if (cc1.CollidingObj)
-                                    //cStartStop = (int)StatusIndicators.Generic;
-                                    //else
-                                    //cStartStop = (int)StatusIndicators.Start;
-                                    //returncollisions = true;
                                 }
                                 break;
                             case ActorTypes.Prim:
@@ -1813,23 +1517,12 @@ namespace OpenSim.Region.PhysicsModule.ODE
                                     cp1 = (OdePrim) p1;
                                     obj2LocalID = cp1.LocalID;
                                     cp1.AddCollisionEvent(cp2.LocalID, contact);
-                                    //ctype = (int)CollisionCategories.Geom;
-
-                                    //if (cp1.CollidingObj)
-                                    //cStartStop = (int)StatusIndicators.Generic;
-                                    //else
-                                    //cStartStop = (int)StatusIndicators.Start;
-
-                                    //returncollisions = true;
                                 }
                                 break;
 
                             case ActorTypes.Ground:
                             case ActorTypes.Unknown:
                                 obj2LocalID = 0;
-                                //ctype = (int)CollisionCategories.Land;
-
-                                //returncollisions = true;
                                 break;
                         }
 
@@ -1837,80 +1530,7 @@ namespace OpenSim.Region.PhysicsModule.ODE
                     }
                     break;
             }
-            //if (returncollisions)
-            //{
-
-                //lock (m_storedCollisions)
-                //{
-                    //cDictKey = obj1LocalID.ToString() + obj2LocalID.ToString() + cStartStop.ToString() + ctype.ToString();
-                    //if (m_storedCollisions.ContainsKey(cDictKey))
-                    //{
-                        //sCollisionData objd = m_storedCollisions[cDictKey];
-                        //objd.NumberOfCollisions += 1;
-                        //objd.lastframe = framecount;
-                        //m_storedCollisions[cDictKey] = objd;
-                    //}
-                    //else
-                    //{
-                        //sCollisionData objd = new sCollisionData();
-                        //objd.ColliderLocalId = obj1LocalID;
-                        //objd.CollidedWithLocalId = obj2LocalID;
-                        //objd.CollisionType = ctype;
-                        //objd.NumberOfCollisions = 1;
-                        //objd.lastframe = framecount;
-                        //objd.StatusIndicator = cStartStop;
-                        //m_storedCollisions.Add(cDictKey, objd);
-                    //}
-                //}
-           // }
-        }
-
-        private int TriArrayCallback(IntPtr trimesh, IntPtr refObject, int[] triangleIndex, int triCount)
-        {
-            /*            String name1 = null;
-                        String name2 = null;
-
-                        if (!geom_name_map.TryGetValue(trimesh, out name1))
-                        {
-                            name1 = "null";
-                        }
-                        if (!geom_name_map.TryGetValue(refObject, out name2))
-                        {
-                            name2 = "null";
-                        }
-
-                        m_log.InfoFormat("TriArrayCallback: A collision was detected between {1} and {2}", 0, name1, name2);
-            */
-            return 1;
-        }
-
-        private int TriCallback(IntPtr trimesh, IntPtr refObject, int triangleIndex)
-        {
-//            String name1 = null;
-//            String name2 = null;
-//
-//            if (!geom_name_map.TryGetValue(trimesh, out name1))
-//            {
-//                name1 = "null";
-//            }
-//
-//            if (!geom_name_map.TryGetValue(refObject, out name2))
-//            {
-//                name2 = "null";
-//            }
-
-            //            m_log.InfoFormat("TriCallback: A collision was detected between {1} and {2}. Index was {3}", 0, name1, name2, triangleIndex);
-
-            d.Vector3 v0 = new d.Vector3();
-            d.Vector3 v1 = new d.Vector3();
-            d.Vector3 v2 = new d.Vector3();
-
-            d.GeomTriMeshGetTriangle(trimesh, 0, ref v0, ref v1, ref v2);
-            //            m_log.DebugFormat("Triangle {0} is <{1},{2},{3}>, <{4},{5},{6}>, <{7},{8},{9}>", triangleIndex, v0.X, v0.Y, v0.Z, v1.X, v1.Y, v1.Z, v2.X, v2.Y, v2.Z);
-
-            return 1;
         }
-
         /// <summary>
         /// This is our collision testing routine in ODE
         /// </summary>
@@ -1924,11 +1544,11 @@ namespace OpenSim.Region.PhysicsModule.ODE
                 // since we don't know if we're colliding yet
                 if (chr.Shell == IntPtr.Zero || chr.Body == IntPtr.Zero)
                     continue;
-                
+
                 chr.IsColliding = false;
                 chr.CollidingGround = false;
                 chr.CollidingObj = false;
-                
+
                 // Test the avatar's geometry for collision with the space
                 // This will return near and the space that they are the closest to
                 // And we'll run this again against the avatar and the space segment
@@ -1942,7 +1562,7 @@ namespace OpenSim.Region.PhysicsModule.ODE
                 {
                     m_log.ErrorFormat("[ODE SCENE]: Unable to space collide {0}", PhysicsSceneName);
                 }
-                
+
                 //float terrainheight = GetTerrainHeightAtXY(chr.Position.X, chr.Position.Y);
                 //if (chr.Position.Z + (chr.Velocity.Z * timeStep) < terrainheight + 10)
                 //{
@@ -2003,15 +1623,6 @@ namespace OpenSim.Region.PhysicsModule.ODE
 
         #endregion
 
-        public override void Combine(PhysicsScene pScene, Vector3 offset, Vector3 extents)
-        {
-            if (!m_suportCombine)
-                return;
-            m_worldOffset = offset;
-            WorldExtents = new Vector2(extents.X, extents.Y);
-            m_parentScene = pScene;
-        }
-
         // Recovered for use by fly height. Kitto Flora
         internal float GetTerrainHeightAtXY(float x, float y)
         {
@@ -2019,12 +1630,6 @@ namespace OpenSim.Region.PhysicsModule.ODE
             int offsetX = 0;
             int offsetY = 0;
 
-            if (m_suportCombine)
-            {
-                offsetX = ((int)(x / (int)Constants.RegionSize)) * (int)Constants.RegionSize;
-                offsetY = ((int)(y / (int)Constants.RegionSize)) * (int)Constants.RegionSize;
-            }
-
             if(RegionTerrain.TryGetValue(new Vector3(offsetX,offsetY,0), out heightFieldGeom))
             {
                 if (heightFieldGeom != IntPtr.Zero)
@@ -2042,14 +1647,15 @@ namespace OpenSim.Region.PhysicsModule.ODE
                         x = x - offsetX + 1f;
                         y = y - offsetY + 1f;
 
-                        index = (int)((int)x * ((int)m_regionHeight +3) + (int)y);
+                        // map is rotated
+                        index = (int)x * ((int)m_regionHeight + 3) + (int)y;
 
                         if (index < TerrainHeightFieldHeights[heightFieldGeom].Length)
                         {
                             //m_log.DebugFormat("x{0} y{1} = {2}", x, y, (float)TerrainHeightFieldHeights[heightFieldGeom][index]);
                             return (float)TerrainHeightFieldHeights[heightFieldGeom][index];
                         }
-                            
+
                         else
                             return 0f;
                     }
@@ -2069,7 +1675,7 @@ namespace OpenSim.Region.PhysicsModule.ODE
             {
                 return 0f;
             }
-        } 
+        }
 // End recovered. Kitto Flora
 
         /// <summary>
@@ -2079,7 +1685,7 @@ namespace OpenSim.Region.PhysicsModule.ODE
         internal void AddCollisionEventReporting(PhysicsActor obj)
         {
 //            m_log.DebugFormat("[PHYSICS]: Adding {0} {1} to collision event reporting", obj.SOPName, obj.LocalID);
-            
+
             lock (m_collisionEventActorsChanges)
                 m_collisionEventActorsChanges[obj.LocalID] = obj;
         }
@@ -2099,7 +1705,9 @@ namespace OpenSim.Region.PhysicsModule.ODE
         #region Add/Remove Entities
 
         public override PhysicsActor AddAvatar(string avName, Vector3 position, Vector3 velocity, Vector3 size, bool isFlying)
-        {                       
+        {
+            d.AllocateODEDataForThread(0);
+
             OdeCharacter newAv
                 = new OdeCharacter(
                     avName, this, position, velocity, size, avPIDD, avPIDP,
@@ -2119,7 +1727,12 @@ namespace OpenSim.Region.PhysicsModule.ODE
 //                "[ODE SCENE]: Removing physics character {0} {1} from physics scene {2}",
 //                actor.Name, actor.LocalID, Name);
 
-            ((OdeCharacter) actor).Destroy();
+            lock (OdeLock)
+            {
+                d.AllocateODEDataForThread(0);
+
+                ((OdeCharacter) actor).Destroy();
+            }
         }
 
         internal void AddCharacter(OdeCharacter chr)
@@ -2169,9 +1782,11 @@ namespace OpenSim.Region.PhysicsModule.ODE
             Vector3 siz = size;
             Quaternion rot = rotation;
 
+
             OdePrim newPrim;
             lock (OdeLock)
             {
+                d.AllocateODEDataForThread(0);
                 newPrim = new OdePrim(name, this, pos, siz, rot, pbs, isphysical);
 
                 lock (_prims)
@@ -2456,7 +2071,7 @@ namespace OpenSim.Region.PhysicsModule.ODE
 
             lock (externalJointRequestsLock)
             {
-                if (!requestedJointsToBeCreated.Contains(joint)) // forbid same creation request from entering twice 
+                if (!requestedJointsToBeCreated.Contains(joint)) // forbid same creation request from entering twice
                 {
                     requestedJointsToBeCreated.Add(joint);
                 }
@@ -2620,59 +2235,13 @@ namespace OpenSim.Region.PhysicsModule.ODE
 
 
                     }
-                    // we don't want to remove the main space
-
-                    // If the geometry is in the targetspace, remove it from the target space
-                    //m_log.Warn(prim.m_targetSpace);
-
-                    //if (prim.m_targetSpace != IntPtr.Zero)
-                    //{
-                    //if (d.SpaceQuery(prim.m_targetSpace, prim.prim_geom))
-                    //{
-
-                    //if (d.GeomIsSpace(prim.m_targetSpace))
-                    //{
-                    //waitForSpaceUnlock(prim.m_targetSpace);
-                    //d.SpaceRemove(prim.m_targetSpace, prim.prim_geom);
                     prim.m_targetSpace = IntPtr.Zero;
-                    //}
-                    //else
-                    //{
-                    // m_log.Info("[Physics]: Invalid Scene passed to 'removeprim from scene':" +
-                    //((OdePrim)prim).m_targetSpace.ToString());
-                    //}
-
-                    //}
-                    //}
-                    //m_log.Warn(prim.prim_geom);
-
                     if (!prim.RemoveGeom())
                         m_log.Warn("[ODE SCENE]: Unable to remove prim from physics scene");
 
                     lock (_prims)
                         _prims.Remove(prim);
 
-                    //If there are no more geometries in the sub-space, we don't need it in the main space anymore
-                    //if (d.SpaceGetNumGeoms(prim.m_targetSpace) == 0)
-                    //{
-                    //if (prim.m_targetSpace != null)
-                    //{
-                    //if (d.GeomIsSpace(prim.m_targetSpace))
-                    //{
-                    //waitForSpaceUnlock(prim.m_targetSpace);
-                    //d.SpaceRemove(space, prim.m_targetSpace);
-                    // free up memory used by the space.
-                    //d.SpaceDestroy(prim.m_targetSpace);
-                    //int[] xyspace = calculateSpaceArrayItemFromPos(prim.Position);
-                    //resetSpaceArrayItemToZero(xyspace[0], xyspace[1]);
-                    //}
-                    //else
-                    //{
-                    //m_log.Info("[Physics]: Invalid Scene passed to 'removeprim from scene':" +
-                    //((OdePrim) prim).m_targetSpace.ToString());
-                    //}
-                    //}
-                    //}
 
                     if (SupportsNINJAJoints)
                         RemoveAllJointsConnectedToActorThreadLocked(prim);
@@ -2768,12 +2337,9 @@ namespace OpenSim.Region.PhysicsModule.ODE
                     {
                         if (d.GeomIsSpace(currentspace))
                         {
-//                            waitForSpaceUnlock(currentspace);
-//                            waitForSpaceUnlock(space);
                             d.SpaceRemove(space, currentspace);
                             // free up memory used by the space.
 
-                            //d.SpaceDestroy(currentspace);
                             resetSpaceArrayItemToZero(currentspace);
                         }
                         else
@@ -2831,7 +2397,7 @@ namespace OpenSim.Region.PhysicsModule.ODE
             if (newspace == IntPtr.Zero)
             {
                 newspace = createprimspace(iprimspaceArrItem[0], iprimspaceArrItem[1]);
-                d.HashSpaceSetLevels(newspace, smallHashspaceLow, smallHashspaceHigh);
+                d.HashSpaceSetLevels(newspace, HashspaceLow, HashspaceHigh);
             }
 
             return newspace;
@@ -2851,7 +2417,7 @@ namespace OpenSim.Region.PhysicsModule.ODE
 //            waitForSpaceUnlock(space);
             d.SpaceSetSublevel(space, 1);
             d.SpaceAdd(space, staticPrimspace[iprimspaceArrItemX, iprimspaceArrItemY]);
-            
+
             return staticPrimspace[iprimspaceArrItemX, iprimspaceArrItemY];
         }
 
@@ -2948,7 +2514,7 @@ namespace OpenSim.Region.PhysicsModule.ODE
                 iPropertiesNotSupportedDefault++;
 
             if ((pbs.PathTwistBegin != 0) || (pbs.PathTwist != 0))
-                iPropertiesNotSupportedDefault++; 
+                iPropertiesNotSupportedDefault++;
 
             if ((pbs.ProfileBegin != 0) || pbs.ProfileEnd != 0)
                 iPropertiesNotSupportedDefault++;
@@ -3021,7 +2587,7 @@ namespace OpenSim.Region.PhysicsModule.ODE
 #if SPAM
             m_log.Debug("Mesh");
 #endif
-            return true; 
+            return true;
         }
 
         /// <summary>
@@ -3052,6 +2618,49 @@ namespace OpenSim.Region.PhysicsModule.ODE
             }
         }
 
+        // does all pending changes generated during region load process
+        public override void ProcessPreSimulation()
+        {
+            lock (OdeLock)
+            {
+                if (world == IntPtr.Zero)
+                {
+                    _taintedPrims.Clear();;
+                    return;
+                }
+
+                int donechanges = 0;
+                if (_taintedPrims.Count > 0)
+                {
+
+                    m_log.InfoFormat("[Ode] start processing pending actor operations");
+                    int tstart = Util.EnvironmentTickCount();
+
+                    d.AllocateODEDataForThread(0);
+
+                    lock (_taintedPrims)
+                    {
+                        foreach (OdePrim prim in _taintedPrims)
+                        {
+                            if (prim.m_taintremove)
+                                RemovePrimThreadLocked(prim);
+                            else
+                                prim.ProcessTaints();
+
+                            prim.m_collisionscore = 0;
+                            donechanges++;
+                        }
+                        _taintedPrims.Clear();
+                    }
+
+                    int time = Util.EnvironmentTickCountSubtract(tstart);
+                    m_log.InfoFormat("[Ode] finished {0} operations in {1}ms", donechanges, time);
+                }
+                m_log.InfoFormat("[Ode] {0} prim actors loaded",_prims.Count);
+            }
+        }
+
+
         /// <summary>
         /// This is our main simulate loop
         /// </summary>
@@ -3065,7 +2674,8 @@ namespace OpenSim.Region.PhysicsModule.ODE
         /// <returns>The number of frames simulated over that period.</returns>
         public override float Simulate(float timeStep)
         {
-            if (!_worldInitialized) return 11f;
+            if (!_worldInitialized)
+                return 1.0f;
 
             int startFrameTick = CollectStats ? Util.EnvironmentTickCount() : 0;
             int tempTick = 0, tempTick2 = 0;
@@ -3077,26 +2687,12 @@ namespace OpenSim.Region.PhysicsModule.ODE
 
             float fps = 0;
 
-            float timeLeft = timeStep;
-
-            //m_log.Info(timeStep.ToString());
-//            step_time += timeSte
-//            
-//            // If We're loaded down by something else,
-//            // or debugging with the Visual Studio project on pause
-//            // skip a few frames to catch up gracefully.
-//            // without shooting the physicsactors all over the place
-//
-//            if (step_time >= m_SkipFramesAtms)
-//            {
-//                // Instead of trying to catch up, it'll do 5 physics frames only
-//                step_time = ODE_STEPSIZE;
-//                m_physicsiterations = 5;
-//            }
-//            else
-//            {
-//                m_physicsiterations = 10;
-//            }
+            step_time += timeStep;
+
+            float HalfOdeStep = ODE_STEPSIZE * 0.5f;
+            if (step_time < HalfOdeStep)
+                return 0;
+
 
             // We change _collisionEventPrimChanges to avoid locking _collisionEventPrim itself and causing potential
             // deadlock if the collision event tries to lock something else later on which is already locked by a
@@ -3120,29 +2716,12 @@ namespace OpenSim.Region.PhysicsModule.ODE
                 CreateRequestedJoints(); // this must be outside of the lock (OdeLock) to avoid deadlocks
             }
 
+
             lock (OdeLock)
             {
-                // Process 10 frames if the sim is running normal..
-                // process 5 frames if the sim is running slow
-                //try
-                //{
-                    //d.WorldSetQuickStepNumIterations(world, m_physicsiterations);
-                //}
-                //catch (StackOverflowException)
-                //{
-                   // m_log.Error("[PHYSICS]: The operating system wasn't able to allocate enough memory for the simulation.  Restarting the sim.");
-                   // ode.drelease(world);
-                    //base.TriggerPhysicsBasedRestart();
-                //}
+                d.AllocateODEDataForThread(~0U);
 
-                // Figure out the Frames Per Second we're going at.
-                //(step_time == 0.004f, there's 250 of those per second.   Times the step time/step size
-
-                fps = (timeStep / ODE_STEPSIZE) * 1000;
-                // HACK: Using a time dilation of 1.0 to debug rubberbanding issues
-                //m_timeDilation = Math.Min((step_time / ODE_STEPSIZE) / (0.09375f / ODE_STEPSIZE), 1.0f);
-
-                while (timeLeft > 0.0f)
+                 while (step_time > HalfOdeStep)
                 {
                     try
                     {
@@ -3241,10 +2820,6 @@ namespace OpenSim.Region.PhysicsModule.ODE
                             tempTick = tempTick2;
                         }
 
-                        //if ((framecount % m_randomizeWater) == 0)
-                           // randomizeWater(waterlevel);
-
-                        //int RayCastTimeMS = m_rayCastManager.ProcessQueuedRequests();
                         m_rayCastManager.ProcessQueuedRequests();
 
                         if (CollectStats)
@@ -3265,7 +2840,7 @@ namespace OpenSim.Region.PhysicsModule.ODE
 
                         foreach (PhysicsActor obj in m_collisionEventActors.Values)
                         {
-//                                m_log.DebugFormat("[PHYSICS]: Assessing {0} {1} for collision events", obj.SOPName, obj.LocalID);
+                            // m_log.DebugFormat("[PHYSICS]: Assessing {0} {1} for collision events", obj.SOPName, obj.LocalID);
 
                             switch ((ActorTypes)obj.PhysicsActorType)
                             {
@@ -3295,7 +2870,8 @@ namespace OpenSim.Region.PhysicsModule.ODE
                             tempTick = tempTick2;
                         }
 
-                        d.WorldQuickStep(world, ODE_STEPSIZE);
+                        lock(SimulationLock)
+                            d.WorldQuickStep(world, ODE_STEPSIZE);
 
                         if (CollectStats)
                             m_stats[ODENativeStepFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick);
@@ -3307,7 +2883,8 @@ namespace OpenSim.Region.PhysicsModule.ODE
                         m_log.ErrorFormat("[ODE SCENE]: {0}, {1}, {2}", e.Message, e.TargetSite, e);
                     }
 
-                    timeLeft -= ODE_STEPSIZE;
+                    step_time -= ODE_STEPSIZE;
+                    fps += ODE_STEPSIZE;
                 }
 
                 if (CollectStats)
@@ -3403,6 +2980,7 @@ namespace OpenSim.Region.PhysicsModule.ODE
                     m_stats[ODETotalFrameMsStatName] += Util.EnvironmentTickCountSubtract(startFrameTick);
             }
 
+            fps *= 1.0f/timeStep;
             return fps;
         }
 
@@ -3531,7 +3109,7 @@ namespace OpenSim.Region.PhysicsModule.ODE
                                         // as the axis for the hinge.
 
                                         // Therefore, we must get the joint's coordinate frame based on the
-                                        // joint.Rotation field, which originates from the orientation of the 
+                                        // joint.Rotation field, which originates from the orientation of the
                                         // joint's proxy object in the scene.
 
                                         // The joint's coordinate frame is defined as the transformation matrix
@@ -3659,7 +3237,7 @@ namespace OpenSim.Region.PhysicsModule.ODE
             int startTime = Util.EnvironmentTickCount();
             m_log.DebugFormat("[ODE SCENE]: Setting terrain for {0} with offset {1}", PhysicsSceneName, pOffset);
 
-            
+
             float[] _heightmap;
 
             // ok im lasy this are just a aliases
@@ -3675,7 +3253,6 @@ namespace OpenSim.Region.PhysicsModule.ODE
 
             _heightmap = new float[heightmapWidthSamples * heightmapHeightSamples];
 
-
             const float scale = 1.0f;
             const float offset = 0.0f;
             const float thickness = 10f;
@@ -3722,6 +3299,8 @@ namespace OpenSim.Region.PhysicsModule.ODE
 
             lock (OdeLock)
             {
+                d.AllocateODEDataForThread(~0U);
+
                 IntPtr GroundGeom = IntPtr.Zero;
                 if (RegionTerrain.TryGetValue(pOffset, out GroundGeom))
                 {
@@ -3740,7 +3319,8 @@ namespace OpenSim.Region.PhysicsModule.ODE
                 IntPtr HeightmapData = d.GeomHeightfieldDataCreate();
                 d.GeomHeightfieldDataBuildSingle(HeightmapData, _heightmap, 0,
                             heightmapWidth, heightmapHeight,
-                            (int)heightmapWidthSamples, (int)heightmapHeightSamples,
+                            (int)heightmapWidthSamples,
+                            (int)heightmapHeightSamples,
                             scale, offset, thickness, wrap);
 
                 d.GeomHeightfieldDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1);
@@ -3765,7 +3345,7 @@ namespace OpenSim.Region.PhysicsModule.ODE
 
                 d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle);
                 d.GeomSetRotation(GroundGeom, ref R);
-                d.GeomSetPosition(GroundGeom, pOffset.X + regionsizeX * 0.5f, pOffset.Y + regionsizeY * 0.5f, 0f);
+                d.GeomSetPosition(GroundGeom, pOffset.X + regionsizeX * 0.5f, pOffset.Y + regionsizeY * 0.5f, 0.0f);
                 IntPtr testGround = IntPtr.Zero;
                 if (RegionTerrain.TryGetValue(pOffset, out testGround))
                 {
@@ -3788,82 +3368,30 @@ namespace OpenSim.Region.PhysicsModule.ODE
             return waterlevel;
         }
 
-        public override bool SupportsCombining()
-        {
-            return m_suportCombine;
-        }
-
         public override void SetWaterLevel(float baseheight)
         {
             waterlevel = baseheight;
-//            randomizeWater(waterlevel);
         }
 
-/*
-        private void randomizeWater(float baseheight)
+        [HandleProcessCorruptedStateExceptions]
+        public override void Dispose()
         {
-            uint heightmapWidth = m_regionWidth + 2;
-            uint heightmapHeight = m_regionHeight + 2;
-            uint heightmapWidthSamples = m_regionWidth + 2;
-            uint heightmapHeightSamples = m_regionHeight + 2;
-            float scale = 1.0f;
-            float offset = 0.0f;
-            float thickness = 2.9f;
-            int wrap = 0;
-
-            for (int i = 0; i < (258 * 258); i++)
-            {
-                _watermap[i] = (baseheight-0.1f) + ((float)fluidRandomizer.Next(1,9) / 10f);
-                // m_log.Info((baseheight - 0.1f) + ((float)fluidRandomizer.Next(1, 9) / 10f));
-            }
-
-            lock (OdeLock)
+            lock(SimulationLock)
+                lock(OdeLock)
             {
-                if (WaterGeom != IntPtr.Zero)
-                {
-                    d.SpaceRemove(space, WaterGeom);
-                }
-                IntPtr HeightmapData = d.GeomHeightfieldDataCreate();
-                d.GeomHeightfieldDataBuildSingle(HeightmapData, _watermap, 0, heightmapWidth, heightmapHeight,
-                                                    (int)heightmapWidthSamples, (int)heightmapHeightSamples, scale,
-                                                    offset, thickness, wrap);
-                d.GeomHeightfieldDataSetBounds(HeightmapData, m_regionWidth, m_regionHeight);
-                WaterGeom = d.CreateHeightfield(space, HeightmapData, 1);
-                if (WaterGeom != IntPtr.Zero)
-                {
-                    d.GeomSetCategoryBits(WaterGeom, (int)(CollisionCategories.Water));
-                    d.GeomSetCollideBits(WaterGeom, (int)(CollisionCategories.Space));
-                }
-
-                geom_name_map[WaterGeom] = "Water";
-
-                d.Matrix3 R = new d.Matrix3();
-
-                Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f);
-                Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f);
-                //Axiom.Math.Quaternion q3 = Axiom.Math.Quaternion.FromAngleAxis(3.14f, new Axiom.Math.Vector3(0, 0, 1));
+                if(world == IntPtr.Zero)
+                    return;
 
-                q1 = q1 * q2;
-                //q1 = q1 * q3;
-                Vector3 v3;
-                float angle;
-                q1.GetAxisAngle(out v3, out angle);
+                _worldInitialized = false;
 
-                d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle);
-                d.GeomSetRotation(WaterGeom, ref R);
-                d.GeomSetPosition(WaterGeom, 128, 128, 0);
-            }
-        }
-*/
-        public override void Dispose()
-        {
-            _worldInitialized = false;
+                d.AllocateODEDataForThread(~0U);
 
-            m_rayCastManager.Dispose();
-            m_rayCastManager = null;
+                if (m_rayCastManager != null)
+                {
+                    m_rayCastManager.Dispose();
+                    m_rayCastManager = null;
+                }
 
-            lock (OdeLock)
-            {
                 lock (_prims)
                 {
                     foreach (OdePrim prm in _prims)
@@ -3876,10 +3404,33 @@ namespace OpenSim.Region.PhysicsModule.ODE
                 //{
                     //RemoveAvatar(act);
                 //}
-                d.WorldDestroy(world);
-                //d.CloseODE();
+                IntPtr GroundGeom = IntPtr.Zero;
+                if (RegionTerrain.TryGetValue(m_worldOffset, out GroundGeom))
+                {
+                    RegionTerrain.Remove(m_worldOffset);
+                    if (GroundGeom != IntPtr.Zero)
+                    {
+                        if (TerrainHeightFieldHeights.ContainsKey(GroundGeom))
+                            TerrainHeightFieldHeights.Remove(GroundGeom);
+                        d.GeomDestroy(GroundGeom);
+                    }
+                 }
+
+                try
+                {
+                    d.WorldDestroy(world);
+                    world = IntPtr.Zero;
+                }
+                catch (AccessViolationException e)
+                {
+                    m_log.ErrorFormat("[ODE SCENE]: exception {0}", e.Message);
+                }
             }
+        }
 
+        private int compareByCollisionsDesc(OdePrim A, OdePrim B)
+        {
+            return -A.CollisionScore.CompareTo(B.CollisionScore);
         }
 
         public override Dictionary<uint, float> GetTopColliders()
@@ -3889,7 +3440,7 @@ namespace OpenSim.Region.PhysicsModule.ODE
             lock (_prims)
             {
                 List<OdePrim> orderedPrims = new List<OdePrim>(_prims);
-                orderedPrims.OrderByDescending(p => p.CollisionScore);
+                orderedPrims.Sort(compareByCollisionsDesc);
                 topColliders = orderedPrims.Take(25).ToDictionary(p => p.LocalID, p => p.CollisionScore);
 
                 foreach (OdePrim p in _prims)
@@ -3940,133 +3491,6 @@ namespace OpenSim.Region.PhysicsModule.ODE
             return new List<ContactResult>(ourResults);
         }
 
-#if USE_DRAWSTUFF
-        // Keyboard callback
-        public void command(int cmd)
-        {
-            IntPtr geom;
-            d.Mass mass;
-            d.Vector3 sides = new d.Vector3(d.RandReal() * 0.5f + 0.1f, d.RandReal() * 0.5f + 0.1f, d.RandReal() * 0.5f + 0.1f);
-
-            
-
-            Char ch = Char.ToLower((Char)cmd);
-            switch ((Char)ch)
-            {
-                case 'w':
-                    try
-                    {
-                        Vector3 rotate = (new Vector3(1, 0, 0) * Quaternion.CreateFromEulers(hpr.Z * Utils.DEG_TO_RAD, hpr.Y * Utils.DEG_TO_RAD, hpr.X * Utils.DEG_TO_RAD));
-
-                        xyz.X += rotate.X; xyz.Y += rotate.Y; xyz.Z += rotate.Z;
-                        ds.SetViewpoint(ref xyz, ref hpr);
-                    }
-                    catch (ArgumentException)
-                    { hpr.X = 0; }
-                    break;
-
-                case 'a':
-                    hpr.X++;
-                    ds.SetViewpoint(ref xyz, ref hpr);
-                    break;
-
-                case 's':
-                    try
-                    {
-                        Vector3 rotate2 = (new Vector3(-1, 0, 0) * Quaternion.CreateFromEulers(hpr.Z * Utils.DEG_TO_RAD, hpr.Y * Utils.DEG_TO_RAD, hpr.X * Utils.DEG_TO_RAD));
-
-                        xyz.X += rotate2.X; xyz.Y += rotate2.Y; xyz.Z += rotate2.Z;
-                        ds.SetViewpoint(ref xyz, ref hpr);
-                    }
-                    catch (ArgumentException)
-                    { hpr.X = 0; }
-                    break;
-                case 'd':
-                    hpr.X--;
-                    ds.SetViewpoint(ref xyz, ref hpr);
-                    break;
-                case 'r':
-                    xyz.Z++;
-                    ds.SetViewpoint(ref xyz, ref hpr);
-                    break;
-                case 'f':
-                    xyz.Z--;
-                    ds.SetViewpoint(ref xyz, ref hpr);
-                    break;
-                case 'e':
-                    xyz.Y++;
-                    ds.SetViewpoint(ref xyz, ref hpr);
-                    break;
-                case 'q':
-                    xyz.Y--;
-                    ds.SetViewpoint(ref xyz, ref hpr);
-                    break;
-            }
-        }
-
-        public void step(int pause)
-        {
-            
-            ds.SetColor(1.0f, 1.0f, 0.0f);
-            ds.SetTexture(ds.Texture.Wood);
-            lock (_prims)
-            {
-                foreach (OdePrim prm in _prims)
-                {
-                    //IntPtr body = d.GeomGetBody(prm.prim_geom);
-                    if (prm.prim_geom != IntPtr.Zero)
-                    {
-                        d.Vector3 pos;
-                        d.GeomCopyPosition(prm.prim_geom, out pos);
-                        //d.BodyCopyPosition(body, out pos);
-
-                        d.Matrix3 R;
-                        d.GeomCopyRotation(prm.prim_geom, out R);
-                        //d.BodyCopyRotation(body, out R);
-
-
-                        d.Vector3 sides = new d.Vector3();
-                        sides.X = prm.Size.X;
-                        sides.Y = prm.Size.Y;
-                        sides.Z = prm.Size.Z;
-
-                        ds.DrawBox(ref pos, ref R, ref sides);
-                    }
-                }
-            }
-            ds.SetColor(1.0f, 0.0f, 0.0f);
-
-            foreach (OdeCharacter chr in _characters)
-            {
-                if (chr.Shell != IntPtr.Zero)
-                {
-                    IntPtr body = d.GeomGetBody(chr.Shell);
-
-                    d.Vector3 pos;
-                    d.GeomCopyPosition(chr.Shell, out pos);
-                    //d.BodyCopyPosition(body, out pos);
-
-                    d.Matrix3 R;
-                    d.GeomCopyRotation(chr.Shell, out R);
-                    //d.BodyCopyRotation(body, out R);
-
-                    ds.DrawCapsule(ref pos, ref R, chr.Size.Z, 0.35f);
-                    d.Vector3 sides = new d.Vector3();
-                    sides.X = 0.5f;
-                    sides.Y = 0.5f;
-                    sides.Z = 0.5f;
-
-                    ds.DrawBox(ref pos, ref R, ref sides);
-                }
-            }
-        }
-
-        public void start(int unused)
-        {
-            ds.SetViewpoint(ref xyz, ref hpr);
-        }
-#endif
-
         public override Dictionary<string, float> GetStats()
         {
             if (!CollectStats)
diff --git a/OpenSim/Region/PhysicsModules/Ode/Tests/ODETestClass.cs b/OpenSim/Region/PhysicsModules/Ode/Tests/ODETestClass.cs
index 6dc22bd..2c134e7 100644
--- a/OpenSim/Region/PhysicsModules/Ode/Tests/ODETestClass.cs
+++ b/OpenSim/Region/PhysicsModules/Ode/Tests/ODETestClass.cs
@@ -47,6 +47,8 @@ namespace OpenSim.Region.PhysicsModule.ODE.Tests
 
         //private OpenSim.Region.PhysicsModule.ODE.OdePlugin cbt;
         private PhysicsScene pScene;
+        private OpenSim.Region.PhysicsModule.ODE.OdeModule odemodule;
+
 
         [SetUp]
         public void Initialize()
@@ -65,19 +67,18 @@ namespace OpenSim.Region.PhysicsModule.ODE.Tests
             info.RegionSizeX = info.RegionSizeY = info.RegionSizeZ = Constants.RegionSize;
             OpenSim.Region.Framework.Scenes.Scene scene = new OpenSim.Region.Framework.Scenes.Scene(info);
 
-            //IMesher mesher = new OpenSim.Region.PhysicsModules.Meshing.Meshmerizer();
+            //IMesher mesher = new OpenSim.Region.PhysicsModule.Meshing.Meshmerizer();
             //INonSharedRegionModule mod = mesher as INonSharedRegionModule;
             //mod.Initialise(openSimINI);
             //mod.AddRegion(scene);
             //mod.RegionLoaded(scene);
 
-            pScene = new OdeScene();
-            Console.WriteLine("HERE " + (pScene == null ? "Null" : "Not null"));
-            INonSharedRegionModule mod = (pScene as INonSharedRegionModule);
-            Console.WriteLine("HERE " + (mod == null ? "Null" : "Not null"));
-            mod.Initialise(openSimINI);
-            mod.AddRegion(scene);
-            mod.RegionLoaded(scene);
+            //            pScene = new OdeScene();
+            odemodule = new OpenSim.Region.PhysicsModule.ODE.OdeModule();
+            Console.WriteLine("HERE " + (odemodule == null ? "Null" : "Not null"));
+            odemodule.Initialise(openSimINI);
+            odemodule.AddRegion(scene);
+            odemodule.RegionLoaded(scene);
 
             // Loading ODEPlugin
             //cbt = new OdePlugin();
@@ -90,6 +91,7 @@ namespace OpenSim.Region.PhysicsModule.ODE.Tests
             {
                 _heightmap[i] = 21f;
             }
+            pScene = scene.PhysicsScene;
             pScene.SetTerrain(_heightmap);
         }
 
diff --git a/OpenSim/Region/PhysicsModules/Ode/drawstuff.cs b/OpenSim/Region/PhysicsModules/Ode/drawstuff.cs
deleted file mode 100644
index 87ca446..0000000
--- a/OpenSim/Region/PhysicsModules/Ode/drawstuff.cs
+++ /dev/null
@@ -1,98 +0,0 @@
-/*
- * Copyright ODE
- * Ode.NET - .NET bindings for ODE
- *  Jason Perkins (starkos@industriousone.com)
- *  Licensed under the New BSD
- *  Part of the OpenDynamicsEngine
-Open Dynamics Engine
-Copyright (c) 2001-2007, Russell L. Smith.
-All rights reserved.
-
-Redistribution and use in source and binary forms, with or without
-modification, are permitted provided that the following conditions
-are met:
-
-Redistributions of source code must retain the above copyright notice,
-this list of conditions and the following disclaimer.
-
-Redistributions in binary form must reproduce the above copyright notice,
-this list of conditions and the following disclaimer in the documentation
-and/or other materials provided with the distribution.
-
-Neither the names of ODE's copyright owner nor the names of its
-contributors may be used to endorse or promote products derived from
-this software without specific prior written permission.
-
-THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
-OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
-SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
-TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
-PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
-LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
-NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
-SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- * 
- * 
- */
-
-using System;
-using System.Runtime.InteropServices;
-using Ode.NET;
-
-namespace Drawstuff.NET
-{
-#if dDOUBLE
-    using dReal = System.Double;
-#else
-    using dReal = System.Single;
-#endif
-
-    public static class ds
-    {
-        public const int VERSION = 2;
-
-        public enum Texture
-        {
-            None,
-            Wood
-        }
-
-        [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
-        public delegate void CallbackFunction(int arg);
-
-        [StructLayout(LayoutKind.Sequential)]
-        public struct Functions
-        {
-            public int version;
-            public CallbackFunction start;
-            public CallbackFunction step;
-            public CallbackFunction command;
-            public CallbackFunction stop;
-            public string path_to_textures;
-        }
-
-        [DllImport("drawstuff", EntryPoint = "dsDrawBox")]
-        public static extern void DrawBox(ref d.Vector3 pos, ref d.Matrix3 R, ref d.Vector3 sides);
-
-        [DllImport("drawstuff", EntryPoint = "dsDrawCapsule")]
-        public static extern void DrawCapsule(ref d.Vector3 pos, ref d.Matrix3 R, dReal length, dReal radius);
-
-        [DllImport("drawstuff", EntryPoint = "dsDrawConvex")]
-        public static extern void DrawConvex(ref d.Vector3 pos, ref d.Matrix3 R, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons);
-
-        [DllImport("drawstuff", EntryPoint = "dsSetColor")]
-        public static extern void SetColor(float red, float green, float blue);
-
-        [DllImport("drawstuff", EntryPoint = "dsSetTexture")]
-        public static extern void SetTexture(Texture texture);
-
-        [DllImport("drawstuff", EntryPoint = "dsSetViewpoint")]
-        public static extern void SetViewpoint(ref d.Vector3 xyz, ref d.Vector3 hpr);
-
-        [DllImport("drawstuff", EntryPoint = "dsSimulationLoop")]
-        public static extern void SimulationLoop(int argc, string[] argv, int window_width, int window_height, ref Functions fn);
-    }
-}
diff --git a/OpenSim/Region/PhysicsModules/POS/AssemblyInfo.cs b/OpenSim/Region/PhysicsModules/POS/AssemblyInfo.cs
index e3a3e35..23fe9ae 100644
--- a/OpenSim/Region/PhysicsModules/POS/AssemblyInfo.cs
+++ b/OpenSim/Region/PhysicsModules/POS/AssemblyInfo.cs
@@ -56,7 +56,7 @@ using Mono.Addins;
 // You can specify all values by your own or you can build default build and revision
 // numbers with the '*' character (the default):
 
-[assembly : AssemblyVersion("0.8.2.*")]
+[assembly : AssemblyVersion(OpenSim.VersionInfo.AssemblyVersionNumber)]
 
 [assembly: Addin("OpenSim.Region.PhysicsModule.POS", OpenSim.VersionInfo.VersionNumber)]
 [assembly: AddinDependency("OpenSim.Region.Framework", OpenSim.VersionInfo.VersionNumber)]
diff --git a/OpenSim/Region/PhysicsModules/POS/POSCharacter.cs b/OpenSim/Region/PhysicsModules/POS/POSCharacter.cs
index 32469d9..341530e 100644
--- a/OpenSim/Region/PhysicsModules/POS/POSCharacter.cs
+++ b/OpenSim/Region/PhysicsModules/POS/POSCharacter.cs
@@ -248,7 +248,7 @@ namespace OpenSim.Region.PhysicsModule.POS
         {
         }
 
-        public override void LockAngularMotion(Vector3 axis)
+        public override void LockAngularMotion(byte axislocks)
         {
         }
 
@@ -273,10 +273,10 @@ namespace OpenSim.Region.PhysicsModule.POS
             set { return; }
         }
 
-        public override bool PIDActive 
-        { 
+        public override bool PIDActive
+        {
             get { return false; }
-            set { return; } 
+            set { return; }
         }
 
         public override float PIDTau
@@ -291,6 +291,7 @@ namespace OpenSim.Region.PhysicsModule.POS
 
         public override bool PIDHoverActive
         {
+            get { return false; }
             set { return; }
         }
 
@@ -303,7 +304,7 @@ namespace OpenSim.Region.PhysicsModule.POS
         {
             set { return; }
         }
-        
+
         public override Quaternion APIDTarget
         {
             set { return; }
diff --git a/OpenSim/Region/PhysicsModules/POS/POSPrim.cs b/OpenSim/Region/PhysicsModules/POS/POSPrim.cs
index 8aae716..180885d 100644
--- a/OpenSim/Region/PhysicsModules/POS/POSPrim.cs
+++ b/OpenSim/Region/PhysicsModules/POS/POSPrim.cs
@@ -250,7 +250,7 @@ namespace OpenSim.Region.PhysicsModule.POS
         {
         }
 
-        public override void LockAngularMotion(Vector3 axis)
+        public override void LockAngularMotion(byte axislocks)
         {
         }
 
@@ -286,6 +286,7 @@ namespace OpenSim.Region.PhysicsModule.POS
 
         public override bool PIDHoverActive
         {
+            get { return false; }
             set { return; }
         }
 
@@ -298,7 +299,7 @@ namespace OpenSim.Region.PhysicsModule.POS
         {
             set { return; }
         }
-        
+
         public override Quaternion APIDTarget
         {
             set { return; }
diff --git a/OpenSim/Region/PhysicsModules/POS/POSScene.cs b/OpenSim/Region/PhysicsModules/POS/POSScene.cs
index 6375f18..e62d876 100644
--- a/OpenSim/Region/PhysicsModules/POS/POSScene.cs
+++ b/OpenSim/Region/PhysicsModules/POS/POSScene.cs
@@ -54,6 +54,11 @@ namespace OpenSim.Region.PhysicsModule.POS
             get { return "POS"; }
         }
 
+        public string Version
+        {
+            get { return "1.0"; }
+        }
+
         public Type ReplaceableInterface
         {
             get { return null; }
@@ -83,6 +88,7 @@ namespace OpenSim.Region.PhysicsModule.POS
 
             EngineType = Name;
             PhysicsSceneName = EngineType + "/" + scene.RegionInfo.RegionName;
+            EngineName = Name + " " + Version;
 
             scene.RegisterModuleInterface<PhysicsScene>(this);
             base.Initialise(scene.PhysicsRequestAsset,
@@ -288,7 +294,7 @@ namespace OpenSim.Region.PhysicsModule.POS
                     character._velocity.Z = (character.Position.Z - oldposZ)/timeStep;
                 }
             }
-            return fps;
+            return 1.0f;
         }
 
         public override void GetResults()
diff --git a/OpenSim/Region/PhysicsModules/SharedBase/AssemblyInfo.cs b/OpenSim/Region/PhysicsModules/SharedBase/AssemblyInfo.cs
index 33f60e4..9027e2e 100644
--- a/OpenSim/Region/PhysicsModules/SharedBase/AssemblyInfo.cs
+++ b/OpenSim/Region/PhysicsModules/SharedBase/AssemblyInfo.cs
@@ -55,4 +55,4 @@ using System.Runtime.InteropServices;
 // You can specify all values by your own or you can build default build and revision
 // numbers with the '*' character (the default):
 
-[assembly : AssemblyVersion("0.8.2.*")]
+[assembly : AssemblyVersion(OpenSim.VersionInfo.AssemblyVersionNumber)]
diff --git a/OpenSim/Region/PhysicsModules/SharedBase/IMesher.cs b/OpenSim/Region/PhysicsModules/SharedBase/IMesher.cs
index 5c75307..1a8409e 100644
--- a/OpenSim/Region/PhysicsModules/SharedBase/IMesher.cs
+++ b/OpenSim/Region/PhysicsModules/SharedBase/IMesher.cs
@@ -27,6 +27,7 @@
 
 using System;
 using System.Collections.Generic;
+using System.Runtime.InteropServices;
 using OpenSim.Framework;
 using OpenMetaverse;
 
@@ -36,13 +37,18 @@ namespace OpenSim.Region.PhysicsModules.SharedBase
     {
         IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod);
         IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical);
-        IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool shouldCache);
+        IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool convex, bool forOde);
+        IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool shouldCache, bool convex, bool forOde);
+        IMesh GetMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool convex);
+        void ReleaseMesh(IMesh mesh);
+        void ExpireReleaseMeshs();
+        void ExpireFileCache();
     }
 
     // Values for level of detail to be passed to the mesher.
     // Values origionally chosen for the LOD of sculpties (the sqrt(width*heigth) of sculpt texture)
     // Lower level of detail reduces the number of vertices used to represent the meshed shape.
-    public enum LevelOfDetail 
+    public enum LevelOfDetail
     {
         High = 32,
         Medium = 16,
@@ -54,6 +60,25 @@ namespace OpenSim.Region.PhysicsModules.SharedBase
     {
     }
 
+    [Serializable()]
+    [StructLayout(LayoutKind.Explicit)]
+    public struct AMeshKey
+    {
+        [FieldOffset(0)]
+        public UUID uuid;
+        [FieldOffset(0)]
+        public ulong hashA;
+        [FieldOffset(8)]
+        public ulong hashB;
+        [FieldOffset(16)]
+        public ulong hashC;
+
+        public override string ToString()
+        {
+            return uuid.ToString() + "-" + hashC.ToString("x") ;
+        }
+    }
+
     public interface IMesh
     {
         List<Vector3> getVertexList();
@@ -67,5 +92,7 @@ namespace OpenSim.Region.PhysicsModules.SharedBase
         void releasePinned();
         void Append(IMesh newMesh);
         void TransformLinear(float[,] matrix, float[] offset);
+        Vector3 GetCentroid();
+        Vector3 GetOBB();
     }
 }
diff --git a/OpenSim/Region/PhysicsModules/SharedBase/IPhysicsParameters.cs b/OpenSim/Region/PhysicsModules/SharedBase/IPhysicsParameters.cs
index fb0c9e2..fb0c9e2 100644..100755
--- a/OpenSim/Region/PhysicsModules/SharedBase/IPhysicsParameters.cs
+++ b/OpenSim/Region/PhysicsModules/SharedBase/IPhysicsParameters.cs
diff --git a/OpenSim/Region/PhysicsModules/SharedBase/NullPhysicsScene.cs b/OpenSim/Region/PhysicsModules/SharedBase/NullPhysicsScene.cs
index 432708c..8079e79 100644
--- a/OpenSim/Region/PhysicsModules/SharedBase/NullPhysicsScene.cs
+++ b/OpenSim/Region/PhysicsModules/SharedBase/NullPhysicsScene.cs
@@ -37,7 +37,7 @@ namespace OpenSim.Region.PhysicsModules.SharedBase
     class NullPhysicsScene : PhysicsScene
     {
         private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
-        
+
         private static int m_workIndicator;
 
         public override PhysicsActor AddAvatar(
diff --git a/OpenSim/Region/PhysicsModules/SharedBase/PhysicsActor.cs b/OpenSim/Region/PhysicsModules/SharedBase/PhysicsActor.cs
index c04ff58..2fa98b5 100644
--- a/OpenSim/Region/PhysicsModules/SharedBase/PhysicsActor.cs
+++ b/OpenSim/Region/PhysicsModules/SharedBase/PhysicsActor.cs
@@ -43,7 +43,8 @@ namespace OpenSim.Region.PhysicsModules.SharedBase
         Unknown = 0,
         Agent = 1,
         Prim = 2,
-        Ground = 3
+        Ground = 3,
+        Water = 4
     }
 
     public enum PIDHoverType
@@ -54,20 +55,54 @@ namespace OpenSim.Region.PhysicsModules.SharedBase
         Absolute
     }
 
+    public struct CameraData
+    {
+        public Quaternion CameraRotation;
+        public Vector3 CameraAtAxis;
+        public bool MouseLook;
+        public bool Valid;
+    }
+
     public struct ContactPoint
     {
         public Vector3 Position;
         public Vector3 SurfaceNormal;
         public float PenetrationDepth;
+        public float RelativeSpeed;
+        public bool CharacterFeet;
 
         public ContactPoint(Vector3 position, Vector3 surfaceNormal, float penetrationDepth)
         {
             Position = position;
             SurfaceNormal = surfaceNormal;
             PenetrationDepth = penetrationDepth;
+            RelativeSpeed = 0f; // for now let this one be set explicity
+            CharacterFeet = true;  // keep other plugins work as before
+        }
+
+        public ContactPoint(Vector3 position, Vector3 surfaceNormal, float penetrationDepth, bool feet)
+        {
+            Position = position;
+            SurfaceNormal = surfaceNormal;
+            PenetrationDepth = penetrationDepth;
+            RelativeSpeed = 0f; // for now let this one be set explicity
+            CharacterFeet = feet;  // keep other plugins work as before
         }
     }
 
+    public struct ContactData
+    {
+        public float mu;
+        public float bounce;
+        public bool softcolide;
+
+        public ContactData(float _mu, float _bounce, bool _softcolide)
+        {
+            mu = _mu;
+            bounce = _bounce;
+            softcolide = _softcolide;
+        }
+    }
     /// <summary>
     /// Used to pass collision information to OnCollisionUpdate listeners.
     /// </summary>
@@ -100,8 +135,19 @@ namespace OpenSim.Region.PhysicsModules.SharedBase
             }
             else
             {
+                float lastVel = m_objCollisionList[localID].RelativeSpeed;
                 if (m_objCollisionList[localID].PenetrationDepth < contact.PenetrationDepth)
+                {
+                    if(Math.Abs(lastVel) > Math.Abs(contact.RelativeSpeed))
+                        contact.RelativeSpeed = lastVel;
                     m_objCollisionList[localID] = contact;
+                }
+                else if(Math.Abs(lastVel) < Math.Abs(contact.RelativeSpeed))
+                {
+                    ContactPoint tmp = m_objCollisionList[localID];
+                    tmp.RelativeSpeed = contact.RelativeSpeed;
+                    m_objCollisionList[localID] = tmp;
+                }
             }
         }
 
@@ -121,13 +167,15 @@ namespace OpenSim.Region.PhysicsModules.SharedBase
         public delegate void RequestTerseUpdate();
         public delegate void CollisionUpdate(EventArgs e);
         public delegate void OutOfBounds(Vector3 pos);
+        public delegate CameraData GetCameraData();
 
-// disable warning: public events
+        // disable warning: public events
 #pragma warning disable 67
         public event PositionUpdate OnPositionUpdate;
         public event VelocityUpdate OnVelocityUpdate;
         public event OrientationUpdate OnOrientationUpdate;
         public event RequestTerseUpdate OnRequestTerseUpdate;
+        public event GetCameraData OnPhysicsRequestingCameraData;
 
         /// <summary>
         /// Subscribers to this event must synchronously handle the dictionary of collisions received, since the event
@@ -138,15 +186,47 @@ namespace OpenSim.Region.PhysicsModules.SharedBase
         public event OutOfBounds OnOutOfBounds;
 #pragma warning restore 67
 
+        public CameraData TryGetCameraData()
+        {
+            GetCameraData handler = OnPhysicsRequestingCameraData;
+            if (handler != null)
+            {
+                return handler();
+            }
+
+            return new CameraData { Valid = false };
+        }
+
         public static PhysicsActor Null
         {
             get { return new NullPhysicsActor(); }
         }
 
+        public virtual bool Building { get; set; }
+
+        public virtual void getContactData(ref ContactData cdata)
+        {
+            cdata.mu = 0;
+            cdata.bounce = 0;
+        }
+
         public abstract bool Stopped { get; }
 
         public abstract Vector3 Size { get; set; }
 
+        public virtual void setAvatarSize(Vector3 size, float feetOffset)
+        {
+            Size = size;
+        }
+
+        public virtual bool Phantom { get; set; }
+
+        public virtual bool IsVolumeDtc
+        {
+            get { return false; }
+            set { return; }
+        }
+
         public virtual byte PhysicsShapeType { get; set; }
 
         public abstract PrimitiveBaseShape Shape { set; }
@@ -169,20 +249,21 @@ namespace OpenSim.Region.PhysicsModules.SharedBase
         /// XXX: Bizarrely, this cannot be "Terrain" or "Water" right now unless it really is simulating terrain or
         /// water.  This is not a problem due to the formatting of names given by prims and avatars.
         /// </remarks>
-        public string Name { get; protected set; }
+        public string Name { get; set; }
 
         /// <summary>
         /// This is being used by ODE joint code.
         /// </summary>
         public string SOPName;
 
+        public virtual void CrossingStart() { }
         public abstract void CrossingFailure();
 
         public abstract void link(PhysicsActor obj);
 
         public abstract void delink();
 
-        public abstract void LockAngularMotion(Vector3 axis);
+        public abstract void LockAngularMotion(byte axislocks);
 
         public virtual void RequestPhysicsterseUpdate()
         {
@@ -245,6 +326,56 @@ namespace OpenSim.Region.PhysicsModules.SharedBase
         public abstract void VehicleRotationParam(int param, Quaternion rotation);
         public abstract void VehicleFlags(int param, bool remove);
 
+        // This is an overridable version of SetVehicle() that works for all physics engines.
+        // This is VERY inefficient. It behoves any physics engine to override this and
+        //     implement a more efficient setting of all the vehicle parameters.
+        public virtual void SetVehicle(object pvdata)
+        {
+            VehicleData vdata = (VehicleData)pvdata;
+            // vehicleActor.ProcessSetVehicle((VehicleData)vdata);
+
+            this.VehicleType = (int)vdata.m_type;
+            this.VehicleFlags(-1, false);   // clears all flags
+            this.VehicleFlags((int)vdata.m_flags, false);
+
+            // Linear properties
+            this.VehicleVectorParam((int)Vehicle.LINEAR_MOTOR_DIRECTION, vdata.m_linearMotorDirection);
+            this.VehicleVectorParam((int)Vehicle.LINEAR_FRICTION_TIMESCALE, vdata.m_linearFrictionTimescale);
+            this.VehicleFloatParam((int)Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE, vdata.m_linearMotorDecayTimescale);
+            this.VehicleFloatParam((int)Vehicle.LINEAR_MOTOR_TIMESCALE, vdata.m_linearMotorTimescale);
+            this.VehicleVectorParam((int)Vehicle.LINEAR_MOTOR_OFFSET, vdata.m_linearMotorOffset);
+
+            //Angular properties
+            this.VehicleVectorParam((int)Vehicle.ANGULAR_MOTOR_DIRECTION, vdata.m_angularMotorDirection);
+            this.VehicleFloatParam((int)Vehicle.ANGULAR_MOTOR_TIMESCALE, vdata.m_angularMotorTimescale);
+            this.VehicleFloatParam((int)Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE, vdata.m_angularMotorDecayTimescale);
+            this.VehicleVectorParam((int)Vehicle.ANGULAR_FRICTION_TIMESCALE, vdata.m_angularFrictionTimescale);
+
+            //Deflection properties
+            this.VehicleFloatParam((int)Vehicle.ANGULAR_DEFLECTION_EFFICIENCY, vdata.m_angularDeflectionEfficiency);
+            this.VehicleFloatParam((int)Vehicle.ANGULAR_DEFLECTION_TIMESCALE, vdata.m_angularDeflectionTimescale);
+            this.VehicleFloatParam((int)Vehicle.LINEAR_DEFLECTION_EFFICIENCY, vdata.m_linearDeflectionEfficiency);
+            this.VehicleFloatParam((int)Vehicle.LINEAR_DEFLECTION_TIMESCALE, vdata.m_linearDeflectionTimescale);
+
+            //Banking properties
+            this.VehicleFloatParam((int)Vehicle.BANKING_EFFICIENCY, vdata.m_bankingEfficiency);
+            this.VehicleFloatParam((int)Vehicle.BANKING_MIX, vdata.m_bankingMix);
+            this.VehicleFloatParam((int)Vehicle.BANKING_TIMESCALE, vdata.m_bankingTimescale);
+
+            //Hover and Buoyancy properties
+            this.VehicleFloatParam((int)Vehicle.HOVER_HEIGHT, vdata.m_VhoverHeight);
+            this.VehicleFloatParam((int)Vehicle.HOVER_EFFICIENCY, vdata.m_VhoverEfficiency);
+            this.VehicleFloatParam((int)Vehicle.HOVER_TIMESCALE, vdata.m_VhoverTimescale);
+            this.VehicleFloatParam((int)Vehicle.BUOYANCY, vdata.m_VehicleBuoyancy);
+
+            //Attractor properties
+            this.VehicleFloatParam((int)Vehicle.VERTICAL_ATTRACTION_EFFICIENCY, vdata.m_verticalAttractionEfficiency);
+            this.VehicleFloatParam((int)Vehicle.VERTICAL_ATTRACTION_TIMESCALE, vdata.m_verticalAttractionTimescale);
+
+            this.VehicleRotationParam((int)Vehicle.REFERENCE_FRAME, vdata.m_referenceFrame);
+        }
+
+
         /// <summary>
         /// Allows the detection of collisions with inherently non-physical prims. see llVolumeDetect for more
         /// </summary>
@@ -253,6 +384,22 @@ namespace OpenSim.Region.PhysicsModules.SharedBase
         public abstract Vector3 GeometricCenter { get; }
         public abstract Vector3 CenterOfMass { get; }
 
+        public virtual float PhysicsCost
+        {
+            get
+            {
+                return 0.1f;
+            }
+        }
+
+        public virtual float StreamCost
+        {
+            get
+            {
+                return 1.0f;
+            }
+        }
+
         /// <summary>
         /// The desired velocity of this actor.
         /// </summary>
@@ -271,6 +418,7 @@ namespace OpenSim.Region.PhysicsModules.SharedBase
         }
 
         public abstract Vector3 Velocity { get; set; }
+        public virtual Vector3 rootVelocity { get { return Vector3.Zero; } }
 
         public abstract Vector3 Torque { get; set; }
         public abstract float CollisionScore { get; set;}
@@ -296,7 +444,7 @@ namespace OpenSim.Region.PhysicsModules.SharedBase
 
         // Used for llSetHoverHeight and maybe vehicle height
         // Hover Height will override MoveTo target's Z
-        public abstract bool PIDHoverActive { set;}
+        public abstract bool PIDHoverActive {get; set;}
         public abstract float PIDHoverHeight { set;}
         public abstract PIDHoverType PIDHoverType { set;}
         public abstract float PIDHoverTau { set;}
@@ -306,7 +454,7 @@ namespace OpenSim.Region.PhysicsModules.SharedBase
         public abstract bool APIDActive { set;}
         public abstract float APIDStrength { set;}
         public abstract float APIDDamping { set;}
-        
+
         public abstract void AddForce(Vector3 force, bool pushforce);
         public abstract void AddAngularForce(Vector3 force, bool pushforce);
         public abstract void SetMomentum(Vector3 momentum);
@@ -314,6 +462,29 @@ namespace OpenSim.Region.PhysicsModules.SharedBase
         public abstract void UnSubscribeEvents();
         public abstract bool SubscribedEvents();
 
+        public virtual void AddCollisionEvent(uint CollidedWith, ContactPoint contact) { }
+        public virtual void AddVDTCCollisionEvent(uint CollidedWith, ContactPoint contact) { }
+
+        public virtual PhysicsInertiaData GetInertiaData()
+        {
+            PhysicsInertiaData data = new PhysicsInertiaData();
+            data.TotalMass = this.Mass;
+            data.CenterOfMass = CenterOfMass - Position;
+            data.Inertia = Vector3.Zero;
+            data.InertiaRotation = Vector4.Zero;
+            return data;
+        }
+
+        public virtual void SetInertiaData(PhysicsInertiaData inertia)
+        {
+        }
+
+        public virtual float SimulationSuspended { get; set; }
+
+        // Warning in a parent part it returns itself, not null
+        public virtual PhysicsActor ParentActor { get { return this; } }
+
+
         // Extendable interface for new, physics engine specific operations
         public virtual object Extension(string pFunct, params object[] pParams)
         {
@@ -324,9 +495,11 @@ namespace OpenSim.Region.PhysicsModules.SharedBase
 
     public class NullPhysicsActor : PhysicsActor
     {
+        private ActorTypes m_actorType = ActorTypes.Unknown;
+
         public override bool Stopped
         {
-            get{ return false; }
+            get{ return true; }
         }
 
         public override Vector3 Position
@@ -343,6 +516,7 @@ namespace OpenSim.Region.PhysicsModules.SharedBase
 
         public override uint LocalID
         {
+            get { return 0; }
             set { return; }
         }
 
@@ -402,50 +576,17 @@ namespace OpenSim.Region.PhysicsModules.SharedBase
             set { return; }
         }
 
-        public override void VehicleFloatParam(int param, float value)
-        {
+        public override void VehicleFloatParam(int param, float value) {}
+        public override void VehicleVectorParam(int param, Vector3 value) { }
+        public override void VehicleRotationParam(int param, Quaternion rotation) { }
+        public override void VehicleFlags(int param, bool remove) { }
+        public override void SetVolumeDetect(int param) {}
+        public override void SetMaterial(int material) {}
+        public override Vector3 CenterOfMass { get { return Vector3.Zero; }}
 
-        }
-
-        public override void VehicleVectorParam(int param, Vector3 value)
-        {
+        public override Vector3 GeometricCenter { get { return Vector3.Zero; }}
 
-        }
-
-        public override void VehicleRotationParam(int param, Quaternion rotation)
-        {
-
-        }
-
-        public override void VehicleFlags(int param, bool remove)
-        {
-            
-        }
-
-        public override void SetVolumeDetect(int param)
-        {
-
-        }
-
-        public override void SetMaterial(int material)
-        {
-            
-        }
-
-        public override Vector3 CenterOfMass
-        {
-            get { return Vector3.Zero; }
-        }
-
-        public override Vector3 GeometricCenter
-        {
-            get { return Vector3.Zero; }
-        }
-
-        public override PrimitiveBaseShape Shape
-        {
-            set { return; }
-        }
+        public override PrimitiveBaseShape Shape { set { return; }}
 
         public override Vector3 Velocity
         {
@@ -465,9 +606,7 @@ namespace OpenSim.Region.PhysicsModules.SharedBase
             set { }
         }
 
-        public override void CrossingFailure()
-        {
-        }
+        public override void CrossingFailure() {}
 
         public override Quaternion Orientation
         {
@@ -507,8 +646,20 @@ namespace OpenSim.Region.PhysicsModules.SharedBase
 
         public override int PhysicsActorType
         {
-            get { return (int) ActorTypes.Unknown; }
-            set { return; }
+            get { return (int)m_actorType; }
+            set {
+                ActorTypes type = (ActorTypes)value;
+                switch (type)
+                {
+                    case ActorTypes.Ground:
+                    case ActorTypes.Water:
+                        m_actorType = type;
+                        break;
+                    default:
+                        m_actorType = ActorTypes.Unknown;
+                        break;
+                }
+            }
         }
 
         public override bool Kinematic
@@ -517,26 +668,11 @@ namespace OpenSim.Region.PhysicsModules.SharedBase
             set { return; }
         }
 
-        public override void link(PhysicsActor obj)
-        {
-        }
-
-        public override void delink()
-        {
-        }
-
-        public override void LockAngularMotion(Vector3 axis)
-        {
-        }
-
-        public override void AddForce(Vector3 force, bool pushforce)
-        {
-        }
-
-        public override void AddAngularForce(Vector3 force, bool pushforce)
-        {
-            
-        }
+        public override void link(PhysicsActor obj) { }
+        public override void delink() { }
+        public override void LockAngularMotion(byte axislocks) { }
+        public override void AddForce(Vector3 force, bool pushforce) { }
+        public override void AddAngularForce(Vector3 force, bool pushforce) { }
 
         public override Vector3 RotationalVelocity
         {
@@ -546,39 +682,28 @@ namespace OpenSim.Region.PhysicsModules.SharedBase
 
         public override Vector3 PIDTarget { set { return; } }
 
-        public override bool PIDActive 
+        public override bool PIDActive
         {
             get { return false; }
-            set { return; } 
+            set { return; }
         }
 
         public override float PIDTau { set { return; } }
 
         public override float PIDHoverHeight { set { return; } }
-        public override bool PIDHoverActive { set { return; } }
+        public override bool PIDHoverActive {get {return false;} set { return; } }
         public override PIDHoverType PIDHoverType { set { return; } }
         public override float PIDHoverTau { set { return; } }
-        
+
         public override Quaternion APIDTarget { set { return; } }
         public override bool APIDActive { set { return; } }
         public override float APIDStrength { set { return; } }
         public override float APIDDamping { set { return; } }
-        
-        public override void SetMomentum(Vector3 momentum)
-        {
-        }
-
-        public override void SubscribeEvents(int ms)
-        {
 
-        }
-        public override void UnSubscribeEvents()
-        {
+        public override void SetMomentum(Vector3 momentum) { }
 
-        }
-        public override bool SubscribedEvents()
-        {
-            return false;
-        }
+        public override void SubscribeEvents(int ms) { }
+        public override void UnSubscribeEvents() { }
+        public override bool SubscribedEvents() { return false; }
     }
 }
diff --git a/OpenSim/Region/PhysicsModules/SharedBase/PhysicsScene.cs b/OpenSim/Region/PhysicsModules/SharedBase/PhysicsScene.cs
index 32691fc..e8d6334 100644
--- a/OpenSim/Region/PhysicsModules/SharedBase/PhysicsScene.cs
+++ b/OpenSim/Region/PhysicsModules/SharedBase/PhysicsScene.cs
@@ -41,6 +41,10 @@ namespace OpenSim.Region.PhysicsModules.SharedBase
 
     public delegate void RaycastCallback(bool hitYN, Vector3 collisionPoint, uint localid, float distance, Vector3 normal);
     public delegate void RayCallback(List<ContactResult> list);
+    public delegate void ProbeBoxCallback(List<ContactResult> list);
+    public delegate void ProbeSphereCallback(List<ContactResult> list);
+    public delegate void ProbePlaneCallback(List<ContactResult> list);
+    public delegate void SitAvatarCallback(int status, uint partID, Vector3 offset, Quaternion Orientation);
 
     public delegate void JointMoved(PhysicsJoint joint);
     public delegate void JointDeactivated(PhysicsJoint joint);
@@ -89,6 +93,8 @@ namespace OpenSim.Region.PhysicsModules.SharedBase
         public Vector3 Normal;
     }
 
+
+
     public abstract class PhysicsScene
     {
 //        private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
@@ -106,6 +112,8 @@ namespace OpenSim.Region.PhysicsModules.SharedBase
         /// </summary>
         public string EngineType { get; protected set; }
 
+        public string EngineName { get; protected set; }
+
         // The only thing that should register for this event is the SceneGraph
         // Anything else could cause problems.
         public event physicsCrash OnPhysicsCrash;
@@ -143,6 +151,7 @@ namespace OpenSim.Region.PhysicsModules.SharedBase
         /// <param name="size"></param>
         /// <param name="isFlying"></param>
         /// <returns></returns>
+
         public abstract PhysicsActor AddAvatar(
             string avName, Vector3 position, Vector3 velocity, Vector3 size, bool isFlying);
 
@@ -161,12 +170,26 @@ namespace OpenSim.Region.PhysicsModules.SharedBase
         {
             PhysicsActor ret = AddAvatar(avName, position, velocity, size, isFlying);
 
-            if (ret != null) 
+            if (ret != null)
                 ret.LocalID = localID;
 
             return ret;
         }
 
+        public virtual PhysicsActor AddAvatar(
+            uint localID, string avName, Vector3 position, Vector3 size, bool isFlying)
+        {
+            PhysicsActor ret = AddAvatar(localID, avName, position, Vector3.Zero, size, isFlying);
+            return ret;
+        }
+
+        public virtual PhysicsActor AddAvatar(
+            uint localID, string avName, Vector3 position, Vector3 size, float feetOffset, bool isFlying)
+        {
+            PhysicsActor ret = AddAvatar(localID, avName, position, Vector3.Zero, size, isFlying);
+            return ret;
+        }
+
         /// <summary>
         /// Remove an avatar.
         /// </summary>
@@ -182,6 +205,19 @@ namespace OpenSim.Region.PhysicsModules.SharedBase
         public abstract PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
                                                   Vector3 size, Quaternion rotation, bool isPhysical, uint localid);
 
+        public virtual PhysicsActor AddPrimShape(string primName, PhysicsActor parent, PrimitiveBaseShape pbs, Vector3 position,
+                                                  uint localid, byte[] sdata)
+        {
+            return null;
+        }
+
+        public virtual PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
+                                                  Vector3 size, Quaternion rotation, bool isPhysical, bool isPhantom, uint localid)
+        {
+            return AddPrimShape(primName, pbs, position, size, rotation, isPhysical, localid);
+        }
+
+
         public virtual PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
                                                   Vector3 size, Quaternion rotation, bool isPhysical, bool isPhantom, byte shapetype, uint localid)
         {
@@ -255,6 +291,9 @@ namespace OpenSim.Region.PhysicsModules.SharedBase
 
         public abstract void AddPhysicsActorTaint(PhysicsActor prim);
 
+
+        public virtual void ProcessPreSimulation() { }
+
         /// <summary>
         /// Perform a simulation of the current physics scene over the given timestep.
         /// </summary>
@@ -293,28 +332,19 @@ namespace OpenSim.Region.PhysicsModules.SharedBase
             return false;
         }
 
-        public virtual bool SupportsCombining()
-        {
-            return false;
-        }
-
-        public virtual void Combine(PhysicsScene pScene, Vector3 offset, Vector3 extents) {}
-
-        public virtual void UnCombine(PhysicsScene pScene) {}
-
         /// <summary>
         /// Queue a raycast against the physics scene.
         /// The provided callback method will be called when the raycast is complete
-        /// 
-        /// Many physics engines don't support collision testing at the same time as 
-        /// manipulating the physics scene, so we queue the request up and callback 
+        ///
+        /// Many physics engines don't support collision testing at the same time as
+        /// manipulating the physics scene, so we queue the request up and callback
         /// a custom method when the raycast is complete.
         /// This allows physics engines that give an immediate result to callback immediately
         /// and ones that don't, to callback when it gets a result back.
-        /// 
+        ///
         /// ODE for example will not allow you to change the scene while collision testing or
         /// it asserts, 'opteration not valid for locked space'.  This includes adding a ray to the scene.
-        /// 
+        ///
         /// This is named RayCastWorld to not conflict with modrex's Raycast method.
         /// </summary>
         /// <param name="position">Origin of the ray</param>
@@ -348,6 +378,31 @@ namespace OpenSim.Region.PhysicsModules.SharedBase
             return false;
         }
 
+        public virtual List<ContactResult> RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count, RayFilterFlags flags)
+        {
+            return new List<ContactResult>();
+        }
+
+        public virtual List<ContactResult> BoxProbe(Vector3 position, Vector3 size, Quaternion orientation, int Count, RayFilterFlags flags)
+        {
+            return new List<ContactResult>();
+        }
+
+        public virtual List<ContactResult> SphereProbe(Vector3 position, float radius, int Count, RayFilterFlags flags)
+        {
+            return new List<ContactResult>();
+        }
+
+        public virtual List<ContactResult> PlaneProbe(PhysicsActor actor, Vector4 plane, int Count, RayFilterFlags flags)
+        {
+            return new List<ContactResult>();
+        }
+
+        public virtual int SitAvatar(PhysicsActor actor, Vector3 AbsolutePosition, Vector3 CameraPosition, Vector3 offset, Vector3 AvatarSize, SitAvatarCallback PhysicsSitResponse)
+        {
+            return 0;
+        }
+
         // Extendable interface for new, physics engine specific operations
         public virtual object Extension(string pFunct, params object[] pParams)
         {
diff --git a/OpenSim/Region/PhysicsModules/SharedBase/PhysicsVector.cs b/OpenSim/Region/PhysicsModules/SharedBase/PhysicsVector.cs
index 76a82fa..5aae42f 100644
--- a/OpenSim/Region/PhysicsModules/SharedBase/PhysicsVector.cs
+++ b/OpenSim/Region/PhysicsModules/SharedBase/PhysicsVector.cs
@@ -49,7 +49,7 @@ namespace OpenSim.Region.PhysicsModules.SharedBase
         public Vector3(Vector3 pv) : this(pv.X, pv.Y, pv.Z)
         {
         }
-        
+
         public void setValues(float x, float y, float z)
         {
             X = x;
diff --git a/OpenSim/Region/PhysicsModules/SharedBase/VehicleConstants.cs b/OpenSim/Region/PhysicsModules/SharedBase/VehicleConstants.cs
index 63a8cb8..0b4c8c6 100644
--- a/OpenSim/Region/PhysicsModules/SharedBase/VehicleConstants.cs
+++ b/OpenSim/Region/PhysicsModules/SharedBase/VehicleConstants.cs
@@ -26,6 +26,7 @@
  */
 
 using System;
+using OpenMetaverse;
 
 namespace OpenSim.Region.PhysicsModules.SharedBase
 {
@@ -37,7 +38,7 @@ namespace OpenSim.Region.PhysicsModules.SharedBase
         TYPE_NONE = 0,
 
         /// <summary>
-        /// No Angular motor, High Left right friction, No Hover, Linear Deflection 1, no angular deflection 
+        /// No Angular motor, High Left right friction, No Hover, Linear Deflection 1, no angular deflection
         /// no vertical attractor, No banking, Identity rotation frame
         /// </summary>
         TYPE_SLED = 1,
@@ -117,5 +118,47 @@ namespace OpenSim.Region.PhysicsModules.SharedBase
         NO_DEFLECTION = 16392,
         LOCK_ROTATION = 32784
     }
-        
+
+    public struct VehicleData
+    {
+        public Vehicle m_type;
+        public VehicleFlag m_flags;
+
+        // Linear properties
+        public Vector3 m_linearMotorDirection;
+        public Vector3 m_linearFrictionTimescale;
+        public float m_linearMotorDecayTimescale;
+        public float m_linearMotorTimescale;
+        public Vector3 m_linearMotorOffset;
+
+        //Angular properties
+        public Vector3 m_angularMotorDirection;
+        public float m_angularMotorTimescale;
+        public float m_angularMotorDecayTimescale;
+        public Vector3 m_angularFrictionTimescale;
+
+        //Deflection properties
+        public float m_angularDeflectionEfficiency;
+        public float m_angularDeflectionTimescale;
+        public float m_linearDeflectionEfficiency;
+        public float m_linearDeflectionTimescale;
+
+        //Banking properties
+        public float m_bankingEfficiency;
+        public float m_bankingMix;
+        public float m_bankingTimescale;
+
+        //Hover and Buoyancy properties
+        public float m_VhoverHeight;
+        public float m_VhoverEfficiency;
+        public float m_VhoverTimescale;
+        public float m_VehicleBuoyancy;
+
+        //Attractor properties
+        public float m_verticalAttractionEfficiency;
+        public float m_verticalAttractionTimescale;
+
+        // Axis
+        public Quaternion m_referenceFrame;
+    }
 }
diff --git a/OpenSim/Region/PhysicsModules/ubOde/ODEApi.cs b/OpenSim/Region/PhysicsModules/ubOde/ODEApi.cs
new file mode 100644
index 0000000..35adf11
--- /dev/null
+++ b/OpenSim/Region/PhysicsModules/ubOde/ODEApi.cs
@@ -0,0 +1,2024 @@
+/*
+ * based on:
+ * Ode.NET - .NET bindings for ODE
+ * Jason Perkins (starkos@industriousone.com)
+  *  Licensed under the New BSD
+ *  Part of the OpenDynamicsEngine
+Open Dynamics Engine
+Copyright (c) 2001-2007, Russell L. Smith.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions
+are met:
+
+Redistributions of source code must retain the above copyright notice,
+this list of conditions and the following disclaimer.
+
+Redistributions in binary form must reproduce the above copyright notice,
+this list of conditions and the following disclaimer in the documentation
+and/or other materials provided with the distribution.
+
+Neither the names of ODE's copyright owner nor the names of its
+contributors may be used to endorse or promote products derived from
+this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ * changes by opensim team;
+ * changes by Aurora team http://www.aurora-sim.org/
+ * changes by Ubit Umarov
+ */
+
+using System;
+using System.Runtime.InteropServices;
+using System.Security;
+using OMV = OpenMetaverse;
+namespace OdeAPI
+{
+//#if dDOUBLE
+// don't see much use in double precision with time steps of 20ms and 10 iterations used on opensim
+// at least we save same memory and memory access time, FPU performance on intel usually is similar
+//      using dReal = System.Double;
+//#else
+    using dReal = System.Single;
+//#endif
+
+    public static class d
+    {
+        public static dReal Infinity = dReal.MaxValue;
+        public static int NTotalBodies = 0;
+        public static int NTotalGeoms = 0;
+
+        public const uint CONTACTS_UNIMPORTANT = 0x80000000;
+
+        #region Flags and Enumerations
+
+        [Flags]
+        public enum AllocateODEDataFlags : uint
+        {
+            BasicData = 0,
+            CollisionData = 0x00000001,
+            All = ~0u
+        }
+
+        [Flags]
+        public enum IniteODEFlags : uint
+        {
+            dInitFlagManualThreadCleanup = 0x00000001
+        }
+
+        [Flags]
+        public enum ContactFlags : int
+        {
+            Mu2 = 0x001,
+            FDir1 = 0x002,
+            Bounce = 0x004,
+            SoftERP = 0x008,
+            SoftCFM = 0x010,
+            Motion1 = 0x020,
+            Motion2 = 0x040,
+            MotionN = 0x080,
+            Slip1 = 0x100,
+            Slip2 = 0x200,
+            Approx0 = 0x0000,
+            Approx1_1 = 0x1000,
+            Approx1_2 = 0x2000,
+            Approx1 = 0x3000
+        }
+
+        public enum GeomClassID : int
+        {
+            SphereClass,
+            BoxClass,
+            CapsuleClass,
+            CylinderClass,
+            PlaneClass,
+            RayClass,
+            ConvexClass,
+            GeomTransformClass,
+            TriMeshClass,
+            HeightfieldClass,
+            FirstSpaceClass,
+            SimpleSpaceClass = FirstSpaceClass,
+            HashSpaceClass,
+            QuadTreeSpaceClass,
+            LastSpaceClass = QuadTreeSpaceClass,
+            ubtTerrainClass,
+            FirstUserClass,
+            LastUserClass = FirstUserClass + MaxUserClasses - 1,
+            NumClasses,
+            MaxUserClasses = 5
+        }
+
+        public enum JointType : int
+        {
+            None,
+            Ball,
+            Hinge,
+            Slider,
+            Contact,
+            Universal,
+            Hinge2,
+            Fixed,
+            Null,
+            AMotor,
+            LMotor,
+            Plane2D
+        }
+
+        public enum JointParam : int
+        {
+            LoStop,
+            HiStop,
+            Vel,
+            FMax,
+            FudgeFactor,
+            Bounce,
+            CFM,
+            StopERP,
+            StopCFM,
+            SuspensionERP,
+            SuspensionCFM,
+            LoStop2 = 256,
+            HiStop2,
+            Vel2,
+            FMax2,
+            FudgeFactor2,
+            Bounce2,
+            CFM2,
+            StopERP2,
+            StopCFM2,
+            SuspensionERP2,
+            SuspensionCFM2,
+            LoStop3 = 512,
+            HiStop3,
+            Vel3,
+            FMax3,
+            FudgeFactor3,
+            Bounce3,
+            CFM3,
+            StopERP3,
+            StopCFM3,
+            SuspensionERP3,
+            SuspensionCFM3
+        }
+
+        public enum dSweepAndPruneAxis : int
+        {
+            XYZ = ((0)|(1<<2)|(2<<4)),
+            XZY = ((0)|(2<<2)|(1<<4)),
+            YXZ = ((1)|(0<<2)|(2<<4)),
+            YZX = ((1)|(2<<2)|(0<<4)),
+            ZXY = ((2)|(0<<2)|(1<<4)),
+            ZYX = ((2)|(1<<2)|(0<<4))
+        }
+
+        #endregion
+
+        #region Callbacks
+
+        [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
+        public delegate int AABBTestFn(IntPtr o1, IntPtr o2, ref AABB aabb);
+
+        [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
+        public delegate int ColliderFn(IntPtr o1, IntPtr o2, int flags, out ContactGeom contact, int skip);
+
+        [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
+        public delegate void GetAABBFn(IntPtr geom, out AABB aabb);
+
+        [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
+        public delegate ColliderFn GetColliderFnFn(int num);
+
+        [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
+        public delegate void GeomDtorFn(IntPtr o);
+
+        [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
+        public delegate dReal HeightfieldGetHeight(IntPtr p_user_data, int x, int z);
+
+        [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
+        public delegate dReal OSTerrainGetHeight(IntPtr p_user_data, int x, int z);
+
+        [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
+        public delegate void NearCallback(IntPtr data, IntPtr geom1, IntPtr geom2);
+
+        [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
+        public delegate int TriCallback(IntPtr trimesh, IntPtr refObject, int triangleIndex);
+
+        [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
+        public delegate int TriArrayCallback(IntPtr trimesh, IntPtr refObject, int[] triangleIndex, int triCount);
+
+        [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
+        public delegate int TriRayCallback(IntPtr trimesh, IntPtr ray, int triangleIndex, dReal u, dReal v);
+
+        #endregion
+
+        #region Structs
+
+        [StructLayout(LayoutKind.Sequential)]
+        public struct AABB
+        {
+            public dReal MinX, MaxX;
+            public dReal MinY, MaxY;
+            public dReal MinZ, MaxZ;
+        }
+
+
+        [StructLayout(LayoutKind.Sequential)]
+        public struct Contact
+        {
+            public SurfaceParameters surface;
+            public ContactGeom geom;
+            public Vector3 fdir1;
+            public static readonly int unmanagedSizeOf = Marshal.SizeOf(typeof(Contact));
+        }
+
+
+        [StructLayout(LayoutKind.Sequential)]
+        public struct ContactGeom
+        {
+
+            public Vector3 pos;
+            public Vector3 normal;
+            public dReal depth;
+            public IntPtr g1;
+            public IntPtr g2;
+            public int side1;
+            public int side2;
+            public static readonly int unmanagedSizeOf = Marshal.SizeOf(typeof(ContactGeom));
+        }
+
+        [StructLayout(LayoutKind.Sequential)]
+        public struct GeomClass
+        {
+            public int bytes;
+            public GetColliderFnFn collider;
+            public GetAABBFn aabb;
+            public AABBTestFn aabb_test;
+            public GeomDtorFn dtor;
+        }
+
+
+        [StructLayout(LayoutKind.Sequential)]
+        public struct JointFeedback
+        {
+            public Vector3 f1;
+            public Vector3 t1;
+            public Vector3 f2;
+            public Vector3 t2;
+        }
+
+
+        [StructLayout(LayoutKind.Sequential)]
+        public struct Mass
+        {
+            public dReal mass;
+            public Vector4 c;
+            public Matrix3 I;
+        }
+
+
+        [StructLayout(LayoutKind.Sequential)]
+        public struct Matrix3
+        {
+            public Matrix3(dReal m00, dReal m10, dReal m20, dReal m01, dReal m11, dReal m21, dReal m02, dReal m12, dReal m22)
+            {
+                M00 = m00;  M10 = m10;  M20 = m20;  _m30 = 0.0f;
+                M01 = m01;  M11 = m11;  M21 = m21;  _m31 = 0.0f;
+                M02 = m02;  M12 = m12;  M22 = m22;  _m32 = 0.0f;
+            }
+            public dReal M00, M10, M20;
+            private dReal _m30;
+            public dReal M01, M11, M21;
+            private dReal _m31;
+            public dReal M02, M12, M22;
+            private dReal _m32;
+        }
+
+        [StructLayout(LayoutKind.Sequential)]
+        public struct Matrix4
+        {
+            public Matrix4(dReal m00, dReal m10, dReal m20, dReal m30,
+                dReal m01, dReal m11, dReal m21, dReal m31,
+                dReal m02, dReal m12, dReal m22, dReal m32,
+                dReal m03, dReal m13, dReal m23, dReal m33)
+            {
+                M00 = m00; M10 = m10; M20 = m20; M30 = m30;
+                M01 = m01; M11 = m11; M21 = m21; M31 = m31;
+                M02 = m02; M12 = m12; M22 = m22; M32 = m32;
+                M03 = m03; M13 = m13; M23 = m23; M33 = m33;
+            }
+            public dReal M00, M10, M20, M30;
+            public dReal M01, M11, M21, M31;
+            public dReal M02, M12, M22, M32;
+            public dReal M03, M13, M23, M33;
+        }
+
+        [StructLayout(LayoutKind.Sequential)]
+        public struct Quaternion
+        {
+            public dReal W, X, Y, Z;
+        }
+
+
+        [StructLayout(LayoutKind.Sequential)]
+        public struct SurfaceParameters
+        {
+            public ContactFlags mode;
+            public dReal mu;
+            public dReal mu2;
+            public dReal bounce;
+            public dReal bounce_vel;
+            public dReal soft_erp;
+            public dReal soft_cfm;
+            public dReal motion1;
+            public dReal motion2;
+            public dReal motionN;
+            public dReal slip1;
+            public dReal slip2;
+        }
+
+
+        [StructLayout(LayoutKind.Sequential)]
+        public struct Vector3
+        {
+            public Vector3(dReal x, dReal y, dReal z)
+            {
+                X = x;  Y = y;  Z = z;  _w = 0.0f;
+            }
+            public dReal X, Y, Z;
+            private dReal _w;
+        }
+
+
+        [StructLayout(LayoutKind.Sequential)]
+        public struct Vector4
+        {
+            public Vector4(dReal x, dReal y, dReal z, dReal w)
+            {
+                X = x;  Y = y;  Z = z;  W = w;
+            }
+            public dReal X, Y, Z, W;
+        }
+
+        #endregion
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dAllocateODEDataForThread"), SuppressUnmanagedCodeSecurity]
+        public static extern int AllocateODEDataForThread(uint ODEInitFlags);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dAreConnected"), SuppressUnmanagedCodeSecurity]
+        public static extern bool AreConnected(IntPtr b1, IntPtr b2);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dAreConnectedExcluding"), SuppressUnmanagedCodeSecurity]
+        public static extern bool AreConnectedExcluding(IntPtr b1, IntPtr b2, JointType joint_type);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddForce"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyAddForce(IntPtr body, dReal fx, dReal fy, dReal fz);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddForceAtPos"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyAddForceAtPos(IntPtr body, dReal fx, dReal fy, dReal fz, dReal px, dReal py, dReal pz);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddForceAtRelPos"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyAddForceAtRelPos(IntPtr body, dReal fx, dReal fy, dReal fz, dReal px, dReal py, dReal pz);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddRelForce"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyAddRelForce(IntPtr body, dReal fx, dReal fy, dReal fz);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddRelForceAtPos"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyAddRelForceAtPos(IntPtr body, dReal fx, dReal fy, dReal fz, dReal px, dReal py, dReal pz);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddRelForceAtRelPos"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyAddRelForceAtRelPos(IntPtr body, dReal fx, dReal fy, dReal fz, dReal px, dReal py, dReal pz);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddRelTorque"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyAddRelTorque(IntPtr body, dReal fx, dReal fy, dReal fz);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddTorque"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyAddTorque(IntPtr body, dReal fx, dReal fy, dReal fz);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyPosition"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyCopyPosition(IntPtr body, out Vector3 pos);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyPosition"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyCopyPosition(IntPtr body, out dReal X);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyQuaternion"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyCopyQuaternion(IntPtr body, out Quaternion quat);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyQuaternion"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyCopyQuaternion(IntPtr body, out dReal X);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyRotation"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyCopyRotation(IntPtr body, out Matrix3 R);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyRotation"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyCopyRotation(IntPtr body, out dReal M00);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCreate"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr BodyiCreate(IntPtr world);
+        public static IntPtr BodyCreate(IntPtr world)
+        {
+            NTotalBodies++;
+            return BodyiCreate(world);
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyDestroy"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyiDestroy(IntPtr body);
+        public static void BodyDestroy(IntPtr body)
+        {
+            NTotalBodies--;
+            BodyiDestroy(body);
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyDisable"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyDisable(IntPtr body);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyEnable"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyEnable(IntPtr body);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableAngularThreshold"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal BodyGetAutoDisableAngularThreshold(IntPtr body);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableFlag"), SuppressUnmanagedCodeSecurity]
+        public static extern bool BodyGetAutoDisableFlag(IntPtr body);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableDefaults"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyGetAutoDisableDefaults(IntPtr body);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableLinearThreshold"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal BodyGetAutoDisableLinearThreshold(IntPtr body);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableSteps"), SuppressUnmanagedCodeSecurity]
+        public static extern int BodyGetAutoDisableSteps(IntPtr body);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableTime"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal BodyGetAutoDisableTime(IntPtr body);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAngularVel"), SuppressUnmanagedCodeSecurity]
+        public extern unsafe static Vector3* BodyGetAngularVelUnsafe(IntPtr body);
+        public static Vector3 BodyGetAngularVel(IntPtr body)
+        {
+            unsafe { return *(BodyGetAngularVelUnsafe(body)); }
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetData"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr BodyGetData(IntPtr body);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetFiniteRotationMode"), SuppressUnmanagedCodeSecurity]
+        public static extern int BodyGetFiniteRotationMode(IntPtr body);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetFiniteRotationAxis"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyGetFiniteRotationAxis(IntPtr body, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetForce"), SuppressUnmanagedCodeSecurity]
+        public extern unsafe static Vector3* BodyGetForceUnsafe(IntPtr body);
+        public static Vector3 BodyGetForce(IntPtr body)
+        {
+            unsafe { return *(BodyGetForceUnsafe(body)); }
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetGravityMode"), SuppressUnmanagedCodeSecurity]
+        public static extern bool BodyGetGravityMode(IntPtr body);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetGyroscopicMode"), SuppressUnmanagedCodeSecurity]
+        public static extern int BodyGetGyroscopicMode(IntPtr body);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetJoint"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr BodyGetJoint(IntPtr body, int index);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetLinearVel"), SuppressUnmanagedCodeSecurity]
+        public extern unsafe static Vector3* BodyGetLinearVelUnsafe(IntPtr body);
+        public static Vector3 BodyGetLinearVel(IntPtr body)
+        {
+            unsafe { return *(BodyGetLinearVelUnsafe(body)); }
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetMass"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyGetMass(IntPtr body, out Mass mass);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetNumJoints"), SuppressUnmanagedCodeSecurity]
+        public static extern int BodyGetNumJoints(IntPtr body);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetPointVel"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyGetPointVel(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetPosition"), SuppressUnmanagedCodeSecurity]
+        public extern unsafe static Vector3* BodyGetPositionUnsafe(IntPtr body);
+        public static Vector3 BodyGetPosition(IntPtr body)
+        {
+            unsafe { return *(BodyGetPositionUnsafe(body)); }
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetPosRelPoint"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyGetPosRelPoint(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetQuaternion"), SuppressUnmanagedCodeSecurity]
+        public extern unsafe static Quaternion* BodyGetQuaternionUnsafe(IntPtr body);
+        public static Quaternion BodyGetQuaternion(IntPtr body)
+        {
+            unsafe { return *(BodyGetQuaternionUnsafe(body)); }
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetRelPointPos"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyGetRelPointPos(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetRelPointVel"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyGetRelPointVel(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetRotation"), SuppressUnmanagedCodeSecurity]
+        public extern unsafe static Matrix3* BodyGetRotationUnsafe(IntPtr body);
+        public static Matrix3 BodyGetRotation(IntPtr body)
+        {
+            unsafe { return *(BodyGetRotationUnsafe(body)); }
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetTorque"), SuppressUnmanagedCodeSecurity]
+        public extern unsafe static Vector3* BodyGetTorqueUnsafe(IntPtr body);
+        public static Vector3 BodyGetTorque(IntPtr body)
+        {
+            unsafe { return *(BodyGetTorqueUnsafe(body)); }
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetWorld"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr BodyGetWorld(IntPtr body);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetFirstGeom"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr BodyGetFirstGeom(IntPtr body);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetNextGeom"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr dBodyGetNextGeom(IntPtr Geom);
+
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyIsEnabled"), SuppressUnmanagedCodeSecurity]
+        public static extern bool BodyIsEnabled(IntPtr body);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAngularVel"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetAngularVel(IntPtr body, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableAngularThreshold"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetAutoDisableAngularThreshold(IntPtr body, dReal angular_threshold);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableDefaults"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetAutoDisableDefaults(IntPtr body);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableFlag"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetAutoDisableFlag(IntPtr body, bool do_auto_disable);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableLinearThreshold"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetAutoDisableLinearThreshold(IntPtr body, dReal linear_threshold);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableSteps"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetAutoDisableSteps(IntPtr body, int steps);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableTime"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetAutoDisableTime(IntPtr body, dReal time);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetData"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetData(IntPtr body, IntPtr data);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetFiniteRotationMode"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetFiniteRotationMode(IntPtr body, int mode);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetFiniteRotationAxis"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetFiniteRotationAxis(IntPtr body, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetLinearDamping"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetLinearDamping(IntPtr body, dReal scale);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAngularDamping"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetAngularDamping(IntPtr body, dReal scale);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetLinearDamping"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal BodyGetLinearDamping(IntPtr body);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAngularDamping"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal BodyGetAngularDamping(IntPtr body);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAngularDamping"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetDamping(IntPtr body, dReal linear_scale, dReal angular_scale);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAngularDampingThreshold"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetAngularDampingThreshold(IntPtr body, dReal threshold);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetLinearDampingThreshold"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetLinearDampingThreshold(IntPtr body, dReal threshold);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetLinearDampingThreshold"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal BodyGetLinearDampingThreshold(IntPtr body);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAngularDampingThreshold"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal BodyGetAngularDampingThreshold(IntPtr body);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetForce"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetForce(IntPtr body, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetGravityMode"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetGravityMode(IntPtr body, bool mode);
+
+        /// <summary>
+        /// Sets the Gyroscopic term status on the body specified.
+        /// </summary>
+        /// <param name="body">Pointer to body</param>
+        /// <param name="enabled">NonZero enabled, Zero disabled</param>
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetGyroscopicMode"), SuppressUnmanagedCodeSecurity]
+        public static extern void dBodySetGyroscopicMode(IntPtr body, int enabled);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetLinearVel"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetLinearVel(IntPtr body, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetMass"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetMass(IntPtr body, ref Mass mass);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetPosition"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetPosition(IntPtr body, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetQuaternion"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetQuaternion(IntPtr body, ref Quaternion q);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetQuaternion"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetQuaternion(IntPtr body, ref dReal w);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetRotation"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetRotation(IntPtr body, ref Matrix3 R);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetRotation"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetRotation(IntPtr body, ref dReal M00);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetTorque"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetTorque(IntPtr body, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyVectorFromWorld"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyVectorFromWorld(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyVectorToWorld"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyVectorToWorld(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBoxBox"), SuppressUnmanagedCodeSecurity]
+        public static extern void BoxBox(ref Vector3 p1, ref Matrix3 R1,
+            ref Vector3 side1, ref Vector3 p2,
+            ref Matrix3 R2, ref Vector3 side2,
+            ref Vector3 normal, out dReal depth, out int return_code,
+            int maxc, out ContactGeom contact, int skip);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBoxTouchesBox"), SuppressUnmanagedCodeSecurity]
+        public static extern void BoxTouchesBox(ref Vector3 _p1, ref Matrix3 R1,
+            ref Vector3 side1, ref Vector3 _p2,
+            ref Matrix3 R2, ref Vector3 side2);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCleanupODEAllDataForThread"), SuppressUnmanagedCodeSecurity]
+        public static extern void CleanupODEAllDataForThread();
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dClosestLineSegmentPoints"), SuppressUnmanagedCodeSecurity]
+        public static extern void ClosestLineSegmentPoints(ref Vector3 a1, ref Vector3 a2,
+            ref Vector3 b1, ref Vector3 b2,
+            ref Vector3 cp1, ref Vector3 cp2);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCloseODE"), SuppressUnmanagedCodeSecurity]
+        public static extern void CloseODE();
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCollide"), SuppressUnmanagedCodeSecurity]
+        public static extern int Collide(IntPtr o1, IntPtr o2, int flags, [In, Out] ContactGeom[] contact, int skip);
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCollide"), SuppressUnmanagedCodeSecurity]
+        public static extern int CollidePtr(IntPtr o1, IntPtr o2, int flags, IntPtr contactgeomarray, int skip);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dConnectingJoint"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr ConnectingJoint(IntPtr j1, IntPtr j2);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateBox"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr CreateiBox(IntPtr space, dReal lx, dReal ly, dReal lz);
+        public static IntPtr CreateBox(IntPtr space, dReal lx, dReal ly, dReal lz)
+        {
+            NTotalGeoms++;
+            return CreateiBox(space, lx, ly, lz);
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateCapsule"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr CreateiCapsule(IntPtr space, dReal radius, dReal length);
+        public static IntPtr CreateCapsule(IntPtr space, dReal radius, dReal length)
+        {
+            NTotalGeoms++;
+            return CreateiCapsule(space, radius, length);
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateConvex"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr CreateiConvex(IntPtr space, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons);
+        public static IntPtr CreateConvex(IntPtr space, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons)
+        {
+            NTotalGeoms++;
+            return CreateiConvex(space, planes, planeCount, points, pointCount, polygons);
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateCylinder"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr CreateiCylinder(IntPtr space, dReal radius, dReal length);
+        public static IntPtr CreateCylinder(IntPtr space, dReal radius, dReal length)
+        {
+            NTotalGeoms++;
+            return CreateiCylinder(space, radius, length);
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateHeightfield"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr CreateiHeightfield(IntPtr space, IntPtr data, int bPlaceable);
+        public static IntPtr CreateHeightfield(IntPtr space, IntPtr data, int bPlaceable)
+        {
+            NTotalGeoms++;
+            return CreateiHeightfield(space, data, bPlaceable);
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateOSTerrain"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr CreateiOSTerrain(IntPtr space, IntPtr data, int bPlaceable);
+        public static IntPtr CreateOSTerrain(IntPtr space, IntPtr data, int bPlaceable)
+        {
+            NTotalGeoms++;
+            return CreateiOSTerrain(space, data, bPlaceable);
+        }
+
+
+
+
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateGeom"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr CreateiGeom(int classnum);
+        public static IntPtr CreateGeom(int classnum)
+        {
+            NTotalGeoms++;
+            return CreateiGeom(classnum);
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateGeomClass"), SuppressUnmanagedCodeSecurity]
+        public static extern int CreateGeomClass(ref GeomClass classptr);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateGeomTransform"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr CreateGeomTransform(IntPtr space);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreatePlane"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr CreateiPlane(IntPtr space, dReal a, dReal b, dReal c, dReal d);
+        public static IntPtr CreatePlane(IntPtr space, dReal a, dReal b, dReal c, dReal d)
+        {
+            NTotalGeoms++;
+            return CreateiPlane(space, a, b, c, d);
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateRay"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr CreateiRay(IntPtr space, dReal length);
+        public static IntPtr CreateRay(IntPtr space, dReal length)
+        {
+            NTotalGeoms++;
+            return CreateiRay(space, length);
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateSphere"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr CreateiSphere(IntPtr space, dReal radius);
+        public static IntPtr CreateSphere(IntPtr space, dReal radius)
+        {
+            NTotalGeoms++;
+            return CreateiSphere(space, radius);
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateTriMesh"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr CreateiTriMesh(IntPtr space, IntPtr data,
+            TriCallback callback, TriArrayCallback arrayCallback, TriRayCallback rayCallback);
+        public static IntPtr CreateTriMesh(IntPtr space, IntPtr data,
+            TriCallback callback, TriArrayCallback arrayCallback, TriRayCallback rayCallback)
+        {
+            NTotalGeoms++;
+            return CreateiTriMesh(space, data, callback, arrayCallback, rayCallback);
+        }
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dDot"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal Dot(ref dReal X0, ref dReal X1, int n);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dDQfromW"), SuppressUnmanagedCodeSecurity]
+        public static extern void DQfromW(dReal[] dq, ref Vector3 w, ref Quaternion q);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dFactorCholesky"), SuppressUnmanagedCodeSecurity]
+        public static extern int FactorCholesky(ref dReal A00, int n);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dFactorLDLT"), SuppressUnmanagedCodeSecurity]
+        public static extern void FactorLDLT(ref dReal A, out dReal d, int n, int nskip);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomBoxGetLengths"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomBoxGetLengths(IntPtr geom, out Vector3 len);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomBoxGetLengths"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomBoxGetLengths(IntPtr geom, out dReal x);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomBoxPointDepth"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal GeomBoxPointDepth(IntPtr geom, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomBoxSetLengths"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomBoxSetLengths(IntPtr geom, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCapsuleGetParams"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomCapsuleGetParams(IntPtr geom, out dReal radius, out dReal length);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCapsulePointDepth"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal GeomCapsulePointDepth(IntPtr geom, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCapsuleSetParams"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomCapsuleSetParams(IntPtr geom, dReal radius, dReal length);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomClearOffset"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomClearOffset(IntPtr geom);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyOffsetPosition"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr GeomCopyOffsetPosition(IntPtr geom, ref Vector3 pos);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyOffsetPosition"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr GeomCopyOffsetPosition(IntPtr geom, ref dReal X);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetOffsetQuaternion"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomCopyOffsetQuaternion(IntPtr geom, ref Quaternion Q);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetOffsetQuaternion"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomCopyOffsetQuaternion(IntPtr geom, ref dReal X);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyOffsetRotation"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr GeomCopyOffsetRotation(IntPtr geom, ref Matrix3 R);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyOffsetRotation"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr GeomCopyOffsetRotation(IntPtr geom, ref dReal M00);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyPosition"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomCopyPosition(IntPtr geom, out Vector3 pos);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyPosition"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomCopyPosition(IntPtr geom, out dReal X);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyRotation"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomCopyRotation(IntPtr geom, out Matrix3 R);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyRotation"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomCopyRotation(IntPtr geom, out dReal M00);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCylinderGetParams"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomCylinderGetParams(IntPtr geom, out dReal radius, out dReal length);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCylinderSetParams"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomCylinderSetParams(IntPtr geom, dReal radius, dReal length);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomDestroy"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomiDestroy(IntPtr geom);
+        public static void GeomDestroy(IntPtr geom)
+        {
+            NTotalGeoms--;
+            GeomiDestroy(geom);
+        }
+
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomDisable"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomDisable(IntPtr geom);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomEnable"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomEnable(IntPtr geom);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetAABB"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomGetAABB(IntPtr geom, out AABB aabb);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetAABB"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomGetAABB(IntPtr geom, out dReal minX);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetBody"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr GeomGetBody(IntPtr geom);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetCategoryBits"), SuppressUnmanagedCodeSecurity]
+        public static extern uint GeomGetCategoryBits(IntPtr geom);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetClassData"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr GeomGetClassData(IntPtr geom);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetCollideBits"), SuppressUnmanagedCodeSecurity]
+        public static extern uint GeomGetCollideBits(IntPtr geom);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetClass"), SuppressUnmanagedCodeSecurity]
+        public static extern GeomClassID GeomGetClass(IntPtr geom);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetData"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr GeomGetData(IntPtr geom);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetOffsetPosition"), SuppressUnmanagedCodeSecurity]
+        public extern unsafe static Vector3* GeomGetOffsetPositionUnsafe(IntPtr geom);
+        public static Vector3 GeomGetOffsetPosition(IntPtr geom)
+        {
+            unsafe { return *(GeomGetOffsetPositionUnsafe(geom)); }
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetOffsetRotation"), SuppressUnmanagedCodeSecurity]
+        public extern unsafe static Matrix3* GeomGetOffsetRotationUnsafe(IntPtr geom);
+        public static Matrix3 GeomGetOffsetRotation(IntPtr geom)
+        {
+            unsafe { return *(GeomGetOffsetRotationUnsafe(geom)); }
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetPosition"), SuppressUnmanagedCodeSecurity]
+        public extern unsafe static Vector3* GeomGetPositionUnsafe(IntPtr geom);
+        public static Vector3 GeomGetPosition(IntPtr geom)
+        {
+            unsafe { return *(GeomGetPositionUnsafe(geom)); }
+        }
+        public static OMV.Vector3 GeomGetPositionOMV(IntPtr geom)
+        {
+            Vector3 vtmp = GeomGetPosition(geom);
+            return new OMV.Vector3(vtmp.X, vtmp.Y, vtmp.Z);
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetQuaternion"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomCopyQuaternion(IntPtr geom, out Quaternion q);
+        public static OMV.Quaternion GeomGetQuaternionOMV(IntPtr geom)
+        {
+            Quaternion qtmp;
+            GeomCopyQuaternion(geom, out qtmp);
+            return new OMV.Quaternion(qtmp.X, qtmp.Y, qtmp.Z, qtmp.W);
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetQuaternion"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomCopyQuaternion(IntPtr geom, out dReal X);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetRotation"), SuppressUnmanagedCodeSecurity]
+        public extern unsafe static Matrix3* GeomGetRotationUnsafe(IntPtr geom);
+        public static Matrix3 GeomGetRotation(IntPtr geom)
+        {
+            unsafe { return *(GeomGetRotationUnsafe(geom)); }
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetSpace"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr GeomGetSpace(IntPtr geom);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildByte"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomHeightfieldDataBuildByte(IntPtr d, byte[] pHeightData, int bCopyHeightData,
+                dReal width, dReal depth, int widthSamples, int depthSamples,
+                dReal scale, dReal offset, dReal thickness, int bWrap);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildByte"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomHeightfieldDataBuildByte(IntPtr d, IntPtr pHeightData, int bCopyHeightData,
+                dReal width, dReal depth, int widthSamples, int depthSamples,
+                dReal scale, dReal offset, dReal thickness,     int bWrap);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildCallback"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomHeightfieldDataBuildCallback(IntPtr d, IntPtr pUserData, HeightfieldGetHeight pCallback,
+                dReal width, dReal depth, int widthSamples, int depthSamples,
+                dReal scale, dReal offset, dReal thickness, int bWrap);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildShort"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomHeightfieldDataBuildShort(IntPtr d, ushort[] pHeightData, int bCopyHeightData,
+                dReal width, dReal depth, int widthSamples, int depthSamples,
+                dReal scale, dReal offset, dReal thickness, int bWrap);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildShort"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomHeightfieldDataBuildShort(IntPtr d, short[] pHeightData, int bCopyHeightData,
+                dReal width, dReal depth, int widthSamples, int depthSamples,
+                dReal scale, dReal offset, dReal thickness, int bWrap);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildShort"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomHeightfieldDataBuildShort(IntPtr d, IntPtr pHeightData, int bCopyHeightData,
+                dReal width, dReal depth, int widthSamples, int depthSamples,
+                dReal scale, dReal offset, dReal thickness, int bWrap);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildSingle"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomHeightfieldDataBuildSingle(IntPtr d, float[] pHeightData, int bCopyHeightData,
+                dReal width, dReal depth, int widthSamples, int depthSamples,
+                dReal scale, dReal offset, dReal thickness, int bWrap);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildSingle"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomHeightfieldDataBuildSingle(IntPtr d, IntPtr pHeightData, int bCopyHeightData,
+                dReal width, dReal depth, int widthSamples, int depthSamples,
+                dReal scale, dReal offset, dReal thickness, int bWrap);
+
+
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildDouble"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomHeightfieldDataBuildDouble(IntPtr d, double[] pHeightData, int bCopyHeightData,
+                dReal width, dReal depth, int widthSamples, int depthSamples,
+                dReal scale, dReal offset, dReal thickness, int bWrap);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildDouble"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomHeightfieldDataBuildDouble(IntPtr d, IntPtr pHeightData, int bCopyHeightData,
+                dReal width, dReal depth, int widthSamples, int depthSamples,
+                dReal scale, dReal offset, dReal thickness, int bWrap);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataCreate"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr GeomHeightfieldDataCreate();
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataDestroy"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomHeightfieldDataDestroy(IntPtr d);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataSetBounds"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomHeightfieldDataSetBounds(IntPtr d, dReal minHeight, dReal maxHeight);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldGetHeightfieldData"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr GeomHeightfieldGetHeightfieldData(IntPtr g);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldSetHeightfieldData"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomHeightfieldSetHeightfieldData(IntPtr g, IntPtr d);
+
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomUbitTerrainDataBuild"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomOSTerrainDataBuild(IntPtr d, float[] pHeightData, int bCopyHeightData,
+                dReal sampleSize, int widthSamples, int depthSamples,
+                dReal offset, dReal thickness, int bWrap);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomOSTerrainDataBuild"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomOSTerrainDataBuild(IntPtr d, IntPtr pHeightData, int bCopyHeightData,
+                dReal sampleSize, int widthSamples, int depthSamples,
+                dReal thickness, int bWrap);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomOSTerrainDataCreate"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr GeomOSTerrainDataCreate();
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomOSTerrainDataDestroy"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomOSTerrainDataDestroy(IntPtr d);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomOSTerrainDataSetBounds"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomOSTerrainDataSetBounds(IntPtr d, dReal minHeight, dReal maxHeight);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomOSTerrainGetHeightfieldData"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr GeomOSTerrainGetHeightfieldData(IntPtr g);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomOSTerrainSetHeightfieldData"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomOSTerrainSetHeightfieldData(IntPtr g, IntPtr d);
+
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomIsEnabled"), SuppressUnmanagedCodeSecurity]
+        public static extern bool GeomIsEnabled(IntPtr geom);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomIsOffset"), SuppressUnmanagedCodeSecurity]
+        public static extern bool GeomIsOffset(IntPtr geom);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomIsSpace"), SuppressUnmanagedCodeSecurity]
+        public static extern bool GeomIsSpace(IntPtr geom);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomPlaneGetParams"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomPlaneGetParams(IntPtr geom, ref Vector4 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomPlaneGetParams"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomPlaneGetParams(IntPtr geom, ref dReal A);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomPlanePointDepth"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal GeomPlanePointDepth(IntPtr geom, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomPlaneSetParams"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomPlaneSetParams(IntPtr plane, dReal a, dReal b, dReal c, dReal d);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGet"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomRayGet(IntPtr ray, ref Vector3 start, ref Vector3 dir);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGet"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomRayGet(IntPtr ray, ref dReal startX, ref dReal dirX);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGetClosestHit"), SuppressUnmanagedCodeSecurity]
+        public static extern int GeomRayGetClosestHit(IntPtr ray);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGetLength"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal GeomRayGetLength(IntPtr ray);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGetParams"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal GeomRayGetParams(IntPtr g, out int firstContact, out int backfaceCull);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRaySet"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomRaySet(IntPtr ray, dReal px, dReal py, dReal pz, dReal dx, dReal dy, dReal dz);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRaySetClosestHit"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomRaySetClosestHit(IntPtr ray, int closestHit);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRaySetLength"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomRaySetLength(IntPtr ray, dReal length);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRaySetParams"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomRaySetParams(IntPtr ray, int firstContact, int backfaceCull);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetBody"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomSetBody(IntPtr geom, IntPtr body);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetCategoryBits"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomSetCategoryBits(IntPtr geom, uint bits);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetCollideBits"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomSetCollideBits(IntPtr geom, uint bits);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetConvex"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr GeomSetConvex(IntPtr geom, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetData"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomSetData(IntPtr geom, IntPtr data);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetPosition"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomSetOffsetPosition(IntPtr geom, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetQuaternion"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomSetOffsetQuaternion(IntPtr geom, ref Quaternion Q);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetQuaternion"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomSetOffsetQuaternion(IntPtr geom, ref dReal X);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetRotation"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomSetOffsetRotation(IntPtr geom, ref Matrix3 R);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetRotation"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomSetOffsetRotation(IntPtr geom, ref dReal M00);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldPosition"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomSetOffsetWorldPosition(IntPtr geom, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldQuaternion"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomSetOffsetWorldQuaternion(IntPtr geom, ref Quaternion Q);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldQuaternion"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomSetOffsetWorldQuaternion(IntPtr geom, ref dReal X);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldRotation"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomSetOffsetWorldRotation(IntPtr geom, ref Matrix3 R);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldRotation"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomSetOffsetWorldRotation(IntPtr geom, ref dReal M00);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetPosition"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomSetPosition(IntPtr geom, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetQuaternion"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomSetQuaternion(IntPtr geom, ref Quaternion quat);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetQuaternion"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomSetQuaternion(IntPtr geom, ref dReal w);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetRotation"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomSetRotation(IntPtr geom, ref Matrix3 R);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetRotation"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomSetRotation(IntPtr geom, ref dReal M00);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSphereGetRadius"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal GeomSphereGetRadius(IntPtr geom);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSpherePointDepth"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal GeomSpherePointDepth(IntPtr geom, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSphereSetRadius"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomSphereSetRadius(IntPtr geom, dReal radius);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformGetCleanup"), SuppressUnmanagedCodeSecurity]
+        public static extern int GeomTransformGetCleanup(IntPtr geom);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformGetGeom"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr GeomTransformGetGeom(IntPtr geom);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformGetInfo"), SuppressUnmanagedCodeSecurity]
+        public static extern int GeomTransformGetInfo(IntPtr geom);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformSetCleanup"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTransformSetCleanup(IntPtr geom, int mode);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformSetGeom"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTransformSetGeom(IntPtr geom, IntPtr obj);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformSetInfo"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTransformSetInfo(IntPtr geom, int info);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildDouble"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTriMeshDataBuildDouble(IntPtr d,
+            double[] vertices, int vertexStride, int vertexCount,
+            int[] indices, int indexCount, int triStride);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildDouble"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTriMeshDataBuildDouble(IntPtr d,
+            IntPtr vertices, int vertexStride, int vertexCount,
+            IntPtr indices, int indexCount, int triStride);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildDouble1"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTriMeshDataBuildDouble1(IntPtr d,
+            double[] vertices, int vertexStride, int vertexCount,
+            int[] indices, int indexCount, int triStride,
+            double[] normals);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildDouble1"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTriMeshDataBuildDouble(IntPtr d,
+            IntPtr vertices, int vertexStride, int vertexCount,
+            IntPtr indices, int indexCount, int triStride,
+            IntPtr normals);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSimple"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTriMeshDataBuildSingle(IntPtr d,
+            dReal[] vertices, int vertexStride, int vertexCount,
+            int[] indices, int indexCount, int triStride);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSimple"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTriMeshDataBuildSingle(IntPtr d,
+            IntPtr vertices, int vertexStride, int vertexCount,
+            IntPtr indices, int indexCount, int triStride);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSimple1"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTriMeshDataBuildSingle1(IntPtr d,
+            dReal[] vertices, int vertexStride, int vertexCount,
+            int[] indices, int indexCount, int triStride,
+            dReal[] normals);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSimple1"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTriMeshDataBuildSingle1(IntPtr d,
+            IntPtr vertices, int vertexStride, int vertexCount,
+            IntPtr indices, int indexCount, int triStride,
+            IntPtr normals);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSingle"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTriMeshDataBuildSimple(IntPtr d,
+            float[] vertices, int vertexStride, int vertexCount,
+            int[] indices, int indexCount, int triStride);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSingle"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTriMeshDataBuildSimple(IntPtr d,
+            IntPtr vertices, int vertexStride, int vertexCount,
+            IntPtr indices, int indexCount, int triStride);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSingle1"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTriMeshDataBuildSimple1(IntPtr d,
+            float[] vertices, int vertexStride, int vertexCount,
+            int[] indices, int indexCount, int triStride,
+            float[] normals);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSingle1"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTriMeshDataBuildSimple1(IntPtr d,
+            IntPtr vertices, int vertexStride, int vertexCount,
+            IntPtr indices, int indexCount, int triStride,
+            IntPtr normals);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshClearTCCache"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTriMeshClearTCCache(IntPtr g);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataCreate"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr GeomTriMeshDataCreate();
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataDestroy"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTriMeshDataDestroy(IntPtr d);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataGet"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr GeomTriMeshDataGet(IntPtr d, int data_id);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataPreprocess"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTriMeshDataPreprocess(IntPtr d);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataSet"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTriMeshDataSet(IntPtr d, int data_id, IntPtr in_data);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataUpdate"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTriMeshDataUpdate(IntPtr d);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshEnableTC"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTriMeshEnableTC(IntPtr g, int geomClass, bool enable);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetArrayCallback"), SuppressUnmanagedCodeSecurity]
+        public static extern TriArrayCallback GeomTriMeshGetArrayCallback(IntPtr g);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetCallback"), SuppressUnmanagedCodeSecurity]
+        public static extern TriCallback GeomTriMeshGetCallback(IntPtr g);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetData"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr GeomTriMeshGetData(IntPtr g);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetLastTransform"), SuppressUnmanagedCodeSecurity]
+        public extern unsafe static Matrix4* GeomTriMeshGetLastTransformUnsafe(IntPtr geom);
+        public static Matrix4 GeomTriMeshGetLastTransform(IntPtr geom)
+        {
+            unsafe { return *(GeomTriMeshGetLastTransformUnsafe(geom)); }
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetPoint"), SuppressUnmanagedCodeSecurity]
+        public extern static void GeomTriMeshGetPoint(IntPtr g, int index, dReal u, dReal v, ref Vector3 outVec);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetRayCallback"), SuppressUnmanagedCodeSecurity]
+        public static extern TriRayCallback GeomTriMeshGetRayCallback(IntPtr g);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetTriangle"), SuppressUnmanagedCodeSecurity]
+        public extern static void GeomTriMeshGetTriangle(IntPtr g, int index, ref Vector3 v0, ref Vector3 v1, ref Vector3 v2);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetTriangleCount"), SuppressUnmanagedCodeSecurity]
+        public extern static int GeomTriMeshGetTriangleCount(IntPtr g);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetTriMeshDataID"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr GeomTriMeshGetTriMeshDataID(IntPtr g);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshIsTCEnabled"), SuppressUnmanagedCodeSecurity]
+        public static extern bool GeomTriMeshIsTCEnabled(IntPtr g, int geomClass);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetArrayCallback"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTriMeshSetArrayCallback(IntPtr g, TriArrayCallback arrayCallback);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetCallback"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTriMeshSetCallback(IntPtr g, TriCallback callback);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetData"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTriMeshSetData(IntPtr g, IntPtr data);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetLastTransform"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTriMeshSetLastTransform(IntPtr g, ref Matrix4 last_trans);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetLastTransform"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTriMeshSetLastTransform(IntPtr g, ref dReal M00);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetRayCallback"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTriMeshSetRayCallback(IntPtr g, TriRayCallback callback);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGetConfiguration"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr iGetConfiguration();
+
+        public static string GetConfiguration()
+        {
+            IntPtr ptr = iGetConfiguration();
+            string s = Marshal.PtrToStringAnsi(ptr);
+            return s;
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dHashSpaceCreate"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr HashSpaceCreate(IntPtr space);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dHashSpaceGetLevels"), SuppressUnmanagedCodeSecurity]
+        public static extern void HashSpaceGetLevels(IntPtr space, out int minlevel, out int maxlevel);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dHashSpaceSetLevels"), SuppressUnmanagedCodeSecurity]
+        public static extern void HashSpaceSetLevels(IntPtr space, int minlevel, int maxlevel);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dInfiniteAABB"), SuppressUnmanagedCodeSecurity]
+        public static extern void InfiniteAABB(IntPtr geom, out AABB aabb);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dInitODE"), SuppressUnmanagedCodeSecurity]
+        public static extern void InitODE();
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dInitODE2"), SuppressUnmanagedCodeSecurity]
+        public static extern int InitODE2(uint ODEInitFlags);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dIsPositiveDefinite"), SuppressUnmanagedCodeSecurity]
+        public static extern int IsPositiveDefinite(ref dReal A, int n);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dInvertPDMatrix"), SuppressUnmanagedCodeSecurity]
+        public static extern int InvertPDMatrix(ref dReal A, out dReal Ainv, int n);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddAMotorTorques"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointAddAMotorTorques(IntPtr joint, dReal torque1, dReal torque2, dReal torque3);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddHingeTorque"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointAddHingeTorque(IntPtr joint, dReal torque);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddHinge2Torque"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointAddHinge2Torques(IntPtr joint, dReal torque1, dReal torque2);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddPRTorque"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointAddPRTorque(IntPtr joint, dReal torque);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddUniversalTorque"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointAddUniversalTorques(IntPtr joint, dReal torque1, dReal torque2);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddSliderForce"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointAddSliderForce(IntPtr joint, dReal force);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAttach"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointAttach(IntPtr joint, IntPtr body1, IntPtr body2);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateAMotor"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr JointCreateAMotor(IntPtr world, IntPtr group);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateBall"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr JointCreateBall(IntPtr world, IntPtr group);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateContact"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr JointCreateContact(IntPtr world, IntPtr group, ref Contact contact);
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateContact"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr JointCreateContactPtr(IntPtr world, IntPtr group, IntPtr contact);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateFixed"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr JointCreateFixed(IntPtr world, IntPtr group);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateHinge"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr JointCreateHinge(IntPtr world, IntPtr group);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateHinge2"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr JointCreateHinge2(IntPtr world, IntPtr group);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateLMotor"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr JointCreateLMotor(IntPtr world, IntPtr group);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateNull"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr JointCreateNull(IntPtr world, IntPtr group);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreatePR"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr JointCreatePR(IntPtr world, IntPtr group);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreatePlane2D"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr JointCreatePlane2D(IntPtr world, IntPtr group);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateSlider"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr JointCreateSlider(IntPtr world, IntPtr group);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateUniversal"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr JointCreateUniversal(IntPtr world, IntPtr group);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointDestroy"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointDestroy(IntPtr j);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorAngle"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal JointGetAMotorAngle(IntPtr j, int anum);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorAngleRate"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal JointGetAMotorAngleRate(IntPtr j, int anum);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorAxis"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointGetAMotorAxis(IntPtr j, int anum, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorAxisRel"), SuppressUnmanagedCodeSecurity]
+        public static extern int JointGetAMotorAxisRel(IntPtr j, int anum);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorMode"), SuppressUnmanagedCodeSecurity]
+        public static extern int JointGetAMotorMode(IntPtr j);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorNumAxes"), SuppressUnmanagedCodeSecurity]
+        public static extern int JointGetAMotorNumAxes(IntPtr j);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorParam"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal JointGetAMotorParam(IntPtr j, int parameter);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetBallAnchor"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointGetBallAnchor(IntPtr j, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetBallAnchor2"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointGetBallAnchor2(IntPtr j, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetBody"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr JointGetBody(IntPtr j);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetData"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr JointGetData(IntPtr j);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetFeedback"), SuppressUnmanagedCodeSecurity]
+        public extern unsafe static JointFeedback* JointGetFeedbackUnsafe(IntPtr j);
+        public static JointFeedback JointGetFeedback(IntPtr j)
+        {
+            unsafe { return *(JointGetFeedbackUnsafe(j)); }
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAnchor"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointGetHingeAnchor(IntPtr j, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAngle"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal JointGetHingeAngle(IntPtr j);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAngleRate"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal JointGetHingeAngleRate(IntPtr j);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAxis"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointGetHingeAxis(IntPtr j, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeParam"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal JointGetHingeParam(IntPtr j, int parameter);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Angle1"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal JointGetHinge2Angle1(IntPtr j);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Angle1Rate"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal JointGetHinge2Angle1Rate(IntPtr j);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Angle2Rate"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal JointGetHinge2Angle2Rate(IntPtr j);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAnchor2"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointGetHingeAnchor2(IntPtr j, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Anchor"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointGetHinge2Anchor(IntPtr j, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Anchor2"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointGetHinge2Anchor2(IntPtr j, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Axis1"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointGetHinge2Axis1(IntPtr j, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Axis2"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointGetHinge2Axis2(IntPtr j, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Param"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal JointGetHinge2Param(IntPtr j, int parameter);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetLMotorAxis"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointGetLMotorAxis(IntPtr j, int anum, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetLMotorNumAxes"), SuppressUnmanagedCodeSecurity]
+        public static extern int JointGetLMotorNumAxes(IntPtr j);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetLMotorParam"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal JointGetLMotorParam(IntPtr j, int parameter);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRAnchor"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointGetPRAnchor(IntPtr j, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRAxis1"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointGetPRAxis1(IntPtr j, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRAxis2"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointGetPRAxis2(IntPtr j, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRParam"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal JointGetPRParam(IntPtr j, int parameter);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRPosition"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal JointGetPRPosition(IntPtr j);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRPositionRate"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal JointGetPRPositionRate(IntPtr j);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetSliderAxis"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointGetSliderAxis(IntPtr j, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetSliderParam"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal JointGetSliderParam(IntPtr j, int parameter);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetSliderPosition"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal JointGetSliderPosition(IntPtr j);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetSliderPositionRate"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal JointGetSliderPositionRate(IntPtr j);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetType"), SuppressUnmanagedCodeSecurity]
+        public static extern JointType JointGetType(IntPtr j);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAnchor"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointGetUniversalAnchor(IntPtr j, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAnchor2"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointGetUniversalAnchor2(IntPtr j, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngle1"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal JointGetUniversalAngle1(IntPtr j);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngle1Rate"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal JointGetUniversalAngle1Rate(IntPtr j);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngle2"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal JointGetUniversalAngle2(IntPtr j);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngle2Rate"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal JointGetUniversalAngle2Rate(IntPtr j);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngles"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointGetUniversalAngles(IntPtr j, out dReal angle1, out dReal angle2);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAxis1"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointGetUniversalAxis1(IntPtr j, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAxis2"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointGetUniversalAxis2(IntPtr j, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalParam"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal JointGetUniversalParam(IntPtr j, int parameter);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGroupCreate"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr JointGroupCreate(int max_size);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGroupDestroy"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointGroupDestroy(IntPtr group);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGroupEmpty"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointGroupEmpty(IntPtr group);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorAngle"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetAMotorAngle(IntPtr j, int anum, dReal angle);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorAxis"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetAMotorAxis(IntPtr j, int anum, int rel, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorMode"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetAMotorMode(IntPtr j, int mode);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorNumAxes"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetAMotorNumAxes(IntPtr group, int num);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorParam"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetAMotorParam(IntPtr group, int parameter, dReal value);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetBallAnchor"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetBallAnchor(IntPtr j, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetBallAnchor2"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetBallAnchor2(IntPtr j, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetData"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetData(IntPtr j, IntPtr data);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetFeedback"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetFeedback(IntPtr j, out JointFeedback feedback);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetFixed"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetFixed(IntPtr j);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHingeAnchor"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetHingeAnchor(IntPtr j, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHingeAnchorDelta"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetHingeAnchorDelta(IntPtr j, dReal x, dReal y, dReal z, dReal ax, dReal ay, dReal az);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHingeAxis"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetHingeAxis(IntPtr j, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHingeParam"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetHingeParam(IntPtr j, int parameter, dReal value);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHinge2Anchor"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetHinge2Anchor(IntPtr j, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHinge2Axis1"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetHinge2Axis1(IntPtr j, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHinge2Axis2"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetHinge2Axis2(IntPtr j, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHinge2Param"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetHinge2Param(IntPtr j, int parameter, dReal value);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetLMotorAxis"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetLMotorAxis(IntPtr j, int anum, int rel, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetLMotorNumAxes"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetLMotorNumAxes(IntPtr j, int num);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetLMotorParam"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetLMotorParam(IntPtr j, int parameter, dReal value);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPlane2DAngleParam"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetPlane2DAngleParam(IntPtr j, int parameter, dReal value);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPlane2DXParam"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetPlane2DXParam(IntPtr j, int parameter, dReal value);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPlane2DYParam"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetPlane2DYParam(IntPtr j, int parameter, dReal value);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPRAnchor"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetPRAnchor(IntPtr j, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPRAxis1"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetPRAxis1(IntPtr j, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPRAxis2"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetPRAxis2(IntPtr j, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPRParam"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetPRParam(IntPtr j, int parameter, dReal value);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetSliderAxis"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetSliderAxis(IntPtr j, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetSliderAxisDelta"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetSliderAxisDelta(IntPtr j, dReal x, dReal y, dReal z, dReal ax, dReal ay, dReal az);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetSliderParam"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetSliderParam(IntPtr j, int parameter, dReal value);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetUniversalAnchor"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetUniversalAnchor(IntPtr j, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetUniversalAxis1"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetUniversalAxis1(IntPtr j, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetUniversalAxis2"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetUniversalAxis2(IntPtr j, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetUniversalParam"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetUniversalParam(IntPtr j, int parameter, dReal value);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dLDLTAddTL"), SuppressUnmanagedCodeSecurity]
+        public static extern void LDLTAddTL(ref dReal L, ref dReal d, ref dReal a, int n, int nskip);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassAdd"), SuppressUnmanagedCodeSecurity]
+        public static extern void MassAdd(ref Mass a, ref Mass b);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassAdjust"), SuppressUnmanagedCodeSecurity]
+        public static extern void MassAdjust(ref Mass m, dReal newmass);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassCheck"), SuppressUnmanagedCodeSecurity]
+        public static extern bool MassCheck(ref Mass m);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassRotate"), SuppressUnmanagedCodeSecurity]
+        public static extern void MassRotate(ref Mass mass, ref Matrix3 R);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassRotate"), SuppressUnmanagedCodeSecurity]
+        public static extern void MassRotate(ref Mass mass, ref dReal M00);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetBox"), SuppressUnmanagedCodeSecurity]
+        public static extern void MassSetBox(out Mass mass, dReal density, dReal lx, dReal ly, dReal lz);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetBoxTotal"), SuppressUnmanagedCodeSecurity]
+        public static extern void MassSetBoxTotal(out Mass mass, dReal total_mass, dReal lx, dReal ly, dReal lz);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetCapsule"), SuppressUnmanagedCodeSecurity]
+        public static extern void MassSetCapsule(out Mass mass, dReal density, int direction, dReal radius, dReal length);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetCapsuleTotal"), SuppressUnmanagedCodeSecurity]
+        public static extern void MassSetCapsuleTotal(out Mass mass, dReal total_mass, int direction, dReal radius, dReal length);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetCylinder"), SuppressUnmanagedCodeSecurity]
+        public static extern void MassSetCylinder(out Mass mass, dReal density, int direction, dReal radius, dReal length);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetCylinderTotal"), SuppressUnmanagedCodeSecurity]
+        public static extern void MassSetCylinderTotal(out Mass mass, dReal total_mass, int direction, dReal radius, dReal length);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetParameters"), SuppressUnmanagedCodeSecurity]
+        public static extern void MassSetParameters(out Mass mass, dReal themass,
+             dReal cgx, dReal cgy, dReal cgz,
+             dReal i11, dReal i22, dReal i33,
+             dReal i12, dReal i13, dReal i23);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetSphere"), SuppressUnmanagedCodeSecurity]
+        public static extern void MassSetSphere(out Mass mass, dReal density, dReal radius);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetSphereTotal"), SuppressUnmanagedCodeSecurity]
+        public static extern void dMassSetSphereTotal(out Mass mass, dReal total_mass, dReal radius);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetTrimesh"), SuppressUnmanagedCodeSecurity]
+        public static extern void MassSetTrimesh(out Mass mass, dReal density, IntPtr g);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetZero"), SuppressUnmanagedCodeSecurity]
+        public static extern void MassSetZero(out Mass mass);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassTranslate"), SuppressUnmanagedCodeSecurity]
+        public static extern void MassTranslate(ref Mass mass, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMultiply0"), SuppressUnmanagedCodeSecurity]
+        public static extern void Multiply0(out dReal A00, ref dReal B00, ref dReal C00, int p, int q, int r);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMultiply0"), SuppressUnmanagedCodeSecurity]
+        private static extern void MultiplyiM3V3(out Vector3 vout, ref Matrix3 matrix, ref Vector3 vect,int p, int q, int r);
+        public static void MultiplyM3V3(out Vector3 outvector, ref Matrix3 matrix, ref Vector3 invector)
+        {
+            MultiplyiM3V3(out outvector, ref matrix, ref invector, 3, 3, 1);
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMultiply1"), SuppressUnmanagedCodeSecurity]
+        public static extern void Multiply1(out dReal A00, ref dReal B00, ref dReal C00, int p, int q, int r);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMultiply2"), SuppressUnmanagedCodeSecurity]
+        public static extern void Multiply2(out dReal A00, ref dReal B00, ref dReal C00, int p, int q, int r);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQFromAxisAndAngle"), SuppressUnmanagedCodeSecurity]
+        public static extern void QFromAxisAndAngle(out Quaternion q, dReal ax, dReal ay, dReal az, dReal angle);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQfromR"), SuppressUnmanagedCodeSecurity]
+        public static extern void QfromR(out Quaternion q, ref Matrix3 R);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQMultiply0"), SuppressUnmanagedCodeSecurity]
+        public static extern void QMultiply0(out Quaternion qa, ref Quaternion qb, ref Quaternion qc);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQMultiply1"), SuppressUnmanagedCodeSecurity]
+        public static extern void QMultiply1(out Quaternion qa, ref Quaternion qb, ref Quaternion qc);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQMultiply2"), SuppressUnmanagedCodeSecurity]
+        public static extern void QMultiply2(out Quaternion qa, ref Quaternion qb, ref Quaternion qc);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQMultiply3"), SuppressUnmanagedCodeSecurity]
+        public static extern void QMultiply3(out Quaternion qa, ref Quaternion qb, ref Quaternion qc);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQSetIdentity"), SuppressUnmanagedCodeSecurity]
+        public static extern void QSetIdentity(out Quaternion q);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQuadTreeSpaceCreate"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr QuadTreeSpaceCreate(IntPtr space, ref Vector3 center, ref Vector3 extents, int depth);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQuadTreeSpaceCreate"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr QuadTreeSpaceCreate(IntPtr space, ref dReal centerX, ref dReal extentsX, int depth);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRandReal"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal RandReal();
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRFrom2Axes"), SuppressUnmanagedCodeSecurity]
+        public static extern void RFrom2Axes(out Matrix3 R, dReal ax, dReal ay, dReal az, dReal bx, dReal by, dReal bz);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRFromAxisAndAngle"), SuppressUnmanagedCodeSecurity]
+        public static extern void RFromAxisAndAngle(out Matrix3 R, dReal x, dReal y, dReal z, dReal angle);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRFromEulerAngles"), SuppressUnmanagedCodeSecurity]
+        public static extern void RFromEulerAngles(out Matrix3 R, dReal phi, dReal theta, dReal psi);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRfromQ"), SuppressUnmanagedCodeSecurity]
+        public static extern void RfromQ(out Matrix3 R, ref Quaternion q);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRFromZAxis"), SuppressUnmanagedCodeSecurity]
+        public static extern void RFromZAxis(out Matrix3 R, dReal ax, dReal ay, dReal az);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRSetIdentity"), SuppressUnmanagedCodeSecurity]
+        public static extern void RSetIdentity(out Matrix3 R);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSetValue"), SuppressUnmanagedCodeSecurity]
+        public static extern void SetValue(out dReal a, int n);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSetZero"), SuppressUnmanagedCodeSecurity]
+        public static extern void SetZero(out dReal a, int n);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSimpleSpaceCreate"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr SimpleSpaceCreate(IntPtr space);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSolveCholesky"), SuppressUnmanagedCodeSecurity]
+        public static extern void SolveCholesky(ref dReal L, out dReal b, int n);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSolveL1"), SuppressUnmanagedCodeSecurity]
+        public static extern void SolveL1(ref dReal L, out dReal b, int n, int nskip);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSolveL1T"), SuppressUnmanagedCodeSecurity]
+        public static extern void SolveL1T(ref dReal L, out dReal b, int n, int nskip);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSolveLDLT"), SuppressUnmanagedCodeSecurity]
+        public static extern void SolveLDLT(ref dReal L, ref dReal d, out dReal b, int n, int nskip);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceAdd"), SuppressUnmanagedCodeSecurity]
+        public static extern void SpaceAdd(IntPtr space, IntPtr geom);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceLockQuery"), SuppressUnmanagedCodeSecurity]
+        public static extern bool SpaceLockQuery(IntPtr space);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceClean"), SuppressUnmanagedCodeSecurity]
+        public static extern void SpaceClean(IntPtr space);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceCollide"), SuppressUnmanagedCodeSecurity]
+        public static extern void SpaceCollide(IntPtr space, IntPtr data, NearCallback callback);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceCollide2"), SuppressUnmanagedCodeSecurity]
+        public static extern void SpaceCollide2(IntPtr space1, IntPtr space2, IntPtr data, NearCallback callback);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceDestroy"), SuppressUnmanagedCodeSecurity]
+        public static extern void SpaceDestroy(IntPtr space);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceGetCleanup"), SuppressUnmanagedCodeSecurity]
+        public static extern bool SpaceGetCleanup(IntPtr space);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceGetNumGeoms"), SuppressUnmanagedCodeSecurity]
+        public static extern int SpaceGetNumGeoms(IntPtr space);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceGetGeom"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr SpaceGetGeom(IntPtr space, int i);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceGetSublevel"), SuppressUnmanagedCodeSecurity]
+        public static extern int SpaceGetSublevel(IntPtr space);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceQuery"), SuppressUnmanagedCodeSecurity]
+        public static extern bool SpaceQuery(IntPtr space, IntPtr geom);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceRemove"), SuppressUnmanagedCodeSecurity]
+        public static extern void SpaceRemove(IntPtr space, IntPtr geom);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceSetCleanup"), SuppressUnmanagedCodeSecurity]
+        public static extern void SpaceSetCleanup(IntPtr space, bool mode);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceSetSublevel"), SuppressUnmanagedCodeSecurity]
+        public static extern void SpaceSetSublevel(IntPtr space, int sublevel);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSweepAndPruneSpaceCreate"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr SweepAndPruneSpaceCreate(IntPtr space, int AxisOrder);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dVectorScale"), SuppressUnmanagedCodeSecurity]
+        public static extern void VectorScale(out dReal a, ref dReal d, int n);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldCreate"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr WorldCreate();
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldDestroy"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldDestroy(IntPtr world);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableAverageSamplesCount"), SuppressUnmanagedCodeSecurity]
+        public static extern int WorldGetAutoDisableAverageSamplesCount(IntPtr world);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableAngularThreshold"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal WorldGetAutoDisableAngularThreshold(IntPtr world);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableFlag"), SuppressUnmanagedCodeSecurity]
+        public static extern bool WorldGetAutoDisableFlag(IntPtr world);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableLinearThreshold"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal WorldGetAutoDisableLinearThreshold(IntPtr world);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableSteps"), SuppressUnmanagedCodeSecurity]
+        public static extern int WorldGetAutoDisableSteps(IntPtr world);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableTime"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal WorldGetAutoDisableTime(IntPtr world);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoEnableDepthSF1"), SuppressUnmanagedCodeSecurity]
+        public static extern int WorldGetAutoEnableDepthSF1(IntPtr world);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetCFM"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal WorldGetCFM(IntPtr world);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetERP"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal WorldGetERP(IntPtr world);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetGravity"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldGetGravity(IntPtr world, out Vector3 gravity);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetGravity"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldGetGravity(IntPtr world, out dReal X);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetContactMaxCorrectingVel"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal WorldGetContactMaxCorrectingVel(IntPtr world);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetContactSurfaceLayer"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal WorldGetContactSurfaceLayer(IntPtr world);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAngularDamping"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal WorldGetAngularDamping(IntPtr world);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAngularDampingThreshold"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal WorldGetAngularDampingThreshold(IntPtr world);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetLinearDamping"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal WorldGetLinearDamping(IntPtr world);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetLinearDampingThreshold"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal WorldGetLinearDampingThreshold(IntPtr world);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetQuickStepNumIterations"), SuppressUnmanagedCodeSecurity]
+        public static extern int WorldGetQuickStepNumIterations(IntPtr world);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetQuickStepW"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal WorldGetQuickStepW(IntPtr world);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetMaxAngularSpeed"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal WorldGetMaxAngularSpeed(IntPtr world);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldImpulseToForce"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldImpulseToForce(IntPtr world, dReal stepsize, dReal ix, dReal iy, dReal iz, out Vector3 force);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldImpulseToForce"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldImpulseToForce(IntPtr world, dReal stepsize, dReal ix, dReal iy, dReal iz, out dReal forceX);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldQuickStep"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldQuickStep(IntPtr world, dReal stepsize);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAngularDamping"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldSetAngularDamping(IntPtr world, dReal scale);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAngularDampingThreshold"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldSetAngularDampingThreshold(IntPtr world, dReal threshold);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableAngularThreshold"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldSetAutoDisableAngularThreshold(IntPtr world, dReal angular_threshold);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableAverageSamplesCount"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldSetAutoDisableAverageSamplesCount(IntPtr world, int average_samples_count);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableFlag"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldSetAutoDisableFlag(IntPtr world, bool do_auto_disable);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableLinearThreshold"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldSetAutoDisableLinearThreshold(IntPtr world, dReal linear_threshold);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableSteps"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldSetAutoDisableSteps(IntPtr world, int steps);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableTime"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldSetAutoDisableTime(IntPtr world, dReal time);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoEnableDepthSF1"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldSetAutoEnableDepthSF1(IntPtr world, int autoEnableDepth);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetCFM"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldSetCFM(IntPtr world, dReal cfm);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetContactMaxCorrectingVel"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldSetContactMaxCorrectingVel(IntPtr world, dReal vel);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetContactSurfaceLayer"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldSetContactSurfaceLayer(IntPtr world, dReal depth);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetDamping"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldSetDamping(IntPtr world, dReal linear_scale, dReal angular_scale);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetERP"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldSetERP(IntPtr world, dReal erp);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetGravity"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldSetGravity(IntPtr world, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetLinearDamping"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldSetLinearDamping(IntPtr world, dReal scale);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetLinearDampingThreshold"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldSetLinearDampingThreshold(IntPtr world, dReal threshold);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetQuickStepNumIterations"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldSetQuickStepNumIterations(IntPtr world, int num);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetQuickStepW"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldSetQuickStepW(IntPtr world, dReal over_relaxation);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetMaxAngularSpeed"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldSetMaxAngularSpeed(IntPtr world, dReal max_speed);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldStep"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldStep(IntPtr world, dReal stepsize);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldStepFast1"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldStepFast1(IntPtr world, dReal stepsize, int maxiterations);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldExportDIF"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldExportDIF(IntPtr world, string filename, bool append, string prefix);
+    }
+}
diff --git a/OpenSim/Region/PhysicsModules/ubOde/ODECharacter.cs b/OpenSim/Region/PhysicsModules/ubOde/ODECharacter.cs
new file mode 100644
index 0000000..0e46471
--- /dev/null
+++ b/OpenSim/Region/PhysicsModules/ubOde/ODECharacter.cs
@@ -0,0 +1,2036 @@
+/*
+ * Copyright (c) Contributors, http://opensimulator.org/
+ * See CONTRIBUTORS.TXT for a full list of copyright holders.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the OpenSimulator Project nor the
+ *       names of its contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+
+// Revision by Ubit 2011/12
+
+using System;
+using System.Collections.Generic;
+using System.Reflection;
+using OpenMetaverse;
+using OdeAPI;
+using OpenSim.Framework;
+using OpenSim.Region.PhysicsModules.SharedBase;
+using log4net;
+
+namespace OpenSim.Region.PhysicsModule.ubOde
+{
+    /// <summary>
+    /// Various properties that ODE uses for AMotors but isn't exposed in ODE.NET so we must define them ourselves.
+    /// </summary>
+
+    public enum dParam : int
+    {
+        LowStop = 0,
+        HiStop = 1,
+        Vel = 2,
+        FMax = 3,
+        FudgeFactor = 4,
+        Bounce = 5,
+        CFM = 6,
+        StopERP = 7,
+        StopCFM = 8,
+        LoStop2 = 256,
+        HiStop2 = 257,
+        Vel2 = 258,
+        FMax2 = 259,
+        StopERP2 = 7 + 256,
+        StopCFM2 = 8 + 256,
+        LoStop3 = 512,
+        HiStop3 = 513,
+        Vel3 = 514,
+        FMax3 = 515,
+        StopERP3 = 7 + 512,
+        StopCFM3 = 8 + 512
+    }
+
+    public class OdeCharacter : PhysicsActor
+    {
+        private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
+
+        private Vector3 _position;
+        private Vector3 _zeroPosition;
+        private Vector3 _velocity;
+        private Vector3 _target_velocity;
+        private Vector3 _acceleration;
+        private Vector3 m_rotationalVelocity;
+        private Vector3 m_size;
+        private Vector3 m_collideNormal;
+        private Vector3 m_lastFallVel;
+        private Quaternion m_orientation;
+        private Quaternion m_orientation2D;
+        private float m_mass = 80f;
+        public float m_density = 60f;
+        private bool m_pidControllerActive = true;
+
+        const float basePID_D = 0.55f; // scaled for unit mass unit time (2200 /(50*80))
+        const float basePID_P = 0.225f; // scaled for unit mass unit time (900 /(50*80))
+        public float PID_D;
+        public float PID_P;
+
+        private float timeStep;
+        private float invtimeStep;
+
+        private float m_feetOffset = 0;
+        private float feetOff = 0;
+        private float boneOff = 0;
+        private float AvaAvaSizeXsq = 0.3f;
+        private float AvaAvaSizeYsq = 0.2f;
+
+        public float walkDivisor = 1.3f;
+        public float runDivisor = 0.8f;
+        private bool m_flying = false;
+        private bool m_iscolliding = false;
+        private bool m_iscollidingGround = false;
+        private bool m_iscollidingObj = false;
+        private bool m_alwaysRun = false;
+
+        private bool _zeroFlag = false;
+        private bool m_haveLastFallVel = false;
+
+        private uint m_localID = 0;
+        public bool m_returnCollisions = false;
+        // taints and their non-tainted counterparts
+        public bool m_isPhysical = false; // the current physical status
+        public float MinimumGroundFlightOffset = 3f;
+
+        private float m_buoyancy = 0f;
+
+        private bool m_freemove = false;
+
+//        private string m_name = String.Empty;
+        // other filter control
+        int m_colliderfilter = 0;
+        int m_colliderGroundfilter = 0;
+        int m_colliderObjectfilter = 0;
+
+        // Default we're a Character
+        private CollisionCategories m_collisionCategories = (CollisionCategories.Character);
+
+        // Default, Collide with Other Geometries, spaces, bodies and characters.
+        private CollisionCategories m_collisionFlags = (CollisionCategories.Character
+                                                        | CollisionCategories.Geom
+                                                        | CollisionCategories.VolumeDtc
+                                                        );
+        // we do land collisions not ode                | CollisionCategories.Land);
+        public IntPtr Body = IntPtr.Zero;
+        private ODEScene m_parent_scene;
+        private IntPtr capsule = IntPtr.Zero;
+        public IntPtr collider = IntPtr.Zero;
+
+        public IntPtr Amotor = IntPtr.Zero;
+
+        public d.Mass ShellMass;
+
+        public int m_eventsubscription = 0;
+        private int m_cureventsubscription = 0;
+        private CollisionEventUpdate CollisionEventsThisFrame = new CollisionEventUpdate();
+        private bool SentEmptyCollisionsEvent;
+
+        // unique UUID of this character object
+        public UUID m_uuid;
+        public bool bad = false;
+
+        float mu;
+
+        // HoverHeight control
+        private float m_PIDHoverHeight;
+        private float m_PIDHoverTau;
+        private bool m_useHoverPID;
+        private PIDHoverType m_PIDHoverType;
+        private float m_targetHoverHeight;
+
+
+        public OdeCharacter(uint localID, String avName, ODEScene parent_scene, Vector3 pos, Vector3 pSize, float pfeetOffset, float density, float walk_divisor, float rundivisor)
+        {
+            m_uuid = UUID.Random();
+            m_localID = localID;
+            m_parent_scene = parent_scene;
+
+            timeStep = parent_scene.ODE_STEPSIZE;
+            invtimeStep = 1 / timeStep;
+
+            if (pos.IsFinite())
+            {
+                if (pos.Z > 99999f)
+                {
+                    pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
+                }
+                if (pos.Z < -100f) // shouldn't this be 0 ?
+                {
+                    pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
+                }
+                _position = pos;
+            }
+            else
+            {
+                _position = new Vector3(((float)m_parent_scene.WorldExtents.X * 0.5f), ((float)m_parent_scene.WorldExtents.Y * 0.5f), parent_scene.GetTerrainHeightAtXY(128f, 128f) + 10f);
+                m_log.Warn("[PHYSICS]: Got NaN Position on Character Create");
+            }
+
+            m_size.X = pSize.X;
+            m_size.Y = pSize.Y;
+            m_size.Z = pSize.Z;
+
+            if(m_size.X <0.01f)
+                m_size.X = 0.01f;
+            if(m_size.Y <0.01f)
+                m_size.Y = 0.01f;
+            if(m_size.Z <0.01f)
+                m_size.Z = 0.01f;
+
+            m_feetOffset = pfeetOffset;
+            m_orientation = Quaternion.Identity;
+            m_orientation2D = Quaternion.Identity;
+            m_density = density;
+
+            // force lower density for testing
+            m_density = 3.0f;
+
+            mu = m_parent_scene.AvatarFriction;
+
+            walkDivisor = walk_divisor;
+            runDivisor = rundivisor;
+
+            m_mass = m_density * m_size.X * m_size.Y * m_size.Z; ; // sure we have a default
+
+            PID_D = basePID_D * m_mass * invtimeStep;
+            PID_P = basePID_P * m_mass * invtimeStep;
+
+            m_isPhysical = false; // current status: no ODE information exists
+
+            Name = avName;
+
+            AddChange(changes.Add, null);
+        }
+
+        public override int PhysicsActorType
+        {
+            get { return (int)ActorTypes.Agent; }
+            set { return; }
+        }
+
+        public override void getContactData(ref ContactData cdata)
+        {
+            cdata.mu = mu;
+            cdata.bounce = 0;
+            cdata.softcolide = false;
+        }
+
+        public override bool Building { get; set; }
+
+        /// <summary>
+        /// If this is set, the avatar will move faster
+        /// </summary>
+        public override bool SetAlwaysRun
+        {
+            get { return m_alwaysRun; }
+            set { m_alwaysRun = value; }
+        }
+
+        public override uint LocalID
+        {
+            get { return m_localID; }
+            set { m_localID = value; }
+        }
+
+        public override PhysicsActor ParentActor
+        {
+            get { return (PhysicsActor)this; }
+        }
+
+        public override bool Grabbed
+        {
+            set { return; }
+        }
+
+        public override bool Selected
+        {
+            set { return; }
+        }
+
+        public override float Buoyancy
+        {
+            get { return m_buoyancy; }
+            set { m_buoyancy = value; }
+        }
+
+        public override bool FloatOnWater
+        {
+            set { return; }
+        }
+
+        public override bool IsPhysical
+        {
+            get { return m_isPhysical; }
+            set { return; }
+        }
+
+        public override bool ThrottleUpdates
+        {
+            get { return false; }
+            set { return; }
+        }
+
+        public override bool Flying
+        {
+            get { return m_flying; }
+            set
+            {
+                m_flying = value;
+//                m_log.DebugFormat("[PHYSICS]: Set OdeCharacter Flying to {0}", flying);
+            }
+        }
+
+        /// <summary>
+        /// Returns if the avatar is colliding in general.
+        /// This includes the ground and objects and avatar.
+        /// </summary>
+        public override bool IsColliding
+        {
+            get { return (m_iscolliding || m_iscollidingGround); }
+            set
+            {
+                if (value)
+                {
+                    m_colliderfilter += 3;
+                    if (m_colliderfilter > 3)
+                        m_colliderfilter = 3;
+                }
+                else
+                {
+                    m_colliderfilter--;
+                    if (m_colliderfilter < 0)
+                        m_colliderfilter = 0;
+                }
+
+                if (m_colliderfilter == 0)
+                    m_iscolliding = false;
+                else
+                {
+                    m_pidControllerActive = true;
+                    m_iscolliding = true;
+                    m_freemove = false;
+                }
+            }
+        }
+
+        /// <summary>
+        /// Returns if an avatar is colliding with the ground
+        /// </summary>
+        public override bool CollidingGround
+        {
+            get { return m_iscollidingGround; }
+            set
+            {
+/*  we now control this
+                if (value)
+                    {
+                    m_colliderGroundfilter += 2;
+                    if (m_colliderGroundfilter > 2)
+                        m_colliderGroundfilter = 2;
+                    }
+                else
+                    {
+                    m_colliderGroundfilter--;
+                    if (m_colliderGroundfilter < 0)
+                        m_colliderGroundfilter = 0;
+                    }
+
+                if (m_colliderGroundfilter == 0)
+                    m_iscollidingGround = false;
+                else
+                    m_iscollidingGround = true;
+ */
+            }
+
+        }
+
+        /// <summary>
+        /// Returns if the avatar is colliding with an object
+        /// </summary>
+        public override bool CollidingObj
+        {
+            get { return m_iscollidingObj; }
+            set
+            {
+                // Ubit filter this also
+                if (value)
+                {
+                    m_colliderObjectfilter += 2;
+                    if (m_colliderObjectfilter > 2)
+                        m_colliderObjectfilter = 2;
+                }
+                else
+                {
+                    m_colliderObjectfilter--;
+                    if (m_colliderObjectfilter < 0)
+                        m_colliderObjectfilter = 0;
+                }
+
+                if (m_colliderObjectfilter == 0)
+                    m_iscollidingObj = false;
+                else
+                    m_iscollidingObj = true;
+
+//            m_iscollidingObj = value;
+
+                if (m_iscollidingObj)
+                    m_pidControllerActive = false;
+                else
+                    m_pidControllerActive = true;
+            }
+        }
+
+        /// <summary>
+        /// turn the PID controller on or off.
+        /// The PID Controller will turn on all by itself in many situations
+        /// </summary>
+        /// <param name="status"></param>
+        public void SetPidStatus(bool status)
+        {
+            m_pidControllerActive = status;
+        }
+
+        public override bool Stopped
+        {
+            get { return _zeroFlag; }
+        }
+
+        /// <summary>
+        /// This 'puts' an avatar somewhere in the physics space.
+        /// Not really a good choice unless you 'know' it's a good
+        /// spot otherwise you're likely to orbit the avatar.
+        /// </summary>
+        public override Vector3 Position
+        {
+            get { return _position; }
+            set
+            {
+                if (value.IsFinite())
+                {
+                    if (value.Z > 9999999f)
+                    {
+                        value.Z = m_parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
+                    }
+                    if (value.Z < -100f)
+                    {
+                        value.Z = m_parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
+                    }
+                    AddChange(changes.Position, value);
+                }
+                else
+                {
+                    m_log.Warn("[PHYSICS]: Got a NaN Position from Scene on a Character");
+                }
+            }
+        }
+
+        public override Vector3 RotationalVelocity
+        {
+            get { return m_rotationalVelocity; }
+            set { m_rotationalVelocity = value; }
+        }
+
+        /// <summary>
+        /// This property sets the height of the avatar only.  We use the height to make sure the avatar stands up straight
+        /// and use it to offset landings properly
+        /// </summary>
+        public override Vector3 Size
+        {
+            get
+            {
+                return m_size;
+            }
+            set
+            {
+                if (value.IsFinite())
+                {
+                    if(value.X <0.01f)
+                        value.X = 0.01f;
+                    if(value.Y <0.01f)
+                        value.Y = 0.01f;
+                    if(value.Z <0.01f)
+                        value.Z = 0.01f;
+
+                    AddChange(changes.Size, value);
+                }
+                else
+                {
+                    m_log.Warn("[PHYSICS]: Got a NaN Size from Scene on a Character");
+                }
+            }
+        }
+
+        public override void setAvatarSize(Vector3 size, float feetOffset)
+        {
+            if (size.IsFinite())
+            {
+                if (size.X < 0.01f)
+                    size.X = 0.01f;
+                if (size.Y < 0.01f)
+                    size.Y = 0.01f;
+                if (size.Z < 0.01f)
+                    size.Z = 0.01f;
+
+                strAvatarSize st = new strAvatarSize();
+                st.size = size;
+                st.offset = feetOffset;
+                AddChange(changes.AvatarSize, st);
+            }
+            else
+            {
+                m_log.Warn("[PHYSICS]: Got a NaN AvatarSize from Scene on a Character");
+            }
+
+        }
+        /// <summary>
+        /// This creates the Avatar's physical Surrogate at the position supplied
+        /// </summary>
+        /// <param name="npositionX"></param>
+        /// <param name="npositionY"></param>
+        /// <param name="npositionZ"></param>
+
+        //
+        /// <summary>
+        /// Uses the capped cyllinder volume formula to calculate the avatar's mass.
+        /// This may be used in calculations in the scene/scenepresence
+        /// </summary>
+        public override float Mass
+        {
+            get
+            {
+                return m_mass;
+            }
+        }
+        public override void link(PhysicsActor obj)
+        {
+
+        }
+
+        public override void delink()
+        {
+
+        }
+
+        public override void LockAngularMotion(byte axislocks)
+        {
+
+        }
+
+
+        public override Vector3 Force
+        {
+            get { return _target_velocity; }
+            set { return; }
+        }
+
+        public override int VehicleType
+        {
+            get { return 0; }
+            set { return; }
+        }
+
+        public override void VehicleFloatParam(int param, float value)
+        {
+
+        }
+
+        public override void VehicleVectorParam(int param, Vector3 value)
+        {
+
+        }
+
+        public override void VehicleRotationParam(int param, Quaternion rotation)
+        {
+
+        }
+
+        public override void VehicleFlags(int param, bool remove)
+        {
+
+        }
+
+        public override void SetVolumeDetect(int param)
+        {
+
+        }
+
+        public override Vector3 CenterOfMass
+        {
+            get
+            {
+                Vector3 pos = _position;
+                return pos;
+            }
+        }
+
+        public override Vector3 GeometricCenter
+        {
+            get
+            {
+                Vector3 pos = _position;
+                return pos;
+            }
+        }
+
+        public override PrimitiveBaseShape Shape
+        {
+            set { return; }
+        }
+
+        public override Vector3 rootVelocity
+        {
+            get
+            {
+                return _velocity;
+            }
+        }
+
+        public override Vector3 Velocity
+        {
+            get
+            {
+                return _velocity;
+            }
+            set
+            {
+                if (value.IsFinite())
+                {
+                    AddChange(changes.Velocity, value);
+                }
+                else
+                {
+                    m_log.Warn("[PHYSICS]: Got a NaN velocity from Scene in a Character");
+                }
+            }
+        }
+
+        public override Vector3 TargetVelocity
+        {
+            get
+            {
+                return m_targetVelocity;
+            }
+            set
+            {
+                if (value.IsFinite())
+                {
+                    AddChange(changes.TargetVelocity, value);
+                }
+                else
+                {
+                    m_log.Warn("[PHYSICS]: Got a NaN velocity from Scene in a Character");
+                }
+            }
+        }
+
+        public override Vector3 Torque
+        {
+            get { return Vector3.Zero; }
+            set { return; }
+        }
+
+        public override float CollisionScore
+        {
+            get { return 0f; }
+            set { }
+        }
+
+        public override bool Kinematic
+        {
+            get { return false; }
+            set { }
+        }
+
+        public override Quaternion Orientation
+        {
+            get { return m_orientation; }
+            set
+            {
+//                fakeori = value;
+//                givefakeori++;
+                value.Normalize();
+                AddChange(changes.Orientation, value);
+            }
+        }
+
+        public override Vector3 Acceleration
+        {
+            get { return _acceleration; }
+            set { }
+        }
+
+        public void SetAcceleration(Vector3 accel)
+        {
+            m_pidControllerActive = true;
+            _acceleration = accel;
+        }
+
+        /// <summary>
+        /// Adds the force supplied to the Target Velocity
+        /// The PID controller takes this target velocity and tries to make it a reality
+        /// </summary>
+        /// <param name="force"></param>
+        public override void AddForce(Vector3 force, bool pushforce)
+        {
+            if (force.IsFinite())
+            {
+                if (pushforce)
+                {
+                    AddChange(changes.Force, force * m_density / (m_parent_scene.ODE_STEPSIZE * 28f));
+                }
+                else
+                {
+                    AddChange(changes.TargetVelocity, force);
+                }
+            }
+            else
+            {
+                m_log.Warn("[PHYSICS]: Got a NaN force applied to a Character");
+            }
+            //m_lastUpdateSent = false;
+        }
+
+        public override void AddAngularForce(Vector3 force, bool pushforce)
+        {
+
+        }
+
+        public override void SetMomentum(Vector3 momentum)
+        {
+            if (momentum.IsFinite())
+                AddChange(changes.Momentum, momentum);
+        }
+
+
+        private void AvatarGeomAndBodyCreation(float npositionX, float npositionY, float npositionZ)
+        {
+            // sizes  one day should came from visual parameters
+            float sx = m_size.X;
+            float sy = m_size.Y;
+            float sz = m_size.Z;
+
+            float bot = -sz * 0.5f + m_feetOffset;
+            boneOff = bot + 0.3f;
+
+            float feetsz = sz * 0.45f;
+            if (feetsz > 0.6f)
+                feetsz = 0.6f;
+
+            feetOff = bot + feetsz;
+
+            AvaAvaSizeXsq = 0.4f * sx;
+            AvaAvaSizeXsq *= AvaAvaSizeXsq;
+            AvaAvaSizeYsq = 0.5f * sy;
+            AvaAvaSizeYsq *= AvaAvaSizeYsq;
+
+            m_parent_scene.waitForSpaceUnlock(m_parent_scene.CharsSpace);
+
+            collider = d.HashSpaceCreate(m_parent_scene.CharsSpace);
+            d.HashSpaceSetLevels(collider, -4, 3);
+            d.SpaceSetSublevel(collider, 3);
+            d.SpaceSetCleanup(collider, false);
+            d.GeomSetCategoryBits(collider, (uint)m_collisionCategories);
+            d.GeomSetCollideBits(collider, (uint)m_collisionFlags);
+
+            float r = m_size.X;
+            if (m_size.Y > r)
+                r = m_size.Y;
+            float l = m_size.Z - r;
+            r *= 0.5f;
+
+            capsule = d.CreateCapsule(collider, r, l);
+
+            m_mass = m_density * m_size.X * m_size.Y * m_size.Z;  // update mass
+
+            d.MassSetBoxTotal(out ShellMass, m_mass, m_size.X, m_size.Y, m_size.Z);
+
+            PID_D = basePID_D * m_mass / m_parent_scene.ODE_STEPSIZE;
+            PID_P = basePID_P * m_mass / m_parent_scene.ODE_STEPSIZE;
+
+            Body = d.BodyCreate(m_parent_scene.world);
+
+            _zeroFlag = false;
+            m_pidControllerActive = true;
+            m_freemove = false;
+
+            _velocity = Vector3.Zero;
+
+            d.BodySetAutoDisableFlag(Body, false);
+            d.BodySetPosition(Body, npositionX, npositionY, npositionZ);
+
+            _position.X = npositionX;
+            _position.Y = npositionY;
+            _position.Z = npositionZ;
+
+            d.BodySetMass(Body, ref ShellMass);
+            d.GeomSetBody(capsule, Body);
+
+            // The purpose of the AMotor here is to keep the avatar's physical
+            // surrogate from rotating while moving
+            Amotor = d.JointCreateAMotor(m_parent_scene.world, IntPtr.Zero);
+            d.JointAttach(Amotor, Body, IntPtr.Zero);
+
+            d.JointSetAMotorMode(Amotor, 0);
+            d.JointSetAMotorNumAxes(Amotor, 3);
+            d.JointSetAMotorAxis(Amotor, 0, 0, 1, 0, 0);
+            d.JointSetAMotorAxis(Amotor, 1, 0, 0, 1, 0);
+            d.JointSetAMotorAxis(Amotor, 2, 0, 0, 0, 1);
+
+            d.JointSetAMotorAngle(Amotor, 0, 0);
+            d.JointSetAMotorAngle(Amotor, 1, 0);
+            d.JointSetAMotorAngle(Amotor, 2, 0);
+
+            d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM, 0f); // make it HARD
+            d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM2, 0f);
+            d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM3, 0f);
+            d.JointSetAMotorParam(Amotor, (int)dParam.StopERP, 0.8f);
+            d.JointSetAMotorParam(Amotor, (int)dParam.StopERP2, 0.8f);
+            d.JointSetAMotorParam(Amotor, (int)dParam.StopERP3, 0.8f);
+
+            // These lowstops and high stops are effectively (no wiggle room)
+            d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, -1e-5f);
+            d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 1e-5f);
+            d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, -1e-5f);
+            d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 1e-5f);
+            d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -1e-5f);
+            d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 1e-5f);
+
+            d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel, 0);
+            d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel2, 0);
+            d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel3, 0);
+
+            d.JointSetAMotorParam(Amotor, (int)dParam.FMax, 5e8f);
+            d.JointSetAMotorParam(Amotor, (int)dParam.FMax2, 5e8f);
+            d.JointSetAMotorParam(Amotor, (int)dParam.FMax3, 5e8f);
+        }
+
+        /// <summary>
+        /// Destroys the avatar body and geom
+
+        private void AvatarGeomAndBodyDestroy()
+        {
+            // Kill the Amotor
+            if (Amotor != IntPtr.Zero)
+            {
+                d.JointDestroy(Amotor);
+                Amotor = IntPtr.Zero;
+            }
+
+            if (Body != IntPtr.Zero)
+            {
+                //kill the body
+                d.BodyDestroy(Body);
+                Body = IntPtr.Zero;
+            }
+
+            //kill the Geoms
+            if (capsule != IntPtr.Zero)
+            {
+                m_parent_scene.actor_name_map.Remove(capsule);
+                m_parent_scene.waitForSpaceUnlock(collider);
+                d.GeomDestroy(capsule);
+                capsule = IntPtr.Zero;
+            }
+
+            if (collider != IntPtr.Zero)
+            {
+                d.SpaceDestroy(collider);
+                collider = IntPtr.Zero;
+            }
+
+        }
+
+        //in place 2D rotation around Z assuming rot is normalised and is a rotation around Z
+        public void RotateXYonZ(ref float x, ref float y, ref Quaternion rot)
+        {
+            float sin = 2.0f * rot.Z * rot.W;
+            float cos = rot.W * rot.W - rot.Z * rot.Z;
+            float tx = x;
+
+            x = tx * cos - y * sin;
+            y = tx * sin + y * cos;
+        }
+        public void RotateXYonZ(ref float x, ref float y, ref float sin, ref float cos)
+        {
+            float tx = x;
+            x = tx * cos - y * sin;
+            y = tx * sin + y * cos;
+        }
+        public void invRotateXYonZ(ref float x, ref float y, ref float sin, ref float cos)
+        {
+            float tx = x;
+            x = tx * cos + y * sin;
+            y = -tx * sin + y * cos;
+        }
+
+        public void invRotateXYonZ(ref float x, ref float y, ref Quaternion rot)
+        {
+            float sin = - 2.0f * rot.Z * rot.W;
+            float cos = rot.W * rot.W - rot.Z * rot.Z;
+            float tx = x;
+
+            x = tx * cos - y * sin;
+            y = tx * sin + y * cos;
+        }
+
+            public bool Collide(IntPtr me, IntPtr other, bool reverse, ref d.ContactGeom contact,
+                ref d.ContactGeom altContact , ref bool useAltcontact, ref bool feetcollision)
+            {
+            feetcollision = false;
+            useAltcontact = false;
+
+            if (me == capsule)
+            {
+                Vector3 offset;
+
+                float h = contact.pos.Z - _position.Z;
+                offset.Z = h - feetOff;
+
+                offset.X = contact.pos.X - _position.X;
+                offset.Y = contact.pos.Y - _position.Y;
+
+                d.GeomClassID gtype = d.GeomGetClass(other);
+                if (gtype == d.GeomClassID.CapsuleClass)
+                {
+                    Vector3 roff = offset * Quaternion.Inverse(m_orientation2D);
+                    float r = roff.X *roff.X / AvaAvaSizeXsq;
+                    r += (roff.Y * roff.Y) / AvaAvaSizeYsq;
+                    if (r > 1.0f)
+                        return false;
+
+                    float dp = 1.0f -(float)Math.Sqrt((double)r);
+                    if (dp > 0.05f)
+                        dp = 0.05f;
+
+                    contact.depth = dp;
+
+                    if (offset.Z < 0)
+                    {
+                        feetcollision = true;
+                        if (h < boneOff)
+                        {
+                            m_collideNormal.X = contact.normal.X;
+                            m_collideNormal.Y = contact.normal.Y;
+                            m_collideNormal.Z = contact.normal.Z;
+                            IsColliding = true;
+                        }
+                    }
+                    return true;
+                }
+
+                if (gtype == d.GeomClassID.SphereClass && d.GeomGetBody(other) != IntPtr.Zero)
+                {
+                    if(d.GeomSphereGetRadius(other) < 0.5)
+                        return true;
+                }
+
+                if (offset.Z > 0 || contact.normal.Z > 0.35f)
+                {
+                    if (offset.Z <= 0)
+                    {
+                        feetcollision = true;
+                        if (h < boneOff)
+                        {
+                            m_collideNormal.X = contact.normal.X;
+                            m_collideNormal.Y = contact.normal.Y;
+                            m_collideNormal.Z = contact.normal.Z;
+                            IsColliding = true;
+                        }
+                    }
+                    return true;
+                }
+
+                if(m_flying)
+                    return true;
+
+                feetcollision = true;
+                if (h < boneOff)
+                {
+                    m_collideNormal.X = contact.normal.X;
+                    m_collideNormal.Y = contact.normal.Y;
+                    m_collideNormal.Z = contact.normal.Z;
+                    IsColliding = true;
+                }
+
+                altContact = contact;
+                useAltcontact = true;
+
+                offset.Z -= 0.2f;
+
+                offset.Normalize();
+
+                float tdp = contact.depth;
+                float t = offset.X;
+                t = Math.Abs(t);
+                if(t > 1e-6)
+                {
+                   tdp /= t;
+                   tdp *= contact.normal.X;
+                }
+                else
+                    tdp *= 10;
+
+                if (tdp > 0.25f)
+                    tdp = 0.25f;
+
+                altContact.depth = tdp;
+
+                if (reverse)
+                {
+                    altContact.normal.X = offset.X;
+                    altContact.normal.Y = offset.Y;
+                    altContact.normal.Z = offset.Z;
+                }
+                else
+                {
+                    altContact.normal.X = -offset.X;
+                    altContact.normal.Y = -offset.Y;
+                    altContact.normal.Z = -offset.Z;
+                }
+                return true;
+            }
+            return false;
+        }
+
+        /// <summary>
+        /// Called from Simulate
+        /// This is the avatar's movement control + PID Controller
+        /// </summary>
+        /// <param name="timeStep"></param>
+        public void Move(List<OdeCharacter> defects)
+        {
+            if (Body == IntPtr.Zero)
+                return;
+
+            d.Vector3 dtmp = d.BodyGetPosition(Body);
+            Vector3 localpos = new Vector3(dtmp.X, dtmp.Y, dtmp.Z);
+
+            // the Amotor still lets avatar rotation to drift during colisions
+            // so force it back to identity
+
+            d.Quaternion qtmp;
+            qtmp.W = m_orientation2D.W;
+            qtmp.X = m_orientation2D.X;
+            qtmp.Y = m_orientation2D.Y;
+            qtmp.Z = m_orientation2D.Z;
+            d.BodySetQuaternion(Body, ref qtmp);
+
+            if (m_pidControllerActive == false)
+            {
+                _zeroPosition = localpos;
+            }
+
+            if (!localpos.IsFinite())
+            {
+                m_log.Warn("[PHYSICS]: Avatar Position is non-finite!");
+                defects.Add(this);
+                // _parent_scene.RemoveCharacter(this);
+
+                // destroy avatar capsule and related ODE data
+                AvatarGeomAndBodyDestroy();
+                return;
+            }
+
+            // check outbounds forcing to be in world
+            bool fixbody = false;
+            if (localpos.X < 0.0f)
+            {
+                fixbody = true;
+                localpos.X = 0.1f;
+            }
+            else if (localpos.X > m_parent_scene.WorldExtents.X - 0.1f)
+            {
+                fixbody = true;
+                localpos.X = m_parent_scene.WorldExtents.X - 0.1f;
+            }
+            if (localpos.Y < 0.0f)
+            {
+                fixbody = true;
+                localpos.Y = 0.1f;
+            }
+            else if (localpos.Y > m_parent_scene.WorldExtents.Y - 0.1)
+            {
+                fixbody = true;
+                localpos.Y = m_parent_scene.WorldExtents.Y - 0.1f;
+            }
+            if (fixbody)
+            {
+                m_freemove = false;
+                d.BodySetPosition(Body, localpos.X, localpos.Y, localpos.Z);
+            }
+
+            float breakfactor;
+
+            Vector3 vec = Vector3.Zero;
+            dtmp = d.BodyGetLinearVel(Body);
+            Vector3 vel = new Vector3(dtmp.X, dtmp.Y, dtmp.Z);
+            float velLengthSquared = vel.LengthSquared();
+
+            Vector3 ctz = _target_velocity;
+
+            float movementdivisor = 1f;
+            //Ubit change divisions into multiplications below
+            if (!m_alwaysRun)
+                movementdivisor = 1 / walkDivisor;
+            else
+                movementdivisor = 1 / runDivisor;
+
+            ctz.X *= movementdivisor;
+            ctz.Y *= movementdivisor;
+
+            //******************************************
+            // colide with land
+
+            d.AABB aabb;
+//            d.GeomGetAABB(feetbox, out aabb);
+            d.GeomGetAABB(capsule, out aabb);
+            float chrminZ = aabb.MinZ; // move up a bit
+            Vector3 posch = localpos;
+
+            float ftmp;
+
+            if (m_flying)
+            {
+                ftmp = timeStep;
+                posch.X += vel.X * ftmp;
+                posch.Y += vel.Y * ftmp;
+            }
+
+            float terrainheight = m_parent_scene.GetTerrainHeightAtXY(posch.X, posch.Y);
+            if (chrminZ < terrainheight)
+            {
+                if (ctz.Z < 0)
+                    ctz.Z = 0;
+
+                if(!m_haveLastFallVel)
+                {
+                    m_lastFallVel = vel;
+                    m_haveLastFallVel = true;
+                }
+
+                Vector3 n = m_parent_scene.GetTerrainNormalAtXY(posch.X, posch.Y);
+                float depth = terrainheight - chrminZ;
+
+                vec.Z = depth * PID_P * 50;
+
+                if (!m_flying)
+                {
+                    vec.Z += -vel.Z * PID_D;
+                    if(n.Z < 0.4f)
+                    {
+                        vec.X = depth * PID_P * 50 - vel.X * PID_D;
+                        vec.X *= n.X;
+                        vec.Y = depth * PID_P * 50 - vel.Y * PID_D;
+                        vec.Y *= n.Y;
+                        vec.Z *= n.Z;
+                        if(n.Z < 0.1f)
+                        {
+                            // cancel the slope pose
+                            n.X = 0f;
+                            n.Y = 0f;
+                            n.Z = 1.0f;
+                         }
+                    }
+                }
+
+                if (depth < 0.2f)
+                {
+                    m_colliderGroundfilter++;
+                    if (m_colliderGroundfilter > 2)
+                    {
+                        m_iscolliding = true;
+                        m_colliderfilter = 2;
+
+                        if (m_colliderGroundfilter > 10)
+                        {
+                            m_colliderGroundfilter = 10;
+                            m_freemove = false;
+                        }
+
+                        m_collideNormal.X = n.X;
+                        m_collideNormal.Y = n.Y;
+                        m_collideNormal.Z = n.Z;
+
+                        m_iscollidingGround = true;
+
+                        ContactPoint contact = new ContactPoint();
+                        contact.PenetrationDepth = depth;
+                        contact.Position.X = localpos.X;
+                        contact.Position.Y = localpos.Y;
+                        contact.Position.Z = terrainheight;
+                        contact.SurfaceNormal.X = -n.X;
+                        contact.SurfaceNormal.Y = -n.Y;
+                        contact.SurfaceNormal.Z = -n.Z;
+                        contact.RelativeSpeed = Vector3.Dot(m_lastFallVel, n);
+                        contact.CharacterFeet = true;
+                        AddCollisionEvent(0, contact);
+                        m_lastFallVel = vel;
+
+//                        vec.Z *= 0.5f;
+                    }
+                }
+
+                else
+                {
+                    m_colliderGroundfilter -= 5;
+                    if (m_colliderGroundfilter <= 0)
+                    {
+                        m_colliderGroundfilter = 0;
+                        m_iscollidingGround = false;
+                    }
+                }
+            }
+            else
+            {
+                m_haveLastFallVel = false;
+                m_colliderGroundfilter -= 5;
+                if (m_colliderGroundfilter <= 0)
+                {
+                    m_colliderGroundfilter = 0;
+                    m_iscollidingGround = false;
+                }
+            }
+
+            bool hoverPIDActive = false;
+
+            if (m_useHoverPID && m_PIDHoverTau != 0 && m_PIDHoverHeight != 0)
+            {
+                hoverPIDActive = true;
+
+                switch (m_PIDHoverType)
+                {
+                    case PIDHoverType.Ground:
+                        m_targetHoverHeight = terrainheight + m_PIDHoverHeight;
+                        break;
+
+                    case PIDHoverType.GroundAndWater:
+                        float waterHeight = m_parent_scene.GetWaterLevel();
+                        if (terrainheight > waterHeight)
+                            m_targetHoverHeight = terrainheight + m_PIDHoverHeight;
+                        else
+                            m_targetHoverHeight = waterHeight + m_PIDHoverHeight;
+                        break;
+                }     // end switch (m_PIDHoverType)
+
+                    // don't go underground
+                if (m_targetHoverHeight > terrainheight + 0.5f * (aabb.MaxZ - aabb.MinZ))
+                {
+                    float fz = (m_targetHoverHeight - localpos.Z);
+
+                    //  if error is zero, use position control; otherwise, velocity control
+                    if (Math.Abs(fz) < 0.01f)
+                    {
+                        ctz.Z = 0;
+                    }
+                    else
+                    {
+                        _zeroFlag = false;
+                        fz /= m_PIDHoverTau;
+
+                        float tmp = Math.Abs(fz);
+                        if (tmp > 50)
+                            fz = 50 * Math.Sign(fz);
+                        else if (tmp < 0.1)
+                            fz = 0.1f * Math.Sign(fz);
+
+                        ctz.Z = fz;
+                    }
+                }
+            }
+
+            //******************************************
+            if (!m_iscolliding)
+                m_collideNormal.Z = 0;
+
+            bool tviszero = (ctz.X == 0.0f && ctz.Y == 0.0f && ctz.Z == 0.0f);
+
+            if (!tviszero)
+            {
+                m_freemove = false;
+
+                // movement relative to surface if moving on it
+                // dont disturbe vertical movement, ie jumps
+                if (m_iscolliding && !m_flying && ctz.Z == 0 && m_collideNormal.Z > 0.2f && m_collideNormal.Z < 0.94f)
+                {
+                    float p = ctz.X * m_collideNormal.X + ctz.Y * m_collideNormal.Y;
+                    ctz.X *= (float)Math.Sqrt(1 - m_collideNormal.X * m_collideNormal.X);
+                    ctz.Y *= (float)Math.Sqrt(1 - m_collideNormal.Y * m_collideNormal.Y);
+                    ctz.Z -= p;
+                    if (ctz.Z < 0)
+                        ctz.Z *= 2;
+
+                }
+
+            }
+
+            if (!m_freemove)
+            {
+
+                //  if velocity is zero, use position control; otherwise, velocity control
+                if (tviszero && m_iscolliding && !m_flying)
+                {
+                    //  keep track of where we stopped.  No more slippin' & slidin'
+                    if (!_zeroFlag)
+                    {
+                        _zeroFlag = true;
+                        _zeroPosition = localpos;
+                    }
+                    if (m_pidControllerActive)
+                    {
+                        // We only want to deactivate the PID Controller if we think we want to have our surrogate
+                        // react to the physics scene by moving it's position.
+                        // Avatar to Avatar collisions
+                        // Prim to avatar collisions
+
+                        vec.X = -vel.X * PID_D * 2f + (_zeroPosition.X - localpos.X) * (PID_P * 5);
+                        vec.Y = -vel.Y * PID_D * 2f + (_zeroPosition.Y - localpos.Y) * (PID_P * 5);
+                        if(vel.Z > 0)
+                            vec.Z += -vel.Z * PID_D + (_zeroPosition.Z - localpos.Z) * PID_P;
+                        else
+                            vec.Z += (-vel.Z * PID_D + (_zeroPosition.Z - localpos.Z) * PID_P) * 0.2f;
+/*
+                        if (flying)
+                        {
+                            vec.Z += -vel.Z * PID_D + (_zeroPosition.Z - localpos.Z) * PID_P;
+                        }
+*/
+                    }
+                    //PidStatus = true;
+                }
+                else
+                {
+                    m_pidControllerActive = true;
+                    _zeroFlag = false;
+
+                    if (m_iscolliding)
+                    {
+                        if (!m_flying)
+                        {
+                            // we are on a surface
+                            if (ctz.Z > 0f)
+                            {
+                                // moving up or JUMPING
+                                vec.Z += (ctz.Z - vel.Z) * PID_D * 2f;
+                                vec.X += (ctz.X - vel.X) * (PID_D);
+                                vec.Y += (ctz.Y - vel.Y) * (PID_D);
+                            }
+                            else
+                            {
+                                // we are moving down on a surface
+                                if (ctz.Z == 0)
+                                {
+                                    if (vel.Z > 0)
+                                        vec.Z -= vel.Z * PID_D * 2f;
+                                    vec.X += (ctz.X - vel.X) * (PID_D);
+                                    vec.Y += (ctz.Y - vel.Y) * (PID_D);
+                                }
+                                // intencionally going down
+                                else
+                                {
+                                    if (ctz.Z < vel.Z)
+                                        vec.Z += (ctz.Z - vel.Z) * PID_D;
+                                    else
+                                    {
+                                    }
+
+                                    if (Math.Abs(ctz.X) > Math.Abs(vel.X))
+                                        vec.X += (ctz.X - vel.X) * (PID_D);
+                                    if (Math.Abs(ctz.Y) > Math.Abs(vel.Y))
+                                        vec.Y += (ctz.Y - vel.Y) * (PID_D);
+                                }
+                            }
+
+                            // We're standing on something
+                        }
+                        else
+                        {
+                            // We're flying and colliding with something
+                            vec.X += (ctz.X - vel.X) * (PID_D * 0.0625f);
+                            vec.Y += (ctz.Y - vel.Y) * (PID_D * 0.0625f);
+                            vec.Z += (ctz.Z - vel.Z) * (PID_D * 0.0625f);
+                        }
+                    }
+                    else // ie not colliding
+                    {
+                        if (m_flying || hoverPIDActive) //(!m_iscolliding && flying)
+                        {
+                            // we're in mid air suspended
+                            vec.X += (ctz.X - vel.X) * (PID_D);
+                            vec.Y += (ctz.Y - vel.Y) * (PID_D);
+                            vec.Z += (ctz.Z - vel.Z) * (PID_D);
+                        }
+
+                        else
+                        {
+                            // we're not colliding and we're not flying so that means we're falling!
+                            // m_iscolliding includes collisions with the ground.
+
+                            // d.Vector3 pos = d.BodyGetPosition(Body);
+                            vec.X += (ctz.X - vel.X) * PID_D * 0.833f;
+                            vec.Y += (ctz.Y - vel.Y) * PID_D * 0.833f;
+                            // hack for  breaking on fall
+                            if (ctz.Z == -9999f)
+                                vec.Z += -vel.Z * PID_D - m_parent_scene.gravityz * m_mass;
+                        }
+                    }
+                }
+
+                if (velLengthSquared > 2500.0f) // 50m/s apply breaks
+                {
+                    breakfactor = 0.16f * m_mass;
+                    vec.X -= breakfactor * vel.X;
+                    vec.Y -= breakfactor * vel.Y;
+                    vec.Z -= breakfactor * vel.Z;
+                }
+            }
+            else
+            {
+                breakfactor = m_mass;
+                vec.X -= breakfactor * vel.X;
+                vec.Y -= breakfactor * vel.Y;
+                if (m_flying)
+                    vec.Z -= 0.5f * breakfactor * vel.Z;
+                else
+                    vec.Z -= .16f* m_mass * vel.Z;
+            }
+
+            if (m_flying || hoverPIDActive)
+            {
+                vec.Z -= m_parent_scene.gravityz * m_mass;
+
+                if(!hoverPIDActive)
+                {
+                    //Added for auto fly height. Kitto Flora
+                    float target_altitude = terrainheight + MinimumGroundFlightOffset;
+
+                    if (localpos.Z < target_altitude)
+                    {
+                       vec.Z += (target_altitude - localpos.Z) * PID_P * 5.0f;
+                    }
+                    // end add Kitto Flora
+                }
+            }
+
+            if (vec.IsFinite())
+            {
+                if (vec.X != 0 || vec.Y !=0 || vec.Z !=0)
+                    d.BodyAddForce(Body, vec.X, vec.Y, vec.Z);
+            }
+            else
+            {
+                m_log.Warn("[PHYSICS]: Got a NaN force vector in Move()");
+                m_log.Warn("[PHYSICS]: Avatar Position is non-finite!");
+                defects.Add(this);
+                // _parent_scene.RemoveCharacter(this);
+                // destroy avatar capsule and related ODE data
+                AvatarGeomAndBodyDestroy();
+                return;
+            }
+
+            // update our local ideia of position velocity and aceleration
+            //            _position = localpos;
+            _position = localpos;
+
+            if (_zeroFlag)
+            {
+                _velocity = Vector3.Zero;
+                _acceleration = Vector3.Zero;
+                m_rotationalVelocity = Vector3.Zero;
+            }
+            else
+            {
+                Vector3 a =_velocity; // previus velocity
+                SetSmooth(ref _velocity, ref vel, 2);
+                a = (_velocity - a) * invtimeStep;
+                SetSmooth(ref _acceleration, ref a, 2);
+
+                dtmp = d.BodyGetAngularVel(Body);
+                m_rotationalVelocity.X = 0f;
+                m_rotationalVelocity.Y = 0f;
+                m_rotationalVelocity.Z = dtmp.Z;
+                Math.Round(m_rotationalVelocity.Z,3);
+            }
+        }
+
+        public void round(ref Vector3 v, int digits)
+        {
+            v.X = (float)Math.Round(v.X, digits);
+            v.Y = (float)Math.Round(v.Y, digits);
+            v.Z = (float)Math.Round(v.Z, digits);
+         }
+
+        public void SetSmooth(ref Vector3 dst, ref Vector3 value)
+        {
+            dst.X = 0.1f * dst.X + 0.9f * value.X;
+            dst.Y = 0.1f * dst.Y + 0.9f * value.Y;
+            dst.Z = 0.1f * dst.Z + 0.9f * value.Z;
+        }
+
+        public void SetSmooth(ref Vector3 dst, ref Vector3 value, int rounddigits)
+        {
+            dst.X = 0.4f * dst.X + 0.6f * value.X;
+            dst.X = (float)Math.Round(dst.X, rounddigits);
+
+            dst.Y = 0.4f * dst.Y + 0.6f * value.Y;
+            dst.Y = (float)Math.Round(dst.Y, rounddigits);
+
+            dst.Z = 0.4f * dst.Z + 0.6f * value.Z;
+            dst.Z = (float)Math.Round(dst.Z, rounddigits);
+        }
+
+
+        /// <summary>
+        /// Updates the reported position and velocity.
+        /// Used to copy variables from unmanaged space at heartbeat rate and also trigger scene updates acording
+        /// also outbounds checking
+        /// copy and outbounds now done in move(..) at ode rate
+        ///
+        /// </summary>
+        public void UpdatePositionAndVelocity()
+        {
+            return;
+
+//            if (Body == IntPtr.Zero)
+//                return;
+
+        }
+
+        /// <summary>
+        /// Cleanup the things we use in the scene.
+        /// </summary>
+        public void Destroy()
+        {
+            AddChange(changes.Remove, null);
+        }
+
+        public override void CrossingFailure()
+        {
+        }
+
+        public override Vector3 PIDTarget { set { return; } }
+        public override bool PIDActive {get {return m_pidControllerActive;} set { return; } }
+        public override float PIDTau { set { return; } }
+
+        public override float PIDHoverHeight
+        {
+            set
+            {
+                AddChange(changes.PIDHoverHeight,value);
+            }
+        }
+        public override bool PIDHoverActive
+        {
+            get
+            {
+            return m_useHoverPID;
+            }
+            set
+            {
+                AddChange(changes.PIDHoverActive, value);
+            }
+        }
+
+        public override PIDHoverType PIDHoverType
+        {
+            set
+            {
+                AddChange(changes.PIDHoverType,value);
+            }
+        }
+
+        public override float PIDHoverTau
+        {
+            set
+            {
+                float tmp =0;
+                if (value > 0)
+                {
+                    float mint = (0.05f > timeStep ? 0.05f : timeStep);
+                    if (value < mint)
+                        tmp = mint;
+                    else
+                        tmp = value;
+                }
+                AddChange(changes.PIDHoverTau, tmp);
+            }
+        }
+
+        public override Quaternion APIDTarget { set { return; } }
+
+        public override bool APIDActive { set { return; } }
+
+        public override float APIDStrength { set { return; } }
+
+        public override float APIDDamping { set { return; } }
+
+        public override void SubscribeEvents(int ms)
+        {
+            m_eventsubscription = ms;
+            m_cureventsubscription = 0;
+            CollisionEventsThisFrame.Clear();
+            SentEmptyCollisionsEvent = false;
+        }
+
+        public override void UnSubscribeEvents()
+        {
+            m_eventsubscription = 0;
+            m_parent_scene.RemoveCollisionEventReporting(this);
+            lock(CollisionEventsThisFrame)
+                CollisionEventsThisFrame.Clear();
+        }
+
+        public override void AddCollisionEvent(uint CollidedWith, ContactPoint contact)
+        {
+            lock(CollisionEventsThisFrame)
+                CollisionEventsThisFrame.AddCollider(CollidedWith, contact);
+            m_parent_scene.AddCollisionEventReporting(this);
+        }
+
+        public void SendCollisions(int timestep)
+        {
+            if (m_cureventsubscription < 50000)
+                m_cureventsubscription += timestep;
+
+            if (m_cureventsubscription < m_eventsubscription)
+                return;
+
+            lock(CollisionEventsThisFrame)
+            {
+                int ncolisions = CollisionEventsThisFrame.m_objCollisionList.Count;
+
+                if (!SentEmptyCollisionsEvent || ncolisions > 0)
+                {
+                    base.SendCollisionUpdate(CollisionEventsThisFrame);
+                    m_cureventsubscription = 0;
+
+                    if (ncolisions == 0)
+                    {
+                        SentEmptyCollisionsEvent = true;
+    //                  _parent_scene.RemoveCollisionEventReporting(this);
+                    }
+                    else
+                    {
+                        SentEmptyCollisionsEvent = false;
+                        CollisionEventsThisFrame.Clear();
+                    }
+                }
+            }
+        }
+
+        public override bool SubscribedEvents()
+        {
+            if (m_eventsubscription > 0)
+                return true;
+            return false;
+        }
+
+        private void changePhysicsStatus(bool NewStatus)
+        {
+            if (NewStatus != m_isPhysical)
+            {
+                if (NewStatus)
+                {
+                    AvatarGeomAndBodyDestroy();
+
+                    AvatarGeomAndBodyCreation(_position.X, _position.Y, _position.Z);
+
+                    m_parent_scene.actor_name_map[collider] = (PhysicsActor)this;
+                    m_parent_scene.actor_name_map[capsule] = (PhysicsActor)this;
+                    m_parent_scene.AddCharacter(this);
+                }
+                else
+                {
+                    m_parent_scene.RemoveCollisionEventReporting(this);
+                    m_parent_scene.RemoveCharacter(this);
+                    // destroy avatar capsule and related ODE data
+                    AvatarGeomAndBodyDestroy();
+                }
+                m_freemove = false;
+                m_isPhysical = NewStatus;
+            }
+        }
+
+        private void changeAdd()
+        {
+            changePhysicsStatus(true);
+        }
+
+        private void changeRemove()
+        {
+            changePhysicsStatus(false);
+        }
+
+        private void changeShape(PrimitiveBaseShape arg)
+        {
+        }
+
+        private void changeAvatarSize(strAvatarSize st)
+        {
+            m_feetOffset = st.offset;
+            changeSize(st.size);
+        }
+
+        private void changeSize(Vector3 pSize)
+        {
+            if (pSize.IsFinite())
+            {
+                // for now only look to Z changes since viewers also don't change X and Y
+                if (pSize.Z != m_size.Z)
+                {
+                    AvatarGeomAndBodyDestroy();
+
+                    float oldsz = m_size.Z;
+                    m_size = pSize;
+
+                    AvatarGeomAndBodyCreation(_position.X, _position.Y,
+                                      _position.Z + (m_size.Z - oldsz) * 0.5f);
+
+//                    Velocity = Vector3.Zero;
+                    m_targetVelocity = Vector3.Zero;
+
+                    m_parent_scene.actor_name_map[collider] = (PhysicsActor)this;
+                    m_parent_scene.actor_name_map[capsule] = (PhysicsActor)this;
+                }
+                m_freemove = false;
+                m_pidControllerActive = true;
+            }
+            else
+            {
+                m_log.Warn("[PHYSICS]: Got a NaN Size from Scene on a Character");
+            }
+        }
+
+        private void changePosition( Vector3 newPos)
+            {
+                if (Body != IntPtr.Zero)
+                    d.BodySetPosition(Body, newPos.X, newPos.Y, newPos.Z);
+                _position = newPos;
+                m_freemove = false;
+                m_pidControllerActive = true;
+            }
+
+        private void changeOrientation(Quaternion newOri)
+        {
+            if (m_orientation != newOri)
+            {
+                m_orientation = newOri; // keep a copy for core use
+                // but only use rotations around Z
+
+                m_orientation2D.W = newOri.W;
+                m_orientation2D.Z = newOri.Z;
+
+                float t = m_orientation2D.W * m_orientation2D.W + m_orientation2D.Z * m_orientation2D.Z;
+                if (t > 0)
+                {
+                    t = 1.0f / (float)Math.Sqrt(t);
+                    m_orientation2D.W *= t;
+                    m_orientation2D.Z *= t;
+                }
+                else
+                {
+                    m_orientation2D.W = 1.0f;
+                    m_orientation2D.Z = 0f;
+                }
+                m_orientation2D.Y = 0f;
+                m_orientation2D.X = 0f;
+
+                d.Quaternion myrot = new d.Quaternion();
+                myrot.X = m_orientation2D.X;
+                myrot.Y = m_orientation2D.Y;
+                myrot.Z = m_orientation2D.Z;
+                myrot.W = m_orientation2D.W;
+                d.BodySetQuaternion(Body, ref myrot);
+            }
+        }
+
+        private void changeVelocity(Vector3 newVel)
+        {
+            _velocity = newVel;
+            setFreeMove();
+
+            if (Body != IntPtr.Zero)
+                d.BodySetLinearVel(Body, newVel.X, newVel.Y, newVel.Z);
+        }
+
+        private void changeTargetVelocity(Vector3 newVel)
+        {
+            m_pidControllerActive = true;
+            m_freemove = false;
+            _target_velocity = newVel;
+        }
+
+        private void changeSetTorque(Vector3 newTorque)
+        {
+        }
+
+        private void changeAddForce(Vector3 newForce)
+        {
+        }
+
+        private void changeAddAngularForce(Vector3 arg)
+        {
+        }
+
+        private void changeAngularLock(byte arg)
+        {
+        }
+
+        private void changeFloatOnWater(bool arg)
+        {
+        }
+
+        private void changeVolumedetetion(bool arg)
+        {
+        }
+
+        private void changeSelectedStatus(bool arg)
+        {
+        }
+
+        private void changeDisable(bool arg)
+        {
+        }
+
+        private void changeBuilding(bool arg)
+        {
+        }
+
+        private void setFreeMove()
+        {
+            m_pidControllerActive = true;
+            _zeroFlag = false;
+            _target_velocity = Vector3.Zero;
+            m_freemove = true;
+            m_colliderfilter = -1;
+            m_colliderObjectfilter = -1;
+            m_colliderGroundfilter = -1;
+
+            m_iscolliding = false;
+            m_iscollidingGround = false;
+            m_iscollidingObj = false;
+
+            CollisionEventsThisFrame.Clear();
+        }
+
+        private void changeForce(Vector3 newForce)
+        {
+            setFreeMove();
+
+            if (Body != IntPtr.Zero)
+            {
+                if (newForce.X != 0f || newForce.Y != 0f || newForce.Z != 0)
+                    d.BodyAddForce(Body, newForce.X, newForce.Y, newForce.Z);
+            }
+        }
+
+        // for now momentum is actually velocity
+        private void changeMomentum(Vector3 newmomentum)
+        {
+            _velocity = newmomentum;
+            setFreeMove();
+
+            if (Body != IntPtr.Zero)
+                d.BodySetLinearVel(Body, newmomentum.X, newmomentum.Y, newmomentum.Z);
+        }
+
+        private void changePIDHoverHeight(float val)
+        {
+          m_PIDHoverHeight = val;
+          if (val == 0)
+            m_useHoverPID = false;
+        }
+
+        private void changePIDHoverType(PIDHoverType type)
+        {
+            m_PIDHoverType = type;
+        }
+
+        private void changePIDHoverTau(float tau)
+        {
+            m_PIDHoverTau = tau;
+        }
+
+        private void changePIDHoverActive(bool active)
+        {
+            m_useHoverPID = active;
+        }
+
+        private void donullchange()
+        {
+        }
+
+        public bool DoAChange(changes what, object arg)
+        {
+            if (collider == IntPtr.Zero && what != changes.Add && what != changes.Remove)
+            {
+                return false;
+            }
+
+            // nasty switch
+            switch (what)
+            {
+                case changes.Add:
+                    changeAdd();
+                    break;
+                case changes.Remove:
+                    changeRemove();
+                    break;
+
+                case changes.Position:
+                    changePosition((Vector3)arg);
+                    break;
+
+                case changes.Orientation:
+                    changeOrientation((Quaternion)arg);
+                    break;
+
+                case changes.PosOffset:
+                    donullchange();
+                    break;
+
+                case changes.OriOffset:
+                    donullchange();
+                    break;
+
+                case changes.Velocity:
+                    changeVelocity((Vector3)arg);
+                    break;
+
+                case changes.TargetVelocity:
+                    changeTargetVelocity((Vector3)arg);
+                    break;
+
+                //                case changes.Acceleration:
+                //                    changeacceleration((Vector3)arg);
+                //                    break;
+                //                case changes.AngVelocity:
+                //                    changeangvelocity((Vector3)arg);
+                //                    break;
+
+                case changes.Force:
+                    changeForce((Vector3)arg);
+                    break;
+
+                case changes.Torque:
+                    changeSetTorque((Vector3)arg);
+                    break;
+
+                case changes.AddForce:
+                    changeAddForce((Vector3)arg);
+                    break;
+
+                case changes.AddAngForce:
+                    changeAddAngularForce((Vector3)arg);
+                    break;
+
+                case changes.AngLock:
+                    changeAngularLock((byte)arg);
+                    break;
+
+                case changes.Size:
+                    changeSize((Vector3)arg);
+                    break;
+
+                case changes.AvatarSize:
+                    changeAvatarSize((strAvatarSize)arg);
+                    break;
+
+                case changes.Momentum:
+                    changeMomentum((Vector3)arg);
+                    break;
+
+                case changes.PIDHoverHeight:
+                    changePIDHoverHeight((float)arg);
+                    break;
+
+                case changes.PIDHoverType:
+                    changePIDHoverType((PIDHoverType)arg);
+                    break;
+
+                case changes.PIDHoverTau:
+                    changePIDHoverTau((float)arg);
+                    break;
+
+                case changes.PIDHoverActive:
+                    changePIDHoverActive((bool)arg);
+                    break;
+
+/* not in use for now
+                case changes.Shape:
+                    changeShape((PrimitiveBaseShape)arg);
+                    break;
+
+                case changes.CollidesWater:
+                    changeFloatOnWater((bool)arg);
+                    break;
+
+                case changes.VolumeDtc:
+                    changeVolumedetetion((bool)arg);
+                    break;
+
+                case changes.Physical:
+                    changePhysicsStatus((bool)arg);
+                    break;
+
+                case changes.Selected:
+                    changeSelectedStatus((bool)arg);
+                    break;
+
+                case changes.disabled:
+                    changeDisable((bool)arg);
+                    break;
+
+                case changes.building:
+                    changeBuilding((bool)arg);
+                    break;
+*/
+                case changes.Null:
+                    donullchange();
+                    break;
+
+                default:
+                    donullchange();
+                    break;
+            }
+            return false;
+        }
+
+        public void AddChange(changes what, object arg)
+        {
+            m_parent_scene.AddChange((PhysicsActor)this, what, arg);
+        }
+
+        private struct strAvatarSize
+        {
+            public Vector3 size;
+            public float offset;
+        }
+
+    }
+}
diff --git a/OpenSim/Region/PhysicsModules/ubOde/ODEDynamics.cs b/OpenSim/Region/PhysicsModules/ubOde/ODEDynamics.cs
new file mode 100644
index 0000000..ce10065
--- /dev/null
+++ b/OpenSim/Region/PhysicsModules/ubOde/ODEDynamics.cs
@@ -0,0 +1,1205 @@
+/*
+ * Copyright (c) Contributors, http://opensimulator.org/
+ * See CONTRIBUTORS.TXT for a full list of copyright holders.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the OpenSimulator Project nor the
+ *       names of its contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/* Revised Aug, Sept 2009 by Kitto Flora. ODEDynamics.cs replaces
+ * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised:
+ * ODEPrim.cs contains methods dealing with Prim editing, Prim
+ * characteristics and Kinetic motion.
+ * ODEDynamics.cs contains methods dealing with Prim Physical motion
+ * (dynamics) and the associated settings. Old Linear and angular
+ * motors for dynamic motion have been replace with  MoveLinear()
+ * and MoveAngular(); 'Physical' is used only to switch ODE dynamic
+ * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_<other> is to
+ * switch between 'VEHICLE' parameter use and general dynamics
+ * settings use.
+ */
+
+// Extensive change Ubit 2012
+
+using System;
+using System.Collections.Generic;
+using System.Reflection;
+using System.Runtime.InteropServices;
+using log4net;
+using OpenMetaverse;
+using OdeAPI;
+using OpenSim.Framework;
+using OpenSim.Region.PhysicsModules.SharedBase;
+
+namespace OpenSim.Region.PhysicsModule.ubOde
+{
+    public class ODEDynamics
+    {
+        public Vehicle Type
+        {
+            get { return m_type; }
+        }
+
+        private OdePrim rootPrim;
+        private ODEScene _pParentScene;
+
+        // Vehicle properties
+        // WARNING this are working copies for internel use
+        // their values may not be the corresponding parameter
+
+        private Quaternion m_referenceFrame = Quaternion.Identity;      // Axis modifier
+        private Quaternion m_RollreferenceFrame = Quaternion.Identity;  // what hell is this ?
+
+        private Vehicle m_type = Vehicle.TYPE_NONE;                     // If a 'VEHICLE', and what kind
+
+        private VehicleFlag m_flags = (VehicleFlag) 0;                  // Boolean settings:
+                                                                        // HOVER_TERRAIN_ONLY
+                                                                        // HOVER_GLOBAL_HEIGHT
+                                                                        // NO_DEFLECTION_UP
+                                                                        // HOVER_WATER_ONLY
+                                                                        // HOVER_UP_ONLY
+                                                                        // LIMIT_MOTOR_UP
+                                                                        // LIMIT_ROLL_ONLY
+        private Vector3 m_BlockingEndPoint = Vector3.Zero;              // not sl
+
+        // Linear properties
+        private Vector3 m_linearMotorDirection = Vector3.Zero;          // velocity requested by LSL, decayed by time
+        private Vector3 m_linearFrictionTimescale = new Vector3(1000, 1000, 1000);
+        private float m_linearMotorDecayTimescale = 120;
+        private float m_linearMotorTimescale = 1000;
+        private Vector3 m_linearMotorOffset = Vector3.Zero;
+
+        //Angular properties
+        private Vector3 m_angularMotorDirection = Vector3.Zero;         // angular velocity requested by LSL motor
+        private float m_angularMotorTimescale = 1000;                      // motor angular velocity ramp up rate
+        private float m_angularMotorDecayTimescale = 120;                 // motor angular velocity decay rate
+        private Vector3 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);      // body angular velocity  decay rate
+
+        //Deflection properties
+        private float m_angularDeflectionEfficiency = 0;
+        private float m_angularDeflectionTimescale = 1000;
+        private float m_linearDeflectionEfficiency = 0;
+        private float m_linearDeflectionTimescale = 1000;
+
+        //Banking properties
+        private float m_bankingEfficiency = 0;
+        private float m_bankingMix = 0;
+        private float m_bankingTimescale = 1000;
+
+        //Hover and Buoyancy properties
+        private float m_VhoverHeight = 0f;
+        private float m_VhoverEfficiency = 0f;
+        private float m_VhoverTimescale = 1000f;
+        private float m_VehicleBuoyancy = 0f;           //KF: m_VehicleBuoyancy is set by VEHICLE_BUOYANCY for a vehicle.
+                    // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity)
+                    // KF: So far I have found no good method to combine a script-requested .Z velocity and gravity.
+                    // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity.
+
+        //Attractor properties
+        private float m_verticalAttractionEfficiency = 1.0f;        // damped
+        private float m_verticalAttractionTimescale = 1000f;        // Timescale > 300  means no vert attractor.
+
+
+        // auxiliar
+        private float m_lmEfect = 0f;                                            // current linear motor eficiency
+        private float m_lmDecay = 0f;                                            // current linear decay
+
+        private float m_amEfect = 0;                                            // current angular motor eficiency
+        private float m_amDecay = 0f;                                            // current linear decay
+
+        private float m_ffactor = 1.0f;
+
+        private float m_timestep = 0.02f;
+        private float m_invtimestep = 50;
+
+
+        float m_ampwr;
+        float m_amdampX;
+        float m_amdampY;
+        float m_amdampZ;
+
+        float m_gravmod;
+
+        public float FrictionFactor
+        {
+            get
+            {
+                return m_ffactor;
+            }
+        }
+
+        public float GravMod
+        {
+            set
+            {
+                m_gravmod = value;
+            }
+        }
+
+
+        public ODEDynamics(OdePrim rootp)
+        {
+            rootPrim = rootp;
+            _pParentScene = rootPrim._parent_scene;
+            m_timestep = _pParentScene.ODE_STEPSIZE;
+            m_invtimestep = 1.0f / m_timestep;
+            m_gravmod = rootPrim.GravModifier;
+        }
+
+        public void DoSetVehicle(VehicleData vd)
+        {
+            m_type = vd.m_type;
+            m_flags = vd.m_flags;
+
+
+            // Linear properties
+            m_linearMotorDirection = vd.m_linearMotorDirection;
+
+            m_linearFrictionTimescale = vd.m_linearFrictionTimescale;
+            if (m_linearFrictionTimescale.X < m_timestep) m_linearFrictionTimescale.X = m_timestep;
+            if (m_linearFrictionTimescale.Y < m_timestep) m_linearFrictionTimescale.Y = m_timestep;
+            if (m_linearFrictionTimescale.Z < m_timestep) m_linearFrictionTimescale.Z = m_timestep;
+
+            m_linearMotorDecayTimescale = vd.m_linearMotorDecayTimescale;
+            if (m_linearMotorDecayTimescale < m_timestep) m_linearMotorDecayTimescale = m_timestep;
+            m_linearMotorDecayTimescale += 0.2f;
+            m_linearMotorDecayTimescale *= m_invtimestep;
+
+            m_linearMotorTimescale = vd.m_linearMotorTimescale;
+            if (m_linearMotorTimescale < m_timestep) m_linearMotorTimescale = m_timestep;
+
+            m_linearMotorOffset = vd.m_linearMotorOffset;
+
+            //Angular properties
+            m_angularMotorDirection = vd.m_angularMotorDirection;
+            m_angularMotorTimescale = vd.m_angularMotorTimescale;
+            if (m_angularMotorTimescale < m_timestep) m_angularMotorTimescale = m_timestep;
+
+            m_angularMotorDecayTimescale = vd.m_angularMotorDecayTimescale;
+            if (m_angularMotorDecayTimescale < m_timestep) m_angularMotorDecayTimescale = m_timestep;
+            m_angularMotorDecayTimescale *= m_invtimestep;
+
+            m_angularFrictionTimescale = vd.m_angularFrictionTimescale;
+            if (m_angularFrictionTimescale.X < m_timestep) m_angularFrictionTimescale.X = m_timestep;
+            if (m_angularFrictionTimescale.Y < m_timestep) m_angularFrictionTimescale.Y = m_timestep;
+            if (m_angularFrictionTimescale.Z < m_timestep) m_angularFrictionTimescale.Z = m_timestep;
+
+            //Deflection properties
+            m_angularDeflectionEfficiency = vd.m_angularDeflectionEfficiency;
+            m_angularDeflectionTimescale = vd.m_angularDeflectionTimescale;
+            if (m_angularDeflectionTimescale < m_timestep) m_angularDeflectionTimescale = m_timestep;
+
+            m_linearDeflectionEfficiency = vd.m_linearDeflectionEfficiency;
+            m_linearDeflectionTimescale = vd.m_linearDeflectionTimescale;
+            if (m_linearDeflectionTimescale < m_timestep) m_linearDeflectionTimescale = m_timestep;
+
+            //Banking properties
+            m_bankingEfficiency = vd.m_bankingEfficiency;
+            m_bankingMix = vd.m_bankingMix;
+            m_bankingTimescale = vd.m_bankingTimescale;
+            if (m_bankingTimescale < m_timestep) m_bankingTimescale = m_timestep;
+
+            //Hover and Buoyancy properties
+            m_VhoverHeight = vd.m_VhoverHeight;
+            m_VhoverEfficiency = vd.m_VhoverEfficiency;
+            m_VhoverTimescale = vd.m_VhoverTimescale;
+            if (m_VhoverTimescale < m_timestep) m_VhoverTimescale = m_timestep;
+
+            m_VehicleBuoyancy = vd.m_VehicleBuoyancy;
+
+            //Attractor properties
+            m_verticalAttractionEfficiency = vd.m_verticalAttractionEfficiency;
+            m_verticalAttractionTimescale = vd.m_verticalAttractionTimescale;
+            if (m_verticalAttractionTimescale < m_timestep) m_verticalAttractionTimescale = m_timestep;
+
+            // Axis
+            m_referenceFrame = vd.m_referenceFrame;
+
+            m_lmEfect = 0;
+            m_lmDecay = (1.0f - 1.0f / m_linearMotorDecayTimescale);
+            m_amEfect = 0;
+            m_ffactor = 1.0f;
+        }
+
+        internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue)
+        {
+            float len;
+            if(float.IsNaN(pValue) || float.IsInfinity(pValue))
+                return;
+
+            switch (pParam)
+            {
+                case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY:
+                    if (pValue < 0f) pValue = 0f;
+                    if (pValue > 1f) pValue = 1f;
+                    m_angularDeflectionEfficiency = pValue;
+                    break;
+                case Vehicle.ANGULAR_DEFLECTION_TIMESCALE:
+                    if (pValue < m_timestep) pValue = m_timestep;
+                    m_angularDeflectionTimescale = pValue;
+                    break;
+                case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE:
+                    if (pValue < m_timestep) pValue = m_timestep;
+                    else if (pValue > 120) pValue = 120;
+                    m_angularMotorDecayTimescale = pValue * m_invtimestep;
+                    m_amDecay = 1.0f - 1.0f / m_angularMotorDecayTimescale;
+                    break;
+                case Vehicle.ANGULAR_MOTOR_TIMESCALE:
+                    if (pValue < m_timestep) pValue = m_timestep;
+                    m_angularMotorTimescale = pValue;
+                    break;
+                case Vehicle.BANKING_EFFICIENCY:
+                    if (pValue < -1f) pValue = -1f;
+                    if (pValue > 1f) pValue = 1f;
+                    m_bankingEfficiency = pValue;
+                    break;
+                case Vehicle.BANKING_MIX:
+                    if (pValue < 0f) pValue = 0f;
+                    if (pValue > 1f) pValue = 1f;
+                    m_bankingMix = pValue;
+                    break;
+                case Vehicle.BANKING_TIMESCALE:
+                    if (pValue < m_timestep) pValue = m_timestep;
+                    m_bankingTimescale = pValue;
+                    break;
+                case Vehicle.BUOYANCY:
+                    if (pValue < -1f) pValue = -1f;
+                    if (pValue > 1f) pValue = 1f;
+                    m_VehicleBuoyancy = pValue;
+                    break;
+                case Vehicle.HOVER_EFFICIENCY:
+                    if (pValue < 0f) pValue = 0f;
+                    if (pValue > 1f) pValue = 1f;
+                    m_VhoverEfficiency = pValue;
+                    break;
+                case Vehicle.HOVER_HEIGHT:
+                    m_VhoverHeight = pValue;
+                    break;
+                case Vehicle.HOVER_TIMESCALE:
+                    if (pValue < m_timestep) pValue = m_timestep;
+                    m_VhoverTimescale = pValue;
+                    break;
+                case Vehicle.LINEAR_DEFLECTION_EFFICIENCY:
+                    if (pValue < 0f) pValue = 0f;
+                    if (pValue > 1f) pValue = 1f;
+                    m_linearDeflectionEfficiency = pValue;
+                    break;
+                case Vehicle.LINEAR_DEFLECTION_TIMESCALE:
+                    if (pValue < m_timestep) pValue = m_timestep;
+                    m_linearDeflectionTimescale = pValue;
+                    break;
+                case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE:
+                    if (pValue < m_timestep) pValue = m_timestep;
+                    else if (pValue > 120) pValue = 120;
+                    m_linearMotorDecayTimescale = (0.2f +pValue) * m_invtimestep;
+                    m_lmDecay = (1.0f - 1.0f / m_linearMotorDecayTimescale);
+                    break;
+                case Vehicle.LINEAR_MOTOR_TIMESCALE:
+                    if (pValue < m_timestep) pValue = m_timestep;
+                    m_linearMotorTimescale = pValue;
+                    break;
+                case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY:
+                    if (pValue < 0f) pValue = 0f;
+                    if (pValue > 1f) pValue = 1f;
+                    m_verticalAttractionEfficiency = pValue;
+                    break;
+                case Vehicle.VERTICAL_ATTRACTION_TIMESCALE:
+                    if (pValue < m_timestep) pValue = m_timestep;
+                    m_verticalAttractionTimescale = pValue;
+                    break;
+
+                // These are vector properties but the engine lets you use a single float value to
+                // set all of the components to the same value
+                case Vehicle.ANGULAR_FRICTION_TIMESCALE:
+                    if (pValue < m_timestep) pValue = m_timestep;
+                    m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue);
+                    break;
+                case Vehicle.ANGULAR_MOTOR_DIRECTION:
+                    m_angularMotorDirection = new Vector3(pValue, pValue, pValue);
+                    len = m_angularMotorDirection.Length();
+                    if (len > 12.566f)
+                        m_angularMotorDirection *= (12.566f / len);
+
+                    m_amEfect = 1.0f ; // turn it on
+                    m_amDecay = 1.0f - 1.0f / m_angularMotorDecayTimescale;
+
+                    if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body)
+                            && !rootPrim.m_isSelected && !rootPrim.m_disabled)
+                        d.BodyEnable(rootPrim.Body);
+
+                    break;
+                case Vehicle.LINEAR_FRICTION_TIMESCALE:
+                    if (pValue < m_timestep) pValue = m_timestep;
+                    m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue);
+                    break;
+                case Vehicle.LINEAR_MOTOR_DIRECTION:
+                    m_linearMotorDirection = new Vector3(pValue, pValue, pValue);
+                    len = m_linearMotorDirection.Length();
+                    if (len > 100.0f)
+                        m_linearMotorDirection *= (100.0f / len);
+
+                    m_lmDecay = 1.0f - 1.0f / m_linearMotorDecayTimescale;
+                    m_lmEfect = 1.0f; // turn it on
+
+                    m_ffactor = 0.0f;
+                    if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body)
+                            && !rootPrim.m_isSelected && !rootPrim.m_disabled)
+                        d.BodyEnable(rootPrim.Body);
+                    break;
+                case Vehicle.LINEAR_MOTOR_OFFSET:
+                    m_linearMotorOffset = new Vector3(pValue, pValue, pValue);
+                    len = m_linearMotorOffset.Length();
+                    if (len > 100.0f)
+                        m_linearMotorOffset *= (100.0f / len);
+                    break;
+            }
+        }//end ProcessFloatVehicleParam
+
+        internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue)
+        {
+            float len;
+            if(!pValue.IsFinite())
+                return;
+
+            switch (pParam)
+            {
+                case Vehicle.ANGULAR_FRICTION_TIMESCALE:
+                    if (pValue.X < m_timestep) pValue.X = m_timestep;
+                    if (pValue.Y < m_timestep) pValue.Y = m_timestep;
+                    if (pValue.Z < m_timestep) pValue.Z = m_timestep;
+
+                    m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
+                    break;
+                case Vehicle.ANGULAR_MOTOR_DIRECTION:
+                    m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
+                    // Limit requested angular speed to 2 rps= 4 pi rads/sec
+                    len = m_angularMotorDirection.Length();
+                    if (len > 12.566f)
+                        m_angularMotorDirection *= (12.566f / len);
+
+                    m_amEfect = 1.0f; // turn it on
+                    m_amDecay = 1.0f - 1.0f / m_angularMotorDecayTimescale;
+
+                    if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body)
+                            && !rootPrim.m_isSelected && !rootPrim.m_disabled)
+                        d.BodyEnable(rootPrim.Body);
+                    break;
+                case Vehicle.LINEAR_FRICTION_TIMESCALE:
+                    if (pValue.X < m_timestep) pValue.X = m_timestep;
+                    if (pValue.Y < m_timestep) pValue.Y = m_timestep;
+                    if (pValue.Z < m_timestep) pValue.Z = m_timestep;
+                    m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
+                    break;
+                case Vehicle.LINEAR_MOTOR_DIRECTION:
+                    m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
+                    len = m_linearMotorDirection.Length();
+                    if (len > 100.0f)
+                        m_linearMotorDirection *= (100.0f / len);
+
+                    m_lmEfect = 1.0f; // turn it on
+                    m_lmDecay = 1.0f - 1.0f / m_linearMotorDecayTimescale;
+
+                    m_ffactor = 0.0f;
+                    if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body)
+                            && !rootPrim.m_isSelected && !rootPrim.m_disabled)
+                        d.BodyEnable(rootPrim.Body);
+                    break;
+                case Vehicle.LINEAR_MOTOR_OFFSET:
+                    m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z);
+                    len = m_linearMotorOffset.Length();
+                    if (len > 100.0f)
+                        m_linearMotorOffset *= (100.0f / len);
+                    break;
+                case Vehicle.BLOCK_EXIT:
+                    m_BlockingEndPoint = new Vector3(pValue.X, pValue.Y, pValue.Z);
+                    break;
+            }
+        }//end ProcessVectorVehicleParam
+
+        internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue)
+        {
+            switch (pParam)
+            {
+                case Vehicle.REFERENCE_FRAME:
+                    //                    m_referenceFrame = Quaternion.Inverse(pValue);
+                    m_referenceFrame = pValue;
+                    break;
+                case Vehicle.ROLL_FRAME:
+                    m_RollreferenceFrame = pValue;
+                    break;
+            }
+        }//end ProcessRotationVehicleParam
+
+        internal void ProcessVehicleFlags(int pParam, bool remove)
+        {
+            if (remove)
+            {
+                m_flags &= ~((VehicleFlag)pParam);
+            }
+            else
+            {
+                m_flags |= (VehicleFlag)pParam;
+            }
+        }//end ProcessVehicleFlags
+
+        internal void ProcessTypeChange(Vehicle pType)
+        {
+            m_lmEfect = 0;
+
+            m_amEfect = 0;
+            m_ffactor = 1f;
+
+            m_linearMotorDirection = Vector3.Zero;
+            m_angularMotorDirection = Vector3.Zero;
+
+            m_BlockingEndPoint = Vector3.Zero;
+            m_RollreferenceFrame = Quaternion.Identity;
+            m_linearMotorOffset = Vector3.Zero;
+
+            m_referenceFrame = Quaternion.Identity;
+
+            // Set Defaults For Type
+            m_type = pType;
+            switch (pType)
+            {
+                case Vehicle.TYPE_NONE:
+                    m_linearFrictionTimescale = new Vector3(1000, 1000, 1000);
+                    m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
+                    m_linearMotorTimescale = 1000;
+                    m_linearMotorDecayTimescale = 120 * m_invtimestep;
+                    m_angularMotorTimescale = 1000;
+                    m_angularMotorDecayTimescale = 1000  * m_invtimestep;
+                    m_VhoverHeight = 0;
+                    m_VhoverEfficiency = 1;
+                    m_VhoverTimescale = 1000;
+                    m_VehicleBuoyancy = 0;
+                    m_linearDeflectionEfficiency = 0;
+                    m_linearDeflectionTimescale = 1000;
+                    m_angularDeflectionEfficiency = 0;
+                    m_angularDeflectionTimescale = 1000;
+                    m_bankingEfficiency = 0;
+                    m_bankingMix = 1;
+                    m_bankingTimescale = 1000;
+                    m_verticalAttractionEfficiency = 0;
+                    m_verticalAttractionTimescale = 1000;
+
+                    m_flags = (VehicleFlag)0;
+                    break;
+
+                case Vehicle.TYPE_SLED:
+                    m_linearFrictionTimescale = new Vector3(30, 1, 1000);
+                    m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
+                    m_linearMotorTimescale = 1000;
+                    m_linearMotorDecayTimescale = 120 * m_invtimestep;
+                    m_angularMotorTimescale = 1000;
+                    m_angularMotorDecayTimescale = 120 * m_invtimestep;
+                    m_VhoverHeight = 0;
+                    m_VhoverEfficiency = 1;
+                    m_VhoverTimescale = 10;
+                    m_VehicleBuoyancy = 0;
+                    m_linearDeflectionEfficiency = 1;
+                    m_linearDeflectionTimescale = 1;
+                    m_angularDeflectionEfficiency = 0;
+                    m_angularDeflectionTimescale = 10;
+                    m_verticalAttractionEfficiency = 1;
+                    m_verticalAttractionTimescale = 1000;
+                    m_bankingEfficiency = 0;
+                    m_bankingMix = 1;
+                    m_bankingTimescale = 10;
+                    m_flags &=
+                         ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
+                           VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
+                    m_flags |= (VehicleFlag.NO_DEFLECTION_UP |
+                        VehicleFlag.LIMIT_ROLL_ONLY |
+                        VehicleFlag.LIMIT_MOTOR_UP);
+                    break;
+
+                case Vehicle.TYPE_CAR:
+                    m_linearFrictionTimescale = new Vector3(100, 2, 1000);
+                    m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
+                    m_linearMotorTimescale = 1;
+                    m_linearMotorDecayTimescale = 60 * m_invtimestep;
+                    m_angularMotorTimescale = 1;
+                    m_angularMotorDecayTimescale = 0.8f * m_invtimestep;
+                    m_VhoverHeight = 0;
+                    m_VhoverEfficiency = 0;
+                    m_VhoverTimescale = 1000;
+                    m_VehicleBuoyancy = 0;
+                    m_linearDeflectionEfficiency = 1;
+                    m_linearDeflectionTimescale = 2;
+                    m_angularDeflectionEfficiency = 0;
+                    m_angularDeflectionTimescale = 10;
+                    m_verticalAttractionEfficiency = 1f;
+                    m_verticalAttractionTimescale = 10f;
+                    m_bankingEfficiency = -0.2f;
+                    m_bankingMix = 1;
+                    m_bankingTimescale = 1;
+                    m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY |
+                                VehicleFlag.HOVER_TERRAIN_ONLY |
+                                VehicleFlag.HOVER_GLOBAL_HEIGHT);
+                    m_flags |= (VehicleFlag.NO_DEFLECTION_UP |
+                                VehicleFlag.LIMIT_ROLL_ONLY |
+                                VehicleFlag.LIMIT_MOTOR_UP |
+                                VehicleFlag.HOVER_UP_ONLY);
+                    break;
+                case Vehicle.TYPE_BOAT:
+                    m_linearFrictionTimescale = new Vector3(10, 3, 2);
+                    m_angularFrictionTimescale = new Vector3(10, 10, 10);
+                    m_linearMotorTimescale = 5;
+                    m_linearMotorDecayTimescale = 60 * m_invtimestep;
+                    m_angularMotorTimescale = 4;
+                    m_angularMotorDecayTimescale = 4 * m_invtimestep;
+                    m_VhoverHeight = 0;
+                    m_VhoverEfficiency = 0.5f;
+                    m_VhoverTimescale = 2;
+                    m_VehicleBuoyancy = 1;
+                    m_linearDeflectionEfficiency = 0.5f;
+                    m_linearDeflectionTimescale = 3;
+                    m_angularDeflectionEfficiency = 0.5f;
+                    m_angularDeflectionTimescale = 5;
+                    m_verticalAttractionEfficiency = 0.5f;
+                    m_verticalAttractionTimescale = 5f;
+                    m_bankingEfficiency = -0.3f;
+                    m_bankingMix = 0.8f;
+                    m_bankingTimescale = 1;
+                    m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY |
+                            VehicleFlag.HOVER_GLOBAL_HEIGHT |
+                            VehicleFlag.HOVER_UP_ONLY); // |
+//                            VehicleFlag.LIMIT_ROLL_ONLY);
+                    m_flags |= (VehicleFlag.NO_DEFLECTION_UP |
+                                VehicleFlag.LIMIT_MOTOR_UP |
+                                VehicleFlag.HOVER_UP_ONLY |  // new sl
+                                VehicleFlag.HOVER_WATER_ONLY);
+                    break;
+
+                case Vehicle.TYPE_AIRPLANE:
+                    m_linearFrictionTimescale = new Vector3(200, 10, 5);
+                    m_angularFrictionTimescale = new Vector3(20, 20, 20);
+                    m_linearMotorTimescale = 2;
+                    m_linearMotorDecayTimescale = 60 * m_invtimestep;
+                    m_angularMotorTimescale = 4;
+                    m_angularMotorDecayTimescale = 8 * m_invtimestep;
+                    m_VhoverHeight = 0;
+                    m_VhoverEfficiency = 0.5f;
+                    m_VhoverTimescale = 1000;
+                    m_VehicleBuoyancy = 0;
+                    m_linearDeflectionEfficiency = 0.5f;
+                    m_linearDeflectionTimescale = 0.5f;
+                    m_angularDeflectionEfficiency = 1;
+                    m_angularDeflectionTimescale = 2;
+                    m_verticalAttractionEfficiency = 0.9f;
+                    m_verticalAttractionTimescale = 2f;
+                    m_bankingEfficiency = 1;
+                    m_bankingMix = 0.7f;
+                    m_bankingTimescale = 2;
+                    m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY |
+                        VehicleFlag.HOVER_TERRAIN_ONLY |
+                        VehicleFlag.HOVER_GLOBAL_HEIGHT |
+                        VehicleFlag.HOVER_UP_ONLY |
+                        VehicleFlag.NO_DEFLECTION_UP |
+                        VehicleFlag.LIMIT_MOTOR_UP);
+                    m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY);
+                    break;
+
+                case Vehicle.TYPE_BALLOON:
+                    m_linearFrictionTimescale = new Vector3(5, 5, 5);
+                    m_angularFrictionTimescale = new Vector3(10, 10, 10);
+                    m_linearMotorTimescale = 5;
+                    m_linearMotorDecayTimescale = 60 * m_invtimestep;
+                    m_angularMotorTimescale = 6;
+                    m_angularMotorDecayTimescale = 10 * m_invtimestep;
+                    m_VhoverHeight = 5;
+                    m_VhoverEfficiency = 0.8f;
+                    m_VhoverTimescale = 10;
+                    m_VehicleBuoyancy = 1;
+                    m_linearDeflectionEfficiency = 0;
+                    m_linearDeflectionTimescale = 5 * m_invtimestep;
+                    m_angularDeflectionEfficiency = 0;
+                    m_angularDeflectionTimescale = 5;
+                    m_verticalAttractionEfficiency = 1f;
+                    m_verticalAttractionTimescale = 1000f;
+                    m_bankingEfficiency = 0;
+                    m_bankingMix = 0.7f;
+                    m_bankingTimescale = 5;
+                    m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY |
+                        VehicleFlag.HOVER_TERRAIN_ONLY |
+                        VehicleFlag.HOVER_UP_ONLY |
+                        VehicleFlag.NO_DEFLECTION_UP |
+                        VehicleFlag.LIMIT_MOTOR_UP  | //);
+                        VehicleFlag.LIMIT_ROLL_ONLY | // new sl
+                        VehicleFlag.HOVER_GLOBAL_HEIGHT); // new sl
+
+//                    m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY |
+//                        VehicleFlag.HOVER_GLOBAL_HEIGHT);
+                    break;
+
+            }
+            // disable mouse steering
+            m_flags &= ~(VehicleFlag.MOUSELOOK_STEER |
+                         VehicleFlag.MOUSELOOK_BANK  |
+                         VehicleFlag.CAMERA_DECOUPLED);
+
+            m_lmDecay = (1.0f - 1.0f / m_linearMotorDecayTimescale);
+            m_amDecay = 1.0f - 1.0f / m_angularMotorDecayTimescale;
+
+        }//end SetDefaultsForType
+
+        internal void Stop()
+        {
+            m_lmEfect = 0;
+            m_lmDecay = 0f;
+            m_amEfect = 0;
+            m_amDecay = 0;
+            m_ffactor = 1f;
+        }
+
+        public static Vector3 Xrot(Quaternion rot)
+        {
+            Vector3 vec;
+            rot.Normalize(); // just in case
+            vec.X = 2 * (rot.X * rot.X + rot.W * rot.W) - 1;
+            vec.Y = 2 * (rot.X * rot.Y + rot.Z * rot.W);
+            vec.Z = 2 * (rot.X * rot.Z - rot.Y * rot.W);
+            return vec;
+        }
+
+        public static Vector3 Zrot(Quaternion rot)
+        {
+            Vector3 vec;
+            rot.Normalize(); // just in case
+            vec.X = 2 * (rot.X * rot.Z + rot.Y * rot.W);
+            vec.Y = 2 * (rot.Y * rot.Z - rot.X * rot.W);
+            vec.Z = 2 * (rot.Z * rot.Z + rot.W * rot.W) - 1;
+
+            return vec;
+        }
+
+        private const float pi = (float)Math.PI;
+        private const float halfpi = 0.5f * (float)Math.PI;
+        private const float twopi = 2.0f * pi;
+
+        public static Vector3 ubRot2Euler(Quaternion rot)
+        {
+            // returns roll in X
+            //         pitch in Y
+            //         yaw in Z
+            Vector3 vec;
+
+            // assuming rot is normalised
+            // rot.Normalize();
+
+            float zX = rot.X * rot.Z + rot.Y * rot.W;
+
+            if (zX < -0.49999f)
+            {
+                vec.X = 0;
+                vec.Y = -halfpi;
+                vec.Z = (float)(-2d * Math.Atan(rot.X / rot.W));
+            }
+            else if (zX > 0.49999f)
+            {
+                vec.X = 0;
+                vec.Y = halfpi;
+                vec.Z = (float)(2d * Math.Atan(rot.X / rot.W));
+            }
+            else
+            {
+                vec.Y = (float)Math.Asin(2 * zX);
+
+                float sqw = rot.W * rot.W;
+
+                float minuszY = rot.X * rot.W - rot.Y * rot.Z;
+                float zZ = rot.Z * rot.Z + sqw - 0.5f;
+
+                vec.X = (float)Math.Atan2(minuszY, zZ);
+
+                float yX = rot.Z * rot.W - rot.X * rot.Y; //( have negative ?)
+                float yY = rot.X * rot.X + sqw - 0.5f;
+                vec.Z = (float)Math.Atan2(yX, yY);
+            }
+            return vec;
+        }
+
+        public static void GetRollPitch(Quaternion rot, out float roll, out float pitch)
+        {
+            // assuming rot is normalised
+            // rot.Normalize();
+
+            float zX = rot.X * rot.Z + rot.Y * rot.W;
+
+            if (zX < -0.49999f)
+            {
+                roll = 0;
+                pitch = -halfpi;
+            }
+            else if (zX > 0.49999f)
+            {
+                roll = 0;
+                pitch = halfpi;
+            }
+            else
+            {
+                pitch = (float)Math.Asin(2 * zX);
+
+                float minuszY = rot.X * rot.W - rot.Y * rot.Z;
+                float zZ = rot.Z * rot.Z + rot.W * rot.W - 0.5f;
+
+                roll = (float)Math.Atan2(minuszY, zZ);
+            }
+            return ;
+        }
+
+        internal void Step()
+        {
+            IntPtr Body = rootPrim.Body;
+
+            d.Mass dmass;
+            d.BodyGetMass(Body, out dmass);
+
+            d.Quaternion rot = d.BodyGetQuaternion(Body);
+            Quaternion objrotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);    // rotq = rotation of object
+            Quaternion rotq = objrotq;    // rotq = rotation of object
+            rotq *= m_referenceFrame; // rotq is now rotation in vehicle reference frame
+            Quaternion irotq = Quaternion.Inverse(rotq);
+
+            d.Vector3 dvtmp;
+            Vector3 tmpV;
+            Vector3 curVel; // velocity in world
+            Vector3 curAngVel; // angular velocity in world
+            Vector3 force = Vector3.Zero; // actually linear aceleration until mult by mass in world frame
+            Vector3 torque = Vector3.Zero;// actually angular aceleration until mult by Inertia in vehicle frame
+            d.Vector3 dtorque = new d.Vector3();
+
+            dvtmp = d.BodyGetLinearVel(Body);
+            curVel.X = dvtmp.X;
+            curVel.Y = dvtmp.Y;
+            curVel.Z = dvtmp.Z;
+            Vector3 curLocalVel = curVel * irotq; // current velocity in  local
+
+            dvtmp = d.BodyGetAngularVel(Body);
+            curAngVel.X = dvtmp.X;
+            curAngVel.Y = dvtmp.Y;
+            curAngVel.Z = dvtmp.Z;
+            Vector3 curLocalAngVel = curAngVel * irotq; // current angular velocity in  local
+
+            float ldampZ = 0;
+
+            bool mousemode = false;
+            bool mousemodebank = false;
+
+            float bankingEfficiency;
+            float verticalAttractionTimescale = m_verticalAttractionTimescale;
+
+            if((m_flags & (VehicleFlag.MOUSELOOK_STEER | VehicleFlag.MOUSELOOK_BANK)) != 0 )
+            {
+                mousemode = true;
+                mousemodebank = (m_flags & VehicleFlag.MOUSELOOK_BANK) != 0;
+                if(mousemodebank)
+                {
+                    bankingEfficiency = m_bankingEfficiency;
+                    if(verticalAttractionTimescale < 149.9)
+                        verticalAttractionTimescale *= 2.0f; // reduce current instability
+                }
+                else
+                    bankingEfficiency = 0;
+            }
+            else
+                bankingEfficiency = m_bankingEfficiency;
+
+            // linear motor
+            if (m_lmEfect > 0.01 && m_linearMotorTimescale < 1000)
+            {
+                tmpV = m_linearMotorDirection - curLocalVel; // velocity error
+                tmpV *= m_lmEfect / m_linearMotorTimescale; // error to correct in this timestep
+                tmpV *= rotq; // to world
+
+                if ((m_flags & VehicleFlag.LIMIT_MOTOR_UP) != 0)
+                    tmpV.Z = 0;
+
+                if (m_linearMotorOffset.X != 0 || m_linearMotorOffset.Y != 0 || m_linearMotorOffset.Z != 0)
+                {
+                    // have offset, do it now
+                    tmpV *= dmass.mass;
+                    d.BodyAddForceAtRelPos(Body, tmpV.X, tmpV.Y, tmpV.Z, m_linearMotorOffset.X, m_linearMotorOffset.Y, m_linearMotorOffset.Z);
+                }
+                else
+                {
+                    force.X += tmpV.X;
+                    force.Y += tmpV.Y;
+                    force.Z += tmpV.Z;
+                }
+
+                m_lmEfect *= m_lmDecay;
+//                m_ffactor = 0.01f + 1e-4f * curVel.LengthSquared();
+                m_ffactor = 0.0f;
+            }
+            else
+            {
+                m_lmEfect = 0;
+                m_ffactor = 1f;
+            }
+
+            // hover
+            if (m_VhoverTimescale < 300 && rootPrim.prim_geom != IntPtr.Zero)
+            {
+                //                d.Vector3 pos = d.BodyGetPosition(Body);
+                d.Vector3 pos = d.GeomGetPosition(rootPrim.prim_geom);
+                pos.Z -= 0.21f; // minor offset that seems to be always there in sl
+
+                float t = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y);
+                float perr;
+
+                // default to global but don't go underground
+                perr = m_VhoverHeight - pos.Z;
+
+                if ((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == 0)
+                {
+                    if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) != 0)
+                    {
+                        perr += _pParentScene.GetWaterLevel();
+                    }
+                    else if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0)
+                    {
+                        perr += t;
+                    }
+                    else
+                    {
+                        float w = _pParentScene.GetWaterLevel();
+                        if (t > w)
+                            perr += t;
+                        else
+                            perr += w;
+                    }
+                }
+                else if (t > m_VhoverHeight)
+                        perr = t - pos.Z; ;
+
+                if ((m_flags & VehicleFlag.HOVER_UP_ONLY) == 0 || perr > -0.1)
+                {
+                    ldampZ = m_VhoverEfficiency * m_invtimestep;
+
+                    perr *= (1.0f + ldampZ) / m_VhoverTimescale;
+
+                    //                    force.Z += perr - curVel.Z * tmp;
+                    force.Z += perr;
+                    ldampZ *= -curVel.Z;
+
+                    force.Z += _pParentScene.gravityz * m_gravmod * (1f - m_VehicleBuoyancy);
+                }
+                else // no buoyancy
+                    force.Z += _pParentScene.gravityz;
+            }
+            else
+            {
+                // default gravity and Buoyancy
+                force.Z += _pParentScene.gravityz * m_gravmod * (1f - m_VehicleBuoyancy);
+            }
+
+            // linear deflection
+            if (m_linearDeflectionEfficiency > 0)
+            {
+                float len = curVel.Length();
+                if (len > 0.01) // if moving
+                {
+                    Vector3 atAxis;
+                    atAxis = Xrot(rotq); // where are we pointing to
+                    atAxis *= len; // make it same size as world velocity vector
+
+                    tmpV = -atAxis; // oposite direction
+                    atAxis -= curVel; // error to one direction
+                    len = atAxis.LengthSquared();
+
+                    tmpV -= curVel; // error to oposite
+                    float lens = tmpV.LengthSquared();
+
+                    if (len > 0.01 || lens > 0.01) // do nothing if close enougth
+                    {
+                        if (len < lens)
+                            tmpV = atAxis;
+
+                        tmpV *= (m_linearDeflectionEfficiency / m_linearDeflectionTimescale); // error to correct in this timestep
+                        force.X += tmpV.X;
+                        force.Y += tmpV.Y;
+                        if ((m_flags & VehicleFlag.NO_DEFLECTION_UP) == 0)
+                            force.Z += tmpV.Z;
+                    }
+                }
+            }
+
+            // linear friction/damping
+            if (curLocalVel.X != 0 || curLocalVel.Y != 0 || curLocalVel.Z != 0)
+            {
+                tmpV.X = -curLocalVel.X / m_linearFrictionTimescale.X;
+                tmpV.Y = -curLocalVel.Y / m_linearFrictionTimescale.Y;
+                tmpV.Z = -curLocalVel.Z / m_linearFrictionTimescale.Z;
+                tmpV *= rotq; // to world
+
+                if(ldampZ != 0 && Math.Abs(ldampZ) > Math.Abs(tmpV.Z))
+                    tmpV.Z = ldampZ;
+                force.X += tmpV.X;
+                force.Y += tmpV.Y;
+                force.Z += tmpV.Z;
+            }
+
+            // vertical atractor
+            if (verticalAttractionTimescale < 300)
+            {
+                float roll;
+                float pitch;
+
+                float ftmp = m_invtimestep / verticalAttractionTimescale / verticalAttractionTimescale;
+
+                float ftmp2;
+                ftmp2 = 0.5f * m_verticalAttractionEfficiency * m_invtimestep;
+                m_amdampX = ftmp2;
+
+                m_ampwr = 1.0f - 0.8f * m_verticalAttractionEfficiency;
+
+                GetRollPitch(irotq, out roll, out pitch);
+
+                if (roll > halfpi)
+                    roll = pi - roll;
+                else if (roll < -halfpi)
+                    roll = -pi - roll;
+
+                float effroll = pitch / halfpi;
+                effroll *= effroll;
+                effroll = 1 - effroll;
+                effroll *= roll;
+
+                torque.X += effroll * ftmp;
+
+                if ((m_flags & VehicleFlag.LIMIT_ROLL_ONLY) == 0)
+                {
+                    float effpitch = roll / halfpi;
+                    effpitch *= effpitch;
+                    effpitch = 1 - effpitch;
+                    effpitch *= pitch;
+
+                    torque.Y += effpitch * ftmp;
+                }
+
+                if (bankingEfficiency != 0 && Math.Abs(effroll) > 0.01)
+                {
+
+                    float broll = effroll;
+                    /*
+                                        if (broll > halfpi)
+                                            broll = pi - broll;
+                                        else if (broll < -halfpi)
+                                            broll = -pi - broll;
+                    */
+                    broll *= m_bankingEfficiency;
+                    if (m_bankingMix != 0)
+                    {
+                        float vfact = Math.Abs(curLocalVel.X) / 10.0f;
+                        if (vfact > 1.0f) vfact = 1.0f;
+
+                        if (curLocalVel.X >= 0)
+                            broll *= (1 + (vfact - 1) * m_bankingMix);
+                        else
+                            broll *= -(1 + (vfact - 1) * m_bankingMix);
+                    }
+                    // make z rot be in world Z not local as seems to be in sl
+
+                    broll = broll / m_bankingTimescale;
+
+
+                    tmpV = Zrot(irotq);
+                    tmpV *= broll;
+
+                    torque.X += tmpV.X;
+                    torque.Y += tmpV.Y;
+                    torque.Z += tmpV.Z;
+
+                    m_amdampZ = Math.Abs(m_bankingEfficiency) / m_bankingTimescale;
+                    m_amdampY = m_amdampZ;
+
+                }
+                else
+                {
+                    m_amdampZ = 1 / m_angularFrictionTimescale.Z;
+                    m_amdampY = m_amdampX;
+                }
+            }
+            else
+            {
+                m_ampwr = 1.0f;
+                m_amdampX = 1 / m_angularFrictionTimescale.X;
+                m_amdampY = 1 / m_angularFrictionTimescale.Y;
+                m_amdampZ = 1 / m_angularFrictionTimescale.Z;
+            }
+
+            if(mousemode)
+            {
+                CameraData cam = rootPrim.TryGetCameraData();
+                if(cam.Valid && cam.MouseLook)
+                {
+                    Vector3 dirv = cam.CameraAtAxis * irotq;
+
+                    float invamts = 1.0f/m_angularMotorTimescale;
+                    float tmp;
+
+                    // get out of x == 0 plane
+                    if(Math.Abs(dirv.X) < 0.001f)
+                        dirv.X = 0.001f;
+
+                    if (Math.Abs(dirv.Z) > 0.01)
+                    {
+                        tmp = -(float)Math.Atan2(dirv.Z, dirv.X) * m_angularMotorDirection.Y;
+                        if(tmp < -4f)
+                            tmp = -4f;
+                        else if(tmp > 4f)
+                            tmp = 4f;
+                        torque.Y += (tmp - curLocalAngVel.Y) * invamts;
+                        torque.Y -= curLocalAngVel.Y * m_amdampY;
+                    }
+                    else
+                        torque.Y -= curLocalAngVel.Y * m_invtimestep;
+
+                    if (Math.Abs(dirv.Y) > 0.01)
+                    {
+                        if(mousemodebank)
+                        {
+                            tmp = -(float)Math.Atan2(dirv.Y, dirv.X) * m_angularMotorDirection.X;
+                            if(tmp < -4f)
+                                tmp = -4f;
+                            else if(tmp > 4f)
+                                tmp = 4f;
+                            torque.X += (tmp - curLocalAngVel.X) * invamts;
+                        }
+                        else
+                        {
+                            tmp = (float)Math.Atan2(dirv.Y, dirv.X) * m_angularMotorDirection.Z;
+                            tmp *= invamts;
+                            if(tmp < -4f)
+                                tmp = -4f;
+                            else if(tmp > 4f)
+                                tmp = 4f;
+                            torque.Z += (tmp - curLocalAngVel.Z) * invamts;
+                        }
+                        torque.X -= curLocalAngVel.X * m_amdampX;
+                        torque.Z -= curLocalAngVel.Z * m_amdampZ;
+                    }
+                    else
+                    {
+                        if(mousemodebank)
+                            torque.X -= curLocalAngVel.X * m_invtimestep;
+                        else
+                            torque.Z -= curLocalAngVel.Z * m_invtimestep;
+                    }
+                }
+                else
+                {
+                    if (curLocalAngVel.X != 0 || curLocalAngVel.Y != 0 || curLocalAngVel.Z != 0)
+                    {
+                        torque.X -= curLocalAngVel.X * 10f;
+                        torque.Y -= curLocalAngVel.Y * 10f;
+                        torque.Z -= curLocalAngVel.Z * 10f;
+                    }
+                }
+            }
+            else
+            {
+                // angular motor
+                if (m_amEfect > 0.01 && m_angularMotorTimescale < 1000)
+                {
+                    tmpV = m_angularMotorDirection - curLocalAngVel; // velocity error
+                    tmpV *= m_amEfect / m_angularMotorTimescale; // error to correct in this timestep
+                    torque.X += tmpV.X * m_ampwr;
+                    torque.Y += tmpV.Y * m_ampwr;
+                    torque.Z += tmpV.Z;
+
+                    m_amEfect *= m_amDecay;
+                }
+                else
+                    m_amEfect = 0;
+
+                // angular deflection
+                if (m_angularDeflectionEfficiency > 0)
+                {
+                    Vector3 dirv;
+
+                    if (curLocalVel.X > 0.01f)
+                        dirv = curLocalVel;
+                    else if (curLocalVel.X < -0.01f)
+                        // use oposite
+                        dirv = -curLocalVel;
+                    else
+                    {
+                        // make it fall into small positive x case
+                        dirv.X = 0.01f;
+                        dirv.Y = curLocalVel.Y;
+                        dirv.Z = curLocalVel.Z;
+                    }
+
+                    float ftmp = m_angularDeflectionEfficiency / m_angularDeflectionTimescale;
+
+                    if (Math.Abs(dirv.Z) > 0.01)
+                    {
+                        torque.Y += - (float)Math.Atan2(dirv.Z, dirv.X) * ftmp;
+                    }
+
+                    if (Math.Abs(dirv.Y) > 0.01)
+                    {
+                        torque.Z += (float)Math.Atan2(dirv.Y, dirv.X) * ftmp;
+                    }
+                }
+
+                if (curLocalAngVel.X != 0 || curLocalAngVel.Y != 0 || curLocalAngVel.Z != 0)
+                {
+                    torque.X -= curLocalAngVel.X * m_amdampX;
+                    torque.Y -= curLocalAngVel.Y * m_amdampY;
+                    torque.Z -= curLocalAngVel.Z * m_amdampZ;
+                }
+            }
+
+            force *= dmass.mass;
+
+            force += rootPrim.m_force;
+            force += rootPrim.m_forceacc;
+            rootPrim.m_forceacc = Vector3.Zero;
+
+            if (force.X != 0 || force.Y != 0 || force.Z != 0)
+            {
+                d.BodyAddForce(Body, force.X, force.Y, force.Z);
+            }
+
+            if (torque.X != 0 || torque.Y != 0 || torque.Z != 0)
+            {
+                torque *= m_referenceFrame; // to object frame
+                dtorque.X = torque.X ;
+                dtorque.Y = torque.Y;
+                dtorque.Z = torque.Z;
+
+                d.MultiplyM3V3(out dvtmp, ref dmass.I, ref dtorque);
+                d.BodyAddRelTorque(Body, dvtmp.X, dvtmp.Y, dvtmp.Z); // add torque in object frame
+            }
+
+            torque = rootPrim.m_torque;
+            torque += rootPrim.m_angularForceacc;
+            rootPrim.m_angularForceacc = Vector3.Zero;
+            if (torque.X != 0 || torque.Y != 0 || torque.Z != 0)
+                d.BodyAddTorque(Body,torque.X, torque.Y, torque.Z);
+        }
+    }
+}
diff --git a/OpenSim/Region/PhysicsModules/ubOde/ODEMeshWorker.cs b/OpenSim/Region/PhysicsModules/ubOde/ODEMeshWorker.cs
new file mode 100644
index 0000000..5465035
--- /dev/null
+++ b/OpenSim/Region/PhysicsModules/ubOde/ODEMeshWorker.cs
@@ -0,0 +1,946 @@
+/*
+ * AJLDuarte 2012
+ */
+
+using System;
+using System.Threading;
+using System.Collections.Generic;
+using System.IO;
+using System.Reflection;
+using System.Runtime.InteropServices;
+using System.Text;
+using OpenSim.Framework;
+using OpenSim.Region.PhysicsModules.SharedBase;
+using OdeAPI;
+using log4net;
+using Nini.Config;
+using OpenMetaverse;
+
+namespace OpenSim.Region.PhysicsModule.ubOde
+{
+    public enum MeshState : byte
+    {
+        noNeed = 0,
+
+        loadingAsset = 1,
+
+        AssetOK = 0x0f,     // 00001111
+
+        NeedMask = 0x30,    // 00110000
+        needMesh = 0x10,    // 00010000
+        needAsset = 0x20,   // 00100000
+
+        FailMask = 0xC0,    // 11000000
+        AssetFailed = 0x40, // 01000000
+        MeshFailed = 0x80,   // 10000000
+
+        MeshNoColide = FailMask | needAsset
+    }
+
+    public enum meshWorkerCmnds : byte
+    {
+        nop = 0,
+        addnew,
+        changefull,
+        changesize,
+        changeshapetype,
+        getmesh,
+    }
+
+    public class ODEPhysRepData
+    {
+        public PhysicsActor actor;
+        public PrimitiveBaseShape pbs;
+        public IMesh mesh;
+
+        public Vector3 size;
+        public Vector3 OBB;
+        public Vector3 OBBOffset;
+
+        public float volume;
+
+        public byte shapetype;
+        public bool hasOBB;
+        public bool hasMeshVolume;
+        public bool isTooSmall;
+        public MeshState meshState;
+        public UUID? assetID;
+        public meshWorkerCmnds comand;
+    }
+
+    public class ODEMeshWorker
+    {
+        private ILog m_log;
+        private ODEScene m_scene;
+        private IMesher m_mesher;
+
+        public bool meshSculptedPrim = true;
+        public float meshSculptLOD = 32;
+        public float MeshSculptphysicalLOD = 32;
+        public float MinSizeToMeshmerize = 0.1f;
+
+        private OpenSim.Framework.BlockingQueue<ODEPhysRepData> workQueue = new OpenSim.Framework.BlockingQueue<ODEPhysRepData>();
+        private bool m_running;
+
+        private Thread m_thread;
+
+        public ODEMeshWorker(ODEScene pScene, ILog pLog, IMesher pMesher, IConfig pConfig)
+        {
+            m_scene = pScene;
+            m_log = pLog;
+            m_mesher = pMesher;
+
+            if (pConfig != null)
+            {
+                meshSculptedPrim = pConfig.GetBoolean("mesh_sculpted_prim", meshSculptedPrim);
+                meshSculptLOD = pConfig.GetFloat("mesh_lod", meshSculptLOD);
+                MinSizeToMeshmerize =  pConfig.GetFloat("mesh_min_size", MinSizeToMeshmerize);
+                MeshSculptphysicalLOD = pConfig.GetFloat("mesh_physical_lod", MeshSculptphysicalLOD);
+            }
+            m_running = true;
+            m_thread = new Thread(DoWork);
+            m_thread.Name = "OdeMeshWorker";
+            m_thread.Start();
+        }
+
+        private void DoWork()
+        {
+            m_mesher.ExpireFileCache();
+
+            while(m_running)
+            {
+                 ODEPhysRepData nextRep = workQueue.Dequeue();
+                if(!m_running)
+                    return;
+                if (nextRep == null)
+                    continue;
+                if (m_scene.haveActor(nextRep.actor))
+                {
+                    switch (nextRep.comand)
+                    {
+                        case meshWorkerCmnds.changefull:
+                        case meshWorkerCmnds.changeshapetype:
+                        case meshWorkerCmnds.changesize:
+                            GetMesh(nextRep);
+                            if (CreateActorPhysRep(nextRep) && m_scene.haveActor(nextRep.actor))
+                                m_scene.AddChange(nextRep.actor, changes.PhysRepData, nextRep);
+                            break;
+                        case meshWorkerCmnds.getmesh:
+                            DoRepDataGetMesh(nextRep);
+                            break;
+                    }
+                }
+            }
+        }
+
+        public void Stop()
+        {
+            try
+            {
+                m_thread.Abort();
+                workQueue.Clear();
+            }
+            catch
+            {
+            }
+        }
+
+        public void ChangeActorPhysRep(PhysicsActor actor, PrimitiveBaseShape pbs,
+                                        Vector3 size, byte shapetype)
+        {
+            ODEPhysRepData repData = new ODEPhysRepData();
+            repData.actor = actor;
+            repData.pbs = pbs;
+            repData.size = size;
+            repData.shapetype = shapetype;
+
+            CheckMesh(repData);
+            CalcVolumeData(repData);
+            m_scene.AddChange(actor, changes.PhysRepData, repData);
+            return;
+        }
+
+        public ODEPhysRepData NewActorPhysRep(PhysicsActor actor, PrimitiveBaseShape pbs,
+                                        Vector3 size, byte shapetype)
+        {
+            ODEPhysRepData repData = new ODEPhysRepData();
+            repData.actor = actor;
+            repData.pbs = pbs;
+            repData.size = size;
+            repData.shapetype = shapetype;
+
+            CheckMesh(repData);
+            CalcVolumeData(repData);
+            m_scene.AddChange(actor, changes.AddPhysRep, repData);
+            return repData;
+        }
+
+        public void RequestMesh(ODEPhysRepData repData)
+        {
+            repData.mesh = null;
+
+            if (repData.meshState == MeshState.needAsset)
+            {
+                PrimitiveBaseShape pbs = repData.pbs;
+
+                // check if we got outdated
+
+                if (!pbs.SculptEntry || pbs.SculptTexture == UUID.Zero)
+                {
+                    repData.meshState = MeshState.noNeed;
+                    return;
+                }
+
+                repData.assetID = pbs.SculptTexture;
+                repData.meshState = MeshState.loadingAsset;
+
+                repData.comand = meshWorkerCmnds.getmesh;
+                workQueue.Enqueue(repData);
+            }
+        }
+
+        // creates and prepares a mesh to use and calls parameters estimation
+        public bool CreateActorPhysRep(ODEPhysRepData repData)
+        {
+            IMesh mesh = repData.mesh;
+
+            if (mesh != null)
+            {
+                IntPtr vertices, indices;
+                int vertexCount, indexCount;
+                int vertexStride, triStride;
+
+                mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount);
+                mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount);
+
+                if (vertexCount == 0 || indexCount == 0)
+                {
+                    m_log.WarnFormat("[PHYSICS]: Invalid mesh data on prim {0} mesh UUID {1}",
+                        repData.actor.Name, repData.pbs.SculptTexture.ToString());
+                    repData.meshState = MeshState.MeshFailed;
+                    repData.hasOBB = false;
+                    repData.mesh = null;
+                    m_scene.mesher.ReleaseMesh(mesh);
+                }
+                else
+                {
+                    repData.OBBOffset = mesh.GetCentroid();
+                    repData.OBB = mesh.GetOBB();
+                    repData.hasOBB = true;
+                    mesh.releaseSourceMeshData();
+                }
+            }
+            CalcVolumeData(repData);
+            return true;
+        }
+
+        public void AssetLoaded(ODEPhysRepData repData)
+        {
+            if (m_scene.haveActor(repData.actor))
+            {
+                if (needsMeshing(repData)) // no need for pbs now?
+                {
+                    repData.comand = meshWorkerCmnds.changefull;
+                    workQueue.Enqueue(repData);
+                }
+            }
+            else
+                repData.pbs.SculptData = Utils.EmptyBytes;
+        }
+
+        public void DoRepDataGetMesh(ODEPhysRepData repData)
+        {
+            if (!repData.pbs.SculptEntry)
+                return;
+
+            if (repData.meshState != MeshState.loadingAsset)
+                return;
+
+            if (repData.assetID == null || repData.assetID == UUID.Zero)
+                return;
+
+            if (repData.assetID != repData.pbs.SculptTexture)
+                return;
+
+            // check if it is in cache
+            GetMesh(repData);
+            if (repData.meshState != MeshState.needAsset)
+            {
+                CreateActorPhysRep(repData);
+                m_scene.AddChange(repData.actor, changes.PhysRepData, repData);
+                return;
+            }
+
+            RequestAssetDelegate assetProvider = m_scene.RequestAssetMethod;
+            if (assetProvider == null)
+                return;
+            ODEAssetRequest asr = new ODEAssetRequest(this, assetProvider, repData, m_log);
+        }
+
+
+        /// <summary>
+        /// Routine to figure out if we need to mesh this prim with our mesher
+        /// </summary>
+        /// <param name="pbs"></param>
+        /// <returns></returns>
+        public bool needsMeshing(ODEPhysRepData repData)
+        {
+            PrimitiveBaseShape pbs = repData.pbs;
+            // check sculpts or meshs
+
+            Vector3 scale = pbs.Scale;
+            if(scale.X <= MinSizeToMeshmerize &&
+               scale.Y <= MinSizeToMeshmerize &&
+               scale.Z <= MinSizeToMeshmerize)
+            {
+                repData.isTooSmall = true;
+                return false;
+            }
+
+            if (pbs.SculptEntry)
+            {
+                if (meshSculptedPrim)
+                    return true;
+
+                if (pbs.SculptType == (byte)SculptType.Mesh) // always do meshs
+                    return true;
+
+                return false;
+            }
+
+            // convex shapes have no holes
+            ushort profilehollow = pbs.ProfileHollow;
+            if(repData.shapetype == 2)
+                profilehollow = 0;
+
+            // if it's a standard box or sphere with no cuts, hollows, twist or top shear, return false since ODE can use an internal representation for the prim
+
+            if ((pbs.ProfileShape == ProfileShape.Square && pbs.PathCurve == (byte)Extrusion.Straight)
+                    || (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1
+                    && pbs.Scale.X == pbs.Scale.Y && pbs.Scale.Y == pbs.Scale.Z))
+            {
+
+                if (pbs.ProfileBegin == 0 && pbs.ProfileEnd == 0
+                    && profilehollow == 0
+                    && pbs.PathTwist == 0 && pbs.PathTwistBegin == 0
+                    && pbs.PathBegin == 0 && pbs.PathEnd == 0
+                    && pbs.PathTaperX == 0 && pbs.PathTaperY == 0
+                    && pbs.PathScaleX == 100 && pbs.PathScaleY == 100
+                    && pbs.PathShearX == 0 && pbs.PathShearY == 0)
+                {
+                    return false;
+                }
+            }
+
+            //  following code doesn't give meshs to boxes and spheres ever
+            // and it's odd..  so for now just return true if asked to force meshs
+            // hopefully mesher will fail if doesn't suport so things still get basic boxes
+
+            int iPropertiesNotSupportedDefault = 0;
+
+            if (profilehollow != 0)
+                iPropertiesNotSupportedDefault++;
+
+            if ((pbs.PathBegin != 0) || pbs.PathEnd != 0)
+                iPropertiesNotSupportedDefault++;
+
+            if ((pbs.PathTwistBegin != 0) || (pbs.PathTwist != 0))
+                iPropertiesNotSupportedDefault++;
+
+            if ((pbs.ProfileBegin != 0) || pbs.ProfileEnd != 0)
+                iPropertiesNotSupportedDefault++;
+
+            if ((pbs.PathScaleX != 100) || (pbs.PathScaleY != 100))
+                iPropertiesNotSupportedDefault++;
+
+            if ((pbs.PathShearX != 0) || (pbs.PathShearY != 0))
+                iPropertiesNotSupportedDefault++;
+
+            if (pbs.ProfileShape == ProfileShape.Circle && pbs.PathCurve == (byte)Extrusion.Straight)
+                iPropertiesNotSupportedDefault++;
+
+            if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1 && (pbs.Scale.X != pbs.Scale.Y || pbs.Scale.Y != pbs.Scale.Z || pbs.Scale.Z != pbs.Scale.X))
+                iPropertiesNotSupportedDefault++;
+
+            if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1)
+                iPropertiesNotSupportedDefault++;
+
+            // test for torus
+            if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Square)
+            {
+                if (pbs.PathCurve == (byte)Extrusion.Curve1)
+                {
+                    iPropertiesNotSupportedDefault++;
+                }
+            }
+            else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Circle)
+            {
+                if (pbs.PathCurve == (byte)Extrusion.Straight)
+                {
+                    iPropertiesNotSupportedDefault++;
+                }
+
+                // ProfileCurve seems to combine hole shape and profile curve so we need to only compare against the lower 3 bits
+                else if (pbs.PathCurve == (byte)Extrusion.Curve1)
+                {
+                    iPropertiesNotSupportedDefault++;
+                }
+            }
+            else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.HalfCircle)
+            {
+                if (pbs.PathCurve == (byte)Extrusion.Curve1 || pbs.PathCurve == (byte)Extrusion.Curve2)
+                {
+                    iPropertiesNotSupportedDefault++;
+                }
+            }
+            else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.EquilateralTriangle)
+            {
+                if (pbs.PathCurve == (byte)Extrusion.Straight)
+                {
+                    iPropertiesNotSupportedDefault++;
+                }
+                else if (pbs.PathCurve == (byte)Extrusion.Curve1)
+                {
+                    iPropertiesNotSupportedDefault++;
+                }
+            }
+
+            if (iPropertiesNotSupportedDefault == 0)
+            {
+                return false;
+            }
+            return true;
+        }
+
+        // see if we need a mesh and if so if we have a cached one
+        // called with a new repData
+        public void CheckMesh(ODEPhysRepData repData)
+        {
+            PhysicsActor actor = repData.actor;
+            PrimitiveBaseShape pbs = repData.pbs;
+
+            if (!needsMeshing(repData))
+            {
+                repData.meshState = MeshState.noNeed;
+                repData.hasOBB = false;
+                return;
+            }
+
+            IMesh mesh = null;
+
+            Vector3 size = repData.size;
+
+            int clod = (int)LevelOfDetail.High;
+            byte shapetype = repData.shapetype;
+            bool convex = shapetype == 2;
+
+            mesh = m_mesher.GetMesh(actor.Name, pbs, size, clod, true, convex);
+
+            if (mesh == null)
+            {
+                if (pbs.SculptEntry)
+                {
+                    if (pbs.SculptTexture != null && pbs.SculptTexture != UUID.Zero)
+                    {
+                        repData.assetID = pbs.SculptTexture;
+                        repData.meshState = MeshState.needAsset;
+                    }
+                    else
+                        repData.meshState = MeshState.MeshFailed;
+
+                    return;
+                }
+                else
+                {
+                    repData.meshState = MeshState.needMesh;
+                    mesh = m_mesher.CreateMesh(actor.Name, pbs, size, clod, true, convex, true);
+                    if (mesh == null)
+                    {
+                        repData.meshState = MeshState.MeshFailed;
+                        return;
+                    }
+                }
+            }
+
+            repData.meshState = MeshState.AssetOK;
+            repData.mesh = mesh;
+            repData.OBB = mesh.GetOBB();
+            repData.OBBOffset = mesh.GetCentroid();
+            repData.hasOBB = true;
+
+            if (pbs.SculptEntry)
+            {
+                repData.assetID = pbs.SculptTexture;
+            }
+
+            pbs.SculptData = Utils.EmptyBytes;
+            return ;
+        }
+
+        public void GetMesh(ODEPhysRepData repData)
+        {
+            PhysicsActor actor = repData.actor;
+
+            PrimitiveBaseShape pbs = repData.pbs;
+
+            repData.mesh = null;
+            repData.hasOBB = false;
+
+            if (!needsMeshing(repData))
+            {
+                repData.meshState = MeshState.noNeed;
+                return;
+            }
+
+            if (repData.meshState == MeshState.MeshFailed)
+                return;
+
+            if (pbs.SculptEntry)
+            {
+                if (repData.meshState == MeshState.AssetFailed)
+                {
+                    if (pbs.SculptTexture == repData.assetID)
+                        return;
+                }
+            }
+
+            repData.meshState = MeshState.noNeed;
+
+            IMesh mesh = null;
+            Vector3 size = repData.size;
+            byte shapetype = repData.shapetype;
+
+            bool convex;
+            int clod = (int)LevelOfDetail.High;
+            if (shapetype == 0)
+                convex = false;
+            else
+            {
+                convex = true;
+                if (pbs.SculptType != (byte)SculptType.Mesh)
+                    clod = (int)LevelOfDetail.Low;
+            }
+
+            mesh = m_mesher.CreateMesh(actor.Name, pbs, size, clod, true, convex, true);
+
+            if (mesh == null)
+            {
+                if (pbs.SculptEntry)
+                {
+                    if (pbs.SculptTexture == UUID.Zero)
+                        return;
+
+                    repData.assetID = pbs.SculptTexture;
+
+                    if (pbs.SculptData == null || pbs.SculptData.Length == 0)
+                    {
+                        repData.meshState = MeshState.needAsset;
+                        return;
+                    }
+                }
+            }
+
+            repData.mesh = mesh;
+            repData.pbs.SculptData = Utils.EmptyBytes;
+
+            if (mesh == null)
+            {
+                if (pbs.SculptEntry)
+                    repData.meshState = MeshState.AssetFailed;
+                else
+                    repData.meshState = MeshState.MeshFailed;
+
+                return;
+            }
+
+            repData.meshState = MeshState.AssetOK;
+
+            return;
+        }
+
+        private void CalculateBasicPrimVolume(ODEPhysRepData repData)
+        {
+            Vector3 _size = repData.size;
+
+            float volume = _size.X * _size.Y * _size.Z; // default
+            if(repData.isTooSmall)
+            {
+                repData.volume = volume;
+                return;
+            }
+
+            PrimitiveBaseShape _pbs = repData.pbs;
+            float tmp;
+
+            float hollowAmount = (float)_pbs.ProfileHollow * 2.0e-5f;
+            float hollowVolume = hollowAmount * hollowAmount;
+
+            switch (_pbs.ProfileShape)
+            {
+                case ProfileShape.Square:
+                    // default box
+
+                    if (_pbs.PathCurve == (byte)Extrusion.Straight)
+                    {
+                        if (hollowAmount > 0.0)
+                        {
+                            switch (_pbs.HollowShape)
+                            {
+                                case HollowShape.Square:
+                                case HollowShape.Same:
+                                    break;
+
+                                case HollowShape.Circle:
+
+                                    hollowVolume *= 0.78539816339f;
+                                    break;
+
+                                case HollowShape.Triangle:
+
+                                    hollowVolume *= (0.5f * .5f);
+                                    break;
+
+                                default:
+                                    hollowVolume = 0;
+                                    break;
+                            }
+                            volume *= (1.0f - hollowVolume);
+                        }
+                    }
+
+                    else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
+                    {
+                        //a tube
+
+                        volume *= 0.78539816339e-2f * (float)(200 - _pbs.PathScaleX);
+                        tmp = 1.0f - 2.0e-2f * (float)(200 - _pbs.PathScaleY);
+                        volume -= volume * tmp * tmp;
+
+                        if (hollowAmount > 0.0)
+                        {
+                            hollowVolume *= hollowAmount;
+
+                            switch (_pbs.HollowShape)
+                            {
+                                case HollowShape.Square:
+                                case HollowShape.Same:
+                                    break;
+
+                                case HollowShape.Circle:
+                                    hollowVolume *= 0.78539816339f;
+                                    break;
+
+                                case HollowShape.Triangle:
+                                    hollowVolume *= 0.5f * 0.5f;
+                                    break;
+                                default:
+                                    hollowVolume = 0;
+                                    break;
+                            }
+                            volume *= (1.0f - hollowVolume);
+                        }
+                    }
+
+                    break;
+
+                case ProfileShape.Circle:
+
+                    if (_pbs.PathCurve == (byte)Extrusion.Straight)
+                    {
+                        volume *= 0.78539816339f; // elipse base
+
+                        if (hollowAmount > 0.0)
+                        {
+                            switch (_pbs.HollowShape)
+                            {
+                                case HollowShape.Same:
+                                case HollowShape.Circle:
+                                    break;
+
+                                case HollowShape.Square:
+                                    hollowVolume *= 0.5f * 2.5984480504799f;
+                                    break;
+
+                                case HollowShape.Triangle:
+                                    hollowVolume *= .5f * 1.27323954473516f;
+                                    break;
+
+                                default:
+                                    hollowVolume = 0;
+                                    break;
+                            }
+                            volume *= (1.0f - hollowVolume);
+                        }
+                    }
+
+                    else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
+                    {
+                        volume *= 0.61685027506808491367715568749226e-2f * (float)(200 - _pbs.PathScaleX);
+                        tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY);
+                        volume *= (1.0f - tmp * tmp);
+
+                        if (hollowAmount > 0.0)
+                        {
+
+                            // calculate the hollow volume by it's shape compared to the prim shape
+                            hollowVolume *= hollowAmount;
+
+                            switch (_pbs.HollowShape)
+                            {
+                                case HollowShape.Same:
+                                case HollowShape.Circle:
+                                    break;
+
+                                case HollowShape.Square:
+                                    hollowVolume *= 0.5f * 2.5984480504799f;
+                                    break;
+
+                                case HollowShape.Triangle:
+                                    hollowVolume *= .5f * 1.27323954473516f;
+                                    break;
+
+                                default:
+                                    hollowVolume = 0;
+                                    break;
+                            }
+                            volume *= (1.0f - hollowVolume);
+                        }
+                    }
+                    break;
+
+                case ProfileShape.HalfCircle:
+                    if (_pbs.PathCurve == (byte)Extrusion.Curve1)
+                    {
+                        volume *= 0.5236f;
+
+                        if (hollowAmount > 0.0)
+                        {
+                            hollowVolume *= hollowAmount;
+
+                            switch (_pbs.HollowShape)
+                            {
+                                case HollowShape.Circle:
+                                case HollowShape.Triangle:  // diference in sl is minor and odd
+                                case HollowShape.Same:
+                                    break;
+
+                                case HollowShape.Square:
+                                    hollowVolume *= 0.909f;
+                                    break;
+
+                                //                                case HollowShape.Triangle:
+                                //                                    hollowVolume *= .827f;
+                                //                                    break;
+                                default:
+                                    hollowVolume = 0;
+                                    break;
+                            }
+                            volume *= (1.0f - hollowVolume);
+                        }
+
+                    }
+                    break;
+
+                case ProfileShape.EquilateralTriangle:
+
+                    if (_pbs.PathCurve == (byte)Extrusion.Straight)
+                    {
+                        volume *= 0.32475953f;
+
+                        if (hollowAmount > 0.0)
+                        {
+
+                            // calculate the hollow volume by it's shape compared to the prim shape
+                            switch (_pbs.HollowShape)
+                            {
+                                case HollowShape.Same:
+                                case HollowShape.Triangle:
+                                    hollowVolume *= .25f;
+                                    break;
+
+                                case HollowShape.Square:
+                                    hollowVolume *= 0.499849f * 3.07920140172638f;
+                                    break;
+
+                                case HollowShape.Circle:
+                                    // Hollow shape is a perfect cyllinder in respect to the cube's scale
+                                    // Cyllinder hollow volume calculation
+
+                                    hollowVolume *= 0.1963495f * 3.07920140172638f;
+                                    break;
+
+                                default:
+                                    hollowVolume = 0;
+                                    break;
+                            }
+                            volume *= (1.0f - hollowVolume);
+                        }
+                    }
+                    else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
+                    {
+                        volume *= 0.32475953f;
+                        volume *= 0.01f * (float)(200 - _pbs.PathScaleX);
+                        tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY);
+                        volume *= (1.0f - tmp * tmp);
+
+                        if (hollowAmount > 0.0)
+                        {
+
+                            hollowVolume *= hollowAmount;
+
+                            switch (_pbs.HollowShape)
+                            {
+                                case HollowShape.Same:
+                                case HollowShape.Triangle:
+                                    hollowVolume *= .25f;
+                                    break;
+
+                                case HollowShape.Square:
+                                    hollowVolume *= 0.499849f * 3.07920140172638f;
+                                    break;
+
+                                case HollowShape.Circle:
+
+                                    hollowVolume *= 0.1963495f * 3.07920140172638f;
+                                    break;
+
+                                default:
+                                    hollowVolume = 0;
+                                    break;
+                            }
+                            volume *= (1.0f - hollowVolume);
+                        }
+                    }
+                    break;
+
+                default:
+                    break;
+            }
+
+            float taperX1;
+            float taperY1;
+            float taperX;
+            float taperY;
+            float pathBegin;
+            float pathEnd;
+            float profileBegin;
+            float profileEnd;
+
+            if (_pbs.PathCurve == (byte)Extrusion.Straight || _pbs.PathCurve == (byte)Extrusion.Flexible)
+            {
+                taperX1 = _pbs.PathScaleX * 0.01f;
+                if (taperX1 > 1.0f)
+                    taperX1 = 2.0f - taperX1;
+                taperX = 1.0f - taperX1;
+
+                taperY1 = _pbs.PathScaleY * 0.01f;
+                if (taperY1 > 1.0f)
+                    taperY1 = 2.0f - taperY1;
+                taperY = 1.0f - taperY1;
+            }
+            else
+            {
+                taperX = _pbs.PathTaperX * 0.01f;
+                if (taperX < 0.0f)
+                    taperX = -taperX;
+                taperX1 = 1.0f - taperX;
+
+                taperY = _pbs.PathTaperY * 0.01f;
+                if (taperY < 0.0f)
+                    taperY = -taperY;
+                taperY1 = 1.0f - taperY;
+            }
+
+            volume *= (taperX1 * taperY1 + 0.5f * (taperX1 * taperY + taperX * taperY1) + 0.3333333333f * taperX * taperY);
+
+            pathBegin = (float)_pbs.PathBegin * 2.0e-5f;
+            pathEnd = 1.0f - (float)_pbs.PathEnd * 2.0e-5f;
+            volume *= (pathEnd - pathBegin);
+
+            // this is crude aproximation
+            profileBegin = (float)_pbs.ProfileBegin * 2.0e-5f;
+            profileEnd = 1.0f - (float)_pbs.ProfileEnd * 2.0e-5f;
+            volume *= (profileEnd - profileBegin);
+
+            repData.volume = volume;
+        }
+
+        private void CalcVolumeData(ODEPhysRepData repData)
+        {
+            if (repData.hasOBB)
+            {
+                Vector3 OBB = repData.OBB;
+            }
+            else
+            {
+                Vector3 OBB = repData.size;
+                OBB.X *= 0.5f;
+                OBB.Y *= 0.5f;
+                OBB.Z *= 0.5f;
+
+                repData.OBB = OBB;
+                repData.OBBOffset = Vector3.Zero;
+            }
+
+            CalculateBasicPrimVolume(repData);
+        }
+    }
+
+    public class ODEAssetRequest
+    {
+        ODEMeshWorker m_worker;
+        private ILog m_log;
+        ODEPhysRepData repData;
+
+        public ODEAssetRequest(ODEMeshWorker pWorker, RequestAssetDelegate provider,
+            ODEPhysRepData pRepData, ILog plog)
+        {
+            m_worker = pWorker;
+            m_log = plog;
+            repData = pRepData;
+
+            repData.meshState = MeshState.AssetFailed;
+            if (provider == null)
+                return;
+
+            if (repData.assetID == null)
+                return;
+
+            UUID assetID = (UUID) repData.assetID;
+            if (assetID == UUID.Zero)
+                return;
+
+            repData.meshState = MeshState.loadingAsset;
+            provider(assetID, ODEassetReceived);
+        }
+
+        void ODEassetReceived(AssetBase asset)
+        {
+            repData.meshState = MeshState.AssetFailed;
+            if (asset != null)
+            {
+                if (asset.Data != null && asset.Data.Length > 0)
+                {
+                    repData.meshState = MeshState.noNeed;
+
+                    if (!repData.pbs.SculptEntry)
+                        return;
+                    if (repData.pbs.SculptTexture != repData.assetID)
+                        return;
+
+//                    repData.pbs.SculptData = new byte[asset.Data.Length];
+//                    asset.Data.CopyTo(repData.pbs.SculptData,0);
+                    repData.pbs.SculptData = asset.Data;
+                    repData.meshState = MeshState.AssetOK;
+                    m_worker.AssetLoaded(repData);
+                }
+                else
+                    m_log.WarnFormat("[PHYSICS]: asset provider returned invalid mesh data for prim {0} asset UUID {1}.",
+                        repData.actor.Name, asset.ID.ToString());
+            }
+            else
+                m_log.WarnFormat("[PHYSICS]: asset provider returned null asset for mesh of prim {0}.",
+                    repData.actor.Name);
+        }
+    }
+}
diff --git a/OpenSim/Region/PhysicsModules/ubOde/ODEModule.cs b/OpenSim/Region/PhysicsModules/ubOde/ODEModule.cs
new file mode 100644
index 0000000..4cb1736
--- /dev/null
+++ b/OpenSim/Region/PhysicsModules/ubOde/ODEModule.cs
@@ -0,0 +1,112 @@
+using System;
+using System.Collections.Generic;
+using System.Reflection;
+using log4net;
+using Nini.Config;
+using Mono.Addins;
+using OdeAPI;
+using OpenSim.Framework;
+using OpenSim.Region.Framework.Scenes;
+using OpenSim.Region.Framework.Interfaces;
+
+namespace OpenSim.Region.PhysicsModule.ubOde
+{
+    [Extension(Path = "/OpenSim/RegionModules", NodeName = "RegionModule", Id = "ubODEPhysicsScene")]
+    class ubOdeModule : INonSharedRegionModule
+    {
+        private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
+
+        private static Dictionary<Scene, ODEScene> m_scenes = new Dictionary<Scene, ODEScene>();
+        private bool m_Enabled = false;
+        private IConfigSource m_config;
+        private bool OSOdeLib;
+
+
+       #region INonSharedRegionModule
+
+        public string Name
+        {
+            get { return "ubODE"; }
+        }
+
+        public string Version
+        {
+            get { return "1.0"; }
+        }
+
+        public Type ReplaceableInterface
+        {
+            get { return null; }
+        }
+
+        public void Initialise(IConfigSource source)
+        {
+            IConfig config = source.Configs["Startup"];
+            if (config != null)
+            {
+                string physics = config.GetString("physics", string.Empty);
+                if (physics == Name)
+                {
+                    m_config = source;
+                    m_Enabled = true;
+
+                    if (Util.IsWindows())
+                        Util.LoadArchSpecificWindowsDll("ode.dll");
+
+                    d.InitODE();
+
+                    string ode_config = d.GetConfiguration();
+                    if (ode_config != null && ode_config != "")
+                    {
+                        m_log.InfoFormat("[ubODE] ode library configuration: {0}", ode_config);
+
+                        if (ode_config.Contains("ODE_OPENSIM"))
+                        {
+                            OSOdeLib = true;
+                        }
+                    }
+                }
+            }
+        }
+
+        public void Close()
+        {
+        }
+
+        public void AddRegion(Scene scene)
+        {
+            if (!m_Enabled)
+                return;
+
+            if(m_scenes.ContainsKey(scene)) // ???
+                return;
+            ODEScene newodescene = new ODEScene(scene, m_config, Name, Version, OSOdeLib);
+            m_scenes[scene] = newodescene;
+        }
+
+        public void RemoveRegion(Scene scene)
+        {
+            if (!m_Enabled)
+                return;
+
+            // a odescene.dispose is called later directly by scene.cs
+            // since it is seen as a module interface
+
+            if(m_scenes.ContainsKey(scene))
+                m_scenes.Remove(scene);
+        }
+
+        public void RegionLoaded(Scene scene)
+        {
+            if (!m_Enabled)
+                return;
+
+            if(m_scenes.ContainsKey(scene))
+            {
+                m_scenes[scene].RegionLoaded();
+            }
+
+        }
+        #endregion
+    }
+}
diff --git a/OpenSim/Region/PhysicsModules/ubOde/ODEPrim.cs b/OpenSim/Region/PhysicsModules/ubOde/ODEPrim.cs
new file mode 100644
index 0000000..aa208e2
--- /dev/null
+++ b/OpenSim/Region/PhysicsModules/ubOde/ODEPrim.cs
@@ -0,0 +1,4256 @@
+/*
+ * Copyright (c) Contributors, http://opensimulator.org/
+ * See CONTRIBUTORS.TXT for a full list of copyright holders.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the OpenSimulator Project nor the
+ *       names of its contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/*
+ * Revised August 26 2009 by Kitto Flora. ODEDynamics.cs replaces
+ * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised:
+ * ODEPrim.cs contains methods dealing with Prim editing, Prim
+ * characteristics and Kinetic motion.
+ * ODEDynamics.cs contains methods dealing with Prim Physical motion
+ * (dynamics) and the associated settings. Old Linear and angular
+ * motors for dynamic motion have been replace with  MoveLinear()
+ * and MoveAngular(); 'Physical' is used only to switch ODE dynamic
+ * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_<other> is to
+ * switch between 'VEHICLE' parameter use and general dynamics
+ * settings use.
+ */
+
+//#define SPAM
+
+using System;
+using System.Collections.Generic;
+using System.Reflection;
+using System.Runtime.InteropServices;
+using System.Threading;
+using log4net;
+using OpenMetaverse;
+using OdeAPI;
+using OpenSim.Framework;
+using OpenSim.Region.PhysicsModules.SharedBase;
+
+namespace OpenSim.Region.PhysicsModule.ubOde
+{
+    public class OdePrim : PhysicsActor
+    {
+        private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
+
+        private bool m_isphysical;
+        private bool m_fakeisphysical;
+        private bool m_isphantom;
+        private bool m_fakeisphantom;
+        internal bool m_isVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively
+        private bool m_fakeisVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively
+
+        internal bool m_building;
+        protected bool m_forcePosOrRotation;
+        private bool m_iscolliding;
+
+        internal bool m_isSelected;
+        private bool m_delaySelect;
+        private bool m_lastdoneSelected;
+        internal bool m_outbounds;
+
+        private byte m_angularlocks = 0;
+
+        private Quaternion m_lastorientation;
+        private Quaternion _orientation;
+
+        private Vector3 _position;
+        private Vector3 _velocity;
+        private Vector3 m_lastVelocity;
+        private Vector3 m_lastposition;
+        private Vector3 m_rotationalVelocity;
+        private Vector3 _size;
+        private Vector3 m_acceleration;
+        private IntPtr Amotor;
+
+        internal Vector3 m_force;
+        internal Vector3 m_forceacc;
+        internal Vector3 m_torque;
+        internal Vector3 m_angularForceacc;
+
+        private float m_invTimeStep;
+        private float m_timeStep;
+
+        private Vector3 m_PIDTarget;
+        private float m_PIDTau;
+        private bool m_usePID;
+
+        private float m_PIDHoverHeight;
+        private float m_PIDHoverTau;
+        private bool m_useHoverPID;
+        private PIDHoverType m_PIDHoverType;
+        private float m_targetHoverHeight;
+        private float m_groundHeight;
+        private float m_waterHeight;
+        private float m_buoyancy;                //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle.
+
+        private int m_body_autodisable_frames;
+        public int m_bodydisablecontrol = 0;
+        private float m_gravmod = 1.0f;
+
+        // Default we're a Geometry
+        private CollisionCategories m_collisionCategories = (CollisionCategories.Geom);
+        // Default colide nonphysical don't try to colide with anything
+        private const CollisionCategories m_default_collisionFlagsNotPhysical = 0;
+
+        private const CollisionCategories m_default_collisionFlagsPhysical = (CollisionCategories.Geom |
+                                        CollisionCategories.Character |
+                                        CollisionCategories.Land |
+                                        CollisionCategories.VolumeDtc);
+
+//        private bool m_collidesLand = true;
+        private bool m_collidesWater;
+//        public bool m_returnCollisions;
+
+        private bool m_NoColide;  // for now only for internal use for bad meshs
+
+
+        // Default, Collide with Other Geometries, spaces and Bodies
+        private CollisionCategories m_collisionFlags = m_default_collisionFlagsNotPhysical;
+
+        public bool m_disabled;
+
+        private uint m_localID;
+
+        private IMesh m_mesh;
+        private object m_meshlock = new object();
+        private PrimitiveBaseShape _pbs;
+
+        private UUID? m_assetID;
+        private MeshState m_meshState;
+
+        public ODEScene _parent_scene;
+
+        /// <summary>
+        /// The physics space which contains prim geometry
+        /// </summary>
+        public IntPtr m_targetSpace;
+
+        public IntPtr prim_geom;
+        public IntPtr _triMeshData;
+
+        private PhysicsActor _parent;
+
+        private List<OdePrim> childrenPrim = new List<OdePrim>();
+
+        public float m_collisionscore;
+        private int m_colliderfilter = 0;
+
+        public IntPtr collide_geom; // for objects: geom if single prim space it linkset
+
+        private float m_density;
+        private byte m_shapetype;
+        private byte m_fakeShapetype;
+        public bool _zeroFlag;
+        private bool m_lastUpdateSent;
+
+        public IntPtr Body;
+
+        private Vector3 _target_velocity;
+
+        public Vector3 m_OBBOffset;
+        public Vector3 m_OBB;
+        public float primOOBradiusSQ;
+
+        private bool m_hasOBB = true;
+
+        private float m_physCost;
+        private float m_streamCost;
+
+        public d.Mass primdMass; // prim inertia information on it's own referencial
+        private PhysicsInertiaData m_InertiaOverride;
+        float primMass; // prim own mass
+        float primVolume; // prim own volume;
+        float m_mass; // object mass acording to case
+
+        public int givefakepos;
+        private Vector3 fakepos;
+        public int givefakeori;
+        private Quaternion fakeori;
+        private PhysicsInertiaData m_fakeInertiaOverride;
+
+        private int m_eventsubscription;
+        private int m_cureventsubscription;
+        private CollisionEventUpdate CollisionEventsThisFrame = null;
+        private CollisionEventUpdate CollisionVDTCEventsThisFrame = null;
+        private bool SentEmptyCollisionsEvent;
+
+        public volatile bool childPrim;
+
+        public ODEDynamics m_vehicle;
+
+        internal int m_material = (int)Material.Wood;
+        private float mu;
+        private float bounce;
+
+        /// <summary>
+        /// Is this prim subject to physics?  Even if not, it's still solid for collision purposes.
+        /// </summary>
+        public override bool IsPhysical  // this is not reliable for internal use
+        {
+            get { return m_fakeisphysical; }
+            set
+            {
+                m_fakeisphysical = value; // we show imediatly to outside that we changed physical
+                // and also to stop imediatly some updates
+                // but real change will only happen in taintprocessing
+
+                if (!value) // Zero the remembered last velocity
+                    m_lastVelocity = Vector3.Zero;
+                AddChange(changes.Physical, value);
+            }
+        }
+
+        public override bool IsVolumeDtc
+        {
+            get { return m_fakeisVolumeDetect; }
+            set
+            {
+                m_fakeisVolumeDetect = value;
+                AddChange(changes.VolumeDtc, value);
+            }
+        }
+
+        public override bool Phantom  // this is not reliable for internal use
+        {
+            get { return m_fakeisphantom; }
+            set
+            {
+                m_fakeisphantom = value;
+                AddChange(changes.Phantom, value);
+            }
+        }
+
+        public override bool Building  // this is not reliable for internal use
+        {
+            get { return m_building; }
+            set
+            {
+//                if (value)
+//                    m_building = true;
+                AddChange(changes.building, value);
+            }
+        }
+
+        public override void getContactData(ref ContactData cdata)
+        {
+            cdata.mu = mu;
+            cdata.bounce = bounce;
+
+            //            cdata.softcolide = m_softcolide;
+            cdata.softcolide = false;
+
+            if (m_isphysical)
+            {
+                ODEDynamics veh;
+                if (_parent != null)
+                    veh = ((OdePrim)_parent).m_vehicle;
+                else
+                    veh = m_vehicle;
+
+                if (veh != null && veh.Type != Vehicle.TYPE_NONE)
+                    cdata.mu *= veh.FrictionFactor;
+//                    cdata.mu *= 0;
+            }
+        }
+
+        public override float PhysicsCost
+        {
+            get
+            {
+                return m_physCost;
+            }
+        }
+
+        public override float StreamCost
+        {
+            get
+            {
+                return m_streamCost;
+            }
+        }
+
+        public override int PhysicsActorType
+        {
+            get { return (int)ActorTypes.Prim; }
+            set { return; }
+        }
+
+        public override bool SetAlwaysRun
+        {
+            get { return false; }
+            set { return; }
+        }
+
+        public override uint LocalID
+        {
+            get { return m_localID; }
+            set
+            {
+                uint oldid = m_localID;
+                m_localID = value;
+                _parent_scene.changePrimID(this, oldid);
+            }
+        }
+
+        public override PhysicsActor ParentActor
+        {
+            get
+            {
+                if (childPrim)
+                    return _parent;
+                else
+                    return (PhysicsActor)this;
+            }
+        }
+
+        public override bool Grabbed
+        {
+            set { return; }
+        }
+
+        public override bool Selected
+        {
+            set
+            {
+                if (value)
+                    m_isSelected = value; // if true set imediatly to stop moves etc
+                AddChange(changes.Selected, value);
+            }
+        }
+
+        public override bool Flying
+        {
+            // no flying prims for you
+            get { return false; }
+            set { }
+        }
+
+        public override bool IsColliding
+        {
+            get { return m_iscolliding; }
+            set
+            {
+                if (value)
+                {
+                    m_colliderfilter += 2;
+                    if (m_colliderfilter > 2)
+                        m_colliderfilter = 2;
+                }
+                else
+                {
+                    m_colliderfilter--;
+                    if (m_colliderfilter < 0)
+                        m_colliderfilter = 0;
+                }
+
+                if (m_colliderfilter == 0)
+                    m_iscolliding = false;
+                else
+                    m_iscolliding = true;
+            }
+        }
+
+        public override bool CollidingGround
+        {
+            get { return false; }
+            set { return; }
+        }
+
+        public override bool CollidingObj
+        {
+            get { return false; }
+            set { return; }
+        }
+
+
+        public override bool ThrottleUpdates {get;set;}
+
+        public override bool Stopped
+        {
+            get { return _zeroFlag; }
+        }
+
+        public override Vector3 Position
+        {
+            get
+            {
+                if (givefakepos > 0)
+                    return fakepos;
+                else
+                    return _position;
+            }
+
+            set
+            {
+                fakepos = value;
+                givefakepos++;
+                AddChange(changes.Position, value);
+            }
+        }
+
+        public override Vector3 Size
+        {
+            get { return _size; }
+            set
+            {
+                if (value.IsFinite())
+                {
+                     _parent_scene.m_meshWorker.ChangeActorPhysRep(this, _pbs, value, m_fakeShapetype);
+                }
+                else
+                {
+                    m_log.WarnFormat("[PHYSICS]: Got NaN Size on object {0}", Name);
+                }
+            }
+        }
+
+        public override float Mass
+        {
+            get { return primMass; }
+        }
+
+        public override Vector3 Force
+        {
+            get { return m_force; }
+            set
+            {
+                if (value.IsFinite())
+                {
+                    AddChange(changes.Force, value);
+                }
+                else
+                {
+                    m_log.WarnFormat("[PHYSICS]: NaN in Force Applied to an Object {0}", Name);
+                }
+            }
+        }
+
+        public override void SetVolumeDetect(int param)
+        {
+            m_fakeisVolumeDetect = (param != 0);
+            AddChange(changes.VolumeDtc, m_fakeisVolumeDetect);
+        }
+
+        public override Vector3 GeometricCenter
+        {
+            // this is not real geometric center but a average of positions relative to root prim acording to
+            // http://wiki.secondlife.com/wiki/llGetGeometricCenter
+            // ignoring tortured prims details since sl also seems to ignore
+            // so no real use in doing it on physics
+            get
+            {
+                return Vector3.Zero;
+            }
+        }
+
+        public override PhysicsInertiaData GetInertiaData()
+        {
+            PhysicsInertiaData inertia;
+            if(childPrim)
+            {
+                if(_parent != null)
+                    return _parent.GetInertiaData();
+                else
+                {
+                    inertia = new PhysicsInertiaData();
+                    inertia.TotalMass = -1;
+                    return inertia;
+                }
+            }
+
+            inertia = new PhysicsInertiaData();
+
+            // double buffering
+            if(m_fakeInertiaOverride != null)
+            { 
+                d.Mass objdmass = new d.Mass();
+                objdmass.I.M00 = m_fakeInertiaOverride.Inertia.X;
+                objdmass.I.M11 = m_fakeInertiaOverride.Inertia.Y;
+                objdmass.I.M22 = m_fakeInertiaOverride.Inertia.Z;
+
+                objdmass.mass = m_fakeInertiaOverride.TotalMass;
+                    
+                if(Math.Abs(m_fakeInertiaOverride.InertiaRotation.W) < 0.999)
+                {
+                    d.Matrix3 inertiarotmat = new d.Matrix3();
+                    d.Quaternion inertiarot = new d.Quaternion();
+
+                    inertiarot.X = m_fakeInertiaOverride.InertiaRotation.X;
+                    inertiarot.Y = m_fakeInertiaOverride.InertiaRotation.Y;
+                    inertiarot.Z = m_fakeInertiaOverride.InertiaRotation.Z;
+                    inertiarot.W = m_fakeInertiaOverride.InertiaRotation.W;
+                    d.RfromQ(out inertiarotmat, ref inertiarot);
+                    d.MassRotate(ref objdmass, ref inertiarotmat);
+                }
+
+                inertia.TotalMass = m_fakeInertiaOverride.TotalMass;
+                inertia.CenterOfMass = m_fakeInertiaOverride.CenterOfMass;
+                inertia.Inertia.X = objdmass.I.M00;
+                inertia.Inertia.Y = objdmass.I.M11;
+                inertia.Inertia.Z = objdmass.I.M22;
+                inertia.InertiaRotation.X =  objdmass.I.M01;
+                inertia.InertiaRotation.Y =  objdmass.I.M02;
+                inertia.InertiaRotation.Z =  objdmass.I.M12;
+                return inertia;
+            }
+
+            inertia.TotalMass = m_mass;
+
+            if(Body == IntPtr.Zero || prim_geom == IntPtr.Zero)
+            {
+                inertia.CenterOfMass = Vector3.Zero;
+                inertia.Inertia = Vector3.Zero;
+                inertia.InertiaRotation =  Vector4.Zero;
+                return inertia;
+            }
+
+            d.Vector3 dtmp;
+            d.Mass m = new d.Mass();
+            lock(_parent_scene.OdeLock)
+            {
+                d.AllocateODEDataForThread(0);
+                dtmp = d.GeomGetOffsetPosition(prim_geom);
+                d.BodyGetMass(Body, out m);
+            }
+
+            Vector3 cm = new Vector3(-dtmp.X, -dtmp.Y, -dtmp.Z);
+            inertia.CenterOfMass = cm;
+            inertia.Inertia = new Vector3(m.I.M00, m.I.M11, m.I.M22);
+            inertia.InertiaRotation = new Vector4(m.I.M01, m.I.M02 , m.I.M12, 0);
+
+            return inertia;
+        }
+
+        public override void SetInertiaData(PhysicsInertiaData inertia)
+        {
+            if(childPrim)
+            {
+                if(_parent != null)
+                    _parent.SetInertiaData(inertia);
+                return;
+            }
+
+            if(inertia.TotalMass > 0)
+                m_fakeInertiaOverride = new PhysicsInertiaData(inertia);
+            else
+                m_fakeInertiaOverride = null;
+
+            if (inertia.TotalMass > _parent_scene.maximumMassObject)
+                inertia.TotalMass = _parent_scene.maximumMassObject;
+            AddChange(changes.SetInertia,(object)m_fakeInertiaOverride);
+        }
+
+        public override Vector3 CenterOfMass
+        {
+            get
+            {
+                lock (_parent_scene.OdeLock)
+                {
+                    d.AllocateODEDataForThread(0);
+
+                    d.Vector3 dtmp;
+                    if (!childPrim && Body != IntPtr.Zero)
+                    {
+                        dtmp = d.BodyGetPosition(Body);
+                        return new Vector3(dtmp.X, dtmp.Y, dtmp.Z);
+                    }
+                    else if (prim_geom != IntPtr.Zero)
+                    {
+                        d.Quaternion dq;
+                        d.GeomCopyQuaternion(prim_geom, out dq);
+                        Quaternion q;
+                        q.X = dq.X;
+                        q.Y = dq.Y;
+                        q.Z = dq.Z;
+                        q.W = dq.W;
+
+                        Vector3 Ptot = m_OBBOffset * q;
+                        dtmp = d.GeomGetPosition(prim_geom);
+                        Ptot.X += dtmp.X;
+                        Ptot.Y += dtmp.Y;
+                        Ptot.Z += dtmp.Z;
+
+                        //                    if(childPrim)  we only know about physical linksets
+                        return Ptot;
+/*
+                        float tmass = _mass;
+                        Ptot *= tmass;
+
+                        float m;
+
+                        foreach (OdePrim prm in childrenPrim)
+                        {
+                            m = prm._mass;
+                            Ptot += prm.CenterOfMass * m;
+                            tmass += m;
+                        }
+
+                        if (tmass == 0)
+                            tmass = 0;
+                        else
+                            tmass = 1.0f / tmass;
+
+                        Ptot *= tmass;
+                        return Ptot;
+*/
+                    }
+                    else
+                        return _position;
+                }
+            }
+        }
+
+        public override PrimitiveBaseShape Shape
+        {
+            set
+            {
+//                AddChange(changes.Shape, value);
+                _parent_scene.m_meshWorker.ChangeActorPhysRep(this, value, _size, m_fakeShapetype);
+            }
+        }
+
+        public override byte PhysicsShapeType
+        {
+            get
+            {
+                return m_fakeShapetype;
+            }
+            set
+            {
+                m_fakeShapetype = value;
+               _parent_scene.m_meshWorker.ChangeActorPhysRep(this, _pbs, _size, value);
+            }
+        }
+
+        public override Vector3 rootVelocity
+        {
+            get
+            {
+                if(_parent != null)
+                    return ((OdePrim)_parent).Velocity;
+                return Velocity;
+            }
+        }
+
+        public override Vector3 Velocity
+        {
+            get
+            {
+                if (_zeroFlag)
+                    return Vector3.Zero;
+                return _velocity;
+            }
+            set
+            {
+                if (value.IsFinite())
+                {
+                    if(m_outbounds)
+                        _velocity = value;
+                    else
+                        AddChange(changes.Velocity, value);
+                }
+                else
+                {
+                    m_log.WarnFormat("[PHYSICS]: Got NaN Velocity in Object {0}", Name);
+                }
+            }
+        }
+
+        public override Vector3 Torque
+        {
+            get
+            {
+                if (!IsPhysical || Body == IntPtr.Zero)
+                    return Vector3.Zero;
+
+                return m_torque;
+            }
+
+            set
+            {
+                if (value.IsFinite())
+                {
+                    AddChange(changes.Torque, value);
+                }
+                else
+                {
+                    m_log.WarnFormat("[PHYSICS]: Got NaN Torque in Object {0}", Name);
+                }
+            }
+        }
+
+        public override float CollisionScore
+        {
+            get { return m_collisionscore; }
+            set { m_collisionscore = value; }
+        }
+
+        public override bool Kinematic
+        {
+            get { return false; }
+            set { }
+        }
+
+        public override Quaternion Orientation
+        {
+            get
+            {
+                if (givefakeori > 0)
+                    return fakeori;
+                else
+
+                    return _orientation;
+            }
+            set
+            {
+                if (QuaternionIsFinite(value))
+                {
+                    fakeori = value;
+                    givefakeori++;
+
+                    value.Normalize();
+
+                    AddChange(changes.Orientation, value);
+                }
+                else
+                    m_log.WarnFormat("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object {0}", Name);
+
+            }
+        }
+
+        public override Vector3 Acceleration
+        {
+            get { return m_acceleration; }
+            set
+            {
+                if(m_outbounds)
+                    m_acceleration = value;
+            }
+        }
+
+        public override Vector3 RotationalVelocity
+        {
+            get
+            {
+                Vector3 pv = Vector3.Zero;
+                if (_zeroFlag)
+                    return pv;
+
+                if (m_rotationalVelocity.ApproxEquals(pv, 0.0001f))
+                    return pv;
+
+                return m_rotationalVelocity;
+            }
+            set
+            {
+                if (value.IsFinite())
+                {
+                    if(m_outbounds)
+                        m_rotationalVelocity = value;
+                    else
+                        AddChange(changes.AngVelocity, value);
+                }
+                else
+                {
+                    m_log.WarnFormat("[PHYSICS]: Got NaN RotationalVelocity in Object {0}", Name);
+                }
+            }
+        }
+
+        public override float Buoyancy
+        {
+            get { return m_buoyancy; }
+            set
+            {
+                 AddChange(changes.Buoyancy,value);
+            }
+        }
+
+        public override bool FloatOnWater
+        {
+            set
+            {
+                AddChange(changes.CollidesWater, value);
+            }
+        }
+
+        public override Vector3 PIDTarget
+        {
+            set
+            {
+                if (value.IsFinite())
+                {
+                    AddChange(changes.PIDTarget,value);
+                }
+                else
+                    m_log.WarnFormat("[PHYSICS]: Got NaN PIDTarget from Scene on Object {0}", Name);
+            }
+        }
+
+        public override bool PIDActive
+        {
+            get
+            {
+                return m_usePID;
+            }
+            set
+            {
+                AddChange(changes.PIDActive,value);
+            }
+        }
+
+        public override float PIDTau
+        {
+            set
+            {
+                float tmp = 0;
+                if (value > 0)
+                {
+                    float mint = (0.05f > m_timeStep ? 0.05f : m_timeStep);
+                    if (value < mint)
+                        tmp = mint;
+                    else
+                        tmp = value;
+                }
+                AddChange(changes.PIDTau,tmp);
+            }
+        }
+
+        public override float PIDHoverHeight
+        {
+            set
+            {
+                AddChange(changes.PIDHoverHeight,value);
+            }
+        }
+        public override bool PIDHoverActive
+        {
+            get
+            {
+                return m_useHoverPID;
+            }
+            set
+            {
+                AddChange(changes.PIDHoverActive, value);
+            }
+        }
+
+        public override PIDHoverType PIDHoverType
+        {
+            set
+            {
+                AddChange(changes.PIDHoverType,value);
+            }
+        }
+
+        public override float PIDHoverTau
+        {
+            set
+            {
+                float tmp =0;
+                if (value > 0)
+                {
+                    float mint = (0.05f > m_timeStep ? 0.05f : m_timeStep);
+                    if (value < mint)
+                        tmp = mint;
+                    else
+                        tmp = value;
+                }
+                AddChange(changes.PIDHoverTau, tmp);
+            }
+        }
+
+        public override Quaternion APIDTarget { set { return; } }
+
+        public override bool APIDActive { set { return; } }
+
+        public override float APIDStrength { set { return; } }
+
+        public override float APIDDamping { set { return; } }
+
+        public override int VehicleType
+        {
+            // we may need to put a fake on this
+            get
+            {
+                if (m_vehicle == null)
+                    return (int)Vehicle.TYPE_NONE;
+                else
+                    return (int)m_vehicle.Type;
+            }
+            set
+            {
+                AddChange(changes.VehicleType, value);
+            }
+        }
+
+        public override void VehicleFloatParam(int param, float value)
+        {
+            strVehicleFloatParam fp = new strVehicleFloatParam();
+            fp.param = param;
+            fp.value = value;
+            AddChange(changes.VehicleFloatParam, fp);
+        }
+
+        public override void VehicleVectorParam(int param, Vector3 value)
+        {
+            strVehicleVectorParam fp = new strVehicleVectorParam();
+            fp.param = param;
+            fp.value = value;
+            AddChange(changes.VehicleVectorParam, fp);
+        }
+
+        public override void VehicleRotationParam(int param, Quaternion value)
+        {
+            strVehicleQuatParam fp = new strVehicleQuatParam();
+            fp.param = param;
+            fp.value = value;
+            AddChange(changes.VehicleRotationParam, fp);
+        }
+
+        public override void VehicleFlags(int param, bool value)
+        {
+            strVehicleBoolParam bp = new strVehicleBoolParam();
+            bp.param = param;
+            bp.value = value;
+            AddChange(changes.VehicleFlags, bp);
+        }
+
+        public override void SetVehicle(object vdata)
+        {
+            AddChange(changes.SetVehicle, vdata);
+        }
+        public void SetAcceleration(Vector3 accel)
+        {
+            m_acceleration = accel;
+        }
+
+        public override void AddForce(Vector3 force, bool pushforce)
+        {
+            if (force.IsFinite())
+            {
+                if(pushforce)
+                    AddChange(changes.AddForce, force);
+                else // a impulse
+                    AddChange(changes.AddForce, force * m_invTimeStep);
+            }
+            else
+            {
+                m_log.WarnFormat("[PHYSICS]: Got Invalid linear force vector from Scene in Object {0}", Name);
+            }
+            //m_log.Info("[PHYSICS]: Added Force:" + force.ToString() +  " to prim at " + Position.ToString());
+        }
+
+        public override void AddAngularForce(Vector3 force, bool pushforce)
+        {
+            if (force.IsFinite())
+            {
+//                if(pushforce)  for now applyrotationimpulse seems more happy applied as a force
+                    AddChange(changes.AddAngForce, force);
+//                else // a impulse
+//                    AddChange(changes.AddAngForce, force * m_invTimeStep);
+            }
+            else
+            {
+                m_log.WarnFormat("[PHYSICS]: Got Invalid Angular force vector from Scene in Object {0}", Name);
+            }
+        }
+
+        public override void CrossingFailure()
+        {
+            lock(_parent_scene.OdeLock)
+            {
+                if (m_outbounds)
+                {
+                    _position.X = Util.Clip(_position.X, 0.5f, _parent_scene.WorldExtents.X - 0.5f);
+                    _position.Y = Util.Clip(_position.Y, 0.5f, _parent_scene.WorldExtents.Y - 0.5f);
+                    _position.Z = Util.Clip(_position.Z + 0.2f, -100f, 50000f);
+
+                    m_lastposition = _position;
+                    _velocity.X = 0;
+                    _velocity.Y = 0;
+                    _velocity.Z = 0;
+
+                    d.AllocateODEDataForThread(0);
+
+                    m_lastVelocity = _velocity;
+                    if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE)
+                        m_vehicle.Stop();
+
+                    if(Body != IntPtr.Zero)
+                        d.BodySetLinearVel(Body, 0, 0, 0); // stop it
+                    if (prim_geom != IntPtr.Zero)
+                        d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
+
+                    m_outbounds = false;
+                    changeDisable(false);
+                    base.RequestPhysicsterseUpdate();
+                }
+            }
+        }
+
+        public override void CrossingStart()
+        {
+            lock(_parent_scene.OdeLock)
+            {
+                if (m_outbounds || childPrim)
+                    return;
+
+                m_outbounds = true;
+
+                m_lastposition = _position;
+                m_lastorientation = _orientation;
+
+                d.AllocateODEDataForThread(0);
+                if(Body != IntPtr.Zero)
+                {
+                    d.Vector3 dtmp = d.BodyGetAngularVel(Body);
+                    m_rotationalVelocity.X = dtmp.X;
+                    m_rotationalVelocity.Y = dtmp.Y;
+                    m_rotationalVelocity.Z = dtmp.Z;
+
+                    dtmp = d.BodyGetLinearVel(Body);
+                    _velocity.X = dtmp.X;
+                    _velocity.Y = dtmp.Y;
+                    _velocity.Z = dtmp.Z;
+
+                    d.BodySetLinearVel(Body, 0, 0, 0); // stop it
+                    d.BodySetAngularVel(Body, 0, 0, 0);
+                }
+                if(prim_geom != IntPtr.Zero)
+                    d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
+                disableBodySoft(); // stop collisions
+                UnSubscribeEvents();
+            }
+        }
+
+        public override void SetMomentum(Vector3 momentum)
+        {
+        }
+
+        public override void SetMaterial(int pMaterial)
+        {
+            m_material = pMaterial;
+            mu = _parent_scene.m_materialContactsData[pMaterial].mu;
+            bounce = _parent_scene.m_materialContactsData[pMaterial].bounce;
+        }
+
+        public override float Density
+        {
+            get
+            {
+                return m_density * 100f;
+            }
+            set
+            {
+                float old = m_density;
+                m_density = value / 100f;
+ //               if(m_density != old)
+ //                   UpdatePrimBodyData();
+            }
+        }
+        public override float GravModifier
+        {
+            get
+            {
+                return m_gravmod;
+            }
+            set
+            {
+                m_gravmod = value;
+                if (m_vehicle != null)
+                    m_vehicle.GravMod = m_gravmod;
+            }
+        }
+        public override float Friction
+        {
+            get
+            {
+                return mu;
+            }
+            set
+            {
+                mu = value;
+            }
+        }
+
+        public override float Restitution
+        {
+            get
+            {
+                return bounce;
+            }
+            set
+            {
+                bounce = value;
+            }
+        }
+
+        public void setPrimForRemoval()
+        {
+            AddChange(changes.Remove, null);
+        }
+
+        public override void link(PhysicsActor obj)
+        {
+            AddChange(changes.Link, obj);
+        }
+
+        public override void delink()
+        {
+            AddChange(changes.DeLink, null);
+        }
+
+        public override void LockAngularMotion(byte axislock)
+        {
+//                m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z);
+            AddChange(changes.AngLock, axislock);
+
+        }
+
+        public override void SubscribeEvents(int ms)
+        {
+            m_eventsubscription = ms;
+            m_cureventsubscription = 0;
+            if (CollisionEventsThisFrame == null)
+                CollisionEventsThisFrame = new CollisionEventUpdate();
+            if (CollisionVDTCEventsThisFrame == null)
+                CollisionVDTCEventsThisFrame = new CollisionEventUpdate();
+            SentEmptyCollisionsEvent = false;
+        }
+
+        public override void UnSubscribeEvents()
+        {
+            if (CollisionVDTCEventsThisFrame != null)
+            {
+                CollisionVDTCEventsThisFrame.Clear();
+                CollisionVDTCEventsThisFrame = null;
+            }
+            if (CollisionEventsThisFrame != null)
+            {
+                CollisionEventsThisFrame.Clear();
+                CollisionEventsThisFrame = null;
+            }
+            m_eventsubscription = 0;
+           _parent_scene.RemoveCollisionEventReporting(this);
+        }
+
+        public override void AddCollisionEvent(uint CollidedWith, ContactPoint contact)
+        {
+            if (CollisionEventsThisFrame == null)
+                CollisionEventsThisFrame = new CollisionEventUpdate();
+
+            CollisionEventsThisFrame.AddCollider(CollidedWith, contact);
+            _parent_scene.AddCollisionEventReporting(this);
+        }
+
+        public override void AddVDTCCollisionEvent(uint CollidedWith, ContactPoint contact)
+        {
+            if (CollisionVDTCEventsThisFrame == null)
+                CollisionVDTCEventsThisFrame = new CollisionEventUpdate();
+
+            CollisionVDTCEventsThisFrame.AddCollider(CollidedWith, contact);
+            _parent_scene.AddCollisionEventReporting(this);
+        }
+
+        internal void SleeperAddCollisionEvents()
+        {
+            if(CollisionEventsThisFrame != null && CollisionEventsThisFrame.m_objCollisionList.Count != 0)
+            {
+                foreach(KeyValuePair<uint,ContactPoint> kvp in CollisionEventsThisFrame.m_objCollisionList)
+                {
+                    if(kvp.Key == 0)
+                        continue;
+                    OdePrim other = _parent_scene.getPrim(kvp.Key);
+                    if(other == null)
+                        continue;
+                    ContactPoint cp = kvp.Value;
+                    cp.SurfaceNormal = - cp.SurfaceNormal;
+                    cp.RelativeSpeed = -cp.RelativeSpeed;
+                    other.AddCollisionEvent(ParentActor.LocalID,cp);
+                }
+            }
+            if(CollisionVDTCEventsThisFrame != null && CollisionVDTCEventsThisFrame.m_objCollisionList.Count != 0)
+            {
+                foreach(KeyValuePair<uint,ContactPoint> kvp in CollisionVDTCEventsThisFrame.m_objCollisionList)
+                {
+                    OdePrim other = _parent_scene.getPrim(kvp.Key);
+                    if(other == null)
+                        continue;
+                    ContactPoint cp = kvp.Value;
+                    cp.SurfaceNormal = - cp.SurfaceNormal;
+                    cp.RelativeSpeed = -cp.RelativeSpeed;
+                    other.AddCollisionEvent(ParentActor.LocalID,cp);
+                }
+            }
+        }
+
+        internal void clearSleeperCollisions()
+        {
+            if(CollisionVDTCEventsThisFrame != null && CollisionVDTCEventsThisFrame.Count >0 )
+                CollisionVDTCEventsThisFrame.Clear();
+        }
+
+        public void SendCollisions(int timestep)
+        {
+            if (m_cureventsubscription < 50000)
+                m_cureventsubscription += timestep;
+
+
+            if (m_cureventsubscription < m_eventsubscription)
+                return;
+
+            if (CollisionEventsThisFrame == null)
+                return;
+
+            int ncolisions = CollisionEventsThisFrame.m_objCollisionList.Count;
+
+            if (!SentEmptyCollisionsEvent || ncolisions > 0)
+            {
+                base.SendCollisionUpdate(CollisionEventsThisFrame);
+                m_cureventsubscription = 0;
+
+                if (ncolisions == 0)
+                {
+                    SentEmptyCollisionsEvent = true;
+//                    _parent_scene.RemoveCollisionEventReporting(this);
+                }
+                else if(Body == IntPtr.Zero || (d.BodyIsEnabled(Body) && m_bodydisablecontrol >= 0 ))
+                {
+                    SentEmptyCollisionsEvent = false;
+                    CollisionEventsThisFrame.Clear();
+                }
+            }
+        }
+
+        public override bool SubscribedEvents()
+        {
+            if (m_eventsubscription > 0)
+                return true;
+            return false;
+        }
+
+        public OdePrim(String primName, ODEScene parent_scene, Vector3 pos, Vector3 size,
+                       Quaternion rotation, PrimitiveBaseShape pbs, bool pisPhysical,bool pisPhantom,byte _shapeType,uint plocalID)
+        {
+            _parent_scene = parent_scene;
+
+            Name = primName;
+            m_localID = plocalID;
+
+            m_vehicle = null;
+
+            if (!pos.IsFinite())
+            {
+                pos = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f),
+                    parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f);
+                m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Position for {0}", Name);
+            }
+            _position = pos;
+            givefakepos = 0;
+
+            m_timeStep = parent_scene.ODE_STEPSIZE;
+            m_invTimeStep = 1f / m_timeStep;
+
+            m_density = parent_scene.geomDefaultDensity;
+            m_body_autodisable_frames = parent_scene.bodyFramesAutoDisable;
+
+            prim_geom = IntPtr.Zero;
+            collide_geom = IntPtr.Zero;
+            Body = IntPtr.Zero;
+
+            if (!size.IsFinite())
+            {
+                size = new Vector3(0.5f, 0.5f, 0.5f);
+                m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Size for {0}", Name);
+            }
+
+            if (size.X <= 0) size.X = 0.01f;
+            if (size.Y <= 0) size.Y = 0.01f;
+            if (size.Z <= 0) size.Z = 0.01f;
+
+            _size = size;
+
+            if (!QuaternionIsFinite(rotation))
+            {
+                rotation = Quaternion.Identity;
+                m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Rotation for {0}", Name);
+            }
+
+            _orientation = rotation;
+            givefakeori = 0;
+
+            _pbs = pbs;
+
+            m_targetSpace = IntPtr.Zero;
+
+            if (pos.Z < 0)
+            {
+                m_isphysical = false;
+            }
+            else
+            {
+                m_isphysical = pisPhysical;
+            }
+            m_fakeisphysical = m_isphysical;
+
+            m_isVolumeDetect = false;
+            m_fakeisVolumeDetect = false;
+
+            m_force = Vector3.Zero;
+
+            m_iscolliding = false;
+            m_colliderfilter = 0;
+            m_NoColide = false;
+
+            _triMeshData = IntPtr.Zero;
+
+            m_fakeShapetype = _shapeType;
+
+            m_lastdoneSelected = false;
+            m_isSelected = false;
+            m_delaySelect = false;
+
+            m_isphantom = pisPhantom;
+            m_fakeisphantom = pisPhantom;
+
+            mu = parent_scene.m_materialContactsData[(int)Material.Wood].mu;
+            bounce = parent_scene.m_materialContactsData[(int)Material.Wood].bounce;
+
+            m_building = true; // control must set this to false when done
+
+            AddChange(changes.Add, null);
+
+            // get basic mass parameters
+            ODEPhysRepData repData = _parent_scene.m_meshWorker.NewActorPhysRep(this, _pbs, _size, _shapeType);
+
+            primVolume = repData.volume;
+            m_OBB = repData.OBB;
+            m_OBBOffset = repData.OBBOffset;
+
+            UpdatePrimBodyData();
+        }
+
+        private void resetCollisionAccounting()
+        {
+            m_collisionscore = 0;
+        }
+
+        private void UpdateCollisionCatFlags()
+        {
+            if(m_isphysical && m_disabled)
+            {
+                m_collisionCategories = 0;
+                m_collisionFlags = 0;
+            }
+
+            else if (m_isSelected)
+            {
+                m_collisionCategories = CollisionCategories.Selected;
+                m_collisionFlags = 0;
+            }
+
+            else if (m_isVolumeDetect)
+            {
+                m_collisionCategories = CollisionCategories.VolumeDtc;
+                if (m_isphysical)
+                    m_collisionFlags = CollisionCategories.Geom | CollisionCategories.Character;
+                else
+                    m_collisionFlags = 0;
+            }
+            else if (m_isphantom)
+            {
+                m_collisionCategories = CollisionCategories.Phantom;
+                if (m_isphysical)
+                    m_collisionFlags = CollisionCategories.Land;
+                else
+                    m_collisionFlags = 0;
+            }
+            else
+            {
+                m_collisionCategories = CollisionCategories.Geom;
+                if (m_isphysical)
+                    m_collisionFlags = m_default_collisionFlagsPhysical;
+                else
+                    m_collisionFlags = m_default_collisionFlagsNotPhysical;
+            }
+        }
+
+        private void ApplyCollisionCatFlags()
+        {
+            if (prim_geom != IntPtr.Zero)
+            {
+                if (!childPrim && childrenPrim.Count > 0)
+                {
+                    foreach (OdePrim prm in childrenPrim)
+                    {
+                        if (m_isphysical && m_disabled)
+                        {
+                            prm.m_collisionCategories = 0;
+                            prm.m_collisionFlags = 0;
+                        }
+                        else
+                        {
+                            // preserve some
+                            if (prm.m_isSelected)
+                            {
+                                prm.m_collisionCategories = CollisionCategories.Selected;
+                                prm.m_collisionFlags = 0;
+                            }
+                            else if (prm.m_isVolumeDetect)
+                            {
+                                prm.m_collisionCategories = CollisionCategories.VolumeDtc;
+                                if (m_isphysical)
+                                    prm.m_collisionFlags = CollisionCategories.Geom | CollisionCategories.Character;
+                                else
+                                    prm.m_collisionFlags = 0;
+                            }
+                            else if (prm.m_isphantom)
+                            {
+                                prm.m_collisionCategories = CollisionCategories.Phantom;
+                                if (m_isphysical)
+                                    prm.m_collisionFlags = CollisionCategories.Land;
+                                else
+                                    prm.m_collisionFlags = 0;
+                            }
+                            else
+                            {
+                                prm.m_collisionCategories = m_collisionCategories;
+                                prm.m_collisionFlags = m_collisionFlags;
+                            }
+                        }
+
+                        if (prm.prim_geom != IntPtr.Zero)
+                        {
+                            if (prm.m_NoColide)
+                            {
+                                d.GeomSetCategoryBits(prm.prim_geom, 0);
+                                if (m_isphysical)
+                                    d.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land);
+                                else
+                                    d.GeomSetCollideBits(prm.prim_geom, 0);
+                            }
+                            else
+                            {
+                                d.GeomSetCategoryBits(prm.prim_geom, (uint)prm.m_collisionCategories);
+                                d.GeomSetCollideBits(prm.prim_geom, (uint)prm.m_collisionFlags);
+                            }
+                        }
+                    }
+                }
+
+                if (m_NoColide)
+                {
+                    d.GeomSetCategoryBits(prim_geom, 0);
+                    d.GeomSetCollideBits(prim_geom, (uint)CollisionCategories.Land);
+                    if (collide_geom != prim_geom && collide_geom != IntPtr.Zero)
+                    {
+                        d.GeomSetCategoryBits(collide_geom, 0);
+                        d.GeomSetCollideBits(collide_geom, (uint)CollisionCategories.Land);
+                    }
+                }
+                else
+                {
+                    d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories);
+                    d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags);
+                    if (collide_geom != prim_geom && collide_geom != IntPtr.Zero)
+                    {
+                        d.GeomSetCategoryBits(collide_geom, (uint)m_collisionCategories);
+                        d.GeomSetCollideBits(collide_geom, (uint)m_collisionFlags);
+                    }
+                }
+            }
+        }
+
+        private void createAMotor(byte axislock)
+        {
+            if (Body == IntPtr.Zero)
+                return;
+
+            if (Amotor != IntPtr.Zero)
+            {
+                d.JointDestroy(Amotor);
+                Amotor = IntPtr.Zero;
+            }
+
+            int axisnum = 0;
+            bool axisX = false;
+            bool axisY = false;
+            bool axisZ = false;
+            if((axislock & 0x02) != 0)
+                {
+                axisnum++;
+                axisX = true;
+                }
+            if((axislock & 0x04) != 0)
+                {
+                axisnum++;
+                axisY = true;
+                }
+            if((axislock & 0x08) != 0)
+                {
+                axisnum++;
+                axisZ = true;
+                }
+
+            if(axisnum == 0)
+                return;
+            // stop it
+            d.BodySetTorque(Body, 0, 0, 0);
+            d.BodySetAngularVel(Body, 0, 0, 0);
+
+            Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero);
+            d.JointAttach(Amotor, Body, IntPtr.Zero);
+
+            d.JointSetAMotorMode(Amotor, 0);
+
+            d.JointSetAMotorNumAxes(Amotor, axisnum);
+
+            // get current orientation to lock
+
+            d.Quaternion dcur = d.BodyGetQuaternion(Body);
+            Quaternion curr; // crap convertion between identical things
+            curr.X = dcur.X;
+            curr.Y = dcur.Y;
+            curr.Z = dcur.Z;
+            curr.W = dcur.W;
+            Vector3 ax;
+
+            int i = 0;
+            int j = 0;
+            if (axisX)
+            {
+                ax = (new Vector3(1, 0, 0)) * curr; // rotate world X to current local X
+                d.JointSetAMotorAxis(Amotor, 0, 0, ax.X, ax.Y, ax.Z);
+                d.JointSetAMotorAngle(Amotor, 0, 0);
+                d.JointSetAMotorParam(Amotor, (int)d.JointParam.LoStop, 0f);
+                d.JointSetAMotorParam(Amotor, (int)d.JointParam.HiStop, 0f);
+                d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel, 0);
+                d.JointSetAMotorParam(Amotor, (int)d.JointParam.FudgeFactor, 0.0001f);
+                d.JointSetAMotorParam(Amotor, (int)d.JointParam.Bounce, 0f);
+                d.JointSetAMotorParam(Amotor, (int)d.JointParam.CFM, 0f);
+                d.JointSetAMotorParam(Amotor, (int)d.JointParam.FMax, 5e8f);
+                d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopCFM, 0f);
+                d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopERP, 0.8f);
+                i++;
+                j = 256; // move to next axis set
+            }
+
+            if (axisY)
+            {
+                ax = (new Vector3(0, 1, 0)) * curr;
+                d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z);
+                d.JointSetAMotorAngle(Amotor, i, 0);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, 0f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Vel, 0);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.CFM, 0f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopCFM, 0f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f);
+                i++;
+                j += 256;
+            }
+
+            if (axisZ)
+            {
+                ax = (new Vector3(0, 0, 1)) * curr;
+                d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z);
+                d.JointSetAMotorAngle(Amotor, i, 0);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, 0f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Vel, 0);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.CFM, 0f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopCFM, 0f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f);
+            }
+        }
+
+
+        private void SetGeom(IntPtr geom)
+        {
+            prim_geom = geom;
+            //Console.WriteLine("SetGeom to " + prim_geom + " for " + Name);
+            if (prim_geom != IntPtr.Zero)
+            {
+
+                if (m_NoColide)
+                {
+                    d.GeomSetCategoryBits(prim_geom, 0);
+                    if (m_isphysical)
+                    {
+                        d.GeomSetCollideBits(prim_geom, (uint)CollisionCategories.Land);
+                    }
+                    else
+                    {
+                        d.GeomSetCollideBits(prim_geom, 0);
+                        d.GeomDisable(prim_geom);
+                    }
+                }
+                else
+                {
+                    d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories);
+                    d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags);
+                }
+
+                UpdatePrimBodyData();
+                _parent_scene.actor_name_map[prim_geom] = this;
+
+/*
+// debug
+                d.AABB aabb;
+                d.GeomGetAABB(prim_geom, out aabb);
+                float x = aabb.MaxX - aabb.MinX;
+                float y = aabb.MaxY - aabb.MinY;
+                float z = aabb.MaxZ - aabb.MinZ;
+                if( x > 60.0f || y > 60.0f || z > 60.0f)
+                    m_log.WarnFormat("[PHYSICS]: large prim geo {0},size {1}, AABBsize <{2},{3},{4}, mesh {5} at {6}",
+                        Name, _size.ToString(), x, y, z, _pbs.SculptEntry ? _pbs.SculptTexture.ToString() : "primMesh", _position.ToString());
+                else if (x < 0.001f || y < 0.001f || z < 0.001f)
+                    m_log.WarnFormat("[PHYSICS]: small prim geo {0},size {1}, AABBsize <{2},{3},{4}, mesh {5} at {6}",
+                        Name, _size.ToString(), x, y, z, _pbs.SculptEntry ? _pbs.SculptTexture.ToString() : "primMesh", _position.ToString());
+*/
+
+            }
+            else
+                m_log.Warn("Setting bad Geom");
+        }
+
+        private bool GetMeshGeom()
+        {
+            IntPtr vertices, indices;
+            int vertexCount, indexCount;
+            int vertexStride, triStride;
+
+            IMesh mesh = m_mesh;
+
+            if (mesh == null)
+                return false;
+
+            mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount);
+            mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount);
+
+            if (vertexCount == 0 || indexCount == 0)
+            {
+                m_log.WarnFormat("[PHYSICS]: Invalid mesh data on OdePrim {0}, mesh {1} at {2}",
+                    Name, _pbs.SculptEntry ? _pbs.SculptTexture.ToString() : "primMesh",_position.ToString());
+
+                m_hasOBB = false;
+                m_OBBOffset = Vector3.Zero;
+                m_OBB = _size * 0.5f;
+
+                m_physCost = 0.1f;
+                m_streamCost = 1.0f;
+
+                _parent_scene.mesher.ReleaseMesh(mesh);
+                m_meshState = MeshState.MeshFailed;
+                m_mesh = null;
+                return false;
+            }
+
+            if (vertexCount > 64000 || indexCount > 64000)
+            {
+                m_log.WarnFormat("[PHYSICS]: large mesh data on OdePrim {0}, mesh {1} at {2}, {3} vertices, {4} indexes",
+                    Name, _pbs.SculptEntry ? _pbs.SculptTexture.ToString() : "primMesh",
+                    _position.ToString() ,vertexCount , indexCount );
+            }
+            IntPtr geo = IntPtr.Zero;
+
+            try
+            {
+                _triMeshData = d.GeomTriMeshDataCreate();
+
+                d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride);
+                d.GeomTriMeshDataPreprocess(_triMeshData);
+
+                geo = d.CreateTriMesh(m_targetSpace, _triMeshData, null, null, null);
+            }
+
+            catch (Exception e)
+            {
+                m_log.ErrorFormat("[PHYSICS]: SetGeom Mesh failed for {0} exception: {1}", Name, e);
+                if (_triMeshData != IntPtr.Zero)
+                {
+                    try
+                    {
+                        d.GeomTriMeshDataDestroy(_triMeshData);
+                    }
+                    catch
+                    {
+                    }
+                }
+                _triMeshData = IntPtr.Zero;
+
+                m_hasOBB = false;
+                m_OBBOffset = Vector3.Zero;
+                m_OBB = _size * 0.5f;
+                m_physCost = 0.1f;
+                m_streamCost = 1.0f;
+
+                _parent_scene.mesher.ReleaseMesh(mesh);
+                m_meshState = MeshState.MeshFailed;
+                m_mesh = null;
+                return false;
+            }
+
+            m_physCost = 0.0013f * (float)indexCount;
+            // todo
+            m_streamCost = 1.0f;
+
+            SetGeom(geo);
+
+            return true;
+        }
+
+        private void CreateGeom(bool OverrideToBox)
+        {
+            bool hasMesh = false;
+
+            m_NoColide = false;
+
+            if ((m_meshState & MeshState.MeshNoColide) != 0)
+                m_NoColide = true;
+
+            else if(!OverrideToBox && m_mesh != null)
+            {
+                if (GetMeshGeom())
+                    hasMesh = true;
+                else
+                    m_NoColide = true;
+            }
+
+
+            if (!hasMesh)
+            {
+                IntPtr geo = IntPtr.Zero;
+
+                if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1
+                    && _size.X == _size.Y && _size.Y == _size.Z)
+                { // it's a sphere
+                    try
+                    {
+                        geo = d.CreateSphere(m_targetSpace, _size.X * 0.5f);
+                    }
+                    catch (Exception e)
+                    {
+                        m_log.WarnFormat("[PHYSICS]: Create sphere failed: {0}", e);
+                        return;
+                    }
+                }
+                else
+                {// do it as a box
+                    try
+                    {
+                        geo = d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z);
+                    }
+                    catch (Exception e)
+                    {
+                        m_log.Warn("[PHYSICS]: Create box failed: {0}", e);
+                        return;
+                    }
+                }
+                m_physCost = 0.1f;
+                m_streamCost = 1.0f;
+                SetGeom(geo);
+            }
+        }
+
+        private void RemoveGeom()
+        {
+            if (prim_geom != IntPtr.Zero)
+            {
+                _parent_scene.actor_name_map.Remove(prim_geom);
+
+                try
+                {
+                    d.GeomDestroy(prim_geom);
+                    if (_triMeshData != IntPtr.Zero)
+                    {
+                        d.GeomTriMeshDataDestroy(_triMeshData);
+                        _triMeshData = IntPtr.Zero;
+                    }
+                }
+                catch (Exception e)
+                {
+                    m_log.ErrorFormat("[PHYSICS]: PrimGeom destruction failed for {0} exception {1}", Name, e);
+                }
+
+                prim_geom = IntPtr.Zero;
+                collide_geom = IntPtr.Zero;
+                m_targetSpace = IntPtr.Zero;
+            }
+            else
+            {
+                m_log.ErrorFormat("[PHYSICS]: PrimGeom destruction BAD {0}", Name);
+            }
+
+            lock (m_meshlock)
+            {
+                if (m_mesh != null)
+                {
+                    _parent_scene.mesher.ReleaseMesh(m_mesh);
+                    m_mesh = null;
+                }
+            }
+
+            Body = IntPtr.Zero;
+            m_hasOBB = false;
+        }
+
+        //sets non physical prim m_targetSpace to right space in spaces grid for static prims
+        // should only be called for non physical prims unless they are becoming non physical
+        private void SetInStaticSpace(OdePrim prim)
+        {
+            IntPtr targetSpace = _parent_scene.MoveGeomToStaticSpace(prim.prim_geom, prim._position, prim.m_targetSpace);
+            prim.m_targetSpace = targetSpace;
+            collide_geom = IntPtr.Zero;
+        }
+
+        public void enableBodySoft()
+        {
+            m_disabled = false;
+            if (!childPrim && !m_isSelected)
+            {
+                if (m_isphysical && Body != IntPtr.Zero)
+                {
+                    UpdateCollisionCatFlags();
+                    ApplyCollisionCatFlags();
+
+                    _zeroFlag = true;
+                    d.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
+                    d.BodyEnable(Body);
+                }
+            }
+            resetCollisionAccounting();
+        }
+
+        private void disableBodySoft()
+        {
+            m_disabled = true;
+            if (!childPrim)
+            {
+                if (m_isphysical && Body != IntPtr.Zero)
+                {
+                    if (m_isSelected)
+                        m_collisionFlags = CollisionCategories.Selected;
+                    else
+                        m_collisionCategories = 0;
+                    m_collisionFlags = 0;
+                    ApplyCollisionCatFlags();
+                    d.BodyDisable(Body);
+                }
+            }
+        }
+
+        private void MakeBody()
+        {
+            if (!m_isphysical) // only physical get bodies
+                return;
+
+            if (childPrim)  // child prims don't get bodies;
+                return;
+
+            if (m_building)
+                return;
+
+            if (prim_geom == IntPtr.Zero)
+            {
+                m_log.Warn("[PHYSICS]: Unable to link the linkset.  Root has no geom yet");
+                return;
+            }
+
+            if (Body != IntPtr.Zero)
+            {
+                DestroyBody();
+                m_log.Warn("[PHYSICS]: MakeBody called having a body");
+            }
+
+            if (d.GeomGetBody(prim_geom) != IntPtr.Zero)
+            {
+                d.GeomSetBody(prim_geom, IntPtr.Zero);
+                m_log.Warn("[PHYSICS]: MakeBody root geom already had a body");
+            }
+
+            bool noInertiaOverride = (m_InertiaOverride == null);
+
+            Body = d.BodyCreate(_parent_scene.world);
+
+            d.Matrix3 mymat = new d.Matrix3();
+            d.Quaternion myrot = new d.Quaternion();
+            d.Mass objdmass = new d.Mass { };
+
+            myrot.X = _orientation.X;
+            myrot.Y = _orientation.Y;
+            myrot.Z = _orientation.Z;
+            myrot.W = _orientation.W;
+            d.RfromQ(out mymat, ref myrot);
+
+            // set the body rotation
+            d.BodySetRotation(Body, ref mymat);
+
+            if(noInertiaOverride)
+            {
+                objdmass = primdMass;
+                d.MassRotate(ref objdmass, ref mymat);
+            }
+    
+            // recompute full object inertia if needed
+            if (childrenPrim.Count > 0)
+            {
+                d.Matrix3 mat = new d.Matrix3();
+                d.Quaternion quat = new d.Quaternion();
+                d.Mass tmpdmass = new d.Mass { };
+                Vector3 rcm;
+
+                rcm.X = _position.X;
+                rcm.Y = _position.Y;
+                rcm.Z = _position.Z;
+
+                lock (childrenPrim)
+                {
+                    foreach (OdePrim prm in childrenPrim)
+                    {
+                        if (prm.prim_geom == IntPtr.Zero)
+                        {
+                            m_log.Warn("[PHYSICS]: Unable to link one of the linkset elements, skipping it.  No geom yet");
+                            continue;
+                        }
+
+                        quat.X = prm._orientation.X;
+                        quat.Y = prm._orientation.Y;
+                        quat.Z = prm._orientation.Z;
+                        quat.W = prm._orientation.W;
+                        d.RfromQ(out mat, ref quat);
+
+                        // fix prim colision cats
+
+                        if (d.GeomGetBody(prm.prim_geom) != IntPtr.Zero)
+                        {
+                            d.GeomSetBody(prm.prim_geom, IntPtr.Zero);
+                            m_log.Warn("[PHYSICS]: MakeBody child geom already had a body");
+                        }
+
+                        d.GeomClearOffset(prm.prim_geom);
+                        d.GeomSetBody(prm.prim_geom, Body);
+                        prm.Body = Body;
+                        d.GeomSetOffsetWorldRotation(prm.prim_geom, ref mat); // set relative rotation
+
+                        if(noInertiaOverride)
+                        {
+                            tmpdmass = prm.primdMass;
+
+                            d.MassRotate(ref tmpdmass, ref mat);
+                            Vector3 ppos = prm._position;
+                            ppos.X -= rcm.X;
+                            ppos.Y -= rcm.Y;
+                            ppos.Z -= rcm.Z;
+                            // refer inertia to root prim center of mass position
+                            d.MassTranslate(ref tmpdmass,
+                                ppos.X,
+                                ppos.Y,
+                                ppos.Z);
+
+                            d.MassAdd(ref objdmass, ref tmpdmass); // add to total object inertia
+                        }
+                    }
+                }
+            }
+
+            d.GeomClearOffset(prim_geom); // make sure we don't have a hidden offset
+            // associate root geom with body
+            d.GeomSetBody(prim_geom, Body);
+
+            if(noInertiaOverride)
+                d.BodySetPosition(Body, _position.X + objdmass.c.X, _position.Y + objdmass.c.Y, _position.Z + objdmass.c.Z);
+            else
+            {
+                Vector3 ncm =  m_InertiaOverride.CenterOfMass * _orientation;
+                d.BodySetPosition(Body,
+                    _position.X + ncm.X,
+                    _position.Y + ncm.Y,
+                    _position.Z + ncm.Z);
+            }
+
+            d.GeomSetOffsetWorldPosition(prim_geom, _position.X, _position.Y, _position.Z);
+
+            if(noInertiaOverride)
+            {
+                d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body
+                myrot.X = -myrot.X;
+                myrot.Y = -myrot.Y;
+                myrot.Z = -myrot.Z;
+
+                d.RfromQ(out mymat, ref myrot);
+                d.MassRotate(ref objdmass, ref mymat);
+
+                d.BodySetMass(Body, ref objdmass);
+                m_mass = objdmass.mass;
+            }
+            else
+            {
+                objdmass.c.X = 0;
+                objdmass.c.Y = 0;
+                objdmass.c.Z = 0;
+
+                objdmass.I.M00 = m_InertiaOverride.Inertia.X;
+                objdmass.I.M11 = m_InertiaOverride.Inertia.Y;
+                objdmass.I.M22 = m_InertiaOverride.Inertia.Z;
+
+                objdmass.mass = m_InertiaOverride.TotalMass;
+
+                if(Math.Abs(m_InertiaOverride.InertiaRotation.W) < 0.999)
+                {
+                    d.Matrix3 inertiarotmat = new d.Matrix3();
+                    d.Quaternion inertiarot = new d.Quaternion();
+
+                    inertiarot.X = m_InertiaOverride.InertiaRotation.X;
+                    inertiarot.Y = m_InertiaOverride.InertiaRotation.Y;
+                    inertiarot.Z = m_InertiaOverride.InertiaRotation.Z;
+                    inertiarot.W = m_InertiaOverride.InertiaRotation.W;
+                    d.RfromQ(out inertiarotmat, ref inertiarot);
+                    d.MassRotate(ref objdmass, ref inertiarotmat);
+                }
+                d.BodySetMass(Body, ref objdmass);
+
+                m_mass = objdmass.mass;
+            }
+
+            // disconnect from world gravity so we can apply buoyancy
+            d.BodySetGravityMode(Body, false);
+
+            d.BodySetAutoDisableFlag(Body, true);
+            d.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
+            d.BodySetAutoDisableAngularThreshold(Body, 0.05f);
+            d.BodySetAutoDisableLinearThreshold(Body, 0.05f);
+            d.BodySetDamping(Body, .004f, .001f);
+
+            if (m_targetSpace != IntPtr.Zero)
+            {
+                _parent_scene.waitForSpaceUnlock(m_targetSpace);
+                if (d.SpaceQuery(m_targetSpace, prim_geom))
+                    d.SpaceRemove(m_targetSpace, prim_geom);
+            }
+
+            if (childrenPrim.Count == 0)
+            {
+                collide_geom = prim_geom;
+                m_targetSpace = _parent_scene.ActiveSpace;
+            }
+            else
+            {
+                m_targetSpace = d.HashSpaceCreate(_parent_scene.ActiveSpace);
+                d.HashSpaceSetLevels(m_targetSpace, -2, 8);
+                d.SpaceSetSublevel(m_targetSpace, 3);
+                d.SpaceSetCleanup(m_targetSpace, false);
+
+                d.GeomSetCategoryBits(m_targetSpace, (uint)(CollisionCategories.Space |
+                                                            CollisionCategories.Geom |
+                                                            CollisionCategories.Phantom |
+                                                            CollisionCategories.VolumeDtc
+                                                            ));
+                d.GeomSetCollideBits(m_targetSpace, 0);
+                collide_geom = m_targetSpace;
+            }
+
+            d.SpaceAdd(m_targetSpace, prim_geom);
+
+            if (m_delaySelect)
+            {
+                m_isSelected = true;
+                m_delaySelect = false;
+            }
+
+            m_collisionscore = 0;
+
+            UpdateCollisionCatFlags();
+            ApplyCollisionCatFlags();
+
+            _parent_scene.addActivePrim(this);
+
+            lock (childrenPrim)
+            {
+                foreach (OdePrim prm in childrenPrim)
+                {
+                    if (prm.prim_geom == IntPtr.Zero)
+                        continue;
+
+                    Vector3 ppos = prm._position;
+                    d.GeomSetOffsetWorldPosition(prm.prim_geom, ppos.X, ppos.Y, ppos.Z); // set relative position
+
+                    if (prm.m_targetSpace != m_targetSpace)
+                    {
+                        if (prm.m_targetSpace != IntPtr.Zero)
+                        {
+                            _parent_scene.waitForSpaceUnlock(prm.m_targetSpace);
+                            if (d.SpaceQuery(prm.m_targetSpace, prm.prim_geom))
+                                d.SpaceRemove(prm.m_targetSpace, prm.prim_geom);
+                        }
+                        prm.m_targetSpace = m_targetSpace;
+                        d.SpaceAdd(m_targetSpace, prm.prim_geom);
+                    }
+
+                    prm.m_collisionscore = 0;
+
+                    if(!m_disabled)
+                        prm.m_disabled = false;
+
+                    _parent_scene.addActivePrim(prm);
+                }
+            }
+
+            // The body doesn't already have a finite rotation mode set here
+            if (m_angularlocks != 0 && _parent == null)
+            {
+                createAMotor(m_angularlocks);
+            }
+
+            if (m_isSelected || m_disabled)
+            {
+                d.BodyDisable(Body);
+                _zeroFlag = true;
+            }
+            else
+            {
+                d.BodySetAngularVel(Body, m_rotationalVelocity.X, m_rotationalVelocity.Y, m_rotationalVelocity.Z);
+                d.BodySetLinearVel(Body, _velocity.X, _velocity.Y, _velocity.Z);
+
+                _zeroFlag = false;
+                m_bodydisablecontrol = 0;
+            }
+            _parent_scene.addActiveGroups(this);
+        }
+
+        private void DestroyBody()
+        {
+            if (Body != IntPtr.Zero)
+            {
+                _parent_scene.remActivePrim(this);
+
+                collide_geom = IntPtr.Zero;
+
+                if (m_disabled)
+                    m_collisionCategories = 0;
+                else if (m_isSelected)
+                    m_collisionCategories = CollisionCategories.Selected;
+                else if (m_isVolumeDetect)
+                    m_collisionCategories = CollisionCategories.VolumeDtc;
+                else if (m_isphantom)
+                    m_collisionCategories = CollisionCategories.Phantom;
+                else
+                    m_collisionCategories = CollisionCategories.Geom;
+
+                m_collisionFlags = 0;
+
+                if (prim_geom != IntPtr.Zero)
+                {
+                    if (m_NoColide)
+                    {
+                        d.GeomSetCategoryBits(prim_geom, 0);
+                        d.GeomSetCollideBits(prim_geom, 0);
+                    }
+                    else
+                    {
+                        d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories);
+                        d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags);
+                    }
+                    UpdateDataFromGeom();
+                    d.GeomSetBody(prim_geom, IntPtr.Zero);
+                    SetInStaticSpace(this);
+                }
+
+                if (!childPrim)
+                {
+                    lock (childrenPrim)
+                    {
+                        foreach (OdePrim prm in childrenPrim)
+                        {
+                            _parent_scene.remActivePrim(prm);
+
+                            if (prm.m_isSelected)
+                                prm.m_collisionCategories = CollisionCategories.Selected;
+                            else if (prm.m_isVolumeDetect)
+                                prm.m_collisionCategories = CollisionCategories.VolumeDtc;
+                            else if (prm.m_isphantom)
+                                prm.m_collisionCategories = CollisionCategories.Phantom;
+                            else
+                                prm.m_collisionCategories = CollisionCategories.Geom;
+
+                            prm.m_collisionFlags = 0;
+
+                            if (prm.prim_geom != IntPtr.Zero)
+                            {
+                                if (prm.m_NoColide)
+                                {
+                                    d.GeomSetCategoryBits(prm.prim_geom, 0);
+                                    d.GeomSetCollideBits(prm.prim_geom, 0);
+                                }
+                                else
+                                {
+                                    d.GeomSetCategoryBits(prm.prim_geom, (uint)prm.m_collisionCategories);
+                                    d.GeomSetCollideBits(prm.prim_geom, (uint)prm.m_collisionFlags);
+                                }
+                                prm.UpdateDataFromGeom();
+                                SetInStaticSpace(prm);
+                            }
+                            prm.Body = IntPtr.Zero;
+                            prm.m_mass = prm.primMass;
+                            prm.m_collisionscore = 0;
+                        }
+                    }
+                    if (Amotor != IntPtr.Zero)
+                    {
+                        d.JointDestroy(Amotor);
+                        Amotor = IntPtr.Zero;
+                    }
+                    _parent_scene.remActiveGroup(this);
+                    d.BodyDestroy(Body);
+                }
+                Body = IntPtr.Zero;
+            }
+            m_mass = primMass;
+            m_collisionscore = 0;
+        }
+
+        private void FixInertia(Vector3 NewPos,Quaternion newrot)
+        {
+            d.Matrix3 mat = new d.Matrix3();
+            d.Quaternion quat = new d.Quaternion();
+
+            d.Mass tmpdmass = new d.Mass { };
+            d.Mass objdmass = new d.Mass { };
+
+            d.BodyGetMass(Body, out tmpdmass);
+            objdmass = tmpdmass;
+
+            d.Vector3 dobjpos;
+            d.Vector3 thispos;
+
+            // get current object position and rotation
+            dobjpos = d.BodyGetPosition(Body);
+
+            // get prim own inertia in its local frame
+            tmpdmass = primdMass;
+
+            // transform to object frame
+            mat = d.GeomGetOffsetRotation(prim_geom);
+            d.MassRotate(ref tmpdmass, ref mat);
+
+            thispos = d.GeomGetOffsetPosition(prim_geom);
+            d.MassTranslate(ref tmpdmass,
+                            thispos.X,
+                            thispos.Y,
+                            thispos.Z);
+
+            // subtract current prim inertia from object
+            DMassSubPartFromObj(ref tmpdmass, ref objdmass);
+
+            // back prim own inertia
+            tmpdmass = primdMass;
+
+            // update to new position and orientation
+            _position = NewPos;
+            d.GeomSetOffsetWorldPosition(prim_geom, NewPos.X, NewPos.Y, NewPos.Z);
+            _orientation = newrot;
+            quat.X = newrot.X;
+            quat.Y = newrot.Y;
+            quat.Z = newrot.Z;
+            quat.W = newrot.W;
+            d.GeomSetOffsetWorldQuaternion(prim_geom, ref quat);
+
+            mat = d.GeomGetOffsetRotation(prim_geom);
+            d.MassRotate(ref tmpdmass, ref mat);
+
+            thispos = d.GeomGetOffsetPosition(prim_geom);
+            d.MassTranslate(ref tmpdmass,
+                            thispos.X,
+                            thispos.Y,
+                            thispos.Z);
+
+            d.MassAdd(ref objdmass, ref tmpdmass);
+
+            // fix all positions
+            IntPtr g = d.BodyGetFirstGeom(Body);
+            while (g != IntPtr.Zero)
+            {
+                thispos = d.GeomGetOffsetPosition(g);
+                thispos.X -= objdmass.c.X;
+                thispos.Y -= objdmass.c.Y;
+                thispos.Z -= objdmass.c.Z;
+                d.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z);
+                g = d.dBodyGetNextGeom(g);
+            }
+            d.BodyVectorToWorld(Body,objdmass.c.X, objdmass.c.Y, objdmass.c.Z,out thispos);
+
+            d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z);
+            d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body
+            d.BodySetMass(Body, ref objdmass);
+            m_mass = objdmass.mass;
+        }
+
+        private void FixInertia(Vector3 NewPos)
+        {
+            d.Matrix3 primmat = new d.Matrix3();
+            d.Mass tmpdmass = new d.Mass { };
+            d.Mass objdmass = new d.Mass { };
+            d.Mass primmass = new d.Mass { };
+
+            d.Vector3 dobjpos;
+            d.Vector3 thispos;
+
+            d.BodyGetMass(Body, out objdmass);
+
+            // get prim own inertia in its local frame
+            primmass = primdMass;
+            // transform to object frame
+            primmat = d.GeomGetOffsetRotation(prim_geom);
+            d.MassRotate(ref primmass, ref primmat);
+
+            tmpdmass = primmass;
+
+            thispos = d.GeomGetOffsetPosition(prim_geom);
+            d.MassTranslate(ref tmpdmass,
+                            thispos.X,
+                            thispos.Y,
+                            thispos.Z);
+
+            // subtract current prim inertia from object
+            DMassSubPartFromObj(ref tmpdmass, ref objdmass);
+
+            // update to new position
+            _position = NewPos;
+            d.GeomSetOffsetWorldPosition(prim_geom, NewPos.X, NewPos.Y, NewPos.Z);
+
+            thispos = d.GeomGetOffsetPosition(prim_geom);
+            d.MassTranslate(ref primmass,
+                            thispos.X,
+                            thispos.Y,
+                            thispos.Z);
+
+            d.MassAdd(ref objdmass, ref primmass);
+
+            // fix all positions
+            IntPtr g = d.BodyGetFirstGeom(Body);
+            while (g != IntPtr.Zero)
+            {
+                thispos = d.GeomGetOffsetPosition(g);
+                thispos.X -= objdmass.c.X;
+                thispos.Y -= objdmass.c.Y;
+                thispos.Z -= objdmass.c.Z;
+                d.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z);
+                g = d.dBodyGetNextGeom(g);
+            }
+
+            d.BodyVectorToWorld(Body, objdmass.c.X, objdmass.c.Y, objdmass.c.Z, out thispos);
+
+            // get current object position and rotation
+            dobjpos = d.BodyGetPosition(Body);
+
+            d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z);
+            d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body
+            d.BodySetMass(Body, ref objdmass);
+            m_mass = objdmass.mass;
+        }
+
+        private void FixInertia(Quaternion newrot)
+        {
+            d.Matrix3 mat = new d.Matrix3();
+            d.Quaternion quat = new d.Quaternion();
+
+            d.Mass tmpdmass = new d.Mass { };
+            d.Mass objdmass = new d.Mass { };
+            d.Vector3 dobjpos;
+            d.Vector3 thispos;
+
+            d.BodyGetMass(Body, out objdmass);
+
+            // get prim own inertia in its local frame
+            tmpdmass = primdMass;
+            mat = d.GeomGetOffsetRotation(prim_geom);
+            d.MassRotate(ref tmpdmass, ref mat);
+            // transform to object frame
+            thispos = d.GeomGetOffsetPosition(prim_geom);
+            d.MassTranslate(ref tmpdmass,
+                            thispos.X,
+                            thispos.Y,
+                            thispos.Z);
+
+            // subtract current prim inertia from object
+            DMassSubPartFromObj(ref tmpdmass, ref objdmass);
+
+            // update to new orientation
+            _orientation = newrot;
+            quat.X = newrot.X;
+            quat.Y = newrot.Y;
+            quat.Z = newrot.Z;
+            quat.W = newrot.W;
+            d.GeomSetOffsetWorldQuaternion(prim_geom, ref quat);
+
+            tmpdmass = primdMass;
+            mat = d.GeomGetOffsetRotation(prim_geom);
+            d.MassRotate(ref tmpdmass, ref mat);
+            d.MassTranslate(ref tmpdmass,
+                            thispos.X,
+                            thispos.Y,
+                            thispos.Z);
+
+            d.MassAdd(ref objdmass, ref tmpdmass);
+
+            // fix all positions
+            IntPtr g = d.BodyGetFirstGeom(Body);
+            while (g != IntPtr.Zero)
+            {
+                thispos = d.GeomGetOffsetPosition(g);
+                thispos.X -= objdmass.c.X;
+                thispos.Y -= objdmass.c.Y;
+                thispos.Z -= objdmass.c.Z;
+                d.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z);
+                g = d.dBodyGetNextGeom(g);
+            }
+
+            d.BodyVectorToWorld(Body, objdmass.c.X, objdmass.c.Y, objdmass.c.Z, out thispos);
+            // get current object position and rotation
+            dobjpos = d.BodyGetPosition(Body);
+
+            d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z);
+            d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body
+            d.BodySetMass(Body, ref objdmass);
+            m_mass = objdmass.mass;
+        }
+
+
+        #region Mass Calculation
+
+        private void UpdatePrimBodyData()
+        {
+            primMass = m_density * primVolume;
+
+            if (primMass <= 0)
+                primMass = 0.0001f;//ckrinke: Mass must be greater then zero.
+            if (primMass > _parent_scene.maximumMassObject)
+                primMass = _parent_scene.maximumMassObject;
+
+            m_mass = primMass; // just in case
+
+            d.MassSetBoxTotal(out primdMass, primMass, 2.0f * m_OBB.X, 2.0f * m_OBB.Y, 2.0f * m_OBB.Z);
+
+            d.MassTranslate(ref primdMass,
+                                m_OBBOffset.X,
+                                m_OBBOffset.Y,
+                                m_OBBOffset.Z);
+
+            primOOBradiusSQ = m_OBB.LengthSquared();
+
+            if (_triMeshData != IntPtr.Zero)
+            {
+                float pc = m_physCost;
+                float psf = primOOBradiusSQ;
+                psf *= 1.33f * .2f;
+                pc *= psf;
+                if (pc < 0.1f)
+                    pc = 0.1f;
+
+                m_physCost = pc;
+            }
+            else
+                m_physCost = 0.1f;
+
+            m_streamCost = 1.0f;
+        }
+
+        #endregion
+
+
+        /// <summary>
+        /// Add a child prim to this parent prim.
+        /// </summary>
+        /// <param name="prim">Child prim</param>
+        // I'm the parent
+        // prim is the child
+        public void ParentPrim(OdePrim prim)
+        {
+            //Console.WriteLine("ParentPrim  " + m_primName);
+            if (this.m_localID != prim.m_localID)
+            {
+                DestroyBody();  // for now we need to rebuil entire object on link change
+
+                lock (childrenPrim)
+                {
+                    // adopt the prim
+                    if (!childrenPrim.Contains(prim))
+                        childrenPrim.Add(prim);
+
+                    // see if this prim has kids and adopt them also
+                    // should not happen for now
+                    foreach (OdePrim prm in prim.childrenPrim)
+                    {
+                        if (!childrenPrim.Contains(prm))
+                        {
+                            if (prm.Body != IntPtr.Zero)
+                            {
+                                if (prm.prim_geom != IntPtr.Zero)
+                                    d.GeomSetBody(prm.prim_geom, IntPtr.Zero);
+                                if (prm.Body != prim.Body)
+                                    prm.DestroyBody(); // don't loose bodies around
+                                prm.Body = IntPtr.Zero;
+                            }
+
+                            childrenPrim.Add(prm);
+                            prm._parent = this;
+                        }
+                    }
+                }
+                //Remove old children from the prim
+                prim.childrenPrim.Clear();
+
+                if (prim.Body != IntPtr.Zero)
+                {
+                    if (prim.prim_geom != IntPtr.Zero)
+                        d.GeomSetBody(prim.prim_geom, IntPtr.Zero);
+                    prim.DestroyBody(); // don't loose bodies around
+                    prim.Body = IntPtr.Zero;
+                }
+
+                prim.childPrim = true;
+                prim._parent = this;
+
+                MakeBody(); // full nasty reconstruction
+            }
+        }
+
+        private void UpdateChildsfromgeom()
+        {
+            if (childrenPrim.Count > 0)
+            {
+                foreach (OdePrim prm in childrenPrim)
+                    prm.UpdateDataFromGeom();
+            }
+        }
+
+        private void UpdateDataFromGeom()
+        {
+            if (prim_geom != IntPtr.Zero)
+            {
+                d.Quaternion qtmp;
+                d.GeomCopyQuaternion(prim_geom, out qtmp);
+                _orientation.X = qtmp.X;
+                _orientation.Y = qtmp.Y;
+                _orientation.Z = qtmp.Z;
+                _orientation.W = qtmp.W;
+/*
+// Debug
+                float qlen = _orientation.Length();
+                if (qlen > 1.01f || qlen < 0.99)
+                    m_log.WarnFormat("[PHYSICS]: Got nonnorm quaternion from geom in Object {0} norm {1}", Name, qlen);
+//
+*/
+                _orientation.Normalize();
+
+                d.Vector3 lpos = d.GeomGetPosition(prim_geom);
+                _position.X = lpos.X;
+                _position.Y = lpos.Y;
+                _position.Z = lpos.Z;
+            }
+        }
+
+        private void ChildDelink(OdePrim odePrim, bool remakebodies)
+        {
+            // Okay, we have a delinked child.. destroy all body and remake
+            if (odePrim != this && !childrenPrim.Contains(odePrim))
+                return;
+
+            DestroyBody();
+
+            if (odePrim == this) // delinking the root prim
+            {
+                OdePrim newroot = null;
+                lock (childrenPrim)
+                {
+                    if (childrenPrim.Count > 0)
+                    {
+                        newroot = childrenPrim[0];
+                        childrenPrim.RemoveAt(0);
+                        foreach (OdePrim prm in childrenPrim)
+                        {
+                            newroot.childrenPrim.Add(prm);
+                        }
+                        childrenPrim.Clear();
+                    }
+                    if (newroot != null)
+                    {
+                        newroot.childPrim = false;
+                        newroot._parent = null;
+                        if (remakebodies)
+                            newroot.MakeBody();
+                    }
+                }
+            }
+
+            else
+            {
+                lock (childrenPrim)
+                {
+                    childrenPrim.Remove(odePrim);
+                    odePrim.childPrim = false;
+                    odePrim._parent = null;
+                    //                    odePrim.UpdateDataFromGeom();
+                    if (remakebodies)
+                        odePrim.MakeBody();
+                }
+            }
+            if (remakebodies)
+                MakeBody();
+        }
+
+        protected void ChildRemove(OdePrim odePrim, bool reMakeBody)
+        {
+            // Okay, we have a delinked child.. destroy all body and remake
+            if (odePrim != this && !childrenPrim.Contains(odePrim))
+                return;
+
+            DestroyBody();
+
+            if (odePrim == this)
+            {
+                OdePrim newroot = null;
+                lock (childrenPrim)
+                {
+                    if (childrenPrim.Count > 0)
+                    {
+                        newroot = childrenPrim[0];
+                        childrenPrim.RemoveAt(0);
+                        foreach (OdePrim prm in childrenPrim)
+                        {
+                            newroot.childrenPrim.Add(prm);
+                        }
+                        childrenPrim.Clear();
+                    }
+                    if (newroot != null)
+                    {
+                        newroot.childPrim = false;
+                        newroot._parent = null;
+                        newroot.MakeBody();
+                    }
+                }
+                if (reMakeBody)
+                    MakeBody();
+                return;
+            }
+            else
+            {
+                lock (childrenPrim)
+                {
+                    childrenPrim.Remove(odePrim);
+                    odePrim.childPrim = false;
+                    odePrim._parent = null;
+                    if (reMakeBody)
+                        odePrim.MakeBody();
+                }
+            }
+            MakeBody();
+        }
+
+
+        #region changes
+
+        private void changeadd()
+        {
+            _parent_scene.addToPrims(this);
+        }
+
+        private void changeAngularLock(byte newLocks)
+        {
+            // do we have a Physical object?
+            if (Body != IntPtr.Zero)
+            {
+                //Check that we have a Parent
+                //If we have a parent then we're not authorative here
+                if (_parent == null)
+                {
+                    if (newLocks != 0)
+                    {
+                        createAMotor(newLocks);
+                    }
+                    else
+                    {
+                        if (Amotor != IntPtr.Zero)
+                        {
+                            d.JointDestroy(Amotor);
+                            Amotor = IntPtr.Zero;
+                        }
+                    }
+                }
+            }
+            // Store this for later in case we get turned into a separate body
+            m_angularlocks = newLocks;
+        }
+
+        private void changeLink(OdePrim NewParent)
+        {
+            if (_parent == null && NewParent != null)
+            {
+                NewParent.ParentPrim(this);
+            }
+            else if (_parent != null)
+            {
+                if (_parent is OdePrim)
+                {
+                    if (NewParent != _parent)
+                    {
+                        (_parent as OdePrim).ChildDelink(this, false); // for now...
+                        childPrim = false;
+
+                        if (NewParent != null)
+                        {
+                            NewParent.ParentPrim(this);
+                        }
+                    }
+                }
+            }
+            _parent = NewParent;
+        }
+
+
+        private void Stop()
+        {
+            if (!childPrim)
+            {
+//                m_force = Vector3.Zero;
+                m_forceacc = Vector3.Zero;
+                m_angularForceacc = Vector3.Zero;
+//                m_torque = Vector3.Zero;
+                _velocity = Vector3.Zero;
+                m_acceleration = Vector3.Zero;
+                m_rotationalVelocity = Vector3.Zero;
+                _target_velocity = Vector3.Zero;
+                if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE)
+                    m_vehicle.Stop();
+
+                _zeroFlag = false;
+                base.RequestPhysicsterseUpdate();
+            }
+
+            if (Body != IntPtr.Zero)
+            {
+                d.BodySetForce(Body, 0f, 0f, 0f);
+                d.BodySetTorque(Body, 0f, 0f, 0f);
+                d.BodySetLinearVel(Body, 0f, 0f, 0f);
+                d.BodySetAngularVel(Body, 0f, 0f, 0f);
+            }
+        }
+
+        private void changePhantomStatus(bool newval)
+        {
+            m_isphantom = newval;
+
+            UpdateCollisionCatFlags();
+            ApplyCollisionCatFlags();
+        }
+
+/* not in use
+        internal void ChildSelectedChange(bool childSelect)
+        {
+            if(childPrim)
+                return;
+
+            if (childSelect == m_isSelected)
+                return;
+
+            if (childSelect)
+            {
+                DoSelectedStatus(true);
+            }
+
+            else
+            {
+                foreach (OdePrim prm in childrenPrim)
+                {
+                    if (prm.m_isSelected)
+                        return;
+                }
+                DoSelectedStatus(false);
+            }
+        }
+*/
+        private void changeSelectedStatus(bool newval)
+        {
+            if (m_lastdoneSelected == newval)
+                return;
+
+            m_lastdoneSelected = newval;
+            DoSelectedStatus(newval);
+        }
+
+        private void CheckDelaySelect()
+        {
+            if (m_delaySelect)
+            {
+                DoSelectedStatus(m_isSelected);
+            }
+        }
+
+        private void DoSelectedStatus(bool newval)
+        {
+            m_isSelected = newval;
+            Stop();
+
+            if (newval)
+            {
+                if (!childPrim && Body != IntPtr.Zero)
+                    d.BodyDisable(Body);
+
+                if (m_delaySelect || m_isphysical)
+                {
+                    m_collisionCategories = CollisionCategories.Selected;
+                    m_collisionFlags = 0;
+
+                    if (!childPrim)
+                    {
+                        foreach (OdePrim prm in childrenPrim)
+                        {
+                            prm.m_collisionCategories = m_collisionCategories;
+                            prm.m_collisionFlags = m_collisionFlags;
+
+                            if (prm.prim_geom != IntPtr.Zero)
+                            {
+
+                                if (prm.m_NoColide)
+                                {
+                                    d.GeomSetCategoryBits(prm.prim_geom, 0);
+                                    d.GeomSetCollideBits(prm.prim_geom, 0);
+                                }
+                                else
+                                {
+                                    d.GeomSetCategoryBits(prm.prim_geom, (uint)m_collisionCategories);
+                                    d.GeomSetCollideBits(prm.prim_geom, (uint)m_collisionFlags);
+                                }
+                            }
+                            prm.m_delaySelect = false;
+                        }
+                    }
+//                    else if (_parent != null)
+//                        ((OdePrim)_parent).ChildSelectedChange(true);
+
+
+                    if (prim_geom != IntPtr.Zero)
+                    {
+                        if (m_NoColide)
+                        {
+                            d.GeomSetCategoryBits(prim_geom, 0);
+                            d.GeomSetCollideBits(prim_geom, 0);
+                            if (collide_geom != prim_geom && collide_geom != IntPtr.Zero)
+                            {
+                                d.GeomSetCategoryBits(collide_geom, 0);
+                                d.GeomSetCollideBits(collide_geom, 0);
+                            }
+
+                        }
+                        else
+                        {
+                            d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories);
+                            d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags);
+                            if (collide_geom != prim_geom && collide_geom != IntPtr.Zero)
+                            {
+                                d.GeomSetCategoryBits(collide_geom, (uint)m_collisionCategories);
+                                d.GeomSetCollideBits(collide_geom, (uint)m_collisionFlags);
+                            }
+                        }
+                    }
+
+                    m_delaySelect = false;
+                }
+                else if(!m_isphysical)
+                {
+                    m_delaySelect = true;
+                }
+            }
+            else
+            {
+                if (!childPrim)
+                {
+                    if (Body != IntPtr.Zero && !m_disabled)
+                    {
+                        _zeroFlag = true;
+                        d.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
+                        d.BodyEnable(Body);
+                    }
+                }
+//                else if (_parent != null)
+//                    ((OdePrim)_parent).ChildSelectedChange(false);
+
+                UpdateCollisionCatFlags();
+                ApplyCollisionCatFlags();
+
+                m_delaySelect = false;
+            }
+
+            resetCollisionAccounting();
+        }
+
+        private void changePosition(Vector3 newPos)
+        {
+            CheckDelaySelect();
+            if (m_isphysical)
+            {
+                if (childPrim)  // inertia is messed, must rebuild
+                {
+                    if (m_building)
+                    {
+                        _position = newPos;
+                    }
+
+                    else if (m_forcePosOrRotation && _position != newPos && Body != IntPtr.Zero)
+                    {
+                        FixInertia(newPos);
+                        if (!d.BodyIsEnabled(Body))
+                        {
+                            _zeroFlag = true;
+                            d.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
+                            d.BodyEnable(Body);
+                        }
+                    }
+                }
+                else
+                {
+                    if (_position != newPos)
+                    {
+                        d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z);
+                        _position = newPos;
+                    }
+                    if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
+                    {
+                        _zeroFlag = true;
+                        d.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
+                        d.BodyEnable(Body);
+                    }
+                }
+            }
+            else
+            {
+                if (prim_geom != IntPtr.Zero)
+                {
+                    if (newPos != _position)
+                    {
+                        d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z);
+                        _position = newPos;
+
+                        m_targetSpace = _parent_scene.MoveGeomToStaticSpace(prim_geom, _position, m_targetSpace);
+                    }
+                }
+            }
+            givefakepos--;
+            if (givefakepos < 0)
+                givefakepos = 0;
+//            changeSelectedStatus();
+            resetCollisionAccounting();
+        }
+
+        private void changeOrientation(Quaternion newOri)
+        {
+            CheckDelaySelect();
+            if (m_isphysical)
+            {
+                if (childPrim)  // inertia is messed, must rebuild
+                {
+                    if (m_building)
+                    {
+                        _orientation = newOri;
+                    }
+/*
+                    else if (m_forcePosOrRotation && _orientation != newOri && Body != IntPtr.Zero)
+                    {
+                        FixInertia(_position, newOri);
+                        if (!d.BodyIsEnabled(Body))
+                            d.BodyEnable(Body);
+                    }
+*/
+                }
+                else
+                {
+                    if (newOri != _orientation)
+                    {
+                        d.Quaternion myrot = new d.Quaternion();
+                        myrot.X = newOri.X;
+                        myrot.Y = newOri.Y;
+                        myrot.Z = newOri.Z;
+                        myrot.W = newOri.W;
+                        d.GeomSetQuaternion(prim_geom, ref myrot);
+                        _orientation = newOri;
+                        
+                        if (Body != IntPtr.Zero)
+                        {
+                            if(m_angularlocks != 0)
+                                createAMotor(m_angularlocks);
+                        }
+                    }
+                    if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
+                    {
+                        _zeroFlag = true;
+                        d.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
+                        d.BodyEnable(Body);
+                    }
+                }
+            }
+            else
+            {
+                if (prim_geom != IntPtr.Zero)
+                {
+                    if (newOri != _orientation)
+                    {
+                        d.Quaternion myrot = new d.Quaternion();
+                        myrot.X = newOri.X;
+                        myrot.Y = newOri.Y;
+                        myrot.Z = newOri.Z;
+                        myrot.W = newOri.W;
+                        d.GeomSetQuaternion(prim_geom, ref myrot);
+                        _orientation = newOri;
+                    }
+                }
+            }
+            givefakeori--;
+            if (givefakeori < 0)
+                givefakeori = 0;
+            resetCollisionAccounting();
+        }
+
+        private void changePositionAndOrientation(Vector3 newPos, Quaternion newOri)
+        {
+            CheckDelaySelect();
+            if (m_isphysical)
+            {
+                if (childPrim && m_building)  // inertia is messed, must rebuild
+                {
+                    _position = newPos;
+                    _orientation = newOri;
+                }
+                else
+                {
+                    if (newOri != _orientation)
+                    {
+                        d.Quaternion myrot = new d.Quaternion();
+                        myrot.X = newOri.X;
+                        myrot.Y = newOri.Y;
+                        myrot.Z = newOri.Z;
+                        myrot.W = newOri.W;
+                        d.GeomSetQuaternion(prim_geom, ref myrot);
+                        _orientation = newOri;
+                        if (Body != IntPtr.Zero && m_angularlocks != 0)
+                            createAMotor(m_angularlocks);
+                    }
+                    if (_position != newPos)
+                    {
+                        d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z);
+                        _position = newPos;
+                    }
+                    if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
+                    {
+                        _zeroFlag = true;
+                        d.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
+                        d.BodyEnable(Body);
+                    }
+                }
+            }
+            else
+            {
+                // string primScenAvatarIn = _parent_scene.whichspaceamIin(_position);
+                // int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position);
+
+                if (prim_geom != IntPtr.Zero)
+                {
+                    if (newOri != _orientation)
+                    {
+                        d.Quaternion myrot = new d.Quaternion();
+                        myrot.X = newOri.X;
+                        myrot.Y = newOri.Y;
+                        myrot.Z = newOri.Z;
+                        myrot.W = newOri.W;
+                        d.GeomSetQuaternion(prim_geom, ref myrot);
+                        _orientation = newOri;
+                    }
+
+                    if (newPos != _position)
+                    {
+                        d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z);
+                        _position = newPos;
+
+                        m_targetSpace = _parent_scene.MoveGeomToStaticSpace(prim_geom, _position, m_targetSpace);
+                    }
+                }
+            }
+            givefakepos--;
+            if (givefakepos < 0)
+                givefakepos = 0;
+            givefakeori--;
+            if (givefakeori < 0)
+                givefakeori = 0;
+            resetCollisionAccounting();
+        }
+
+        private void changeDisable(bool disable)
+        {
+            if (disable)
+            {
+                if (!m_disabled)
+                    disableBodySoft();
+            }
+            else
+            {
+                if (m_disabled)
+                    enableBodySoft();
+            }
+        }
+
+        private void changePhysicsStatus(bool NewStatus)
+        {
+            CheckDelaySelect();
+
+            m_isphysical = NewStatus;
+
+            if (!childPrim)
+            {
+                if (NewStatus)
+                {
+                    if (Body == IntPtr.Zero)
+                        MakeBody();
+                }
+                else
+                {
+                    if (Body != IntPtr.Zero)
+                    {
+                        DestroyBody();
+                    }
+                    Stop();
+                }
+            }
+
+            resetCollisionAccounting();
+        }
+
+        private void changeSize(Vector3 newSize)
+        {
+        }
+
+        private void changeShape(PrimitiveBaseShape newShape)
+        {
+        }
+
+        private void changeAddPhysRep(ODEPhysRepData repData)
+        {
+            _size = repData.size; //??
+            _pbs = repData.pbs;
+
+            m_mesh = repData.mesh;
+
+            m_assetID = repData.assetID;
+            m_meshState = repData.meshState;
+
+            m_hasOBB = repData.hasOBB;
+            m_OBBOffset = repData.OBBOffset;
+            m_OBB = repData.OBB;
+
+            primVolume = repData.volume;
+
+            CreateGeom(repData.isTooSmall);
+
+            if (prim_geom != IntPtr.Zero)
+            {
+                d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
+                d.Quaternion myrot = new d.Quaternion();
+                myrot.X = _orientation.X;
+                myrot.Y = _orientation.Y;
+                myrot.Z = _orientation.Z;
+                myrot.W = _orientation.W;
+                d.GeomSetQuaternion(prim_geom, ref myrot);
+            }
+
+            if (!m_isphysical)
+            {
+                SetInStaticSpace(this);
+                UpdateCollisionCatFlags();
+                ApplyCollisionCatFlags();
+            }
+            else
+                MakeBody();
+
+            if ((m_meshState & MeshState.NeedMask) != 0)
+            {
+                repData.size = _size;
+                repData.pbs = _pbs;
+                repData.shapetype = m_fakeShapetype;
+                _parent_scene.m_meshWorker.RequestMesh(repData);
+            }
+            else
+                m_shapetype = repData.shapetype;
+        }
+
+        private void changePhysRepData(ODEPhysRepData repData)
+        {
+            if(_size == repData.size &&
+                    _pbs == repData.pbs &&
+                    m_shapetype == repData.shapetype &&
+                    m_mesh == repData.mesh &&
+                    primVolume == repData.volume)
+                return;
+
+            CheckDelaySelect();
+
+            OdePrim parent = (OdePrim)_parent;
+
+            bool chp = childPrim;
+
+            if (chp)
+            {
+                if (parent != null)
+                {
+                    parent.DestroyBody();
+                }
+            }
+            else
+            {
+                DestroyBody();
+            }
+
+            RemoveGeom();
+
+            _size = repData.size;
+            _pbs = repData.pbs;
+
+            m_mesh = repData.mesh;
+
+            m_assetID = repData.assetID;
+            m_meshState = repData.meshState;
+
+            m_hasOBB = repData.hasOBB;
+            m_OBBOffset = repData.OBBOffset;
+            m_OBB = repData.OBB;
+
+            primVolume = repData.volume;
+
+            CreateGeom(repData.isTooSmall);
+
+            if (prim_geom != IntPtr.Zero)
+            {
+                d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
+                d.Quaternion myrot = new d.Quaternion();
+                myrot.X = _orientation.X;
+                myrot.Y = _orientation.Y;
+                myrot.Z = _orientation.Z;
+                myrot.W = _orientation.W;
+                d.GeomSetQuaternion(prim_geom, ref myrot);
+            }
+
+            if (m_isphysical)
+            {
+                if (chp)
+                {
+                    if (parent != null)
+                    {
+                        parent.MakeBody();
+                    }
+                }
+                else
+                    MakeBody();
+            }
+            else
+            {
+                SetInStaticSpace(this);
+                UpdateCollisionCatFlags();
+                ApplyCollisionCatFlags();
+            }
+
+            resetCollisionAccounting();
+
+            if ((m_meshState & MeshState.NeedMask) != 0)
+            {
+                repData.size = _size;
+                repData.pbs = _pbs;
+                repData.shapetype = m_fakeShapetype;
+                _parent_scene.m_meshWorker.RequestMesh(repData);
+            }
+            else
+                m_shapetype = repData.shapetype;
+        }
+
+        private void changeFloatOnWater(bool newval)
+        {
+            m_collidesWater = newval;
+
+            UpdateCollisionCatFlags();
+            ApplyCollisionCatFlags();
+        }
+
+        private void changeSetTorque(Vector3 newtorque)
+        {
+            if (!m_isSelected && !m_outbounds)
+            {
+                if (m_isphysical && Body != IntPtr.Zero)
+                {
+                    if (m_disabled)
+                        enableBodySoft();
+                    else if (!d.BodyIsEnabled(Body))
+                    {
+                        d.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
+                        d.BodyEnable(Body);
+                    }
+                }
+                m_torque = newtorque;
+            }
+        }
+
+        private void changeForce(Vector3 force)
+        {
+            m_force = force;
+            if (!m_isSelected && !m_outbounds && Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
+            {
+                d.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
+                d.BodyEnable(Body);
+            }
+        }
+
+        private void changeAddForce(Vector3 theforce)
+        {
+            m_forceacc += theforce;
+            if (!m_isSelected && !m_outbounds)
+            {
+                lock (this)
+                {
+                    //m_log.Info("[PHYSICS]: dequeing forcelist");
+                    if (m_isphysical && Body != IntPtr.Zero)
+                    {
+                        if (m_disabled)
+                            enableBodySoft();
+                        else if (!d.BodyIsEnabled(Body))
+                        {
+                            d.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
+                            d.BodyEnable(Body);
+                        }
+                    }
+                }
+                m_collisionscore = 0;
+            }
+        }
+
+        // actually angular impulse
+        private void changeAddAngularImpulse(Vector3 aimpulse)
+        {
+            m_angularForceacc += aimpulse * m_invTimeStep;
+            if (!m_isSelected && !m_outbounds)
+            {
+                lock (this)
+                {
+                    if (m_isphysical && Body != IntPtr.Zero)
+                    {
+                        if (m_disabled)
+                            enableBodySoft();
+                        else if (!d.BodyIsEnabled(Body))
+                        {
+                            d.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
+                            d.BodyEnable(Body);
+                        }                            
+                    }
+                }
+                m_collisionscore = 0;
+            }
+        }
+
+        private void changevelocity(Vector3 newVel)
+        {
+            float len = newVel.LengthSquared();
+            if (len > 100000.0f) // limit to 100m/s
+            {
+                len = 100.0f / (float)Math.Sqrt(len);
+                newVel *= len;
+            }
+
+            if (!m_isSelected && !m_outbounds)
+            {
+                if (Body != IntPtr.Zero)
+                {
+                    if (m_disabled)
+                        enableBodySoft();
+                    else if (!d.BodyIsEnabled(Body))
+                    {
+                        d.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
+                        d.BodyEnable(Body);
+                    }
+                    d.BodySetLinearVel(Body, newVel.X, newVel.Y, newVel.Z);
+                }
+                //resetCollisionAccounting();
+            }
+            _velocity = newVel;
+        }
+
+        private void changeangvelocity(Vector3 newAngVel)
+        {
+            float len = newAngVel.LengthSquared();
+            if (len > _parent_scene.maxAngVelocitySQ)
+            {
+                len = _parent_scene.maximumAngularVelocity / (float)Math.Sqrt(len);
+                newAngVel *= len;
+            }
+
+            if (!m_isSelected && !m_outbounds)
+            {
+                if (Body != IntPtr.Zero)
+                {
+                    if (m_disabled)
+                        enableBodySoft();
+                    else if (!d.BodyIsEnabled(Body))
+                    {
+                        d.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
+                        d.BodyEnable(Body);
+                    }
+                    d.BodySetAngularVel(Body, newAngVel.X, newAngVel.Y, newAngVel.Z);
+                }
+                //resetCollisionAccounting();
+            }
+            m_rotationalVelocity = newAngVel;
+        }
+
+        private void changeVolumedetetion(bool newVolDtc)
+        {
+            m_isVolumeDetect = newVolDtc;
+            m_fakeisVolumeDetect = newVolDtc;
+            UpdateCollisionCatFlags();
+            ApplyCollisionCatFlags();
+        }
+
+        protected void changeBuilding(bool newbuilding)
+        {
+            // Check if we need to do anything
+            if (newbuilding == m_building)
+                return;
+
+            if ((bool)newbuilding)
+            {
+                m_building = true;
+                if (!childPrim)
+                    DestroyBody();
+            }
+            else
+            {
+                m_building = false;
+                CheckDelaySelect();
+                if (!childPrim)
+                    MakeBody();
+            }
+            if (!childPrim && childrenPrim.Count > 0)
+            {
+                foreach (OdePrim prm in childrenPrim)
+                    prm.changeBuilding(m_building); // call directly
+            }
+        }
+
+       public void changeSetVehicle(VehicleData vdata)
+        {
+            if (m_vehicle == null)
+                m_vehicle = new ODEDynamics(this);
+            m_vehicle.DoSetVehicle(vdata);
+        }
+
+        private void changeVehicleType(int value)
+        {
+            if (value == (int)Vehicle.TYPE_NONE)
+            {
+                if (m_vehicle != null)
+                    m_vehicle = null;
+            }
+            else
+            {
+                if (m_vehicle == null)
+                    m_vehicle = new ODEDynamics(this);
+
+                m_vehicle.ProcessTypeChange((Vehicle)value);
+            }
+        }
+
+        private void changeVehicleFloatParam(strVehicleFloatParam fp)
+        {
+            if (m_vehicle == null)
+                return;
+
+            m_vehicle.ProcessFloatVehicleParam((Vehicle)fp.param, fp.value);
+        }
+
+        private void changeVehicleVectorParam(strVehicleVectorParam vp)
+        {
+            if (m_vehicle == null)
+                return;
+            m_vehicle.ProcessVectorVehicleParam((Vehicle)vp.param, vp.value);
+        }
+
+        private void changeVehicleRotationParam(strVehicleQuatParam qp)
+        {
+            if (m_vehicle == null)
+                return;
+            m_vehicle.ProcessRotationVehicleParam((Vehicle)qp.param, qp.value);
+        }
+
+        private void changeVehicleFlags(strVehicleBoolParam bp)
+        {
+            if (m_vehicle == null)
+                return;
+            m_vehicle.ProcessVehicleFlags(bp.param, bp.value);
+        }
+
+        private void changeBuoyancy(float b)
+        {
+            m_buoyancy = b;
+        }
+
+        private void changePIDTarget(Vector3 trg)
+        {
+            m_PIDTarget = trg;
+        }
+
+        private void changePIDTau(float tau)
+        {
+            m_PIDTau = tau;
+        }
+
+        private void changePIDActive(bool val)
+        {
+            m_usePID = val;
+        }
+
+        private void changePIDHoverHeight(float val)
+        {
+          m_PIDHoverHeight = val;
+          if (val == 0)
+            m_useHoverPID = false;
+        }
+
+        private void changePIDHoverType(PIDHoverType type)
+        {
+            m_PIDHoverType = type;
+        }
+
+        private void changePIDHoverTau(float tau)
+        {
+            m_PIDHoverTau = tau;
+        }
+
+        private void changePIDHoverActive(bool active)
+        {
+            m_useHoverPID = active;
+        }
+
+        private void changeInertia(PhysicsInertiaData inertia)
+        {
+            m_InertiaOverride = inertia;
+
+            if (Body != IntPtr.Zero)
+                DestroyBody();
+            MakeBody();
+        }
+
+        #endregion
+
+        public void Move()
+        {
+            if (!childPrim && m_isphysical && Body != IntPtr.Zero &&
+                !m_disabled && !m_isSelected && !m_building && !m_outbounds)
+            {
+                if (!d.BodyIsEnabled(Body))
+                {
+                    // let vehicles sleep
+                    if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE)
+                        return;
+
+                    if (++m_bodydisablecontrol < 50)
+                        return;
+
+                    // clear residuals
+                    d.BodySetAngularVel(Body,0f,0f,0f);
+                    d.BodySetLinearVel(Body,0f,0f,0f);
+                    _zeroFlag = true;
+                    d.BodySetAutoDisableSteps(Body, 1);
+                    d.BodyEnable(Body);
+                    m_bodydisablecontrol = -3;
+                }
+
+                if(m_bodydisablecontrol < 0)
+                    m_bodydisablecontrol++;
+
+                d.Vector3 lpos = d.GeomGetPosition(prim_geom); // root position that is seem by rest of simulator
+
+                if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE)
+                {
+                    // 'VEHICLES' are dealt with in ODEDynamics.cs
+                    m_vehicle.Step();
+                    return;
+                }
+
+                float fx = 0;
+                float fy = 0;
+                float fz = 0;
+
+                float mass = m_mass;
+
+                if (m_usePID && m_PIDTau > 0)
+                {
+                    // for now position error
+                    _target_velocity =
+                        new Vector3(
+                            (m_PIDTarget.X - lpos.X),
+                            (m_PIDTarget.Y - lpos.Y),
+                            (m_PIDTarget.Z - lpos.Z)
+                            );
+
+                    if (_target_velocity.ApproxEquals(Vector3.Zero, 0.02f))
+                    {
+                        d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);
+                        d.BodySetLinearVel(Body, 0, 0, 0);
+                        return;
+                    }
+                    else
+                    {
+                        _zeroFlag = false;
+
+                        float tmp = 1 / m_PIDTau;
+                        _target_velocity *= tmp;
+
+                        // apply limits
+                        tmp = _target_velocity.Length();
+                        if (tmp > 50.0f)
+                        {
+                            tmp = 50 / tmp;
+                            _target_velocity *= tmp;
+                        }
+                        else if (tmp < 0.05f)
+                        {
+                            tmp = 0.05f / tmp;
+                            _target_velocity *= tmp;
+                        }
+
+                        d.Vector3 vel = d.BodyGetLinearVel(Body);
+                        fx = (_target_velocity.X - vel.X) * m_invTimeStep;
+                        fy = (_target_velocity.Y - vel.Y) * m_invTimeStep;
+                        fz = (_target_velocity.Z - vel.Z) * m_invTimeStep;
+//                        d.BodySetLinearVel(Body, _target_velocity.X, _target_velocity.Y, _target_velocity.Z);
+                    }
+                }        // end if (m_usePID)
+
+                // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller
+                else if (m_useHoverPID && m_PIDHoverTau != 0 && m_PIDHoverHeight != 0)
+                {
+
+                    //    Non-Vehicles have a limited set of Hover options.
+                    // determine what our target height really is based on HoverType
+
+                    m_groundHeight = _parent_scene.GetTerrainHeightAtXY(lpos.X, lpos.Y);
+
+                    switch (m_PIDHoverType)
+                    {
+                        case PIDHoverType.Ground:
+                            m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
+                            break;
+
+                        case PIDHoverType.GroundAndWater:
+                            m_waterHeight = _parent_scene.GetWaterLevel();
+                            if (m_groundHeight > m_waterHeight)
+                                m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
+                            else
+                                m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
+                            break;
+                    }     // end switch (m_PIDHoverType)
+
+                    // don't go underground unless volumedetector
+
+                    if (m_targetHoverHeight > m_groundHeight || m_isVolumeDetect)
+                    {
+                        d.Vector3 vel = d.BodyGetLinearVel(Body);
+
+                        fz = (m_targetHoverHeight - lpos.Z);
+
+                        //  if error is zero, use position control; otherwise, velocity control
+                        if (Math.Abs(fz) < 0.01f)
+                        {
+                            d.BodySetPosition(Body, lpos.X, lpos.Y, m_targetHoverHeight);
+                            d.BodySetLinearVel(Body, vel.X, vel.Y, 0);
+                        }
+                        else
+                        {
+                            _zeroFlag = false;
+                            fz /= m_PIDHoverTau;
+
+                            float tmp = Math.Abs(fz);
+                            if (tmp > 50)
+                                fz = 50 * Math.Sign(fz);
+                            else if (tmp < 0.1)
+                                fz = 0.1f * Math.Sign(fz);
+
+                            fz = ((fz - vel.Z) * m_invTimeStep);
+                        }
+                    }
+                }
+                else
+                {
+                    float b = (1.0f - m_buoyancy) * m_gravmod;
+                    fx = _parent_scene.gravityx * b;
+                    fy = _parent_scene.gravityy * b;
+                    fz = _parent_scene.gravityz * b;
+                }
+
+                fx *= mass;
+                fy *= mass;
+                fz *= mass;
+
+                // constant force
+                fx += m_force.X;
+                fy += m_force.Y;
+                fz += m_force.Z;
+
+                fx += m_forceacc.X;
+                fy += m_forceacc.Y;
+                fz += m_forceacc.Z;
+
+                m_forceacc = Vector3.Zero;
+
+                //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString());
+                if (fx != 0 || fy != 0 || fz != 0)
+                {
+                    d.BodyAddForce(Body, fx, fy, fz);
+                    //Console.WriteLine("AddForce " + fx + "," + fy + "," + fz);
+                }
+
+                Vector3 trq;
+
+                trq = m_torque;
+                trq += m_angularForceacc;
+                m_angularForceacc = Vector3.Zero;
+                if (trq.X != 0 || trq.Y != 0 || trq.Z != 0)
+                {
+                    d.BodyAddTorque(Body, trq.X, trq.Y, trq.Z);
+                }
+            }
+            else
+            {    // is not physical, or is not a body or is selected
+                //  _zeroPosition = d.BodyGetPosition(Body);
+                return;
+                //Console.WriteLine("Nothing " +  Name);
+
+            }
+        }
+
+        public void UpdatePositionAndVelocity(int frame)
+        {
+            if (_parent == null && !m_isSelected && !m_disabled && !m_building && !m_outbounds && Body != IntPtr.Zero)
+            {
+                if(m_bodydisablecontrol < 0)
+                    return;
+
+                bool bodyenabled = d.BodyIsEnabled(Body);
+                if (bodyenabled || !_zeroFlag)
+                {
+                    bool lastZeroFlag = _zeroFlag;
+
+                    d.Vector3 lpos = d.GeomGetPosition(prim_geom);
+
+                    // check outside region
+                    if (lpos.Z < -100 || lpos.Z > 100000f)
+                    {
+                        m_outbounds = true;
+
+                        lpos.Z = Util.Clip(lpos.Z, -100f, 100000f);
+                        m_acceleration.X = 0;
+                        m_acceleration.Y = 0;
+                        m_acceleration.Z = 0;
+
+                        _velocity.X = 0;
+                        _velocity.Y = 0;
+                        _velocity.Z = 0;
+                        m_rotationalVelocity.X = 0;
+                        m_rotationalVelocity.Y = 0;
+                        m_rotationalVelocity.Z = 0;
+
+                        d.BodySetLinearVel(Body, 0, 0, 0); // stop it
+                        d.BodySetAngularVel(Body, 0, 0, 0); // stop it
+                        d.BodySetPosition(Body, lpos.X, lpos.Y, lpos.Z); // put it somewhere
+                        m_lastposition = _position;
+                        m_lastorientation = _orientation;
+
+                        base.RequestPhysicsterseUpdate();
+
+//                        throttleCounter = 0;
+                        _zeroFlag = true;
+
+                        disableBodySoft(); // disable it and colisions
+                        base.RaiseOutOfBounds(_position);
+                        return;
+                    }
+
+                    if (lpos.X < 0f)
+                    {
+                        _position.X = Util.Clip(lpos.X, -2f, -0.1f);
+                        m_outbounds = true;
+                    }
+                    else if (lpos.X > _parent_scene.WorldExtents.X)
+                    {
+                        _position.X = Util.Clip(lpos.X, _parent_scene.WorldExtents.X + 0.1f, _parent_scene.WorldExtents.X + 2f);
+                        m_outbounds = true;
+                    }
+                    if (lpos.Y < 0f)
+                    {
+                        _position.Y = Util.Clip(lpos.Y, -2f, -0.1f);
+                        m_outbounds = true;
+                    }
+                    else if (lpos.Y > _parent_scene.WorldExtents.Y)
+                    {
+                        _position.Y = Util.Clip(lpos.Y, _parent_scene.WorldExtents.Y + 0.1f, _parent_scene.WorldExtents.Y + 2f);
+                        m_outbounds = true;
+                    }
+
+                    if (m_outbounds)
+                    {
+                        m_lastposition = _position;
+                        m_lastorientation = _orientation;
+
+                        d.Vector3 dtmp = d.BodyGetAngularVel(Body);
+                        m_rotationalVelocity.X = dtmp.X;
+                        m_rotationalVelocity.Y = dtmp.Y;
+                        m_rotationalVelocity.Z = dtmp.Z;
+
+                        dtmp = d.BodyGetLinearVel(Body);
+                        _velocity.X = dtmp.X;
+                        _velocity.Y = dtmp.Y;
+                        _velocity.Z = dtmp.Z;
+
+                        d.BodySetLinearVel(Body, 0, 0, 0); // stop it
+                        d.BodySetAngularVel(Body, 0, 0, 0);
+                        d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
+                        disableBodySoft(); // stop collisions
+                        UnSubscribeEvents();
+
+                        base.RequestPhysicsterseUpdate();
+                        return;
+                    }
+
+                    d.Quaternion ori;
+                    d.GeomCopyQuaternion(prim_geom, out ori);
+
+                    // decide if moving
+                    // use positions since this are integrated quantities
+                    // tolerance values depende a lot on simulation noise...
+                    // use simple math.abs since we dont need to be exact
+                    if(!bodyenabled)
+                    {
+                        _zeroFlag = true;
+                    }
+                    else
+                    {
+                        float poserror;
+                        float angerror;
+                        if(_zeroFlag)
+                        {
+                            poserror = 0.01f;
+                            angerror = 0.001f;
+                        }
+                        else
+                        {
+                            poserror = 0.005f;
+                            angerror = 0.0005f;
+                        }
+
+                        if (
+                            (Math.Abs(_position.X - lpos.X) < poserror)
+                            && (Math.Abs(_position.Y - lpos.Y) < poserror)
+                            && (Math.Abs(_position.Z - lpos.Z) < poserror)
+                            && (Math.Abs(_orientation.X - ori.X) < angerror)
+                            && (Math.Abs(_orientation.Y - ori.Y) < angerror)
+                            && (Math.Abs(_orientation.Z - ori.Z) < angerror)  // ignore W
+                            )
+                            _zeroFlag = true;
+                        else
+                            _zeroFlag = false;
+                    }
+
+                    // update position
+                    if (!(_zeroFlag && lastZeroFlag))
+                    {
+                        _position.X = lpos.X;
+                        _position.Y = lpos.Y;
+                        _position.Z = lpos.Z;
+
+                        _orientation.X = ori.X;
+                        _orientation.Y = ori.Y;
+                        _orientation.Z = ori.Z;
+                        _orientation.W = ori.W;
+                    }
+
+                    // update velocities and acceleration
+                    if (_zeroFlag || lastZeroFlag)
+                    {
+                         // disable interpolators
+                        _velocity = Vector3.Zero;
+                        m_acceleration = Vector3.Zero;
+                        m_rotationalVelocity = Vector3.Zero;
+                    }
+                    else
+                    {
+                        d.Vector3 vel = d.BodyGetLinearVel(Body);
+
+                        m_acceleration = _velocity;
+
+                        if ((Math.Abs(vel.X) < 0.005f) &&
+                            (Math.Abs(vel.Y) < 0.005f) &&
+                            (Math.Abs(vel.Z) < 0.005f))
+                        {
+                            _velocity = Vector3.Zero;
+                            float t = -m_invTimeStep;
+                            m_acceleration = m_acceleration * t;
+                        }
+                        else
+                        {
+                            _velocity.X = vel.X;
+                            _velocity.Y = vel.Y;
+                            _velocity.Z = vel.Z;
+                            m_acceleration = (_velocity - m_acceleration) * m_invTimeStep;
+                        }
+
+                        if ((Math.Abs(m_acceleration.X) < 0.01f) &&
+                            (Math.Abs(m_acceleration.Y) < 0.01f) &&
+                            (Math.Abs(m_acceleration.Z) < 0.01f))
+                        {
+                            m_acceleration = Vector3.Zero;
+                        }
+
+                        vel = d.BodyGetAngularVel(Body);
+                        if ((Math.Abs(vel.X) < 0.0001) &&
+                            (Math.Abs(vel.Y) < 0.0001) &&
+                            (Math.Abs(vel.Z) < 0.0001)
+                            )
+                        {
+                            m_rotationalVelocity = Vector3.Zero;
+                        }
+                        else
+                        {
+                            m_rotationalVelocity.X = vel.X;
+                            m_rotationalVelocity.Y = vel.Y;
+                            m_rotationalVelocity.Z = vel.Z;
+                        }
+                    }
+
+                    if (_zeroFlag)
+                    {
+                        if (!m_lastUpdateSent)
+                        {
+                            base.RequestPhysicsterseUpdate();
+                            if (lastZeroFlag)
+                                m_lastUpdateSent = true;
+                        }
+                        return;
+                    }
+
+                    base.RequestPhysicsterseUpdate();
+                    m_lastUpdateSent = false;
+                }
+            }
+        }
+
+        internal static bool QuaternionIsFinite(Quaternion q)
+        {
+            if (Single.IsNaN(q.X) || Single.IsInfinity(q.X))
+                return false;
+            if (Single.IsNaN(q.Y) || Single.IsInfinity(q.Y))
+                return false;
+            if (Single.IsNaN(q.Z) || Single.IsInfinity(q.Z))
+                return false;
+            if (Single.IsNaN(q.W) || Single.IsInfinity(q.W))
+                return false;
+            return true;
+        }
+
+        internal static void DMassSubPartFromObj(ref d.Mass part, ref d.Mass theobj)
+        {
+            // assumes object center of mass is zero
+            float smass = part.mass;
+            theobj.mass -= smass;
+
+            smass *= 1.0f / (theobj.mass); ;
+
+            theobj.c.X -= part.c.X * smass;
+            theobj.c.Y -= part.c.Y * smass;
+            theobj.c.Z -= part.c.Z * smass;
+
+            theobj.I.M00 -= part.I.M00;
+            theobj.I.M01 -= part.I.M01;
+            theobj.I.M02 -= part.I.M02;
+            theobj.I.M10 -= part.I.M10;
+            theobj.I.M11 -= part.I.M11;
+            theobj.I.M12 -= part.I.M12;
+            theobj.I.M20 -= part.I.M20;
+            theobj.I.M21 -= part.I.M21;
+            theobj.I.M22 -= part.I.M22;
+        }
+
+        private void donullchange()
+        {
+        }
+
+        public bool DoAChange(changes what, object arg)
+        {
+            if (prim_geom == IntPtr.Zero && what != changes.Add && what != changes.AddPhysRep && what != changes.Remove)
+            {
+                return false;
+            }
+
+            // nasty switch
+            switch (what)
+            {
+                case changes.Add:
+                    changeadd();
+                    break;
+
+                case changes.AddPhysRep:
+                    changeAddPhysRep((ODEPhysRepData)arg);
+                    break;
+
+                case changes.Remove:
+                    //If its being removed, we don't want to rebuild the physical rep at all, so ignore this stuff...
+                    //When we return true, it destroys all of the prims in the linkset anyway
+                    if (_parent != null)
+                    {
+                        OdePrim parent = (OdePrim)_parent;
+                        parent.ChildRemove(this, false);
+                    }
+                    else
+                        ChildRemove(this, false);
+
+                    m_vehicle = null;
+                    RemoveGeom();
+                    m_targetSpace = IntPtr.Zero;
+                    UnSubscribeEvents();
+                    return true;
+
+                case changes.Link:
+                    OdePrim tmp = (OdePrim)arg;
+                    changeLink(tmp);
+                    break;
+
+                case changes.DeLink:
+                    changeLink(null);
+                    break;
+
+                case changes.Position:
+                    changePosition((Vector3)arg);
+                    break;
+
+                case changes.Orientation:
+                    changeOrientation((Quaternion)arg);
+                    break;
+
+                case changes.PosOffset:
+                    donullchange();
+                    break;
+
+                case changes.OriOffset:
+                    donullchange();
+                    break;
+
+                case changes.Velocity:
+                    changevelocity((Vector3)arg);
+                    break;
+
+                case changes.TargetVelocity:
+                    break;
+
+//                case changes.Acceleration:
+//                    changeacceleration((Vector3)arg);
+//                    break;
+
+                case changes.AngVelocity:
+                    changeangvelocity((Vector3)arg);
+                    break;
+
+                case changes.Force:
+                    changeForce((Vector3)arg);
+                    break;
+
+                case changes.Torque:
+                    changeSetTorque((Vector3)arg);
+                    break;
+
+                case changes.AddForce:
+                    changeAddForce((Vector3)arg);
+                    break;
+
+                case changes.AddAngForce:
+                    changeAddAngularImpulse((Vector3)arg);
+                    break;
+
+                case changes.AngLock:
+                    changeAngularLock((byte)arg);
+                    break;
+
+                case changes.Size:
+                    changeSize((Vector3)arg);
+                    break;
+
+                case changes.Shape:
+                    changeShape((PrimitiveBaseShape)arg);
+                    break;
+
+                case changes.PhysRepData:
+                    changePhysRepData((ODEPhysRepData) arg);
+                    break;
+
+                case changes.CollidesWater:
+                    changeFloatOnWater((bool)arg);
+                    break;
+
+                case changes.VolumeDtc:
+                    changeVolumedetetion((bool)arg);
+                    break;
+
+                case changes.Phantom:
+                    changePhantomStatus((bool)arg);
+                    break;
+
+                case changes.Physical:
+                    changePhysicsStatus((bool)arg);
+                    break;
+
+                case changes.Selected:
+                    changeSelectedStatus((bool)arg);
+                    break;
+
+                case changes.disabled:
+                    changeDisable((bool)arg);
+                    break;
+
+                case changes.building:
+                    changeBuilding((bool)arg);
+                    break;
+
+                case changes.VehicleType:
+                    changeVehicleType((int)arg);
+                    break;
+
+                case changes.VehicleFlags:
+                    changeVehicleFlags((strVehicleBoolParam) arg);
+                    break;
+
+                case changes.VehicleFloatParam:
+                    changeVehicleFloatParam((strVehicleFloatParam) arg);
+                    break;
+
+                case changes.VehicleVectorParam:
+                    changeVehicleVectorParam((strVehicleVectorParam) arg);
+                    break;
+
+                case changes.VehicleRotationParam:
+                    changeVehicleRotationParam((strVehicleQuatParam) arg);
+                    break;
+
+                case changes.SetVehicle:
+                    changeSetVehicle((VehicleData) arg);
+                    break;
+
+                case changes.Buoyancy:
+                    changeBuoyancy((float)arg);
+                    break;
+
+                case changes.PIDTarget:
+                    changePIDTarget((Vector3)arg);
+                    break;
+
+                case changes.PIDTau:
+                    changePIDTau((float)arg);
+                    break;
+
+                case changes.PIDActive:
+                    changePIDActive((bool)arg);
+                    break;
+
+                case changes.PIDHoverHeight:
+                    changePIDHoverHeight((float)arg);
+                    break;
+
+                case changes.PIDHoverType:
+                    changePIDHoverType((PIDHoverType)arg);
+                    break;
+
+                case changes.PIDHoverTau:
+                    changePIDHoverTau((float)arg);
+                    break;
+
+                case changes.PIDHoverActive:
+                    changePIDHoverActive((bool)arg);
+                    break;
+
+                case changes.SetInertia:
+                    changeInertia((PhysicsInertiaData) arg);
+                    break;
+
+                case changes.Null:
+                    donullchange();
+                    break;
+
+                default:
+                    donullchange();
+                    break;
+            }
+            return false;
+        }
+
+        public void AddChange(changes what, object arg)
+        {
+            _parent_scene.AddChange((PhysicsActor) this, what, arg);
+        }
+
+        private struct strVehicleBoolParam
+        {
+            public int param;
+            public bool value;
+        }
+
+        private struct strVehicleFloatParam
+        {
+            public int param;
+            public float value;
+        }
+
+        private struct strVehicleQuatParam
+        {
+            public int param;
+            public Quaternion value;
+        }
+
+        private struct strVehicleVectorParam
+        {
+            public int param;
+            public Vector3 value;
+        }
+    }
+}
diff --git a/OpenSim/Region/PhysicsModules/ubOde/ODERayCastRequestManager.cs b/OpenSim/Region/PhysicsModules/ubOde/ODERayCastRequestManager.cs
new file mode 100644
index 0000000..f671722
--- /dev/null
+++ b/OpenSim/Region/PhysicsModules/ubOde/ODERayCastRequestManager.cs
@@ -0,0 +1,689 @@
+/*
+ * Copyright (c) Contributors, http://opensimulator.org/
+ * See CONTRIBUTORS.TXT for a full list of copyright holders.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the OpenSimulator Project nor the
+ *       names of its contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+using System;
+using System.Collections.Generic;
+using System.Reflection;
+using System.Runtime.InteropServices;
+using System.Text;
+using OpenSim.Framework;
+using OpenSim.Region.PhysicsModules.SharedBase;
+using OdeAPI;
+using log4net;
+using OpenMetaverse;
+
+namespace OpenSim.Region.PhysicsModule.ubOde
+{
+    /// <summary>
+    /// Processes raycast requests as ODE is in a state to be able to do them.
+    /// This ensures that it's thread safe and there will be no conflicts.
+    /// Requests get returned by a different thread then they were requested by.
+    /// </summary>
+    public class ODERayCastRequestManager
+    {
+        /// <summary>
+        /// Pending ray requests
+        /// </summary>
+        protected OpenSim.Framework.LocklessQueue<ODERayRequest> m_PendingRequests = new OpenSim.Framework.LocklessQueue<ODERayRequest>();
+
+        /// <summary>
+        /// Scene that created this object.
+        /// </summary>
+        private ODEScene m_scene;
+
+        IntPtr ray; // the ray. we only need one for our lifetime
+        IntPtr Sphere;
+        IntPtr Box;
+        IntPtr Plane;
+
+        private int CollisionContactGeomsPerTest = 25;
+        private const int DefaultMaxCount = 25;
+        private const int MaxTimePerCallMS = 30;
+
+        /// <summary>
+        /// ODE near callback delegate
+        /// </summary>
+        private d.NearCallback nearCallback;
+        private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
+        private List<ContactResult> m_contactResults = new List<ContactResult>();
+        private RayFilterFlags CurrentRayFilter;
+        private int CurrentMaxCount;
+
+        public ODERayCastRequestManager(ODEScene pScene)
+        {
+            m_scene = pScene;
+            nearCallback = near;
+            ray = d.CreateRay(IntPtr.Zero, 1.0f);
+            d.GeomSetCategoryBits(ray, 0);
+            Box = d.CreateBox(IntPtr.Zero, 1.0f, 1.0f, 1.0f);
+            d.GeomSetCategoryBits(Box, 0);
+            Sphere = d.CreateSphere(IntPtr.Zero,1.0f);
+            d.GeomSetCategoryBits(Sphere, 0);
+            Plane = d.CreatePlane(IntPtr.Zero, 0f,0f,1f,1f);
+            d.GeomSetCategoryBits(Sphere, 0);
+        }
+
+        public void QueueRequest(ODERayRequest req)
+        {
+            if (req.Count == 0)
+                req.Count = DefaultMaxCount;
+
+            m_PendingRequests.Enqueue(req);
+        }
+
+        /// <summary>
+        /// Process all queued raycast requests
+        /// </summary>
+        /// <returns>Time in MS the raycasts took to process.</returns>
+        public int ProcessQueuedRequests()
+        {
+
+            if (m_PendingRequests.Count <= 0)
+                return 0;
+
+            if (m_scene.ContactgeomsArray == IntPtr.Zero || ray == IntPtr.Zero)
+                // oops something got wrong or scene isn't ready still
+            {
+                m_PendingRequests.Clear();
+                return 0;
+            }
+
+            int time = Util.EnvironmentTickCount();
+
+            ODERayRequest req;
+            int closestHit;
+            int backfacecull;
+            CollisionCategories catflags;
+
+            while (m_PendingRequests.Dequeue(out req))
+            {
+                if (req.callbackMethod != null)
+                {
+                    IntPtr geom = IntPtr.Zero;
+                    if (req.actor != null)
+                    {
+                        if (m_scene.haveActor(req.actor))
+                        {
+                            if (req.actor is OdePrim)
+                                geom = ((OdePrim)req.actor).prim_geom;
+                            else if (req.actor is OdeCharacter)
+                                geom = ((OdePrim)req.actor).prim_geom;
+                        }
+                        if (geom == IntPtr.Zero)
+                        {
+                            NoContacts(req);
+                            continue;
+                        }
+                    }
+
+                    CurrentRayFilter = req.filter;
+                    CurrentMaxCount = req.Count;
+
+                    CollisionContactGeomsPerTest = req.Count & 0xffff;
+
+                    closestHit = ((CurrentRayFilter & RayFilterFlags.ClosestHit) == 0 ? 0 : 1);
+                    backfacecull = ((CurrentRayFilter & RayFilterFlags.BackFaceCull) == 0 ? 0 : 1);
+
+                    if (req.callbackMethod is ProbeBoxCallback)
+                    {
+                        if (CollisionContactGeomsPerTest > 80)
+                            CollisionContactGeomsPerTest = 80;
+                        d.GeomBoxSetLengths(Box, req.Normal.X, req.Normal.Y, req.Normal.Z);
+                        d.GeomSetPosition(Box, req.Origin.X, req.Origin.Y, req.Origin.Z);
+                        d.Quaternion qtmp;
+                        qtmp.X = req.orientation.X;
+                        qtmp.Y = req.orientation.Y;
+                        qtmp.Z = req.orientation.Z;
+                        qtmp.W = req.orientation.W;
+                        d.GeomSetQuaternion(Box, ref qtmp);
+                    }
+                    else if (req.callbackMethod is ProbeSphereCallback)
+                    {
+                        if (CollisionContactGeomsPerTest > 80)
+                            CollisionContactGeomsPerTest = 80;
+
+                        d.GeomSphereSetRadius(Sphere, req.length);
+                        d.GeomSetPosition(Sphere, req.Origin.X, req.Origin.Y, req.Origin.Z);
+                    }
+                    else if (req.callbackMethod is ProbePlaneCallback)
+                    {
+                        if (CollisionContactGeomsPerTest > 80)
+                            CollisionContactGeomsPerTest = 80;
+
+                        d.GeomPlaneSetParams(Plane, req.Normal.X, req.Normal.Y, req.Normal.Z, req.length);
+                    }
+
+                    else
+                    {
+                        if (CollisionContactGeomsPerTest > 25)
+                            CollisionContactGeomsPerTest = 25;
+
+                        d.GeomRaySetLength(ray, req.length);
+                        d.GeomRaySet(ray, req.Origin.X, req.Origin.Y, req.Origin.Z, req.Normal.X, req.Normal.Y, req.Normal.Z);
+                        d.GeomRaySetParams(ray, 0, backfacecull);
+
+                        if (req.callbackMethod is RaycastCallback)
+                        {
+                            // if we only want one get only one per Collision pair saving memory
+                            CurrentRayFilter |= RayFilterFlags.ClosestHit;
+                            d.GeomRaySetClosestHit(ray, 1);
+                        }
+                        else
+                            d.GeomRaySetClosestHit(ray, closestHit);
+                    }
+
+                    if ((CurrentRayFilter & RayFilterFlags.ContactsUnImportant) != 0)
+                        unchecked
+                        {
+                            CollisionContactGeomsPerTest |= (int)d.CONTACTS_UNIMPORTANT;
+                        }
+
+                    if (geom == IntPtr.Zero)
+                    {
+                        // translate ray filter to Collision flags
+                        catflags = 0;
+                        if ((CurrentRayFilter & RayFilterFlags.volumedtc) != 0)
+                            catflags |= CollisionCategories.VolumeDtc;
+                        if ((CurrentRayFilter & RayFilterFlags.phantom) != 0)
+                            catflags |= CollisionCategories.Phantom;
+                        if ((CurrentRayFilter & RayFilterFlags.agent) != 0)
+                            catflags |= CollisionCategories.Character;
+                        if ((CurrentRayFilter & RayFilterFlags.PrimsNonPhantom) != 0)
+                            catflags |= CollisionCategories.Geom;
+                        if ((CurrentRayFilter & RayFilterFlags.land) != 0)
+                            catflags |= CollisionCategories.Land;
+                        if ((CurrentRayFilter & RayFilterFlags.water) != 0)
+                            catflags |= CollisionCategories.Water;
+
+                        if (catflags != 0)
+                        {
+                            if (req.callbackMethod is ProbeBoxCallback)
+                            {
+                                catflags |= CollisionCategories.Space;
+                                d.GeomSetCollideBits(Box, (uint)catflags);
+                                d.GeomSetCategoryBits(Box, (uint)catflags);
+                                doProbe(req, Box);
+                            }
+                            else if (req.callbackMethod is ProbeSphereCallback)
+                            {
+                                catflags |= CollisionCategories.Space;
+                                d.GeomSetCollideBits(Sphere, (uint)catflags);
+                                d.GeomSetCategoryBits(Sphere, (uint)catflags);
+                                doProbe(req, Sphere);
+                            }
+                            else if (req.callbackMethod is ProbePlaneCallback)
+                            {
+                                catflags |= CollisionCategories.Space;
+                                d.GeomSetCollideBits(Plane, (uint)catflags);
+                                d.GeomSetCategoryBits(Plane, (uint)catflags);
+                                doPlane(req,IntPtr.Zero);
+                            }
+                            else
+                            {
+                                d.GeomSetCollideBits(ray, (uint)catflags);
+                                doSpaceRay(req);
+                            }
+                        }
+                    }
+                    else
+                    {
+                        // if we select a geom don't use filters
+
+                        if (req.callbackMethod is ProbePlaneCallback)
+                        {
+                            d.GeomSetCollideBits(Plane, (uint)CollisionCategories.All);
+                            doPlane(req,geom);
+                        }
+                        else
+                        {
+                            d.GeomSetCollideBits(ray, (uint)CollisionCategories.All);
+                            doGeomRay(req,geom);
+                        }
+                    }
+                }
+
+                if (Util.EnvironmentTickCountSubtract(time) > MaxTimePerCallMS)
+                    break;
+            }
+
+            lock (m_contactResults)
+                m_contactResults.Clear();
+
+            return Util.EnvironmentTickCountSubtract(time);
+        }
+        /// <summary>
+        /// Method that actually initiates the raycast with spaces
+        /// </summary>
+        /// <param name="req"></param>
+        ///
+
+        private void NoContacts(ODERayRequest req)
+        {
+            if (req.callbackMethod is RaycastCallback)
+            {
+                ((RaycastCallback)req.callbackMethod)(false, Vector3.Zero, 0, 0, Vector3.Zero);
+                return;
+            }
+            List<ContactResult> cresult = new List<ContactResult>();
+
+            if (req.callbackMethod is RayCallback)
+                ((RayCallback)req.callbackMethod)(cresult);
+            else if (req.callbackMethod is ProbeBoxCallback)
+                ((ProbeBoxCallback)req.callbackMethod)(cresult);
+            else if (req.callbackMethod is ProbeSphereCallback)
+                ((ProbeSphereCallback)req.callbackMethod)(cresult);
+        }
+
+        private const RayFilterFlags FilterActiveSpace = RayFilterFlags.agent | RayFilterFlags.physical | RayFilterFlags.LSLPhantom;
+//        private const RayFilterFlags FilterStaticSpace = RayFilterFlags.water | RayFilterFlags.land | RayFilterFlags.nonphysical | RayFilterFlags.LSLPhanton;
+        private const RayFilterFlags FilterStaticSpace = RayFilterFlags.water | RayFilterFlags.nonphysical | RayFilterFlags.LSLPhantom;
+
+        private void doSpaceRay(ODERayRequest req)
+        {
+            // Collide tests
+            if ((CurrentRayFilter & FilterActiveSpace) != 0)
+            {
+                d.SpaceCollide2(ray, m_scene.ActiveSpace, IntPtr.Zero, nearCallback);
+                d.SpaceCollide2(ray, m_scene.CharsSpace, IntPtr.Zero, nearCallback);
+            }
+            if ((CurrentRayFilter & FilterStaticSpace) != 0 && (m_contactResults.Count < CurrentMaxCount))
+                d.SpaceCollide2(ray, m_scene.StaticSpace, IntPtr.Zero, nearCallback);
+            if ((CurrentRayFilter & RayFilterFlags.land) != 0 && (m_contactResults.Count < CurrentMaxCount))
+            {
+                // current ode land to ray collisions is very bad
+                // so for now limit its range badly
+                if (req.length > 60.0f)
+                {
+                    Vector3 t = req.Normal * req.length;
+                    float tmp = t.X * t.X + t.Y * t.Y;
+                    if(tmp > 2500)
+                    {
+                        float tmp2 = req.length * req.length - tmp + 2500;
+                        tmp2 = (float)Math.Sqrt(tmp2);
+                        d.GeomRaySetLength(ray, tmp2);
+                    }
+
+                }
+                d.SpaceCollide2(ray, m_scene.GroundSpace, IntPtr.Zero, nearCallback);
+            }
+
+            if (req.callbackMethod is RaycastCallback)
+            {
+                // Define default results
+                bool hitYN = false;
+                uint hitConsumerID = 0;
+                float distance = float.MaxValue;
+                Vector3 closestcontact = Vector3.Zero;
+                Vector3 snormal = Vector3.Zero;
+
+                // Find closest contact and object.
+                lock (m_contactResults)
+                {
+                    foreach (ContactResult cResult in m_contactResults)
+                    {
+                        if(cResult.Depth < distance)
+                        {
+                            closestcontact = cResult.Pos;
+                            hitConsumerID = cResult.ConsumerID;
+                            distance = cResult.Depth;
+                            snormal = cResult.Normal;
+                        }
+                    }
+                    m_contactResults.Clear();
+                }
+
+                if (distance > 0 && distance < float.MaxValue)
+                    hitYN = true;
+                ((RaycastCallback)req.callbackMethod)(hitYN, closestcontact, hitConsumerID, distance, snormal);
+            }
+            else
+            {
+                List<ContactResult> cresult = new List<ContactResult>(m_contactResults.Count);
+                lock (m_PendingRequests)
+                {
+                    cresult.AddRange(m_contactResults);
+                    m_contactResults.Clear();
+                }
+                ((RayCallback)req.callbackMethod)(cresult);
+            }
+        }
+
+        private void doProbe(ODERayRequest req, IntPtr probe)
+        {
+            // Collide tests
+            if ((CurrentRayFilter & FilterActiveSpace) != 0)
+            {
+                d.SpaceCollide2(probe, m_scene.ActiveSpace, IntPtr.Zero, nearCallback);
+                d.SpaceCollide2(probe, m_scene.CharsSpace, IntPtr.Zero, nearCallback);
+            }
+            if ((CurrentRayFilter & FilterStaticSpace) != 0 && (m_contactResults.Count < CurrentMaxCount))
+                d.SpaceCollide2(probe, m_scene.StaticSpace, IntPtr.Zero, nearCallback);
+            if ((CurrentRayFilter & RayFilterFlags.land) != 0 && (m_contactResults.Count < CurrentMaxCount))
+                d.SpaceCollide2(probe, m_scene.GroundSpace, IntPtr.Zero, nearCallback);
+
+            List<ContactResult> cresult = new List<ContactResult>(m_contactResults.Count);
+            lock (m_PendingRequests)
+            {
+                cresult.AddRange(m_contactResults);
+                m_contactResults.Clear();
+            }
+            if (req.callbackMethod is ProbeBoxCallback)
+                ((ProbeBoxCallback)req.callbackMethod)(cresult);
+            else if (req.callbackMethod is ProbeSphereCallback)
+                ((ProbeSphereCallback)req.callbackMethod)(cresult);
+        }
+
+        private void doPlane(ODERayRequest req,IntPtr geom)
+        {
+            // Collide tests
+            if (geom == IntPtr.Zero)
+            {
+                if ((CurrentRayFilter & FilterActiveSpace) != 0)
+                {
+                    d.SpaceCollide2(Plane, m_scene.ActiveSpace, IntPtr.Zero, nearCallback);
+                    d.SpaceCollide2(Plane, m_scene.CharsSpace, IntPtr.Zero, nearCallback);
+                }
+                if ((CurrentRayFilter & FilterStaticSpace) != 0 && (m_contactResults.Count < CurrentMaxCount))
+                    d.SpaceCollide2(Plane, m_scene.StaticSpace, IntPtr.Zero, nearCallback);
+                if ((CurrentRayFilter & RayFilterFlags.land) != 0 && (m_contactResults.Count < CurrentMaxCount))
+                    d.SpaceCollide2(Plane, m_scene.GroundSpace, IntPtr.Zero, nearCallback);
+            }
+            else
+            {
+                d.SpaceCollide2(Plane, geom, IntPtr.Zero, nearCallback);
+            }
+
+            List<ContactResult> cresult = new List<ContactResult>(m_contactResults.Count);
+            lock (m_PendingRequests)
+            {
+                cresult.AddRange(m_contactResults);
+                m_contactResults.Clear();
+            }
+
+            ((ProbePlaneCallback)req.callbackMethod)(cresult);
+        }
+
+        /// <summary>
+        /// Method that actually initiates the raycast with a geom
+        /// </summary>
+        /// <param name="req"></param>
+        private void doGeomRay(ODERayRequest req, IntPtr geom)
+        {
+            // Collide test
+            d.SpaceCollide2(ray, geom, IntPtr.Zero, nearCallback); // still do this to have full AABB pre test
+
+            if (req.callbackMethod is RaycastCallback)
+            {
+                // Define default results
+                bool hitYN = false;
+                uint hitConsumerID = 0;
+                float distance = float.MaxValue;
+                Vector3 closestcontact = Vector3.Zero;
+                Vector3 snormal = Vector3.Zero;
+
+                // Find closest contact and object.
+                lock (m_contactResults)
+                {
+                    foreach (ContactResult cResult in m_contactResults)
+                    {
+                        if(cResult.Depth < distance )
+                        {
+                            closestcontact = cResult.Pos;
+                            hitConsumerID = cResult.ConsumerID;
+                            distance = cResult.Depth;
+                            snormal = cResult.Normal;
+                        }
+                    }
+                    m_contactResults.Clear();
+                }
+
+                if (distance > 0 && distance < float.MaxValue)
+                    hitYN = true;
+
+                ((RaycastCallback)req.callbackMethod)(hitYN, closestcontact, hitConsumerID, distance, snormal);
+            }
+            else
+            {
+                List<ContactResult> cresult = new List<ContactResult>(m_contactResults.Count);
+                lock (m_PendingRequests)
+                {
+                    cresult.AddRange(m_contactResults);
+                    m_contactResults.Clear();
+                }
+                ((RayCallback)req.callbackMethod)(cresult);
+            }
+        }
+
+        private bool GetCurContactGeom(int index, ref d.ContactGeom newcontactgeom)
+        {
+            IntPtr ContactgeomsArray = m_scene.ContactgeomsArray;
+            if (ContactgeomsArray == IntPtr.Zero || index >= CollisionContactGeomsPerTest)
+                return false;
+
+            IntPtr contactptr = new IntPtr(ContactgeomsArray.ToInt64() + (Int64)(index * d.ContactGeom.unmanagedSizeOf));
+            newcontactgeom = (d.ContactGeom)Marshal.PtrToStructure(contactptr, typeof(d.ContactGeom));
+            return true;
+        }
+
+        // This is the standard Near. g1 is the ray
+        private void near(IntPtr space, IntPtr g1, IntPtr g2)
+        {
+             if (g2 == IntPtr.Zero || g1 == g2)
+                return;
+
+             if (m_contactResults.Count >= CurrentMaxCount)
+                return;
+
+            if (d.GeomIsSpace(g2))
+            {
+                try
+                {
+                    d.SpaceCollide2(g1, g2, IntPtr.Zero, nearCallback);
+                }
+                catch (Exception e)
+                {
+                    m_log.WarnFormat("[PHYSICS Ray]: Unable to Space collide test an object: {0}", e.Message);
+                }
+                return;
+            }
+
+            int count = 0;
+            try
+            {
+                count = d.CollidePtr(g1, g2, CollisionContactGeomsPerTest, m_scene.ContactgeomsArray, d.ContactGeom.unmanagedSizeOf);
+            }
+            catch (Exception e)
+            {
+                m_log.WarnFormat("[PHYSICS Ray]: Unable to collide test an object: {0}", e.Message);
+                return;
+            }
+
+            if (count == 0)
+                return;
+/*
+            uint cat1 = d.GeomGetCategoryBits(g1);
+            uint cat2 = d.GeomGetCategoryBits(g2);
+            uint col1 = d.GeomGetCollideBits(g1);
+            uint col2 = d.GeomGetCollideBits(g2);
+*/
+
+            uint ID = 0;
+            PhysicsActor p2 = null;
+
+            m_scene.actor_name_map.TryGetValue(g2, out p2);
+
+            if (p2 == null)
+                return;
+
+            switch (p2.PhysicsActorType)
+            {
+                case (int)ActorTypes.Prim:
+
+                    RayFilterFlags thisFlags;
+
+                    if (p2.IsPhysical)
+                        thisFlags = RayFilterFlags.physical;
+                    else
+                        thisFlags = RayFilterFlags.nonphysical;
+
+                    if (p2.Phantom)
+                        thisFlags |= RayFilterFlags.phantom;
+
+                    if (p2.IsVolumeDtc)
+                        thisFlags |= RayFilterFlags.volumedtc;
+
+                    if ((thisFlags & CurrentRayFilter) == 0)
+                        return;
+
+                    ID = ((OdePrim)p2).LocalID;
+                    break;
+
+                case (int)ActorTypes.Agent:
+
+                    if ((CurrentRayFilter & RayFilterFlags.agent) == 0)
+                        return;
+                    else
+                        ID = ((OdeCharacter)p2).LocalID;
+                    break;
+
+                case (int)ActorTypes.Ground:
+
+                    if ((CurrentRayFilter & RayFilterFlags.land) == 0)
+                        return;
+                    break;
+
+                case (int)ActorTypes.Water:
+
+                    if ((CurrentRayFilter & RayFilterFlags.water) == 0)
+                        return;
+                    break;
+
+                default:
+                    break;
+            }
+
+            d.ContactGeom curcontact = new d.ContactGeom();
+
+            // closestHit for now only works for meshs, so must do it for others
+            if ((CurrentRayFilter & RayFilterFlags.ClosestHit) == 0)
+            {
+                // Loop all contacts, build results.
+                for (int i = 0; i < count; i++)
+                {
+                    if (!GetCurContactGeom(i, ref curcontact))
+                        break;
+
+                    ContactResult collisionresult = new ContactResult();
+                    collisionresult.ConsumerID = ID;
+                    collisionresult.Pos.X = curcontact.pos.X;
+                    collisionresult.Pos.Y = curcontact.pos.Y;
+                    collisionresult.Pos.Z = curcontact.pos.Z;
+                    collisionresult.Depth = curcontact.depth;
+                    collisionresult.Normal.X = curcontact.normal.X;
+                    collisionresult.Normal.Y = curcontact.normal.Y;
+                    collisionresult.Normal.Z = curcontact.normal.Z;
+                    lock (m_contactResults)
+                    {
+                        m_contactResults.Add(collisionresult);
+                        if (m_contactResults.Count >= CurrentMaxCount)
+                            return;
+                    }
+                }
+            }
+            else
+            {
+                // keep only closest contact
+                ContactResult collisionresult = new ContactResult();
+                collisionresult.ConsumerID = ID;
+                collisionresult.Depth = float.MaxValue;
+
+                for (int i = 0; i < count; i++)
+                {
+                    if (!GetCurContactGeom(i, ref curcontact))
+                        break;
+
+                    if (curcontact.depth < collisionresult.Depth)
+                    {
+                        collisionresult.Pos.X = curcontact.pos.X;
+                        collisionresult.Pos.Y = curcontact.pos.Y;
+                        collisionresult.Pos.Z = curcontact.pos.Z;
+                        collisionresult.Depth = curcontact.depth;
+                        collisionresult.Normal.X = curcontact.normal.X;
+                        collisionresult.Normal.Y = curcontact.normal.Y;
+                        collisionresult.Normal.Z = curcontact.normal.Z;
+                    }
+                }
+
+                if (collisionresult.Depth != float.MaxValue)
+                {
+                    lock (m_contactResults)
+                        m_contactResults.Add(collisionresult);
+                }
+            }
+        }
+
+        /// <summary>
+        /// Dereference the creator scene so that it can be garbage collected if needed.
+        /// </summary>
+        internal void Dispose()
+        {
+            m_scene = null;
+            if (ray != IntPtr.Zero)
+            {
+                d.GeomDestroy(ray);
+                ray = IntPtr.Zero;
+            }
+            if (Box != IntPtr.Zero)
+            {
+                d.GeomDestroy(Box);
+                Box = IntPtr.Zero;
+            }
+            if (Sphere != IntPtr.Zero)
+            {
+                d.GeomDestroy(Sphere);
+                Sphere = IntPtr.Zero;
+            }
+            if (Plane != IntPtr.Zero)
+            {
+                d.GeomDestroy(Plane);
+                Plane = IntPtr.Zero;
+            }
+        }
+    }
+
+    public struct ODERayRequest
+    {
+        public PhysicsActor actor;
+        public Vector3 Origin;
+        public Vector3 Normal;
+        public int Count;
+        public float length;
+        public object callbackMethod;
+        public RayFilterFlags filter;
+        public Quaternion orientation;
+    }
+}
diff --git a/OpenSim/Region/PhysicsModules/ubOde/ODEScene.cs b/OpenSim/Region/PhysicsModules/ubOde/ODEScene.cs
new file mode 100644
index 0000000..0b51820
--- /dev/null
+++ b/OpenSim/Region/PhysicsModules/ubOde/ODEScene.cs
@@ -0,0 +1,2824 @@
+/*
+ * Copyright (c) Contributors, http://opensimulator.org/
+ * See CONTRIBUTORS.TXT for a full list of copyright holders.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the OpenSimulator Project nor the
+ *       names of its contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+// Revision 2011/12/13 by Ubit Umarov
+//#define SPAM
+
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Reflection;
+using System.Runtime.InteropServices;
+using System.Threading;
+using System.IO;
+using System.Diagnostics;
+using log4net;
+using Nini.Config;
+using Mono.Addins;
+using OdeAPI;
+using OpenSim.Framework;
+using OpenSim.Region.Framework.Scenes;
+using OpenSim.Region.Framework.Interfaces;
+using OpenSim.Region.PhysicsModules.SharedBase;
+using OpenMetaverse;
+
+namespace OpenSim.Region.PhysicsModule.ubOde
+{
+     // colision flags of things others can colide with
+    // rays, sensors, probes removed since can't  be colided with
+    // The top space where things are placed provided further selection
+    // ie physical are in active space nonphysical in static
+    // this should be exclusive as possible
+
+    [Flags]
+    public enum CollisionCategories : uint
+    {
+        Disabled = 0,
+        //by 'things' types
+        Space =         0x01,
+        Geom =          0x02, // aka prim/part
+        Character =     0x04,
+        Land =          0x08,
+        Water =         0x010,
+
+        // by state
+        Phantom =       0x01000,
+        VolumeDtc =     0x02000,
+        Selected =      0x04000,
+        NoShape =       0x08000,
+
+
+        All =           0xffffffff
+    }
+
+    /// <summary>
+    /// Material type for a primitive
+    /// </summary>
+    public enum Material : int
+    {
+        /// <summary></summary>
+        Stone = 0,
+        /// <summary></summary>
+        Metal = 1,
+        /// <summary></summary>
+        Glass = 2,
+        /// <summary></summary>
+        Wood = 3,
+        /// <summary></summary>
+        Flesh = 4,
+        /// <summary></summary>
+        Plastic = 5,
+        /// <summary></summary>
+        Rubber = 6,
+
+        light = 7 // compatibility with old viewers
+    }
+
+    public enum changes : int
+    {
+        Add = 0,                // arg null. finishs the prim creation. should be used internally only ( to remove later ?)
+        Remove,
+        Link,               // arg AuroraODEPrim new parent prim or null to delink. Makes the prim part of a object with prim parent as root
+        //  or removes from a object if arg is null
+        DeLink,
+        Position,           // arg Vector3 new position in world coords. Changes prim position. Prim must know if it is root or child
+        Orientation,        // arg Quaternion new orientation in world coords. Changes prim position. Prim must know it it is root or child
+        PosOffset,          // not in use
+        // arg Vector3 new position in local coords. Changes prim position in object
+        OriOffset,          // not in use
+        // arg Vector3 new position in local coords. Changes prim position in object
+        Velocity,
+        TargetVelocity,
+        AngVelocity,
+        Acceleration,
+        Force,
+        Torque,
+        Momentum,
+
+        AddForce,
+        AddAngForce,
+        AngLock,
+
+        Buoyancy,
+
+        PIDTarget,
+        PIDTau,
+        PIDActive,
+
+        PIDHoverHeight,
+        PIDHoverType,
+        PIDHoverTau,
+        PIDHoverActive,
+
+        Size,
+        AvatarSize,
+        Shape,
+        PhysRepData,
+        AddPhysRep,
+
+        CollidesWater,
+        VolumeDtc,
+
+        Physical,
+        Phantom,
+        Selected,
+        disabled,
+        building,
+
+        VehicleType,
+        VehicleFloatParam,
+        VehicleVectorParam,
+        VehicleRotationParam,
+        VehicleFlags,
+        SetVehicle,
+        SetInertia,
+
+        Null             //keep this last used do dim the methods array. does nothing but pulsing the prim
+    }
+
+    public struct ODEchangeitem
+    {
+        public PhysicsActor actor;
+        public OdeCharacter character;
+        public changes what;
+        public Object arg;
+    }
+
+    public class ODEScene : PhysicsScene
+    {
+        private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
+
+        public bool m_OSOdeLib = false;
+        public Scene m_frameWorkScene = null;
+
+//        private int threadid = 0;
+
+//        const d.ContactFlags comumContactFlags = d.ContactFlags.SoftERP | d.ContactFlags.SoftCFM |d.ContactFlags.Approx1 | d.ContactFlags.Bounce;
+
+        const d.ContactFlags comumContactFlags = d.ContactFlags.Bounce | d.ContactFlags.Approx1 | d.ContactFlags.Slip1 | d.ContactFlags.Slip2;
+        const float comumContactERP = 0.75f;
+        const float comumContactCFM = 0.0001f;
+        const float comumContactSLIP = 0f;
+
+        float frictionMovementMult = 0.8f;
+
+        float TerrainBounce = 0.001f;
+        float TerrainFriction = 0.3f;
+
+        public float AvatarFriction = 0;// 0.9f * 0.5f;
+
+        // this netx dimensions are only relevant for terrain partition (mega regions)
+        // WorldExtents below has the simulation dimensions
+        // they should be identical except on mega regions
+        private uint m_regionWidth = Constants.RegionSize;
+        private uint m_regionHeight = Constants.RegionSize;
+
+        public float ODE_STEPSIZE = 0.020f;
+        public float HalfOdeStep = 0.01f;
+        public int odetimestepMS = 20; // rounded
+        private float metersInSpace = 25.6f;
+        private float m_timeDilation = 1.0f;
+
+        private double m_lastframe;
+        private double m_lastMeshExpire;
+
+        public float gravityx = 0f;
+        public float gravityy = 0f;
+        public float gravityz = -9.8f;
+
+        private float waterlevel = 0f;
+        private int framecount = 0;
+
+        private float avDensity = 80f;
+        private float avMovementDivisorWalk = 1.3f;
+        private float avMovementDivisorRun = 0.8f;
+        private float minimumGroundFlightOffset = 3f;
+        public float maximumMassObject = 10000.01f;
+        public float geomDefaultDensity = 10.0f;
+
+        public float maximumAngularVelocity = 12.0f; // default 12rad/s
+        public float maxAngVelocitySQ = 144f;   // squared value
+
+        public float bodyPIDD = 35f;
+        public float bodyPIDG = 25;
+
+        public int bodyFramesAutoDisable = 5;
+
+        private d.NearCallback nearCallback;
+
+        private Dictionary<uint,OdePrim> _prims = new Dictionary<uint,OdePrim>();
+        private HashSet<OdeCharacter> _characters = new HashSet<OdeCharacter>();
+        private HashSet<OdePrim> _activeprims = new HashSet<OdePrim>();
+        private HashSet<OdePrim> _activegroups = new HashSet<OdePrim>();
+
+        public OpenSim.Framework.LocklessQueue<ODEchangeitem> ChangesQueue = new OpenSim.Framework.LocklessQueue<ODEchangeitem>();
+
+        /// <summary>
+        /// A list of actors that should receive collision events.
+        /// </summary>
+        private List<PhysicsActor> _collisionEventPrim = new List<PhysicsActor>();
+        private List<PhysicsActor> _collisionEventPrimRemove = new List<PhysicsActor>();
+
+        private HashSet<OdeCharacter> _badCharacter = new HashSet<OdeCharacter>();
+        public Dictionary<IntPtr, PhysicsActor> actor_name_map = new Dictionary<IntPtr, PhysicsActor>();
+
+        private float contactsurfacelayer = 0.002f;
+
+        private int contactsPerCollision = 80;
+        internal IntPtr ContactgeomsArray = IntPtr.Zero;
+        private IntPtr GlobalContactsArray = IntPtr.Zero;
+        private d.Contact SharedTmpcontact = new d.Contact();
+
+        const int maxContactsbeforedeath = 6000;
+        private volatile int m_global_contactcount = 0;
+
+        private IntPtr contactgroup;
+
+        public ContactData[] m_materialContactsData = new ContactData[8];
+
+        private IntPtr TerrainGeom;
+        private float[] TerrainHeightFieldHeight;
+        private GCHandle TerrainHeightFieldHeightsHandler = new GCHandle();
+
+        private int m_physicsiterations = 15;
+        private const float m_SkipFramesAtms = 0.40f; // Drop frames gracefully at a 400 ms lag
+//        private PhysicsActor PANull = new NullPhysicsActor();
+        private float step_time = 0.0f;
+
+        public IntPtr world;
+
+        // split the spaces acording to contents type
+        // ActiveSpace contains characters and active prims
+        // StaticSpace contains land and other that is mostly static in enviroment
+        // this can contain subspaces, like the grid in staticspace
+        // as now space only contains this 2 top spaces
+
+        public IntPtr TopSpace; // the global space
+        public IntPtr ActiveSpace; // space for active prims
+        public IntPtr CharsSpace; // space for active prims
+        public IntPtr StaticSpace; // space for the static things around
+        public IntPtr GroundSpace; // space for ground
+
+        // some speedup variables
+        private int spaceGridMaxX;
+        private int spaceGridMaxY;
+        private float spacesPerMeterX;
+        private float spacesPerMeterY;
+
+        // split static geometry collision into a grid as before
+        private IntPtr[,] staticPrimspace;
+        private IntPtr[] staticPrimspaceOffRegion;
+
+        public Object OdeLock;
+        public static Object SimulationLock;
+
+        public IMesher mesher;
+
+        public IConfigSource m_config;
+
+        public bool physics_logging = false;
+        public int physics_logging_interval = 0;
+        public bool physics_logging_append_existing_logfile = false;
+
+        public Vector2 WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize);
+
+        private ODERayCastRequestManager m_rayCastManager;
+        public ODEMeshWorker m_meshWorker;
+
+        /* maybe needed if ode uses tls
+                private void checkThread()
+                {
+
+                    int th = Thread.CurrentThread.ManagedThreadId;
+                    if(th != threadid)
+                    {
+                        threadid = th;
+                        d.AllocateODEDataForThread(~0U);
+                    }
+                }
+         */
+
+        IConfig physicsconfig = null;
+
+        public ODEScene(Scene pscene, IConfigSource psourceconfig, string pname, string pversion, bool pOSOdeLib)
+        {
+            OdeLock = new Object();
+
+            EngineType = pname;
+            PhysicsSceneName = EngineType + "/" + pscene.RegionInfo.RegionName;
+            EngineName = pname + " " + pversion;
+            m_config = psourceconfig;
+            m_OSOdeLib = pOSOdeLib;
+
+//            m_OSOdeLib = false; //debug
+
+            m_frameWorkScene = pscene;
+
+            m_frameWorkScene.RegisterModuleInterface<PhysicsScene>(this);
+
+            Initialization();
+
+            base.Initialise(m_frameWorkScene.PhysicsRequestAsset,
+                (m_frameWorkScene.Heightmap != null ? m_frameWorkScene.Heightmap.GetFloatsSerialised() : new float[m_frameWorkScene.RegionInfo.RegionSizeX * m_frameWorkScene.RegionInfo.RegionSizeY]),
+                (float)m_frameWorkScene.RegionInfo.RegionSettings.WaterHeight);
+        }
+
+        public void RegionLoaded()
+        {
+            mesher = m_frameWorkScene.RequestModuleInterface<IMesher>();
+            if (mesher == null)
+            {
+                m_log.ErrorFormat("[ubOde] No mesher. module disabled");
+                return;
+            }
+
+            m_meshWorker = new ODEMeshWorker(this, m_log, mesher, physicsconfig);
+            m_frameWorkScene.PhysicsEnabled = true;
+        }
+        /// <summary>
+        /// Initiailizes the scene
+        /// Sets many properties that ODE requires to be stable
+        /// These settings need to be tweaked 'exactly' right or weird stuff happens.
+        /// </summary>
+        private void Initialization()
+        {
+            d.AllocateODEDataForThread(~0U);
+
+            SimulationLock = new Object();
+
+            nearCallback = near;
+
+            m_rayCastManager = new ODERayCastRequestManager(this);
+
+            WorldExtents.X = m_frameWorkScene.RegionInfo.RegionSizeX;
+            m_regionWidth = (uint)WorldExtents.X;
+            WorldExtents.Y = m_frameWorkScene.RegionInfo.RegionSizeY;
+            m_regionHeight = (uint)WorldExtents.Y;
+
+            lock (OdeLock)
+            {
+                // Create the world and the first space
+                try
+                {
+                    world = d.WorldCreate();
+                    TopSpace = d.HashSpaceCreate(IntPtr.Zero);
+
+                    // now the major subspaces
+                    ActiveSpace = d.HashSpaceCreate(TopSpace);
+                    CharsSpace = d.HashSpaceCreate(TopSpace);
+                    StaticSpace = d.HashSpaceCreate(TopSpace);
+                    GroundSpace = d.HashSpaceCreate(TopSpace);
+                }
+                catch
+                {
+                    // i must RtC#FM
+                    // i did!
+                }
+
+                d.HashSpaceSetLevels(TopSpace, -5, 12);
+                d.HashSpaceSetLevels(ActiveSpace, -5, 10);
+                d.HashSpaceSetLevels(CharsSpace, -4, 3);
+                d.HashSpaceSetLevels(StaticSpace, -5, 12);
+                d.HashSpaceSetLevels(GroundSpace, 0, 8);
+
+                // demote to second level
+                d.SpaceSetSublevel(ActiveSpace, 1);
+                d.SpaceSetSublevel(CharsSpace, 1);
+                d.SpaceSetSublevel(StaticSpace, 1);
+                d.SpaceSetSublevel(GroundSpace, 1);
+
+                d.GeomSetCategoryBits(ActiveSpace, (uint)(CollisionCategories.Space |
+                                                        CollisionCategories.Geom |
+                                                        CollisionCategories.Character |
+                                                        CollisionCategories.Phantom |
+                                                        CollisionCategories.VolumeDtc
+                                                        ));
+                d.GeomSetCollideBits(ActiveSpace, (uint)(CollisionCategories.Space |
+                                                        CollisionCategories.Geom |
+                                                        CollisionCategories.Character |
+                                                        CollisionCategories.Phantom |
+                                                        CollisionCategories.VolumeDtc
+                                                        ));
+                d.GeomSetCategoryBits(CharsSpace, (uint)(CollisionCategories.Space |
+                                        CollisionCategories.Geom |
+                                        CollisionCategories.Character |
+                                        CollisionCategories.Phantom |
+                                        CollisionCategories.VolumeDtc
+                                        ));
+                d.GeomSetCollideBits(CharsSpace, 0);
+
+                d.GeomSetCategoryBits(StaticSpace, (uint)(CollisionCategories.Space |
+                                                        CollisionCategories.Geom |
+                                                        //                                                        CollisionCategories.Land |
+                                                        //                                                        CollisionCategories.Water |
+                                                        CollisionCategories.Phantom |
+                                                        CollisionCategories.VolumeDtc
+                                                        ));
+                d.GeomSetCollideBits(StaticSpace, 0);
+
+                d.GeomSetCategoryBits(GroundSpace, (uint)(CollisionCategories.Land));
+                d.GeomSetCollideBits(GroundSpace, 0);
+
+                contactgroup = d.JointGroupCreate(maxContactsbeforedeath + 1);
+                //contactgroup
+
+                d.WorldSetAutoDisableFlag(world, false);
+            }
+
+
+            //  checkThread();
+
+
+            // Defaults
+
+            int contactsPerCollision = 80;
+
+            physicsconfig = null;
+
+            if (m_config != null)
+            {
+                physicsconfig = m_config.Configs["ODEPhysicsSettings"];
+                if (physicsconfig != null)
+                {
+                    gravityx = physicsconfig.GetFloat("world_gravityx", gravityx);
+                    gravityy = physicsconfig.GetFloat("world_gravityy", gravityy);
+                    gravityz = physicsconfig.GetFloat("world_gravityz", gravityz);
+
+                    metersInSpace = physicsconfig.GetFloat("meters_in_small_space", metersInSpace);
+
+                    //                    contactsurfacelayer = physicsconfig.GetFloat("world_contact_surface_layer", contactsurfacelayer);
+
+                    ODE_STEPSIZE = physicsconfig.GetFloat("world_stepsize", ODE_STEPSIZE);
+
+                    avDensity = physicsconfig.GetFloat("av_density", avDensity);
+                    avMovementDivisorWalk = physicsconfig.GetFloat("av_movement_divisor_walk", avMovementDivisorWalk);
+                    avMovementDivisorRun = physicsconfig.GetFloat("av_movement_divisor_run", avMovementDivisorRun);
+
+                    contactsPerCollision = physicsconfig.GetInt("contacts_per_collision", contactsPerCollision);
+
+                    geomDefaultDensity = physicsconfig.GetFloat("geometry_default_density", geomDefaultDensity);
+//                    bodyFramesAutoDisable = physicsconfig.GetInt("body_frames_auto_disable", bodyFramesAutoDisable);
+
+                    physics_logging = physicsconfig.GetBoolean("physics_logging", false);
+                    physics_logging_interval = physicsconfig.GetInt("physics_logging_interval", 0);
+                    physics_logging_append_existing_logfile = physicsconfig.GetBoolean("physics_logging_append_existing_logfile", false);
+
+                    minimumGroundFlightOffset = physicsconfig.GetFloat("minimum_ground_flight_offset", minimumGroundFlightOffset);
+                    maximumMassObject = physicsconfig.GetFloat("maximum_mass_object", maximumMassObject);
+
+                    avDensity *= 3f / 80f;  // scale other engines density option to this
+                }
+            }
+
+            float heartbeat = 1/m_frameWorkScene.FrameTime;
+            maximumAngularVelocity = 0.49f * heartbeat *(float)Math.PI;
+            maxAngVelocitySQ = maximumAngularVelocity * maximumAngularVelocity;
+
+            d.WorldSetCFM(world, comumContactCFM);
+            d.WorldSetERP(world, comumContactERP);
+
+            d.WorldSetGravity(world, gravityx, gravityy, gravityz);
+
+            d.WorldSetLinearDamping(world, 0.001f);
+            d.WorldSetAngularDamping(world, 0.002f);
+            d.WorldSetAngularDampingThreshold(world, 0f);
+            d.WorldSetLinearDampingThreshold(world, 0f);
+            d.WorldSetMaxAngularSpeed(world, maximumAngularVelocity);
+
+            d.WorldSetQuickStepNumIterations(world, m_physicsiterations);
+
+            d.WorldSetContactSurfaceLayer(world, contactsurfacelayer);
+            d.WorldSetContactMaxCorrectingVel(world, 60.0f);
+
+            HalfOdeStep = ODE_STEPSIZE * 0.5f;
+            odetimestepMS = (int)(1000.0f * ODE_STEPSIZE + 0.5f);
+
+            ContactgeomsArray = Marshal.AllocHGlobal(contactsPerCollision * d.ContactGeom.unmanagedSizeOf);
+            GlobalContactsArray = Marshal.AllocHGlobal((maxContactsbeforedeath + 100) * d.Contact.unmanagedSizeOf);
+
+            SharedTmpcontact.geom.g1 = IntPtr.Zero;
+            SharedTmpcontact.geom.g2 = IntPtr.Zero;
+
+            SharedTmpcontact.geom.side1 = -1;
+            SharedTmpcontact.geom.side2 = -1;
+
+            SharedTmpcontact.surface.mode = comumContactFlags;
+            SharedTmpcontact.surface.mu = 0;
+            SharedTmpcontact.surface.bounce = 0;
+            SharedTmpcontact.surface.bounce_vel = 1.5f;
+            SharedTmpcontact.surface.soft_cfm = comumContactCFM;
+            SharedTmpcontact.surface.soft_erp = comumContactERP;
+            SharedTmpcontact.surface.slip1 = comumContactSLIP;
+            SharedTmpcontact.surface.slip2 = comumContactSLIP;
+
+            m_materialContactsData[(int)Material.Stone].mu = 0.8f;
+            m_materialContactsData[(int)Material.Stone].bounce = 0.4f;
+
+            m_materialContactsData[(int)Material.Metal].mu = 0.3f;
+            m_materialContactsData[(int)Material.Metal].bounce = 0.4f;
+
+            m_materialContactsData[(int)Material.Glass].mu = 0.2f;
+            m_materialContactsData[(int)Material.Glass].bounce = 0.7f;
+
+            m_materialContactsData[(int)Material.Wood].mu = 0.6f;
+            m_materialContactsData[(int)Material.Wood].bounce = 0.5f;
+
+            m_materialContactsData[(int)Material.Flesh].mu = 0.9f;
+            m_materialContactsData[(int)Material.Flesh].bounce = 0.3f;
+
+            m_materialContactsData[(int)Material.Plastic].mu = 0.4f;
+            m_materialContactsData[(int)Material.Plastic].bounce = 0.7f;
+
+            m_materialContactsData[(int)Material.Rubber].mu = 0.9f;
+            m_materialContactsData[(int)Material.Rubber].bounce = 0.95f;
+
+            m_materialContactsData[(int)Material.light].mu = 0.0f;
+            m_materialContactsData[(int)Material.light].bounce = 0.0f;
+
+
+            spacesPerMeterX = 1.0f / metersInSpace;
+            spacesPerMeterY = spacesPerMeterX;
+            spaceGridMaxX = (int)(WorldExtents.X * spacesPerMeterX);
+            spaceGridMaxY = (int)(WorldExtents.Y * spacesPerMeterY);
+
+            if (spaceGridMaxX > 24)
+            {
+                spaceGridMaxX = 24;
+                spacesPerMeterX = spaceGridMaxX / WorldExtents.X;
+            }
+
+            if (spaceGridMaxY > 24)
+            {
+                spaceGridMaxY = 24;
+                spacesPerMeterY = spaceGridMaxY / WorldExtents.Y;
+            }
+
+            staticPrimspace = new IntPtr[spaceGridMaxX, spaceGridMaxY];
+
+            // create all spaces now
+            int i, j;
+            IntPtr newspace;
+
+            for (i = 0; i < spaceGridMaxX; i++)
+                for (j = 0; j < spaceGridMaxY; j++)
+                {
+                    newspace = d.HashSpaceCreate(StaticSpace);
+                    d.GeomSetCategoryBits(newspace, (int)CollisionCategories.Space);
+                    waitForSpaceUnlock(newspace);
+                    d.SpaceSetSublevel(newspace, 2);
+                    d.HashSpaceSetLevels(newspace, -2, 8);
+                    d.GeomSetCategoryBits(newspace, (uint)(CollisionCategories.Space |
+                                        CollisionCategories.Geom |
+                                        CollisionCategories.Land |
+                                        CollisionCategories.Water |
+                                        CollisionCategories.Phantom |
+                                        CollisionCategories.VolumeDtc
+                                        ));
+                    d.GeomSetCollideBits(newspace, 0);
+
+                    staticPrimspace[i, j] = newspace;
+                }
+
+            // let this now be index limit
+            spaceGridMaxX--;
+            spaceGridMaxY--;
+
+            // create 4 off world spaces (x<0,x>max,y<0,y>max)
+            staticPrimspaceOffRegion = new IntPtr[4];
+
+            for (i = 0; i < 4; i++)
+            {
+                newspace = d.HashSpaceCreate(StaticSpace);
+                d.GeomSetCategoryBits(newspace, (int)CollisionCategories.Space);
+                waitForSpaceUnlock(newspace);
+                d.SpaceSetSublevel(newspace, 2);
+                d.HashSpaceSetLevels(newspace, -2, 8);
+                d.GeomSetCategoryBits(newspace, (uint)(CollisionCategories.Space |
+                                    CollisionCategories.Geom |
+                                    CollisionCategories.Land |
+                                    CollisionCategories.Water |
+                                    CollisionCategories.Phantom |
+                                    CollisionCategories.VolumeDtc
+                                    ));
+                d.GeomSetCollideBits(newspace, 0);
+
+                staticPrimspaceOffRegion[i] = newspace;
+            }
+
+            m_lastframe = Util.GetTimeStamp();
+            m_lastMeshExpire = m_lastframe;
+            step_time = -1;
+        }
+
+        internal void waitForSpaceUnlock(IntPtr space)
+        {
+            //if (space != IntPtr.Zero)
+                //while (d.SpaceLockQuery(space)) { } // Wait and do nothing
+        }
+
+        #region Collision Detection
+
+        // sets a global contact for a joint for contactgeom , and base contact description)
+        private IntPtr CreateContacJoint(ref d.ContactGeom contactGeom,bool smooth)
+        {
+            if (m_global_contactcount >= maxContactsbeforedeath)
+                return IntPtr.Zero;
+
+            m_global_contactcount++;
+            if(smooth)
+                SharedTmpcontact.geom.depth = contactGeom.depth * 0.05f;
+            else
+                SharedTmpcontact.geom.depth = contactGeom.depth;
+            SharedTmpcontact.geom.pos = contactGeom.pos;
+            SharedTmpcontact.geom.normal = contactGeom.normal;
+
+            IntPtr contact = new IntPtr(GlobalContactsArray.ToInt64() + (Int64)(m_global_contactcount * d.Contact.unmanagedSizeOf));
+            Marshal.StructureToPtr(SharedTmpcontact, contact, true);
+            return d.JointCreateContactPtr(world, contactgroup, contact);
+        }
+
+        private bool GetCurContactGeom(int index, ref d.ContactGeom newcontactgeom)
+        {
+            if (ContactgeomsArray == IntPtr.Zero || index >= contactsPerCollision)
+                return false;
+
+            IntPtr contactptr = new IntPtr(ContactgeomsArray.ToInt64() + (Int64)(index * d.ContactGeom.unmanagedSizeOf));
+            newcontactgeom = (d.ContactGeom)Marshal.PtrToStructure(contactptr, typeof(d.ContactGeom));
+            return true;
+        }
+
+        /// <summary>
+        /// This is our near callback.  A geometry is near a body
+        /// </summary>
+        /// <param name="space">The space that contains the geoms.  Remember, spaces are also geoms</param>
+        /// <param name="g1">a geometry or space</param>
+        /// <param name="g2">another geometry or space</param>
+        ///
+
+        private void near(IntPtr space, IntPtr g1, IntPtr g2)
+        {
+            //  no lock here!  It's invoked from within Simulate(), which is thread-locked
+
+            if (m_global_contactcount >= maxContactsbeforedeath)
+                return;
+
+            // Test if we're colliding a geom with a space.
+            // If so we have to drill down into the space recursively
+
+            if (g1 == IntPtr.Zero || g2 == IntPtr.Zero)
+                return;
+
+            if (d.GeomIsSpace(g1) || d.GeomIsSpace(g2))
+            {
+                // We'll be calling near recursivly if one
+                // of them is a space to find all of the
+                // contact points in the space
+                try
+                {
+                    d.SpaceCollide2(g1, g2, IntPtr.Zero, nearCallback);
+                }
+                catch (AccessViolationException)
+                {
+                    m_log.Warn("[PHYSICS]: Unable to collide test a space");
+                    return;
+                }
+                //here one should check collisions of geoms inside a space
+                // but on each space we only should have geoms that not colide amoung each other
+                // so we don't dig inside spaces
+                return;
+            }
+
+            // get geom bodies to check if we already a joint contact
+            // guess this shouldn't happen now
+            IntPtr b1 = d.GeomGetBody(g1);
+            IntPtr b2 = d.GeomGetBody(g2);
+
+            // d.GeomClassID id = d.GeomGetClass(g1);
+
+            // Figure out how many contact points we have
+            int count = 0;
+            try
+            {
+                // Colliding Geom To Geom
+                // This portion of the function 'was' blatantly ripped off from BoxStack.cs
+
+                if (g1 == g2)
+                    return; // Can't collide with yourself
+
+//                if (b1 != IntPtr.Zero && b2 != IntPtr.Zero && d.AreConnectedExcluding(b1, b2, d.JointType.Contact))
+//                    return;
+                /*
+                // debug
+                                PhysicsActor dp2;
+                                if (d.GeomGetClass(g1) == d.GeomClassID.HeightfieldClass)
+                                {
+                                    d.AABB aabb;
+                                    d.GeomGetAABB(g2, out aabb);
+                                    float x = aabb.MaxX - aabb.MinX;
+                                    float y = aabb.MaxY - aabb.MinY;
+                                    float z = aabb.MaxZ - aabb.MinZ;
+                                    if (x > 60.0f || y > 60.0f || z > 60.0f)
+                                    {
+                                        if (!actor_name_map.TryGetValue(g2, out dp2))
+                                            m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 2");
+                                        else
+                                            m_log.WarnFormat("[PHYSICS]: land versus large prim geo {0},size {1}, AABBsize <{2},{3},{4}>, at {5} ori {6},({7})",
+                                                dp2.Name, dp2.Size, x, y, z,
+                                                dp2.Position.ToString(),
+                                                dp2.Orientation.ToString(),
+                                                dp2.Orientation.Length());
+                                        return;
+                                    }
+                                }
+                //
+                */
+
+
+                if (d.GeomGetCategoryBits(g1) == (uint)CollisionCategories.VolumeDtc ||
+                    d.GeomGetCategoryBits(g2) == (uint)CollisionCategories.VolumeDtc)
+                {
+                    int cflags;
+                    unchecked
+                    {
+                        cflags = (int)(1 | d.CONTACTS_UNIMPORTANT);
+                    }
+                    count = d.CollidePtr(g1, g2, cflags, ContactgeomsArray, d.ContactGeom.unmanagedSizeOf);
+                }
+                else
+                    count = d.CollidePtr(g1, g2, (contactsPerCollision & 0xffff), ContactgeomsArray, d.ContactGeom.unmanagedSizeOf);
+            }
+            catch (SEHException)
+            {
+                m_log.Error("[PHYSICS]: The Operating system shut down ODE because of corrupt memory.  This could be a result of really irregular terrain.  If this repeats continuously, restart using Basic Physics and terrain fill your terrain.  Restarting the sim.");
+                //                ode.drelease(world);
+                base.TriggerPhysicsBasedRestart();
+            }
+            catch (Exception e)
+            {
+                m_log.WarnFormat("[PHYSICS]: Unable to collide test an object: {0}", e.Message);
+                return;
+            }
+
+            // contacts done
+            if (count == 0)
+                return;
+
+            // try get physical actors
+            PhysicsActor p1;
+            PhysicsActor p2;
+
+            if (!actor_name_map.TryGetValue(g1, out p1))
+            {
+                m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 1");
+                return;
+            }
+
+            if (!actor_name_map.TryGetValue(g2, out p2))
+            {
+                m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 2");
+                return;
+            }
+
+
+            // get first contact
+            d.ContactGeom curContact = new d.ContactGeom();
+
+            if (!GetCurContactGeom(0, ref curContact))
+                return;
+
+            ContactPoint maxDepthContact = new ContactPoint();
+
+            // do volume detection case
+            if ((p1.IsVolumeDtc || p2.IsVolumeDtc))
+            {
+                maxDepthContact = new ContactPoint(
+                    new Vector3(curContact.pos.X, curContact.pos.Y, curContact.pos.Z),
+                    new Vector3(curContact.normal.X, curContact.normal.Y, curContact.normal.Z),
+                    curContact.depth, false
+                    );
+
+                collision_accounting_events(p1, p2, maxDepthContact);
+                return;
+            }
+
+            // big messy collision analises
+
+            float mu = 0;
+            float bounce = 0;
+//            bool IgnoreNegSides = false;
+
+            ContactData contactdata1 = new ContactData(0, 0, false);
+            ContactData contactdata2 = new ContactData(0, 0, false);
+
+            bool dop1ava = false;
+            bool dop2ava = false;
+            bool ignore = false;
+            bool smoothMesh = false;
+
+            switch (p1.PhysicsActorType)
+            {
+                case (int)ActorTypes.Agent:
+                    {
+                        dop1ava = true;
+                        switch (p2.PhysicsActorType)
+                        {
+                            case (int)ActorTypes.Agent:
+                            case (int)ActorTypes.Prim:
+                                break;
+
+                            default:
+                                ignore = true; // avatar to terrain and water ignored
+                                break;
+                        }
+                        break;
+                    }
+
+                case (int)ActorTypes.Prim:
+                    {
+                        switch (p2.PhysicsActorType)
+                        {
+                            case (int)ActorTypes.Agent:
+                                dop2ava = true;
+                                break;
+
+                            case (int)ActorTypes.Prim:
+                                Vector3 relV = p1.rootVelocity - p2.rootVelocity;
+                                float relVlenSQ = relV.LengthSquared();
+                                if (relVlenSQ > 0.0001f)
+                                {
+                                    p1.CollidingObj = true;
+                                    p2.CollidingObj = true;
+                                }
+                                p1.getContactData(ref contactdata1);
+                                p2.getContactData(ref contactdata2);
+                                bounce = contactdata1.bounce * contactdata2.bounce;
+                                mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu);
+
+                                if (relVlenSQ > 0.01f)
+                                    mu *= frictionMovementMult;
+
+                                if(d.GeomGetClass(g2) == d.GeomClassID.TriMeshClass &&
+                                    d.GeomGetClass(g1) == d.GeomClassID.TriMeshClass)
+                                    smoothMesh = true;
+                                break;
+
+                            case (int)ActorTypes.Ground:
+                                p1.getContactData(ref contactdata1);
+                                bounce = contactdata1.bounce * TerrainBounce;
+                                mu = (float)Math.Sqrt(contactdata1.mu * TerrainFriction);
+
+                                Vector3 v1 = p1.rootVelocity;
+                                if (Math.Abs(v1.X) > 0.1f || Math.Abs(v1.Y) > 0.1f)
+                                    mu *= frictionMovementMult;
+                                p1.CollidingGround = true;
+
+                                if(d.GeomGetClass(g1) == d.GeomClassID.TriMeshClass)
+                                    smoothMesh = true;
+                                break;
+
+                            case (int)ActorTypes.Water:
+                            default:
+                                ignore = true;
+                                break;
+                        }
+                    }
+                    break;
+
+                case (int)ActorTypes.Ground:
+                    if (p2.PhysicsActorType == (int)ActorTypes.Prim)
+                    {
+                        p2.CollidingGround = true;
+                        p2.getContactData(ref contactdata2);
+                        bounce = contactdata2.bounce * TerrainBounce;
+                        mu = (float)Math.Sqrt(contactdata2.mu * TerrainFriction);
+
+//                        if (curContact.side1 > 0) // should be 2 ?
+//                            IgnoreNegSides = true;
+                        Vector3 v2 = p2.rootVelocity;
+                        if (Math.Abs(v2.X) > 0.1f || Math.Abs(v2.Y) > 0.1f)
+                            mu *= frictionMovementMult;
+
+                        if(d.GeomGetClass(g2) == d.GeomClassID.TriMeshClass)
+                            smoothMesh = true;
+                    }
+                    else
+                        ignore = true;
+                    break;
+
+                case (int)ActorTypes.Water:
+                default:
+                    break;
+            }
+
+            if (ignore)
+                return;
+
+            IntPtr Joint;
+            bool FeetCollision = false;
+            int ncontacts = 0;
+
+            int i = 0;
+
+            maxDepthContact = new ContactPoint();
+            maxDepthContact.PenetrationDepth = float.MinValue;
+            ContactPoint minDepthContact = new ContactPoint();
+            minDepthContact.PenetrationDepth = float.MaxValue;
+
+            SharedTmpcontact.geom.depth = 0;
+            SharedTmpcontact.surface.mu = mu;
+            SharedTmpcontact.surface.bounce = bounce;
+
+            d.ContactGeom altContact = new d.ContactGeom();
+            bool useAltcontact;
+            bool noskip;
+
+            if(dop1ava || dop2ava)
+                smoothMesh = false;
+
+            while (true)
+            {
+                noskip = true;
+                useAltcontact = false;
+
+                if (dop1ava)
+                {
+                    if ((((OdeCharacter)p1).Collide(g1, g2, false, ref curContact, ref altContact , ref useAltcontact, ref FeetCollision)))
+                    {
+                        if (p2.PhysicsActorType == (int)ActorTypes.Agent)
+                        {
+                            p1.CollidingObj = true;
+                            p2.CollidingObj = true;
+                        }
+                        else if (p2.rootVelocity.LengthSquared() > 0.0f)
+                            p2.CollidingObj = true;
+                    }
+                    else
+                        noskip = false;
+                }
+                else if (dop2ava)
+                {
+                if ((((OdeCharacter)p2).Collide(g2, g1, true, ref curContact, ref altContact , ref useAltcontact, ref FeetCollision)))
+                    {
+                    if (p1.PhysicsActorType == (int)ActorTypes.Agent)
+                        {
+                            p1.CollidingObj = true;
+                            p2.CollidingObj = true;
+                        }
+                    else if (p1.rootVelocity.LengthSquared() > 0.0f)
+                        p1.CollidingObj = true;
+                    }
+                else
+                    noskip = false;
+                }
+
+                if (noskip)
+                {
+                    if(useAltcontact)
+                        Joint = CreateContacJoint(ref altContact,smoothMesh);
+                    else
+                        Joint = CreateContacJoint(ref curContact,smoothMesh);
+                    if (Joint == IntPtr.Zero)
+                        break;
+
+                    d.JointAttach(Joint, b1, b2);
+
+                    ncontacts++;
+
+                    if (curContact.depth > maxDepthContact.PenetrationDepth)
+                    {
+                        maxDepthContact.Position.X = curContact.pos.X;
+                        maxDepthContact.Position.Y = curContact.pos.Y;
+                        maxDepthContact.Position.Z = curContact.pos.Z;
+                        maxDepthContact.PenetrationDepth = curContact.depth;
+                        maxDepthContact.CharacterFeet = FeetCollision;
+                    }
+
+                    if (curContact.depth < minDepthContact.PenetrationDepth)
+                    {
+                        minDepthContact.PenetrationDepth = curContact.depth;
+                        minDepthContact.SurfaceNormal.X = curContact.normal.X;
+                        minDepthContact.SurfaceNormal.Y = curContact.normal.Y;
+                        minDepthContact.SurfaceNormal.Z = curContact.normal.Z;
+                    }
+                }
+
+                if (++i >= count)
+                    break;
+
+                if (!GetCurContactGeom(i, ref curContact))
+                    break;
+            }
+
+            if (ncontacts > 0)
+            {
+                maxDepthContact.SurfaceNormal.X = minDepthContact.SurfaceNormal.X;
+                maxDepthContact.SurfaceNormal.Y = minDepthContact.SurfaceNormal.Y;
+                maxDepthContact.SurfaceNormal.Z = minDepthContact.SurfaceNormal.Z;
+
+                collision_accounting_events(p1, p2, maxDepthContact);
+            }
+        }
+
+        private void collision_accounting_events(PhysicsActor p1, PhysicsActor p2, ContactPoint contact)
+        {
+            uint obj2LocalID = 0;
+
+            // update actors collision score
+            if (p1.CollisionScore < float.MaxValue)
+                p1.CollisionScore += 1.0f;
+            if (p2.CollisionScore < float.MaxValue)
+                p2.CollisionScore += 1.0f;
+
+            bool p1events = p1.SubscribedEvents();
+            bool p2events = p2.SubscribedEvents();
+
+            if (p1.IsVolumeDtc)
+                p2events = false;
+            if (p2.IsVolumeDtc)
+                p1events = false;
+
+            if (!p2events && !p1events)
+                return;
+
+            Vector3 vel = Vector3.Zero;
+            if (p2 != null && p2.IsPhysical)
+                vel = p2.rootVelocity;
+
+            if (p1 != null && p1.IsPhysical)
+                vel -= p1.rootVelocity;
+
+            contact.RelativeSpeed = Vector3.Dot(vel, contact.SurfaceNormal);
+
+            switch ((ActorTypes)p1.PhysicsActorType)
+            {
+                case ActorTypes.Agent:
+                case ActorTypes.Prim:
+                {
+                    switch ((ActorTypes)p2.PhysicsActorType)
+                    {
+                        case ActorTypes.Agent:
+                        case ActorTypes.Prim:
+                            if (p2events)
+                            {
+                                //AddCollisionEventReporting(p2);
+                                p2.AddCollisionEvent(p1.ParentActor.LocalID, contact);
+                            }
+                            else if(p1.IsVolumeDtc)
+                                p2.AddVDTCCollisionEvent(p1.ParentActor.LocalID, contact);
+
+                            obj2LocalID = p2.ParentActor.LocalID;
+                            break;
+
+                        case ActorTypes.Ground:
+                        case ActorTypes.Unknown:
+                        default:
+                            obj2LocalID = 0;
+                            break;
+                    }
+                    if (p1events)
+                    {
+                        contact.SurfaceNormal = -contact.SurfaceNormal;
+                        contact.RelativeSpeed = -contact.RelativeSpeed;
+                        //AddCollisionEventReporting(p1);
+                        p1.AddCollisionEvent(obj2LocalID, contact);
+                    }
+                    else if(p2.IsVolumeDtc)
+                    {
+                        contact.SurfaceNormal = -contact.SurfaceNormal;
+                        contact.RelativeSpeed = -contact.RelativeSpeed;
+                        //AddCollisionEventReporting(p1);
+                        p1.AddVDTCCollisionEvent(obj2LocalID, contact);
+                    }
+                    break;
+                }
+                case ActorTypes.Ground:
+                case ActorTypes.Unknown:
+                default:
+                {
+                    if (p2events && !p2.IsVolumeDtc)
+                    {
+                        //AddCollisionEventReporting(p2);
+                        p2.AddCollisionEvent(0, contact);
+                    }
+                    break;
+                }
+            }
+        }
+
+        /// <summary>
+        /// This is our collision testing routine in ODE
+        /// </summary>
+        /// <param name="timeStep"></param>
+        private void collision_optimized()
+        {
+            lock (_characters)
+                {
+                try
+                {
+                    foreach (OdeCharacter chr in _characters)
+                    {
+                        if (chr == null)
+                            continue;
+
+                        chr.IsColliding = false;
+                        // chr.CollidingGround = false; not done here
+                        chr.CollidingObj = false;
+
+                        if(chr.Body == IntPtr.Zero || chr.collider == IntPtr.Zero )
+                            continue;
+
+                        // do colisions with static space
+                        d.SpaceCollide2(chr.collider, StaticSpace, IntPtr.Zero, nearCallback);
+
+                        // no coll with gnd
+                    }
+                    // chars with chars
+                    d.SpaceCollide(CharsSpace, IntPtr.Zero, nearCallback);
+
+                }
+                catch (AccessViolationException)
+                {
+                    m_log.Warn("[PHYSICS]: Unable to collide Character to static space");
+                }
+
+            }
+
+            lock (_activeprims)
+            {
+                foreach (OdePrim aprim in _activeprims)
+                {
+                    aprim.CollisionScore = 0;
+                    aprim.IsColliding = false;
+                    if(!aprim.m_outbounds && d.BodyIsEnabled(aprim.Body))
+                        aprim.clearSleeperCollisions();
+                }
+            }
+
+            lock (_activegroups)
+            {
+                try
+                {
+                    foreach (OdePrim aprim in _activegroups)
+                    {
+                        if(!aprim.m_outbounds && d.BodyIsEnabled(aprim.Body) &&
+                                aprim.collide_geom != IntPtr.Zero)
+                        {
+                            d.SpaceCollide2(StaticSpace, aprim.collide_geom, IntPtr.Zero, nearCallback);
+                            d.SpaceCollide2(GroundSpace, aprim.collide_geom, IntPtr.Zero, nearCallback);
+                        }
+                    }
+                }
+                catch (Exception e)
+                {
+                    m_log.Warn("[PHYSICS]: Unable to collide Active to Static: " + e.Message);
+                }
+            }
+
+            // colide active amoung them
+            try
+            {
+                d.SpaceCollide(ActiveSpace, IntPtr.Zero, nearCallback);
+            }
+            catch (Exception e)
+            {
+                    m_log.Warn("[PHYSICS]: Unable to collide in Active: " + e.Message);
+            }
+
+            // and with chars
+            try
+            {
+                d.SpaceCollide2(CharsSpace,ActiveSpace, IntPtr.Zero, nearCallback);
+            }
+            catch (Exception e)
+            {
+                    m_log.Warn("[PHYSICS]: Unable to collide Active to Character: " + e.Message);
+            }
+        }
+
+        #endregion
+        /// <summary>
+        /// Add actor to the list that should receive collision events in the simulate loop.
+        /// </summary>
+        /// <param name="obj"></param>
+        public void AddCollisionEventReporting(PhysicsActor obj)
+        {
+            if (!_collisionEventPrim.Contains(obj))
+                _collisionEventPrim.Add(obj);
+        }
+
+        /// <summary>
+        /// Remove actor from the list that should receive collision events in the simulate loop.
+        /// </summary>
+        /// <param name="obj"></param>
+        public void RemoveCollisionEventReporting(PhysicsActor obj)
+        {
+            lock(_collisionEventPrimRemove)
+            {
+               if (_collisionEventPrim.Contains(obj) && !_collisionEventPrimRemove.Contains(obj))
+                    _collisionEventPrimRemove.Add(obj);
+            }
+        }
+
+        public override float TimeDilation
+        {
+            get { return m_timeDilation; }
+        }
+
+        public override bool SupportsNINJAJoints
+        {
+            get { return false; }
+        }
+
+        #region Add/Remove Entities
+
+        public override PhysicsActor AddAvatar(string avName, Vector3 position, Vector3 velocity, Vector3 size, bool isFlying)
+        {
+            return null;
+        }
+
+        public override PhysicsActor AddAvatar(uint localID, string avName, Vector3 position, Vector3 size, float feetOffset, bool isFlying)
+        {
+             OdeCharacter newAv = new OdeCharacter(localID, avName, this, position,
+                size, feetOffset, avDensity, avMovementDivisorWalk, avMovementDivisorRun);
+            newAv.Flying = isFlying;
+            newAv.MinimumGroundFlightOffset = minimumGroundFlightOffset;
+
+            return newAv;
+        }
+
+        public void AddCharacter(OdeCharacter chr)
+        {
+            lock (_characters)
+            {
+                if (!_characters.Contains(chr))
+                {
+                    _characters.Add(chr);
+                    if (chr.bad)
+                        m_log.DebugFormat("[PHYSICS] Added BAD actor {0} to characters list", chr.m_uuid);
+                }
+            }
+        }
+
+        public void RemoveCharacter(OdeCharacter chr)
+        {
+            lock (_characters)
+            {
+                if (_characters.Contains(chr))
+                {
+                    _characters.Remove(chr);
+                }
+            }
+        }
+
+        public void BadCharacter(OdeCharacter chr)
+        {
+            lock (_badCharacter)
+            {
+                if (!_badCharacter.Contains(chr))
+                    _badCharacter.Add(chr);
+            }
+        }
+
+        public override void RemoveAvatar(PhysicsActor actor)
+        {
+            //m_log.Debug("[PHYSICS]:ODELOCK");
+            lock (OdeLock)
+            {
+                d.AllocateODEDataForThread(0);
+                ((OdeCharacter) actor).Destroy();
+            }
+        }
+
+
+        public void addActivePrim(OdePrim activatePrim)
+        {
+            // adds active prim..
+            lock (_activeprims)
+            {
+                if (!_activeprims.Contains(activatePrim))
+                    _activeprims.Add(activatePrim);
+            }
+        }
+
+        public void addActiveGroups(OdePrim activatePrim)
+        {
+            lock (_activegroups)
+            {
+                if (!_activegroups.Contains(activatePrim))
+                    _activegroups.Add(activatePrim);
+            }
+        }
+
+        private PhysicsActor AddPrim(String name, Vector3 position, Vector3 size, Quaternion rotation,
+                                     PrimitiveBaseShape pbs, bool isphysical, bool isPhantom, byte shapeType, uint localID)
+        {
+            OdePrim newPrim;
+            lock (OdeLock)
+            {
+
+                newPrim = new OdePrim(name, this, position, size, rotation, pbs, isphysical, isPhantom, shapeType, localID);
+            }
+            return newPrim;
+        }
+
+        public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
+                                                  Vector3 size, Quaternion rotation, bool isPhysical, bool isPhantom, uint localid)
+        {
+            return AddPrim(primName, position, size, rotation, pbs, isPhysical, isPhantom, 0 , localid);
+        }
+
+
+        public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
+                                                  Vector3 size, Quaternion rotation, bool isPhysical, uint localid)
+        {
+            return AddPrim(primName, position, size, rotation, pbs, isPhysical,false, 0, localid);
+        }
+
+        public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
+                                                  Vector3 size, Quaternion rotation, bool isPhysical, bool isPhantom, byte shapeType, uint localid)
+        {
+            return AddPrim(primName, position, size, rotation, pbs, isPhysical,isPhantom, shapeType, localid);
+        }
+
+        public void remActivePrim(OdePrim deactivatePrim)
+        {
+            lock (_activeprims)
+            {
+                _activeprims.Remove(deactivatePrim);
+            }
+        }
+        public void remActiveGroup(OdePrim deactivatePrim)
+        {
+            lock (_activegroups)
+            {
+                _activegroups.Remove(deactivatePrim);
+            }
+        }
+
+        public override void RemovePrim(PhysicsActor prim)
+        {
+            // As with all ODE physics operations, we don't remove the prim immediately but signal that it should be
+            // removed in the next physics simulate pass.
+            if (prim is OdePrim)
+            {
+//                lock (OdeLock)
+                {
+
+                    OdePrim p = (OdePrim)prim;
+                    p.setPrimForRemoval();
+                }
+            }
+        }
+
+        public void RemovePrimThreadLocked(OdePrim prim)
+        {
+            //Console.WriteLine("RemovePrimThreadLocked " +  prim.m_primName);
+            lock (prim)
+            {
+//                RemoveCollisionEventReporting(prim);
+                lock (_prims)
+                    _prims.Remove(prim.LocalID);
+            }
+
+        }
+
+        public void addToPrims(OdePrim prim)
+        {
+            lock (_prims)
+                _prims[prim.LocalID] = prim;
+        }
+
+        public OdePrim getPrim(uint id)
+        {
+            lock (_prims)
+            {
+                if(_prims.ContainsKey(id))
+                    return _prims[id];
+                else
+                    return null;
+            }
+        }
+
+        public bool havePrim(OdePrim prm)
+        {
+            lock (_prims)
+                return _prims.ContainsKey(prm.LocalID);
+        }
+
+        public void changePrimID(OdePrim prim,uint oldID)
+        {
+            lock (_prims)
+            {
+                if(_prims.ContainsKey(oldID))
+                    _prims.Remove(oldID);
+                _prims[prim.LocalID] = prim;
+            }
+        }
+
+        public bool haveActor(PhysicsActor actor)
+        {
+            if (actor is OdePrim)
+            {
+                lock (_prims)
+                    return _prims.ContainsKey(((OdePrim)actor).LocalID);
+            }
+            else if (actor is OdeCharacter)
+            {
+                lock (_characters)
+                    return _characters.Contains((OdeCharacter)actor);
+            }
+            return false;
+        }
+
+        #endregion
+
+        #region Space Separation Calculation
+
+        /// <summary>
+        /// Called when a static prim moves or becomes static
+        /// Places the prim in a space one the static sub-spaces grid
+        /// </summary>
+        /// <param name="geom">the pointer to the geom that moved</param>
+        /// <param name="pos">the position that the geom moved to</param>
+        /// <param name="currentspace">a pointer to the space it was in before it was moved.</param>
+        /// <returns>a pointer to the new space it's in</returns>
+        public IntPtr MoveGeomToStaticSpace(IntPtr geom, Vector3 pos, IntPtr currentspace)
+        {
+            // moves a prim into another static sub-space or from another space into a static sub-space
+
+            // Called ODEPrim so
+            // it's already in locked space.
+
+            if (geom == IntPtr.Zero) // shouldn't happen
+                return IntPtr.Zero;
+
+            // get the static sub-space for current position
+            IntPtr newspace = calculateSpaceForGeom(pos);
+
+            if (newspace == currentspace) // if we are there all done
+                return newspace;
+
+            // else remove it from its current space
+            if (currentspace != IntPtr.Zero && d.SpaceQuery(currentspace, geom))
+            {
+                if (d.GeomIsSpace(currentspace))
+                {
+                    waitForSpaceUnlock(currentspace);
+                    d.SpaceRemove(currentspace, geom);
+
+                    if (d.SpaceGetSublevel(currentspace) > 2 && d.SpaceGetNumGeoms(currentspace) == 0)
+                    {
+                        d.SpaceDestroy(currentspace);
+                    }
+                }
+                else
+                {
+                    m_log.Info("[Physics]: Invalid or empty Space passed to 'MoveGeomToStaticSpace':" + currentspace +
+                                   " Geom:" + geom);
+                }
+            }
+            else // odd currentspace is null or doesn't contain the geom? lets try the geom ideia of current space
+            {
+                currentspace = d.GeomGetSpace(geom);
+                if (currentspace != IntPtr.Zero)
+                {
+                    if (d.GeomIsSpace(currentspace))
+                    {
+                        waitForSpaceUnlock(currentspace);
+                        d.SpaceRemove(currentspace, geom);
+
+                        if (d.SpaceGetSublevel(currentspace) > 2 && d.SpaceGetNumGeoms(currentspace) == 0)
+                        {
+                            d.SpaceDestroy(currentspace);
+                        }
+
+                    }
+                }
+            }
+
+            // put the geom in the newspace
+            waitForSpaceUnlock(newspace);
+            d.SpaceAdd(newspace, geom);
+
+            // let caller know this newspace
+            return newspace;
+        }
+
+        /// <summary>
+        /// Calculates the space the prim should be in by its position
+        /// </summary>
+        /// <param name="pos"></param>
+        /// <returns>a pointer to the space. This could be a new space or reused space.</returns>
+        public IntPtr calculateSpaceForGeom(Vector3 pos)
+        {
+            int x, y;
+
+            if (pos.X < 0)
+                return staticPrimspaceOffRegion[0];
+
+            if (pos.Y < 0)
+                return staticPrimspaceOffRegion[2];
+
+            x = (int)(pos.X * spacesPerMeterX);
+            if (x > spaceGridMaxX)
+                return staticPrimspaceOffRegion[1];
+
+            y = (int)(pos.Y * spacesPerMeterY);
+            if (y > spaceGridMaxY)
+                return staticPrimspaceOffRegion[3];
+
+            return staticPrimspace[x, y];
+        }
+
+        #endregion
+
+
+        /// <summary>
+        /// Called to queue a change to a actor
+        /// to use in place of old taint mechanism so changes do have a time sequence
+        /// </summary>
+
+        public void AddChange(PhysicsActor actor, changes what, Object arg)
+        {
+            ODEchangeitem item = new ODEchangeitem();
+            item.actor = actor;
+            item.what = what;
+            item.arg = arg;
+            ChangesQueue.Enqueue(item);
+        }
+
+        /// <summary>
+        /// Called after our prim properties are set Scale, position etc.
+        /// We use this event queue like method to keep changes to the physical scene occuring in the threadlocked mutex
+        /// This assures us that we have no race conditions
+        /// </summary>
+        /// <param name="prim"></param>
+        public override void AddPhysicsActorTaint(PhysicsActor prim)
+        {
+        }
+
+        // does all pending changes generated during region load process
+        public override void ProcessPreSimulation()
+        {
+            lock (OdeLock)
+            {
+                if (world == IntPtr.Zero)
+                {
+                    ChangesQueue.Clear();
+                    return;
+                }
+
+                d.AllocateODEDataForThread(~0U);
+
+                ODEchangeitem item;
+
+                int donechanges = 0;
+                if (ChangesQueue.Count > 0)
+                {
+                    m_log.InfoFormat("[ubOde] start processing pending actor operations");
+                    int tstart = Util.EnvironmentTickCount();
+
+                    while (ChangesQueue.Dequeue(out item))
+                    {
+                        if (item.actor != null)
+                        {
+                            try
+                            {
+                                if (item.actor is OdeCharacter)
+                                    ((OdeCharacter)item.actor).DoAChange(item.what, item.arg);
+                                else if (((OdePrim)item.actor).DoAChange(item.what, item.arg))
+                                    RemovePrimThreadLocked((OdePrim)item.actor);
+                            }
+                            catch
+                            {
+                                m_log.WarnFormat("[PHYSICS]: Operation failed for a actor {0} {1}",
+                                    item.actor.Name, item.what.ToString());
+                            }
+                        }
+                        donechanges++;
+                    }
+                    int time = Util.EnvironmentTickCountSubtract(tstart);
+                    m_log.InfoFormat("[ubOde] finished {0} operations in {1}ms", donechanges, time);
+                }
+                m_log.InfoFormat("[ubOde] {0} prim actors loaded",_prims.Count);
+            }
+            m_lastframe = Util.GetTimeStamp() + 0.5;
+            step_time = -0.5f;
+        }
+
+        /// <summary>
+        /// This is our main simulate loop
+        /// It's thread locked by a Mutex in the scene.
+        /// It holds Collisions, it instructs ODE to step through the physical reactions
+        /// It moves the objects around in memory
+        /// It calls the methods that report back to the object owners.. (scenepresence, SceneObjectGroup)
+        /// </summary>
+        /// <param name="timeStep"></param>
+        /// <returns></returns>
+        public override float Simulate(float reqTimeStep)
+        {
+            double now = Util.GetTimeStamp();
+            double timeStep = now - m_lastframe;
+            m_lastframe = now;
+
+            // acumulate time so we can reduce error
+            step_time += (float)timeStep;
+
+            if (step_time < HalfOdeStep)
+                return 0;
+
+            if (framecount < 0)
+                framecount = 0;
+
+            framecount++;
+
+//            checkThread();
+            int nodeframes = 0;
+            float fps = 0;
+
+            lock (SimulationLock)
+                lock(OdeLock)
+            {
+                if (world == IntPtr.Zero)
+                {
+                    ChangesQueue.Clear();
+                    return 0;
+                }
+
+                ODEchangeitem item;
+
+//                d.WorldSetQuickStepNumIterations(world, curphysiteractions);
+
+                double loopstartMS = Util.GetTimeStampMS();
+                double looptimeMS = 0;
+                double changestimeMS = 0;
+                double maxChangestime = (int)(reqTimeStep * 500f); // half the time
+                double maxLoopTime = (int)(reqTimeStep * 1200f); // 1.2 the time
+
+//                double collisionTime = 0;
+//                double qstepTIme = 0;
+//                double tmpTime = 0;
+
+                d.AllocateODEDataForThread(~0U);
+
+                if (ChangesQueue.Count > 0)
+                {
+                    while (ChangesQueue.Dequeue(out item))
+                    {
+                        if (item.actor != null)
+                        {
+                            try
+                            {
+                                if (item.actor is OdeCharacter)
+                                    ((OdeCharacter)item.actor).DoAChange(item.what, item.arg);
+                                else if (((OdePrim)item.actor).DoAChange(item.what, item.arg))
+                                    RemovePrimThreadLocked((OdePrim)item.actor);
+                            }
+                            catch
+                            {
+                                m_log.WarnFormat("[PHYSICS]: doChange failed for a actor {0} {1}",
+                                    item.actor.Name, item.what.ToString());
+                            }
+                        }
+                        changestimeMS = Util.GetTimeStampMS() - loopstartMS;
+                        if (changestimeMS > maxChangestime)
+                            break;
+                    }
+                }
+
+                // do simulation taking at most 150ms total time including changes
+                while (step_time > HalfOdeStep)
+                {
+                    try
+                    {
+                        // clear pointer/counter to contacts to pass into joints
+                        m_global_contactcount = 0;
+
+
+                        // Move characters
+                        lock (_characters)
+                        {
+                            List<OdeCharacter> defects = new List<OdeCharacter>();
+                            foreach (OdeCharacter actor in _characters)
+                            {
+                                if (actor != null)
+                                    actor.Move(defects);
+                            }
+                            if (defects.Count != 0)
+                            {
+                                foreach (OdeCharacter defect in defects)
+                                {
+                                    RemoveCharacter(defect);
+                                }
+                                defects.Clear();
+                            }
+                        }
+
+                        // Move other active objects
+                        lock (_activegroups)
+                        {
+                            foreach (OdePrim aprim in _activegroups)
+                            {
+                                aprim.Move();
+                            }
+                        }
+
+                        m_rayCastManager.ProcessQueuedRequests();
+
+//                        tmpTime =  Util.GetTimeStampMS();
+                        collision_optimized();
+//                        collisionTime += Util.GetTimeStampMS() - tmpTime;
+
+                        lock(_collisionEventPrimRemove)
+                        {
+                            foreach (PhysicsActor obj in _collisionEventPrimRemove)
+                                _collisionEventPrim.Remove(obj);
+
+                            _collisionEventPrimRemove.Clear();
+                        }
+
+                        List<OdePrim> sleepers = new List<OdePrim>();
+                        foreach (PhysicsActor obj in _collisionEventPrim)
+                        {
+                            if (obj == null)
+                                continue;
+
+                            switch ((ActorTypes)obj.PhysicsActorType)
+                            {
+                                case ActorTypes.Agent:
+                                    OdeCharacter cobj = (OdeCharacter)obj;
+                                    cobj.SendCollisions((int)(odetimestepMS));
+                                    break;
+
+                                case ActorTypes.Prim:
+                                    OdePrim pobj = (OdePrim)obj;
+                                    if (!pobj.m_outbounds)
+                                    {
+                                        pobj.SendCollisions((int)(odetimestepMS));
+                                        if(pobj.Body != IntPtr.Zero && !pobj.m_isSelected &&
+                                            !pobj.m_disabled && !pobj.m_building &&
+                                            !d.BodyIsEnabled(pobj.Body))
+                                        sleepers.Add(pobj);
+                                    }
+                                    break;
+                            }
+                        }
+
+                        foreach(OdePrim prm in sleepers)
+                            prm.SleeperAddCollisionEvents();
+                        sleepers.Clear();
+ 
+                        // do a ode simulation step
+//                        tmpTime =  Util.GetTimeStampMS();
+                        d.WorldQuickStep(world, ODE_STEPSIZE);
+                        d.JointGroupEmpty(contactgroup);
+//                        qstepTIme += Util.GetTimeStampMS() - tmpTime;
+
+                        // update managed ideia of physical data and do updates to core
+        /*
+                        lock (_characters)
+                        {
+                            foreach (OdeCharacter actor in _characters)
+                            {
+                                if (actor != null)
+                                {
+                                    if (actor.bad)
+                                        m_log.WarnFormat("[PHYSICS]: BAD Actor {0} in _characters list was not removed?", actor.m_uuid);
+
+                                    actor.UpdatePositionAndVelocity();
+                                }
+                            }
+                        }
+        */
+
+                        lock (_activegroups)
+                        {
+                            {
+                                foreach (OdePrim actor in _activegroups)
+                                {
+                                    if (actor.IsPhysical)
+                                    {
+                                        actor.UpdatePositionAndVelocity(framecount);
+                                    }
+                                }
+                            }
+                        }
+                    }
+                    catch (Exception e)
+                    {
+                        m_log.ErrorFormat("[PHYSICS]: {0}, {1}, {2}", e.Message, e.TargetSite, e);
+//                        ode.dunlock(world);
+                    }
+
+                    step_time -= ODE_STEPSIZE;
+                    nodeframes++;
+
+                    looptimeMS = Util.GetTimeStampMS() - loopstartMS;
+                    if (looptimeMS > maxLoopTime)
+                        break;
+                }
+
+                lock (_badCharacter)
+                {
+                    if (_badCharacter.Count > 0)
+                    {
+                        foreach (OdeCharacter chr in _badCharacter)
+                        {
+                            RemoveCharacter(chr);
+                        }
+
+                        _badCharacter.Clear();
+                    }
+                }
+
+// information block for in debug breakpoint only
+/*
+                int ntopactivegeoms = d.SpaceGetNumGeoms(ActiveSpace);
+                int ntopstaticgeoms = d.SpaceGetNumGeoms(StaticSpace);
+                int ngroundgeoms = d.SpaceGetNumGeoms(GroundSpace);
+
+                int nactivegeoms = 0;
+                int nactivespaces = 0;
+
+                int nstaticgeoms = 0;
+                int nstaticspaces = 0;
+                IntPtr sp;
+
+                for (int i = 0; i < ntopactivegeoms; i++)
+                {
+                    sp = d.SpaceGetGeom(ActiveSpace, i);
+                    if (d.GeomIsSpace(sp))
+                    {
+                        nactivespaces++;
+                        nactivegeoms += d.SpaceGetNumGeoms(sp);
+                    }
+                    else
+                        nactivegeoms++;
+                }
+
+                for (int i = 0; i < ntopstaticgeoms; i++)
+                {
+                    sp = d.SpaceGetGeom(StaticSpace, i);
+                    if (d.GeomIsSpace(sp))
+                    {
+                        nstaticspaces++;
+                        nstaticgeoms += d.SpaceGetNumGeoms(sp);
+                    }
+                    else
+                        nstaticgeoms++;
+                }
+
+                int ntopgeoms = d.SpaceGetNumGeoms(TopSpace);
+
+                int totgeoms = nstaticgeoms + nactivegeoms + ngroundgeoms + 1; // one ray
+                int nbodies = d.NTotalBodies;
+                int ngeoms = d.NTotalGeoms;
+*/
+/*
+                looptimeMS /= nodeframes;
+                if(looptimeMS > 0.080)
+                {
+                    collisionTime /= nodeframes;
+                    qstepTIme /= nodeframes;    
+                }
+*/
+                // Finished with all sim stepping. If requested, dump world state to file for debugging.
+                // TODO: This call to the export function is already inside lock (OdeLock) - but is an extra lock needed?
+                // TODO: This overwrites all dump files in-place. Should this be a growing logfile, or separate snapshots?
+                if (physics_logging && (physics_logging_interval > 0) && (framecount % physics_logging_interval == 0))
+                {
+                    string fname = "state-" + world.ToString() + ".DIF"; // give each physics world a separate filename
+                    string prefix = "world" + world.ToString(); // prefix for variable names in exported .DIF file
+
+                    if (physics_logging_append_existing_logfile)
+                    {
+                        string header = "-------------- START OF PHYSICS FRAME " + framecount.ToString() + " --------------";
+                        TextWriter fwriter = File.AppendText(fname);
+                        fwriter.WriteLine(header);
+                        fwriter.Close();
+                    }
+
+                    d.WorldExportDIF(world, fname, physics_logging_append_existing_logfile, prefix);
+                }
+
+                fps = (float)nodeframes * ODE_STEPSIZE / reqTimeStep;
+
+                if(step_time < HalfOdeStep)
+                    m_timeDilation = 1.0f;
+                else if (step_time > m_SkipFramesAtms)
+                {
+                    // if we lag too much skip frames
+                    m_timeDilation = 0.0f;
+                    step_time = 0;
+                    m_lastframe = Util.GetTimeStamp(); // skip also the time lost
+                }
+                else
+                {
+                    m_timeDilation = ODE_STEPSIZE / step_time;
+                    if (m_timeDilation > 1)
+                        m_timeDilation = 1;
+                }
+
+                if (m_timeDilation == 1 && now - m_lastMeshExpire > 30)
+                {
+                    mesher.ExpireReleaseMeshs();
+                    m_lastMeshExpire = now;
+                }
+
+
+            }
+
+            return fps;
+        }
+
+        /// <summary>
+        public override void GetResults()
+        {
+        }
+
+        public override bool IsThreaded
+        {
+            // for now we won't be multithreaded
+            get { return (false); }
+        }
+
+        public float GetTerrainHeightAtXY(float x, float y)
+        {
+            if (TerrainGeom == IntPtr.Zero)
+                return 0f;
+
+            if (TerrainHeightFieldHeight == null || TerrainHeightFieldHeight.Length == 0)
+                return 0f;
+
+            // TerrainHeightField for ODE as offset 1m
+            x += 1f;
+            y += 1f;
+
+            // make position fit into array
+            if (x < 0)
+                x = 0;
+            if (y < 0)
+                y = 0;
+
+            // integer indexs
+            int ix;
+            int iy;
+            //  interpolators offset
+            float dx;
+            float dy;
+
+            int regsizeX = (int)m_regionWidth + 3; // map size see setterrain number of samples
+            int regsizeY = (int)m_regionHeight + 3; // map size see setterrain number of samples
+            int regsize = regsizeX;
+
+            if (m_OSOdeLib)
+            {
+                if (x < regsizeX - 1)
+                {
+                    ix = (int)x;
+                    dx = x - (float)ix;
+                }
+                else // out world use external height
+                {
+                    ix = regsizeX - 2;
+                    dx = 0;
+                }
+                if (y < regsizeY - 1)
+                {
+                    iy = (int)y;
+                    dy = y - (float)iy;
+                }
+                else
+                {
+                    iy = regsizeY - 2;
+                    dy = 0;
+                }
+            }
+            else
+            {
+                // we  still have square fixed size regions
+                // also flip x and y because of how map is done for ODE fliped axis
+                // so ix,iy,dx and dy are inter exchanged
+
+                regsize = regsizeY;
+
+                if (x < regsizeX - 1)
+                {
+                    iy = (int)x;
+                    dy = x - (float)iy;
+                }
+                else // out world use external height
+                {
+                    iy = regsizeX - 2;
+                    dy = 0;
+                }
+                if (y < regsizeY - 1)
+                {
+                    ix = (int)y;
+                    dx = y - (float)ix;
+                }
+                else
+                {
+                    ix = regsizeY - 2;
+                    dx = 0;
+                }
+            }
+
+            float h0;
+            float h1;
+            float h2;
+
+            iy *= regsize;
+            iy += ix; // all indexes have iy + ix
+
+            float[] heights = TerrainHeightFieldHeight;
+            /*
+                        if ((dx + dy) <= 1.0f)
+                        {
+                            h0 = ((float)heights[iy]); // 0,0 vertice
+                            h1 = (((float)heights[iy + 1]) - h0) * dx; // 1,0 vertice minus 0,0
+                            h2 = (((float)heights[iy + regsize]) - h0) * dy; // 0,1 vertice minus 0,0
+                        }
+                        else
+                        {
+                            h0 = ((float)heights[iy + regsize + 1]); // 1,1 vertice
+                            h1 = (((float)heights[iy + 1]) - h0) * (1 - dy); // 1,1 vertice minus 1,0
+                            h2 = (((float)heights[iy + regsize]) - h0) * (1 - dx); // 1,1 vertice minus 0,1
+                        }
+            */
+            h0 = ((float)heights[iy]); // 0,0 vertice
+
+            if (dy>dx)
+            {
+                iy += regsize;
+                h2 = (float)heights[iy]; // 0,1 vertice
+                h1 = (h2 - h0) * dy; // 0,1 vertice minus 0,0
+                h2 = ((float)heights[iy + 1] - h2) * dx; // 1,1 vertice minus 0,1
+            }
+            else
+            {
+                iy++;
+                h2 = (float)heights[iy]; // vertice 1,0
+                h1 = (h2 - h0) * dx; // 1,0 vertice minus 0,0
+                h2 = (((float)heights[iy + regsize]) - h2) * dy; // 1,1 vertice minus 1,0
+            }
+
+            return h0 + h1 + h2;
+        }
+
+        public Vector3 GetTerrainNormalAtXY(float x, float y)
+        {
+            Vector3 norm = new Vector3(0, 0, 1);
+
+            if (TerrainGeom == IntPtr.Zero)
+                return norm;
+
+            if (TerrainHeightFieldHeight == null || TerrainHeightFieldHeight.Length == 0)
+                return norm;
+
+            // TerrainHeightField for ODE as offset 1m
+            x += 1f;
+            y += 1f;
+
+            // make position fit into array
+            if (x < 0)
+                x = 0;
+            if (y < 0)
+                y = 0;
+
+            // integer indexs
+            int ix;
+            int iy;
+            //  interpolators offset
+            float dx;
+            float dy;
+
+            int regsizeX = (int)m_regionWidth + 3; // map size see setterrain number of samples
+            int regsizeY = (int)m_regionHeight + 3; // map size see setterrain number of samples
+            int regsize = regsizeX;
+
+            int xstep = 1;
+            int ystep = regsizeX;
+            bool firstTri = false;
+
+            if (m_OSOdeLib)
+            {
+                if (x < regsizeX - 1)
+                {
+                    ix = (int)x;
+                    dx = x - (float)ix;
+                }
+                else // out world use external height
+                {
+                    ix = regsizeX - 2;
+                    dx = 0;
+                }
+                if (y < regsizeY - 1)
+                {
+                    iy = (int)y;
+                    dy = y - (float)iy;
+                }
+                else
+                {
+                    iy = regsizeY - 2;
+                    dy = 0;
+                }
+                firstTri = dy > dx;
+            }
+
+            else
+            {
+                xstep = regsizeY;
+                ystep = 1;
+                regsize = regsizeY;
+
+                // we  still have square fixed size regions
+                // also flip x and y because of how map is done for ODE fliped axis
+                // so ix,iy,dx and dy are inter exchanged
+                if (x < regsizeX - 1)
+                {
+                    iy = (int)x;
+                    dy = x - (float)iy;
+                }
+                else // out world use external height
+                {
+                    iy = regsizeX - 2;
+                    dy = 0;
+                }
+                if (y < regsizeY - 1)
+                {
+                    ix = (int)y;
+                    dx = y - (float)ix;
+                }
+                else
+                {
+                    ix = regsizeY - 2;
+                    dx = 0;
+                }
+                firstTri = dx > dy;
+            }
+
+            float h0;
+            float h1;
+            float h2;
+
+            iy *= regsize;
+            iy += ix; // all indexes have iy + ix
+
+            float[] heights = TerrainHeightFieldHeight;
+
+            if (firstTri)
+            {
+                h1 = ((float)heights[iy]); // 0,0 vertice
+                iy += ystep;
+                h0 = (float)heights[iy]; // 0,1
+                h2 = (float)heights[iy+xstep]; // 1,1 vertice
+                norm.X = h0 - h2;
+                norm.Y = h1 - h0;
+            }
+            else
+            {
+                h2 = ((float)heights[iy]); // 0,0 vertice
+                iy += xstep;
+                h0 = ((float)heights[iy]); // 1,0 vertice
+                h1 = (float)heights[iy+ystep]; // vertice 1,1
+                norm.X = h2 - h0;
+                norm.Y = h0 - h1;
+            }
+            norm.Z = 1;
+            norm.Normalize();
+            return norm;
+        }
+
+        public override void SetTerrain(float[] heightMap)
+        {
+            if (m_OSOdeLib)
+                OSSetTerrain(heightMap);
+            else
+                OriSetTerrain(heightMap);
+        }
+
+        public void OriSetTerrain(float[] heightMap)
+        {
+            // assumes 1m size grid and constante size square regions
+            // needs to know about sims around in future
+
+            float[] _heightmap;
+
+            uint regionsizeX = m_regionWidth;
+            uint regionsizeY = m_regionHeight;
+
+            // map is rotated
+            uint heightmapWidth = regionsizeY + 2;
+            uint heightmapHeight = regionsizeX + 2;
+
+            uint heightmapWidthSamples = heightmapWidth + 1;
+            uint heightmapHeightSamples = heightmapHeight + 1;
+
+            _heightmap = new float[heightmapWidthSamples * heightmapHeightSamples];
+
+            const float scale = 1.0f;
+            const float offset = 0.0f;
+            const float thickness = 10f;
+            const int wrap = 0;
+
+
+            float hfmin = float.MaxValue;
+            float hfmax = float.MinValue;
+            float val;
+            uint xx;
+            uint yy;
+
+            uint maxXX = regionsizeX - 1;
+            uint maxYY = regionsizeY - 1;
+            // flipping map adding one margin all around so things don't fall in edges
+
+            uint xt = 0;
+            xx = 0;
+
+            for (uint x = 0; x < heightmapWidthSamples; x++)
+            {
+                if (x > 1 && xx < maxXX)
+                    xx++;
+                yy = 0;
+                for (uint y = 0; y < heightmapHeightSamples; y++)
+                {
+                    if (y > 1 && y < maxYY)
+                        yy += regionsizeX;
+
+                    val = heightMap[yy + xx];
+                    if (val < 0.0f)
+                        val = 0.0f; // no neg terrain as in chode
+                    _heightmap[xt + y] = val;
+
+                    if (hfmin > val)
+                        hfmin = val;
+                    if (hfmax < val)
+                        hfmax = val;
+                }
+                xt += heightmapHeightSamples;
+            }
+
+            lock (OdeLock)
+            {
+                d.AllocateODEDataForThread(~0U);
+
+                if (TerrainGeom != IntPtr.Zero)
+                {
+                    actor_name_map.Remove(TerrainGeom);
+                    d.GeomDestroy(TerrainGeom);
+
+                }
+
+                if (TerrainHeightFieldHeightsHandler.IsAllocated)
+                    TerrainHeightFieldHeightsHandler.Free();
+
+                IntPtr HeightmapData = d.GeomHeightfieldDataCreate();
+
+                TerrainHeightFieldHeightsHandler = GCHandle.Alloc(_heightmap, GCHandleType.Pinned);
+
+                d.GeomHeightfieldDataBuildSingle(HeightmapData, TerrainHeightFieldHeightsHandler.AddrOfPinnedObject(), 0,
+                                                heightmapHeight, heightmapWidth ,
+                                                 (int)heightmapHeightSamples, (int)heightmapWidthSamples, scale,
+                                                offset, thickness, wrap);
+
+                d.GeomHeightfieldDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1);
+
+                TerrainGeom = d.CreateHeightfield(GroundSpace, HeightmapData, 1);
+
+                if (TerrainGeom != IntPtr.Zero)
+                {
+                    d.GeomSetCategoryBits(TerrainGeom, (uint)(CollisionCategories.Land));
+                    d.GeomSetCollideBits(TerrainGeom, 0);
+
+                    PhysicsActor pa = new NullPhysicsActor();
+                    pa.Name = "Terrain";
+                    pa.PhysicsActorType = (int)ActorTypes.Ground;
+                    actor_name_map[TerrainGeom] = pa;
+
+//                    geom_name_map[GroundGeom] = "Terrain";
+
+                    d.Quaternion q = new d.Quaternion();
+                    q.X = 0.5f;
+                    q.Y = 0.5f;
+                    q.Z = 0.5f;
+                    q.W = 0.5f;
+
+                    d.GeomSetQuaternion(TerrainGeom, ref q);
+                    d.GeomSetPosition(TerrainGeom, m_regionWidth * 0.5f, m_regionHeight * 0.5f, 0.0f);
+                    TerrainHeightFieldHeight = _heightmap;
+                }
+                else
+                    TerrainHeightFieldHeightsHandler.Free();
+            }
+        }
+
+        public void OSSetTerrain(float[] heightMap)
+        {
+            // assumes 1m size grid and constante size square regions
+            // needs to know about sims around in future
+
+            float[] _heightmap;
+
+            uint regionsizeX = m_regionWidth;
+            uint regionsizeY = m_regionHeight;
+
+            uint heightmapWidth = regionsizeX + 2;
+            uint heightmapHeight = regionsizeY + 2;
+
+            uint heightmapWidthSamples = heightmapWidth + 1;
+            uint heightmapHeightSamples = heightmapHeight + 1;
+
+            _heightmap = new float[heightmapWidthSamples * heightmapHeightSamples];
+
+
+            float hfmin = float.MaxValue;
+//            float hfmax = float.MinValue;
+            float val;
+
+
+            uint maxXX = regionsizeX + 1;
+            uint maxYY = regionsizeY + 1;
+            // adding one margin all around so things don't fall in edges
+
+            uint xx;
+            uint yy = 0;
+            uint yt = 0;
+
+            for (uint y = 0; y < heightmapHeightSamples; y++)
+            {
+                if (y > 1 && y < maxYY)
+                    yy += regionsizeX;
+                xx = 0;
+                for (uint x = 0; x < heightmapWidthSamples; x++)
+                {
+                    if (x > 1 && x < maxXX)
+                        xx++;
+
+                    val = heightMap[yy + xx];
+                    if (val < 0.0f)
+                        val = 0.0f; // no neg terrain as in chode
+                    _heightmap[yt + x] = val;
+
+                    if (hfmin > val)
+                        hfmin = val;
+//                    if (hfmax < val)
+//                        hfmax = val;
+                }
+                yt += heightmapWidthSamples;
+            }
+
+            lock (OdeLock)
+            {
+                if (TerrainGeom != IntPtr.Zero)
+                {
+                    actor_name_map.Remove(TerrainGeom);
+                    d.GeomDestroy(TerrainGeom);
+                }
+
+                if (TerrainHeightFieldHeightsHandler.IsAllocated)
+                            TerrainHeightFieldHeightsHandler.Free();
+
+                TerrainHeightFieldHeight = null;
+
+                IntPtr HeightmapData = d.GeomOSTerrainDataCreate();
+
+                const int wrap = 0;
+                float thickness = hfmin;
+                if (thickness < 0)
+                    thickness = 1;
+
+                TerrainHeightFieldHeightsHandler = GCHandle.Alloc(_heightmap, GCHandleType.Pinned);
+
+                d.GeomOSTerrainDataBuild(HeightmapData, TerrainHeightFieldHeightsHandler.AddrOfPinnedObject(), 0, 1.0f,
+                                                 (int)heightmapWidthSamples, (int)heightmapHeightSamples,
+                                                 thickness, wrap);
+
+//                d.GeomOSTerrainDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1);
+                TerrainGeom = d.CreateOSTerrain(GroundSpace, HeightmapData, 1);
+                if (TerrainGeom != IntPtr.Zero)
+                {
+                    d.GeomSetCategoryBits(TerrainGeom, (uint)(CollisionCategories.Land));
+                    d.GeomSetCollideBits(TerrainGeom, 0);
+
+                    PhysicsActor pa = new NullPhysicsActor();
+                    pa.Name = "Terrain";
+                    pa.PhysicsActorType = (int)ActorTypes.Ground;
+                    actor_name_map[TerrainGeom] = pa;
+
+//                    geom_name_map[GroundGeom] = "Terrain";
+
+                    d.GeomSetPosition(TerrainGeom, m_regionWidth * 0.5f, m_regionHeight * 0.5f, 0.0f);
+                    TerrainHeightFieldHeight = _heightmap;
+                 }
+                 else
+                    TerrainHeightFieldHeightsHandler.Free();
+            }
+        }
+
+        public override void DeleteTerrain()
+        {
+        }
+
+        public float GetWaterLevel()
+        {
+            return waterlevel;
+        }
+
+        public override void SetWaterLevel(float baseheight)
+        {
+            waterlevel = baseheight;
+        }
+
+        public override void Dispose()
+        {
+            lock (OdeLock)
+            {
+
+                if (world == IntPtr.Zero)
+                    return;
+
+                d.AllocateODEDataForThread(~0U);
+
+                if (m_meshWorker != null)
+                    m_meshWorker.Stop();
+
+                if (m_rayCastManager != null)
+                {
+                    m_rayCastManager.Dispose();
+                    m_rayCastManager = null;
+                }
+
+                lock (_prims)
+                {
+                    ChangesQueue.Clear();
+                    foreach (OdePrim prm in _prims.Values)
+                    {
+                        prm.DoAChange(changes.Remove, null);
+                        _collisionEventPrim.Remove(prm);
+                    }
+                    _prims.Clear();
+                }
+
+                OdeCharacter[] chtorem;
+                lock (_characters)
+                {
+                    chtorem = new OdeCharacter[_characters.Count];
+                    _characters.CopyTo(chtorem);
+                }
+
+                ChangesQueue.Clear();
+                foreach (OdeCharacter ch in chtorem)
+                    ch.DoAChange(changes.Remove, null);
+
+                if (TerrainGeom != IntPtr.Zero)
+                        d.GeomDestroy(TerrainGeom);
+                TerrainGeom = IntPtr.Zero;
+
+                if (TerrainHeightFieldHeightsHandler.IsAllocated)
+                    TerrainHeightFieldHeightsHandler.Free();
+
+                TerrainHeightFieldHeight = null;
+
+                if (ContactgeomsArray != IntPtr.Zero)
+                {
+                    Marshal.FreeHGlobal(ContactgeomsArray);
+                    ContactgeomsArray = IntPtr.Zero;
+                }
+                if (GlobalContactsArray != IntPtr.Zero)
+                {
+                    Marshal.FreeHGlobal(GlobalContactsArray);
+                    GlobalContactsArray = IntPtr.Zero;
+                }
+
+                d.WorldDestroy(world);
+                world = IntPtr.Zero;
+                //d.CloseODE();
+            }
+        }
+
+        private int compareByCollisionsDesc(OdePrim A, OdePrim B)
+        {
+            return -A.CollisionScore.CompareTo(B.CollisionScore);
+        }
+
+        public override Dictionary<uint, float> GetTopColliders()
+        {
+            Dictionary<uint, float> topColliders;
+            List<OdePrim> orderedPrims;
+            lock (_activeprims)
+                orderedPrims = new List<OdePrim>(_activeprims);
+
+            orderedPrims.Sort(compareByCollisionsDesc);
+            topColliders = orderedPrims.Take(25).ToDictionary(p => p.LocalID, p => p.CollisionScore);
+
+            return topColliders;
+        }
+
+        public override bool SupportsRayCast()
+        {
+            return true;
+        }
+
+        public override void RaycastWorld(Vector3 position, Vector3 direction, float length, RaycastCallback retMethod)
+        {
+            if (retMethod != null)
+            {
+                ODERayRequest req = new ODERayRequest();
+                req.actor = null;
+                req.callbackMethod = retMethod;
+                req.length = length;
+                req.Normal = direction;
+                req.Origin = position;
+                req.Count = 0;
+                req.filter = RayFilterFlags.AllPrims;
+
+                m_rayCastManager.QueueRequest(req);
+            }
+        }
+
+        public override void RaycastWorld(Vector3 position, Vector3 direction, float length, int Count, RayCallback retMethod)
+        {
+            if (retMethod != null)
+            {
+                ODERayRequest req = new ODERayRequest();
+                req.actor = null;
+                req.callbackMethod = retMethod;
+                req.length = length;
+                req.Normal = direction;
+                req.Origin = position;
+                req.Count = Count;
+                req.filter = RayFilterFlags.AllPrims;
+
+                m_rayCastManager.QueueRequest(req);
+            }
+        }
+
+
+        public override List<ContactResult> RaycastWorld(Vector3 position, Vector3 direction, float length, int Count)
+        {
+            List<ContactResult> ourresults = new List<ContactResult>();
+            object SyncObject = new object();
+
+            RayCallback retMethod = delegate(List<ContactResult> results)
+            {
+                lock (SyncObject)
+                {
+                    ourresults = results;
+                    Monitor.PulseAll(SyncObject);
+                }
+            };
+
+            ODERayRequest req = new ODERayRequest();
+            req.actor = null;
+            req.callbackMethod = retMethod;
+            req.length = length;
+            req.Normal = direction;
+            req.Origin = position;
+            req.Count = Count;
+            req.filter = RayFilterFlags.AllPrims;
+
+            lock (SyncObject)
+            {
+                m_rayCastManager.QueueRequest(req);
+                if (!Monitor.Wait(SyncObject, 500))
+                    return null;
+                else
+                    return ourresults;
+            }
+        }
+
+        public override bool SupportsRaycastWorldFiltered()
+        {
+            return true;
+        }
+
+        public override object RaycastWorld(Vector3 position, Vector3 direction, float length, int Count, RayFilterFlags filter)
+        {
+            object SyncObject = new object();
+            List<ContactResult> ourresults = new List<ContactResult>();
+
+            RayCallback retMethod = delegate(List<ContactResult> results)
+            {
+                lock (SyncObject)
+                {
+                    ourresults = results;
+                    Monitor.PulseAll(SyncObject);
+                }
+            };
+
+            ODERayRequest req = new ODERayRequest();
+            req.actor = null;
+            req.callbackMethod = retMethod;
+            req.length = length;
+            req.Normal = direction;
+            req.Origin = position;
+            req.Count = Count;
+            req.filter = filter;
+
+            lock (SyncObject)
+            {
+                m_rayCastManager.QueueRequest(req);
+                if (!Monitor.Wait(SyncObject, 500))
+                    return null;
+                else
+                    return ourresults;
+            }
+        }
+
+        public override List<ContactResult> RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count, RayFilterFlags flags)
+        {
+            if (actor == null)
+                return new List<ContactResult>();
+
+            IntPtr geom;
+            if (actor is OdePrim)
+                geom = ((OdePrim)actor).prim_geom;
+            else if (actor is OdeCharacter)
+                geom = ((OdePrim)actor).prim_geom;
+            else
+                return new List<ContactResult>();
+
+            if (geom == IntPtr.Zero)
+                return new List<ContactResult>();
+
+            List<ContactResult> ourResults = null;
+            object SyncObject = new object();
+
+            RayCallback retMethod = delegate(List<ContactResult> results)
+            {
+                lock (SyncObject)
+                {
+                    ourResults = results;
+                    Monitor.PulseAll(SyncObject);
+                }
+            };
+
+            ODERayRequest req = new ODERayRequest();
+            req.actor = actor;
+            req.callbackMethod = retMethod;
+            req.length = length;
+            req.Normal = direction;
+            req.Origin = position;
+            req.Count = Count;
+            req.filter = flags;
+
+            lock (SyncObject)
+            {
+                m_rayCastManager.QueueRequest(req);
+                if (!Monitor.Wait(SyncObject, 500))
+                    return new List<ContactResult>();
+            }
+
+            if (ourResults == null)
+                return new List<ContactResult>();
+            return ourResults;
+        }
+
+        public override List<ContactResult> BoxProbe(Vector3 position, Vector3 size, Quaternion orientation, int Count, RayFilterFlags flags)
+        {
+            List<ContactResult> ourResults = null;
+            object SyncObject = new object();
+
+            ProbeBoxCallback retMethod = delegate(List<ContactResult> results)
+            {
+                lock (SyncObject)
+                {
+                    ourResults = results;
+                    Monitor.PulseAll(SyncObject);
+                }
+            };
+
+            ODERayRequest req = new ODERayRequest();
+            req.actor = null;
+            req.callbackMethod = retMethod;
+            req.Normal = size;
+            req.Origin = position;
+            req.orientation = orientation;
+            req.Count = Count;
+            req.filter = flags;
+
+            lock (SyncObject)
+            {
+                m_rayCastManager.QueueRequest(req);
+                if (!Monitor.Wait(SyncObject, 500))
+                    return new List<ContactResult>();
+            }
+
+            if (ourResults == null)
+                return new List<ContactResult>();
+            return ourResults;
+        }
+
+        public override List<ContactResult> SphereProbe(Vector3 position, float radius, int Count, RayFilterFlags flags)
+        {
+            List<ContactResult> ourResults = null;
+            object SyncObject = new object();
+
+            ProbeSphereCallback retMethod = delegate(List<ContactResult> results)
+            {
+                ourResults = results;
+                Monitor.PulseAll(SyncObject);
+            };
+
+            ODERayRequest req = new ODERayRequest();
+            req.actor = null;
+            req.callbackMethod = retMethod;
+            req.length = radius;
+            req.Origin = position;
+            req.Count = Count;
+            req.filter = flags;
+
+
+            lock (SyncObject)
+            {
+                m_rayCastManager.QueueRequest(req);
+                if (!Monitor.Wait(SyncObject, 500))
+                    return new List<ContactResult>();
+            }
+
+            if (ourResults == null)
+                return new List<ContactResult>();
+            return ourResults;
+        }
+
+        public override List<ContactResult> PlaneProbe(PhysicsActor actor, Vector4 plane, int Count, RayFilterFlags flags)
+        {
+            IntPtr geom = IntPtr.Zero;;
+
+            if (actor != null)
+            {
+                if (actor is OdePrim)
+                    geom = ((OdePrim)actor).prim_geom;
+                else if (actor is OdeCharacter)
+                    geom = ((OdePrim)actor).prim_geom;
+            }
+
+            List<ContactResult> ourResults = null;
+            object SyncObject = new object();
+
+            ProbePlaneCallback retMethod = delegate(List<ContactResult> results)
+            {
+                ourResults = results;
+                Monitor.PulseAll(SyncObject);
+            };
+
+            ODERayRequest req = new ODERayRequest();
+            req.actor = null;
+            req.callbackMethod = retMethod;
+            req.length = plane.W;
+            req.Normal.X = plane.X;
+            req.Normal.Y = plane.Y;
+            req.Normal.Z = plane.Z;
+            req.Count = Count;
+            req.filter = flags;
+
+            lock (SyncObject)
+            {
+                m_rayCastManager.QueueRequest(req);
+                if (!Monitor.Wait(SyncObject, 500))
+                    return new List<ContactResult>();
+            }
+
+            if (ourResults == null)
+                return new List<ContactResult>();
+            return ourResults;
+        }
+
+        public override int SitAvatar(PhysicsActor actor, Vector3 AbsolutePosition, Vector3 CameraPosition, Vector3 offset, Vector3 AvatarSize, SitAvatarCallback PhysicsSitResponse)
+        {
+            Util.FireAndForget( delegate
+            {
+                ODESitAvatar sitAvatar = new ODESitAvatar(this, m_rayCastManager);
+                if(sitAvatar != null)
+                    sitAvatar.Sit(actor, AbsolutePosition, CameraPosition, offset, AvatarSize, PhysicsSitResponse);
+            });
+            return 1;
+        }
+
+    }
+}
diff --git a/OpenSim/Region/PhysicsModules/ubOde/ODESitAvatar.cs b/OpenSim/Region/PhysicsModules/ubOde/ODESitAvatar.cs
new file mode 100644
index 0000000..214205d
--- /dev/null
+++ b/OpenSim/Region/PhysicsModules/ubOde/ODESitAvatar.cs
@@ -0,0 +1,356 @@
+/*
+ * Copyright (c) Contributors, http://opensimulator.org/
+ * See CONTRIBUTORS.TXT for a full list of copyright holders.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the OpenSimulator Project nor the
+ *       names of its contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+// Ubit Umarov 2012
+using System;
+using System.Collections.Generic;
+using System.Reflection;
+using System.Runtime.InteropServices;
+using System.Text;
+using OpenSim.Framework;
+using OpenSim.Region.PhysicsModules.SharedBase;
+using OdeAPI;
+using log4net;
+using OpenMetaverse;
+
+namespace OpenSim.Region.PhysicsModule.ubOde
+{
+    /// <summary>
+    /// </summary>
+    public class ODESitAvatar
+    {
+        private ODEScene m_scene;
+        private ODERayCastRequestManager m_raymanager;
+
+        public ODESitAvatar(ODEScene pScene, ODERayCastRequestManager raymanager)
+        {
+            m_scene = pScene;
+            m_raymanager = raymanager;
+        }
+
+        private static Vector3 SitAjust = new Vector3(0, 0, 0.4f);
+        private const RayFilterFlags RaySitFlags = RayFilterFlags.AllPrims | RayFilterFlags.ClosestHit;
+
+        private void RotAroundZ(float x, float y, ref Quaternion ori)
+        {
+            double ang = Math.Atan2(y, x);
+            ang *= 0.5d;
+            float s = (float)Math.Sin(ang);
+            float c = (float)Math.Cos(ang);
+
+            ori.X = 0;
+            ori.Y = 0;
+            ori.Z = s;
+            ori.W = c;
+        }
+
+
+        public void Sit(PhysicsActor actor, Vector3 avPos, Vector3 avCameraPosition, Vector3 offset, Vector3 avOffset, SitAvatarCallback PhysicsSitResponse)
+        {
+            if (!m_scene.haveActor(actor) || !(actor is OdePrim) || ((OdePrim)actor).prim_geom == IntPtr.Zero)
+            {
+                PhysicsSitResponse(-1, actor.LocalID, offset, Quaternion.Identity);
+                return;
+            }
+
+            IntPtr geom = ((OdePrim)actor).prim_geom;
+
+            Vector3 geopos = d.GeomGetPositionOMV(geom);
+            Quaternion geomOri = d.GeomGetQuaternionOMV(geom);
+
+//            Vector3 geopos = actor.Position;
+//            Quaternion geomOri = actor.Orientation;
+
+            Quaternion geomInvOri = Quaternion.Conjugate(geomOri);
+
+            Quaternion ori = Quaternion.Identity;
+
+            Vector3 rayDir = geopos + offset - avCameraPosition;
+
+            float raylen = rayDir.Length();
+            if (raylen < 0.001f)
+            {
+                PhysicsSitResponse(-1, actor.LocalID, offset, Quaternion.Identity);
+                return;
+            }
+            float t = 1 / raylen;
+            rayDir.X *= t;
+            rayDir.Y *= t;
+            rayDir.Z *= t;
+
+            raylen += 30f; // focal point may be far
+            List<ContactResult> rayResults;
+
+            rayResults = m_scene.RaycastActor(actor, avCameraPosition, rayDir, raylen, 1, RaySitFlags);
+            if (rayResults.Count == 0)
+            {
+/* if this fundamental ray failed, then just fail so user can try another spot and not be sitted far on a big prim
+                d.AABB aabb;
+                d.GeomGetAABB(geom, out aabb);
+                offset = new Vector3(avOffset.X, 0, aabb.MaxZ + avOffset.Z - geopos.Z);
+                ori = geomInvOri;
+                offset *= geomInvOri;
+                PhysicsSitResponse(1, actor.LocalID, offset, ori);
+*/
+                PhysicsSitResponse(0, actor.LocalID, offset, ori);
+                return;
+            }
+
+            int status = 1;
+
+            offset = rayResults[0].Pos - geopos;
+
+            d.GeomClassID geoclass = d.GeomGetClass(geom);
+
+            if (geoclass == d.GeomClassID.SphereClass)
+            {
+                float r = d.GeomSphereGetRadius(geom);
+
+                offset.Normalize();
+                offset *= r;
+
+                RotAroundZ(offset.X, offset.Y, ref ori);
+
+                if (r < 0.4f)
+                {
+                    offset = new Vector3(0, 0, r);
+                }
+                else
+                {
+                    if (offset.Z < 0.4f)
+                    {
+                        t = offset.Z;
+                        float rsq = r * r;
+
+                        t = 1.0f / (rsq - t * t);
+                        offset.X *= t;
+                        offset.Y *= t;
+                        offset.Z = 0.4f;
+                        t = rsq - 0.16f;
+                        offset.X *= t;
+                        offset.Y *= t;
+                    }
+                    else if (r > 0.8f && offset.Z > 0.8f * r)
+                    {
+                        status = 3;
+                        avOffset.X = -avOffset.X;
+                        avOffset.Z *= 1.6f;
+                    }
+                }
+
+                offset += avOffset * ori;
+
+                ori = geomInvOri * ori;
+                offset *= geomInvOri;
+
+                PhysicsSitResponse(status, actor.LocalID, offset, ori);
+                return;
+            }
+
+            Vector3 norm = rayResults[0].Normal;
+
+            if (norm.Z < -0.4f)
+            {
+                PhysicsSitResponse(0, actor.LocalID, offset, Quaternion.Identity);
+                return;
+            }
+
+
+            float SitNormX = -rayDir.X;
+            float SitNormY = -rayDir.Y;
+
+            Vector3 pivot = geopos + offset;
+
+            float edgeNormalX = norm.X;
+            float edgeNormalY = norm.Y;
+            float edgeDirX = -rayDir.X;
+            float edgeDirY = -rayDir.Y;
+            Vector3 edgePos = rayResults[0].Pos;
+            float edgeDist = float.MaxValue;
+
+            bool foundEdge = false;
+
+            if (norm.Z < 0.5f)
+            {
+                float rayDist = 4.0f;
+
+                for (int i = 0; i < 6; i++)
+                {
+                    pivot.X -= 0.01f * norm.X;
+                    pivot.Y -= 0.01f * norm.Y;
+                    pivot.Z -= 0.01f * norm.Z;
+
+                    rayDir.X = -norm.X * norm.Z;
+                    rayDir.Y = -norm.Y * norm.Z;
+                    rayDir.Z = 1.0f - norm.Z * norm.Z;
+                    rayDir.Normalize();
+
+                    rayResults = m_scene.RaycastActor(actor, pivot, rayDir, rayDist, 1, RayFilterFlags.AllPrims);
+                    if (rayResults.Count == 0)
+                        break;
+
+                    if (Math.Abs(rayResults[0].Normal.Z) < 0.7f)
+                    {
+                        rayDist -= rayResults[0].Depth;
+                        if (rayDist < 0f)
+                            break;
+
+                        pivot = rayResults[0].Pos;
+                        norm = rayResults[0].Normal;
+                        edgeNormalX = norm.X;
+                        edgeNormalY = norm.Y;
+                        edgeDirX = -rayDir.X;
+                        edgeDirY = -rayDir.Y;
+                    }
+                    else
+                    {
+                        foundEdge = true;
+                        edgePos = rayResults[0].Pos;
+                        break;
+                    }
+                }
+
+                if (!foundEdge)
+                {
+                    PhysicsSitResponse(0, actor.LocalID, offset, ori);
+                    return;
+                }
+                avOffset.X *= 0.5f;
+            }
+
+            else if (norm.Z > 0.866f)
+            {
+                float toCamBaseX = avCameraPosition.X - pivot.X;
+                float toCamBaseY = avCameraPosition.Y - pivot.Y;
+                float toCamX = toCamBaseX;
+                float toCamY = toCamBaseY;
+
+                for (int j = 0; j < 4; j++)
+                {
+                    float rayDist = 1.0f;
+                    float curEdgeDist = 0.0f;
+
+                    for (int i = 0; i < 3; i++)
+                    {
+                        pivot.Z -= 0.01f;
+                        rayDir.X = toCamX;
+                        rayDir.Y = toCamY;
+                        rayDir.Z = (-toCamX * norm.X - toCamY * norm.Y) / norm.Z;
+                        rayDir.Normalize();
+
+                        rayResults = m_scene.RaycastActor(actor, pivot, rayDir, rayDist, 1, RayFilterFlags.AllPrims);
+                        if (rayResults.Count == 0)
+                            break;
+
+                        curEdgeDist += rayResults[0].Depth;
+
+                        if (rayResults[0].Normal.Z > 0.5f)
+                        {
+                            rayDist -= rayResults[0].Depth;
+                            if (rayDist < 0f)
+                                break;
+
+                            pivot = rayResults[0].Pos;
+                            norm = rayResults[0].Normal;
+                        }
+                        else
+                        {
+                            foundEdge = true;
+                            if (curEdgeDist < edgeDist)
+                            {
+                                edgeDist = curEdgeDist;
+                                edgeNormalX = rayResults[0].Normal.X;
+                                edgeNormalY = rayResults[0].Normal.Y;
+                                edgeDirX = rayDir.X;
+                                edgeDirY = rayDir.Y;
+                                edgePos = rayResults[0].Pos;
+                            }
+                            break;
+                        }
+                    }
+                    if (foundEdge && edgeDist < 0.2f)
+                        break;
+
+                    pivot = geopos + offset;
+
+                    switch (j)
+                    {
+                        case 0:
+                            toCamX = -toCamBaseY;
+                            toCamY = toCamBaseX;
+                            break;
+                        case 1:
+                            toCamX = toCamBaseY;
+                            toCamY = -toCamBaseX;
+                            break;
+                        case 2:
+                            toCamX = -toCamBaseX;
+                            toCamY = -toCamBaseY;
+                            break;
+                        default:
+                            break;
+                    }
+                }
+
+                if (!foundEdge)
+                {
+                    avOffset.X = -avOffset.X;
+                    avOffset.Z *= 1.6f;
+
+                    RotAroundZ(SitNormX, SitNormY, ref ori);
+
+                    offset += avOffset * ori;
+
+                    ori = geomInvOri * ori;
+                    offset *= geomInvOri;
+
+                    PhysicsSitResponse(3, actor.LocalID, offset, ori);
+                    return;
+                }
+                avOffset.X *= 0.5f;
+            }
+
+            SitNormX = edgeNormalX;
+            SitNormY = edgeNormalY;
+            if (edgeDirX * SitNormX + edgeDirY * SitNormY < 0)
+            {
+                SitNormX = -SitNormX;
+                SitNormY = -SitNormY;
+            }
+
+            RotAroundZ(SitNormX, SitNormY, ref ori);
+
+            offset = edgePos + avOffset * ori;
+            offset -= geopos;
+
+            ori = geomInvOri * ori;
+            offset *= geomInvOri;
+
+            PhysicsSitResponse(1, actor.LocalID, offset, ori);
+            return;
+        }
+    }
+}
diff --git a/OpenSim/Region/PhysicsModules/ubOde/Properties/AssemblyInfo.cs b/OpenSim/Region/PhysicsModules/ubOde/Properties/AssemblyInfo.cs
new file mode 100644
index 0000000..5baab9f
--- /dev/null
+++ b/OpenSim/Region/PhysicsModules/ubOde/Properties/AssemblyInfo.cs
@@ -0,0 +1,61 @@
+/*
+ * Copyright (c) Contributors, http://opensimulator.org/
+ * See CONTRIBUTORS.TXT for a full list of copyright holders.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the OpenSimulator Project nor the
+ *       names of its contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+using System.Reflection;
+using System.Runtime.InteropServices;
+using Mono.Addins;
+
+// Information about this assembly is defined by the following
+// attributes.
+//
+// change them to the information which is associated with the assembly
+// you compile.
+
+[assembly : AssemblyTitle("OpenSim.Region.PhysicsModule.ubOde")]
+[assembly : AssemblyDescription("Ubit Variation of ODE")]
+[assembly : AssemblyConfiguration("")]
+[assembly : AssemblyCompany("http://opensimulator.org")]
+[assembly : AssemblyProduct("ubOde")]
+[assembly : AssemblyCopyright("Copyright (c) OpenSimulator.org Developers 2007-2009")]
+[assembly : AssemblyTrademark("")]
+[assembly : AssemblyCulture("")]
+
+// This sets the default COM visibility of types in the assembly to invisible.
+// If you need to expose a type to COM, use [ComVisible(true)] on that type.
+
+[assembly : ComVisible(false)]
+
+// The assembly version has following format :
+//
+// Major.Minor.Build.Revision
+//
+// You can specify all values by your own or you can build default build and revision
+// numbers with the '*' character (the default):
+
+[assembly : AssemblyVersion(OpenSim.VersionInfo.AssemblyVersionNumber)]
+[assembly: Addin("OpenSim.Region.PhysicsModule.ubOde", OpenSim.VersionInfo.VersionNumber)]
+[assembly: AddinDependency("OpenSim.Region.Framework", OpenSim.VersionInfo.VersionNumber)]
\ No newline at end of file
diff --git a/OpenSim/Region/PhysicsModules/ubOdeMeshing/HelperTypes.cs b/OpenSim/Region/PhysicsModules/ubOdeMeshing/HelperTypes.cs
new file mode 100644
index 0000000..ea37301
--- /dev/null
+++ b/OpenSim/Region/PhysicsModules/ubOdeMeshing/HelperTypes.cs
@@ -0,0 +1,340 @@
+/*
+ * Copyright (c) Contributors, http://opensimulator.org/
+ * See CONTRIBUTORS.TXT for a full list of copyright holders.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the OpenSimulator Project nor the
+ *       names of its contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+using System;
+using System.Collections.Generic;
+using System.Diagnostics;
+using System.Globalization;
+using OpenMetaverse;
+using OpenSim.Region.PhysicsModules.SharedBase;
+using OpenSim.Region.PhysicsModule.ubODEMeshing;
+
+public class Vertex : IComparable<Vertex>
+{
+    Vector3 vector;
+
+    public float X
+    {
+        get { return vector.X; }
+        set { vector.X = value; }
+    }
+
+    public float Y
+    {
+        get { return vector.Y; }
+        set { vector.Y = value; }
+    }
+
+    public float Z
+    {
+        get { return vector.Z; }
+        set { vector.Z = value; }
+    }
+
+    public Vertex(float x, float y, float z)
+    {
+        vector.X = x;
+        vector.Y = y;
+        vector.Z = z;
+    }
+
+    public Vertex normalize()
+    {
+        float tlength = vector.Length();
+        if (tlength != 0f)
+        {
+            float mul = 1.0f / tlength;
+            return new Vertex(vector.X * mul, vector.Y * mul, vector.Z * mul);
+        }
+        else
+        {
+            return new Vertex(0f, 0f, 0f);
+        }
+    }
+
+    public Vertex cross(Vertex v)
+    {
+        return new Vertex(vector.Y * v.Z - vector.Z * v.Y, vector.Z * v.X - vector.X * v.Z, vector.X * v.Y - vector.Y * v.X);
+    }
+
+    // disable warning: mono compiler moans about overloading
+    // operators hiding base operator but should not according to C#
+    // language spec
+#pragma warning disable 0108
+    public static Vertex operator *(Vertex v, Quaternion q)
+    {
+        // From http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/
+
+        Vertex v2 = new Vertex(0f, 0f, 0f);
+
+        v2.X =   q.W * q.W * v.X +
+            2f * q.Y * q.W * v.Z -
+            2f * q.Z * q.W * v.Y +
+                 q.X * q.X * v.X +
+            2f * q.Y * q.X * v.Y +
+            2f * q.Z * q.X * v.Z -
+                 q.Z * q.Z * v.X -
+                 q.Y * q.Y * v.X;
+
+        v2.Y =
+            2f * q.X * q.Y * v.X +
+                 q.Y * q.Y * v.Y +
+            2f * q.Z * q.Y * v.Z +
+            2f * q.W * q.Z * v.X -
+                 q.Z * q.Z * v.Y +
+                 q.W * q.W * v.Y -
+            2f * q.X * q.W * v.Z -
+                 q.X * q.X * v.Y;
+
+        v2.Z =
+            2f * q.X * q.Z * v.X +
+            2f * q.Y * q.Z * v.Y +
+                 q.Z * q.Z * v.Z -
+            2f * q.W * q.Y * v.X -
+                 q.Y * q.Y * v.Z +
+            2f * q.W * q.X * v.Y -
+                 q.X * q.X * v.Z +
+                 q.W * q.W * v.Z;
+
+        return v2;
+    }
+
+    public static Vertex operator +(Vertex v1, Vertex v2)
+    {
+        return new Vertex(v1.X + v2.X, v1.Y + v2.Y, v1.Z + v2.Z);
+    }
+
+    public static Vertex operator -(Vertex v1, Vertex v2)
+    {
+        return new Vertex(v1.X - v2.X, v1.Y - v2.Y, v1.Z - v2.Z);
+    }
+
+    public static Vertex operator *(Vertex v1, Vertex v2)
+    {
+        return new Vertex(v1.X * v2.X, v1.Y * v2.Y, v1.Z * v2.Z);
+    }
+
+    public static Vertex operator +(Vertex v1, float am)
+    {
+        v1.X += am;
+        v1.Y += am;
+        v1.Z += am;
+        return v1;
+    }
+
+    public static Vertex operator -(Vertex v1, float am)
+    {
+        v1.X -= am;
+        v1.Y -= am;
+        v1.Z -= am;
+        return v1;
+    }
+
+    public static Vertex operator *(Vertex v1, float am)
+    {
+        v1.X *= am;
+        v1.Y *= am;
+        v1.Z *= am;
+        return v1;
+    }
+
+    public static Vertex operator /(Vertex v1, float am)
+    {
+        if (am == 0f)
+        {
+            return new Vertex(0f,0f,0f);
+        }
+        float mul = 1.0f / am;
+        v1.X *= mul;
+        v1.Y *= mul;
+        v1.Z *= mul;
+        return v1;
+    }
+#pragma warning restore 0108
+
+
+    public float dot(Vertex v)
+    {
+        return X * v.X + Y * v.Y + Z * v.Z;
+    }
+
+    public Vertex(Vector3 v)
+    {
+        vector = v;
+    }
+
+    public Vertex Clone()
+    {
+        return new Vertex(X, Y, Z);
+    }
+
+    public static Vertex FromAngle(double angle)
+    {
+        return new Vertex((float) Math.Cos(angle), (float) Math.Sin(angle), 0.0f);
+    }
+
+    public float Length()
+    {
+        return vector.Length();
+    }
+
+    public virtual bool Equals(Vertex v, float tolerance)
+    {
+        Vertex diff = this - v;
+        float d = diff.Length();
+        if (d < tolerance)
+            return true;
+
+        return false;
+    }
+
+
+    public int CompareTo(Vertex other)
+    {
+        if (X < other.X)
+            return -1;
+
+        if (X > other.X)
+            return 1;
+
+        if (Y < other.Y)
+            return -1;
+
+        if (Y > other.Y)
+            return 1;
+
+        if (Z < other.Z)
+            return -1;
+
+        if (Z > other.Z)
+            return 1;
+
+        return 0;
+    }
+
+    public static bool operator >(Vertex me, Vertex other)
+    {
+        return me.CompareTo(other) > 0;
+    }
+
+    public static bool operator <(Vertex me, Vertex other)
+    {
+        return me.CompareTo(other) < 0;
+    }
+
+    public String ToRaw()
+    {
+        // Why this stuff with the number formatter?
+        // Well, the raw format uses the english/US notation of numbers
+        // where the "," separates groups of 1000 while the "." marks the border between 1 and 10E-1.
+        // The german notation uses these characters exactly vice versa!
+        // The Float.ToString() routine is a localized one, giving different results depending on the country
+        // settings your machine works with. Unusable for a machine readable file format :-(
+        NumberFormatInfo nfi = new NumberFormatInfo();
+        nfi.NumberDecimalSeparator = ".";
+        nfi.NumberDecimalDigits = 6;
+
+        String s1 = X.ToString(nfi) + " " + Y.ToString(nfi) + " " + Z.ToString(nfi);
+
+        return s1;
+    }
+}
+
+public class Triangle
+{
+    public Vertex v1;
+    public Vertex v2;
+    public Vertex v3;
+
+    public Triangle(Vertex _v1, Vertex _v2, Vertex _v3)
+    {
+        v1 = _v1;
+        v2 = _v2;
+        v3 = _v3;
+    }
+
+    public Triangle(float _v1x,float _v1y,float _v1z,
+                    float _v2x,float _v2y,float _v2z,
+                    float _v3x,float _v3y,float _v3z)
+    {
+        v1 = new Vertex(_v1x, _v1y, _v1z);
+        v2 = new Vertex(_v2x, _v2y, _v2z);
+        v3 = new Vertex(_v3x, _v3y, _v3z);
+    }
+
+    public override String ToString()
+    {
+        NumberFormatInfo nfi = new NumberFormatInfo();
+        nfi.CurrencyDecimalDigits = 2;
+        nfi.CurrencyDecimalSeparator = ".";
+
+        String s1 = "<" + v1.X.ToString(nfi) + "," + v1.Y.ToString(nfi) + "," + v1.Z.ToString(nfi) + ">";
+        String s2 = "<" + v2.X.ToString(nfi) + "," + v2.Y.ToString(nfi) + "," + v2.Z.ToString(nfi) + ">";
+        String s3 = "<" + v3.X.ToString(nfi) + "," + v3.Y.ToString(nfi) + "," + v3.Z.ToString(nfi) + ">";
+
+        return s1 + ";" + s2 + ";" + s3;
+    }
+
+    public Vector3 getNormal()
+    {
+        // Vertices
+
+        // Vectors for edges
+        Vector3 e1;
+        Vector3 e2;
+
+        e1 = new Vector3(v1.X - v2.X, v1.Y - v2.Y, v1.Z - v2.Z);
+        e2 = new Vector3(v1.X - v3.X, v1.Y - v3.Y, v1.Z - v3.Z);
+
+        // Cross product for normal
+        Vector3 n = Vector3.Cross(e1, e2);
+
+        // Length
+        float l = n.Length();
+
+        // Normalized "normal"
+        n = n/l;
+
+        return n;
+    }
+
+    public void invertNormal()
+    {
+        Vertex vt;
+        vt = v1;
+        v1 = v2;
+        v2 = vt;
+    }
+
+    // Dumps a triangle in the "raw faces" format, blender can import. This is for visualisation and
+    // debugging purposes
+    public String ToStringRaw()
+    {
+        String output = v1.ToRaw() + " " + v2.ToRaw() + " " + v3.ToRaw();
+        return output;
+    }
+}
diff --git a/OpenSim/Region/PhysicsModules/ubOdeMeshing/Mesh.cs b/OpenSim/Region/PhysicsModules/ubOdeMeshing/Mesh.cs
new file mode 100644
index 0000000..5d2b1f7
--- /dev/null
+++ b/OpenSim/Region/PhysicsModules/ubOdeMeshing/Mesh.cs
@@ -0,0 +1,636 @@
+/*
+ * Copyright (c) Contributors, http://opensimulator.org/
+ * See CONTRIBUTORS.TXT for a full list of copyright holders.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the OpenSimulator Project nor the
+ *       names of its contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+using System;
+using System.Collections.Generic;
+using System.IO;
+using System.Runtime.InteropServices;
+using OpenSim.Region.PhysicsModules.SharedBase;
+using PrimMesher;
+using OpenMetaverse;
+using System.Runtime.Serialization;
+using System.Runtime.Serialization.Formatters.Binary;
+
+namespace OpenSim.Region.PhysicsModule.ubODEMeshing
+{
+    public class MeshBuildingData
+    {
+        private class vertexcomp : IEqualityComparer<Vertex>
+        {
+            public bool Equals(Vertex v1, Vertex v2)
+            {
+                if (v1.X == v2.X && v1.Y == v2.Y && v1.Z == v2.Z)
+                    return true;
+                else
+                    return false;
+            }
+            public int GetHashCode(Vertex v)
+            {
+                int a = v.X.GetHashCode();
+                int b = v.Y.GetHashCode();
+                int c = v.Z.GetHashCode();
+                return (a << 16) ^ (b << 8) ^ c;
+            }
+        }
+
+        public Dictionary<Vertex, int> m_vertices;
+        public List<Triangle> m_triangles;
+        public float m_obbXmin;
+        public float m_obbXmax;
+        public float m_obbYmin;
+        public float m_obbYmax;
+        public float m_obbZmin;
+        public float m_obbZmax;
+        public Vector3 m_centroid;
+        public int m_centroidDiv;
+
+        public  MeshBuildingData()
+        {
+            vertexcomp vcomp = new vertexcomp();
+            m_vertices = new Dictionary<Vertex, int>(vcomp);
+            m_triangles = new List<Triangle>();
+            m_centroid = Vector3.Zero;
+            m_centroidDiv = 0;
+            m_obbXmin = float.MaxValue;
+            m_obbXmax = float.MinValue;
+            m_obbYmin = float.MaxValue;
+            m_obbYmax = float.MinValue;
+            m_obbZmin = float.MaxValue;
+            m_obbZmax = float.MinValue;
+        }
+    }
+
+    [Serializable()]
+    public class Mesh : IMesh
+    {
+        float[] vertices;
+        int[] indexes;
+        Vector3 m_obb;
+        Vector3 m_obboffset;
+        [NonSerialized()]
+        MeshBuildingData m_bdata;
+        [NonSerialized()]
+        GCHandle vhandler;
+        [NonSerialized()]
+        GCHandle ihandler;
+        [NonSerialized()]
+        IntPtr m_verticesPtr = IntPtr.Zero;
+        [NonSerialized()]
+        IntPtr m_indicesPtr = IntPtr.Zero;
+        [NonSerialized()]
+        int m_vertexCount = 0;
+        [NonSerialized()]
+        int m_indexCount = 0;
+
+        public int RefCount { get; set; }
+        public AMeshKey Key { get; set; }
+
+        public Mesh(bool forbuild)
+        {
+            if(forbuild)
+                m_bdata = new MeshBuildingData();
+            m_obb = new Vector3(0.5f, 0.5f, 0.5f);
+            m_obboffset = Vector3.Zero;
+        }
+
+        public Mesh Scale(Vector3 scale)
+        {
+            if (m_verticesPtr == null || m_indicesPtr == null)
+                return null;
+
+            Mesh result = new Mesh(false);
+
+            float x = scale.X;
+            float y = scale.Y;
+            float z = scale.Z;
+
+            float tmp;
+            tmp = m_obb.X * x;
+            if(tmp < 0.0005f)
+                tmp = 0.0005f;
+            result.m_obb.X = tmp;
+
+            tmp =  m_obb.Y * y;
+            if(tmp < 0.0005f)
+                tmp = 0.0005f;
+            result.m_obb.Y = tmp;
+
+            tmp =  m_obb.Z * z;
+            if(tmp < 0.0005f)
+                tmp = 0.0005f;
+            result.m_obb.Z = tmp;
+
+            result.m_obboffset.X = m_obboffset.X * x;
+            result.m_obboffset.Y = m_obboffset.Y * y;
+            result.m_obboffset.Z = m_obboffset.Z * z;
+
+            result.vertices = new float[vertices.Length];
+            int j = 0;
+            for (int i = 0; i < m_vertexCount; i++)
+            {
+                result.vertices[j] = vertices[j] * x;
+                j++;
+                result.vertices[j] = vertices[j] * y;
+                j++;
+                result.vertices[j] = vertices[j] * z;
+                j++;
+            }
+
+            result.indexes = new int[indexes.Length];
+            indexes.CopyTo(result.indexes,0);
+
+            result.pinMemory();
+
+            return result;
+        }
+
+        public Mesh Clone()
+        {
+            Mesh result = new Mesh(false);
+
+            if (m_bdata != null)
+            {
+                result.m_bdata = new MeshBuildingData();
+                foreach (Triangle t in m_bdata.m_triangles)
+                {
+                    result.Add(new Triangle(t.v1.Clone(), t.v2.Clone(), t.v3.Clone()));
+                }
+                result.m_bdata.m_centroid = m_bdata.m_centroid;
+                result.m_bdata.m_centroidDiv = m_bdata.m_centroidDiv;
+                result.m_bdata.m_obbXmin = m_bdata.m_obbXmin;
+                result.m_bdata.m_obbXmax = m_bdata.m_obbXmax;
+                result.m_bdata.m_obbYmin = m_bdata.m_obbYmin;
+                result.m_bdata.m_obbYmax = m_bdata.m_obbYmax;
+                result.m_bdata.m_obbZmin = m_bdata.m_obbZmin;
+                result.m_bdata.m_obbZmax = m_bdata.m_obbZmax;
+            }
+            result.m_obb = m_obb;
+            result.m_obboffset = m_obboffset;
+            return result;
+        }
+
+        public void addVertexLStats(Vertex v)
+        {
+            float x = v.X;
+            float y = v.Y;
+            float z = v.Z;
+
+            m_bdata.m_centroid.X += x;
+            m_bdata.m_centroid.Y += y;
+            m_bdata.m_centroid.Z += z;
+            m_bdata.m_centroidDiv++;
+
+            if (x > m_bdata.m_obbXmax)
+                m_bdata.m_obbXmax = x;
+            if (x < m_bdata.m_obbXmin)
+                m_bdata.m_obbXmin = x;
+
+            if (y > m_bdata.m_obbYmax)
+                m_bdata.m_obbYmax = y;
+            if (y < m_bdata.m_obbYmin)
+                m_bdata.m_obbYmin = y;
+
+            if (z > m_bdata.m_obbZmax)
+                m_bdata.m_obbZmax = z;
+            if (z < m_bdata.m_obbZmin)
+                m_bdata.m_obbZmin = z;
+
+        }
+
+        public void Add(Triangle triangle)
+        {
+            if (m_indicesPtr != IntPtr.Zero || m_verticesPtr != IntPtr.Zero)
+                throw new NotSupportedException("Attempt to Add to a pinned Mesh");
+
+
+            triangle.v1.X = (float)Math.Round(triangle.v1.X, 6);
+            triangle.v1.Y = (float)Math.Round(triangle.v1.Y, 6);
+            triangle.v1.Z = (float)Math.Round(triangle.v1.Z, 6);
+            triangle.v2.X = (float)Math.Round(triangle.v2.X, 6);
+            triangle.v2.Y = (float)Math.Round(triangle.v2.Y, 6);
+            triangle.v2.Z = (float)Math.Round(triangle.v2.Z, 6);
+            triangle.v3.X = (float)Math.Round(triangle.v3.X, 6);
+            triangle.v3.Y = (float)Math.Round(triangle.v3.Y, 6);
+            triangle.v3.Z = (float)Math.Round(triangle.v3.Z, 6);
+
+            if ((triangle.v1.X == triangle.v2.X && triangle.v1.Y == triangle.v2.Y && triangle.v1.Z ==
+                    triangle.v2.Z)
+                || (triangle.v1.X == triangle.v3.X && triangle.v1.Y == triangle.v3.Y && triangle.v1.Z ==
+                    triangle.v3.Z)
+                || (triangle.v2.X == triangle.v3.X && triangle.v2.Y == triangle.v3.Y && triangle.v2.Z ==
+                    triangle.v3.Z)
+                )
+            {
+                return;
+            }
+
+            if (m_bdata.m_vertices.Count == 0)
+            {
+                m_bdata.m_centroidDiv = 0;
+                m_bdata.m_centroid = Vector3.Zero;
+            }
+
+            if (!m_bdata.m_vertices.ContainsKey(triangle.v1))
+            {
+                m_bdata.m_vertices[triangle.v1] = m_bdata.m_vertices.Count;
+                addVertexLStats(triangle.v1);
+            }
+            if (!m_bdata.m_vertices.ContainsKey(triangle.v2))
+            {
+                m_bdata.m_vertices[triangle.v2] = m_bdata.m_vertices.Count;
+                addVertexLStats(triangle.v2);
+            }
+            if (!m_bdata.m_vertices.ContainsKey(triangle.v3))
+            {
+                m_bdata.m_vertices[triangle.v3] = m_bdata.m_vertices.Count;
+                addVertexLStats(triangle.v3);
+            }
+            m_bdata.m_triangles.Add(triangle);
+        }
+
+        public Vector3 GetCentroid()
+        {
+            return m_obboffset;
+
+        }
+
+        public Vector3 GetOBB()
+        {
+            return m_obb;
+/*
+            float x, y, z;
+            if (m_bdata.m_centroidDiv > 0)
+            {
+                x = (m_bdata.m_obbXmax - m_bdata.m_obbXmin) * 0.5f;
+                y = (m_bdata.m_obbYmax - m_bdata.m_obbYmin) * 0.5f;
+                z = (m_bdata.m_obbZmax - m_bdata.m_obbZmin) * 0.5f;
+            }
+            else // ??
+            {
+                x = 0.5f;
+                y = 0.5f;
+                z = 0.5f;
+            }
+            return new Vector3(x, y, z);
+*/
+        }
+
+        public int numberVertices()
+        {
+            return m_bdata.m_vertices.Count;
+        }
+
+        public int numberTriangles()
+        {
+            return m_bdata.m_triangles.Count;
+        }
+
+        public List<Vector3> getVertexList()
+        {
+            List<Vector3> result = new List<Vector3>();
+            foreach (Vertex v in m_bdata.m_vertices.Keys)
+            {
+                result.Add(new Vector3(v.X, v.Y, v.Z));
+            }
+            return result;
+        }
+
+        public float[] getVertexListAsFloat()
+        {
+            if (m_bdata.m_vertices == null)
+                throw new NotSupportedException();
+            float[] result = new float[m_bdata.m_vertices.Count * 3];
+            foreach (KeyValuePair<Vertex, int> kvp in m_bdata.m_vertices)
+            {
+                Vertex v = kvp.Key;
+                int i = kvp.Value;
+                result[3 * i + 0] = v.X;
+                result[3 * i + 1] = v.Y;
+                result[3 * i + 2] = v.Z;
+            }
+            return result;
+        }
+
+        public float[] getVertexListAsFloatLocked()
+        {
+            return null;
+        }
+
+        public void getVertexListAsPtrToFloatArray(out IntPtr _vertices, out int vertexStride, out int vertexCount)
+        {
+            // A vertex is 3 floats
+            vertexStride = 3 * sizeof(float);
+
+            // If there isn't an unmanaged array allocated yet, do it now
+            if (m_verticesPtr == IntPtr.Zero && m_bdata != null)
+            {
+                vertices = getVertexListAsFloat();
+                // Each vertex is 3 elements (floats)
+                m_vertexCount = vertices.Length / 3;
+                vhandler = GCHandle.Alloc(vertices, GCHandleType.Pinned);
+                m_verticesPtr = vhandler.AddrOfPinnedObject();
+                GC.AddMemoryPressure(Buffer.ByteLength(vertices));
+            }
+            _vertices = m_verticesPtr;
+            vertexCount = m_vertexCount;
+        }
+
+        public int[] getIndexListAsInt()
+        {
+            if (m_bdata.m_triangles == null)
+                throw new NotSupportedException();
+            int[] result = new int[m_bdata.m_triangles.Count * 3];
+            for (int i = 0; i < m_bdata.m_triangles.Count; i++)
+            {
+                Triangle t = m_bdata.m_triangles[i];
+                result[3 * i + 0] = m_bdata.m_vertices[t.v1];
+                result[3 * i + 1] = m_bdata.m_vertices[t.v2];
+                result[3 * i + 2] = m_bdata.m_vertices[t.v3];
+            }
+            return result;
+        }
+
+        /// <summary>
+        /// creates a list of index values that defines triangle faces. THIS METHOD FREES ALL NON-PINNED MESH DATA
+        /// </summary>
+        /// <returns></returns>
+        public int[] getIndexListAsIntLocked()
+        {
+            return null;
+        }
+
+        public void getIndexListAsPtrToIntArray(out IntPtr indices, out int triStride, out int indexCount)
+        {
+            // If there isn't an unmanaged array allocated yet, do it now
+            if (m_indicesPtr == IntPtr.Zero && m_bdata != null)
+            {
+                indexes = getIndexListAsInt();
+                m_indexCount = indexes.Length;
+                ihandler = GCHandle.Alloc(indexes, GCHandleType.Pinned);
+                m_indicesPtr = ihandler.AddrOfPinnedObject();
+                GC.AddMemoryPressure(Buffer.ByteLength(indexes));
+            }
+            // A triangle is 3 ints (indices)
+            triStride = 3 * sizeof(int);
+            indices = m_indicesPtr;
+            indexCount = m_indexCount;
+        }
+
+        public void releasePinned()
+        {
+            if (m_verticesPtr != IntPtr.Zero)
+            {
+                vhandler.Free();
+                GC.RemoveMemoryPressure(Buffer.ByteLength(vertices));
+                vertices = null;
+                m_verticesPtr = IntPtr.Zero;
+            }
+            if (m_indicesPtr != IntPtr.Zero)
+            {
+                ihandler.Free();
+                GC.RemoveMemoryPressure(Buffer.ByteLength(indexes));
+                indexes = null;
+                m_indicesPtr = IntPtr.Zero;
+            }
+        }
+
+        /// <summary>
+        /// frees up the source mesh data to minimize memory - call this method after calling get*Locked() functions
+        /// </summary>
+        public void releaseSourceMeshData()
+        {
+            if (m_bdata != null)
+            {
+                m_bdata.m_triangles = null;
+                m_bdata.m_vertices = null;
+            }
+        }
+
+        public void releaseBuildingMeshData()
+        {
+            if (m_bdata != null)
+            {
+                m_bdata.m_triangles = null;
+                m_bdata.m_vertices = null;
+                m_bdata = null;
+            }
+        }
+
+        public void Append(IMesh newMesh)
+        {
+            if (m_indicesPtr != IntPtr.Zero || m_verticesPtr != IntPtr.Zero)
+                throw new NotSupportedException("Attempt to Append to a pinned Mesh");
+
+            if (!(newMesh is Mesh))
+                return;
+
+            foreach (Triangle t in ((Mesh)newMesh).m_bdata.m_triangles)
+                Add(t);
+        }
+
+        // Do a linear transformation of  mesh.
+        public void TransformLinear(float[,] matrix, float[] offset)
+        {
+            if (m_indicesPtr != IntPtr.Zero || m_verticesPtr != IntPtr.Zero)
+                throw new NotSupportedException("Attempt to TransformLinear a pinned Mesh");
+
+            foreach (Vertex v in m_bdata.m_vertices.Keys)
+            {
+                if (v == null)
+                    continue;
+                float x, y, z;
+                x = v.X*matrix[0, 0] + v.Y*matrix[1, 0] + v.Z*matrix[2, 0];
+                y = v.X*matrix[0, 1] + v.Y*matrix[1, 1] + v.Z*matrix[2, 1];
+                z = v.X*matrix[0, 2] + v.Y*matrix[1, 2] + v.Z*matrix[2, 2];
+                v.X = x + offset[0];
+                v.Y = y + offset[1];
+                v.Z = z + offset[2];
+            }
+        }
+
+        public void DumpRaw(String path, String name, String title)
+        {
+            if (path == null)
+                return;
+            if (m_bdata == null)
+                return;
+            String fileName = name + "_" + title + ".raw";
+            String completePath = System.IO.Path.Combine(path, fileName);
+            StreamWriter sw = new StreamWriter(completePath);
+            foreach (Triangle t in m_bdata.m_triangles)
+            {
+                String s = t.ToStringRaw();
+                sw.WriteLine(s);
+            }
+            sw.Close();
+        }
+
+        public void TrimExcess()
+        {
+            m_bdata.m_triangles.TrimExcess();
+        }
+
+        public void pinMemory()
+        {
+            m_vertexCount = vertices.Length / 3;
+            vhandler = GCHandle.Alloc(vertices, GCHandleType.Pinned);
+            m_verticesPtr = vhandler.AddrOfPinnedObject();
+            GC.AddMemoryPressure(Buffer.ByteLength(vertices));
+
+            m_indexCount = indexes.Length;
+            ihandler = GCHandle.Alloc(indexes, GCHandleType.Pinned);
+            m_indicesPtr = ihandler.AddrOfPinnedObject();
+            GC.AddMemoryPressure(Buffer.ByteLength(indexes));
+        }
+
+        public void PrepForOde()
+        {
+            // If there isn't an unmanaged array allocated yet, do it now
+            if (m_verticesPtr == IntPtr.Zero)
+                vertices = getVertexListAsFloat();
+
+            // If there isn't an unmanaged array allocated yet, do it now
+            if (m_indicesPtr == IntPtr.Zero)
+                indexes = getIndexListAsInt();
+
+            float x, y, z;
+
+            if (m_bdata.m_centroidDiv > 0)
+            {
+                m_obboffset = new Vector3(m_bdata.m_centroid.X / m_bdata.m_centroidDiv, m_bdata.m_centroid.Y / m_bdata.m_centroidDiv, m_bdata.m_centroid.Z / m_bdata.m_centroidDiv);
+                x = (m_bdata.m_obbXmax - m_bdata.m_obbXmin) * 0.5f;
+                if(x < 0.0005f)
+                    x = 0.0005f;
+                y = (m_bdata.m_obbYmax - m_bdata.m_obbYmin) * 0.5f;
+                if(y < 0.0005f)
+                    y = 0.0005f;
+                z = (m_bdata.m_obbZmax - m_bdata.m_obbZmin) * 0.5f;
+                if(z < 0.0005f)
+                    z = 0.0005f;
+            }
+
+            else
+            {
+                m_obboffset = Vector3.Zero;
+                x = 0.5f;
+                y = 0.5f;
+                z = 0.5f;
+            }
+
+            m_obb = new Vector3(x, y, z);
+
+            releaseBuildingMeshData();
+            pinMemory();
+        }
+
+        public bool ToStream(Stream st)
+        {
+            if (m_indicesPtr == IntPtr.Zero || m_verticesPtr == IntPtr.Zero)
+                return false;
+
+            bool ok = true;
+
+            try
+            {
+                using(BinaryWriter bw = new BinaryWriter(st))
+                {
+                    bw.Write(m_vertexCount);
+                    bw.Write(m_indexCount);
+
+                    for (int i = 0; i < 3 * m_vertexCount; i++)
+                        bw.Write(vertices[i]);
+                    for (int i = 0; i < m_indexCount; i++)
+                        bw.Write(indexes[i]);
+                    bw.Write(m_obb.X);
+                    bw.Write(m_obb.Y);
+                    bw.Write(m_obb.Z);
+                    bw.Write(m_obboffset.X);
+                    bw.Write(m_obboffset.Y);
+                    bw.Write(m_obboffset.Z);
+                    bw.Flush();
+                    bw.Close();
+               }
+            }
+            catch
+            {
+                ok = false;
+            }
+
+            return ok;
+        }
+
+        public static Mesh FromStream(Stream st, AMeshKey key)
+        {
+            Mesh mesh = new Mesh(false);
+
+            bool ok = true;
+            try
+            {
+                using(BinaryReader br = new BinaryReader(st))
+                {
+                    mesh.m_vertexCount = br.ReadInt32();
+                    mesh.m_indexCount = br.ReadInt32();
+
+                    int n = 3 * mesh.m_vertexCount;
+                    mesh.vertices = new float[n];
+                    for (int i = 0; i < n; i++)
+                        mesh.vertices[i] = br.ReadSingle();
+
+                    mesh.indexes = new int[mesh.m_indexCount];
+                    for (int i = 0; i < mesh.m_indexCount; i++)
+                        mesh.indexes[i] = br.ReadInt32();
+
+                    mesh.m_obb.X = br.ReadSingle();
+                    mesh.m_obb.Y = br.ReadSingle();
+                    mesh.m_obb.Z = br.ReadSingle();
+
+                    mesh.m_obboffset.X = br.ReadSingle();
+                    mesh.m_obboffset.Y = br.ReadSingle();
+                    mesh.m_obboffset.Z = br.ReadSingle();
+                }
+            }
+            catch
+            {
+                ok = false;
+            }
+
+            if (ok)
+            {
+                mesh.pinMemory();
+
+                mesh.Key = key;
+                mesh.RefCount = 1;
+
+                return mesh;
+            }
+
+            mesh.vertices = null;
+            mesh.indexes = null;
+            return null;
+        }
+    }
+}
diff --git a/OpenSim/Region/PhysicsModules/ubOdeMeshing/Meshmerizer.cs b/OpenSim/Region/PhysicsModules/ubOdeMeshing/Meshmerizer.cs
new file mode 100644
index 0000000..032d4ed
--- /dev/null
+++ b/OpenSim/Region/PhysicsModules/ubOdeMeshing/Meshmerizer.cs
@@ -0,0 +1,1602 @@
+/*
+ * Copyright (c) Contributors, http://opensimulator.org/
+ * See CONTRIBUTORS.TXT for a full list of copyright holders.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the OpenSimulator Project nor the
+ *       names of its contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+//#define SPAM
+
+using System;
+using System.Collections.Generic;
+using OpenSim.Framework;
+using OpenSim.Region.Framework.Scenes;
+using OpenSim.Region.Framework.Interfaces;
+using OpenSim.Region.PhysicsModules.SharedBase;
+using OpenSim.Region.PhysicsModules.ConvexDecompositionDotNet;
+using OpenMetaverse;
+using OpenMetaverse.StructuredData;
+using System.Drawing;
+using System.Threading;
+using System.IO.Compression;
+using PrimMesher;
+using log4net;
+using Nini.Config;
+using System.Reflection;
+using System.IO;
+
+using Mono.Addins;
+
+namespace OpenSim.Region.PhysicsModule.ubODEMeshing
+{
+    [Extension(Path = "/OpenSim/RegionModules", NodeName = "RegionModule", Id = "ubODEMeshmerizer")]
+    public class ubMeshmerizer : IMesher, INonSharedRegionModule
+    {
+        private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
+
+        // Setting baseDir to a path will enable the dumping of raw files
+        // raw files can be imported by blender so a visual inspection of the results can be done
+
+        private static string cacheControlFilename = "cntr";
+        private bool m_Enabled = false;
+
+        public static object diskLock = new object();
+
+        public bool doMeshFileCache = true;
+        public bool doCacheExpire = true;
+        public string cachePath = "MeshCache";
+        public TimeSpan CacheExpire;
+
+//        const string baseDir = "rawFiles";
+        private const string baseDir = null; //"rawFiles";
+
+        private bool useMeshiesPhysicsMesh = true;
+        private bool doConvexPrims = true;
+        private bool doConvexSculpts = true;
+
+        private Dictionary<AMeshKey, Mesh> m_uniqueMeshes = new Dictionary<AMeshKey, Mesh>();
+        private Dictionary<AMeshKey, Mesh> m_uniqueReleasedMeshes = new Dictionary<AMeshKey, Mesh>();
+
+       #region INonSharedRegionModule
+        public string Name
+        {
+            get { return "ubODEMeshmerizer"; }
+        }
+
+        public Type ReplaceableInterface
+        {
+            get { return null; }
+        }
+
+        public void Initialise(IConfigSource config)
+        {
+            IConfig start_config = config.Configs["Startup"];
+
+            string mesher = start_config.GetString("meshing", string.Empty);
+            if (mesher == Name)
+            {
+                float fcache = 48.0f;
+                //            float fcache = 0.02f;
+
+                IConfig mesh_config = config.Configs["Mesh"];
+                if (mesh_config != null)
+                {
+                    useMeshiesPhysicsMesh = mesh_config.GetBoolean("UseMeshiesPhysicsMesh", useMeshiesPhysicsMesh);
+                    doConvexPrims = mesh_config.GetBoolean("ConvexPrims",doConvexPrims);
+                    doConvexSculpts = mesh_config.GetBoolean("ConvexSculpts",doConvexPrims);
+                    doMeshFileCache = mesh_config.GetBoolean("MeshFileCache", doMeshFileCache);
+                    cachePath = mesh_config.GetString("MeshFileCachePath", cachePath);
+                    fcache = mesh_config.GetFloat("MeshFileCacheExpireHours", fcache);
+                    doCacheExpire = mesh_config.GetBoolean("MeshFileCacheDoExpire", doCacheExpire);
+
+                    m_Enabled = true;
+                }
+
+                CacheExpire = TimeSpan.FromHours(fcache);
+
+                if(String.IsNullOrEmpty(cachePath))
+                    doMeshFileCache = false;
+
+                if(doMeshFileCache)
+                {
+                    if(!checkCache())
+                    {
+                        doMeshFileCache = false;
+                        doCacheExpire = false;
+                    }
+                }
+                else
+                    doCacheExpire = false;
+            }
+        }
+
+        public void Close()
+        {
+        }
+
+        public void AddRegion(Scene scene)
+        {
+            if (!m_Enabled)
+                return;
+
+            scene.RegisterModuleInterface<IMesher>(this);
+        }
+
+        public void RemoveRegion(Scene scene)
+        {
+            if (!m_Enabled)
+                return;
+
+            scene.UnregisterModuleInterface<IMesher>(this);
+        }
+
+        public void RegionLoaded(Scene scene)
+        {
+            if (!m_Enabled)
+                return;
+        }
+
+        #endregion
+
+        private void ReportPrimError(string message, string primName, PrimMesh primMesh)
+        {
+            m_log.Error(message);
+            m_log.Error("\nPrim Name: " + primName);
+            m_log.Error("****** PrimMesh Parameters ******\n" + primMesh.ParamsToDisplayString());
+        }
+
+        /// <summary>
+        /// Add a submesh to an existing list of coords and faces.
+        /// </summary>
+        /// <param name="subMeshData"></param>
+        /// <param name="size">Size of entire object</param>
+        /// <param name="coords"></param>
+        /// <param name="faces"></param>
+        private void AddSubMesh(OSDMap subMeshData, List<Coord> coords, List<Face> faces)
+        {
+            // Console.WriteLine("subMeshMap for {0} - {1}", primName, Util.GetFormattedXml((OSD)subMeshMap));
+
+            // As per http://wiki.secondlife.com/wiki/Mesh/Mesh_Asset_Format, some Mesh Level
+            // of Detail Blocks (maps) contain just a NoGeometry key to signal there is no
+            // geometry for this submesh.
+            if (subMeshData.ContainsKey("NoGeometry") && ((OSDBoolean)subMeshData["NoGeometry"]))
+                return;
+
+            OpenMetaverse.Vector3 posMax;
+            OpenMetaverse.Vector3 posMin;
+            if (subMeshData.ContainsKey("PositionDomain"))
+            {
+                posMax = ((OSDMap)subMeshData["PositionDomain"])["Max"].AsVector3();
+                posMin = ((OSDMap)subMeshData["PositionDomain"])["Min"].AsVector3();
+            }
+            else
+            {
+                posMax = new Vector3(0.5f, 0.5f, 0.5f);
+                posMin = new Vector3(-0.5f, -0.5f, -0.5f);
+            }
+
+            ushort faceIndexOffset = (ushort)coords.Count;
+
+            byte[] posBytes = subMeshData["Position"].AsBinary();
+            for (int i = 0; i < posBytes.Length; i += 6)
+            {
+                ushort uX = Utils.BytesToUInt16(posBytes, i);
+                ushort uY = Utils.BytesToUInt16(posBytes, i + 2);
+                ushort uZ = Utils.BytesToUInt16(posBytes, i + 4);
+
+                Coord c = new Coord(
+                Utils.UInt16ToFloat(uX, posMin.X, posMax.X),
+                Utils.UInt16ToFloat(uY, posMin.Y, posMax.Y),
+                Utils.UInt16ToFloat(uZ, posMin.Z, posMax.Z));
+
+                coords.Add(c);
+            }
+
+            byte[] triangleBytes = subMeshData["TriangleList"].AsBinary();
+            for (int i = 0; i < triangleBytes.Length; i += 6)
+            {
+                ushort v1 = (ushort)(Utils.BytesToUInt16(triangleBytes, i) + faceIndexOffset);
+                ushort v2 = (ushort)(Utils.BytesToUInt16(triangleBytes, i + 2) + faceIndexOffset);
+                ushort v3 = (ushort)(Utils.BytesToUInt16(triangleBytes, i + 4) + faceIndexOffset);
+                Face f = new Face(v1, v2, v3);
+                faces.Add(f);
+            }
+        }
+
+        /// <summary>
+        /// Create a physics mesh from data that comes with the prim.  The actual data used depends on the prim type.
+        /// </summary>
+        /// <param name="primName"></param>
+        /// <param name="primShape"></param>
+        /// <param name="size"></param>
+        /// <param name="lod"></param>
+        /// <returns></returns>
+        private Mesh CreateMeshFromPrimMesher(string primName, PrimitiveBaseShape primShape, float lod, bool convex)
+        {
+//            m_log.DebugFormat(
+//                "[MESH]: Creating physics proxy for {0}, shape {1}",
+//                primName, (OpenMetaverse.SculptType)primShape.SculptType);
+
+            List<Coord> coords;
+            List<Face> faces;
+            bool needsConvexProcessing = convex;
+
+            if (primShape.SculptEntry)
+            {
+                if (((OpenMetaverse.SculptType)primShape.SculptType) == SculptType.Mesh)
+                {
+                    if (!useMeshiesPhysicsMesh)
+                        return null;
+
+                    if (!GenerateCoordsAndFacesFromPrimMeshData(primName, primShape, out coords, out faces, convex))
+                        return null;
+                    needsConvexProcessing = false;
+                }
+                else
+                {
+                    if (!GenerateCoordsAndFacesFromPrimSculptData(primName, primShape, lod, out coords, out faces))
+                        return null;
+                    needsConvexProcessing &= doConvexSculpts;
+                }
+            }
+            else
+            {
+                if (!GenerateCoordsAndFacesFromPrimShapeData(primName, primShape, lod, convex, out coords, out faces))
+                    return null;
+                 needsConvexProcessing &= doConvexPrims;
+            }
+
+            int numCoords = coords.Count;
+            int numFaces = faces.Count;
+
+            if(numCoords < 3 || (!needsConvexProcessing && numFaces < 1))
+            {
+                m_log.ErrorFormat("[MESH]: invalid degenerated mesh for prim {0} ignored", primName);
+                return null;
+            }
+
+            if(needsConvexProcessing)
+            {
+                List<Coord> convexcoords;
+                List<Face> convexfaces;
+                if(CreateBoundingHull(coords, out convexcoords, out convexfaces) && convexcoords != null && convexfaces != null)
+                {
+                    coords.Clear();
+                    coords = convexcoords;
+                    numCoords = coords.Count;
+
+                    faces.Clear();
+                    faces = convexfaces;
+                    numFaces = faces.Count;
+                }
+                else
+                     m_log.ErrorFormat("[ubMESH]: failed to create convex for {0} using normal mesh", primName);
+            }
+
+            Mesh mesh = new Mesh(true);
+            // Add the corresponding triangles to the mesh
+            for (int i = 0; i < numFaces; i++)
+            {
+                Face f = faces[i];
+                mesh.Add(new Triangle(coords[f.v1].X, coords[f.v1].Y, coords[f.v1].Z,
+                                      coords[f.v2].X, coords[f.v2].Y, coords[f.v2].Z,
+                                      coords[f.v3].X, coords[f.v3].Y, coords[f.v3].Z));
+            }
+
+            coords.Clear();
+            faces.Clear();
+
+            if(mesh.numberVertices() < 3 || mesh.numberTriangles() < 1)
+            {
+                m_log.ErrorFormat("[MESH]: invalid degenerated mesh for prim {0} ignored", primName);
+                return null;
+            }
+
+            primShape.SculptData = Utils.EmptyBytes;
+
+            return mesh;
+        }
+
+        /// <summary>
+        /// Generate the co-ords and faces necessary to construct a mesh from the mesh data the accompanies a prim.
+        /// </summary>
+        /// <param name="primName"></param>
+        /// <param name="primShape"></param>
+        /// <param name="size"></param>
+        /// <param name="coords">Coords are added to this list by the method.</param>
+        /// <param name="faces">Faces are added to this list by the method.</param>
+        /// <returns>true if coords and faces were successfully generated, false if not</returns>
+        private bool GenerateCoordsAndFacesFromPrimMeshData(
+            string primName, PrimitiveBaseShape primShape, out List<Coord> coords, out List<Face> faces, bool convex)
+        {
+//            m_log.DebugFormat("[MESH]: experimental mesh proxy generation for {0}", primName);
+
+
+            // for ubOde we have a diferent mesh use priority
+            // priority is to use full mesh then decomposition
+            // SL does the oposite
+            bool usemesh = false;
+
+            coords = new List<Coord>();
+            faces = new List<Face>();
+            OSD meshOsd = null;
+
+            if (primShape.SculptData == null || primShape.SculptData.Length <= 0)
+            {
+//                m_log.InfoFormat("[MESH]: asset data for {0} is zero length", primName);
+                return false;
+            }
+
+            long start = 0;
+            using (MemoryStream data = new MemoryStream(primShape.SculptData))
+            {
+                try
+                {
+                    OSD osd = OSDParser.DeserializeLLSDBinary(data);
+                    if (osd is OSDMap)
+                        meshOsd = (OSDMap)osd;
+                    else
+                    {
+                        m_log.WarnFormat("[Mesh}: unable to cast mesh asset to OSDMap prim: {0}",primName);
+                        return false;
+                    }
+                }
+                catch (Exception e)
+                {
+                    m_log.Error("[MESH]: Exception deserializing mesh asset header:" + e.ToString());
+                    return false;
+                }
+
+                start = data.Position;
+            }
+
+            if (meshOsd is OSDMap)
+            {
+                OSDMap physicsParms = null;
+                OSDMap map = (OSDMap)meshOsd;
+
+                if (!convex)
+                {
+                    if (map.ContainsKey("physics_shape"))
+                        physicsParms = (OSDMap)map["physics_shape"]; // old asset format
+                    else if (map.ContainsKey("physics_mesh"))
+                        physicsParms = (OSDMap)map["physics_mesh"]; // new asset format
+
+                    if (physicsParms != null)
+                        usemesh = true;
+                }
+
+                if(!usemesh && (map.ContainsKey("physics_convex")))
+                        physicsParms = (OSDMap)map["physics_convex"];
+
+                if (physicsParms == null)
+                {
+                    //m_log.WarnFormat("[MESH]: unknown mesh type for prim {0}",primName);
+                    return false;
+                }
+
+                int physOffset = physicsParms["offset"].AsInteger() + (int)start;
+                int physSize = physicsParms["size"].AsInteger();
+
+                if (physOffset < 0 || physSize == 0)
+                    return false; // no mesh data in asset
+
+                OSD decodedMeshOsd = new OSD();
+                byte[] meshBytes = new byte[physSize];
+                System.Buffer.BlockCopy(primShape.SculptData, physOffset, meshBytes, 0, physSize);
+
+                try
+                {
+                    using (MemoryStream inMs = new MemoryStream(meshBytes))
+                    {
+                        using (MemoryStream outMs = new MemoryStream())
+                        {
+                            using (DeflateStream decompressionStream = new DeflateStream(inMs, CompressionMode.Decompress))
+                            {
+                                byte[] readBuffer = new byte[2048];
+                                inMs.Read(readBuffer, 0, 2); // skip first 2 bytes in header
+                                int readLen = 0;
+
+                                while ((readLen = decompressionStream.Read(readBuffer, 0, readBuffer.Length)) > 0)
+                                    outMs.Write(readBuffer, 0, readLen);
+
+                                outMs.Flush();
+                                outMs.Seek(0, SeekOrigin.Begin);
+
+                                byte[] decompressedBuf = outMs.GetBuffer();
+
+                                decodedMeshOsd = OSDParser.DeserializeLLSDBinary(decompressedBuf);
+                            }
+                        }
+                    }
+                }
+                catch (Exception e)
+                {
+                    m_log.Error("[MESH]: exception decoding physical mesh prim " + primName +" : " + e.ToString());
+                    return false;
+                }
+
+                if (usemesh)
+                {
+                    OSDArray decodedMeshOsdArray = null;
+
+                    // physics_shape is an array of OSDMaps, one for each submesh
+                    if (decodedMeshOsd is OSDArray)
+                    {
+//                      Console.WriteLine("decodedMeshOsd for {0} - {1}", primName, Util.GetFormattedXml(decodedMeshOsd));
+
+                        decodedMeshOsdArray = (OSDArray)decodedMeshOsd;
+                        foreach (OSD subMeshOsd in decodedMeshOsdArray)
+                        {
+                            if (subMeshOsd is OSDMap)
+                                AddSubMesh(subMeshOsd as OSDMap, coords, faces);
+                        }
+                    }
+                }
+                else
+                {
+                    OSDMap cmap = (OSDMap)decodedMeshOsd;
+                    if (cmap == null)
+                        return false;
+
+                    byte[] data;
+
+                    List<float3> vs = new List<float3>();
+                    PHullResult hullr = new PHullResult();
+                    float3 f3;
+                    Coord c;
+                    Face f;
+                    Vector3 range;
+                    Vector3 min;
+
+                    const float invMaxU16 = 1.0f / 65535f;
+                    int t1;
+                    int t2;
+                    int t3;
+                    int i;
+                    int nverts;
+                    int nindexs;
+
+                    if (cmap.ContainsKey("Max"))
+                        range = cmap["Max"].AsVector3();
+                    else
+                        range = new Vector3(0.5f, 0.5f, 0.5f);
+
+                    if (cmap.ContainsKey("Min"))
+                        min = cmap["Min"].AsVector3();
+                    else
+                        min = new Vector3(-0.5f, -0.5f, -0.5f);
+
+                    range = range - min;
+                    range *= invMaxU16;
+
+                    if(!convex)
+                    {
+                        // if mesh data not present and not convex then we need convex decomposition data
+                        if (cmap.ContainsKey("HullList") && cmap.ContainsKey("Positions"))
+                        {
+                            List<int> hsizes = new List<int>();
+                            int totalpoints = 0;
+                            data = cmap["HullList"].AsBinary();
+                            for (i = 0; i < data.Length; i++)
+                            {
+                                t1 = data[i];
+                                if (t1 == 0)
+                                    t1 = 256;
+                                totalpoints += t1;
+                                hsizes.Add(t1);
+                            }
+
+                            data = cmap["Positions"].AsBinary();
+                            int ptr = 0;
+                            int vertsoffset = 0;
+
+                            if (totalpoints == data.Length / 6) // 2 bytes per coord, 3 coords per point
+                            {
+                                foreach (int hullsize in hsizes)
+                                {
+                                    for (i = 0; i < hullsize; i++ )
+                                    {
+                                        t1 = data[ptr++];
+                                        t1 += data[ptr++] << 8;
+                                        t2 = data[ptr++];
+                                        t2 += data[ptr++] << 8;
+                                        t3 = data[ptr++];
+                                        t3 += data[ptr++] << 8;
+
+                                        f3 = new float3((t1 * range.X + min.X),
+                                                  (t2 * range.Y + min.Y),
+                                                  (t3 * range.Z + min.Z));
+                                        vs.Add(f3);
+                                    }
+
+                                    if(hullsize <3)
+                                    {
+                                        vs.Clear();
+                                        continue;
+                                    }
+
+                                    if (hullsize <5)
+                                    {
+                                        foreach (float3 point in vs)
+                                        {
+                                            c.X = point.x;
+                                            c.Y = point.y;
+                                            c.Z = point.z;
+                                            coords.Add(c);
+                                        }
+                                        f = new Face(vertsoffset, vertsoffset + 1, vertsoffset + 2);
+                                        faces.Add(f);
+
+                                        if (hullsize == 4)
+                                        {
+                                            // not sure about orientation..
+                                            f = new Face(vertsoffset, vertsoffset + 2, vertsoffset + 3);
+                                            faces.Add(f);
+                                            f = new Face(vertsoffset, vertsoffset + 3, vertsoffset + 1);
+                                            faces.Add(f);
+                                            f = new Face(vertsoffset + 3, vertsoffset + 2, vertsoffset + 1);
+                                            faces.Add(f);
+                                        }
+                                        vertsoffset += vs.Count;
+                                        vs.Clear();
+                                        continue;
+                                    }
+
+                                    List<int> indices;
+                                    if (!HullUtils.ComputeHull(vs, out indices))
+                                    {
+                                        vs.Clear();
+                                        continue;
+                                    }
+
+                                    nverts = vs.Count;
+                                    nindexs = indices.Count;
+
+                                    if (nindexs % 3 != 0)
+                                    {
+                                        vs.Clear();
+                                        continue;
+                                    }
+
+                                    for (i = 0; i < nverts; i++)
+                                    {
+                                        c.X = vs[i].x;
+                                        c.Y = vs[i].y;
+                                        c.Z = vs[i].z;
+                                        coords.Add(c);
+                                    }
+
+                                    for (i = 0; i < nindexs; i += 3)
+                                    {
+                                        t1 = indices[i];
+                                        if (t1 > nverts)
+                                            break;
+                                        t2 = indices[i + 1];
+                                        if (t2 > nverts)
+                                            break;
+                                        t3 = indices[i + 2];
+                                        if (t3 > nverts)
+                                            break;
+                                        f = new Face(vertsoffset + t1, vertsoffset + t2, vertsoffset + t3);
+                                        faces.Add(f);
+                                    }
+                                    vertsoffset += nverts;
+                                    vs.Clear();
+                                }
+                            }
+                            if (coords.Count > 0 && faces.Count > 0)
+                                return true;
+                        }
+                        else
+                        {
+                            // if neither mesh or decomposition present, warn and use convex
+                            //m_log.WarnFormat("[MESH]: Data for PRIM shape type ( mesh or decomposition) not found for prim {0}",primName);
+                        }
+                    }
+                    vs.Clear();
+
+                    if (cmap.ContainsKey("BoundingVerts"))
+                    {
+                        data = cmap["BoundingVerts"].AsBinary();
+
+                        for (i = 0; i < data.Length; )
+                        {
+                            t1 = data[i++];
+                            t1 += data[i++] << 8;
+                            t2 = data[i++];
+                            t2 += data[i++] << 8;
+                            t3 = data[i++];
+                            t3 += data[i++] << 8;
+
+                            f3 = new float3((t1 * range.X + min.X),
+                                      (t2 * range.Y + min.Y),
+                                      (t3 * range.Z + min.Z));
+                            vs.Add(f3);
+                        }
+
+                        nverts = vs.Count;
+
+                        if (nverts < 3)
+                        {
+                            vs.Clear();
+                            return false;
+                        }
+
+                        if (nverts < 5)
+                        {
+                            foreach (float3 point in vs)
+                            {
+                                c.X = point.x;
+                                c.Y = point.y;
+                                c.Z = point.z;
+                                coords.Add(c);
+                            }
+
+                            f = new Face(0, 1, 2);
+                            faces.Add(f);
+
+                            if (nverts == 4)
+                            {
+                                f = new Face(0, 2, 3);
+                                faces.Add(f);
+                                f = new Face(0, 3, 1);
+                                faces.Add(f);
+                                f = new Face( 3, 2, 1);
+                                faces.Add(f);
+                            }
+                            vs.Clear();
+                            return true;
+                        }
+
+                        List<int> indices;
+                        if (!HullUtils.ComputeHull(vs, out indices))
+                            return false;
+
+                        nindexs = indices.Count;
+
+                        if (nindexs % 3 != 0)
+                            return false;
+
+                        for (i = 0; i < nverts; i++)
+                        {
+                            c.X = vs[i].x;
+                            c.Y = vs[i].y;
+                            c.Z = vs[i].z;
+                            coords.Add(c);
+                        }
+                        for (i = 0; i < nindexs; i += 3)
+                        {
+                            t1 = indices[i];
+                            if (t1 > nverts)
+                                break;
+                            t2 = indices[i + 1];
+                            if (t2 > nverts)
+                                break;
+                            t3 = indices[i + 2];
+                            if (t3 > nverts)
+                                break;
+                            f = new Face(t1, t2, t3);
+                            faces.Add(f);
+                        }
+                        vs.Clear();
+                        if (coords.Count > 0 && faces.Count > 0)
+                            return true;
+                    }
+                    else
+                        return false;
+                }
+            }
+
+            return true;
+        }
+
+        /// <summary>
+        /// Generate the co-ords and faces necessary to construct a mesh from the sculpt data the accompanies a prim.
+        /// </summary>
+        /// <param name="primName"></param>
+        /// <param name="primShape"></param>
+        /// <param name="size"></param>
+        /// <param name="lod"></param>
+        /// <param name="coords">Coords are added to this list by the method.</param>
+        /// <param name="faces">Faces are added to this list by the method.</param>
+        /// <returns>true if coords and faces were successfully generated, false if not</returns>
+        private bool GenerateCoordsAndFacesFromPrimSculptData(
+            string primName, PrimitiveBaseShape primShape, float lod, out List<Coord> coords, out List<Face> faces)
+        {
+            coords = new List<Coord>();
+            faces = new List<Face>();
+            PrimMesher.SculptMesh sculptMesh;
+            Image idata = null;
+
+                if (primShape.SculptData == null || primShape.SculptData.Length == 0)
+                    return false;
+
+                try
+                {
+                    OpenMetaverse.Imaging.ManagedImage unusedData;
+                    OpenMetaverse.Imaging.OpenJPEG.DecodeToImage(primShape.SculptData, out unusedData, out idata);
+
+                    unusedData = null;
+
+                    if (idata == null)
+                    {
+                        // In some cases it seems that the decode can return a null bitmap without throwing
+                        // an exception
+                        m_log.WarnFormat("[PHYSICS]: OpenJPEG decoded sculpt data for {0} to a null bitmap.  Ignoring.", primName);
+                        return false;
+                    }
+                }
+                catch (DllNotFoundException)
+                {
+                    m_log.Error("[PHYSICS]: OpenJpeg is not installed correctly on this system. Physics Proxy generation failed.  Often times this is because of an old version of GLIBC.  You must have version 2.4 or above!");
+                    return false;
+                }
+                catch (IndexOutOfRangeException)
+                {
+                    m_log.Error("[PHYSICS]: OpenJpeg was unable to decode this. Physics Proxy generation failed");
+                    return false;
+                }
+                catch (Exception ex)
+                {
+                    m_log.Error("[PHYSICS]: Unable to generate a Sculpty physics proxy. Sculpty texture decode failed: " + ex.Message);
+                    return false;
+                }
+
+            PrimMesher.SculptMesh.SculptType sculptType;
+            // remove mirror and invert bits
+            OpenMetaverse.SculptType pbsSculptType = ((OpenMetaverse.SculptType)(primShape.SculptType & 0x3f));
+            switch (pbsSculptType)
+            {
+                case OpenMetaverse.SculptType.Cylinder:
+                    sculptType = PrimMesher.SculptMesh.SculptType.cylinder;
+                    break;
+                case OpenMetaverse.SculptType.Plane:
+                    sculptType = PrimMesher.SculptMesh.SculptType.plane;
+                    break;
+                case OpenMetaverse.SculptType.Torus:
+                    sculptType = PrimMesher.SculptMesh.SculptType.torus;
+                    break;
+                case OpenMetaverse.SculptType.Sphere:
+                    sculptType = PrimMesher.SculptMesh.SculptType.sphere;
+                    break;
+                default:
+                    sculptType = PrimMesher.SculptMesh.SculptType.plane;
+                    break;
+            }
+
+            bool mirror = ((primShape.SculptType & 128) != 0);
+            bool invert = ((primShape.SculptType & 64) != 0);
+
+            sculptMesh = new PrimMesher.SculptMesh((Bitmap)idata, sculptType, (int)lod, mirror, invert);
+
+            idata.Dispose();
+
+//            sculptMesh.DumpRaw(baseDir, primName, "primMesh");
+
+            coords = sculptMesh.coords;
+            faces = sculptMesh.faces;
+
+            return true;
+        }
+
+        /// <summary>
+        /// Generate the co-ords and faces necessary to construct a mesh from the shape data the accompanies a prim.
+        /// </summary>
+        /// <param name="primName"></param>
+        /// <param name="primShape"></param>
+        /// <param name="size"></param>
+        /// <param name="coords">Coords are added to this list by the method.</param>
+        /// <param name="faces">Faces are added to this list by the method.</param>
+        /// <returns>true if coords and faces were successfully generated, false if not</returns>
+        private bool GenerateCoordsAndFacesFromPrimShapeData(
+                string primName, PrimitiveBaseShape primShape, float lod, bool convex,
+                out List<Coord> coords, out List<Face> faces)
+        {
+            PrimMesh primMesh;
+            coords = new List<Coord>();
+            faces = new List<Face>();
+
+            float pathShearX = primShape.PathShearX < 128 ? (float)primShape.PathShearX * 0.01f : (float)(primShape.PathShearX - 256) * 0.01f;
+            float pathShearY = primShape.PathShearY < 128 ? (float)primShape.PathShearY * 0.01f : (float)(primShape.PathShearY - 256) * 0.01f;
+            float pathBegin = (float)primShape.PathBegin * 2.0e-5f;
+            float pathEnd = 1.0f - (float)primShape.PathEnd * 2.0e-5f;
+            float pathScaleX = (float)(primShape.PathScaleX - 100) * 0.01f;
+            float pathScaleY = (float)(primShape.PathScaleY - 100) * 0.01f;
+
+            float profileBegin = (float)primShape.ProfileBegin * 2.0e-5f;
+            float profileEnd = 1.0f - (float)primShape.ProfileEnd * 2.0e-5f;
+
+            if (profileBegin < 0.0f)
+                profileBegin = 0.0f;
+
+            if (profileEnd < 0.02f)
+                profileEnd = 0.02f;
+            else if (profileEnd > 1.0f)
+                profileEnd = 1.0f;
+
+            if (profileBegin >= profileEnd)
+                profileBegin = profileEnd - 0.02f;
+
+            float profileHollow = (float)primShape.ProfileHollow * 2.0e-5f;
+            if(convex)
+                profileHollow = 0.0f;
+            else if (profileHollow > 0.95f)
+                profileHollow = 0.95f;
+
+            int sides = 4;
+            LevelOfDetail iLOD = (LevelOfDetail)lod;
+            byte profshape = (byte)(primShape.ProfileCurve & 0x07);
+
+            if (profshape == (byte)ProfileShape.EquilateralTriangle
+                || profshape == (byte)ProfileShape.IsometricTriangle
+                || profshape == (byte)ProfileShape.RightTriangle)
+                sides = 3;
+            else if (profshape == (byte)ProfileShape.Circle)
+            {
+                switch (iLOD)
+                {
+                    case LevelOfDetail.High:    sides = 24;     break;
+                    case LevelOfDetail.Medium:  sides = 12;     break;
+                    case LevelOfDetail.Low:     sides = 6;      break;
+                    case LevelOfDetail.VeryLow: sides = 3;      break;
+                    default:                    sides = 24;     break;
+                }
+            }
+            else if (profshape == (byte)ProfileShape.HalfCircle)
+            { // half circle, prim is a sphere
+                switch (iLOD)
+                {
+                    case LevelOfDetail.High:    sides = 24;     break;
+                    case LevelOfDetail.Medium:  sides = 12;     break;
+                    case LevelOfDetail.Low:     sides = 6;      break;
+                    case LevelOfDetail.VeryLow: sides = 3;      break;
+                    default:                    sides = 24;     break;
+                }
+
+                profileBegin = 0.5f * profileBegin + 0.5f;
+                profileEnd = 0.5f * profileEnd + 0.5f;
+            }
+
+            int hollowSides = sides;
+            if (primShape.HollowShape == HollowShape.Circle)
+            {
+                switch (iLOD)
+                {
+                    case LevelOfDetail.High:    hollowSides = 24;     break;
+                    case LevelOfDetail.Medium:  hollowSides = 12;     break;
+                    case LevelOfDetail.Low:     hollowSides = 6;      break;
+                    case LevelOfDetail.VeryLow: hollowSides = 3;      break;
+                    default:                    hollowSides = 24;     break;
+                }
+            }
+            else if (primShape.HollowShape == HollowShape.Square)
+                hollowSides = 4;
+            else if (primShape.HollowShape == HollowShape.Triangle)
+            {
+                if (profshape == (byte)ProfileShape.HalfCircle)
+                    hollowSides = 6;
+                else
+                    hollowSides = 3;
+            }
+
+            primMesh = new PrimMesh(sides, profileBegin, profileEnd, profileHollow, hollowSides);
+
+            if (primMesh.errorMessage != null)
+                if (primMesh.errorMessage.Length > 0)
+                    m_log.Error("[ERROR] " + primMesh.errorMessage);
+
+            primMesh.topShearX = pathShearX;
+            primMesh.topShearY = pathShearY;
+            primMesh.pathCutBegin = pathBegin;
+            primMesh.pathCutEnd = pathEnd;
+
+            if (primShape.PathCurve == (byte)Extrusion.Straight || primShape.PathCurve == (byte) Extrusion.Flexible)
+            {
+                primMesh.twistBegin = (primShape.PathTwistBegin * 18) / 10;
+                primMesh.twistEnd = (primShape.PathTwist * 18) / 10;
+                primMesh.taperX = pathScaleX;
+                primMesh.taperY = pathScaleY;
+
+#if SPAM
+            m_log.Debug("****** PrimMesh Parameters (Linear) ******\n" + primMesh.ParamsToDisplayString());
+#endif
+                try
+                {
+                    primMesh.ExtrudeLinear();
+                }
+                catch (Exception ex)
+                {
+                    ReportPrimError("Extrusion failure: exception: " + ex.ToString(), primName, primMesh);
+                    return false;
+                }
+            }
+            else
+            {
+                primMesh.holeSizeX = (200 - primShape.PathScaleX) * 0.01f;
+                primMesh.holeSizeY = (200 - primShape.PathScaleY) * 0.01f;
+                primMesh.radius = 0.01f * primShape.PathRadiusOffset;
+                primMesh.revolutions = 1.0f + 0.015f * primShape.PathRevolutions;
+                primMesh.skew = 0.01f * primShape.PathSkew;
+                primMesh.twistBegin = (primShape.PathTwistBegin * 36) / 10;
+                primMesh.twistEnd = (primShape.PathTwist * 36) / 10;
+                primMesh.taperX = primShape.PathTaperX * 0.01f;
+                primMesh.taperY = primShape.PathTaperY * 0.01f;
+
+                if(profshape == (byte)ProfileShape.HalfCircle)
+                {
+                    if(primMesh.holeSizeY < 0.01f)
+                        primMesh.holeSizeY = 0.01f;
+                    else if(primMesh.holeSizeY > 1.0f)
+                        primMesh.holeSizeY = 1.0f;
+                }
+
+#if SPAM
+            m_log.Debug("****** PrimMesh Parameters (Circular) ******\n" + primMesh.ParamsToDisplayString());
+#endif
+                try
+                {
+                    primMesh.ExtrudeCircular();
+                }
+                catch (Exception ex)
+                {
+                    ReportPrimError("Extrusion failure: exception: " + ex.ToString(), primName, primMesh);
+                    return false;
+                }
+            }
+
+//            primMesh.DumpRaw(baseDir, primName, "primMesh");
+
+            coords = primMesh.coords;
+            faces = primMesh.faces;
+
+            return true;
+        }
+
+        public AMeshKey GetMeshUniqueKey(PrimitiveBaseShape primShape, Vector3 size, byte lod, bool convex)
+        {
+            AMeshKey key = new AMeshKey();
+            Byte[] someBytes;
+
+            key.hashB = 5181;
+            key.hashC = 5181;
+            ulong hash = 5381;
+
+            if (primShape.SculptEntry)
+            {
+                key.uuid = primShape.SculptTexture;
+                key.hashC = mdjb2(key.hashC, primShape.SculptType);
+                key.hashC = mdjb2(key.hashC, primShape.PCode);
+            }
+            else
+            {
+                hash = mdjb2(hash, primShape.PathCurve);
+                hash = mdjb2(hash, (byte)primShape.HollowShape);
+                hash = mdjb2(hash, (byte)primShape.ProfileShape);
+                hash = mdjb2(hash, primShape.PathBegin);
+                hash = mdjb2(hash, primShape.PathEnd);
+                hash = mdjb2(hash, primShape.PathScaleX);
+                hash = mdjb2(hash, primShape.PathScaleY);
+                hash = mdjb2(hash, primShape.PathShearX);
+                key.hashA = hash;
+                hash = key.hashB;
+                hash = mdjb2(hash, primShape.PathShearY);
+                hash = mdjb2(hash, (byte)primShape.PathTwist);
+                hash = mdjb2(hash, (byte)primShape.PathTwistBegin);
+                hash = mdjb2(hash, (byte)primShape.PathRadiusOffset);
+                hash = mdjb2(hash, (byte)primShape.PathTaperX);
+                hash = mdjb2(hash, (byte)primShape.PathTaperY);
+                hash = mdjb2(hash, primShape.PathRevolutions);
+                hash = mdjb2(hash, (byte)primShape.PathSkew);
+                hash = mdjb2(hash, primShape.ProfileBegin);
+                hash = mdjb2(hash, primShape.ProfileEnd);
+                hash = mdjb2(hash, primShape.ProfileHollow);
+                hash = mdjb2(hash, primShape.PCode);
+                key.hashB = hash;
+            }
+
+            hash = key.hashC;
+
+            hash = mdjb2(hash, lod);
+
+            if (size == m_MeshUnitSize)
+            {
+                hash = hash << 8;
+                hash |= 8;
+            }
+            else
+            {
+                someBytes = size.GetBytes();
+                for (int i = 0; i < someBytes.Length; i++)
+                    hash = mdjb2(hash, someBytes[i]);
+                hash = hash << 8;
+            }
+
+            if (convex)
+                hash |= 4;
+
+            if (primShape.SculptEntry)
+            {
+                hash |= 1;
+                if (primShape.SculptType == (byte)SculptType.Mesh)
+                    hash |= 2;
+            }
+
+            key.hashC = hash;
+
+            return key;
+        }
+
+        private ulong mdjb2(ulong hash, byte c)
+        {
+            return ((hash << 5) + hash) + (ulong)c;
+        }
+
+        private ulong mdjb2(ulong hash, ushort c)
+        {
+            hash = ((hash << 5) + hash) + (ulong)((byte)c);
+            return ((hash << 5) + hash) + (ulong)(c >> 8);
+        }
+
+        public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod)
+        {
+            return CreateMesh(primName, primShape, size, lod, false,false,false);
+        }
+
+        public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical)
+        {
+            return CreateMesh(primName, primShape, size, lod, false,false,false);
+        }
+
+        public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool shouldCache, bool convex, bool forOde)
+        {
+            return CreateMesh(primName, primShape, size, lod, false, false, false);
+        }
+
+        public IMesh GetMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool convex)
+        {
+            Mesh mesh = null;
+
+            if (size.X < 0.01f) size.X = 0.01f;
+            if (size.Y < 0.01f) size.Y = 0.01f;
+            if (size.Z < 0.01f) size.Z = 0.01f;
+
+            AMeshKey key = GetMeshUniqueKey(primShape, size, (byte)lod, convex);
+            lock (m_uniqueMeshes)
+            {
+                m_uniqueMeshes.TryGetValue(key, out mesh);
+
+                if (mesh != null)
+                {
+                    mesh.RefCount++;
+                    return mesh;
+                }
+
+                // try to find a identical mesh on meshs recently released
+                lock (m_uniqueReleasedMeshes)
+                {
+                    m_uniqueReleasedMeshes.TryGetValue(key, out mesh);
+                    if (mesh != null)
+                    {
+                        m_uniqueReleasedMeshes.Remove(key);
+                        try
+                        {
+                            m_uniqueMeshes.Add(key, mesh);
+                        }
+                        catch { }
+                        mesh.RefCount = 1;
+                        return mesh;
+                    }
+                }
+            }
+            return null;
+        }
+
+        private static Vector3 m_MeshUnitSize = new Vector3(1.0f, 1.0f, 1.0f);
+
+        public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool convex, bool forOde)
+        {
+#if SPAM
+            m_log.DebugFormat("[MESH]: Creating mesh for {0}", primName);
+#endif
+
+            Mesh mesh = null;
+
+            if (size.X < 0.01f) size.X = 0.01f;
+            if (size.Y < 0.01f) size.Y = 0.01f;
+            if (size.Z < 0.01f) size.Z = 0.01f;
+
+            // try to find a identical mesh on meshs in use
+
+            AMeshKey key = GetMeshUniqueKey(primShape,size,(byte)lod, convex);
+
+            lock (m_uniqueMeshes)
+            {
+                m_uniqueMeshes.TryGetValue(key, out mesh);
+
+                if (mesh != null)
+                {
+                    mesh.RefCount++;
+                    return mesh;
+                }
+
+                // try to find a identical mesh on meshs recently released
+                lock (m_uniqueReleasedMeshes)
+                {
+                    m_uniqueReleasedMeshes.TryGetValue(key, out mesh);
+                    if (mesh != null)
+                    {
+                        m_uniqueReleasedMeshes.Remove(key);
+                        try
+                        {
+                            m_uniqueMeshes.Add(key, mesh);
+                        }
+                        catch { }
+                        mesh.RefCount = 1;
+                        return mesh;
+                    }
+                }
+            }
+
+            Mesh UnitMesh = null;
+            AMeshKey unitKey = GetMeshUniqueKey(primShape, m_MeshUnitSize, (byte)lod, convex);
+
+            lock (m_uniqueReleasedMeshes)
+            {
+                m_uniqueReleasedMeshes.TryGetValue(unitKey, out UnitMesh);
+                if (UnitMesh != null)
+                {
+                    UnitMesh.RefCount = 1;
+                }
+            }
+
+            if (UnitMesh == null && primShape.SculptEntry && doMeshFileCache)
+                UnitMesh = GetFromFileCache(unitKey);
+
+            if (UnitMesh == null)
+            {
+                UnitMesh = CreateMeshFromPrimMesher(primName, primShape, lod, convex);
+
+                if (UnitMesh == null)
+                    return null;
+
+                UnitMesh.DumpRaw(baseDir, unitKey.ToString(), "Z");
+
+                if (forOde)
+                {
+                    // force pinned mem allocation
+                    UnitMesh.PrepForOde();
+                }
+                else
+                    UnitMesh.TrimExcess();
+
+                UnitMesh.Key = unitKey;
+                UnitMesh.RefCount = 1;
+
+                if (doMeshFileCache && primShape.SculptEntry)
+                    StoreToFileCache(unitKey, UnitMesh);
+
+                lock (m_uniqueReleasedMeshes)
+                {
+                    try
+                    {
+                        m_uniqueReleasedMeshes.Add(unitKey, UnitMesh);
+                    }
+                    catch { }
+                }
+            }
+
+            mesh = UnitMesh.Scale(size);
+            mesh.Key = key;
+            mesh.RefCount = 1;
+            lock (m_uniqueMeshes)
+            {
+                try
+                {
+                    m_uniqueMeshes.Add(key, mesh);
+                }
+                catch { }
+            }
+
+            return mesh;
+        }
+
+        public void ReleaseMesh(IMesh imesh)
+        {
+            if (imesh == null)
+                return;
+
+            Mesh mesh = (Mesh)imesh;
+
+            lock (m_uniqueMeshes)
+            {
+                int curRefCount = mesh.RefCount;
+                curRefCount--;
+
+                if (curRefCount > 0)
+                {
+                    mesh.RefCount = curRefCount;
+                    return;
+                }
+
+                mesh.RefCount = 0;
+                m_uniqueMeshes.Remove(mesh.Key);
+                lock (m_uniqueReleasedMeshes)
+                {
+                    try
+                    {
+                        m_uniqueReleasedMeshes.Add(mesh.Key, mesh);
+                    }
+                    catch { }
+                }
+            }
+        }
+
+        public void ExpireReleaseMeshs()
+        {
+            if (m_uniqueReleasedMeshes.Count == 0)
+                return;
+
+            List<Mesh> meshstodelete = new List<Mesh>();
+            int refcntr;
+
+            lock (m_uniqueReleasedMeshes)
+            {
+                foreach (Mesh m in m_uniqueReleasedMeshes.Values)
+                {
+                    refcntr = m.RefCount;
+                    refcntr--;
+                    if (refcntr > -6)
+                        m.RefCount = refcntr;
+                    else
+                        meshstodelete.Add(m);
+                }
+
+                foreach (Mesh m in meshstodelete)
+                {
+                    m_uniqueReleasedMeshes.Remove(m.Key);
+                    m.releaseBuildingMeshData();
+                    m.releasePinned();
+                }
+            }
+        }
+
+        public void FileNames(AMeshKey key, out string dir, out string fullFileName)
+        {
+            string id = key.ToString();
+            string init = id.Substring(0, 1);
+            dir = System.IO.Path.Combine(cachePath, init);
+            fullFileName = System.IO.Path.Combine(dir, id);
+        }
+
+        public string FullFileName(AMeshKey key)
+        {
+            string id = key.ToString();
+            string init = id.Substring(0,1);
+            id = System.IO.Path.Combine(init, id);
+            id = System.IO.Path.Combine(cachePath, id);
+            return id;
+        }
+
+        private Mesh GetFromFileCache(AMeshKey key)
+        {
+            Mesh mesh = null;
+            string filename = FullFileName(key);
+            bool ok = true;
+
+            lock (diskLock)
+            {
+                if (File.Exists(filename))
+                {
+                    try
+                    {
+                        using(FileStream stream = File.Open(filename, FileMode.Open, FileAccess.Read, FileShare.Read))
+                        {
+//                            BinaryFormatter bformatter = new BinaryFormatter();
+                            mesh = Mesh.FromStream(stream,key);
+                        }
+
+                    }
+                    catch (Exception e)
+                    {
+                        ok = false;
+                        m_log.ErrorFormat(
+                            "[MESH CACHE]: Failed to get file {0}.  Exception {1} {2}",
+                            filename, e.Message, e.StackTrace);
+                    }
+
+                    try
+                    {
+                        if (mesh == null || !ok)
+                            File.Delete(filename);
+                        else
+                            File.SetLastAccessTimeUtc(filename, DateTime.UtcNow);
+                    }
+                    catch
+                    {
+                    }
+
+                }
+            }
+
+            return mesh;
+        }
+
+        private void StoreToFileCache(AMeshKey key, Mesh mesh)
+        {
+            bool ok = false;
+
+            // Make sure the target cache directory exists
+            string dir = String.Empty;
+            string filename = String.Empty;
+
+            FileNames(key, out dir, out filename);
+
+            lock (diskLock)
+            {
+                Stream stream = null;
+                try
+                {
+                    if (!Directory.Exists(dir))
+                    {
+                        Directory.CreateDirectory(dir);
+                    }
+
+                    stream = File.Open(filename, FileMode.Create);
+                    ok = mesh.ToStream(stream);
+                }
+                catch (IOException e)
+                {
+                    m_log.ErrorFormat(
+                        "[MESH CACHE]: Failed to write file {0}.  Exception {1} {2}.",
+                        filename, e.Message, e.StackTrace);
+                    ok = false;
+                }
+                finally
+                {
+                    if(stream != null)
+                        stream.Dispose();
+                }
+
+                if (!ok && File.Exists(filename))
+                {
+                    try
+                    {
+                        File.Delete(filename);
+                    }
+                    catch (IOException e)
+                    {
+                         m_log.ErrorFormat(
+                        "[MESH CACHE]: Failed to delete file {0}",filename);
+                    }
+                }
+            }
+        }
+
+        public void ExpireFileCache()
+        {
+            if (!doCacheExpire)
+                return;
+
+            string controlfile = System.IO.Path.Combine(cachePath, cacheControlFilename);
+
+            lock (diskLock)
+            {
+                try
+                {
+                    if (File.Exists(controlfile))
+                    {
+                        int ndeleted = 0;
+                        int totalfiles = 0;
+                        int ndirs = 0;
+                        DateTime OlderTime = File.GetLastAccessTimeUtc(controlfile) - CacheExpire;
+                        File.SetLastAccessTimeUtc(controlfile, DateTime.UtcNow);
+
+                        foreach (string dir in Directory.GetDirectories(cachePath))
+                        {
+                            try
+                            {
+                                foreach (string file in Directory.GetFiles(dir))
+                                {
+                                    try
+                                    {
+                                        if (File.GetLastAccessTimeUtc(file) < OlderTime)
+                                        {
+                                            File.Delete(file);
+                                            ndeleted++;
+                                        }
+                                    }
+                                    catch { }
+                                    totalfiles++;
+                                }
+                            }
+                            catch { }
+                            ndirs++;
+                        }
+
+                        if (ndeleted == 0)
+                            m_log.InfoFormat("[MESH CACHE]: {0} Files in {1} cache folders, no expires",
+                                totalfiles,ndirs);
+                        else
+                            m_log.InfoFormat("[MESH CACHE]: {0} Files in {1} cache folders, expired {2} files accessed before {3}",
+                                totalfiles,ndirs, ndeleted, OlderTime.ToString());
+                    }
+                    else
+                    {
+                        m_log.Info("[MESH CACHE]: Expire delayed to next startup");
+                        FileStream fs = File.Create(controlfile,4096,FileOptions.WriteThrough);
+                        fs.Close();
+                    }
+                }
+                catch { }
+            }
+        }
+
+        public bool checkCache()
+        {
+            string controlfile = System.IO.Path.Combine(cachePath, cacheControlFilename);
+            lock (diskLock)
+            {
+                try
+                {
+                    if (!Directory.Exists(cachePath))
+                    {
+                        Directory.CreateDirectory(cachePath);
+                        Thread.Sleep(100);
+                        FileStream fs = File.Create(controlfile, 4096, FileOptions.WriteThrough);
+                        fs.Close();
+                        return true;
+                    }
+                }
+                catch
+                {
+                    doMeshFileCache = false;
+                    doCacheExpire = false;
+                    return false;
+                }
+                finally {}
+
+                if (File.Exists(controlfile))
+                    return true;
+
+                try
+                {
+                    Directory.Delete(cachePath, true);
+                    while(Directory.Exists(cachePath))
+                        Thread.Sleep(100);
+                }
+                catch(Exception e)
+                {
+                    m_log.Error("[MESH CACHE]: failed to delete old version of the cache: " + e.Message);
+                    doMeshFileCache = false;
+                    doCacheExpire = false;
+                    return false;
+                } 
+                finally {}
+                try
+                {
+                    Directory.CreateDirectory(cachePath);
+                    while(!Directory.Exists(cachePath))
+                        Thread.Sleep(100);
+                }
+                catch(Exception e)
+                {
+                    m_log.Error("[MESH CACHE]: failed to create new cache folder: " + e.Message);
+                    doMeshFileCache = false;
+                    doCacheExpire = false;
+                    return false;
+                } 
+                finally {}
+
+                try
+                {
+                    FileStream fs = File.Create(controlfile, 4096, FileOptions.WriteThrough);
+                    fs.Close();
+                }
+                catch(Exception e)
+                {
+                    m_log.Error("[MESH CACHE]: failed to create new control file: " + e.Message);
+                    doMeshFileCache = false;
+                    doCacheExpire = false;
+                    return false;
+                } 
+                finally {}
+            
+                return true;
+            }
+        }
+
+        public bool CreateBoundingHull(List<Coord> inputVertices, out List<Coord> convexcoords, out List<Face> newfaces)
+        {
+            convexcoords = null;
+            newfaces = null;
+            HullDesc desc = new HullDesc();
+            HullResult result = new HullResult();
+
+            int nInputVerts = inputVertices.Count;
+            int i;
+
+            List<float3> vs = new List<float3>(nInputVerts);
+            float3 f3;
+
+            //useless copy
+            for(i = 0 ; i < nInputVerts; i++)
+            {
+                f3 = new float3(inputVertices[i].X, inputVertices[i].Y, inputVertices[i].Z);
+                vs.Add(f3);
+            }
+
+            desc.Vertices = vs;
+            desc.Flags = HullFlag.QF_TRIANGLES;
+            desc.MaxVertices = 256;
+
+            try
+            {
+                HullError ret = HullUtils.CreateConvexHull(desc, ref result);
+                if (ret != HullError.QE_OK)
+                    return false;
+                int nverts = result.OutputVertices.Count;
+                int nindx = result.Indices.Count;
+                if(nverts < 3 || nindx< 3)
+                    return false;
+                if(nindx % 3 != 0)
+                    return false;
+
+                convexcoords = new List<Coord>(nverts);
+                Coord c;
+                vs = result.OutputVertices;
+
+                for(i = 0 ; i < nverts; i++)
+                {
+                    c = new Coord(vs[i].x, vs[i].y, vs[i].z);
+                    convexcoords.Add(c);
+                }
+
+                newfaces = new List<Face>(nindx / 3);
+                List<int> indxs = result.Indices;
+                int k, l, m;
+                Face f;
+                for(i = 0 ; i < nindx;)
+                {
+                    k = indxs[i++];
+                    l = indxs[i++];
+                    m = indxs[i++];
+                    if(k > nInputVerts)
+                        continue;
+                    if(l > nInputVerts)
+                        continue;
+                    if(m > nInputVerts)
+                        continue;
+                    f = new Face(k,l,m);
+                    newfaces.Add(f);
+                }
+                return true;
+            }
+            catch
+            {
+
+                return false;
+            }
+            return false;
+        }
+    }
+}
diff --git a/OpenSim/Region/PhysicsModules/ubOdeMeshing/PrimMesher.cs b/OpenSim/Region/PhysicsModules/ubOdeMeshing/PrimMesher.cs
new file mode 100644
index 0000000..e93175f
--- /dev/null
+++ b/OpenSim/Region/PhysicsModules/ubOdeMeshing/PrimMesher.cs
@@ -0,0 +1,1707 @@
+/*
+ * Copyright (c) Contributors
+ * See CONTRIBUTORS.TXT for a full list of copyright holders.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the OpenSimulator Project nor the
+ *       names of its contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+using System;
+using System.Collections.Generic;
+using System.Text;
+using System.IO;
+
+namespace PrimMesher
+{
+    public struct Quat
+    {
+        /// <summary>X value</summary>
+        public float X;
+        /// <summary>Y value</summary>
+        public float Y;
+        /// <summary>Z value</summary>
+        public float Z;
+        /// <summary>W value</summary>
+        public float W;
+
+        public Quat(float x, float y, float z, float w)
+        {
+            X = x;
+            Y = y;
+            Z = z;
+            W = w;
+        }
+
+        public Quat(Coord axis, float angle)
+        {
+            axis = axis.Normalize();
+
+            angle *= 0.5f;
+            float c = (float)Math.Cos(angle);
+            float s = (float)Math.Sin(angle);
+
+            X = axis.X * s;
+            Y = axis.Y * s;
+            Z = axis.Z * s;
+            W = c;
+
+            Normalize();
+        }
+
+        public float Length()
+        {
+            return (float)Math.Sqrt(X * X + Y * Y + Z * Z + W * W);
+        }
+
+        public Quat Normalize()
+        {
+            const float MAG_THRESHOLD = 0.0000001f;
+            float mag = Length();
+
+            // Catch very small rounding errors when normalizing
+            if (mag > MAG_THRESHOLD)
+            {
+                float oomag = 1f / mag;
+                X *= oomag;
+                Y *= oomag;
+                Z *= oomag;
+                W *= oomag;
+            }
+            else
+            {
+                X = 0f;
+                Y = 0f;
+                Z = 0f;
+                W = 1f;
+            }
+
+            return this;
+        }
+
+        public static Quat operator *(Quat q1, Quat q2)
+        {
+            float x = q1.W * q2.X + q1.X * q2.W + q1.Y * q2.Z - q1.Z * q2.Y;
+            float y = q1.W * q2.Y - q1.X * q2.Z + q1.Y * q2.W + q1.Z * q2.X;
+            float z = q1.W * q2.Z + q1.X * q2.Y - q1.Y * q2.X + q1.Z * q2.W;
+            float w = q1.W * q2.W - q1.X * q2.X - q1.Y * q2.Y - q1.Z * q2.Z;
+            return new Quat(x, y, z, w);
+        }
+
+        public override string ToString()
+        {
+            return "< X: " + this.X.ToString() + ", Y: " + this.Y.ToString() + ", Z: " + this.Z.ToString() + ", W: " + this.W.ToString() + ">";
+        }
+    }
+
+    public struct Coord
+    {
+        public float X;
+        public float Y;
+        public float Z;
+
+        public Coord(float x, float y, float z)
+        {
+            this.X = x;
+            this.Y = y;
+            this.Z = z;
+        }
+
+        public float Length()
+        {
+            return (float)Math.Sqrt(this.X * this.X + this.Y * this.Y + this.Z * this.Z);
+        }
+
+        public Coord Invert()
+        {
+            this.X = -this.X;
+            this.Y = -this.Y;
+            this.Z = -this.Z;
+
+            return this;
+        }
+
+        public Coord Normalize()
+        {
+            const float MAG_THRESHOLD = 0.0000001f;
+            float mag = Length();
+
+            // Catch very small rounding errors when normalizing
+            if (mag > MAG_THRESHOLD)
+            {
+                float oomag = 1.0f / mag;
+                this.X *= oomag;
+                this.Y *= oomag;
+                this.Z *= oomag;
+            }
+            else
+            {
+                this.X = 0.0f;
+                this.Y = 0.0f;
+                this.Z = 0.0f;
+            }
+
+            return this;
+        }
+
+        public override string ToString()
+        {
+            return this.X.ToString() + " " + this.Y.ToString() + " " + this.Z.ToString();
+        }
+
+        public static Coord Cross(Coord c1, Coord c2)
+        {
+            return new Coord(
+                c1.Y * c2.Z - c2.Y * c1.Z,
+                c1.Z * c2.X - c2.Z * c1.X,
+                c1.X * c2.Y - c2.X * c1.Y
+                );
+        }
+
+        public static Coord operator +(Coord v, Coord a)
+        {
+            return new Coord(v.X + a.X, v.Y + a.Y, v.Z + a.Z);
+        }
+
+        public static Coord operator *(Coord v, Coord m)
+        {
+            return new Coord(v.X * m.X, v.Y * m.Y, v.Z * m.Z);
+        }
+
+        public static Coord operator *(Coord v, Quat q)
+        {
+            // From http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/
+
+            Coord c2 = new Coord(0.0f, 0.0f, 0.0f);
+
+            c2.X = q.W * q.W * v.X +
+                2f * q.Y * q.W * v.Z -
+                2f * q.Z * q.W * v.Y +
+                     q.X * q.X * v.X +
+                2f * q.Y * q.X * v.Y +
+                2f * q.Z * q.X * v.Z -
+                     q.Z * q.Z * v.X -
+                     q.Y * q.Y * v.X;
+
+            c2.Y =
+                2f * q.X * q.Y * v.X +
+                     q.Y * q.Y * v.Y +
+                2f * q.Z * q.Y * v.Z +
+                2f * q.W * q.Z * v.X -
+                     q.Z * q.Z * v.Y +
+                     q.W * q.W * v.Y -
+                2f * q.X * q.W * v.Z -
+                     q.X * q.X * v.Y;
+
+            c2.Z =
+                2f * q.X * q.Z * v.X +
+                2f * q.Y * q.Z * v.Y +
+                     q.Z * q.Z * v.Z -
+                2f * q.W * q.Y * v.X -
+                     q.Y * q.Y * v.Z +
+                2f * q.W * q.X * v.Y -
+                     q.X * q.X * v.Z +
+                     q.W * q.W * v.Z;
+
+            return c2;
+        }
+    }
+
+    public struct Face
+    {
+        public int primFace;
+
+        // vertices
+        public int v1;
+        public int v2;
+        public int v3;
+
+        public Face(int v1, int v2, int v3)
+        {
+            primFace = 0;
+
+            this.v1 = v1;
+            this.v2 = v2;
+            this.v3 = v3;
+        }
+
+        public Coord SurfaceNormal(List<Coord> coordList)
+        {
+            Coord c1 = coordList[this.v1];
+            Coord c2 = coordList[this.v2];
+            Coord c3 = coordList[this.v3];
+
+            Coord edge1 = new Coord(c2.X - c1.X, c2.Y - c1.Y, c2.Z - c1.Z);
+            Coord edge2 = new Coord(c3.X - c1.X, c3.Y - c1.Y, c3.Z - c1.Z);
+
+            return Coord.Cross(edge1, edge2).Normalize();
+        }
+    }
+
+    internal struct Angle
+    {
+        internal float angle;
+        internal float X;
+        internal float Y;
+
+        internal Angle(float angle, float x, float y)
+        {
+            this.angle = angle;
+            this.X = x;
+            this.Y = y;
+        }
+    }
+
+    internal class AngleList
+    {
+        private float iX, iY; // intersection point
+
+        private static Angle[] angles3 =
+        {
+            new Angle(0.0f, 1.0f, 0.0f),
+            new Angle(0.33333333333333333f, -0.5f, 0.86602540378443871f),
+            new Angle(0.66666666666666667f, -0.5f, -0.86602540378443837f),
+            new Angle(1.0f, 1.0f, 0.0f)
+        };
+
+        private static Angle[] angles4 =
+        {
+            new Angle(0.0f, 1.0f, 0.0f),
+            new Angle(0.25f, 0.0f, 1.0f),
+            new Angle(0.5f, -1.0f, 0.0f),
+            new Angle(0.75f, 0.0f, -1.0f),
+            new Angle(1.0f, 1.0f, 0.0f)
+        };
+
+        private static Angle[] angles6 =
+        {
+            new Angle(0.0f, 1.0f, 0.0f),
+            new Angle(0.16666666666666667f, 0.5f, 0.8660254037844386f),
+            new Angle(0.33333333333333333f, -0.5f, 0.86602540378443871f),
+            new Angle(0.5f, -1.0f, 0.0f),
+            new Angle(0.66666666666666667f, -0.5f, -0.86602540378443837f),
+            new Angle(0.83333333333333326f, 0.5f, -0.86602540378443904f),
+            new Angle(1.0f, 1.0f, 0.0f)
+        };
+
+        private static Angle[] angles12 =
+        {
+            new Angle(0.0f, 1.0f, 0.0f),
+            new Angle(0.083333333333333329f, 0.86602540378443871f, 0.5f),
+            new Angle(0.16666666666666667f, 0.5f, 0.8660254037844386f),
+            new Angle(0.25f, 0.0f, 1.0f),
+            new Angle(0.33333333333333333f, -0.5f, 0.86602540378443871f),
+            new Angle(0.41666666666666663f, -0.86602540378443849f, 0.5f),
+            new Angle(0.5f, -1.0f, 0.0f),
+            new Angle(0.58333333333333326f, -0.86602540378443882f, -0.5f),
+            new Angle(0.66666666666666667f, -0.5f, -0.86602540378443837f),
+            new Angle(0.75f, 0.0f, -1.0f),
+            new Angle(0.83333333333333326f, 0.5f, -0.86602540378443904f),
+            new Angle(0.91666666666666663f, 0.86602540378443837f, -0.5f),
+            new Angle(1.0f, 1.0f, 0.0f)
+        };
+
+        private static Angle[] angles24 =
+        {
+            new Angle(0.0f, 1.0f, 0.0f),
+            new Angle(0.041666666666666664f, 0.96592582628906831f, 0.25881904510252074f),
+            new Angle(0.083333333333333329f, 0.86602540378443871f, 0.5f),
+            new Angle(0.125f, 0.70710678118654757f, 0.70710678118654746f),
+            new Angle(0.16666666666666667f, 0.5f, 0.8660254037844386f),
+            new Angle(0.20833333333333331f, 0.25881904510252096f, 0.9659258262890682f),
+            new Angle(0.25f, 0.0f, 1.0f),
+            new Angle(0.29166666666666663f, -0.25881904510252063f, 0.96592582628906831f),
+            new Angle(0.33333333333333333f, -0.5f, 0.86602540378443871f),
+            new Angle(0.375f, -0.70710678118654746f, 0.70710678118654757f),
+            new Angle(0.41666666666666663f, -0.86602540378443849f, 0.5f),
+            new Angle(0.45833333333333331f, -0.9659258262890682f, 0.25881904510252102f),
+            new Angle(0.5f, -1.0f, 0.0f),
+            new Angle(0.54166666666666663f, -0.96592582628906842f, -0.25881904510252035f),
+            new Angle(0.58333333333333326f, -0.86602540378443882f, -0.5f),
+            new Angle(0.62499999999999989f, -0.70710678118654791f, -0.70710678118654713f),
+            new Angle(0.66666666666666667f, -0.5f, -0.86602540378443837f),
+            new Angle(0.70833333333333326f, -0.25881904510252152f, -0.96592582628906809f),
+            new Angle(0.75f, 0.0f, -1.0f),
+            new Angle(0.79166666666666663f, 0.2588190451025203f, -0.96592582628906842f),
+            new Angle(0.83333333333333326f, 0.5f, -0.86602540378443904f),
+            new Angle(0.875f, 0.70710678118654735f, -0.70710678118654768f),
+            new Angle(0.91666666666666663f, 0.86602540378443837f, -0.5f),
+            new Angle(0.95833333333333326f, 0.96592582628906809f, -0.25881904510252157f),
+            new Angle(1.0f, 1.0f, 0.0f)
+        };
+
+        private Angle interpolatePoints(float newPoint, Angle p1, Angle p2)
+        {
+            float m = (newPoint - p1.angle) / (p2.angle - p1.angle);
+            return new Angle(newPoint, p1.X + m * (p2.X - p1.X), p1.Y + m * (p2.Y - p1.Y));
+        }
+
+        private void intersection(double x1, double y1, double x2, double y2, double x3, double y3, double x4, double y4)
+        { // ref: http://local.wasp.uwa.edu.au/~pbourke/geometry/lineline2d/
+            double denom = (y4 - y3) * (x2 - x1) - (x4 - x3) * (y2 - y1);
+            double uaNumerator = (x4 - x3) * (y1 - y3) - (y4 - y3) * (x1 - x3);
+
+            if (denom != 0.0)
+            {
+                double ua = uaNumerator / denom;
+                iX = (float)(x1 + ua * (x2 - x1));
+                iY = (float)(y1 + ua * (y2 - y1));
+            }
+        }
+
+        internal List<Angle> angles;
+
+        internal void makeAngles(int sides, float startAngle, float stopAngle, bool hasCut)
+        {
+            angles = new List<Angle>();
+
+            const double twoPi = System.Math.PI * 2.0;
+            const float twoPiInv = (float)(1.0d / twoPi);
+
+            if (sides < 1)
+                throw new Exception("number of sides not greater than zero");
+            if (stopAngle <= startAngle)
+                throw new Exception("stopAngle not greater than startAngle");
+
+            if ((sides == 3 || sides == 4 || sides == 6 || sides == 12 || sides == 24))
+            {
+                startAngle *= twoPiInv;
+                stopAngle *= twoPiInv;
+
+                Angle[] sourceAngles;
+                switch (sides)
+                {
+                    case 3:
+                        sourceAngles = angles3;
+                        break;
+                    case 4:
+                        sourceAngles = angles4;
+                        break;
+                    case 6:
+                        sourceAngles = angles6;
+                        break;
+                    case 12:
+                        sourceAngles = angles12;
+                        break;
+                    default:
+                        sourceAngles = angles24;
+                        break;
+                }
+
+                int startAngleIndex = (int)(startAngle * sides);
+                int endAngleIndex = sourceAngles.Length - 1;
+
+                if (hasCut)
+                {
+                    if (stopAngle < 1.0f)
+                        endAngleIndex = (int)(stopAngle * sides) + 1;
+                    if (endAngleIndex == startAngleIndex)
+                        endAngleIndex++;
+
+                    for (int angleIndex = startAngleIndex; angleIndex < endAngleIndex + 1; angleIndex++)
+                    {
+                        angles.Add(sourceAngles[angleIndex]);
+                    }
+
+                    if (startAngle > 0.0f)
+                        angles[0] = interpolatePoints(startAngle, angles[0], angles[1]);
+
+                    if (stopAngle < 1.0f)
+                    {
+                        int lastAngleIndex = angles.Count - 1;
+                        angles[lastAngleIndex] = interpolatePoints(stopAngle, angles[lastAngleIndex - 1], angles[lastAngleIndex]);
+                    }
+                }
+                else
+                {
+                    for (int angleIndex = startAngleIndex; angleIndex < endAngleIndex; angleIndex++)
+                        angles.Add(sourceAngles[angleIndex]);
+                }
+            }
+            else
+            {
+                double stepSize = twoPi / sides;
+
+                int startStep = (int)(startAngle / stepSize);
+                double angle = stepSize * startStep;
+                int step = startStep;
+                double stopAngleTest = stopAngle;
+                if (stopAngle < twoPi)
+                {
+                    stopAngleTest = stepSize * ((int)(stopAngle / stepSize) + 1);
+                    if (stopAngleTest < stopAngle)
+                        stopAngleTest += stepSize;
+                    if (stopAngleTest > twoPi)
+                        stopAngleTest = twoPi;
+                }
+
+                while (angle <= stopAngleTest)
+                {
+                    Angle newAngle;
+                    newAngle.angle = (float)angle;
+                    newAngle.X = (float)System.Math.Cos(angle);
+                    newAngle.Y = (float)System.Math.Sin(angle);
+                    angles.Add(newAngle);
+                    step += 1;
+                    angle = stepSize * step;
+                }
+
+                if (startAngle > angles[0].angle)
+                {
+                    Angle newAngle;
+                    intersection(angles[0].X, angles[0].Y, angles[1].X, angles[1].Y, 0.0f, 0.0f, (float)Math.Cos(startAngle), (float)Math.Sin(startAngle));
+                    newAngle.angle = startAngle;
+                    newAngle.X = iX;
+                    newAngle.Y = iY;
+                    angles[0] = newAngle;
+                }
+
+                int index = angles.Count - 1;
+                if (stopAngle < angles[index].angle)
+                {
+                    Angle newAngle;
+                    intersection(angles[index - 1].X, angles[index - 1].Y, angles[index].X, angles[index].Y, 0.0f, 0.0f, (float)Math.Cos(stopAngle), (float)Math.Sin(stopAngle));
+                    newAngle.angle = stopAngle;
+                    newAngle.X = iX;
+                    newAngle.Y = iY;
+                    angles[index] = newAngle;
+                }
+            }
+        }
+    }
+
+    /// <summary>
+    /// generates a profile for extrusion
+    /// </summary>
+    public class Profile
+    {
+        private const float twoPi = 2.0f * (float)Math.PI;
+
+        public string errorMessage = null;
+
+        public List<Coord> coords;
+        public List<Face> faces;
+
+        // use these for making individual meshes for each prim face
+        public List<int> outerCoordIndices = null;
+        public List<int> hollowCoordIndices = null;
+
+        public int numOuterVerts = 0;
+        public int numHollowVerts = 0;
+
+        public int outerFaceNumber = -1;
+        public int hollowFaceNumber = -1;
+
+        public int bottomFaceNumber = 0;
+        public int numPrimFaces = 0;
+
+        public Profile()
+        {
+            coords = new List<Coord>();
+            faces = new List<Face>();
+        }
+
+        public Profile(int sides, float profileStart, float profileEnd, float hollow, int hollowSides, bool hasProfileCut, bool createFaces)
+        {
+            const float halfSqr2 = 0.7071067811866f;
+
+            coords = new List<Coord>();
+            faces = new List<Face>();
+
+            List<Coord> hollowCoords = new List<Coord>();
+
+            bool hasHollow = (hollow > 0.0f);
+
+            AngleList angles = new AngleList();
+            AngleList hollowAngles = new AngleList();
+
+            float xScale = 0.5f;
+            float yScale = 0.5f;
+            if (sides == 4)  // corners of a square are sqrt(2) from center
+            {
+                xScale = halfSqr2;
+                yScale = halfSqr2;
+            }
+
+            float startAngle = profileStart * twoPi;
+            float stopAngle = profileEnd * twoPi;
+
+            try { angles.makeAngles(sides, startAngle, stopAngle,hasProfileCut); }
+            catch (Exception ex)
+            {
+
+                errorMessage = "makeAngles failed: Exception: " + ex.ToString()
+                + "\nsides: " + sides.ToString() + " startAngle: " + startAngle.ToString() + " stopAngle: " + stopAngle.ToString();
+
+                return;
+            }
+
+            numOuterVerts = angles.angles.Count;
+
+            Angle angle;
+            Coord newVert = new Coord();
+
+            // flag to create as few triangles as possible for 3 or 4 side profile
+            bool simpleFace = (sides < 5 && !hasHollow && !hasProfileCut);
+
+            if (hasHollow)
+            {
+                if (sides == hollowSides)
+                    hollowAngles = angles;
+                else
+                {
+                    try { hollowAngles.makeAngles(hollowSides, startAngle, stopAngle, hasProfileCut); }
+                    catch (Exception ex)
+                    {
+                        errorMessage = "makeAngles failed: Exception: " + ex.ToString()
+                        + "\nsides: " + sides.ToString() + " startAngle: " + startAngle.ToString() + " stopAngle: " + stopAngle.ToString();
+
+                        return;
+                    }
+
+                    int numHollowAngles = hollowAngles.angles.Count;
+                    for (int i = 0; i < numHollowAngles; i++)
+                    {
+                        angle = hollowAngles.angles[i];
+                        newVert.X = hollow * xScale * angle.X;
+                        newVert.Y = hollow * yScale * angle.Y;
+                        newVert.Z = 0.0f;
+
+                        hollowCoords.Add(newVert);
+                    }
+                }
+                numHollowVerts = hollowAngles.angles.Count;
+            }
+            else if (!simpleFace)
+            {
+                Coord center = new Coord(0.0f, 0.0f, 0.0f);
+                this.coords.Add(center);
+            }
+
+            int numAngles = angles.angles.Count;
+            bool hollowsame = (hasHollow && hollowSides == sides);
+
+            for (int i = 0; i < numAngles; i++)
+            {
+                angle = angles.angles[i];
+                newVert.X = angle.X * xScale;
+                newVert.Y = angle.Y * yScale;
+                newVert.Z = 0.0f;
+                coords.Add(newVert);
+                if (hollowsame)
+                {
+                    newVert.X *= hollow;
+                    newVert.Y *= hollow;
+                    hollowCoords.Add(newVert);
+                }
+            }
+
+            if (hasHollow)
+            {
+                hollowCoords.Reverse();
+                coords.AddRange(hollowCoords);
+
+                if (createFaces)
+                {
+                    int numTotalVerts = numOuterVerts + numHollowVerts;
+
+                    if (numOuterVerts == numHollowVerts)
+                    {
+                        Face newFace = new Face();
+
+                        for (int coordIndex = 0; coordIndex < numOuterVerts - 1; coordIndex++)
+                        {
+                            newFace.v1 = coordIndex;
+                            newFace.v2 = coordIndex + 1;
+                            newFace.v3 = numTotalVerts - coordIndex - 1;
+                            faces.Add(newFace);
+
+                            newFace.v1 = coordIndex + 1;
+                            newFace.v2 = numTotalVerts - coordIndex - 2;
+                            newFace.v3 = numTotalVerts - coordIndex - 1;
+                            faces.Add(newFace);
+                        }
+                        if (!hasProfileCut)
+                        {
+                            newFace.v1 = numOuterVerts - 1;
+                            newFace.v2 = 0;
+                            newFace.v3 = numOuterVerts;
+                            faces.Add(newFace);
+
+                            newFace.v1 = 0;
+                            newFace.v2 = numTotalVerts - 1;
+                            newFace.v3 = numOuterVerts;
+                            faces.Add(newFace);
+                        }
+                    }
+                    else if (numOuterVerts < numHollowVerts)
+                    {
+                        Face newFace = new Face();
+                        int j = 0; // j is the index for outer vertices
+                        int i;
+                        int maxJ = numOuterVerts - 1;
+                        float curHollowAngle = 0;
+                        for (i = 0; i < numHollowVerts; i++) // i is the index for inner vertices
+                        {
+                            curHollowAngle = hollowAngles.angles[i].angle;
+                            if (j < maxJ)
+                            {
+                                if (angles.angles[j + 1].angle - curHollowAngle < curHollowAngle - angles.angles[j].angle + 0.000001f)
+                                {
+                                    newFace.v1 = numTotalVerts - i - 1;
+                                    newFace.v2 = j;
+                                    newFace.v3 = j + 1;
+                                    faces.Add(newFace);
+                                    j++;
+                                }
+                            }
+                            else
+                            {
+                                if (1.0f - curHollowAngle < curHollowAngle - angles.angles[j].angle + 0.000001f)
+                                    break;
+                            }
+
+                            newFace.v1 = j;
+                            newFace.v2 = numTotalVerts - i - 2;
+                            newFace.v3 = numTotalVerts - i - 1;
+
+                            faces.Add(newFace);
+                        }
+
+                        if (!hasProfileCut)
+                        {
+                            if (i == numHollowVerts)
+                            {
+                                newFace.v1 = numTotalVerts - numHollowVerts;
+                                newFace.v2 = maxJ;
+                                newFace.v3 = 0;
+
+                                faces.Add(newFace);
+                            }
+                            else
+                            {
+                                if (1.0f - curHollowAngle < curHollowAngle - angles.angles[maxJ].angle + 0.000001f)
+                                {
+                                    newFace.v1 = numTotalVerts - i - 1;
+                                    newFace.v2 = maxJ;
+                                    newFace.v3 = 0;
+
+                                    faces.Add(newFace);
+                                }
+
+                                for (; i < numHollowVerts - 1; i++)
+                                {
+                                    newFace.v1 = 0;
+                                    newFace.v2 = numTotalVerts - i - 2;
+                                    newFace.v3 = numTotalVerts - i - 1;
+
+                                    faces.Add(newFace);
+                                }
+                            }
+
+                            newFace.v1 = 0;
+                            newFace.v2 = numTotalVerts - 1;
+                            newFace.v3 = numTotalVerts - numHollowVerts;
+                            faces.Add(newFace);
+                        }
+                    }
+                    else // numHollowVerts < numOuterVerts
+                    {
+                        Face newFace = new Face();
+                        int j = 0; // j is the index for inner vertices
+                        int maxJ = numHollowVerts - 1;
+                        for (int i = 0; i < numOuterVerts; i++)
+                        {
+                            if (j < maxJ)
+                                if (hollowAngles.angles[j + 1].angle - angles.angles[i].angle < angles.angles[i].angle - hollowAngles.angles[j].angle + 0.000001f)
+                                {
+                                    newFace.v1 = i;
+                                    newFace.v2 = numTotalVerts - j - 2;
+                                    newFace.v3 = numTotalVerts - j - 1;
+
+                                    faces.Add(newFace);
+                                    j += 1;
+                                }
+
+                            newFace.v1 = numTotalVerts - j - 1;
+                            newFace.v2 = i;
+                            newFace.v3 = i + 1;
+
+                            faces.Add(newFace);
+                        }
+
+                        if (!hasProfileCut)
+                        {
+                            int i = numOuterVerts - 1;
+
+                            if (hollowAngles.angles[0].angle - angles.angles[i].angle < angles.angles[i].angle - hollowAngles.angles[maxJ].angle + 0.000001f)
+                            {
+                                newFace.v1 = 0;
+                                newFace.v2 = numTotalVerts - 1;
+                                newFace.v3 = numTotalVerts - maxJ - 1;
+
+                                faces.Add(newFace);
+                            }
+
+                            newFace.v1 = numTotalVerts - maxJ - 1;
+                            newFace.v2 = i;
+                            newFace.v3 = 0;
+
+                            faces.Add(newFace);
+                        }
+                    }
+                }
+
+            }
+
+            else if (createFaces)
+            {
+                if (simpleFace)
+                {
+                    if (sides == 3)
+                        faces.Add(new Face(0, 1, 2));
+                    else if (sides == 4)
+                    {
+                        faces.Add(new Face(0, 1, 2));
+                        faces.Add(new Face(0, 2, 3));
+                    }
+                }
+                else
+                {
+                    for (int i = 1; i < numAngles ; i++)
+                    {
+                        Face newFace = new Face();
+                        newFace.v1 = 0;
+                        newFace.v2 = i;
+                        newFace.v3 = i + 1;
+                        faces.Add(newFace);
+                    }
+                    if (!hasProfileCut)
+                    {
+                        Face newFace = new Face();
+                        newFace.v1 = 0;
+                        newFace.v2 = numAngles;
+                        newFace.v3 = 1;
+                        faces.Add(newFace);
+                    }
+                }
+            }
+
+
+            hollowCoords = null;
+        }
+
+
+        public Profile Copy()
+        {
+            return Copy(true);
+        }
+
+        public Profile Copy(bool needFaces)
+        {
+            Profile copy = new Profile();
+
+            copy.coords.AddRange(coords);
+
+            if (needFaces)
+                copy.faces.AddRange(faces);
+
+            copy.numOuterVerts = numOuterVerts;
+            copy.numHollowVerts = numHollowVerts;
+
+            return copy;
+        }
+
+        public void AddPos(Coord v)
+        {
+            this.AddPos(v.X, v.Y, v.Z);
+        }
+
+        public void AddPos(float x, float y, float z)
+        {
+            int i;
+            int numVerts = coords.Count;
+            Coord vert;
+
+            for (i = 0; i < numVerts; i++)
+            {
+                vert = coords[i];
+                vert.X += x;
+                vert.Y += y;
+                vert.Z += z;
+                this.coords[i] = vert;
+            }
+        }
+
+        public void AddRot(Quat q)
+        {
+            int i;
+            int numVerts = coords.Count;
+
+            for (i = 0; i < numVerts; i++)
+                coords[i] *= q;
+        }
+
+        public void Scale(float x, float y)
+        {
+            int i;
+            int numVerts = coords.Count;
+            Coord vert;
+
+            for (i = 0; i < numVerts; i++)
+            {
+                vert = coords[i];
+                vert.X *= x;
+                vert.X = (float)Math.Round(vert.X,5);
+                vert.Y *= y;
+                vert.Y = (float)Math.Round(vert.Y,5);
+                coords[i] = vert;
+            }
+
+            if(x == 0f || y == 0f)
+                faces = new List<Face>();
+        }
+
+        /// <summary>
+        /// Changes order of the vertex indices and negates the center vertex normal. Does not alter vertex normals of radial vertices
+        /// </summary>
+        public void FlipNormals()
+        {
+            int numFaces = faces.Count;
+            if(numFaces == 0)
+                return;
+
+            int i;
+            Face tmpFace;
+            int tmp;
+
+            for (i = 0; i < numFaces; i++)
+            {
+                tmpFace = faces[i];
+                tmp = tmpFace.v3;
+                tmpFace.v3 = tmpFace.v1;
+                tmpFace.v1 = tmp;
+                faces[i] = tmpFace;
+            }
+        }
+
+        public void AddValue2FaceVertexIndices(int num)
+        {
+            int numFaces = faces.Count;
+            if(numFaces == 0)
+                return;
+
+            Face tmpFace;
+
+            for (int i = 0; i < numFaces; i++)
+            {
+                tmpFace = faces[i];
+                tmpFace.v1 += num;
+                tmpFace.v2 += num;
+                tmpFace.v3 += num;
+
+                faces[i] = tmpFace;
+            }
+        }
+
+         public void DumpRaw(String path, String name, String title)
+        {
+            if (path == null)
+                return;
+            String fileName = name + "_" + title + ".raw";
+            String completePath = System.IO.Path.Combine(path, fileName);
+            StreamWriter sw = new StreamWriter(completePath);
+
+            for (int i = 0; i < faces.Count; i++)
+            {
+                string s = coords[faces[i].v1].ToString();
+                s += " " + coords[faces[i].v2].ToString();
+                s += " " + coords[faces[i].v3].ToString();
+
+                sw.WriteLine(s);
+            }
+
+            sw.Close();
+        }
+    }
+
+    public struct PathNode
+    {
+        public Coord position;
+        public Quat rotation;
+        public float xScale;
+        public float yScale;
+        public float percentOfPath;
+    }
+
+    public enum PathType { Linear = 0, Circular = 1, Flexible = 2 }
+
+    public class Path
+    {
+        public List<PathNode> pathNodes = new List<PathNode>();
+
+        public float twistBegin = 0.0f;
+        public float twistEnd = 0.0f;
+        public float topShearX = 0.0f;
+        public float topShearY = 0.0f;
+        public float pathCutBegin = 0.0f;
+        public float pathCutEnd = 1.0f;
+        public float dimpleBegin = 0.0f;
+        public float dimpleEnd = 1.0f;
+        public float skew = 0.0f;
+        public float holeSizeX = 1.0f; // called pathScaleX in pbs
+        public float holeSizeY = 0.25f;
+        public float taperX = 0.0f;
+        public float taperY = 0.0f;
+        public float radius = 0.0f;
+        public float revolutions = 1.0f;
+        public int stepsPerRevolution = 24;
+
+        private const float twoPi = 2.0f * (float)Math.PI;
+
+        public void Create(PathType pathType, int steps)
+        {
+            if (taperX > .9999f)
+                taperX = 1.0f;
+            else if (taperX < -.9999f)
+                taperX = -1.0f;
+            if (taperY > .9999f)
+                taperY = 1.0f;
+            else if (taperY < -.9999f)
+                taperY = -1.0f;
+
+            if (pathType == PathType.Linear || pathType == PathType.Flexible)
+            {
+                int step = 0;
+
+                float length = pathCutEnd - pathCutBegin;
+                float twistTotal = twistEnd - twistBegin;
+                float twistTotalAbs = Math.Abs(twistTotal);
+                if (twistTotalAbs > 0.01f)
+                    steps += (int)(twistTotalAbs * 3.66); //  dahlia's magic number
+
+                float start = -0.5f;
+                float stepSize = length / (float)steps;
+                float percentOfPathMultiplier = stepSize * 0.999999f;
+                float xOffset = topShearX * pathCutBegin;
+                float yOffset = topShearY * pathCutBegin;
+                float zOffset = start;
+                float xOffsetStepIncrement = topShearX * length / steps;
+                float yOffsetStepIncrement = topShearY * length / steps;
+
+                float percentOfPath = pathCutBegin;
+                zOffset += percentOfPath;
+
+                // sanity checks
+
+                bool done = false;
+
+                while (!done)
+                {
+                    PathNode newNode = new PathNode();
+
+                    newNode.xScale = 1.0f;
+                    if (taperX > 0.0f)
+                        newNode.xScale -= percentOfPath * taperX;
+                    else if(taperX < 0.0f)
+                        newNode.xScale += (1.0f - percentOfPath) * taperX;
+
+                    newNode.yScale = 1.0f;
+                    if (taperY > 0.0f)
+                        newNode.yScale -= percentOfPath * taperY;
+                    else if(taperY < 0.0f)
+                        newNode.yScale += (1.0f - percentOfPath) * taperY;
+
+                    float twist = twistBegin + twistTotal * percentOfPath;
+
+                    newNode.rotation = new Quat(new Coord(0.0f, 0.0f, 1.0f), twist);
+                    newNode.position = new Coord(xOffset, yOffset, zOffset);
+                    newNode.percentOfPath = percentOfPath;
+
+                    pathNodes.Add(newNode);
+
+                    if (step < steps)
+                    {
+                        step += 1;
+                        percentOfPath += percentOfPathMultiplier;
+                        xOffset += xOffsetStepIncrement;
+                        yOffset += yOffsetStepIncrement;
+                        zOffset += stepSize;
+                        if (percentOfPath > pathCutEnd)
+                            done = true;
+                    }
+                    else done = true;
+                }
+            } // end of linear path code
+
+            else // pathType == Circular
+            {
+                float twistTotal = twistEnd - twistBegin;
+
+                // if the profile has a lot of twist, add more layers otherwise the layers may overlap
+                // and the resulting mesh may be quite inaccurate. This method is arbitrary and doesn't
+                // accurately match the viewer
+                float twistTotalAbs = Math.Abs(twistTotal);
+                if (twistTotalAbs > 0.01f)
+                {
+                    if (twistTotalAbs > Math.PI * 1.5f)
+                        steps *= 2;
+                    if (twistTotalAbs > Math.PI * 3.0f)
+                        steps *= 2;
+                }
+
+                float yPathScale = holeSizeY * 0.5f;
+                float pathLength = pathCutEnd - pathCutBegin;
+                float totalSkew = skew * 2.0f * pathLength;
+                float skewStart = pathCutBegin * 2.0f * skew - skew;
+                float xOffsetTopShearXFactor = topShearX * (0.25f + 0.5f * (0.5f - holeSizeY));
+                float yShearCompensation = 1.0f + Math.Abs(topShearY) * 0.25f;
+
+                // It's not quite clear what pushY (Y top shear) does, but subtracting it from the start and end
+                // angles appears to approximate it's effects on path cut. Likewise, adding it to the angle used
+                // to calculate the sine for generating the path radius appears to approximate it's effects there
+                // too, but there are some subtle differences in the radius which are noticeable as the prim size
+                // increases and it may affect megaprims quite a bit. The effect of the Y top shear parameter on
+                // the meshes generated with this technique appear nearly identical in shape to the same prims when
+                // displayed by the viewer.
+
+                float startAngle = (twoPi * pathCutBegin * revolutions) - topShearY * 0.9f;
+                float endAngle = (twoPi * pathCutEnd * revolutions) - topShearY * 0.9f;
+                float stepSize = twoPi / stepsPerRevolution;
+
+                int step = (int)(startAngle / stepSize);
+                float angle = startAngle;
+
+                bool done = false;
+                while (!done) // loop through the length of the path and add the layers
+                {
+                    PathNode newNode = new PathNode();
+
+                    float xProfileScale = (1.0f - Math.Abs(skew)) * holeSizeX;
+                    float yProfileScale = holeSizeY;
+
+                    float percentOfPath = angle / (twoPi * revolutions);
+                    float percentOfAngles = (angle - startAngle) / (endAngle - startAngle);
+
+                    if (taperX > 0.01f)
+                        xProfileScale *= 1.0f - percentOfPath * taperX;
+                    else if (taperX < -0.01f)
+                        xProfileScale *= 1.0f + (1.0f - percentOfPath) * taperX;
+
+                    if (taperY > 0.01f)
+                        yProfileScale *= 1.0f - percentOfPath * taperY;
+                    else if (taperY < -0.01f)
+                        yProfileScale *= 1.0f + (1.0f - percentOfPath) * taperY;
+
+                    newNode.xScale = xProfileScale;
+                    newNode.yScale = yProfileScale;
+
+                    float radiusScale = 1.0f;
+                    if (radius > 0.001f)
+                        radiusScale = 1.0f - radius * percentOfPath;
+                    else if (radius < 0.001f)
+                        radiusScale = 1.0f + radius * (1.0f - percentOfPath);
+
+                    float twist = twistBegin + twistTotal * percentOfPath;
+
+                    float xOffset = 0.5f * (skewStart + totalSkew * percentOfAngles);
+                    xOffset += (float)Math.Sin(angle) * xOffsetTopShearXFactor;
+
+                    float yOffset = yShearCompensation * (float)Math.Cos(angle) * (0.5f - yPathScale) * radiusScale;
+
+                    float zOffset = (float)Math.Sin(angle + topShearY) * (0.5f - yPathScale) * radiusScale;
+
+                    newNode.position = new Coord(xOffset, yOffset, zOffset);
+
+                    // now orient the rotation of the profile layer relative to it's position on the path
+                    // adding taperY to the angle used to generate the quat appears to approximate the viewer
+
+                    newNode.rotation = new Quat(new Coord(1.0f, 0.0f, 0.0f), angle + topShearY);
+
+                    // next apply twist rotation to the profile layer
+                    if (twistTotal != 0.0f || twistBegin != 0.0f)
+                        newNode.rotation *= new Quat(new Coord(0.0f, 0.0f, 1.0f), twist);
+
+                    newNode.percentOfPath = percentOfPath;
+
+                    pathNodes.Add(newNode);
+
+                    // calculate terms for next iteration
+                    // calculate the angle for the next iteration of the loop
+
+                    if (angle >= endAngle - 0.01)
+                        done = true;
+                    else
+                    {
+                        step += 1;
+                        angle = stepSize * step;
+                        if (angle > endAngle)
+                            angle = endAngle;
+                    }
+                }
+            }
+        }
+    }
+
+    public class PrimMesh
+    {
+        public string errorMessage = "";
+        private const float twoPi = 2.0f * (float)Math.PI;
+
+        public List<Coord> coords;
+//        public List<Coord> normals;
+        public List<Face> faces;
+
+        private int sides = 4;
+        private int hollowSides = 4;
+        private float profileStart = 0.0f;
+        private float profileEnd = 1.0f;
+        private float hollow = 0.0f;
+        public int twistBegin = 0;
+        public int twistEnd = 0;
+        public float topShearX = 0.0f;
+        public float topShearY = 0.0f;
+        public float pathCutBegin = 0.0f;
+        public float pathCutEnd = 1.0f;
+        public float dimpleBegin = 0.0f;
+        public float dimpleEnd = 1.0f;
+        public float skew = 0.0f;
+        public float holeSizeX = 1.0f; // called pathScaleX in pbs
+        public float holeSizeY = 0.25f;
+        public float taperX = 0.0f;
+        public float taperY = 0.0f;
+        public float radius = 0.0f;
+        public float revolutions = 1.0f;
+        public int stepsPerRevolution = 24;
+
+        private bool hasProfileCut = false;
+        private bool hasHollow = false;
+
+        public int numPrimFaces = 0;
+
+        /// <summary>
+        /// Human readable string representation of the parameters used to create a mesh.
+        /// </summary>
+        /// <returns></returns>
+        public string ParamsToDisplayString()
+        {
+            string s = "";
+            s += "sides..................: " + this.sides.ToString();
+            s += "\nhollowSides..........: " + this.hollowSides.ToString();
+            s += "\nprofileStart.........: " + this.profileStart.ToString();
+            s += "\nprofileEnd...........: " + this.profileEnd.ToString();
+            s += "\nhollow...............: " + this.hollow.ToString();
+            s += "\ntwistBegin...........: " + this.twistBegin.ToString();
+            s += "\ntwistEnd.............: " + this.twistEnd.ToString();
+            s += "\ntopShearX............: " + this.topShearX.ToString();
+            s += "\ntopShearY............: " + this.topShearY.ToString();
+            s += "\npathCutBegin.........: " + this.pathCutBegin.ToString();
+            s += "\npathCutEnd...........: " + this.pathCutEnd.ToString();
+            s += "\ndimpleBegin..........: " + this.dimpleBegin.ToString();
+            s += "\ndimpleEnd............: " + this.dimpleEnd.ToString();
+            s += "\nskew.................: " + this.skew.ToString();
+            s += "\nholeSizeX............: " + this.holeSizeX.ToString();
+            s += "\nholeSizeY............: " + this.holeSizeY.ToString();
+            s += "\ntaperX...............: " + this.taperX.ToString();
+            s += "\ntaperY...............: " + this.taperY.ToString();
+            s += "\nradius...............: " + this.radius.ToString();
+            s += "\nrevolutions..........: " + this.revolutions.ToString();
+            s += "\nstepsPerRevolution...: " + this.stepsPerRevolution.ToString();
+            s += "\nhasProfileCut........: " + this.hasProfileCut.ToString();
+            s += "\nhasHollow............: " + this.hasHollow.ToString();
+
+            return s;
+        }
+
+        public bool HasProfileCut
+        {
+            get { return hasProfileCut; }
+            set { hasProfileCut = value; }
+        }
+
+        public bool HasHollow
+        {
+            get { return hasHollow; }
+        }
+
+
+        /// <summary>
+        /// Constructs a PrimMesh object and creates the profile for extrusion.
+        /// </summary>
+        /// <param name="sides"></param>
+        /// <param name="profileStart"></param>
+        /// <param name="profileEnd"></param>
+        /// <param name="hollow"></param>
+        /// <param name="hollowSides"></param>
+        /// <param name="sphereMode"></param>
+        public PrimMesh(int _sides, float _profileStart, float _profileEnd, float _hollow, int _hollowSides)
+        {
+            coords = new List<Coord>();
+            faces = new List<Face>();
+
+            sides = _sides;
+            profileStart = _profileStart;
+            profileEnd = _profileEnd;
+            hollow = _hollow;
+            hollowSides = _hollowSides;
+
+            if (sides < 3)
+                sides = 3;
+            if (hollowSides < 3)
+                hollowSides = 3;
+            if (profileStart < 0.0f)
+                profileStart = 0.0f;
+            if (profileEnd > 1.0f)
+                profileEnd = 1.0f;
+            if (profileEnd < 0.02f)
+                profileEnd = 0.02f;
+            if (profileStart >= profileEnd)
+                profileStart = profileEnd - 0.02f;
+            if (hollow > 0.99f)
+                hollow = 0.99f;
+            if (hollow < 0.0f)
+                hollow = 0.0f;
+        }
+
+        /// <summary>
+        /// Extrudes a profile along a path.
+        /// </summary>
+        public void Extrude(PathType pathType)
+        {
+            bool needEndFaces = false;
+
+            coords = new List<Coord>();
+            faces = new List<Face>();
+
+            int steps = 1;
+
+            float length = pathCutEnd - pathCutBegin;
+
+            hasProfileCut = this.profileEnd - this.profileStart < 0.9999f;
+
+            hasHollow = (this.hollow > 0.001f);
+
+            float twistBegin = this.twistBegin / 360.0f * twoPi;
+            float twistEnd = this.twistEnd / 360.0f * twoPi;
+            float twistTotal = twistEnd - twistBegin;
+            float twistTotalAbs = Math.Abs(twistTotal);
+            if (twistTotalAbs > 0.01f)
+                steps += (int)(twistTotalAbs * 3.66); //  dahlia's magic number
+
+            float hollow = this.hollow;
+            float initialProfileRot = 0.0f;
+
+            if (pathType == PathType.Circular)
+            {
+                needEndFaces = false;
+                if (pathCutBegin != 0.0f || pathCutEnd != 1.0f)
+                    needEndFaces = true;
+                else if (taperX != 0.0f || taperY != 0.0f)
+                    needEndFaces = true;
+                else if (skew != 0.0f)
+                    needEndFaces = true;
+                else if (twistTotal != 0.0f)
+                    needEndFaces = true;
+                else if (radius != 0.0f)
+                    needEndFaces = true;
+            }
+            else needEndFaces = true;
+
+            if (pathType == PathType.Circular)
+            {
+                if (sides == 3)
+                {
+                    initialProfileRot = (float)Math.PI;
+                    if (hollowSides == 4)
+                    {
+                        if (hollow > 0.7f)
+                            hollow = 0.7f;
+                        hollow *= 0.707f;
+                    }
+                    else hollow *= 0.5f;
+                }
+                else if (sides == 4)
+                {
+                    initialProfileRot = 0.25f * (float)Math.PI;
+                    if (hollowSides != 4)
+                        hollow *= 0.707f;
+                }
+                else if (sides > 4)
+                {
+                    initialProfileRot = (float)Math.PI;
+                    if (hollowSides == 4)
+                    {
+                        if (hollow > 0.7f)
+                            hollow = 0.7f;
+                        hollow /= 0.7f;
+                    }
+                }
+            }
+            else
+            {
+                if (sides == 3)
+                {
+                    if (hollowSides == 4)
+                    {
+                        if (hollow > 0.7f)
+                            hollow = 0.7f;
+                        hollow *= 0.707f;
+                    }
+                    else hollow *= 0.5f;
+                }
+                else if (sides == 4)
+                {
+                    initialProfileRot = 1.25f * (float)Math.PI;
+                    if (hollowSides != 4)
+                        hollow *= 0.707f;
+                }
+                else if (sides == 24 && hollowSides == 4)
+                    hollow *= 1.414f;
+            }
+
+            Profile profile = new Profile(sides, profileStart, profileEnd, hollow, hollowSides,
+                                HasProfileCut,true);
+            errorMessage = profile.errorMessage;
+
+            numPrimFaces = profile.numPrimFaces;
+
+            if (initialProfileRot != 0.0f)
+            {
+                profile.AddRot(new Quat(new Coord(0.0f, 0.0f, 1.0f), initialProfileRot));
+            }
+
+            Path path = new Path();
+            path.twistBegin = twistBegin;
+            path.twistEnd = twistEnd;
+            path.topShearX = topShearX;
+            path.topShearY = topShearY;
+            path.pathCutBegin = pathCutBegin;
+            path.pathCutEnd = pathCutEnd;
+            path.dimpleBegin = dimpleBegin;
+            path.dimpleEnd = dimpleEnd;
+            path.skew = skew;
+            path.holeSizeX = holeSizeX;
+            path.holeSizeY = holeSizeY;
+            path.taperX = taperX;
+            path.taperY = taperY;
+            path.radius = radius;
+            path.revolutions = revolutions;
+            path.stepsPerRevolution = stepsPerRevolution;
+
+            path.Create(pathType, steps);
+
+            int lastNode = path.pathNodes.Count - 1;
+
+            for (int nodeIndex = 0; nodeIndex < path.pathNodes.Count; nodeIndex++)
+            {
+                PathNode node = path.pathNodes[nodeIndex];
+                Profile newLayer = profile.Copy();
+
+                newLayer.Scale(node.xScale, node.yScale);
+                newLayer.AddRot(node.rotation);
+                newLayer.AddPos(node.position);
+
+                // append this layer
+                int coordsStart = coords.Count;
+                coords.AddRange(newLayer.coords);
+
+                if (needEndFaces && nodeIndex == 0 && newLayer.faces.Count > 0)
+                {
+                    newLayer.AddValue2FaceVertexIndices(coordsStart);
+                    newLayer.FlipNormals();
+                    faces.AddRange(newLayer.faces);
+                }
+
+                // fill faces between layers
+
+                List<Face> linkfaces = new List<Face>();
+                int numVerts = newLayer.coords.Count;
+                Face newFace1 = new Face();
+                Face newFace2 = new Face();
+
+                if (nodeIndex > 0)
+                {
+                    int startVert = coordsStart;
+                    int endVert = coords.Count;
+                    if (!hasProfileCut)
+                    {
+                        if(numVerts > 5 && !hasHollow)
+                            startVert++;
+                        int i = startVert;
+                        for (int l = 0; l < profile.numOuterVerts - 1; l++)
+                        {
+                            newFace1.v1 = i;
+                            newFace1.v2 = i - numVerts;
+                            newFace1.v3 = i + 1;
+                            linkfaces.Add(newFace1);
+
+                            newFace2.v1 = i + 1;
+                            newFace2.v2 = i - numVerts;
+                            newFace2.v3 = i + 1 - numVerts;
+                            linkfaces.Add(newFace2);
+                            i++;
+                        }
+
+                        newFace1.v1 = i;
+                        newFace1.v2 = i - numVerts;
+                        newFace1.v3 = startVert;
+                        linkfaces.Add(newFace1);
+
+                        newFace2.v1 = startVert;
+                        newFace2.v2 = i - numVerts;
+                        newFace2.v3 = startVert - numVerts;
+                        linkfaces.Add(newFace2);
+
+                        if (hasHollow)
+                        {
+                            startVert = ++i;
+                            for (int l = 0; l < profile.numHollowVerts - 1; l++)
+                            {
+                                newFace1.v1 = i;
+                                newFace1.v2 = i - numVerts;
+                                newFace1.v3 = i + 1;
+                                linkfaces.Add(newFace1);
+
+                                newFace2.v1 = i + 1;
+                                newFace2.v2 = i - numVerts;
+                                newFace2.v3 = i + 1 - numVerts;
+                                linkfaces.Add(newFace2);
+                                i++;
+                            }
+
+                            newFace1.v1 = i;
+                            newFace1.v2 = i - numVerts;
+                            newFace1.v3 = startVert;
+                            linkfaces.Add(newFace1);
+
+                            newFace2.v1 = startVert;
+                            newFace2.v2 = i - numVerts;
+                            newFace2.v3 = startVert - numVerts;
+                            linkfaces.Add(newFace2);
+                        }
+                    }
+                    else
+                    {
+                        for (int i = startVert; i < endVert; i++)
+                        {
+                            int iNext = i + 1;
+                            if (i == endVert - 1)
+                                iNext = startVert;
+
+                            newFace1.v1 = i;
+                            newFace1.v2 = i - numVerts;
+                            newFace1.v3 = iNext;
+                            linkfaces.Add(newFace1);
+
+                            newFace2.v1 = iNext;
+                            newFace2.v2 = i - numVerts;
+                            newFace2.v3 = iNext - numVerts;
+                            linkfaces.Add(newFace2);
+                        }
+                    }
+                }
+
+                if(linkfaces.Count > 0)
+                    faces.AddRange(linkfaces);
+
+                if (needEndFaces && nodeIndex == lastNode && newLayer.faces.Count > 0)
+                {
+                    newLayer.AddValue2FaceVertexIndices(coordsStart);
+                    faces.AddRange(newLayer.faces);
+                }
+
+            } // for (int nodeIndex = 0; nodeIndex < path.pathNodes.Count; nodeIndex++)
+        // more cleanup will be done at Meshmerizer.cs
+        }
+
+
+        /// <summary>
+        /// DEPRICATED - use Extrude(PathType.Linear) instead
+        /// Extrudes a profile along a straight line path. Used for prim types box, cylinder, and prism.
+        /// </summary>
+        ///
+        public void ExtrudeLinear()
+        {
+            Extrude(PathType.Linear);
+        }
+
+
+        /// <summary>
+        /// DEPRICATED - use Extrude(PathType.Circular) instead
+        /// Extrude a profile into a circular path prim mesh. Used for prim types torus, tube, and ring.
+        /// </summary>
+        ///
+        public void ExtrudeCircular()
+        {
+            Extrude(PathType.Circular);
+        }
+
+
+        private Coord SurfaceNormal(Coord c1, Coord c2, Coord c3)
+        {
+            Coord edge1 = new Coord(c2.X - c1.X, c2.Y - c1.Y, c2.Z - c1.Z);
+            Coord edge2 = new Coord(c3.X - c1.X, c3.Y - c1.Y, c3.Z - c1.Z);
+
+            Coord normal = Coord.Cross(edge1, edge2);
+
+            normal.Normalize();
+
+            return normal;
+        }
+
+        private Coord SurfaceNormal(Face face)
+        {
+            return SurfaceNormal(this.coords[face.v1], this.coords[face.v2], this.coords[face.v3]);
+        }
+
+        /// <summary>
+        /// Calculate the surface normal for a face in the list of faces
+        /// </summary>
+        /// <param name="faceIndex"></param>
+        /// <returns></returns>
+        public Coord SurfaceNormal(int faceIndex)
+        {
+            int numFaces = this.faces.Count;
+            if (faceIndex < 0 || faceIndex >= numFaces)
+                throw new Exception("faceIndex out of range");
+
+            return SurfaceNormal(this.faces[faceIndex]);
+        }
+
+        /// <summary>
+        /// Duplicates a PrimMesh object. All object properties are copied by value, including lists.
+        /// </summary>
+        /// <returns></returns>
+        public PrimMesh Copy()
+        {
+            PrimMesh copy = new PrimMesh(this.sides, this.profileStart, this.profileEnd, this.hollow, this.hollowSides);
+            copy.twistBegin = this.twistBegin;
+            copy.twistEnd = this.twistEnd;
+            copy.topShearX = this.topShearX;
+            copy.topShearY = this.topShearY;
+            copy.pathCutBegin = this.pathCutBegin;
+            copy.pathCutEnd = this.pathCutEnd;
+            copy.dimpleBegin = this.dimpleBegin;
+            copy.dimpleEnd = this.dimpleEnd;
+            copy.skew = this.skew;
+            copy.holeSizeX = this.holeSizeX;
+            copy.holeSizeY = this.holeSizeY;
+            copy.taperX = this.taperX;
+            copy.taperY = this.taperY;
+            copy.radius = this.radius;
+            copy.revolutions = this.revolutions;
+            copy.stepsPerRevolution = this.stepsPerRevolution;
+
+            copy.numPrimFaces = this.numPrimFaces;
+            copy.errorMessage = this.errorMessage;
+
+            copy.coords = new List<Coord>(this.coords);
+            copy.faces = new List<Face>(this.faces);
+
+            return copy;
+        }
+
+        /// <summary>
+        /// Adds a value to each XYZ vertex coordinate in the mesh
+        /// </summary>
+        /// <param name="x"></param>
+        /// <param name="y"></param>
+        /// <param name="z"></param>
+        public void AddPos(float x, float y, float z)
+        {
+            int i;
+            int numVerts = this.coords.Count;
+            Coord vert;
+
+            for (i = 0; i < numVerts; i++)
+            {
+                vert = this.coords[i];
+                vert.X += x;
+                vert.Y += y;
+                vert.Z += z;
+                this.coords[i] = vert;
+            }
+        }
+
+        /// <summary>
+        /// Rotates the mesh
+        /// </summary>
+        /// <param name="q"></param>
+        public void AddRot(Quat q)
+        {
+            int i;
+            int numVerts = this.coords.Count;
+
+            for (i = 0; i < numVerts; i++)
+                this.coords[i] *= q;
+        }
+
+#if VERTEX_INDEXER
+        public VertexIndexer GetVertexIndexer()
+        {
+            return null;
+        }
+#endif
+
+        /// <summary>
+        /// Scales the mesh
+        /// </summary>
+        /// <param name="x"></param>
+        /// <param name="y"></param>
+        /// <param name="z"></param>
+        public void Scale(float x, float y, float z)
+        {
+            int i;
+            int numVerts = this.coords.Count;
+            //Coord vert;
+
+            Coord m = new Coord(x, y, z);
+            for (i = 0; i < numVerts; i++)
+                this.coords[i] *= m;
+        }
+
+        /// <summary>
+        /// Dumps the mesh to a Blender compatible "Raw" format file
+        /// </summary>
+        /// <param name="path"></param>
+        /// <param name="name"></param>
+        /// <param name="title"></param>
+        public void DumpRaw(String path, String name, String title)
+        {
+            if (path == null)
+                return;
+            String fileName = name + "_" + title + ".raw";
+            String completePath = System.IO.Path.Combine(path, fileName);
+            StreamWriter sw = new StreamWriter(completePath);
+
+            for (int i = 0; i < this.faces.Count; i++)
+            {
+                string s = this.coords[this.faces[i].v1].ToString();
+                s += " " + this.coords[this.faces[i].v2].ToString();
+                s += " " + this.coords[this.faces[i].v3].ToString();
+
+                sw.WriteLine(s);
+            }
+
+            sw.Close();
+        }
+    }
+}
diff --git a/OpenSim/Region/PhysicsModules/ubOdeMeshing/Properties/AssemblyInfo.cs b/OpenSim/Region/PhysicsModules/ubOdeMeshing/Properties/AssemblyInfo.cs
new file mode 100644
index 0000000..6503e35
--- /dev/null
+++ b/OpenSim/Region/PhysicsModules/ubOdeMeshing/Properties/AssemblyInfo.cs
@@ -0,0 +1,36 @@
+using System.Reflection;
+using System.Runtime.CompilerServices;
+using System.Runtime.InteropServices;
+using Mono.Addins;
+
+// General Information about an assembly is controlled through the following
+// set of attributes. Change these attribute values to modify the information
+// associated with an assembly.
+[assembly: AssemblyTitle("OpenSim.Region.PhysicsModule.ubODEMeshing")]
+[assembly: AssemblyDescription("Mesher for ubODE")]
+[assembly: AssemblyConfiguration("")]
+[assembly: AssemblyCompany("http://opensimulator.org")]
+[assembly: AssemblyProduct("OpenSim")]
+[assembly: AssemblyCopyright("OpenSimulator developers")]
+[assembly: AssemblyTrademark("")]
+[assembly: AssemblyCulture("")]
+
+// Setting ComVisible to false makes the types in this assembly not visible
+// to COM components.  If you need to access a type in this assembly from
+// COM, set the ComVisible attribute to true on that type.
+[assembly: ComVisible(false)]
+
+// The following GUID is for the ID of the typelib if this project is exposed to COM
+[assembly: Guid("4b7e35c2-a9dd-4b10-b778-eb417f4f6884")]
+
+// Version information for an assembly consists of the following four values:
+//
+//      Major Version
+//      Minor Version
+//      Build Number
+//      Revision
+//
+[assembly: AssemblyVersion(OpenSim.VersionInfo.AssemblyVersionNumber)]
+
+[assembly: Addin("OpenSim.Region.PhysicsModule.ubOdeMeshing", OpenSim.VersionInfo.VersionNumber)]
+[assembly: AddinDependency("OpenSim.Region.Framework", OpenSim.VersionInfo.VersionNumber)]
diff --git a/OpenSim/Region/PhysicsModules/ubOdeMeshing/SculptMap.cs b/OpenSim/Region/PhysicsModules/ubOdeMeshing/SculptMap.cs
new file mode 100644
index 0000000..ebe2523
--- /dev/null
+++ b/OpenSim/Region/PhysicsModules/ubOdeMeshing/SculptMap.cs
@@ -0,0 +1,238 @@
+/*
+ * Copyright (c) Contributors
+ * See CONTRIBUTORS.TXT for a full list of copyright holders.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the OpenSimulator Project nor the
+ *       names of its contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+using System;
+using System.Collections.Generic;
+using System.Text;
+
+using System.Drawing;
+using System.Drawing.Imaging;
+
+namespace PrimMesher
+{
+    public class SculptMap
+    {
+        public int width;
+        public int height;
+        public byte[] redBytes;
+        public byte[] greenBytes;
+        public byte[] blueBytes;
+
+        public SculptMap()
+        {
+        }
+
+        public SculptMap(Bitmap bm, int lod)
+        {
+            int bmW = bm.Width;
+            int bmH = bm.Height;
+
+            if (bmW == 0 || bmH == 0)
+                throw new Exception("SculptMap: bitmap has no data");
+
+            int numLodPixels = lod * lod;  // (32 * 2)^2  = 64^2 pixels for default sculpt map image
+
+            bool needsScaling = false;
+            bool smallMap = false;
+
+            width = bmW;
+            height = bmH;
+
+            while (width * height > numLodPixels * 4)
+            {
+                width >>= 1;
+                height >>= 1;
+                needsScaling = true;
+            }
+
+            if (needsScaling)
+                bm = ScaleImage(bm, width, height);
+
+            if (width * height > numLodPixels)
+            {
+                smallMap = false;
+                width >>= 1;
+                height >>= 1;
+            }
+            else
+                smallMap = true;
+
+            int numBytes = (width + 1) * (height + 1);
+            redBytes = new byte[numBytes];
+            greenBytes = new byte[numBytes];
+            blueBytes = new byte[numBytes];
+
+            int byteNdx = 0;
+            Color c;
+
+            try
+            {
+                for (int y = 0; y <= height; y++)
+                {
+                    for (int x = 0; x < width; x++)
+                    {
+                        if (smallMap)
+                            c = bm.GetPixel(x,  y < height ? y : y - 1);
+                        else
+                            c = bm.GetPixel(x * 2, y < height ? y * 2 : y * 2 - 1);
+
+                        redBytes[byteNdx] = c.R;
+                        greenBytes[byteNdx] = c.G;
+                        blueBytes[byteNdx] = c.B;
+
+                        ++byteNdx;
+                    }
+
+                    if (smallMap)
+                        c = bm.GetPixel(width - 1, y < height ? y : y - 1);
+                    else
+                        c = bm.GetPixel(width * 2 - 1, y < height ? y * 2 : y * 2 - 1);
+
+                    redBytes[byteNdx] = c.R;
+                    greenBytes[byteNdx] = c.G;
+                    blueBytes[byteNdx] = c.B;
+
+                    ++byteNdx;
+                }
+            }
+            catch (Exception e)
+            {
+                if (needsScaling)
+                    bm.Dispose();
+                throw new Exception("Caught exception processing byte arrays in SculptMap(): e: " + e.ToString());
+            }
+
+            width++;
+            height++;
+            if(needsScaling)
+                bm.Dispose();
+        }
+
+        public List<List<Coord>> ToRows(bool mirror)
+        {
+            int numRows = height;
+            int numCols = width;
+
+            List<List<Coord>> rows = new List<List<Coord>>(numRows);
+
+            float pixScale = 1.0f / 255;
+
+            int rowNdx, colNdx;
+            int smNdx = 0;
+
+            for (rowNdx = 0; rowNdx < numRows; rowNdx++)
+            {
+                List<Coord> row = new List<Coord>(numCols);
+                for (colNdx = 0; colNdx < numCols; colNdx++)
+                {
+
+                    if (mirror)
+                        row.Add(new Coord(-((float)redBytes[smNdx] * pixScale - 0.5f), ((float)greenBytes[smNdx] * pixScale - 0.5f), (float)blueBytes[smNdx] * pixScale - 0.5f));
+                    else
+                        row.Add(new Coord((float)redBytes[smNdx] * pixScale - 0.5f, (float)greenBytes[smNdx] * pixScale - 0.5f, (float)blueBytes[smNdx] * pixScale - 0.5f));
+
+                    ++smNdx;
+                }
+                rows.Add(row);
+            }
+            return rows;
+        }
+
+        private Bitmap ScaleImage(Bitmap srcImage, int destWidth, int destHeight)
+        {
+            Bitmap scaledImage = new Bitmap(destWidth, destHeight, PixelFormat.Format24bppRgb);
+
+            Color c;
+
+            // will let last step to be eventually diferent, as seems to be in sl
+
+            float xscale = (float)srcImage.Width / (float)destWidth;
+            float yscale = (float)srcImage.Height / (float)destHeight;
+
+            int lastsx = srcImage.Width - 1;
+            int lastsy = srcImage.Height - 1;
+            int lastdx = destWidth - 1;
+            int lastdy = destHeight - 1;
+
+            float sy = 0.5f;
+            float sx;
+
+            for (int y = 0; y < lastdy; y++)
+            {
+                sx = 0.5f;
+                for (int x = 0; x < lastdx; x++)
+                {
+                    try
+                    {
+                        c = srcImage.GetPixel((int)(sx), (int)(sy));
+                        scaledImage.SetPixel(x, y, Color.FromArgb(c.R, c.G, c.B));
+                    }
+                    catch (IndexOutOfRangeException)
+                    {
+                    }
+                    sx += xscale;
+                }
+                try
+                {
+                    c = srcImage.GetPixel(lastsx, (int)(sy));
+                    scaledImage.SetPixel(lastdx, y, Color.FromArgb(c.R, c.G, c.B));
+                }
+                catch (IndexOutOfRangeException)
+                {
+                }
+
+                sy += yscale;
+            }
+
+            sx = 0.5f;
+            for (int x = 0; x < lastdx; x++)
+            {
+                try
+                {
+                    c = srcImage.GetPixel((int)(sx), lastsy);
+                    scaledImage.SetPixel(x, lastdy, Color.FromArgb(c.R, c.G, c.B));
+                }
+                catch (IndexOutOfRangeException)
+                {
+                }
+
+                sx += xscale;
+            }
+            try
+            {
+                c = srcImage.GetPixel(lastsx, lastsy);
+                scaledImage.SetPixel(lastdx, lastdy, Color.FromArgb(c.R, c.G, c.B));
+            }
+            catch (IndexOutOfRangeException)
+            {
+            }
+
+            srcImage.Dispose();
+            return scaledImage;
+        }
+    }
+}
\ No newline at end of file
diff --git a/OpenSim/Region/PhysicsModules/ubOdeMeshing/SculptMesh.cs b/OpenSim/Region/PhysicsModules/ubOdeMeshing/SculptMesh.cs
new file mode 100644
index 0000000..bc1375b
--- /dev/null
+++ b/OpenSim/Region/PhysicsModules/ubOdeMeshing/SculptMesh.cs
@@ -0,0 +1,220 @@
+/*
+ * Copyright (c) Contributors
+ * See CONTRIBUTORS.TXT for a full list of copyright holders.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the OpenSimulator Project nor the
+ *       names of its contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+using System;
+using System.Collections.Generic;
+using System.Text;
+using System.IO;
+
+using System.Drawing;
+using System.Drawing.Imaging;
+
+namespace PrimMesher
+{
+
+    public class SculptMesh
+    {
+        public List<Coord> coords;
+        public List<Face> faces;
+
+        public enum SculptType { sphere = 1, torus = 2, plane = 3, cylinder = 4 };
+
+
+        public SculptMesh(Bitmap sculptBitmap, SculptType sculptType, int lod, bool mirror, bool invert)
+        {
+            if (mirror)
+                invert = !invert;
+
+            SculptMap smap = new SculptMap(sculptBitmap, lod);
+
+            List<List<Coord>> rows = smap.ToRows(mirror);
+
+            _SculptMesh(rows, sculptType, invert);
+        }
+
+        private void _SculptMesh(List<List<Coord>> rows, SculptType sculptType, bool invert)
+        {
+            coords = new List<Coord>();
+            faces = new List<Face>();
+
+            sculptType = (SculptType)(((int)sculptType) & 0x07);
+
+            int width = rows[0].Count;
+
+            int p1, p2, p3, p4;
+
+            int imageX, imageY;
+
+            if (sculptType != SculptType.plane)
+            {
+                if (rows.Count % 2 == 0)
+                {
+                    for (int rowNdx = 0; rowNdx < rows.Count; rowNdx++)
+                        rows[rowNdx].Add(rows[rowNdx][0]);
+                }
+                else
+                {
+                    int lastIndex = rows[0].Count - 1;
+
+                    for (int i = 0; i < rows.Count; i++)
+                        rows[i][0] = rows[i][lastIndex];
+                }
+            }
+
+            Coord topPole = rows[0][width / 2];
+            Coord bottomPole = rows[rows.Count - 1][width / 2];
+
+            if (sculptType == SculptType.sphere)
+            {
+                if (rows.Count % 2 == 0)
+                {
+                    int count = rows[0].Count;
+                    List<Coord> topPoleRow = new List<Coord>(count);
+                    List<Coord> bottomPoleRow = new List<Coord>(count);
+
+                    for (int i = 0; i < count; i++)
+                    {
+                        topPoleRow.Add(topPole);
+                        bottomPoleRow.Add(bottomPole);
+                    }
+                    rows.Insert(0, topPoleRow);
+                    rows.Add(bottomPoleRow);
+                }
+                else
+                {
+                    int count = rows[0].Count;
+
+                    List<Coord> topPoleRow = rows[0];
+                    List<Coord> bottomPoleRow = rows[rows.Count - 1];
+
+                    for (int i = 0; i < count; i++)
+                    {
+                        topPoleRow[i] = topPole;
+                        bottomPoleRow[i] = bottomPole;
+                    }
+                }
+            }
+
+            if (sculptType == SculptType.torus)
+                rows.Add(rows[0]);
+
+            int coordsDown = rows.Count;
+            int coordsAcross = rows[0].Count;
+
+            float widthUnit = 1.0f / (coordsAcross - 1);
+            float heightUnit = 1.0f / (coordsDown - 1);
+
+            for (imageY = 0; imageY < coordsDown; imageY++)
+            {
+                int rowOffset = imageY * coordsAcross;
+
+                for (imageX = 0; imageX < coordsAcross; imageX++)
+                {
+                    /*
+                    *   p1-----p2
+                    *   | \ f2 |
+                    *   |   \  |
+                    *   | f1  \|
+                    *   p3-----p4
+                    */
+
+                    p4 = rowOffset + imageX;
+                    p3 = p4 - 1;
+
+                    p2 = p4 - coordsAcross;
+                    p1 = p3 - coordsAcross;
+
+                    this.coords.Add(rows[imageY][imageX]);
+
+                    if (imageY > 0 && imageX > 0)
+                    {
+                        Face f1, f2;
+
+                            if (invert)
+                            {
+                                f1 = new Face(p1, p4, p3);
+                                f2 = new Face(p1, p2, p4);
+                            }
+                            else
+                            {
+                                f1 = new Face(p1, p3, p4);
+                                f2 = new Face(p1, p4, p2);
+                            }
+
+                        this.faces.Add(f1);
+                        this.faces.Add(f2);
+                    }
+                }
+            }
+        }
+
+        /// <summary>
+        /// Duplicates a SculptMesh object. All object properties are copied by value, including lists.
+        /// </summary>
+        /// <returns></returns>
+        public SculptMesh Copy()
+        {
+            return new SculptMesh(this);
+        }
+
+        public SculptMesh(SculptMesh sm)
+        {
+            coords = new List<Coord>(sm.coords);
+            faces = new List<Face>(sm.faces);
+        }
+
+        public void Scale(float x, float y, float z)
+        {
+            int i;
+            int numVerts = this.coords.Count;
+
+            Coord m = new Coord(x, y, z);
+            for (i = 0; i < numVerts; i++)
+                this.coords[i] *= m;
+        }
+
+        public void DumpRaw(String path, String name, String title)
+        {
+            if (path == null)
+                return;
+            String fileName = name + "_" + title + ".raw";
+            String completePath = System.IO.Path.Combine(path, fileName);
+            StreamWriter sw = new StreamWriter(completePath);
+
+            for (int i = 0; i < this.faces.Count; i++)
+            {
+                string s = this.coords[this.faces[i].v1].ToString();
+                s += " " + this.coords[this.faces[i].v2].ToString();
+                s += " " + this.coords[this.faces[i].v3].ToString();
+
+                sw.WriteLine(s);
+            }
+
+            sw.Close();
+        }
+    }
+}