11 files changed, 2644 insertions, 1431 deletions

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);
-    }
-}