Dump OpenSim 0.9.0.1 into it's own branch.

17 files changed, 19288 insertions, 0 deletions

diff --git a/OpenSim/Region/PhysicsModules/ubOde/ODEApi.cs b/OpenSim/Region/PhysicsModules/ubOde/ODEApi.cs
new file mode 100644
index 0000000..35adf11
--- /dev/null
+++ b/OpenSim/Region/PhysicsModules/ubOde/ODEApi.cs
@@ -0,0 +1,2024 @@
+/*
+ * based on:
+ * Ode.NET - .NET bindings for ODE
+ * Jason Perkins (starkos@industriousone.com)
+  *  Licensed under the New BSD
+ *  Part of the OpenDynamicsEngine
+Open Dynamics Engine
+Copyright (c) 2001-2007, Russell L. Smith.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions
+are met:
+
+Redistributions of source code must retain the above copyright notice,
+this list of conditions and the following disclaimer.
+
+Redistributions in binary form must reproduce the above copyright notice,
+this list of conditions and the following disclaimer in the documentation
+and/or other materials provided with the distribution.
+
+Neither the names of ODE's copyright owner nor the names of its
+contributors may be used to endorse or promote products derived from
+this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ * changes by opensim team;
+ * changes by Aurora team http://www.aurora-sim.org/
+ * changes by Ubit Umarov
+ */
+
+using System;
+using System.Runtime.InteropServices;
+using System.Security;
+using OMV = OpenMetaverse;
+namespace OdeAPI
+{
+//#if dDOUBLE
+// don't see much use in double precision with time steps of 20ms and 10 iterations used on opensim
+// at least we save same memory and memory access time, FPU performance on intel usually is similar
+//      using dReal = System.Double;
+//#else
+    using dReal = System.Single;
+//#endif
+
+    public static class d
+    {
+        public static dReal Infinity = dReal.MaxValue;
+        public static int NTotalBodies = 0;
+        public static int NTotalGeoms = 0;
+
+        public const uint CONTACTS_UNIMPORTANT = 0x80000000;
+
+        #region Flags and Enumerations
+
+        [Flags]
+        public enum AllocateODEDataFlags : uint
+        {
+            BasicData = 0,
+            CollisionData = 0x00000001,
+            All = ~0u
+        }
+
+        [Flags]
+        public enum IniteODEFlags : uint
+        {
+            dInitFlagManualThreadCleanup = 0x00000001
+        }
+
+        [Flags]
+        public enum ContactFlags : int
+        {
+            Mu2 = 0x001,
+            FDir1 = 0x002,
+            Bounce = 0x004,
+            SoftERP = 0x008,
+            SoftCFM = 0x010,
+            Motion1 = 0x020,
+            Motion2 = 0x040,
+            MotionN = 0x080,
+            Slip1 = 0x100,
+            Slip2 = 0x200,
+            Approx0 = 0x0000,
+            Approx1_1 = 0x1000,
+            Approx1_2 = 0x2000,
+            Approx1 = 0x3000
+        }
+
+        public enum GeomClassID : int
+        {
+            SphereClass,
+            BoxClass,
+            CapsuleClass,
+            CylinderClass,
+            PlaneClass,
+            RayClass,
+            ConvexClass,
+            GeomTransformClass,
+            TriMeshClass,
+            HeightfieldClass,
+            FirstSpaceClass,
+            SimpleSpaceClass = FirstSpaceClass,
+            HashSpaceClass,
+            QuadTreeSpaceClass,
+            LastSpaceClass = QuadTreeSpaceClass,
+            ubtTerrainClass,
+            FirstUserClass,
+            LastUserClass = FirstUserClass + MaxUserClasses - 1,
+            NumClasses,
+            MaxUserClasses = 5
+        }
+
+        public enum JointType : int
+        {
+            None,
+            Ball,
+            Hinge,
+            Slider,
+            Contact,
+            Universal,
+            Hinge2,
+            Fixed,
+            Null,
+            AMotor,
+            LMotor,
+            Plane2D
+        }
+
+        public enum JointParam : int
+        {
+            LoStop,
+            HiStop,
+            Vel,
+            FMax,
+            FudgeFactor,
+            Bounce,
+            CFM,
+            StopERP,
+            StopCFM,
+            SuspensionERP,
+            SuspensionCFM,
+            LoStop2 = 256,
+            HiStop2,
+            Vel2,
+            FMax2,
+            FudgeFactor2,
+            Bounce2,
+            CFM2,
+            StopERP2,
+            StopCFM2,
+            SuspensionERP2,
+            SuspensionCFM2,
+            LoStop3 = 512,
+            HiStop3,
+            Vel3,
+            FMax3,
+            FudgeFactor3,
+            Bounce3,
+            CFM3,
+            StopERP3,
+            StopCFM3,
+            SuspensionERP3,
+            SuspensionCFM3
+        }
+
+        public enum dSweepAndPruneAxis : int
+        {
+            XYZ = ((0)|(1<<2)|(2<<4)),
+            XZY = ((0)|(2<<2)|(1<<4)),
+            YXZ = ((1)|(0<<2)|(2<<4)),
+            YZX = ((1)|(2<<2)|(0<<4)),
+            ZXY = ((2)|(0<<2)|(1<<4)),
+            ZYX = ((2)|(1<<2)|(0<<4))
+        }
+
+        #endregion
+
+        #region Callbacks
+
+        [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
+        public delegate int AABBTestFn(IntPtr o1, IntPtr o2, ref AABB aabb);
+
+        [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
+        public delegate int ColliderFn(IntPtr o1, IntPtr o2, int flags, out ContactGeom contact, int skip);
+
+        [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
+        public delegate void GetAABBFn(IntPtr geom, out AABB aabb);
+
+        [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
+        public delegate ColliderFn GetColliderFnFn(int num);
+
+        [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
+        public delegate void GeomDtorFn(IntPtr o);
+
+        [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
+        public delegate dReal HeightfieldGetHeight(IntPtr p_user_data, int x, int z);
+
+        [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
+        public delegate dReal OSTerrainGetHeight(IntPtr p_user_data, int x, int z);
+
+        [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
+        public delegate void NearCallback(IntPtr data, IntPtr geom1, IntPtr geom2);
+
+        [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
+        public delegate int TriCallback(IntPtr trimesh, IntPtr refObject, int triangleIndex);
+
+        [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
+        public delegate int TriArrayCallback(IntPtr trimesh, IntPtr refObject, int[] triangleIndex, int triCount);
+
+        [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
+        public delegate int TriRayCallback(IntPtr trimesh, IntPtr ray, int triangleIndex, dReal u, dReal v);
+
+        #endregion
+
+        #region Structs
+
+        [StructLayout(LayoutKind.Sequential)]
+        public struct AABB
+        {
+            public dReal MinX, MaxX;
+            public dReal MinY, MaxY;
+            public dReal MinZ, MaxZ;
+        }
+
+
+        [StructLayout(LayoutKind.Sequential)]
+        public struct Contact
+        {
+            public SurfaceParameters surface;
+            public ContactGeom geom;
+            public Vector3 fdir1;
+            public static readonly int unmanagedSizeOf = Marshal.SizeOf(typeof(Contact));
+        }
+
+
+        [StructLayout(LayoutKind.Sequential)]
+        public struct ContactGeom
+        {
+
+            public Vector3 pos;
+            public Vector3 normal;
+            public dReal depth;
+            public IntPtr g1;
+            public IntPtr g2;
+            public int side1;
+            public int side2;
+            public static readonly int unmanagedSizeOf = Marshal.SizeOf(typeof(ContactGeom));
+        }
+
+        [StructLayout(LayoutKind.Sequential)]
+        public struct GeomClass
+        {
+            public int bytes;
+            public GetColliderFnFn collider;
+            public GetAABBFn aabb;
+            public AABBTestFn aabb_test;
+            public GeomDtorFn dtor;
+        }
+
+
+        [StructLayout(LayoutKind.Sequential)]
+        public struct JointFeedback
+        {
+            public Vector3 f1;
+            public Vector3 t1;
+            public Vector3 f2;
+            public Vector3 t2;
+        }
+
+
+        [StructLayout(LayoutKind.Sequential)]
+        public struct Mass
+        {
+            public dReal mass;
+            public Vector4 c;
+            public Matrix3 I;
+        }
+
+
+        [StructLayout(LayoutKind.Sequential)]
+        public struct Matrix3
+        {
+            public Matrix3(dReal m00, dReal m10, dReal m20, dReal m01, dReal m11, dReal m21, dReal m02, dReal m12, dReal m22)
+            {
+                M00 = m00;  M10 = m10;  M20 = m20;  _m30 = 0.0f;
+                M01 = m01;  M11 = m11;  M21 = m21;  _m31 = 0.0f;
+                M02 = m02;  M12 = m12;  M22 = m22;  _m32 = 0.0f;
+            }
+            public dReal M00, M10, M20;
+            private dReal _m30;
+            public dReal M01, M11, M21;
+            private dReal _m31;
+            public dReal M02, M12, M22;
+            private dReal _m32;
+        }
+
+        [StructLayout(LayoutKind.Sequential)]
+        public struct Matrix4
+        {
+            public Matrix4(dReal m00, dReal m10, dReal m20, dReal m30,
+                dReal m01, dReal m11, dReal m21, dReal m31,
+                dReal m02, dReal m12, dReal m22, dReal m32,
+                dReal m03, dReal m13, dReal m23, dReal m33)
+            {
+                M00 = m00; M10 = m10; M20 = m20; M30 = m30;
+                M01 = m01; M11 = m11; M21 = m21; M31 = m31;
+                M02 = m02; M12 = m12; M22 = m22; M32 = m32;
+                M03 = m03; M13 = m13; M23 = m23; M33 = m33;
+            }
+            public dReal M00, M10, M20, M30;
+            public dReal M01, M11, M21, M31;
+            public dReal M02, M12, M22, M32;
+            public dReal M03, M13, M23, M33;
+        }
+
+        [StructLayout(LayoutKind.Sequential)]
+        public struct Quaternion
+        {
+            public dReal W, X, Y, Z;
+        }
+
+
+        [StructLayout(LayoutKind.Sequential)]
+        public struct SurfaceParameters
+        {
+            public ContactFlags mode;
+            public dReal mu;
+            public dReal mu2;
+            public dReal bounce;
+            public dReal bounce_vel;
+            public dReal soft_erp;
+            public dReal soft_cfm;
+            public dReal motion1;
+            public dReal motion2;
+            public dReal motionN;
+            public dReal slip1;
+            public dReal slip2;
+        }
+
+
+        [StructLayout(LayoutKind.Sequential)]
+        public struct Vector3
+        {
+            public Vector3(dReal x, dReal y, dReal z)
+            {
+                X = x;  Y = y;  Z = z;  _w = 0.0f;
+            }
+            public dReal X, Y, Z;
+            private dReal _w;
+        }
+
+
+        [StructLayout(LayoutKind.Sequential)]
+        public struct Vector4
+        {
+            public Vector4(dReal x, dReal y, dReal z, dReal w)
+            {
+                X = x;  Y = y;  Z = z;  W = w;
+            }
+            public dReal X, Y, Z, W;
+        }
+
+        #endregion
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dAllocateODEDataForThread"), SuppressUnmanagedCodeSecurity]
+        public static extern int AllocateODEDataForThread(uint ODEInitFlags);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dAreConnected"), SuppressUnmanagedCodeSecurity]
+        public static extern bool AreConnected(IntPtr b1, IntPtr b2);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dAreConnectedExcluding"), SuppressUnmanagedCodeSecurity]
+        public static extern bool AreConnectedExcluding(IntPtr b1, IntPtr b2, JointType joint_type);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddForce"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyAddForce(IntPtr body, dReal fx, dReal fy, dReal fz);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddForceAtPos"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyAddForceAtPos(IntPtr body, dReal fx, dReal fy, dReal fz, dReal px, dReal py, dReal pz);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddForceAtRelPos"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyAddForceAtRelPos(IntPtr body, dReal fx, dReal fy, dReal fz, dReal px, dReal py, dReal pz);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddRelForce"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyAddRelForce(IntPtr body, dReal fx, dReal fy, dReal fz);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddRelForceAtPos"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyAddRelForceAtPos(IntPtr body, dReal fx, dReal fy, dReal fz, dReal px, dReal py, dReal pz);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddRelForceAtRelPos"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyAddRelForceAtRelPos(IntPtr body, dReal fx, dReal fy, dReal fz, dReal px, dReal py, dReal pz);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddRelTorque"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyAddRelTorque(IntPtr body, dReal fx, dReal fy, dReal fz);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddTorque"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyAddTorque(IntPtr body, dReal fx, dReal fy, dReal fz);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyPosition"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyCopyPosition(IntPtr body, out Vector3 pos);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyPosition"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyCopyPosition(IntPtr body, out dReal X);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyQuaternion"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyCopyQuaternion(IntPtr body, out Quaternion quat);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyQuaternion"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyCopyQuaternion(IntPtr body, out dReal X);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyRotation"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyCopyRotation(IntPtr body, out Matrix3 R);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyRotation"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyCopyRotation(IntPtr body, out dReal M00);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCreate"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr BodyiCreate(IntPtr world);
+        public static IntPtr BodyCreate(IntPtr world)
+        {
+            NTotalBodies++;
+            return BodyiCreate(world);
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyDestroy"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyiDestroy(IntPtr body);
+        public static void BodyDestroy(IntPtr body)
+        {
+            NTotalBodies--;
+            BodyiDestroy(body);
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyDisable"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyDisable(IntPtr body);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyEnable"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyEnable(IntPtr body);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableAngularThreshold"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal BodyGetAutoDisableAngularThreshold(IntPtr body);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableFlag"), SuppressUnmanagedCodeSecurity]
+        public static extern bool BodyGetAutoDisableFlag(IntPtr body);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableDefaults"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyGetAutoDisableDefaults(IntPtr body);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableLinearThreshold"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal BodyGetAutoDisableLinearThreshold(IntPtr body);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableSteps"), SuppressUnmanagedCodeSecurity]
+        public static extern int BodyGetAutoDisableSteps(IntPtr body);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableTime"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal BodyGetAutoDisableTime(IntPtr body);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAngularVel"), SuppressUnmanagedCodeSecurity]
+        public extern unsafe static Vector3* BodyGetAngularVelUnsafe(IntPtr body);
+        public static Vector3 BodyGetAngularVel(IntPtr body)
+        {
+            unsafe { return *(BodyGetAngularVelUnsafe(body)); }
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetData"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr BodyGetData(IntPtr body);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetFiniteRotationMode"), SuppressUnmanagedCodeSecurity]
+        public static extern int BodyGetFiniteRotationMode(IntPtr body);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetFiniteRotationAxis"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyGetFiniteRotationAxis(IntPtr body, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetForce"), SuppressUnmanagedCodeSecurity]
+        public extern unsafe static Vector3* BodyGetForceUnsafe(IntPtr body);
+        public static Vector3 BodyGetForce(IntPtr body)
+        {
+            unsafe { return *(BodyGetForceUnsafe(body)); }
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetGravityMode"), SuppressUnmanagedCodeSecurity]
+        public static extern bool BodyGetGravityMode(IntPtr body);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetGyroscopicMode"), SuppressUnmanagedCodeSecurity]
+        public static extern int BodyGetGyroscopicMode(IntPtr body);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetJoint"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr BodyGetJoint(IntPtr body, int index);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetLinearVel"), SuppressUnmanagedCodeSecurity]
+        public extern unsafe static Vector3* BodyGetLinearVelUnsafe(IntPtr body);
+        public static Vector3 BodyGetLinearVel(IntPtr body)
+        {
+            unsafe { return *(BodyGetLinearVelUnsafe(body)); }
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetMass"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyGetMass(IntPtr body, out Mass mass);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetNumJoints"), SuppressUnmanagedCodeSecurity]
+        public static extern int BodyGetNumJoints(IntPtr body);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetPointVel"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyGetPointVel(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetPosition"), SuppressUnmanagedCodeSecurity]
+        public extern unsafe static Vector3* BodyGetPositionUnsafe(IntPtr body);
+        public static Vector3 BodyGetPosition(IntPtr body)
+        {
+            unsafe { return *(BodyGetPositionUnsafe(body)); }
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetPosRelPoint"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyGetPosRelPoint(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetQuaternion"), SuppressUnmanagedCodeSecurity]
+        public extern unsafe static Quaternion* BodyGetQuaternionUnsafe(IntPtr body);
+        public static Quaternion BodyGetQuaternion(IntPtr body)
+        {
+            unsafe { return *(BodyGetQuaternionUnsafe(body)); }
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetRelPointPos"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyGetRelPointPos(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetRelPointVel"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyGetRelPointVel(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetRotation"), SuppressUnmanagedCodeSecurity]
+        public extern unsafe static Matrix3* BodyGetRotationUnsafe(IntPtr body);
+        public static Matrix3 BodyGetRotation(IntPtr body)
+        {
+            unsafe { return *(BodyGetRotationUnsafe(body)); }
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetTorque"), SuppressUnmanagedCodeSecurity]
+        public extern unsafe static Vector3* BodyGetTorqueUnsafe(IntPtr body);
+        public static Vector3 BodyGetTorque(IntPtr body)
+        {
+            unsafe { return *(BodyGetTorqueUnsafe(body)); }
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetWorld"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr BodyGetWorld(IntPtr body);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetFirstGeom"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr BodyGetFirstGeom(IntPtr body);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetNextGeom"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr dBodyGetNextGeom(IntPtr Geom);
+
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyIsEnabled"), SuppressUnmanagedCodeSecurity]
+        public static extern bool BodyIsEnabled(IntPtr body);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAngularVel"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetAngularVel(IntPtr body, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableAngularThreshold"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetAutoDisableAngularThreshold(IntPtr body, dReal angular_threshold);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableDefaults"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetAutoDisableDefaults(IntPtr body);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableFlag"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetAutoDisableFlag(IntPtr body, bool do_auto_disable);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableLinearThreshold"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetAutoDisableLinearThreshold(IntPtr body, dReal linear_threshold);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableSteps"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetAutoDisableSteps(IntPtr body, int steps);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableTime"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetAutoDisableTime(IntPtr body, dReal time);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetData"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetData(IntPtr body, IntPtr data);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetFiniteRotationMode"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetFiniteRotationMode(IntPtr body, int mode);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetFiniteRotationAxis"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetFiniteRotationAxis(IntPtr body, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetLinearDamping"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetLinearDamping(IntPtr body, dReal scale);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAngularDamping"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetAngularDamping(IntPtr body, dReal scale);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetLinearDamping"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal BodyGetLinearDamping(IntPtr body);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAngularDamping"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal BodyGetAngularDamping(IntPtr body);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAngularDamping"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetDamping(IntPtr body, dReal linear_scale, dReal angular_scale);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAngularDampingThreshold"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetAngularDampingThreshold(IntPtr body, dReal threshold);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetLinearDampingThreshold"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetLinearDampingThreshold(IntPtr body, dReal threshold);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetLinearDampingThreshold"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal BodyGetLinearDampingThreshold(IntPtr body);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAngularDampingThreshold"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal BodyGetAngularDampingThreshold(IntPtr body);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetForce"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetForce(IntPtr body, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetGravityMode"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetGravityMode(IntPtr body, bool mode);
+
+        /// <summary>
+        /// Sets the Gyroscopic term status on the body specified.
+        /// </summary>
+        /// <param name="body">Pointer to body</param>
+        /// <param name="enabled">NonZero enabled, Zero disabled</param>
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetGyroscopicMode"), SuppressUnmanagedCodeSecurity]
+        public static extern void dBodySetGyroscopicMode(IntPtr body, int enabled);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetLinearVel"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetLinearVel(IntPtr body, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetMass"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetMass(IntPtr body, ref Mass mass);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetPosition"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetPosition(IntPtr body, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetQuaternion"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetQuaternion(IntPtr body, ref Quaternion q);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetQuaternion"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetQuaternion(IntPtr body, ref dReal w);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetRotation"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetRotation(IntPtr body, ref Matrix3 R);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetRotation"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetRotation(IntPtr body, ref dReal M00);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetTorque"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodySetTorque(IntPtr body, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyVectorFromWorld"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyVectorFromWorld(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyVectorToWorld"), SuppressUnmanagedCodeSecurity]
+        public static extern void BodyVectorToWorld(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBoxBox"), SuppressUnmanagedCodeSecurity]
+        public static extern void BoxBox(ref Vector3 p1, ref Matrix3 R1,
+            ref Vector3 side1, ref Vector3 p2,
+            ref Matrix3 R2, ref Vector3 side2,
+            ref Vector3 normal, out dReal depth, out int return_code,
+            int maxc, out ContactGeom contact, int skip);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBoxTouchesBox"), SuppressUnmanagedCodeSecurity]
+        public static extern void BoxTouchesBox(ref Vector3 _p1, ref Matrix3 R1,
+            ref Vector3 side1, ref Vector3 _p2,
+            ref Matrix3 R2, ref Vector3 side2);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCleanupODEAllDataForThread"), SuppressUnmanagedCodeSecurity]
+        public static extern void CleanupODEAllDataForThread();
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dClosestLineSegmentPoints"), SuppressUnmanagedCodeSecurity]
+        public static extern void ClosestLineSegmentPoints(ref Vector3 a1, ref Vector3 a2,
+            ref Vector3 b1, ref Vector3 b2,
+            ref Vector3 cp1, ref Vector3 cp2);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCloseODE"), SuppressUnmanagedCodeSecurity]
+        public static extern void CloseODE();
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCollide"), SuppressUnmanagedCodeSecurity]
+        public static extern int Collide(IntPtr o1, IntPtr o2, int flags, [In, Out] ContactGeom[] contact, int skip);
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCollide"), SuppressUnmanagedCodeSecurity]
+        public static extern int CollidePtr(IntPtr o1, IntPtr o2, int flags, IntPtr contactgeomarray, int skip);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dConnectingJoint"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr ConnectingJoint(IntPtr j1, IntPtr j2);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateBox"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr CreateiBox(IntPtr space, dReal lx, dReal ly, dReal lz);
+        public static IntPtr CreateBox(IntPtr space, dReal lx, dReal ly, dReal lz)
+        {
+            NTotalGeoms++;
+            return CreateiBox(space, lx, ly, lz);
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateCapsule"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr CreateiCapsule(IntPtr space, dReal radius, dReal length);
+        public static IntPtr CreateCapsule(IntPtr space, dReal radius, dReal length)
+        {
+            NTotalGeoms++;
+            return CreateiCapsule(space, radius, length);
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateConvex"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr CreateiConvex(IntPtr space, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons);
+        public static IntPtr CreateConvex(IntPtr space, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons)
+        {
+            NTotalGeoms++;
+            return CreateiConvex(space, planes, planeCount, points, pointCount, polygons);
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateCylinder"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr CreateiCylinder(IntPtr space, dReal radius, dReal length);
+        public static IntPtr CreateCylinder(IntPtr space, dReal radius, dReal length)
+        {
+            NTotalGeoms++;
+            return CreateiCylinder(space, radius, length);
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateHeightfield"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr CreateiHeightfield(IntPtr space, IntPtr data, int bPlaceable);
+        public static IntPtr CreateHeightfield(IntPtr space, IntPtr data, int bPlaceable)
+        {
+            NTotalGeoms++;
+            return CreateiHeightfield(space, data, bPlaceable);
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateOSTerrain"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr CreateiOSTerrain(IntPtr space, IntPtr data, int bPlaceable);
+        public static IntPtr CreateOSTerrain(IntPtr space, IntPtr data, int bPlaceable)
+        {
+            NTotalGeoms++;
+            return CreateiOSTerrain(space, data, bPlaceable);
+        }
+
+
+
+
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateGeom"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr CreateiGeom(int classnum);
+        public static IntPtr CreateGeom(int classnum)
+        {
+            NTotalGeoms++;
+            return CreateiGeom(classnum);
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateGeomClass"), SuppressUnmanagedCodeSecurity]
+        public static extern int CreateGeomClass(ref GeomClass classptr);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateGeomTransform"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr CreateGeomTransform(IntPtr space);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreatePlane"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr CreateiPlane(IntPtr space, dReal a, dReal b, dReal c, dReal d);
+        public static IntPtr CreatePlane(IntPtr space, dReal a, dReal b, dReal c, dReal d)
+        {
+            NTotalGeoms++;
+            return CreateiPlane(space, a, b, c, d);
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateRay"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr CreateiRay(IntPtr space, dReal length);
+        public static IntPtr CreateRay(IntPtr space, dReal length)
+        {
+            NTotalGeoms++;
+            return CreateiRay(space, length);
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateSphere"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr CreateiSphere(IntPtr space, dReal radius);
+        public static IntPtr CreateSphere(IntPtr space, dReal radius)
+        {
+            NTotalGeoms++;
+            return CreateiSphere(space, radius);
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateTriMesh"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr CreateiTriMesh(IntPtr space, IntPtr data,
+            TriCallback callback, TriArrayCallback arrayCallback, TriRayCallback rayCallback);
+        public static IntPtr CreateTriMesh(IntPtr space, IntPtr data,
+            TriCallback callback, TriArrayCallback arrayCallback, TriRayCallback rayCallback)
+        {
+            NTotalGeoms++;
+            return CreateiTriMesh(space, data, callback, arrayCallback, rayCallback);
+        }
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dDot"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal Dot(ref dReal X0, ref dReal X1, int n);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dDQfromW"), SuppressUnmanagedCodeSecurity]
+        public static extern void DQfromW(dReal[] dq, ref Vector3 w, ref Quaternion q);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dFactorCholesky"), SuppressUnmanagedCodeSecurity]
+        public static extern int FactorCholesky(ref dReal A00, int n);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dFactorLDLT"), SuppressUnmanagedCodeSecurity]
+        public static extern void FactorLDLT(ref dReal A, out dReal d, int n, int nskip);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomBoxGetLengths"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomBoxGetLengths(IntPtr geom, out Vector3 len);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomBoxGetLengths"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomBoxGetLengths(IntPtr geom, out dReal x);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomBoxPointDepth"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal GeomBoxPointDepth(IntPtr geom, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomBoxSetLengths"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomBoxSetLengths(IntPtr geom, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCapsuleGetParams"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomCapsuleGetParams(IntPtr geom, out dReal radius, out dReal length);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCapsulePointDepth"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal GeomCapsulePointDepth(IntPtr geom, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCapsuleSetParams"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomCapsuleSetParams(IntPtr geom, dReal radius, dReal length);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomClearOffset"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomClearOffset(IntPtr geom);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyOffsetPosition"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr GeomCopyOffsetPosition(IntPtr geom, ref Vector3 pos);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyOffsetPosition"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr GeomCopyOffsetPosition(IntPtr geom, ref dReal X);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetOffsetQuaternion"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomCopyOffsetQuaternion(IntPtr geom, ref Quaternion Q);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetOffsetQuaternion"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomCopyOffsetQuaternion(IntPtr geom, ref dReal X);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyOffsetRotation"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr GeomCopyOffsetRotation(IntPtr geom, ref Matrix3 R);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyOffsetRotation"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr GeomCopyOffsetRotation(IntPtr geom, ref dReal M00);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyPosition"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomCopyPosition(IntPtr geom, out Vector3 pos);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyPosition"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomCopyPosition(IntPtr geom, out dReal X);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyRotation"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomCopyRotation(IntPtr geom, out Matrix3 R);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyRotation"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomCopyRotation(IntPtr geom, out dReal M00);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCylinderGetParams"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomCylinderGetParams(IntPtr geom, out dReal radius, out dReal length);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCylinderSetParams"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomCylinderSetParams(IntPtr geom, dReal radius, dReal length);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomDestroy"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomiDestroy(IntPtr geom);
+        public static void GeomDestroy(IntPtr geom)
+        {
+            NTotalGeoms--;
+            GeomiDestroy(geom);
+        }
+
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomDisable"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomDisable(IntPtr geom);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomEnable"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomEnable(IntPtr geom);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetAABB"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomGetAABB(IntPtr geom, out AABB aabb);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetAABB"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomGetAABB(IntPtr geom, out dReal minX);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetBody"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr GeomGetBody(IntPtr geom);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetCategoryBits"), SuppressUnmanagedCodeSecurity]
+        public static extern uint GeomGetCategoryBits(IntPtr geom);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetClassData"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr GeomGetClassData(IntPtr geom);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetCollideBits"), SuppressUnmanagedCodeSecurity]
+        public static extern uint GeomGetCollideBits(IntPtr geom);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetClass"), SuppressUnmanagedCodeSecurity]
+        public static extern GeomClassID GeomGetClass(IntPtr geom);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetData"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr GeomGetData(IntPtr geom);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetOffsetPosition"), SuppressUnmanagedCodeSecurity]
+        public extern unsafe static Vector3* GeomGetOffsetPositionUnsafe(IntPtr geom);
+        public static Vector3 GeomGetOffsetPosition(IntPtr geom)
+        {
+            unsafe { return *(GeomGetOffsetPositionUnsafe(geom)); }
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetOffsetRotation"), SuppressUnmanagedCodeSecurity]
+        public extern unsafe static Matrix3* GeomGetOffsetRotationUnsafe(IntPtr geom);
+        public static Matrix3 GeomGetOffsetRotation(IntPtr geom)
+        {
+            unsafe { return *(GeomGetOffsetRotationUnsafe(geom)); }
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetPosition"), SuppressUnmanagedCodeSecurity]
+        public extern unsafe static Vector3* GeomGetPositionUnsafe(IntPtr geom);
+        public static Vector3 GeomGetPosition(IntPtr geom)
+        {
+            unsafe { return *(GeomGetPositionUnsafe(geom)); }
+        }
+        public static OMV.Vector3 GeomGetPositionOMV(IntPtr geom)
+        {
+            Vector3 vtmp = GeomGetPosition(geom);
+            return new OMV.Vector3(vtmp.X, vtmp.Y, vtmp.Z);
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetQuaternion"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomCopyQuaternion(IntPtr geom, out Quaternion q);
+        public static OMV.Quaternion GeomGetQuaternionOMV(IntPtr geom)
+        {
+            Quaternion qtmp;
+            GeomCopyQuaternion(geom, out qtmp);
+            return new OMV.Quaternion(qtmp.X, qtmp.Y, qtmp.Z, qtmp.W);
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetQuaternion"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomCopyQuaternion(IntPtr geom, out dReal X);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetRotation"), SuppressUnmanagedCodeSecurity]
+        public extern unsafe static Matrix3* GeomGetRotationUnsafe(IntPtr geom);
+        public static Matrix3 GeomGetRotation(IntPtr geom)
+        {
+            unsafe { return *(GeomGetRotationUnsafe(geom)); }
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetSpace"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr GeomGetSpace(IntPtr geom);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildByte"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomHeightfieldDataBuildByte(IntPtr d, byte[] pHeightData, int bCopyHeightData,
+                dReal width, dReal depth, int widthSamples, int depthSamples,
+                dReal scale, dReal offset, dReal thickness, int bWrap);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildByte"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomHeightfieldDataBuildByte(IntPtr d, IntPtr pHeightData, int bCopyHeightData,
+                dReal width, dReal depth, int widthSamples, int depthSamples,
+                dReal scale, dReal offset, dReal thickness,     int bWrap);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildCallback"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomHeightfieldDataBuildCallback(IntPtr d, IntPtr pUserData, HeightfieldGetHeight pCallback,
+                dReal width, dReal depth, int widthSamples, int depthSamples,
+                dReal scale, dReal offset, dReal thickness, int bWrap);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildShort"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomHeightfieldDataBuildShort(IntPtr d, ushort[] pHeightData, int bCopyHeightData,
+                dReal width, dReal depth, int widthSamples, int depthSamples,
+                dReal scale, dReal offset, dReal thickness, int bWrap);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildShort"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomHeightfieldDataBuildShort(IntPtr d, short[] pHeightData, int bCopyHeightData,
+                dReal width, dReal depth, int widthSamples, int depthSamples,
+                dReal scale, dReal offset, dReal thickness, int bWrap);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildShort"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomHeightfieldDataBuildShort(IntPtr d, IntPtr pHeightData, int bCopyHeightData,
+                dReal width, dReal depth, int widthSamples, int depthSamples,
+                dReal scale, dReal offset, dReal thickness, int bWrap);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildSingle"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomHeightfieldDataBuildSingle(IntPtr d, float[] pHeightData, int bCopyHeightData,
+                dReal width, dReal depth, int widthSamples, int depthSamples,
+                dReal scale, dReal offset, dReal thickness, int bWrap);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildSingle"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomHeightfieldDataBuildSingle(IntPtr d, IntPtr pHeightData, int bCopyHeightData,
+                dReal width, dReal depth, int widthSamples, int depthSamples,
+                dReal scale, dReal offset, dReal thickness, int bWrap);
+
+
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildDouble"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomHeightfieldDataBuildDouble(IntPtr d, double[] pHeightData, int bCopyHeightData,
+                dReal width, dReal depth, int widthSamples, int depthSamples,
+                dReal scale, dReal offset, dReal thickness, int bWrap);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildDouble"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomHeightfieldDataBuildDouble(IntPtr d, IntPtr pHeightData, int bCopyHeightData,
+                dReal width, dReal depth, int widthSamples, int depthSamples,
+                dReal scale, dReal offset, dReal thickness, int bWrap);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataCreate"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr GeomHeightfieldDataCreate();
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataDestroy"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomHeightfieldDataDestroy(IntPtr d);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataSetBounds"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomHeightfieldDataSetBounds(IntPtr d, dReal minHeight, dReal maxHeight);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldGetHeightfieldData"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr GeomHeightfieldGetHeightfieldData(IntPtr g);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldSetHeightfieldData"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomHeightfieldSetHeightfieldData(IntPtr g, IntPtr d);
+
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomUbitTerrainDataBuild"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomOSTerrainDataBuild(IntPtr d, float[] pHeightData, int bCopyHeightData,
+                dReal sampleSize, int widthSamples, int depthSamples,
+                dReal offset, dReal thickness, int bWrap);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomOSTerrainDataBuild"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomOSTerrainDataBuild(IntPtr d, IntPtr pHeightData, int bCopyHeightData,
+                dReal sampleSize, int widthSamples, int depthSamples,
+                dReal thickness, int bWrap);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomOSTerrainDataCreate"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr GeomOSTerrainDataCreate();
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomOSTerrainDataDestroy"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomOSTerrainDataDestroy(IntPtr d);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomOSTerrainDataSetBounds"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomOSTerrainDataSetBounds(IntPtr d, dReal minHeight, dReal maxHeight);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomOSTerrainGetHeightfieldData"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr GeomOSTerrainGetHeightfieldData(IntPtr g);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomOSTerrainSetHeightfieldData"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomOSTerrainSetHeightfieldData(IntPtr g, IntPtr d);
+
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomIsEnabled"), SuppressUnmanagedCodeSecurity]
+        public static extern bool GeomIsEnabled(IntPtr geom);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomIsOffset"), SuppressUnmanagedCodeSecurity]
+        public static extern bool GeomIsOffset(IntPtr geom);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomIsSpace"), SuppressUnmanagedCodeSecurity]
+        public static extern bool GeomIsSpace(IntPtr geom);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomPlaneGetParams"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomPlaneGetParams(IntPtr geom, ref Vector4 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomPlaneGetParams"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomPlaneGetParams(IntPtr geom, ref dReal A);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomPlanePointDepth"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal GeomPlanePointDepth(IntPtr geom, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomPlaneSetParams"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomPlaneSetParams(IntPtr plane, dReal a, dReal b, dReal c, dReal d);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGet"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomRayGet(IntPtr ray, ref Vector3 start, ref Vector3 dir);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGet"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomRayGet(IntPtr ray, ref dReal startX, ref dReal dirX);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGetClosestHit"), SuppressUnmanagedCodeSecurity]
+        public static extern int GeomRayGetClosestHit(IntPtr ray);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGetLength"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal GeomRayGetLength(IntPtr ray);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGetParams"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal GeomRayGetParams(IntPtr g, out int firstContact, out int backfaceCull);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRaySet"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomRaySet(IntPtr ray, dReal px, dReal py, dReal pz, dReal dx, dReal dy, dReal dz);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRaySetClosestHit"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomRaySetClosestHit(IntPtr ray, int closestHit);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRaySetLength"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomRaySetLength(IntPtr ray, dReal length);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRaySetParams"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomRaySetParams(IntPtr ray, int firstContact, int backfaceCull);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetBody"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomSetBody(IntPtr geom, IntPtr body);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetCategoryBits"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomSetCategoryBits(IntPtr geom, uint bits);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetCollideBits"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomSetCollideBits(IntPtr geom, uint bits);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetConvex"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr GeomSetConvex(IntPtr geom, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetData"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomSetData(IntPtr geom, IntPtr data);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetPosition"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomSetOffsetPosition(IntPtr geom, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetQuaternion"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomSetOffsetQuaternion(IntPtr geom, ref Quaternion Q);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetQuaternion"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomSetOffsetQuaternion(IntPtr geom, ref dReal X);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetRotation"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomSetOffsetRotation(IntPtr geom, ref Matrix3 R);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetRotation"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomSetOffsetRotation(IntPtr geom, ref dReal M00);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldPosition"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomSetOffsetWorldPosition(IntPtr geom, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldQuaternion"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomSetOffsetWorldQuaternion(IntPtr geom, ref Quaternion Q);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldQuaternion"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomSetOffsetWorldQuaternion(IntPtr geom, ref dReal X);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldRotation"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomSetOffsetWorldRotation(IntPtr geom, ref Matrix3 R);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldRotation"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomSetOffsetWorldRotation(IntPtr geom, ref dReal M00);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetPosition"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomSetPosition(IntPtr geom, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetQuaternion"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomSetQuaternion(IntPtr geom, ref Quaternion quat);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetQuaternion"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomSetQuaternion(IntPtr geom, ref dReal w);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetRotation"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomSetRotation(IntPtr geom, ref Matrix3 R);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetRotation"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomSetRotation(IntPtr geom, ref dReal M00);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSphereGetRadius"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal GeomSphereGetRadius(IntPtr geom);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSpherePointDepth"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal GeomSpherePointDepth(IntPtr geom, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSphereSetRadius"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomSphereSetRadius(IntPtr geom, dReal radius);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformGetCleanup"), SuppressUnmanagedCodeSecurity]
+        public static extern int GeomTransformGetCleanup(IntPtr geom);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformGetGeom"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr GeomTransformGetGeom(IntPtr geom);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformGetInfo"), SuppressUnmanagedCodeSecurity]
+        public static extern int GeomTransformGetInfo(IntPtr geom);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformSetCleanup"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTransformSetCleanup(IntPtr geom, int mode);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformSetGeom"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTransformSetGeom(IntPtr geom, IntPtr obj);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformSetInfo"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTransformSetInfo(IntPtr geom, int info);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildDouble"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTriMeshDataBuildDouble(IntPtr d,
+            double[] vertices, int vertexStride, int vertexCount,
+            int[] indices, int indexCount, int triStride);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildDouble"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTriMeshDataBuildDouble(IntPtr d,
+            IntPtr vertices, int vertexStride, int vertexCount,
+            IntPtr indices, int indexCount, int triStride);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildDouble1"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTriMeshDataBuildDouble1(IntPtr d,
+            double[] vertices, int vertexStride, int vertexCount,
+            int[] indices, int indexCount, int triStride,
+            double[] normals);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildDouble1"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTriMeshDataBuildDouble(IntPtr d,
+            IntPtr vertices, int vertexStride, int vertexCount,
+            IntPtr indices, int indexCount, int triStride,
+            IntPtr normals);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSimple"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTriMeshDataBuildSingle(IntPtr d,
+            dReal[] vertices, int vertexStride, int vertexCount,
+            int[] indices, int indexCount, int triStride);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSimple"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTriMeshDataBuildSingle(IntPtr d,
+            IntPtr vertices, int vertexStride, int vertexCount,
+            IntPtr indices, int indexCount, int triStride);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSimple1"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTriMeshDataBuildSingle1(IntPtr d,
+            dReal[] vertices, int vertexStride, int vertexCount,
+            int[] indices, int indexCount, int triStride,
+            dReal[] normals);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSimple1"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTriMeshDataBuildSingle1(IntPtr d,
+            IntPtr vertices, int vertexStride, int vertexCount,
+            IntPtr indices, int indexCount, int triStride,
+            IntPtr normals);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSingle"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTriMeshDataBuildSimple(IntPtr d,
+            float[] vertices, int vertexStride, int vertexCount,
+            int[] indices, int indexCount, int triStride);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSingle"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTriMeshDataBuildSimple(IntPtr d,
+            IntPtr vertices, int vertexStride, int vertexCount,
+            IntPtr indices, int indexCount, int triStride);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSingle1"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTriMeshDataBuildSimple1(IntPtr d,
+            float[] vertices, int vertexStride, int vertexCount,
+            int[] indices, int indexCount, int triStride,
+            float[] normals);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSingle1"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTriMeshDataBuildSimple1(IntPtr d,
+            IntPtr vertices, int vertexStride, int vertexCount,
+            IntPtr indices, int indexCount, int triStride,
+            IntPtr normals);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshClearTCCache"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTriMeshClearTCCache(IntPtr g);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataCreate"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr GeomTriMeshDataCreate();
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataDestroy"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTriMeshDataDestroy(IntPtr d);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataGet"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr GeomTriMeshDataGet(IntPtr d, int data_id);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataPreprocess"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTriMeshDataPreprocess(IntPtr d);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataSet"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTriMeshDataSet(IntPtr d, int data_id, IntPtr in_data);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataUpdate"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTriMeshDataUpdate(IntPtr d);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshEnableTC"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTriMeshEnableTC(IntPtr g, int geomClass, bool enable);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetArrayCallback"), SuppressUnmanagedCodeSecurity]
+        public static extern TriArrayCallback GeomTriMeshGetArrayCallback(IntPtr g);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetCallback"), SuppressUnmanagedCodeSecurity]
+        public static extern TriCallback GeomTriMeshGetCallback(IntPtr g);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetData"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr GeomTriMeshGetData(IntPtr g);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetLastTransform"), SuppressUnmanagedCodeSecurity]
+        public extern unsafe static Matrix4* GeomTriMeshGetLastTransformUnsafe(IntPtr geom);
+        public static Matrix4 GeomTriMeshGetLastTransform(IntPtr geom)
+        {
+            unsafe { return *(GeomTriMeshGetLastTransformUnsafe(geom)); }
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetPoint"), SuppressUnmanagedCodeSecurity]
+        public extern static void GeomTriMeshGetPoint(IntPtr g, int index, dReal u, dReal v, ref Vector3 outVec);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetRayCallback"), SuppressUnmanagedCodeSecurity]
+        public static extern TriRayCallback GeomTriMeshGetRayCallback(IntPtr g);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetTriangle"), SuppressUnmanagedCodeSecurity]
+        public extern static void GeomTriMeshGetTriangle(IntPtr g, int index, ref Vector3 v0, ref Vector3 v1, ref Vector3 v2);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetTriangleCount"), SuppressUnmanagedCodeSecurity]
+        public extern static int GeomTriMeshGetTriangleCount(IntPtr g);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetTriMeshDataID"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr GeomTriMeshGetTriMeshDataID(IntPtr g);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshIsTCEnabled"), SuppressUnmanagedCodeSecurity]
+        public static extern bool GeomTriMeshIsTCEnabled(IntPtr g, int geomClass);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetArrayCallback"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTriMeshSetArrayCallback(IntPtr g, TriArrayCallback arrayCallback);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetCallback"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTriMeshSetCallback(IntPtr g, TriCallback callback);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetData"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTriMeshSetData(IntPtr g, IntPtr data);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetLastTransform"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTriMeshSetLastTransform(IntPtr g, ref Matrix4 last_trans);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetLastTransform"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTriMeshSetLastTransform(IntPtr g, ref dReal M00);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetRayCallback"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTriMeshSetRayCallback(IntPtr g, TriRayCallback callback);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGetConfiguration"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr iGetConfiguration();
+
+        public static string GetConfiguration()
+        {
+            IntPtr ptr = iGetConfiguration();
+            string s = Marshal.PtrToStringAnsi(ptr);
+            return s;
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dHashSpaceCreate"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr HashSpaceCreate(IntPtr space);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dHashSpaceGetLevels"), SuppressUnmanagedCodeSecurity]
+        public static extern void HashSpaceGetLevels(IntPtr space, out int minlevel, out int maxlevel);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dHashSpaceSetLevels"), SuppressUnmanagedCodeSecurity]
+        public static extern void HashSpaceSetLevels(IntPtr space, int minlevel, int maxlevel);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dInfiniteAABB"), SuppressUnmanagedCodeSecurity]
+        public static extern void InfiniteAABB(IntPtr geom, out AABB aabb);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dInitODE"), SuppressUnmanagedCodeSecurity]
+        public static extern void InitODE();
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dInitODE2"), SuppressUnmanagedCodeSecurity]
+        public static extern int InitODE2(uint ODEInitFlags);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dIsPositiveDefinite"), SuppressUnmanagedCodeSecurity]
+        public static extern int IsPositiveDefinite(ref dReal A, int n);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dInvertPDMatrix"), SuppressUnmanagedCodeSecurity]
+        public static extern int InvertPDMatrix(ref dReal A, out dReal Ainv, int n);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddAMotorTorques"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointAddAMotorTorques(IntPtr joint, dReal torque1, dReal torque2, dReal torque3);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddHingeTorque"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointAddHingeTorque(IntPtr joint, dReal torque);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddHinge2Torque"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointAddHinge2Torques(IntPtr joint, dReal torque1, dReal torque2);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddPRTorque"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointAddPRTorque(IntPtr joint, dReal torque);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddUniversalTorque"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointAddUniversalTorques(IntPtr joint, dReal torque1, dReal torque2);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddSliderForce"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointAddSliderForce(IntPtr joint, dReal force);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAttach"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointAttach(IntPtr joint, IntPtr body1, IntPtr body2);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateAMotor"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr JointCreateAMotor(IntPtr world, IntPtr group);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateBall"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr JointCreateBall(IntPtr world, IntPtr group);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateContact"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr JointCreateContact(IntPtr world, IntPtr group, ref Contact contact);
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateContact"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr JointCreateContactPtr(IntPtr world, IntPtr group, IntPtr contact);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateFixed"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr JointCreateFixed(IntPtr world, IntPtr group);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateHinge"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr JointCreateHinge(IntPtr world, IntPtr group);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateHinge2"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr JointCreateHinge2(IntPtr world, IntPtr group);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateLMotor"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr JointCreateLMotor(IntPtr world, IntPtr group);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateNull"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr JointCreateNull(IntPtr world, IntPtr group);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreatePR"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr JointCreatePR(IntPtr world, IntPtr group);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreatePlane2D"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr JointCreatePlane2D(IntPtr world, IntPtr group);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateSlider"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr JointCreateSlider(IntPtr world, IntPtr group);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateUniversal"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr JointCreateUniversal(IntPtr world, IntPtr group);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointDestroy"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointDestroy(IntPtr j);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorAngle"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal JointGetAMotorAngle(IntPtr j, int anum);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorAngleRate"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal JointGetAMotorAngleRate(IntPtr j, int anum);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorAxis"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointGetAMotorAxis(IntPtr j, int anum, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorAxisRel"), SuppressUnmanagedCodeSecurity]
+        public static extern int JointGetAMotorAxisRel(IntPtr j, int anum);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorMode"), SuppressUnmanagedCodeSecurity]
+        public static extern int JointGetAMotorMode(IntPtr j);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorNumAxes"), SuppressUnmanagedCodeSecurity]
+        public static extern int JointGetAMotorNumAxes(IntPtr j);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorParam"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal JointGetAMotorParam(IntPtr j, int parameter);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetBallAnchor"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointGetBallAnchor(IntPtr j, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetBallAnchor2"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointGetBallAnchor2(IntPtr j, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetBody"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr JointGetBody(IntPtr j);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetData"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr JointGetData(IntPtr j);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetFeedback"), SuppressUnmanagedCodeSecurity]
+        public extern unsafe static JointFeedback* JointGetFeedbackUnsafe(IntPtr j);
+        public static JointFeedback JointGetFeedback(IntPtr j)
+        {
+            unsafe { return *(JointGetFeedbackUnsafe(j)); }
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAnchor"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointGetHingeAnchor(IntPtr j, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAngle"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal JointGetHingeAngle(IntPtr j);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAngleRate"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal JointGetHingeAngleRate(IntPtr j);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAxis"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointGetHingeAxis(IntPtr j, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeParam"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal JointGetHingeParam(IntPtr j, int parameter);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Angle1"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal JointGetHinge2Angle1(IntPtr j);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Angle1Rate"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal JointGetHinge2Angle1Rate(IntPtr j);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Angle2Rate"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal JointGetHinge2Angle2Rate(IntPtr j);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAnchor2"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointGetHingeAnchor2(IntPtr j, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Anchor"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointGetHinge2Anchor(IntPtr j, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Anchor2"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointGetHinge2Anchor2(IntPtr j, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Axis1"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointGetHinge2Axis1(IntPtr j, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Axis2"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointGetHinge2Axis2(IntPtr j, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Param"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal JointGetHinge2Param(IntPtr j, int parameter);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetLMotorAxis"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointGetLMotorAxis(IntPtr j, int anum, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetLMotorNumAxes"), SuppressUnmanagedCodeSecurity]
+        public static extern int JointGetLMotorNumAxes(IntPtr j);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetLMotorParam"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal JointGetLMotorParam(IntPtr j, int parameter);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRAnchor"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointGetPRAnchor(IntPtr j, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRAxis1"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointGetPRAxis1(IntPtr j, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRAxis2"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointGetPRAxis2(IntPtr j, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRParam"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal JointGetPRParam(IntPtr j, int parameter);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRPosition"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal JointGetPRPosition(IntPtr j);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRPositionRate"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal JointGetPRPositionRate(IntPtr j);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetSliderAxis"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointGetSliderAxis(IntPtr j, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetSliderParam"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal JointGetSliderParam(IntPtr j, int parameter);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetSliderPosition"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal JointGetSliderPosition(IntPtr j);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetSliderPositionRate"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal JointGetSliderPositionRate(IntPtr j);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetType"), SuppressUnmanagedCodeSecurity]
+        public static extern JointType JointGetType(IntPtr j);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAnchor"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointGetUniversalAnchor(IntPtr j, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAnchor2"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointGetUniversalAnchor2(IntPtr j, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngle1"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal JointGetUniversalAngle1(IntPtr j);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngle1Rate"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal JointGetUniversalAngle1Rate(IntPtr j);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngle2"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal JointGetUniversalAngle2(IntPtr j);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngle2Rate"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal JointGetUniversalAngle2Rate(IntPtr j);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngles"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointGetUniversalAngles(IntPtr j, out dReal angle1, out dReal angle2);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAxis1"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointGetUniversalAxis1(IntPtr j, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAxis2"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointGetUniversalAxis2(IntPtr j, out Vector3 result);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalParam"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal JointGetUniversalParam(IntPtr j, int parameter);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGroupCreate"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr JointGroupCreate(int max_size);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGroupDestroy"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointGroupDestroy(IntPtr group);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGroupEmpty"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointGroupEmpty(IntPtr group);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorAngle"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetAMotorAngle(IntPtr j, int anum, dReal angle);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorAxis"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetAMotorAxis(IntPtr j, int anum, int rel, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorMode"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetAMotorMode(IntPtr j, int mode);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorNumAxes"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetAMotorNumAxes(IntPtr group, int num);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorParam"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetAMotorParam(IntPtr group, int parameter, dReal value);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetBallAnchor"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetBallAnchor(IntPtr j, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetBallAnchor2"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetBallAnchor2(IntPtr j, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetData"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetData(IntPtr j, IntPtr data);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetFeedback"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetFeedback(IntPtr j, out JointFeedback feedback);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetFixed"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetFixed(IntPtr j);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHingeAnchor"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetHingeAnchor(IntPtr j, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHingeAnchorDelta"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetHingeAnchorDelta(IntPtr j, dReal x, dReal y, dReal z, dReal ax, dReal ay, dReal az);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHingeAxis"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetHingeAxis(IntPtr j, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHingeParam"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetHingeParam(IntPtr j, int parameter, dReal value);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHinge2Anchor"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetHinge2Anchor(IntPtr j, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHinge2Axis1"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetHinge2Axis1(IntPtr j, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHinge2Axis2"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetHinge2Axis2(IntPtr j, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHinge2Param"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetHinge2Param(IntPtr j, int parameter, dReal value);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetLMotorAxis"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetLMotorAxis(IntPtr j, int anum, int rel, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetLMotorNumAxes"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetLMotorNumAxes(IntPtr j, int num);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetLMotorParam"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetLMotorParam(IntPtr j, int parameter, dReal value);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPlane2DAngleParam"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetPlane2DAngleParam(IntPtr j, int parameter, dReal value);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPlane2DXParam"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetPlane2DXParam(IntPtr j, int parameter, dReal value);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPlane2DYParam"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetPlane2DYParam(IntPtr j, int parameter, dReal value);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPRAnchor"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetPRAnchor(IntPtr j, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPRAxis1"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetPRAxis1(IntPtr j, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPRAxis2"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetPRAxis2(IntPtr j, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPRParam"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetPRParam(IntPtr j, int parameter, dReal value);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetSliderAxis"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetSliderAxis(IntPtr j, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetSliderAxisDelta"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetSliderAxisDelta(IntPtr j, dReal x, dReal y, dReal z, dReal ax, dReal ay, dReal az);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetSliderParam"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetSliderParam(IntPtr j, int parameter, dReal value);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetUniversalAnchor"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetUniversalAnchor(IntPtr j, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetUniversalAxis1"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetUniversalAxis1(IntPtr j, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetUniversalAxis2"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetUniversalAxis2(IntPtr j, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetUniversalParam"), SuppressUnmanagedCodeSecurity]
+        public static extern void JointSetUniversalParam(IntPtr j, int parameter, dReal value);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dLDLTAddTL"), SuppressUnmanagedCodeSecurity]
+        public static extern void LDLTAddTL(ref dReal L, ref dReal d, ref dReal a, int n, int nskip);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassAdd"), SuppressUnmanagedCodeSecurity]
+        public static extern void MassAdd(ref Mass a, ref Mass b);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassAdjust"), SuppressUnmanagedCodeSecurity]
+        public static extern void MassAdjust(ref Mass m, dReal newmass);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassCheck"), SuppressUnmanagedCodeSecurity]
+        public static extern bool MassCheck(ref Mass m);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassRotate"), SuppressUnmanagedCodeSecurity]
+        public static extern void MassRotate(ref Mass mass, ref Matrix3 R);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassRotate"), SuppressUnmanagedCodeSecurity]
+        public static extern void MassRotate(ref Mass mass, ref dReal M00);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetBox"), SuppressUnmanagedCodeSecurity]
+        public static extern void MassSetBox(out Mass mass, dReal density, dReal lx, dReal ly, dReal lz);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetBoxTotal"), SuppressUnmanagedCodeSecurity]
+        public static extern void MassSetBoxTotal(out Mass mass, dReal total_mass, dReal lx, dReal ly, dReal lz);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetCapsule"), SuppressUnmanagedCodeSecurity]
+        public static extern void MassSetCapsule(out Mass mass, dReal density, int direction, dReal radius, dReal length);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetCapsuleTotal"), SuppressUnmanagedCodeSecurity]
+        public static extern void MassSetCapsuleTotal(out Mass mass, dReal total_mass, int direction, dReal radius, dReal length);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetCylinder"), SuppressUnmanagedCodeSecurity]
+        public static extern void MassSetCylinder(out Mass mass, dReal density, int direction, dReal radius, dReal length);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetCylinderTotal"), SuppressUnmanagedCodeSecurity]
+        public static extern void MassSetCylinderTotal(out Mass mass, dReal total_mass, int direction, dReal radius, dReal length);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetParameters"), SuppressUnmanagedCodeSecurity]
+        public static extern void MassSetParameters(out Mass mass, dReal themass,
+             dReal cgx, dReal cgy, dReal cgz,
+             dReal i11, dReal i22, dReal i33,
+             dReal i12, dReal i13, dReal i23);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetSphere"), SuppressUnmanagedCodeSecurity]
+        public static extern void MassSetSphere(out Mass mass, dReal density, dReal radius);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetSphereTotal"), SuppressUnmanagedCodeSecurity]
+        public static extern void dMassSetSphereTotal(out Mass mass, dReal total_mass, dReal radius);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetTrimesh"), SuppressUnmanagedCodeSecurity]
+        public static extern void MassSetTrimesh(out Mass mass, dReal density, IntPtr g);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetZero"), SuppressUnmanagedCodeSecurity]
+        public static extern void MassSetZero(out Mass mass);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassTranslate"), SuppressUnmanagedCodeSecurity]
+        public static extern void MassTranslate(ref Mass mass, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMultiply0"), SuppressUnmanagedCodeSecurity]
+        public static extern void Multiply0(out dReal A00, ref dReal B00, ref dReal C00, int p, int q, int r);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMultiply0"), SuppressUnmanagedCodeSecurity]
+        private static extern void MultiplyiM3V3(out Vector3 vout, ref Matrix3 matrix, ref Vector3 vect,int p, int q, int r);
+        public static void MultiplyM3V3(out Vector3 outvector, ref Matrix3 matrix, ref Vector3 invector)
+        {
+            MultiplyiM3V3(out outvector, ref matrix, ref invector, 3, 3, 1);
+        }
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMultiply1"), SuppressUnmanagedCodeSecurity]
+        public static extern void Multiply1(out dReal A00, ref dReal B00, ref dReal C00, int p, int q, int r);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMultiply2"), SuppressUnmanagedCodeSecurity]
+        public static extern void Multiply2(out dReal A00, ref dReal B00, ref dReal C00, int p, int q, int r);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQFromAxisAndAngle"), SuppressUnmanagedCodeSecurity]
+        public static extern void QFromAxisAndAngle(out Quaternion q, dReal ax, dReal ay, dReal az, dReal angle);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQfromR"), SuppressUnmanagedCodeSecurity]
+        public static extern void QfromR(out Quaternion q, ref Matrix3 R);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQMultiply0"), SuppressUnmanagedCodeSecurity]
+        public static extern void QMultiply0(out Quaternion qa, ref Quaternion qb, ref Quaternion qc);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQMultiply1"), SuppressUnmanagedCodeSecurity]
+        public static extern void QMultiply1(out Quaternion qa, ref Quaternion qb, ref Quaternion qc);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQMultiply2"), SuppressUnmanagedCodeSecurity]
+        public static extern void QMultiply2(out Quaternion qa, ref Quaternion qb, ref Quaternion qc);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQMultiply3"), SuppressUnmanagedCodeSecurity]
+        public static extern void QMultiply3(out Quaternion qa, ref Quaternion qb, ref Quaternion qc);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQSetIdentity"), SuppressUnmanagedCodeSecurity]
+        public static extern void QSetIdentity(out Quaternion q);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQuadTreeSpaceCreate"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr QuadTreeSpaceCreate(IntPtr space, ref Vector3 center, ref Vector3 extents, int depth);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQuadTreeSpaceCreate"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr QuadTreeSpaceCreate(IntPtr space, ref dReal centerX, ref dReal extentsX, int depth);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRandReal"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal RandReal();
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRFrom2Axes"), SuppressUnmanagedCodeSecurity]
+        public static extern void RFrom2Axes(out Matrix3 R, dReal ax, dReal ay, dReal az, dReal bx, dReal by, dReal bz);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRFromAxisAndAngle"), SuppressUnmanagedCodeSecurity]
+        public static extern void RFromAxisAndAngle(out Matrix3 R, dReal x, dReal y, dReal z, dReal angle);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRFromEulerAngles"), SuppressUnmanagedCodeSecurity]
+        public static extern void RFromEulerAngles(out Matrix3 R, dReal phi, dReal theta, dReal psi);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRfromQ"), SuppressUnmanagedCodeSecurity]
+        public static extern void RfromQ(out Matrix3 R, ref Quaternion q);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRFromZAxis"), SuppressUnmanagedCodeSecurity]
+        public static extern void RFromZAxis(out Matrix3 R, dReal ax, dReal ay, dReal az);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRSetIdentity"), SuppressUnmanagedCodeSecurity]
+        public static extern void RSetIdentity(out Matrix3 R);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSetValue"), SuppressUnmanagedCodeSecurity]
+        public static extern void SetValue(out dReal a, int n);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSetZero"), SuppressUnmanagedCodeSecurity]
+        public static extern void SetZero(out dReal a, int n);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSimpleSpaceCreate"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr SimpleSpaceCreate(IntPtr space);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSolveCholesky"), SuppressUnmanagedCodeSecurity]
+        public static extern void SolveCholesky(ref dReal L, out dReal b, int n);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSolveL1"), SuppressUnmanagedCodeSecurity]
+        public static extern void SolveL1(ref dReal L, out dReal b, int n, int nskip);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSolveL1T"), SuppressUnmanagedCodeSecurity]
+        public static extern void SolveL1T(ref dReal L, out dReal b, int n, int nskip);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSolveLDLT"), SuppressUnmanagedCodeSecurity]
+        public static extern void SolveLDLT(ref dReal L, ref dReal d, out dReal b, int n, int nskip);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceAdd"), SuppressUnmanagedCodeSecurity]
+        public static extern void SpaceAdd(IntPtr space, IntPtr geom);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceLockQuery"), SuppressUnmanagedCodeSecurity]
+        public static extern bool SpaceLockQuery(IntPtr space);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceClean"), SuppressUnmanagedCodeSecurity]
+        public static extern void SpaceClean(IntPtr space);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceCollide"), SuppressUnmanagedCodeSecurity]
+        public static extern void SpaceCollide(IntPtr space, IntPtr data, NearCallback callback);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceCollide2"), SuppressUnmanagedCodeSecurity]
+        public static extern void SpaceCollide2(IntPtr space1, IntPtr space2, IntPtr data, NearCallback callback);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceDestroy"), SuppressUnmanagedCodeSecurity]
+        public static extern void SpaceDestroy(IntPtr space);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceGetCleanup"), SuppressUnmanagedCodeSecurity]
+        public static extern bool SpaceGetCleanup(IntPtr space);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceGetNumGeoms"), SuppressUnmanagedCodeSecurity]
+        public static extern int SpaceGetNumGeoms(IntPtr space);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceGetGeom"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr SpaceGetGeom(IntPtr space, int i);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceGetSublevel"), SuppressUnmanagedCodeSecurity]
+        public static extern int SpaceGetSublevel(IntPtr space);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceQuery"), SuppressUnmanagedCodeSecurity]
+        public static extern bool SpaceQuery(IntPtr space, IntPtr geom);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceRemove"), SuppressUnmanagedCodeSecurity]
+        public static extern void SpaceRemove(IntPtr space, IntPtr geom);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceSetCleanup"), SuppressUnmanagedCodeSecurity]
+        public static extern void SpaceSetCleanup(IntPtr space, bool mode);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceSetSublevel"), SuppressUnmanagedCodeSecurity]
+        public static extern void SpaceSetSublevel(IntPtr space, int sublevel);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSweepAndPruneSpaceCreate"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr SweepAndPruneSpaceCreate(IntPtr space, int AxisOrder);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dVectorScale"), SuppressUnmanagedCodeSecurity]
+        public static extern void VectorScale(out dReal a, ref dReal d, int n);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldCreate"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr WorldCreate();
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldDestroy"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldDestroy(IntPtr world);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableAverageSamplesCount"), SuppressUnmanagedCodeSecurity]
+        public static extern int WorldGetAutoDisableAverageSamplesCount(IntPtr world);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableAngularThreshold"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal WorldGetAutoDisableAngularThreshold(IntPtr world);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableFlag"), SuppressUnmanagedCodeSecurity]
+        public static extern bool WorldGetAutoDisableFlag(IntPtr world);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableLinearThreshold"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal WorldGetAutoDisableLinearThreshold(IntPtr world);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableSteps"), SuppressUnmanagedCodeSecurity]
+        public static extern int WorldGetAutoDisableSteps(IntPtr world);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableTime"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal WorldGetAutoDisableTime(IntPtr world);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoEnableDepthSF1"), SuppressUnmanagedCodeSecurity]
+        public static extern int WorldGetAutoEnableDepthSF1(IntPtr world);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetCFM"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal WorldGetCFM(IntPtr world);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetERP"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal WorldGetERP(IntPtr world);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetGravity"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldGetGravity(IntPtr world, out Vector3 gravity);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetGravity"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldGetGravity(IntPtr world, out dReal X);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetContactMaxCorrectingVel"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal WorldGetContactMaxCorrectingVel(IntPtr world);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetContactSurfaceLayer"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal WorldGetContactSurfaceLayer(IntPtr world);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAngularDamping"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal WorldGetAngularDamping(IntPtr world);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAngularDampingThreshold"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal WorldGetAngularDampingThreshold(IntPtr world);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetLinearDamping"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal WorldGetLinearDamping(IntPtr world);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetLinearDampingThreshold"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal WorldGetLinearDampingThreshold(IntPtr world);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetQuickStepNumIterations"), SuppressUnmanagedCodeSecurity]
+        public static extern int WorldGetQuickStepNumIterations(IntPtr world);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetQuickStepW"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal WorldGetQuickStepW(IntPtr world);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetMaxAngularSpeed"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal WorldGetMaxAngularSpeed(IntPtr world);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldImpulseToForce"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldImpulseToForce(IntPtr world, dReal stepsize, dReal ix, dReal iy, dReal iz, out Vector3 force);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldImpulseToForce"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldImpulseToForce(IntPtr world, dReal stepsize, dReal ix, dReal iy, dReal iz, out dReal forceX);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldQuickStep"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldQuickStep(IntPtr world, dReal stepsize);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAngularDamping"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldSetAngularDamping(IntPtr world, dReal scale);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAngularDampingThreshold"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldSetAngularDampingThreshold(IntPtr world, dReal threshold);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableAngularThreshold"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldSetAutoDisableAngularThreshold(IntPtr world, dReal angular_threshold);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableAverageSamplesCount"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldSetAutoDisableAverageSamplesCount(IntPtr world, int average_samples_count);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableFlag"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldSetAutoDisableFlag(IntPtr world, bool do_auto_disable);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableLinearThreshold"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldSetAutoDisableLinearThreshold(IntPtr world, dReal linear_threshold);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableSteps"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldSetAutoDisableSteps(IntPtr world, int steps);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableTime"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldSetAutoDisableTime(IntPtr world, dReal time);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoEnableDepthSF1"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldSetAutoEnableDepthSF1(IntPtr world, int autoEnableDepth);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetCFM"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldSetCFM(IntPtr world, dReal cfm);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetContactMaxCorrectingVel"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldSetContactMaxCorrectingVel(IntPtr world, dReal vel);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetContactSurfaceLayer"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldSetContactSurfaceLayer(IntPtr world, dReal depth);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetDamping"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldSetDamping(IntPtr world, dReal linear_scale, dReal angular_scale);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetERP"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldSetERP(IntPtr world, dReal erp);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetGravity"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldSetGravity(IntPtr world, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetLinearDamping"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldSetLinearDamping(IntPtr world, dReal scale);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetLinearDampingThreshold"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldSetLinearDampingThreshold(IntPtr world, dReal threshold);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetQuickStepNumIterations"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldSetQuickStepNumIterations(IntPtr world, int num);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetQuickStepW"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldSetQuickStepW(IntPtr world, dReal over_relaxation);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetMaxAngularSpeed"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldSetMaxAngularSpeed(IntPtr world, dReal max_speed);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldStep"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldStep(IntPtr world, dReal stepsize);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldStepFast1"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldStepFast1(IntPtr world, dReal stepsize, int maxiterations);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldExportDIF"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldExportDIF(IntPtr world, string filename, bool append, string prefix);
+    }
+}
diff --git a/OpenSim/Region/PhysicsModules/ubOde/ODECharacter.cs b/OpenSim/Region/PhysicsModules/ubOde/ODECharacter.cs
new file mode 100644
index 0000000..0e46471
--- /dev/null
+++ b/OpenSim/Region/PhysicsModules/ubOde/ODECharacter.cs
@@ -0,0 +1,2036 @@
+/*
+ * Copyright (c) Contributors, http://opensimulator.org/
+ * See CONTRIBUTORS.TXT for a full list of copyright holders.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the OpenSimulator Project nor the
+ *       names of its contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+
+// Revision by Ubit 2011/12
+
+using System;
+using System.Collections.Generic;
+using System.Reflection;
+using OpenMetaverse;
+using OdeAPI;
+using OpenSim.Framework;
+using OpenSim.Region.PhysicsModules.SharedBase;
+using log4net;
+
+namespace OpenSim.Region.PhysicsModule.ubOde
+{
+    /// <summary>
+    /// Various properties that ODE uses for AMotors but isn't exposed in ODE.NET so we must define them ourselves.
+    /// </summary>
+
+    public enum dParam : int
+    {
+        LowStop = 0,
+        HiStop = 1,
+        Vel = 2,
+        FMax = 3,
+        FudgeFactor = 4,
+        Bounce = 5,
+        CFM = 6,
+        StopERP = 7,
+        StopCFM = 8,
+        LoStop2 = 256,
+        HiStop2 = 257,
+        Vel2 = 258,
+        FMax2 = 259,
+        StopERP2 = 7 + 256,
+        StopCFM2 = 8 + 256,
+        LoStop3 = 512,
+        HiStop3 = 513,
+        Vel3 = 514,
+        FMax3 = 515,
+        StopERP3 = 7 + 512,
+        StopCFM3 = 8 + 512
+    }
+
+    public class OdeCharacter : PhysicsActor
+    {
+        private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
+
+        private Vector3 _position;
+        private Vector3 _zeroPosition;
+        private Vector3 _velocity;
+        private Vector3 _target_velocity;
+        private Vector3 _acceleration;
+        private Vector3 m_rotationalVelocity;
+        private Vector3 m_size;
+        private Vector3 m_collideNormal;
+        private Vector3 m_lastFallVel;
+        private Quaternion m_orientation;
+        private Quaternion m_orientation2D;
+        private float m_mass = 80f;
+        public float m_density = 60f;
+        private bool m_pidControllerActive = true;
+
+        const float basePID_D = 0.55f; // scaled for unit mass unit time (2200 /(50*80))
+        const float basePID_P = 0.225f; // scaled for unit mass unit time (900 /(50*80))
+        public float PID_D;
+        public float PID_P;
+
+        private float timeStep;
+        private float invtimeStep;
+
+        private float m_feetOffset = 0;
+        private float feetOff = 0;
+        private float boneOff = 0;
+        private float AvaAvaSizeXsq = 0.3f;
+        private float AvaAvaSizeYsq = 0.2f;
+
+        public float walkDivisor = 1.3f;
+        public float runDivisor = 0.8f;
+        private bool m_flying = false;
+        private bool m_iscolliding = false;
+        private bool m_iscollidingGround = false;
+        private bool m_iscollidingObj = false;
+        private bool m_alwaysRun = false;
+
+        private bool _zeroFlag = false;
+        private bool m_haveLastFallVel = false;
+
+        private uint m_localID = 0;
+        public bool m_returnCollisions = false;
+        // taints and their non-tainted counterparts
+        public bool m_isPhysical = false; // the current physical status
+        public float MinimumGroundFlightOffset = 3f;
+
+        private float m_buoyancy = 0f;
+
+        private bool m_freemove = false;
+
+//        private string m_name = String.Empty;
+        // other filter control
+        int m_colliderfilter = 0;
+        int m_colliderGroundfilter = 0;
+        int m_colliderObjectfilter = 0;
+
+        // Default we're a Character
+        private CollisionCategories m_collisionCategories = (CollisionCategories.Character);
+
+        // Default, Collide with Other Geometries, spaces, bodies and characters.
+        private CollisionCategories m_collisionFlags = (CollisionCategories.Character
+                                                        | CollisionCategories.Geom
+                                                        | CollisionCategories.VolumeDtc
+                                                        );
+        // we do land collisions not ode                | CollisionCategories.Land);
+        public IntPtr Body = IntPtr.Zero;
+        private ODEScene m_parent_scene;
+        private IntPtr capsule = IntPtr.Zero;
+        public IntPtr collider = IntPtr.Zero;
+
+        public IntPtr Amotor = IntPtr.Zero;
+
+        public d.Mass ShellMass;
+
+        public int m_eventsubscription = 0;
+        private int m_cureventsubscription = 0;
+        private CollisionEventUpdate CollisionEventsThisFrame = new CollisionEventUpdate();
+        private bool SentEmptyCollisionsEvent;
+
+        // unique UUID of this character object
+        public UUID m_uuid;
+        public bool bad = false;
+
+        float mu;
+
+        // HoverHeight control
+        private float m_PIDHoverHeight;
+        private float m_PIDHoverTau;
+        private bool m_useHoverPID;
+        private PIDHoverType m_PIDHoverType;
+        private float m_targetHoverHeight;
+
+
+        public OdeCharacter(uint localID, String avName, ODEScene parent_scene, Vector3 pos, Vector3 pSize, float pfeetOffset, float density, float walk_divisor, float rundivisor)
+        {
+            m_uuid = UUID.Random();
+            m_localID = localID;
+            m_parent_scene = parent_scene;
+
+            timeStep = parent_scene.ODE_STEPSIZE;
+            invtimeStep = 1 / timeStep;
+
+            if (pos.IsFinite())
+            {
+                if (pos.Z > 99999f)
+                {
+                    pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
+                }
+                if (pos.Z < -100f) // shouldn't this be 0 ?
+                {
+                    pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
+                }
+                _position = pos;
+            }
+            else
+            {
+                _position = new Vector3(((float)m_parent_scene.WorldExtents.X * 0.5f), ((float)m_parent_scene.WorldExtents.Y * 0.5f), parent_scene.GetTerrainHeightAtXY(128f, 128f) + 10f);
+                m_log.Warn("[PHYSICS]: Got NaN Position on Character Create");
+            }
+
+            m_size.X = pSize.X;
+            m_size.Y = pSize.Y;
+            m_size.Z = pSize.Z;
+
+            if(m_size.X <0.01f)
+                m_size.X = 0.01f;
+            if(m_size.Y <0.01f)
+                m_size.Y = 0.01f;
+            if(m_size.Z <0.01f)
+                m_size.Z = 0.01f;
+
+            m_feetOffset = pfeetOffset;
+            m_orientation = Quaternion.Identity;
+            m_orientation2D = Quaternion.Identity;
+            m_density = density;
+
+            // force lower density for testing
+            m_density = 3.0f;
+
+            mu = m_parent_scene.AvatarFriction;
+
+            walkDivisor = walk_divisor;
+            runDivisor = rundivisor;
+
+            m_mass = m_density * m_size.X * m_size.Y * m_size.Z; ; // sure we have a default
+
+            PID_D = basePID_D * m_mass * invtimeStep;
+            PID_P = basePID_P * m_mass * invtimeStep;
+
+            m_isPhysical = false; // current status: no ODE information exists
+
+            Name = avName;
+
+            AddChange(changes.Add, null);
+        }
+
+        public override int PhysicsActorType
+        {
+            get { return (int)ActorTypes.Agent; }
+            set { return; }
+        }
+
+        public override void getContactData(ref ContactData cdata)
+        {
+            cdata.mu = mu;
+            cdata.bounce = 0;
+            cdata.softcolide = false;
+        }
+
+        public override bool Building { get; set; }
+
+        /// <summary>
+        /// If this is set, the avatar will move faster
+        /// </summary>
+        public override bool SetAlwaysRun
+        {
+            get { return m_alwaysRun; }
+            set { m_alwaysRun = value; }
+        }
+
+        public override uint LocalID
+        {
+            get { return m_localID; }
+            set { m_localID = value; }
+        }
+
+        public override PhysicsActor ParentActor
+        {
+            get { return (PhysicsActor)this; }
+        }
+
+        public override bool Grabbed
+        {
+            set { return; }
+        }
+
+        public override bool Selected
+        {
+            set { return; }
+        }
+
+        public override float Buoyancy
+        {
+            get { return m_buoyancy; }
+            set { m_buoyancy = value; }
+        }
+
+        public override bool FloatOnWater
+        {
+            set { return; }
+        }
+
+        public override bool IsPhysical
+        {
+            get { return m_isPhysical; }
+            set { return; }
+        }
+
+        public override bool ThrottleUpdates
+        {
+            get { return false; }
+            set { return; }
+        }
+
+        public override bool Flying
+        {
+            get { return m_flying; }
+            set
+            {
+                m_flying = value;
+//                m_log.DebugFormat("[PHYSICS]: Set OdeCharacter Flying to {0}", flying);
+            }
+        }
+
+        /// <summary>
+        /// Returns if the avatar is colliding in general.
+        /// This includes the ground and objects and avatar.
+        /// </summary>
+        public override bool IsColliding
+        {
+            get { return (m_iscolliding || m_iscollidingGround); }
+            set
+            {
+                if (value)
+                {
+                    m_colliderfilter += 3;
+                    if (m_colliderfilter > 3)
+                        m_colliderfilter = 3;
+                }
+                else
+                {
+                    m_colliderfilter--;
+                    if (m_colliderfilter < 0)
+                        m_colliderfilter = 0;
+                }
+
+                if (m_colliderfilter == 0)
+                    m_iscolliding = false;
+                else
+                {
+                    m_pidControllerActive = true;
+                    m_iscolliding = true;
+                    m_freemove = false;
+                }
+            }
+        }
+
+        /// <summary>
+        /// Returns if an avatar is colliding with the ground
+        /// </summary>
+        public override bool CollidingGround
+        {
+            get { return m_iscollidingGround; }
+            set
+            {
+/*  we now control this
+                if (value)
+                    {
+                    m_colliderGroundfilter += 2;
+                    if (m_colliderGroundfilter > 2)
+                        m_colliderGroundfilter = 2;
+                    }
+                else
+                    {
+                    m_colliderGroundfilter--;
+                    if (m_colliderGroundfilter < 0)
+                        m_colliderGroundfilter = 0;
+                    }
+
+                if (m_colliderGroundfilter == 0)
+                    m_iscollidingGround = false;
+                else
+                    m_iscollidingGround = true;
+ */
+            }
+
+        }
+
+        /// <summary>
+        /// Returns if the avatar is colliding with an object
+        /// </summary>
+        public override bool CollidingObj
+        {
+            get { return m_iscollidingObj; }
+            set
+            {
+                // Ubit filter this also
+                if (value)
+                {
+                    m_colliderObjectfilter += 2;
+                    if (m_colliderObjectfilter > 2)
+                        m_colliderObjectfilter = 2;
+                }
+                else
+                {
+                    m_colliderObjectfilter--;
+                    if (m_colliderObjectfilter < 0)
+                        m_colliderObjectfilter = 0;
+                }
+
+                if (m_colliderObjectfilter == 0)
+                    m_iscollidingObj = false;
+                else
+                    m_iscollidingObj = true;
+
+//            m_iscollidingObj = value;
+
+                if (m_iscollidingObj)
+                    m_pidControllerActive = false;
+                else
+                    m_pidControllerActive = true;
+            }
+        }
+
+        /// <summary>
+        /// turn the PID controller on or off.
+        /// The PID Controller will turn on all by itself in many situations
+        /// </summary>
+        /// <param name="status"></param>
+        public void SetPidStatus(bool status)
+        {
+            m_pidControllerActive = status;
+        }
+
+        public override bool Stopped
+        {
+            get { return _zeroFlag; }
+        }
+
+        /// <summary>
+        /// This 'puts' an avatar somewhere in the physics space.
+        /// Not really a good choice unless you 'know' it's a good
+        /// spot otherwise you're likely to orbit the avatar.
+        /// </summary>
+        public override Vector3 Position
+        {
+            get { return _position; }
+            set
+            {
+                if (value.IsFinite())
+                {
+                    if (value.Z > 9999999f)
+                    {
+                        value.Z = m_parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
+                    }
+                    if (value.Z < -100f)
+                    {
+                        value.Z = m_parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
+                    }
+                    AddChange(changes.Position, value);
+                }
+                else
+                {
+                    m_log.Warn("[PHYSICS]: Got a NaN Position from Scene on a Character");
+                }
+            }
+        }
+
+        public override Vector3 RotationalVelocity
+        {
+            get { return m_rotationalVelocity; }
+            set { m_rotationalVelocity = value; }
+        }
+
+        /// <summary>
+        /// This property sets the height of the avatar only.  We use the height to make sure the avatar stands up straight
+        /// and use it to offset landings properly
+        /// </summary>
+        public override Vector3 Size
+        {
+            get
+            {
+                return m_size;
+            }
+            set
+            {
+                if (value.IsFinite())
+                {
+                    if(value.X <0.01f)
+                        value.X = 0.01f;
+                    if(value.Y <0.01f)
+                        value.Y = 0.01f;
+                    if(value.Z <0.01f)
+                        value.Z = 0.01f;
+
+                    AddChange(changes.Size, value);
+                }
+                else
+                {
+                    m_log.Warn("[PHYSICS]: Got a NaN Size from Scene on a Character");
+                }
+            }
+        }
+
+        public override void setAvatarSize(Vector3 size, float feetOffset)
+        {
+            if (size.IsFinite())
+            {
+                if (size.X < 0.01f)
+                    size.X = 0.01f;
+                if (size.Y < 0.01f)
+                    size.Y = 0.01f;
+                if (size.Z < 0.01f)
+                    size.Z = 0.01f;
+
+                strAvatarSize st = new strAvatarSize();
+                st.size = size;
+                st.offset = feetOffset;
+                AddChange(changes.AvatarSize, st);
+            }
+            else
+            {
+                m_log.Warn("[PHYSICS]: Got a NaN AvatarSize from Scene on a Character");
+            }
+
+        }
+        /// <summary>
+        /// This creates the Avatar's physical Surrogate at the position supplied
+        /// </summary>
+        /// <param name="npositionX"></param>
+        /// <param name="npositionY"></param>
+        /// <param name="npositionZ"></param>
+
+        //
+        /// <summary>
+        /// Uses the capped cyllinder volume formula to calculate the avatar's mass.
+        /// This may be used in calculations in the scene/scenepresence
+        /// </summary>
+        public override float Mass
+        {
+            get
+            {
+                return m_mass;
+            }
+        }
+        public override void link(PhysicsActor obj)
+        {
+
+        }
+
+        public override void delink()
+        {
+
+        }
+
+        public override void LockAngularMotion(byte axislocks)
+        {
+
+        }
+
+
+        public override Vector3 Force
+        {
+            get { return _target_velocity; }
+            set { return; }
+        }
+
+        public override int VehicleType
+        {
+            get { return 0; }
+            set { return; }
+        }
+
+        public override void VehicleFloatParam(int param, float value)
+        {
+
+        }
+
+        public override void VehicleVectorParam(int param, Vector3 value)
+        {
+
+        }
+
+        public override void VehicleRotationParam(int param, Quaternion rotation)
+        {
+
+        }
+
+        public override void VehicleFlags(int param, bool remove)
+        {
+
+        }
+
+        public override void SetVolumeDetect(int param)
+        {
+
+        }
+
+        public override Vector3 CenterOfMass
+        {
+            get
+            {
+                Vector3 pos = _position;
+                return pos;
+            }
+        }
+
+        public override Vector3 GeometricCenter
+        {
+            get
+            {
+                Vector3 pos = _position;
+                return pos;
+            }
+        }
+
+        public override PrimitiveBaseShape Shape
+        {
+            set { return; }
+        }
+
+        public override Vector3 rootVelocity
+        {
+            get
+            {
+                return _velocity;
+            }
+        }
+
+        public override Vector3 Velocity
+        {
+            get
+            {
+                return _velocity;
+            }
+            set
+            {
+                if (value.IsFinite())
+                {
+                    AddChange(changes.Velocity, value);
+                }
+                else
+                {
+                    m_log.Warn("[PHYSICS]: Got a NaN velocity from Scene in a Character");
+                }
+            }
+        }
+
+        public override Vector3 TargetVelocity
+        {
+            get
+            {
+                return m_targetVelocity;
+            }
+            set
+            {
+                if (value.IsFinite())
+                {
+                    AddChange(changes.TargetVelocity, value);
+                }
+                else
+                {
+                    m_log.Warn("[PHYSICS]: Got a NaN velocity from Scene in a Character");
+                }
+            }
+        }
+
+        public override Vector3 Torque
+        {
+            get { return Vector3.Zero; }
+            set { return; }
+        }
+
+        public override float CollisionScore
+        {
+            get { return 0f; }
+            set { }
+        }
+
+        public override bool Kinematic
+        {
+            get { return false; }
+            set { }
+        }
+
+        public override Quaternion Orientation
+        {
+            get { return m_orientation; }
+            set
+            {
+//                fakeori = value;
+//                givefakeori++;
+                value.Normalize();
+                AddChange(changes.Orientation, value);
+            }
+        }
+
+        public override Vector3 Acceleration
+        {
+            get { return _acceleration; }
+            set { }
+        }
+
+        public void SetAcceleration(Vector3 accel)
+        {
+            m_pidControllerActive = true;
+            _acceleration = accel;
+        }
+
+        /// <summary>
+        /// Adds the force supplied to the Target Velocity
+        /// The PID controller takes this target velocity and tries to make it a reality
+        /// </summary>
+        /// <param name="force"></param>
+        public override void AddForce(Vector3 force, bool pushforce)
+        {
+            if (force.IsFinite())
+            {
+                if (pushforce)
+                {
+                    AddChange(changes.Force, force * m_density / (m_parent_scene.ODE_STEPSIZE * 28f));
+                }
+                else
+                {
+                    AddChange(changes.TargetVelocity, force);
+                }
+            }
+            else
+            {
+                m_log.Warn("[PHYSICS]: Got a NaN force applied to a Character");
+            }
+            //m_lastUpdateSent = false;
+        }
+
+        public override void AddAngularForce(Vector3 force, bool pushforce)
+        {
+
+        }
+
+        public override void SetMomentum(Vector3 momentum)
+        {
+            if (momentum.IsFinite())
+                AddChange(changes.Momentum, momentum);
+        }
+
+
+        private void AvatarGeomAndBodyCreation(float npositionX, float npositionY, float npositionZ)
+        {
+            // sizes  one day should came from visual parameters
+            float sx = m_size.X;
+            float sy = m_size.Y;
+            float sz = m_size.Z;
+
+            float bot = -sz * 0.5f + m_feetOffset;
+            boneOff = bot + 0.3f;
+
+            float feetsz = sz * 0.45f;
+            if (feetsz > 0.6f)
+                feetsz = 0.6f;
+
+            feetOff = bot + feetsz;
+
+            AvaAvaSizeXsq = 0.4f * sx;
+            AvaAvaSizeXsq *= AvaAvaSizeXsq;
+            AvaAvaSizeYsq = 0.5f * sy;
+            AvaAvaSizeYsq *= AvaAvaSizeYsq;
+
+            m_parent_scene.waitForSpaceUnlock(m_parent_scene.CharsSpace);
+
+            collider = d.HashSpaceCreate(m_parent_scene.CharsSpace);
+            d.HashSpaceSetLevels(collider, -4, 3);
+            d.SpaceSetSublevel(collider, 3);
+            d.SpaceSetCleanup(collider, false);
+            d.GeomSetCategoryBits(collider, (uint)m_collisionCategories);
+            d.GeomSetCollideBits(collider, (uint)m_collisionFlags);
+
+            float r = m_size.X;
+            if (m_size.Y > r)
+                r = m_size.Y;
+            float l = m_size.Z - r;
+            r *= 0.5f;
+
+            capsule = d.CreateCapsule(collider, r, l);
+
+            m_mass = m_density * m_size.X * m_size.Y * m_size.Z;  // update mass
+
+            d.MassSetBoxTotal(out ShellMass, m_mass, m_size.X, m_size.Y, m_size.Z);
+
+            PID_D = basePID_D * m_mass / m_parent_scene.ODE_STEPSIZE;
+            PID_P = basePID_P * m_mass / m_parent_scene.ODE_STEPSIZE;
+
+            Body = d.BodyCreate(m_parent_scene.world);
+
+            _zeroFlag = false;
+            m_pidControllerActive = true;
+            m_freemove = false;
+
+            _velocity = Vector3.Zero;
+
+            d.BodySetAutoDisableFlag(Body, false);
+            d.BodySetPosition(Body, npositionX, npositionY, npositionZ);
+
+            _position.X = npositionX;
+            _position.Y = npositionY;
+            _position.Z = npositionZ;
+
+            d.BodySetMass(Body, ref ShellMass);
+            d.GeomSetBody(capsule, Body);
+
+            // The purpose of the AMotor here is to keep the avatar's physical
+            // surrogate from rotating while moving
+            Amotor = d.JointCreateAMotor(m_parent_scene.world, IntPtr.Zero);
+            d.JointAttach(Amotor, Body, IntPtr.Zero);
+
+            d.JointSetAMotorMode(Amotor, 0);
+            d.JointSetAMotorNumAxes(Amotor, 3);
+            d.JointSetAMotorAxis(Amotor, 0, 0, 1, 0, 0);
+            d.JointSetAMotorAxis(Amotor, 1, 0, 0, 1, 0);
+            d.JointSetAMotorAxis(Amotor, 2, 0, 0, 0, 1);
+
+            d.JointSetAMotorAngle(Amotor, 0, 0);
+            d.JointSetAMotorAngle(Amotor, 1, 0);
+            d.JointSetAMotorAngle(Amotor, 2, 0);
+
+            d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM, 0f); // make it HARD
+            d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM2, 0f);
+            d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM3, 0f);
+            d.JointSetAMotorParam(Amotor, (int)dParam.StopERP, 0.8f);
+            d.JointSetAMotorParam(Amotor, (int)dParam.StopERP2, 0.8f);
+            d.JointSetAMotorParam(Amotor, (int)dParam.StopERP3, 0.8f);
+
+            // These lowstops and high stops are effectively (no wiggle room)
+            d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, -1e-5f);
+            d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 1e-5f);
+            d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, -1e-5f);
+            d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 1e-5f);
+            d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -1e-5f);
+            d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 1e-5f);
+
+            d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel, 0);
+            d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel2, 0);
+            d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel3, 0);
+
+            d.JointSetAMotorParam(Amotor, (int)dParam.FMax, 5e8f);
+            d.JointSetAMotorParam(Amotor, (int)dParam.FMax2, 5e8f);
+            d.JointSetAMotorParam(Amotor, (int)dParam.FMax3, 5e8f);
+        }
+
+        /// <summary>
+        /// Destroys the avatar body and geom
+
+        private void AvatarGeomAndBodyDestroy()
+        {
+            // Kill the Amotor
+            if (Amotor != IntPtr.Zero)
+            {
+                d.JointDestroy(Amotor);
+                Amotor = IntPtr.Zero;
+            }
+
+            if (Body != IntPtr.Zero)
+            {
+                //kill the body
+                d.BodyDestroy(Body);
+                Body = IntPtr.Zero;
+            }
+
+            //kill the Geoms
+            if (capsule != IntPtr.Zero)
+            {
+                m_parent_scene.actor_name_map.Remove(capsule);
+                m_parent_scene.waitForSpaceUnlock(collider);
+                d.GeomDestroy(capsule);
+                capsule = IntPtr.Zero;
+            }
+
+            if (collider != IntPtr.Zero)
+            {
+                d.SpaceDestroy(collider);
+                collider = IntPtr.Zero;
+            }
+
+        }
+
+        //in place 2D rotation around Z assuming rot is normalised and is a rotation around Z
+        public void RotateXYonZ(ref float x, ref float y, ref Quaternion rot)
+        {
+            float sin = 2.0f * rot.Z * rot.W;
+            float cos = rot.W * rot.W - rot.Z * rot.Z;
+            float tx = x;
+
+            x = tx * cos - y * sin;
+            y = tx * sin + y * cos;
+        }
+        public void RotateXYonZ(ref float x, ref float y, ref float sin, ref float cos)
+        {
+            float tx = x;
+            x = tx * cos - y * sin;
+            y = tx * sin + y * cos;
+        }
+        public void invRotateXYonZ(ref float x, ref float y, ref float sin, ref float cos)
+        {
+            float tx = x;
+            x = tx * cos + y * sin;
+            y = -tx * sin + y * cos;
+        }
+
+        public void invRotateXYonZ(ref float x, ref float y, ref Quaternion rot)
+        {
+            float sin = - 2.0f * rot.Z * rot.W;
+            float cos = rot.W * rot.W - rot.Z * rot.Z;
+            float tx = x;
+
+            x = tx * cos - y * sin;
+            y = tx * sin + y * cos;
+        }
+
+            public bool Collide(IntPtr me, IntPtr other, bool reverse, ref d.ContactGeom contact,
+                ref d.ContactGeom altContact , ref bool useAltcontact, ref bool feetcollision)
+            {
+            feetcollision = false;
+            useAltcontact = false;
+
+            if (me == capsule)
+            {
+                Vector3 offset;
+
+                float h = contact.pos.Z - _position.Z;
+                offset.Z = h - feetOff;
+
+                offset.X = contact.pos.X - _position.X;
+                offset.Y = contact.pos.Y - _position.Y;
+
+                d.GeomClassID gtype = d.GeomGetClass(other);
+                if (gtype == d.GeomClassID.CapsuleClass)
+                {
+                    Vector3 roff = offset * Quaternion.Inverse(m_orientation2D);
+                    float r = roff.X *roff.X / AvaAvaSizeXsq;
+                    r += (roff.Y * roff.Y) / AvaAvaSizeYsq;
+                    if (r > 1.0f)
+                        return false;
+
+                    float dp = 1.0f -(float)Math.Sqrt((double)r);
+                    if (dp > 0.05f)
+                        dp = 0.05f;
+
+                    contact.depth = dp;
+
+                    if (offset.Z < 0)
+                    {
+                        feetcollision = true;
+                        if (h < boneOff)
+                        {
+                            m_collideNormal.X = contact.normal.X;
+                            m_collideNormal.Y = contact.normal.Y;
+                            m_collideNormal.Z = contact.normal.Z;
+                            IsColliding = true;
+                        }
+                    }
+                    return true;
+                }
+
+                if (gtype == d.GeomClassID.SphereClass && d.GeomGetBody(other) != IntPtr.Zero)
+                {
+                    if(d.GeomSphereGetRadius(other) < 0.5)
+                        return true;
+                }
+
+                if (offset.Z > 0 || contact.normal.Z > 0.35f)
+                {
+                    if (offset.Z <= 0)
+                    {
+                        feetcollision = true;
+                        if (h < boneOff)
+                        {
+                            m_collideNormal.X = contact.normal.X;
+                            m_collideNormal.Y = contact.normal.Y;
+                            m_collideNormal.Z = contact.normal.Z;
+                            IsColliding = true;
+                        }
+                    }
+                    return true;
+                }
+
+                if(m_flying)
+                    return true;
+
+                feetcollision = true;
+                if (h < boneOff)
+                {
+                    m_collideNormal.X = contact.normal.X;
+                    m_collideNormal.Y = contact.normal.Y;
+                    m_collideNormal.Z = contact.normal.Z;
+                    IsColliding = true;
+                }
+
+                altContact = contact;
+                useAltcontact = true;
+
+                offset.Z -= 0.2f;
+
+                offset.Normalize();
+
+                float tdp = contact.depth;
+                float t = offset.X;
+                t = Math.Abs(t);
+                if(t > 1e-6)
+                {
+                   tdp /= t;
+                   tdp *= contact.normal.X;
+                }
+                else
+                    tdp *= 10;
+
+                if (tdp > 0.25f)
+                    tdp = 0.25f;
+
+                altContact.depth = tdp;
+
+                if (reverse)
+                {
+                    altContact.normal.X = offset.X;
+                    altContact.normal.Y = offset.Y;
+                    altContact.normal.Z = offset.Z;
+                }
+                else
+                {
+                    altContact.normal.X = -offset.X;
+                    altContact.normal.Y = -offset.Y;
+                    altContact.normal.Z = -offset.Z;
+                }
+                return true;
+            }
+            return false;
+        }
+
+        /// <summary>
+        /// Called from Simulate
+        /// This is the avatar's movement control + PID Controller
+        /// </summary>
+        /// <param name="timeStep"></param>
+        public void Move(List<OdeCharacter> defects)
+        {
+            if (Body == IntPtr.Zero)
+                return;
+
+            d.Vector3 dtmp = d.BodyGetPosition(Body);
+            Vector3 localpos = new Vector3(dtmp.X, dtmp.Y, dtmp.Z);
+
+            // the Amotor still lets avatar rotation to drift during colisions
+            // so force it back to identity
+
+            d.Quaternion qtmp;
+            qtmp.W = m_orientation2D.W;
+            qtmp.X = m_orientation2D.X;
+            qtmp.Y = m_orientation2D.Y;
+            qtmp.Z = m_orientation2D.Z;
+            d.BodySetQuaternion(Body, ref qtmp);
+
+            if (m_pidControllerActive == false)
+            {
+                _zeroPosition = localpos;
+            }
+
+            if (!localpos.IsFinite())
+            {
+                m_log.Warn("[PHYSICS]: Avatar Position is non-finite!");
+                defects.Add(this);
+                // _parent_scene.RemoveCharacter(this);
+
+                // destroy avatar capsule and related ODE data
+                AvatarGeomAndBodyDestroy();
+                return;
+            }
+
+            // check outbounds forcing to be in world
+            bool fixbody = false;
+            if (localpos.X < 0.0f)
+            {
+                fixbody = true;
+                localpos.X = 0.1f;
+            }
+            else if (localpos.X > m_parent_scene.WorldExtents.X - 0.1f)
+            {
+                fixbody = true;
+                localpos.X = m_parent_scene.WorldExtents.X - 0.1f;
+            }
+            if (localpos.Y < 0.0f)
+            {
+                fixbody = true;
+                localpos.Y = 0.1f;
+            }
+            else if (localpos.Y > m_parent_scene.WorldExtents.Y - 0.1)
+            {
+                fixbody = true;
+                localpos.Y = m_parent_scene.WorldExtents.Y - 0.1f;
+            }
+            if (fixbody)
+            {
+                m_freemove = false;
+                d.BodySetPosition(Body, localpos.X, localpos.Y, localpos.Z);
+            }
+
+            float breakfactor;
+
+            Vector3 vec = Vector3.Zero;
+            dtmp = d.BodyGetLinearVel(Body);
+            Vector3 vel = new Vector3(dtmp.X, dtmp.Y, dtmp.Z);
+            float velLengthSquared = vel.LengthSquared();
+
+            Vector3 ctz = _target_velocity;
+
+            float movementdivisor = 1f;
+            //Ubit change divisions into multiplications below
+            if (!m_alwaysRun)
+                movementdivisor = 1 / walkDivisor;
+            else
+                movementdivisor = 1 / runDivisor;
+
+            ctz.X *= movementdivisor;
+            ctz.Y *= movementdivisor;
+
+            //******************************************
+            // colide with land
+
+            d.AABB aabb;
+//            d.GeomGetAABB(feetbox, out aabb);
+            d.GeomGetAABB(capsule, out aabb);
+            float chrminZ = aabb.MinZ; // move up a bit
+            Vector3 posch = localpos;
+
+            float ftmp;
+
+            if (m_flying)
+            {
+                ftmp = timeStep;
+                posch.X += vel.X * ftmp;
+                posch.Y += vel.Y * ftmp;
+            }
+
+            float terrainheight = m_parent_scene.GetTerrainHeightAtXY(posch.X, posch.Y);
+            if (chrminZ < terrainheight)
+            {
+                if (ctz.Z < 0)
+                    ctz.Z = 0;
+
+                if(!m_haveLastFallVel)
+                {
+                    m_lastFallVel = vel;
+                    m_haveLastFallVel = true;
+                }
+
+                Vector3 n = m_parent_scene.GetTerrainNormalAtXY(posch.X, posch.Y);
+                float depth = terrainheight - chrminZ;
+
+                vec.Z = depth * PID_P * 50;
+
+                if (!m_flying)
+                {
+                    vec.Z += -vel.Z * PID_D;
+                    if(n.Z < 0.4f)
+                    {
+                        vec.X = depth * PID_P * 50 - vel.X * PID_D;
+                        vec.X *= n.X;
+                        vec.Y = depth * PID_P * 50 - vel.Y * PID_D;
+                        vec.Y *= n.Y;
+                        vec.Z *= n.Z;
+                        if(n.Z < 0.1f)
+                        {
+                            // cancel the slope pose
+                            n.X = 0f;
+                            n.Y = 0f;
+                            n.Z = 1.0f;
+                         }
+                    }
+                }
+
+                if (depth < 0.2f)
+                {
+                    m_colliderGroundfilter++;
+                    if (m_colliderGroundfilter > 2)
+                    {
+                        m_iscolliding = true;
+                        m_colliderfilter = 2;
+
+                        if (m_colliderGroundfilter > 10)
+                        {
+                            m_colliderGroundfilter = 10;
+                            m_freemove = false;
+                        }
+
+                        m_collideNormal.X = n.X;
+                        m_collideNormal.Y = n.Y;
+                        m_collideNormal.Z = n.Z;
+
+                        m_iscollidingGround = true;
+
+                        ContactPoint contact = new ContactPoint();
+                        contact.PenetrationDepth = depth;
+                        contact.Position.X = localpos.X;
+                        contact.Position.Y = localpos.Y;
+                        contact.Position.Z = terrainheight;
+                        contact.SurfaceNormal.X = -n.X;
+                        contact.SurfaceNormal.Y = -n.Y;
+                        contact.SurfaceNormal.Z = -n.Z;
+                        contact.RelativeSpeed = Vector3.Dot(m_lastFallVel, n);
+                        contact.CharacterFeet = true;
+                        AddCollisionEvent(0, contact);
+                        m_lastFallVel = vel;
+
+//                        vec.Z *= 0.5f;
+                    }
+                }
+
+                else
+                {
+                    m_colliderGroundfilter -= 5;
+                    if (m_colliderGroundfilter <= 0)
+                    {
+                        m_colliderGroundfilter = 0;
+                        m_iscollidingGround = false;
+                    }
+                }
+            }
+            else
+            {
+                m_haveLastFallVel = false;
+                m_colliderGroundfilter -= 5;
+                if (m_colliderGroundfilter <= 0)
+                {
+                    m_colliderGroundfilter = 0;
+                    m_iscollidingGround = false;
+                }
+            }
+
+            bool hoverPIDActive = false;
+
+            if (m_useHoverPID && m_PIDHoverTau != 0 && m_PIDHoverHeight != 0)
+            {
+                hoverPIDActive = true;
+
+                switch (m_PIDHoverType)
+                {
+                    case PIDHoverType.Ground:
+                        m_targetHoverHeight = terrainheight + m_PIDHoverHeight;
+                        break;
+
+                    case PIDHoverType.GroundAndWater:
+                        float waterHeight = m_parent_scene.GetWaterLevel();
+                        if (terrainheight > waterHeight)
+                            m_targetHoverHeight = terrainheight + m_PIDHoverHeight;
+                        else
+                            m_targetHoverHeight = waterHeight + m_PIDHoverHeight;
+                        break;
+                }     // end switch (m_PIDHoverType)
+
+                    // don't go underground
+                if (m_targetHoverHeight > terrainheight + 0.5f * (aabb.MaxZ - aabb.MinZ))
+                {
+                    float fz = (m_targetHoverHeight - localpos.Z);
+
+                    //  if error is zero, use position control; otherwise, velocity control
+                    if (Math.Abs(fz) < 0.01f)
+                    {
+                        ctz.Z = 0;
+                    }
+                    else
+                    {
+                        _zeroFlag = false;
+                        fz /= m_PIDHoverTau;
+
+                        float tmp = Math.Abs(fz);
+                        if (tmp > 50)
+                            fz = 50 * Math.Sign(fz);
+                        else if (tmp < 0.1)
+                            fz = 0.1f * Math.Sign(fz);
+
+                        ctz.Z = fz;
+                    }
+                }
+            }
+
+            //******************************************
+            if (!m_iscolliding)
+                m_collideNormal.Z = 0;
+
+            bool tviszero = (ctz.X == 0.0f && ctz.Y == 0.0f && ctz.Z == 0.0f);
+
+            if (!tviszero)
+            {
+                m_freemove = false;
+
+                // movement relative to surface if moving on it
+                // dont disturbe vertical movement, ie jumps
+                if (m_iscolliding && !m_flying && ctz.Z == 0 && m_collideNormal.Z > 0.2f && m_collideNormal.Z < 0.94f)
+                {
+                    float p = ctz.X * m_collideNormal.X + ctz.Y * m_collideNormal.Y;
+                    ctz.X *= (float)Math.Sqrt(1 - m_collideNormal.X * m_collideNormal.X);
+                    ctz.Y *= (float)Math.Sqrt(1 - m_collideNormal.Y * m_collideNormal.Y);
+                    ctz.Z -= p;
+                    if (ctz.Z < 0)
+                        ctz.Z *= 2;
+
+                }
+
+            }
+
+            if (!m_freemove)
+            {
+
+                //  if velocity is zero, use position control; otherwise, velocity control
+                if (tviszero && m_iscolliding && !m_flying)
+                {
+                    //  keep track of where we stopped.  No more slippin' & slidin'
+                    if (!_zeroFlag)
+                    {
+                        _zeroFlag = true;
+                        _zeroPosition = localpos;
+                    }
+                    if (m_pidControllerActive)
+                    {
+                        // We only want to deactivate the PID Controller if we think we want to have our surrogate
+                        // react to the physics scene by moving it's position.
+                        // Avatar to Avatar collisions
+                        // Prim to avatar collisions
+
+                        vec.X = -vel.X * PID_D * 2f + (_zeroPosition.X - localpos.X) * (PID_P * 5);
+                        vec.Y = -vel.Y * PID_D * 2f + (_zeroPosition.Y - localpos.Y) * (PID_P * 5);
+                        if(vel.Z > 0)
+                            vec.Z += -vel.Z * PID_D + (_zeroPosition.Z - localpos.Z) * PID_P;
+                        else
+                            vec.Z += (-vel.Z * PID_D + (_zeroPosition.Z - localpos.Z) * PID_P) * 0.2f;
+/*
+                        if (flying)
+                        {
+                            vec.Z += -vel.Z * PID_D + (_zeroPosition.Z - localpos.Z) * PID_P;
+                        }
+*/
+                    }
+                    //PidStatus = true;
+                }
+                else
+                {
+                    m_pidControllerActive = true;
+                    _zeroFlag = false;
+
+                    if (m_iscolliding)
+                    {
+                        if (!m_flying)
+                        {
+                            // we are on a surface
+                            if (ctz.Z > 0f)
+                            {
+                                // moving up or JUMPING
+                                vec.Z += (ctz.Z - vel.Z) * PID_D * 2f;
+                                vec.X += (ctz.X - vel.X) * (PID_D);
+                                vec.Y += (ctz.Y - vel.Y) * (PID_D);
+                            }
+                            else
+                            {
+                                // we are moving down on a surface
+                                if (ctz.Z == 0)
+                                {
+                                    if (vel.Z > 0)
+                                        vec.Z -= vel.Z * PID_D * 2f;
+                                    vec.X += (ctz.X - vel.X) * (PID_D);
+                                    vec.Y += (ctz.Y - vel.Y) * (PID_D);
+                                }
+                                // intencionally going down
+                                else
+                                {
+                                    if (ctz.Z < vel.Z)
+                                        vec.Z += (ctz.Z - vel.Z) * PID_D;
+                                    else
+                                    {
+                                    }
+
+                                    if (Math.Abs(ctz.X) > Math.Abs(vel.X))
+                                        vec.X += (ctz.X - vel.X) * (PID_D);
+                                    if (Math.Abs(ctz.Y) > Math.Abs(vel.Y))
+                                        vec.Y += (ctz.Y - vel.Y) * (PID_D);
+                                }
+                            }
+
+                            // We're standing on something
+                        }
+                        else
+                        {
+                            // We're flying and colliding with something
+                            vec.X += (ctz.X - vel.X) * (PID_D * 0.0625f);
+                            vec.Y += (ctz.Y - vel.Y) * (PID_D * 0.0625f);
+                            vec.Z += (ctz.Z - vel.Z) * (PID_D * 0.0625f);
+                        }
+                    }
+                    else // ie not colliding
+                    {
+                        if (m_flying || hoverPIDActive) //(!m_iscolliding && flying)
+                        {
+                            // we're in mid air suspended
+                            vec.X += (ctz.X - vel.X) * (PID_D);
+                            vec.Y += (ctz.Y - vel.Y) * (PID_D);
+                            vec.Z += (ctz.Z - vel.Z) * (PID_D);
+                        }
+
+                        else
+                        {
+                            // we're not colliding and we're not flying so that means we're falling!
+                            // m_iscolliding includes collisions with the ground.
+
+                            // d.Vector3 pos = d.BodyGetPosition(Body);
+                            vec.X += (ctz.X - vel.X) * PID_D * 0.833f;
+                            vec.Y += (ctz.Y - vel.Y) * PID_D * 0.833f;
+                            // hack for  breaking on fall
+                            if (ctz.Z == -9999f)
+                                vec.Z += -vel.Z * PID_D - m_parent_scene.gravityz * m_mass;
+                        }
+                    }
+                }
+
+                if (velLengthSquared > 2500.0f) // 50m/s apply breaks
+                {
+                    breakfactor = 0.16f * m_mass;
+                    vec.X -= breakfactor * vel.X;
+                    vec.Y -= breakfactor * vel.Y;
+                    vec.Z -= breakfactor * vel.Z;
+                }
+            }
+            else
+            {
+                breakfactor = m_mass;
+                vec.X -= breakfactor * vel.X;
+                vec.Y -= breakfactor * vel.Y;
+                if (m_flying)
+                    vec.Z -= 0.5f * breakfactor * vel.Z;
+                else
+                    vec.Z -= .16f* m_mass * vel.Z;
+            }
+
+            if (m_flying || hoverPIDActive)
+            {
+                vec.Z -= m_parent_scene.gravityz * m_mass;
+
+                if(!hoverPIDActive)
+                {
+                    //Added for auto fly height. Kitto Flora
+                    float target_altitude = terrainheight + MinimumGroundFlightOffset;
+
+                    if (localpos.Z < target_altitude)
+                    {
+                       vec.Z += (target_altitude - localpos.Z) * PID_P * 5.0f;
+                    }
+                    // end add Kitto Flora
+                }
+            }
+
+            if (vec.IsFinite())
+            {
+                if (vec.X != 0 || vec.Y !=0 || vec.Z !=0)
+                    d.BodyAddForce(Body, vec.X, vec.Y, vec.Z);
+            }
+            else
+            {
+                m_log.Warn("[PHYSICS]: Got a NaN force vector in Move()");
+                m_log.Warn("[PHYSICS]: Avatar Position is non-finite!");
+                defects.Add(this);
+                // _parent_scene.RemoveCharacter(this);
+                // destroy avatar capsule and related ODE data
+                AvatarGeomAndBodyDestroy();
+                return;
+            }
+
+            // update our local ideia of position velocity and aceleration
+            //            _position = localpos;
+            _position = localpos;
+
+            if (_zeroFlag)
+            {
+                _velocity = Vector3.Zero;
+                _acceleration = Vector3.Zero;
+                m_rotationalVelocity = Vector3.Zero;
+            }
+            else
+            {
+                Vector3 a =_velocity; // previus velocity
+                SetSmooth(ref _velocity, ref vel, 2);
+                a = (_velocity - a) * invtimeStep;
+                SetSmooth(ref _acceleration, ref a, 2);
+
+                dtmp = d.BodyGetAngularVel(Body);
+                m_rotationalVelocity.X = 0f;
+                m_rotationalVelocity.Y = 0f;
+                m_rotationalVelocity.Z = dtmp.Z;
+                Math.Round(m_rotationalVelocity.Z,3);
+            }
+        }
+
+        public void round(ref Vector3 v, int digits)
+        {
+            v.X = (float)Math.Round(v.X, digits);
+            v.Y = (float)Math.Round(v.Y, digits);
+            v.Z = (float)Math.Round(v.Z, digits);
+         }
+
+        public void SetSmooth(ref Vector3 dst, ref Vector3 value)
+        {
+            dst.X = 0.1f * dst.X + 0.9f * value.X;
+            dst.Y = 0.1f * dst.Y + 0.9f * value.Y;
+            dst.Z = 0.1f * dst.Z + 0.9f * value.Z;
+        }
+
+        public void SetSmooth(ref Vector3 dst, ref Vector3 value, int rounddigits)
+        {
+            dst.X = 0.4f * dst.X + 0.6f * value.X;
+            dst.X = (float)Math.Round(dst.X, rounddigits);
+
+            dst.Y = 0.4f * dst.Y + 0.6f * value.Y;
+            dst.Y = (float)Math.Round(dst.Y, rounddigits);
+
+            dst.Z = 0.4f * dst.Z + 0.6f * value.Z;
+            dst.Z = (float)Math.Round(dst.Z, rounddigits);
+        }
+
+
+        /// <summary>
+        /// Updates the reported position and velocity.
+        /// Used to copy variables from unmanaged space at heartbeat rate and also trigger scene updates acording
+        /// also outbounds checking
+        /// copy and outbounds now done in move(..) at ode rate
+        ///
+        /// </summary>
+        public void UpdatePositionAndVelocity()
+        {
+            return;
+
+//            if (Body == IntPtr.Zero)
+//                return;
+
+        }
+
+        /// <summary>
+        /// Cleanup the things we use in the scene.
+        /// </summary>
+        public void Destroy()
+        {
+            AddChange(changes.Remove, null);
+        }
+
+        public override void CrossingFailure()
+        {
+        }
+
+        public override Vector3 PIDTarget { set { return; } }
+        public override bool PIDActive {get {return m_pidControllerActive;} set { return; } }
+        public override float PIDTau { set { return; } }
+
+        public override float PIDHoverHeight
+        {
+            set
+            {
+                AddChange(changes.PIDHoverHeight,value);
+            }
+        }
+        public override bool PIDHoverActive
+        {
+            get
+            {
+            return m_useHoverPID;
+            }
+            set
+            {
+                AddChange(changes.PIDHoverActive, value);
+            }
+        }
+
+        public override PIDHoverType PIDHoverType
+        {
+            set
+            {
+                AddChange(changes.PIDHoverType,value);
+            }
+        }
+
+        public override float PIDHoverTau
+        {
+            set
+            {
+                float tmp =0;
+                if (value > 0)
+                {
+                    float mint = (0.05f > timeStep ? 0.05f : timeStep);
+                    if (value < mint)
+                        tmp = mint;
+                    else
+                        tmp = value;
+                }
+                AddChange(changes.PIDHoverTau, tmp);
+            }
+        }
+
+        public override Quaternion APIDTarget { set { return; } }
+
+        public override bool APIDActive { set { return; } }
+
+        public override float APIDStrength { set { return; } }
+
+        public override float APIDDamping { set { return; } }
+
+        public override void SubscribeEvents(int ms)
+        {
+            m_eventsubscription = ms;
+            m_cureventsubscription = 0;
+            CollisionEventsThisFrame.Clear();
+            SentEmptyCollisionsEvent = false;
+        }
+
+        public override void UnSubscribeEvents()
+        {
+            m_eventsubscription = 0;
+            m_parent_scene.RemoveCollisionEventReporting(this);
+            lock(CollisionEventsThisFrame)
+                CollisionEventsThisFrame.Clear();
+        }
+
+        public override void AddCollisionEvent(uint CollidedWith, ContactPoint contact)
+        {
+            lock(CollisionEventsThisFrame)
+                CollisionEventsThisFrame.AddCollider(CollidedWith, contact);
+            m_parent_scene.AddCollisionEventReporting(this);
+        }
+
+        public void SendCollisions(int timestep)
+        {
+            if (m_cureventsubscription < 50000)
+                m_cureventsubscription += timestep;
+
+            if (m_cureventsubscription < m_eventsubscription)
+                return;
+
+            lock(CollisionEventsThisFrame)
+            {
+                int ncolisions = CollisionEventsThisFrame.m_objCollisionList.Count;
+
+                if (!SentEmptyCollisionsEvent || ncolisions > 0)
+                {
+                    base.SendCollisionUpdate(CollisionEventsThisFrame);
+                    m_cureventsubscription = 0;
+
+                    if (ncolisions == 0)
+                    {
+                        SentEmptyCollisionsEvent = true;
+    //                  _parent_scene.RemoveCollisionEventReporting(this);
+                    }
+                    else
+                    {
+                        SentEmptyCollisionsEvent = false;
+                        CollisionEventsThisFrame.Clear();
+                    }
+                }
+            }
+        }
+
+        public override bool SubscribedEvents()
+        {
+            if (m_eventsubscription > 0)
+                return true;
+            return false;
+        }
+
+        private void changePhysicsStatus(bool NewStatus)
+        {
+            if (NewStatus != m_isPhysical)
+            {
+                if (NewStatus)
+                {
+                    AvatarGeomAndBodyDestroy();
+
+                    AvatarGeomAndBodyCreation(_position.X, _position.Y, _position.Z);
+
+                    m_parent_scene.actor_name_map[collider] = (PhysicsActor)this;
+                    m_parent_scene.actor_name_map[capsule] = (PhysicsActor)this;
+                    m_parent_scene.AddCharacter(this);
+                }
+                else
+                {
+                    m_parent_scene.RemoveCollisionEventReporting(this);
+                    m_parent_scene.RemoveCharacter(this);
+                    // destroy avatar capsule and related ODE data
+                    AvatarGeomAndBodyDestroy();
+                }
+                m_freemove = false;
+                m_isPhysical = NewStatus;
+            }
+        }
+
+        private void changeAdd()
+        {
+            changePhysicsStatus(true);
+        }
+
+        private void changeRemove()
+        {
+            changePhysicsStatus(false);
+        }
+
+        private void changeShape(PrimitiveBaseShape arg)
+        {
+        }
+
+        private void changeAvatarSize(strAvatarSize st)
+        {
+            m_feetOffset = st.offset;
+            changeSize(st.size);
+        }
+
+        private void changeSize(Vector3 pSize)
+        {
+            if (pSize.IsFinite())
+            {
+                // for now only look to Z changes since viewers also don't change X and Y
+                if (pSize.Z != m_size.Z)
+                {
+                    AvatarGeomAndBodyDestroy();
+
+                    float oldsz = m_size.Z;
+                    m_size = pSize;
+
+                    AvatarGeomAndBodyCreation(_position.X, _position.Y,
+                                      _position.Z + (m_size.Z - oldsz) * 0.5f);
+
+//                    Velocity = Vector3.Zero;
+                    m_targetVelocity = Vector3.Zero;
+
+                    m_parent_scene.actor_name_map[collider] = (PhysicsActor)this;
+                    m_parent_scene.actor_name_map[capsule] = (PhysicsActor)this;
+                }
+                m_freemove = false;
+                m_pidControllerActive = true;
+            }
+            else
+            {
+                m_log.Warn("[PHYSICS]: Got a NaN Size from Scene on a Character");
+            }
+        }
+
+        private void changePosition( Vector3 newPos)
+            {
+                if (Body != IntPtr.Zero)
+                    d.BodySetPosition(Body, newPos.X, newPos.Y, newPos.Z);
+                _position = newPos;
+                m_freemove = false;
+                m_pidControllerActive = true;
+            }
+
+        private void changeOrientation(Quaternion newOri)
+        {
+            if (m_orientation != newOri)
+            {
+                m_orientation = newOri; // keep a copy for core use
+                // but only use rotations around Z
+
+                m_orientation2D.W = newOri.W;
+                m_orientation2D.Z = newOri.Z;
+
+                float t = m_orientation2D.W * m_orientation2D.W + m_orientation2D.Z * m_orientation2D.Z;
+                if (t > 0)
+                {
+                    t = 1.0f / (float)Math.Sqrt(t);
+                    m_orientation2D.W *= t;
+                    m_orientation2D.Z *= t;
+                }
+                else
+                {
+                    m_orientation2D.W = 1.0f;
+                    m_orientation2D.Z = 0f;
+                }
+                m_orientation2D.Y = 0f;
+                m_orientation2D.X = 0f;
+
+                d.Quaternion myrot = new d.Quaternion();
+                myrot.X = m_orientation2D.X;
+                myrot.Y = m_orientation2D.Y;
+                myrot.Z = m_orientation2D.Z;
+                myrot.W = m_orientation2D.W;
+                d.BodySetQuaternion(Body, ref myrot);
+            }
+        }
+
+        private void changeVelocity(Vector3 newVel)
+        {
+            _velocity = newVel;
+            setFreeMove();
+
+            if (Body != IntPtr.Zero)
+                d.BodySetLinearVel(Body, newVel.X, newVel.Y, newVel.Z);
+        }
+
+        private void changeTargetVelocity(Vector3 newVel)
+        {
+            m_pidControllerActive = true;
+            m_freemove = false;
+            _target_velocity = newVel;
+        }
+
+        private void changeSetTorque(Vector3 newTorque)
+        {
+        }
+
+        private void changeAddForce(Vector3 newForce)
+        {
+        }
+
+        private void changeAddAngularForce(Vector3 arg)
+        {
+        }
+
+        private void changeAngularLock(byte arg)
+        {
+        }
+
+        private void changeFloatOnWater(bool arg)
+        {
+        }
+
+        private void changeVolumedetetion(bool arg)
+        {
+        }
+
+        private void changeSelectedStatus(bool arg)
+        {
+        }
+
+        private void changeDisable(bool arg)
+        {
+        }
+
+        private void changeBuilding(bool arg)
+        {
+        }
+
+        private void setFreeMove()
+        {
+            m_pidControllerActive = true;
+            _zeroFlag = false;
+            _target_velocity = Vector3.Zero;
+            m_freemove = true;
+            m_colliderfilter = -1;
+            m_colliderObjectfilter = -1;
+            m_colliderGroundfilter = -1;
+
+            m_iscolliding = false;
+            m_iscollidingGround = false;
+            m_iscollidingObj = false;
+
+            CollisionEventsThisFrame.Clear();
+        }
+
+        private void changeForce(Vector3 newForce)
+        {
+            setFreeMove();
+
+            if (Body != IntPtr.Zero)
+            {
+                if (newForce.X != 0f || newForce.Y != 0f || newForce.Z != 0)
+                    d.BodyAddForce(Body, newForce.X, newForce.Y, newForce.Z);
+            }
+        }
+
+        // for now momentum is actually velocity
+        private void changeMomentum(Vector3 newmomentum)
+        {
+            _velocity = newmomentum;
+            setFreeMove();
+
+            if (Body != IntPtr.Zero)
+                d.BodySetLinearVel(Body, newmomentum.X, newmomentum.Y, newmomentum.Z);
+        }
+
+        private void changePIDHoverHeight(float val)
+        {
+          m_PIDHoverHeight = val;
+          if (val == 0)
+            m_useHoverPID = false;
+        }
+
+        private void changePIDHoverType(PIDHoverType type)
+        {
+            m_PIDHoverType = type;
+        }
+
+        private void changePIDHoverTau(float tau)
+        {
+            m_PIDHoverTau = tau;
+        }
+
+        private void changePIDHoverActive(bool active)
+        {
+            m_useHoverPID = active;
+        }
+
+        private void donullchange()
+        {
+        }
+
+        public bool DoAChange(changes what, object arg)
+        {
+            if (collider == IntPtr.Zero && what != changes.Add && what != changes.Remove)
+            {
+                return false;
+            }
+
+            // nasty switch
+            switch (what)
+            {
+                case changes.Add:
+                    changeAdd();
+                    break;
+                case changes.Remove:
+                    changeRemove();
+                    break;
+
+                case changes.Position:
+                    changePosition((Vector3)arg);
+                    break;
+
+                case changes.Orientation:
+                    changeOrientation((Quaternion)arg);
+                    break;
+
+                case changes.PosOffset:
+                    donullchange();
+                    break;
+
+                case changes.OriOffset:
+                    donullchange();
+                    break;
+
+                case changes.Velocity:
+                    changeVelocity((Vector3)arg);
+                    break;
+
+                case changes.TargetVelocity:
+                    changeTargetVelocity((Vector3)arg);
+                    break;
+
+                //                case changes.Acceleration:
+                //                    changeacceleration((Vector3)arg);
+                //                    break;
+                //                case changes.AngVelocity:
+                //                    changeangvelocity((Vector3)arg);
+                //                    break;
+
+                case changes.Force:
+                    changeForce((Vector3)arg);
+                    break;
+
+                case changes.Torque:
+                    changeSetTorque((Vector3)arg);
+                    break;
+
+                case changes.AddForce:
+                    changeAddForce((Vector3)arg);
+                    break;
+
+                case changes.AddAngForce:
+                    changeAddAngularForce((Vector3)arg);
+                    break;
+
+                case changes.AngLock:
+                    changeAngularLock((byte)arg);
+                    break;
+
+                case changes.Size:
+                    changeSize((Vector3)arg);
+                    break;
+
+                case changes.AvatarSize:
+                    changeAvatarSize((strAvatarSize)arg);
+                    break;
+
+                case changes.Momentum:
+                    changeMomentum((Vector3)arg);
+                    break;
+
+                case changes.PIDHoverHeight:
+                    changePIDHoverHeight((float)arg);
+                    break;
+
+                case changes.PIDHoverType:
+                    changePIDHoverType((PIDHoverType)arg);
+                    break;
+
+                case changes.PIDHoverTau:
+                    changePIDHoverTau((float)arg);
+                    break;
+
+                case changes.PIDHoverActive:
+                    changePIDHoverActive((bool)arg);
+                    break;
+
+/* not in use for now
+                case changes.Shape:
+                    changeShape((PrimitiveBaseShape)arg);
+                    break;
+
+                case changes.CollidesWater:
+                    changeFloatOnWater((bool)arg);
+                    break;
+
+                case changes.VolumeDtc:
+                    changeVolumedetetion((bool)arg);
+                    break;
+
+                case changes.Physical:
+                    changePhysicsStatus((bool)arg);
+                    break;
+
+                case changes.Selected:
+                    changeSelectedStatus((bool)arg);
+                    break;
+
+                case changes.disabled:
+                    changeDisable((bool)arg);
+                    break;
+
+                case changes.building:
+                    changeBuilding((bool)arg);
+                    break;
+*/
+                case changes.Null:
+                    donullchange();
+                    break;
+
+                default:
+                    donullchange();
+                    break;
+            }
+            return false;
+        }
+
+        public void AddChange(changes what, object arg)
+        {
+            m_parent_scene.AddChange((PhysicsActor)this, what, arg);
+        }
+
+        private struct strAvatarSize
+        {
+            public Vector3 size;
+            public float offset;
+        }
+
+    }
+}
diff --git a/OpenSim/Region/PhysicsModules/ubOde/ODEDynamics.cs b/OpenSim/Region/PhysicsModules/ubOde/ODEDynamics.cs
new file mode 100644
index 0000000..ce10065
--- /dev/null
+++ b/OpenSim/Region/PhysicsModules/ubOde/ODEDynamics.cs
@@ -0,0 +1,1205 @@
+/*
+ * Copyright (c) Contributors, http://opensimulator.org/
+ * See CONTRIBUTORS.TXT for a full list of copyright holders.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the OpenSimulator Project nor the
+ *       names of its contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/* Revised Aug, Sept 2009 by Kitto Flora. ODEDynamics.cs replaces
+ * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised:
+ * ODEPrim.cs contains methods dealing with Prim editing, Prim
+ * characteristics and Kinetic motion.
+ * ODEDynamics.cs contains methods dealing with Prim Physical motion
+ * (dynamics) and the associated settings. Old Linear and angular
+ * motors for dynamic motion have been replace with  MoveLinear()
+ * and MoveAngular(); 'Physical' is used only to switch ODE dynamic
+ * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_<other> is to
+ * switch between 'VEHICLE' parameter use and general dynamics
+ * settings use.
+ */
+
+// Extensive change Ubit 2012
+
+using System;
+using System.Collections.Generic;
+using System.Reflection;
+using System.Runtime.InteropServices;
+using log4net;
+using OpenMetaverse;
+using OdeAPI;
+using OpenSim.Framework;
+using OpenSim.Region.PhysicsModules.SharedBase;
+
+namespace OpenSim.Region.PhysicsModule.ubOde
+{
+    public class ODEDynamics
+    {
+        public Vehicle Type
+        {
+            get { return m_type; }
+        }
+
+        private OdePrim rootPrim;
+        private ODEScene _pParentScene;
+
+        // Vehicle properties
+        // WARNING this are working copies for internel use
+        // their values may not be the corresponding parameter
+
+        private Quaternion m_referenceFrame = Quaternion.Identity;      // Axis modifier
+        private Quaternion m_RollreferenceFrame = Quaternion.Identity;  // what hell is this ?
+
+        private Vehicle m_type = Vehicle.TYPE_NONE;                     // If a 'VEHICLE', and what kind
+
+        private VehicleFlag m_flags = (VehicleFlag) 0;                  // Boolean settings:
+                                                                        // HOVER_TERRAIN_ONLY
+                                                                        // HOVER_GLOBAL_HEIGHT
+                                                                        // NO_DEFLECTION_UP
+                                                                        // HOVER_WATER_ONLY
+                                                                        // HOVER_UP_ONLY
+                                                                        // LIMIT_MOTOR_UP
+                                                                        // LIMIT_ROLL_ONLY
+        private Vector3 m_BlockingEndPoint = Vector3.Zero;              // not sl
+
+        // Linear properties
+        private Vector3 m_linearMotorDirection = Vector3.Zero;          // velocity requested by LSL, decayed by time
+        private Vector3 m_linearFrictionTimescale = new Vector3(1000, 1000, 1000);
+        private float m_linearMotorDecayTimescale = 120;
+        private float m_linearMotorTimescale = 1000;
+        private Vector3 m_linearMotorOffset = Vector3.Zero;
+
+        //Angular properties
+        private Vector3 m_angularMotorDirection = Vector3.Zero;         // angular velocity requested by LSL motor
+        private float m_angularMotorTimescale = 1000;                      // motor angular velocity ramp up rate
+        private float m_angularMotorDecayTimescale = 120;                 // motor angular velocity decay rate
+        private Vector3 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);      // body angular velocity  decay rate
+
+        //Deflection properties
+        private float m_angularDeflectionEfficiency = 0;
+        private float m_angularDeflectionTimescale = 1000;
+        private float m_linearDeflectionEfficiency = 0;
+        private float m_linearDeflectionTimescale = 1000;
+
+        //Banking properties
+        private float m_bankingEfficiency = 0;
+        private float m_bankingMix = 0;
+        private float m_bankingTimescale = 1000;
+
+        //Hover and Buoyancy properties
+        private float m_VhoverHeight = 0f;
+        private float m_VhoverEfficiency = 0f;
+        private float m_VhoverTimescale = 1000f;
+        private float m_VehicleBuoyancy = 0f;           //KF: m_VehicleBuoyancy is set by VEHICLE_BUOYANCY for a vehicle.
+                    // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity)
+                    // KF: So far I have found no good method to combine a script-requested .Z velocity and gravity.
+                    // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity.
+
+        //Attractor properties
+        private float m_verticalAttractionEfficiency = 1.0f;        // damped
+        private float m_verticalAttractionTimescale = 1000f;        // Timescale > 300  means no vert attractor.
+
+
+        // auxiliar
+        private float m_lmEfect = 0f;                                            // current linear motor eficiency
+        private float m_lmDecay = 0f;                                            // current linear decay
+
+        private float m_amEfect = 0;                                            // current angular motor eficiency
+        private float m_amDecay = 0f;                                            // current linear decay
+
+        private float m_ffactor = 1.0f;
+
+        private float m_timestep = 0.02f;
+        private float m_invtimestep = 50;
+
+
+        float m_ampwr;
+        float m_amdampX;
+        float m_amdampY;
+        float m_amdampZ;
+
+        float m_gravmod;
+
+        public float FrictionFactor
+        {
+            get
+            {
+                return m_ffactor;
+            }
+        }
+
+        public float GravMod
+        {
+            set
+            {
+                m_gravmod = value;
+            }
+        }
+
+
+        public ODEDynamics(OdePrim rootp)
+        {
+            rootPrim = rootp;
+            _pParentScene = rootPrim._parent_scene;
+            m_timestep = _pParentScene.ODE_STEPSIZE;
+            m_invtimestep = 1.0f / m_timestep;
+            m_gravmod = rootPrim.GravModifier;
+        }
+
+        public void DoSetVehicle(VehicleData vd)
+        {
+            m_type = vd.m_type;
+            m_flags = vd.m_flags;
+
+
+            // Linear properties
+            m_linearMotorDirection = vd.m_linearMotorDirection;
+
+            m_linearFrictionTimescale = vd.m_linearFrictionTimescale;
+            if (m_linearFrictionTimescale.X < m_timestep) m_linearFrictionTimescale.X = m_timestep;
+            if (m_linearFrictionTimescale.Y < m_timestep) m_linearFrictionTimescale.Y = m_timestep;
+            if (m_linearFrictionTimescale.Z < m_timestep) m_linearFrictionTimescale.Z = m_timestep;
+
+            m_linearMotorDecayTimescale = vd.m_linearMotorDecayTimescale;
+            if (m_linearMotorDecayTimescale < m_timestep) m_linearMotorDecayTimescale = m_timestep;
+            m_linearMotorDecayTimescale += 0.2f;
+            m_linearMotorDecayTimescale *= m_invtimestep;
+
+            m_linearMotorTimescale = vd.m_linearMotorTimescale;
+            if (m_linearMotorTimescale < m_timestep) m_linearMotorTimescale = m_timestep;
+
+            m_linearMotorOffset = vd.m_linearMotorOffset;
+
+            //Angular properties
+            m_angularMotorDirection = vd.m_angularMotorDirection;
+            m_angularMotorTimescale = vd.m_angularMotorTimescale;
+            if (m_angularMotorTimescale < m_timestep) m_angularMotorTimescale = m_timestep;
+
+            m_angularMotorDecayTimescale = vd.m_angularMotorDecayTimescale;
+            if (m_angularMotorDecayTimescale < m_timestep) m_angularMotorDecayTimescale = m_timestep;
+            m_angularMotorDecayTimescale *= m_invtimestep;
+
+            m_angularFrictionTimescale = vd.m_angularFrictionTimescale;
+            if (m_angularFrictionTimescale.X < m_timestep) m_angularFrictionTimescale.X = m_timestep;
+            if (m_angularFrictionTimescale.Y < m_timestep) m_angularFrictionTimescale.Y = m_timestep;
+            if (m_angularFrictionTimescale.Z < m_timestep) m_angularFrictionTimescale.Z = m_timestep;
+
+            //Deflection properties
+            m_angularDeflectionEfficiency = vd.m_angularDeflectionEfficiency;
+            m_angularDeflectionTimescale = vd.m_angularDeflectionTimescale;
+            if (m_angularDeflectionTimescale < m_timestep) m_angularDeflectionTimescale = m_timestep;
+
+            m_linearDeflectionEfficiency = vd.m_linearDeflectionEfficiency;
+            m_linearDeflectionTimescale = vd.m_linearDeflectionTimescale;
+            if (m_linearDeflectionTimescale < m_timestep) m_linearDeflectionTimescale = m_timestep;
+
+            //Banking properties
+            m_bankingEfficiency = vd.m_bankingEfficiency;
+            m_bankingMix = vd.m_bankingMix;
+            m_bankingTimescale = vd.m_bankingTimescale;
+            if (m_bankingTimescale < m_timestep) m_bankingTimescale = m_timestep;
+
+            //Hover and Buoyancy properties
+            m_VhoverHeight = vd.m_VhoverHeight;
+            m_VhoverEfficiency = vd.m_VhoverEfficiency;
+            m_VhoverTimescale = vd.m_VhoverTimescale;
+            if (m_VhoverTimescale < m_timestep) m_VhoverTimescale = m_timestep;
+
+            m_VehicleBuoyancy = vd.m_VehicleBuoyancy;
+
+            //Attractor properties
+            m_verticalAttractionEfficiency = vd.m_verticalAttractionEfficiency;
+            m_verticalAttractionTimescale = vd.m_verticalAttractionTimescale;
+            if (m_verticalAttractionTimescale < m_timestep) m_verticalAttractionTimescale = m_timestep;
+
+            // Axis
+            m_referenceFrame = vd.m_referenceFrame;
+
+            m_lmEfect = 0;
+            m_lmDecay = (1.0f - 1.0f / m_linearMotorDecayTimescale);
+            m_amEfect = 0;
+            m_ffactor = 1.0f;
+        }
+
+        internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue)
+        {
+            float len;
+            if(float.IsNaN(pValue) || float.IsInfinity(pValue))
+                return;
+
+            switch (pParam)
+            {
+                case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY:
+                    if (pValue < 0f) pValue = 0f;
+                    if (pValue > 1f) pValue = 1f;
+                    m_angularDeflectionEfficiency = pValue;
+                    break;
+                case Vehicle.ANGULAR_DEFLECTION_TIMESCALE:
+                    if (pValue < m_timestep) pValue = m_timestep;
+                    m_angularDeflectionTimescale = pValue;
+                    break;
+                case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE:
+                    if (pValue < m_timestep) pValue = m_timestep;
+                    else if (pValue > 120) pValue = 120;
+                    m_angularMotorDecayTimescale = pValue * m_invtimestep;
+                    m_amDecay = 1.0f - 1.0f / m_angularMotorDecayTimescale;
+                    break;
+                case Vehicle.ANGULAR_MOTOR_TIMESCALE:
+                    if (pValue < m_timestep) pValue = m_timestep;
+                    m_angularMotorTimescale = pValue;
+                    break;
+                case Vehicle.BANKING_EFFICIENCY:
+                    if (pValue < -1f) pValue = -1f;
+                    if (pValue > 1f) pValue = 1f;
+                    m_bankingEfficiency = pValue;
+                    break;
+                case Vehicle.BANKING_MIX:
+                    if (pValue < 0f) pValue = 0f;
+                    if (pValue > 1f) pValue = 1f;
+                    m_bankingMix = pValue;
+                    break;
+                case Vehicle.BANKING_TIMESCALE:
+                    if (pValue < m_timestep) pValue = m_timestep;
+                    m_bankingTimescale = pValue;
+                    break;
+                case Vehicle.BUOYANCY:
+                    if (pValue < -1f) pValue = -1f;
+                    if (pValue > 1f) pValue = 1f;
+                    m_VehicleBuoyancy = pValue;
+                    break;
+                case Vehicle.HOVER_EFFICIENCY:
+                    if (pValue < 0f) pValue = 0f;
+                    if (pValue > 1f) pValue = 1f;
+                    m_VhoverEfficiency = pValue;
+                    break;
+                case Vehicle.HOVER_HEIGHT:
+                    m_VhoverHeight = pValue;
+                    break;
+                case Vehicle.HOVER_TIMESCALE:
+                    if (pValue < m_timestep) pValue = m_timestep;
+                    m_VhoverTimescale = pValue;
+                    break;
+                case Vehicle.LINEAR_DEFLECTION_EFFICIENCY:
+                    if (pValue < 0f) pValue = 0f;
+                    if (pValue > 1f) pValue = 1f;
+                    m_linearDeflectionEfficiency = pValue;
+                    break;
+                case Vehicle.LINEAR_DEFLECTION_TIMESCALE:
+                    if (pValue < m_timestep) pValue = m_timestep;
+                    m_linearDeflectionTimescale = pValue;
+                    break;
+                case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE:
+                    if (pValue < m_timestep) pValue = m_timestep;
+                    else if (pValue > 120) pValue = 120;
+                    m_linearMotorDecayTimescale = (0.2f +pValue) * m_invtimestep;
+                    m_lmDecay = (1.0f - 1.0f / m_linearMotorDecayTimescale);
+                    break;
+                case Vehicle.LINEAR_MOTOR_TIMESCALE:
+                    if (pValue < m_timestep) pValue = m_timestep;
+                    m_linearMotorTimescale = pValue;
+                    break;
+                case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY:
+                    if (pValue < 0f) pValue = 0f;
+                    if (pValue > 1f) pValue = 1f;
+                    m_verticalAttractionEfficiency = pValue;
+                    break;
+                case Vehicle.VERTICAL_ATTRACTION_TIMESCALE:
+                    if (pValue < m_timestep) pValue = m_timestep;
+                    m_verticalAttractionTimescale = pValue;
+                    break;
+
+                // These are vector properties but the engine lets you use a single float value to
+                // set all of the components to the same value
+                case Vehicle.ANGULAR_FRICTION_TIMESCALE:
+                    if (pValue < m_timestep) pValue = m_timestep;
+                    m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue);
+                    break;
+                case Vehicle.ANGULAR_MOTOR_DIRECTION:
+                    m_angularMotorDirection = new Vector3(pValue, pValue, pValue);
+                    len = m_angularMotorDirection.Length();
+                    if (len > 12.566f)
+                        m_angularMotorDirection *= (12.566f / len);
+
+                    m_amEfect = 1.0f ; // turn it on
+                    m_amDecay = 1.0f - 1.0f / m_angularMotorDecayTimescale;
+
+                    if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body)
+                            && !rootPrim.m_isSelected && !rootPrim.m_disabled)
+                        d.BodyEnable(rootPrim.Body);
+
+                    break;
+                case Vehicle.LINEAR_FRICTION_TIMESCALE:
+                    if (pValue < m_timestep) pValue = m_timestep;
+                    m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue);
+                    break;
+                case Vehicle.LINEAR_MOTOR_DIRECTION:
+                    m_linearMotorDirection = new Vector3(pValue, pValue, pValue);
+                    len = m_linearMotorDirection.Length();
+                    if (len > 100.0f)
+                        m_linearMotorDirection *= (100.0f / len);
+
+                    m_lmDecay = 1.0f - 1.0f / m_linearMotorDecayTimescale;
+                    m_lmEfect = 1.0f; // turn it on
+
+                    m_ffactor = 0.0f;
+                    if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body)
+                            && !rootPrim.m_isSelected && !rootPrim.m_disabled)
+                        d.BodyEnable(rootPrim.Body);
+                    break;
+                case Vehicle.LINEAR_MOTOR_OFFSET:
+                    m_linearMotorOffset = new Vector3(pValue, pValue, pValue);
+                    len = m_linearMotorOffset.Length();
+                    if (len > 100.0f)
+                        m_linearMotorOffset *= (100.0f / len);
+                    break;
+            }
+        }//end ProcessFloatVehicleParam
+
+        internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue)
+        {
+            float len;
+            if(!pValue.IsFinite())
+                return;
+
+            switch (pParam)
+            {
+                case Vehicle.ANGULAR_FRICTION_TIMESCALE:
+                    if (pValue.X < m_timestep) pValue.X = m_timestep;
+                    if (pValue.Y < m_timestep) pValue.Y = m_timestep;
+                    if (pValue.Z < m_timestep) pValue.Z = m_timestep;
+
+                    m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
+                    break;
+                case Vehicle.ANGULAR_MOTOR_DIRECTION:
+                    m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
+                    // Limit requested angular speed to 2 rps= 4 pi rads/sec
+                    len = m_angularMotorDirection.Length();
+                    if (len > 12.566f)
+                        m_angularMotorDirection *= (12.566f / len);
+
+                    m_amEfect = 1.0f; // turn it on
+                    m_amDecay = 1.0f - 1.0f / m_angularMotorDecayTimescale;
+
+                    if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body)
+                            && !rootPrim.m_isSelected && !rootPrim.m_disabled)
+                        d.BodyEnable(rootPrim.Body);
+                    break;
+                case Vehicle.LINEAR_FRICTION_TIMESCALE:
+                    if (pValue.X < m_timestep) pValue.X = m_timestep;
+                    if (pValue.Y < m_timestep) pValue.Y = m_timestep;
+                    if (pValue.Z < m_timestep) pValue.Z = m_timestep;
+                    m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
+                    break;
+                case Vehicle.LINEAR_MOTOR_DIRECTION:
+                    m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
+                    len = m_linearMotorDirection.Length();
+                    if (len > 100.0f)
+                        m_linearMotorDirection *= (100.0f / len);
+
+                    m_lmEfect = 1.0f; // turn it on
+                    m_lmDecay = 1.0f - 1.0f / m_linearMotorDecayTimescale;
+
+                    m_ffactor = 0.0f;
+                    if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body)
+                            && !rootPrim.m_isSelected && !rootPrim.m_disabled)
+                        d.BodyEnable(rootPrim.Body);
+                    break;
+                case Vehicle.LINEAR_MOTOR_OFFSET:
+                    m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z);
+                    len = m_linearMotorOffset.Length();
+                    if (len > 100.0f)
+                        m_linearMotorOffset *= (100.0f / len);
+                    break;
+                case Vehicle.BLOCK_EXIT:
+                    m_BlockingEndPoint = new Vector3(pValue.X, pValue.Y, pValue.Z);
+                    break;
+            }
+        }//end ProcessVectorVehicleParam
+
+        internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue)
+        {
+            switch (pParam)
+            {
+                case Vehicle.REFERENCE_FRAME:
+                    //                    m_referenceFrame = Quaternion.Inverse(pValue);
+                    m_referenceFrame = pValue;
+                    break;
+                case Vehicle.ROLL_FRAME:
+                    m_RollreferenceFrame = pValue;
+                    break;
+            }
+        }//end ProcessRotationVehicleParam
+
+        internal void ProcessVehicleFlags(int pParam, bool remove)
+        {
+            if (remove)
+            {
+                m_flags &= ~((VehicleFlag)pParam);
+            }
+            else
+            {
+                m_flags |= (VehicleFlag)pParam;
+            }
+        }//end ProcessVehicleFlags
+
+        internal void ProcessTypeChange(Vehicle pType)
+        {
+            m_lmEfect = 0;
+
+            m_amEfect = 0;
+            m_ffactor = 1f;
+
+            m_linearMotorDirection = Vector3.Zero;
+            m_angularMotorDirection = Vector3.Zero;
+
+            m_BlockingEndPoint = Vector3.Zero;
+            m_RollreferenceFrame = Quaternion.Identity;
+            m_linearMotorOffset = Vector3.Zero;
+
+            m_referenceFrame = Quaternion.Identity;
+
+            // Set Defaults For Type
+            m_type = pType;
+            switch (pType)
+            {
+                case Vehicle.TYPE_NONE:
+                    m_linearFrictionTimescale = new Vector3(1000, 1000, 1000);
+                    m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
+                    m_linearMotorTimescale = 1000;
+                    m_linearMotorDecayTimescale = 120 * m_invtimestep;
+                    m_angularMotorTimescale = 1000;
+                    m_angularMotorDecayTimescale = 1000  * m_invtimestep;
+                    m_VhoverHeight = 0;
+                    m_VhoverEfficiency = 1;
+                    m_VhoverTimescale = 1000;
+                    m_VehicleBuoyancy = 0;
+                    m_linearDeflectionEfficiency = 0;
+                    m_linearDeflectionTimescale = 1000;
+                    m_angularDeflectionEfficiency = 0;
+                    m_angularDeflectionTimescale = 1000;
+                    m_bankingEfficiency = 0;
+                    m_bankingMix = 1;
+                    m_bankingTimescale = 1000;
+                    m_verticalAttractionEfficiency = 0;
+                    m_verticalAttractionTimescale = 1000;
+
+                    m_flags = (VehicleFlag)0;
+                    break;
+
+                case Vehicle.TYPE_SLED:
+                    m_linearFrictionTimescale = new Vector3(30, 1, 1000);
+                    m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
+                    m_linearMotorTimescale = 1000;
+                    m_linearMotorDecayTimescale = 120 * m_invtimestep;
+                    m_angularMotorTimescale = 1000;
+                    m_angularMotorDecayTimescale = 120 * m_invtimestep;
+                    m_VhoverHeight = 0;
+                    m_VhoverEfficiency = 1;
+                    m_VhoverTimescale = 10;
+                    m_VehicleBuoyancy = 0;
+                    m_linearDeflectionEfficiency = 1;
+                    m_linearDeflectionTimescale = 1;
+                    m_angularDeflectionEfficiency = 0;
+                    m_angularDeflectionTimescale = 10;
+                    m_verticalAttractionEfficiency = 1;
+                    m_verticalAttractionTimescale = 1000;
+                    m_bankingEfficiency = 0;
+                    m_bankingMix = 1;
+                    m_bankingTimescale = 10;
+                    m_flags &=
+                         ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
+                           VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
+                    m_flags |= (VehicleFlag.NO_DEFLECTION_UP |
+                        VehicleFlag.LIMIT_ROLL_ONLY |
+                        VehicleFlag.LIMIT_MOTOR_UP);
+                    break;
+
+                case Vehicle.TYPE_CAR:
+                    m_linearFrictionTimescale = new Vector3(100, 2, 1000);
+                    m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
+                    m_linearMotorTimescale = 1;
+                    m_linearMotorDecayTimescale = 60 * m_invtimestep;
+                    m_angularMotorTimescale = 1;
+                    m_angularMotorDecayTimescale = 0.8f * m_invtimestep;
+                    m_VhoverHeight = 0;
+                    m_VhoverEfficiency = 0;
+                    m_VhoverTimescale = 1000;
+                    m_VehicleBuoyancy = 0;
+                    m_linearDeflectionEfficiency = 1;
+                    m_linearDeflectionTimescale = 2;
+                    m_angularDeflectionEfficiency = 0;
+                    m_angularDeflectionTimescale = 10;
+                    m_verticalAttractionEfficiency = 1f;
+                    m_verticalAttractionTimescale = 10f;
+                    m_bankingEfficiency = -0.2f;
+                    m_bankingMix = 1;
+                    m_bankingTimescale = 1;
+                    m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY |
+                                VehicleFlag.HOVER_TERRAIN_ONLY |
+                                VehicleFlag.HOVER_GLOBAL_HEIGHT);
+                    m_flags |= (VehicleFlag.NO_DEFLECTION_UP |
+                                VehicleFlag.LIMIT_ROLL_ONLY |
+                                VehicleFlag.LIMIT_MOTOR_UP |
+                                VehicleFlag.HOVER_UP_ONLY);
+                    break;
+                case Vehicle.TYPE_BOAT:
+                    m_linearFrictionTimescale = new Vector3(10, 3, 2);
+                    m_angularFrictionTimescale = new Vector3(10, 10, 10);
+                    m_linearMotorTimescale = 5;
+                    m_linearMotorDecayTimescale = 60 * m_invtimestep;
+                    m_angularMotorTimescale = 4;
+                    m_angularMotorDecayTimescale = 4 * m_invtimestep;
+                    m_VhoverHeight = 0;
+                    m_VhoverEfficiency = 0.5f;
+                    m_VhoverTimescale = 2;
+                    m_VehicleBuoyancy = 1;
+                    m_linearDeflectionEfficiency = 0.5f;
+                    m_linearDeflectionTimescale = 3;
+                    m_angularDeflectionEfficiency = 0.5f;
+                    m_angularDeflectionTimescale = 5;
+                    m_verticalAttractionEfficiency = 0.5f;
+                    m_verticalAttractionTimescale = 5f;
+                    m_bankingEfficiency = -0.3f;
+                    m_bankingMix = 0.8f;
+                    m_bankingTimescale = 1;
+                    m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY |
+                            VehicleFlag.HOVER_GLOBAL_HEIGHT |
+                            VehicleFlag.HOVER_UP_ONLY); // |
+//                            VehicleFlag.LIMIT_ROLL_ONLY);
+                    m_flags |= (VehicleFlag.NO_DEFLECTION_UP |
+                                VehicleFlag.LIMIT_MOTOR_UP |
+                                VehicleFlag.HOVER_UP_ONLY |  // new sl
+                                VehicleFlag.HOVER_WATER_ONLY);
+                    break;
+
+                case Vehicle.TYPE_AIRPLANE:
+                    m_linearFrictionTimescale = new Vector3(200, 10, 5);
+                    m_angularFrictionTimescale = new Vector3(20, 20, 20);
+                    m_linearMotorTimescale = 2;
+                    m_linearMotorDecayTimescale = 60 * m_invtimestep;
+                    m_angularMotorTimescale = 4;
+                    m_angularMotorDecayTimescale = 8 * m_invtimestep;
+                    m_VhoverHeight = 0;
+                    m_VhoverEfficiency = 0.5f;
+                    m_VhoverTimescale = 1000;
+                    m_VehicleBuoyancy = 0;
+                    m_linearDeflectionEfficiency = 0.5f;
+                    m_linearDeflectionTimescale = 0.5f;
+                    m_angularDeflectionEfficiency = 1;
+                    m_angularDeflectionTimescale = 2;
+                    m_verticalAttractionEfficiency = 0.9f;
+                    m_verticalAttractionTimescale = 2f;
+                    m_bankingEfficiency = 1;
+                    m_bankingMix = 0.7f;
+                    m_bankingTimescale = 2;
+                    m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY |
+                        VehicleFlag.HOVER_TERRAIN_ONLY |
+                        VehicleFlag.HOVER_GLOBAL_HEIGHT |
+                        VehicleFlag.HOVER_UP_ONLY |
+                        VehicleFlag.NO_DEFLECTION_UP |
+                        VehicleFlag.LIMIT_MOTOR_UP);
+                    m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY);
+                    break;
+
+                case Vehicle.TYPE_BALLOON:
+                    m_linearFrictionTimescale = new Vector3(5, 5, 5);
+                    m_angularFrictionTimescale = new Vector3(10, 10, 10);
+                    m_linearMotorTimescale = 5;
+                    m_linearMotorDecayTimescale = 60 * m_invtimestep;
+                    m_angularMotorTimescale = 6;
+                    m_angularMotorDecayTimescale = 10 * m_invtimestep;
+                    m_VhoverHeight = 5;
+                    m_VhoverEfficiency = 0.8f;
+                    m_VhoverTimescale = 10;
+                    m_VehicleBuoyancy = 1;
+                    m_linearDeflectionEfficiency = 0;
+                    m_linearDeflectionTimescale = 5 * m_invtimestep;
+                    m_angularDeflectionEfficiency = 0;
+                    m_angularDeflectionTimescale = 5;
+                    m_verticalAttractionEfficiency = 1f;
+                    m_verticalAttractionTimescale = 1000f;
+                    m_bankingEfficiency = 0;
+                    m_bankingMix = 0.7f;
+                    m_bankingTimescale = 5;
+                    m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY |
+                        VehicleFlag.HOVER_TERRAIN_ONLY |
+                        VehicleFlag.HOVER_UP_ONLY |
+                        VehicleFlag.NO_DEFLECTION_UP |
+                        VehicleFlag.LIMIT_MOTOR_UP  | //);
+                        VehicleFlag.LIMIT_ROLL_ONLY | // new sl
+                        VehicleFlag.HOVER_GLOBAL_HEIGHT); // new sl
+
+//                    m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY |
+//                        VehicleFlag.HOVER_GLOBAL_HEIGHT);
+                    break;
+
+            }
+            // disable mouse steering
+            m_flags &= ~(VehicleFlag.MOUSELOOK_STEER |
+                         VehicleFlag.MOUSELOOK_BANK  |
+                         VehicleFlag.CAMERA_DECOUPLED);
+
+            m_lmDecay = (1.0f - 1.0f / m_linearMotorDecayTimescale);
+            m_amDecay = 1.0f - 1.0f / m_angularMotorDecayTimescale;
+
+        }//end SetDefaultsForType
+
+        internal void Stop()
+        {
+            m_lmEfect = 0;
+            m_lmDecay = 0f;
+            m_amEfect = 0;
+            m_amDecay = 0;
+            m_ffactor = 1f;
+        }
+
+        public static Vector3 Xrot(Quaternion rot)
+        {
+            Vector3 vec;
+            rot.Normalize(); // just in case
+            vec.X = 2 * (rot.X * rot.X + rot.W * rot.W) - 1;
+            vec.Y = 2 * (rot.X * rot.Y + rot.Z * rot.W);
+            vec.Z = 2 * (rot.X * rot.Z - rot.Y * rot.W);
+            return vec;
+        }
+
+        public static Vector3 Zrot(Quaternion rot)
+        {
+            Vector3 vec;
+            rot.Normalize(); // just in case
+            vec.X = 2 * (rot.X * rot.Z + rot.Y * rot.W);
+            vec.Y = 2 * (rot.Y * rot.Z - rot.X * rot.W);
+            vec.Z = 2 * (rot.Z * rot.Z + rot.W * rot.W) - 1;
+
+            return vec;
+        }
+
+        private const float pi = (float)Math.PI;
+        private const float halfpi = 0.5f * (float)Math.PI;
+        private const float twopi = 2.0f * pi;
+
+        public static Vector3 ubRot2Euler(Quaternion rot)
+        {
+            // returns roll in X
+            //         pitch in Y
+            //         yaw in Z
+            Vector3 vec;
+
+            // assuming rot is normalised
+            // rot.Normalize();
+
+            float zX = rot.X * rot.Z + rot.Y * rot.W;
+
+            if (zX < -0.49999f)
+            {
+                vec.X = 0;
+                vec.Y = -halfpi;
+                vec.Z = (float)(-2d * Math.Atan(rot.X / rot.W));
+            }
+            else if (zX > 0.49999f)
+            {
+                vec.X = 0;
+                vec.Y = halfpi;
+                vec.Z = (float)(2d * Math.Atan(rot.X / rot.W));
+            }
+            else
+            {
+                vec.Y = (float)Math.Asin(2 * zX);
+
+                float sqw = rot.W * rot.W;
+
+                float minuszY = rot.X * rot.W - rot.Y * rot.Z;
+                float zZ = rot.Z * rot.Z + sqw - 0.5f;
+
+                vec.X = (float)Math.Atan2(minuszY, zZ);
+
+                float yX = rot.Z * rot.W - rot.X * rot.Y; //( have negative ?)
+                float yY = rot.X * rot.X + sqw - 0.5f;
+                vec.Z = (float)Math.Atan2(yX, yY);
+            }
+            return vec;
+        }
+
+        public static void GetRollPitch(Quaternion rot, out float roll, out float pitch)
+        {
+            // assuming rot is normalised
+            // rot.Normalize();
+
+            float zX = rot.X * rot.Z + rot.Y * rot.W;
+
+            if (zX < -0.49999f)
+            {
+                roll = 0;
+                pitch = -halfpi;
+            }
+            else if (zX > 0.49999f)
+            {
+                roll = 0;
+                pitch = halfpi;
+            }
+            else
+            {
+                pitch = (float)Math.Asin(2 * zX);
+
+                float minuszY = rot.X * rot.W - rot.Y * rot.Z;
+                float zZ = rot.Z * rot.Z + rot.W * rot.W - 0.5f;
+
+                roll = (float)Math.Atan2(minuszY, zZ);
+            }
+            return ;
+        }
+
+        internal void Step()
+        {
+            IntPtr Body = rootPrim.Body;
+
+            d.Mass dmass;
+            d.BodyGetMass(Body, out dmass);
+
+            d.Quaternion rot = d.BodyGetQuaternion(Body);
+            Quaternion objrotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);    // rotq = rotation of object
+            Quaternion rotq = objrotq;    // rotq = rotation of object
+            rotq *= m_referenceFrame; // rotq is now rotation in vehicle reference frame
+            Quaternion irotq = Quaternion.Inverse(rotq);
+
+            d.Vector3 dvtmp;
+            Vector3 tmpV;
+            Vector3 curVel; // velocity in world
+            Vector3 curAngVel; // angular velocity in world
+            Vector3 force = Vector3.Zero; // actually linear aceleration until mult by mass in world frame
+            Vector3 torque = Vector3.Zero;// actually angular aceleration until mult by Inertia in vehicle frame
+            d.Vector3 dtorque = new d.Vector3();
+
+            dvtmp = d.BodyGetLinearVel(Body);
+            curVel.X = dvtmp.X;
+            curVel.Y = dvtmp.Y;
+            curVel.Z = dvtmp.Z;
+            Vector3 curLocalVel = curVel * irotq; // current velocity in  local
+
+            dvtmp = d.BodyGetAngularVel(Body);
+            curAngVel.X = dvtmp.X;
+            curAngVel.Y = dvtmp.Y;
+            curAngVel.Z = dvtmp.Z;
+            Vector3 curLocalAngVel = curAngVel * irotq; // current angular velocity in  local
+
+            float ldampZ = 0;
+
+            bool mousemode = false;
+            bool mousemodebank = false;
+
+            float bankingEfficiency;
+            float verticalAttractionTimescale = m_verticalAttractionTimescale;
+
+            if((m_flags & (VehicleFlag.MOUSELOOK_STEER | VehicleFlag.MOUSELOOK_BANK)) != 0 )
+            {
+                mousemode = true;
+                mousemodebank = (m_flags & VehicleFlag.MOUSELOOK_BANK) != 0;
+                if(mousemodebank)
+                {
+                    bankingEfficiency = m_bankingEfficiency;
+                    if(verticalAttractionTimescale < 149.9)
+                        verticalAttractionTimescale *= 2.0f; // reduce current instability
+                }
+                else
+                    bankingEfficiency = 0;
+            }
+            else
+                bankingEfficiency = m_bankingEfficiency;
+
+            // linear motor
+            if (m_lmEfect > 0.01 && m_linearMotorTimescale < 1000)
+            {
+                tmpV = m_linearMotorDirection - curLocalVel; // velocity error
+                tmpV *= m_lmEfect / m_linearMotorTimescale; // error to correct in this timestep
+                tmpV *= rotq; // to world
+
+                if ((m_flags & VehicleFlag.LIMIT_MOTOR_UP) != 0)
+                    tmpV.Z = 0;
+
+                if (m_linearMotorOffset.X != 0 || m_linearMotorOffset.Y != 0 || m_linearMotorOffset.Z != 0)
+                {
+                    // have offset, do it now
+                    tmpV *= dmass.mass;
+                    d.BodyAddForceAtRelPos(Body, tmpV.X, tmpV.Y, tmpV.Z, m_linearMotorOffset.X, m_linearMotorOffset.Y, m_linearMotorOffset.Z);
+                }
+                else
+                {
+                    force.X += tmpV.X;
+                    force.Y += tmpV.Y;
+                    force.Z += tmpV.Z;
+                }
+
+                m_lmEfect *= m_lmDecay;
+//                m_ffactor = 0.01f + 1e-4f * curVel.LengthSquared();
+                m_ffactor = 0.0f;
+            }
+            else
+            {
+                m_lmEfect = 0;
+                m_ffactor = 1f;
+            }
+
+            // hover
+            if (m_VhoverTimescale < 300 && rootPrim.prim_geom != IntPtr.Zero)
+            {
+                //                d.Vector3 pos = d.BodyGetPosition(Body);
+                d.Vector3 pos = d.GeomGetPosition(rootPrim.prim_geom);
+                pos.Z -= 0.21f; // minor offset that seems to be always there in sl
+
+                float t = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y);
+                float perr;
+
+                // default to global but don't go underground
+                perr = m_VhoverHeight - pos.Z;
+
+                if ((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == 0)
+                {
+                    if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) != 0)
+                    {
+                        perr += _pParentScene.GetWaterLevel();
+                    }
+                    else if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0)
+                    {
+                        perr += t;
+                    }
+                    else
+                    {
+                        float w = _pParentScene.GetWaterLevel();
+                        if (t > w)
+                            perr += t;
+                        else
+                            perr += w;
+                    }
+                }
+                else if (t > m_VhoverHeight)
+                        perr = t - pos.Z; ;
+
+                if ((m_flags & VehicleFlag.HOVER_UP_ONLY) == 0 || perr > -0.1)
+                {
+                    ldampZ = m_VhoverEfficiency * m_invtimestep;
+
+                    perr *= (1.0f + ldampZ) / m_VhoverTimescale;
+
+                    //                    force.Z += perr - curVel.Z * tmp;
+                    force.Z += perr;
+                    ldampZ *= -curVel.Z;
+
+                    force.Z += _pParentScene.gravityz * m_gravmod * (1f - m_VehicleBuoyancy);
+                }
+                else // no buoyancy
+                    force.Z += _pParentScene.gravityz;
+            }
+            else
+            {
+                // default gravity and Buoyancy
+                force.Z += _pParentScene.gravityz * m_gravmod * (1f - m_VehicleBuoyancy);
+            }
+
+            // linear deflection
+            if (m_linearDeflectionEfficiency > 0)
+            {
+                float len = curVel.Length();
+                if (len > 0.01) // if moving
+                {
+                    Vector3 atAxis;
+                    atAxis = Xrot(rotq); // where are we pointing to
+                    atAxis *= len; // make it same size as world velocity vector
+
+                    tmpV = -atAxis; // oposite direction
+                    atAxis -= curVel; // error to one direction
+                    len = atAxis.LengthSquared();
+
+                    tmpV -= curVel; // error to oposite
+                    float lens = tmpV.LengthSquared();
+
+                    if (len > 0.01 || lens > 0.01) // do nothing if close enougth
+                    {
+                        if (len < lens)
+                            tmpV = atAxis;
+
+                        tmpV *= (m_linearDeflectionEfficiency / m_linearDeflectionTimescale); // error to correct in this timestep
+                        force.X += tmpV.X;
+                        force.Y += tmpV.Y;
+                        if ((m_flags & VehicleFlag.NO_DEFLECTION_UP) == 0)
+                            force.Z += tmpV.Z;
+                    }
+                }
+            }
+
+            // linear friction/damping
+            if (curLocalVel.X != 0 || curLocalVel.Y != 0 || curLocalVel.Z != 0)
+            {
+                tmpV.X = -curLocalVel.X / m_linearFrictionTimescale.X;
+                tmpV.Y = -curLocalVel.Y / m_linearFrictionTimescale.Y;
+                tmpV.Z = -curLocalVel.Z / m_linearFrictionTimescale.Z;
+                tmpV *= rotq; // to world
+
+                if(ldampZ != 0 && Math.Abs(ldampZ) > Math.Abs(tmpV.Z))
+                    tmpV.Z = ldampZ;
+                force.X += tmpV.X;
+                force.Y += tmpV.Y;
+                force.Z += tmpV.Z;
+            }
+
+            // vertical atractor
+            if (verticalAttractionTimescale < 300)
+            {
+                float roll;
+                float pitch;
+
+                float ftmp = m_invtimestep / verticalAttractionTimescale / verticalAttractionTimescale;
+
+                float ftmp2;
+                ftmp2 = 0.5f * m_verticalAttractionEfficiency * m_invtimestep;
+                m_amdampX = ftmp2;
+
+                m_ampwr = 1.0f - 0.8f * m_verticalAttractionEfficiency;
+
+                GetRollPitch(irotq, out roll, out pitch);
+
+                if (roll > halfpi)
+                    roll = pi - roll;
+                else if (roll < -halfpi)
+                    roll = -pi - roll;
+
+                float effroll = pitch / halfpi;
+                effroll *= effroll;
+                effroll = 1 - effroll;
+                effroll *= roll;
+
+                torque.X += effroll * ftmp;
+
+                if ((m_flags & VehicleFlag.LIMIT_ROLL_ONLY) == 0)
+                {
+                    float effpitch = roll / halfpi;
+                    effpitch *= effpitch;
+                    effpitch = 1 - effpitch;
+                    effpitch *= pitch;
+
+                    torque.Y += effpitch * ftmp;
+                }
+
+                if (bankingEfficiency != 0 && Math.Abs(effroll) > 0.01)
+                {
+
+                    float broll = effroll;
+                    /*
+                                        if (broll > halfpi)
+                                            broll = pi - broll;
+                                        else if (broll < -halfpi)
+                                            broll = -pi - broll;
+                    */
+                    broll *= m_bankingEfficiency;
+                    if (m_bankingMix != 0)
+                    {
+                        float vfact = Math.Abs(curLocalVel.X) / 10.0f;
+                        if (vfact > 1.0f) vfact = 1.0f;
+
+                        if (curLocalVel.X >= 0)
+                            broll *= (1 + (vfact - 1) * m_bankingMix);
+                        else
+                            broll *= -(1 + (vfact - 1) * m_bankingMix);
+                    }
+                    // make z rot be in world Z not local as seems to be in sl
+
+                    broll = broll / m_bankingTimescale;
+
+
+                    tmpV = Zrot(irotq);
+                    tmpV *= broll;
+
+                    torque.X += tmpV.X;
+                    torque.Y += tmpV.Y;
+                    torque.Z += tmpV.Z;
+
+                    m_amdampZ = Math.Abs(m_bankingEfficiency) / m_bankingTimescale;
+                    m_amdampY = m_amdampZ;
+
+                }
+                else
+                {
+                    m_amdampZ = 1 / m_angularFrictionTimescale.Z;
+                    m_amdampY = m_amdampX;
+                }
+            }
+            else
+            {
+                m_ampwr = 1.0f;
+                m_amdampX = 1 / m_angularFrictionTimescale.X;
+                m_amdampY = 1 / m_angularFrictionTimescale.Y;
+                m_amdampZ = 1 / m_angularFrictionTimescale.Z;
+            }
+
+            if(mousemode)
+            {
+                CameraData cam = rootPrim.TryGetCameraData();
+                if(cam.Valid && cam.MouseLook)
+                {
+                    Vector3 dirv = cam.CameraAtAxis * irotq;
+
+                    float invamts = 1.0f/m_angularMotorTimescale;
+                    float tmp;
+
+                    // get out of x == 0 plane
+                    if(Math.Abs(dirv.X) < 0.001f)
+                        dirv.X = 0.001f;
+
+                    if (Math.Abs(dirv.Z) > 0.01)
+                    {
+                        tmp = -(float)Math.Atan2(dirv.Z, dirv.X) * m_angularMotorDirection.Y;
+                        if(tmp < -4f)
+                            tmp = -4f;
+                        else if(tmp > 4f)
+                            tmp = 4f;
+                        torque.Y += (tmp - curLocalAngVel.Y) * invamts;
+                        torque.Y -= curLocalAngVel.Y * m_amdampY;
+                    }
+                    else
+                        torque.Y -= curLocalAngVel.Y * m_invtimestep;
+
+                    if (Math.Abs(dirv.Y) > 0.01)
+                    {
+                        if(mousemodebank)
+                        {
+                            tmp = -(float)Math.Atan2(dirv.Y, dirv.X) * m_angularMotorDirection.X;
+                            if(tmp < -4f)
+                                tmp = -4f;
+                            else if(tmp > 4f)
+                                tmp = 4f;
+                            torque.X += (tmp - curLocalAngVel.X) * invamts;
+                        }
+                        else
+                        {
+                            tmp = (float)Math.Atan2(dirv.Y, dirv.X) * m_angularMotorDirection.Z;
+                            tmp *= invamts;
+                            if(tmp < -4f)
+                                tmp = -4f;
+                            else if(tmp > 4f)
+                                tmp = 4f;
+                            torque.Z += (tmp - curLocalAngVel.Z) * invamts;
+                        }
+                        torque.X -= curLocalAngVel.X * m_amdampX;
+                        torque.Z -= curLocalAngVel.Z * m_amdampZ;
+                    }
+                    else
+                    {
+                        if(mousemodebank)
+                            torque.X -= curLocalAngVel.X * m_invtimestep;
+                        else
+                            torque.Z -= curLocalAngVel.Z * m_invtimestep;
+                    }
+                }
+                else
+                {
+                    if (curLocalAngVel.X != 0 || curLocalAngVel.Y != 0 || curLocalAngVel.Z != 0)
+                    {
+                        torque.X -= curLocalAngVel.X * 10f;
+                        torque.Y -= curLocalAngVel.Y * 10f;
+                        torque.Z -= curLocalAngVel.Z * 10f;
+                    }
+                }
+            }
+            else
+            {
+                // angular motor
+                if (m_amEfect > 0.01 && m_angularMotorTimescale < 1000)
+                {
+                    tmpV = m_angularMotorDirection - curLocalAngVel; // velocity error
+                    tmpV *= m_amEfect / m_angularMotorTimescale; // error to correct in this timestep
+                    torque.X += tmpV.X * m_ampwr;
+                    torque.Y += tmpV.Y * m_ampwr;
+                    torque.Z += tmpV.Z;
+
+                    m_amEfect *= m_amDecay;
+                }
+                else
+                    m_amEfect = 0;
+
+                // angular deflection
+                if (m_angularDeflectionEfficiency > 0)
+                {
+                    Vector3 dirv;
+
+                    if (curLocalVel.X > 0.01f)
+                        dirv = curLocalVel;
+                    else if (curLocalVel.X < -0.01f)
+                        // use oposite
+                        dirv = -curLocalVel;
+                    else
+                    {
+                        // make it fall into small positive x case
+                        dirv.X = 0.01f;
+                        dirv.Y = curLocalVel.Y;
+                        dirv.Z = curLocalVel.Z;
+                    }
+
+                    float ftmp = m_angularDeflectionEfficiency / m_angularDeflectionTimescale;
+
+                    if (Math.Abs(dirv.Z) > 0.01)
+                    {
+                        torque.Y += - (float)Math.Atan2(dirv.Z, dirv.X) * ftmp;
+                    }
+
+                    if (Math.Abs(dirv.Y) > 0.01)
+                    {
+                        torque.Z += (float)Math.Atan2(dirv.Y, dirv.X) * ftmp;
+                    }
+                }
+
+                if (curLocalAngVel.X != 0 || curLocalAngVel.Y != 0 || curLocalAngVel.Z != 0)
+                {
+                    torque.X -= curLocalAngVel.X * m_amdampX;
+                    torque.Y -= curLocalAngVel.Y * m_amdampY;
+                    torque.Z -= curLocalAngVel.Z * m_amdampZ;
+                }
+            }
+
+            force *= dmass.mass;
+
+            force += rootPrim.m_force;
+            force += rootPrim.m_forceacc;
+            rootPrim.m_forceacc = Vector3.Zero;
+
+            if (force.X != 0 || force.Y != 0 || force.Z != 0)
+            {
+                d.BodyAddForce(Body, force.X, force.Y, force.Z);
+            }
+
+            if (torque.X != 0 || torque.Y != 0 || torque.Z != 0)
+            {
+                torque *= m_referenceFrame; // to object frame
+                dtorque.X = torque.X ;
+                dtorque.Y = torque.Y;
+                dtorque.Z = torque.Z;
+
+                d.MultiplyM3V3(out dvtmp, ref dmass.I, ref dtorque);
+                d.BodyAddRelTorque(Body, dvtmp.X, dvtmp.Y, dvtmp.Z); // add torque in object frame
+            }
+
+            torque = rootPrim.m_torque;
+            torque += rootPrim.m_angularForceacc;
+            rootPrim.m_angularForceacc = Vector3.Zero;
+            if (torque.X != 0 || torque.Y != 0 || torque.Z != 0)
+                d.BodyAddTorque(Body,torque.X, torque.Y, torque.Z);
+        }
+    }
+}
diff --git a/OpenSim/Region/PhysicsModules/ubOde/ODEMeshWorker.cs b/OpenSim/Region/PhysicsModules/ubOde/ODEMeshWorker.cs
new file mode 100644
index 0000000..5465035
--- /dev/null
+++ b/OpenSim/Region/PhysicsModules/ubOde/ODEMeshWorker.cs
@@ -0,0 +1,946 @@
+/*
+ * AJLDuarte 2012
+ */
+
+using System;
+using System.Threading;
+using System.Collections.Generic;
+using System.IO;
+using System.Reflection;
+using System.Runtime.InteropServices;
+using System.Text;
+using OpenSim.Framework;
+using OpenSim.Region.PhysicsModules.SharedBase;
+using OdeAPI;
+using log4net;
+using Nini.Config;
+using OpenMetaverse;
+
+namespace OpenSim.Region.PhysicsModule.ubOde
+{
+    public enum MeshState : byte
+    {
+        noNeed = 0,
+
+        loadingAsset = 1,
+
+        AssetOK = 0x0f,     // 00001111
+
+        NeedMask = 0x30,    // 00110000
+        needMesh = 0x10,    // 00010000
+        needAsset = 0x20,   // 00100000
+
+        FailMask = 0xC0,    // 11000000
+        AssetFailed = 0x40, // 01000000
+        MeshFailed = 0x80,   // 10000000
+
+        MeshNoColide = FailMask | needAsset
+    }
+
+    public enum meshWorkerCmnds : byte
+    {
+        nop = 0,
+        addnew,
+        changefull,
+        changesize,
+        changeshapetype,
+        getmesh,
+    }
+
+    public class ODEPhysRepData
+    {
+        public PhysicsActor actor;
+        public PrimitiveBaseShape pbs;
+        public IMesh mesh;
+
+        public Vector3 size;
+        public Vector3 OBB;
+        public Vector3 OBBOffset;
+
+        public float volume;
+
+        public byte shapetype;
+        public bool hasOBB;
+        public bool hasMeshVolume;
+        public bool isTooSmall;
+        public MeshState meshState;
+        public UUID? assetID;
+        public meshWorkerCmnds comand;
+    }
+
+    public class ODEMeshWorker
+    {
+        private ILog m_log;
+        private ODEScene m_scene;
+        private IMesher m_mesher;
+
+        public bool meshSculptedPrim = true;
+        public float meshSculptLOD = 32;
+        public float MeshSculptphysicalLOD = 32;
+        public float MinSizeToMeshmerize = 0.1f;
+
+        private OpenSim.Framework.BlockingQueue<ODEPhysRepData> workQueue = new OpenSim.Framework.BlockingQueue<ODEPhysRepData>();
+        private bool m_running;
+
+        private Thread m_thread;
+
+        public ODEMeshWorker(ODEScene pScene, ILog pLog, IMesher pMesher, IConfig pConfig)
+        {
+            m_scene = pScene;
+            m_log = pLog;
+            m_mesher = pMesher;
+
+            if (pConfig != null)
+            {
+                meshSculptedPrim = pConfig.GetBoolean("mesh_sculpted_prim", meshSculptedPrim);
+                meshSculptLOD = pConfig.GetFloat("mesh_lod", meshSculptLOD);
+                MinSizeToMeshmerize =  pConfig.GetFloat("mesh_min_size", MinSizeToMeshmerize);
+                MeshSculptphysicalLOD = pConfig.GetFloat("mesh_physical_lod", MeshSculptphysicalLOD);
+            }
+            m_running = true;
+            m_thread = new Thread(DoWork);
+            m_thread.Name = "OdeMeshWorker";
+            m_thread.Start();
+        }
+
+        private void DoWork()
+        {
+            m_mesher.ExpireFileCache();
+
+            while(m_running)
+            {
+                 ODEPhysRepData nextRep = workQueue.Dequeue();
+                if(!m_running)
+                    return;
+                if (nextRep == null)
+                    continue;
+                if (m_scene.haveActor(nextRep.actor))
+                {
+                    switch (nextRep.comand)
+                    {
+                        case meshWorkerCmnds.changefull:
+                        case meshWorkerCmnds.changeshapetype:
+                        case meshWorkerCmnds.changesize:
+                            GetMesh(nextRep);
+                            if (CreateActorPhysRep(nextRep) && m_scene.haveActor(nextRep.actor))
+                                m_scene.AddChange(nextRep.actor, changes.PhysRepData, nextRep);
+                            break;
+                        case meshWorkerCmnds.getmesh:
+                            DoRepDataGetMesh(nextRep);
+                            break;
+                    }
+                }
+            }
+        }
+
+        public void Stop()
+        {
+            try
+            {
+                m_thread.Abort();
+                workQueue.Clear();
+            }
+            catch
+            {
+            }
+        }
+
+        public void ChangeActorPhysRep(PhysicsActor actor, PrimitiveBaseShape pbs,
+                                        Vector3 size, byte shapetype)
+        {
+            ODEPhysRepData repData = new ODEPhysRepData();
+            repData.actor = actor;
+            repData.pbs = pbs;
+            repData.size = size;
+            repData.shapetype = shapetype;
+
+            CheckMesh(repData);
+            CalcVolumeData(repData);
+            m_scene.AddChange(actor, changes.PhysRepData, repData);
+            return;
+        }
+
+        public ODEPhysRepData NewActorPhysRep(PhysicsActor actor, PrimitiveBaseShape pbs,
+                                        Vector3 size, byte shapetype)
+        {
+            ODEPhysRepData repData = new ODEPhysRepData();
+            repData.actor = actor;
+            repData.pbs = pbs;
+            repData.size = size;
+            repData.shapetype = shapetype;
+
+            CheckMesh(repData);
+            CalcVolumeData(repData);
+            m_scene.AddChange(actor, changes.AddPhysRep, repData);
+            return repData;
+        }
+
+        public void RequestMesh(ODEPhysRepData repData)
+        {
+            repData.mesh = null;
+
+            if (repData.meshState == MeshState.needAsset)
+            {
+                PrimitiveBaseShape pbs = repData.pbs;
+
+                // check if we got outdated
+
+                if (!pbs.SculptEntry || pbs.SculptTexture == UUID.Zero)
+                {
+                    repData.meshState = MeshState.noNeed;
+                    return;
+                }
+
+                repData.assetID = pbs.SculptTexture;
+                repData.meshState = MeshState.loadingAsset;
+
+                repData.comand = meshWorkerCmnds.getmesh;
+                workQueue.Enqueue(repData);
+            }
+        }
+
+        // creates and prepares a mesh to use and calls parameters estimation
+        public bool CreateActorPhysRep(ODEPhysRepData repData)
+        {
+            IMesh mesh = repData.mesh;
+
+            if (mesh != null)
+            {
+                IntPtr vertices, indices;
+                int vertexCount, indexCount;
+                int vertexStride, triStride;
+
+                mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount);
+                mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount);
+
+                if (vertexCount == 0 || indexCount == 0)
+                {
+                    m_log.WarnFormat("[PHYSICS]: Invalid mesh data on prim {0} mesh UUID {1}",
+                        repData.actor.Name, repData.pbs.SculptTexture.ToString());
+                    repData.meshState = MeshState.MeshFailed;
+                    repData.hasOBB = false;
+                    repData.mesh = null;
+                    m_scene.mesher.ReleaseMesh(mesh);
+                }
+                else
+                {
+                    repData.OBBOffset = mesh.GetCentroid();
+                    repData.OBB = mesh.GetOBB();
+                    repData.hasOBB = true;
+                    mesh.releaseSourceMeshData();
+                }
+            }
+            CalcVolumeData(repData);
+            return true;
+        }
+
+        public void AssetLoaded(ODEPhysRepData repData)
+        {
+            if (m_scene.haveActor(repData.actor))
+            {
+                if (needsMeshing(repData)) // no need for pbs now?
+                {
+                    repData.comand = meshWorkerCmnds.changefull;
+                    workQueue.Enqueue(repData);
+                }
+            }
+            else
+                repData.pbs.SculptData = Utils.EmptyBytes;
+        }
+
+        public void DoRepDataGetMesh(ODEPhysRepData repData)
+        {
+            if (!repData.pbs.SculptEntry)
+                return;
+
+            if (repData.meshState != MeshState.loadingAsset)
+                return;
+
+            if (repData.assetID == null || repData.assetID == UUID.Zero)
+                return;
+
+            if (repData.assetID != repData.pbs.SculptTexture)
+                return;
+
+            // check if it is in cache
+            GetMesh(repData);
+            if (repData.meshState != MeshState.needAsset)
+            {
+                CreateActorPhysRep(repData);
+                m_scene.AddChange(repData.actor, changes.PhysRepData, repData);
+                return;
+            }
+
+            RequestAssetDelegate assetProvider = m_scene.RequestAssetMethod;
+            if (assetProvider == null)
+                return;
+            ODEAssetRequest asr = new ODEAssetRequest(this, assetProvider, repData, m_log);
+        }
+
+
+        /// <summary>
+        /// Routine to figure out if we need to mesh this prim with our mesher
+        /// </summary>
+        /// <param name="pbs"></param>
+        /// <returns></returns>
+        public bool needsMeshing(ODEPhysRepData repData)
+        {
+            PrimitiveBaseShape pbs = repData.pbs;
+            // check sculpts or meshs
+
+            Vector3 scale = pbs.Scale;
+            if(scale.X <= MinSizeToMeshmerize &&
+               scale.Y <= MinSizeToMeshmerize &&
+               scale.Z <= MinSizeToMeshmerize)
+            {
+                repData.isTooSmall = true;
+                return false;
+            }
+
+            if (pbs.SculptEntry)
+            {
+                if (meshSculptedPrim)
+                    return true;
+
+                if (pbs.SculptType == (byte)SculptType.Mesh) // always do meshs
+                    return true;
+
+                return false;
+            }
+
+            // convex shapes have no holes
+            ushort profilehollow = pbs.ProfileHollow;
+            if(repData.shapetype == 2)
+                profilehollow = 0;
+
+            // if it's a standard box or sphere with no cuts, hollows, twist or top shear, return false since ODE can use an internal representation for the prim
+
+            if ((pbs.ProfileShape == ProfileShape.Square && pbs.PathCurve == (byte)Extrusion.Straight)
+                    || (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1
+                    && pbs.Scale.X == pbs.Scale.Y && pbs.Scale.Y == pbs.Scale.Z))
+            {
+
+                if (pbs.ProfileBegin == 0 && pbs.ProfileEnd == 0
+                    && profilehollow == 0
+                    && pbs.PathTwist == 0 && pbs.PathTwistBegin == 0
+                    && pbs.PathBegin == 0 && pbs.PathEnd == 0
+                    && pbs.PathTaperX == 0 && pbs.PathTaperY == 0
+                    && pbs.PathScaleX == 100 && pbs.PathScaleY == 100
+                    && pbs.PathShearX == 0 && pbs.PathShearY == 0)
+                {
+                    return false;
+                }
+            }
+
+            //  following code doesn't give meshs to boxes and spheres ever
+            // and it's odd..  so for now just return true if asked to force meshs
+            // hopefully mesher will fail if doesn't suport so things still get basic boxes
+
+            int iPropertiesNotSupportedDefault = 0;
+
+            if (profilehollow != 0)
+                iPropertiesNotSupportedDefault++;
+
+            if ((pbs.PathBegin != 0) || pbs.PathEnd != 0)
+                iPropertiesNotSupportedDefault++;
+
+            if ((pbs.PathTwistBegin != 0) || (pbs.PathTwist != 0))
+                iPropertiesNotSupportedDefault++;
+
+            if ((pbs.ProfileBegin != 0) || pbs.ProfileEnd != 0)
+                iPropertiesNotSupportedDefault++;
+
+            if ((pbs.PathScaleX != 100) || (pbs.PathScaleY != 100))
+                iPropertiesNotSupportedDefault++;
+
+            if ((pbs.PathShearX != 0) || (pbs.PathShearY != 0))
+                iPropertiesNotSupportedDefault++;
+
+            if (pbs.ProfileShape == ProfileShape.Circle && pbs.PathCurve == (byte)Extrusion.Straight)
+                iPropertiesNotSupportedDefault++;
+
+            if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1 && (pbs.Scale.X != pbs.Scale.Y || pbs.Scale.Y != pbs.Scale.Z || pbs.Scale.Z != pbs.Scale.X))
+                iPropertiesNotSupportedDefault++;
+
+            if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1)
+                iPropertiesNotSupportedDefault++;
+
+            // test for torus
+            if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Square)
+            {
+                if (pbs.PathCurve == (byte)Extrusion.Curve1)
+                {
+                    iPropertiesNotSupportedDefault++;
+                }
+            }
+            else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Circle)
+            {
+                if (pbs.PathCurve == (byte)Extrusion.Straight)
+                {
+                    iPropertiesNotSupportedDefault++;
+                }
+
+                // ProfileCurve seems to combine hole shape and profile curve so we need to only compare against the lower 3 bits
+                else if (pbs.PathCurve == (byte)Extrusion.Curve1)
+                {
+                    iPropertiesNotSupportedDefault++;
+                }
+            }
+            else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.HalfCircle)
+            {
+                if (pbs.PathCurve == (byte)Extrusion.Curve1 || pbs.PathCurve == (byte)Extrusion.Curve2)
+                {
+                    iPropertiesNotSupportedDefault++;
+                }
+            }
+            else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.EquilateralTriangle)
+            {
+                if (pbs.PathCurve == (byte)Extrusion.Straight)
+                {
+                    iPropertiesNotSupportedDefault++;
+                }
+                else if (pbs.PathCurve == (byte)Extrusion.Curve1)
+                {
+                    iPropertiesNotSupportedDefault++;
+                }
+            }
+
+            if (iPropertiesNotSupportedDefault == 0)
+            {
+                return false;
+            }
+            return true;
+        }
+
+        // see if we need a mesh and if so if we have a cached one
+        // called with a new repData
+        public void CheckMesh(ODEPhysRepData repData)
+        {
+            PhysicsActor actor = repData.actor;
+            PrimitiveBaseShape pbs = repData.pbs;
+
+            if (!needsMeshing(repData))
+            {
+                repData.meshState = MeshState.noNeed;
+                repData.hasOBB = false;
+                return;
+            }
+
+            IMesh mesh = null;
+
+            Vector3 size = repData.size;
+
+            int clod = (int)LevelOfDetail.High;
+            byte shapetype = repData.shapetype;
+            bool convex = shapetype == 2;
+
+            mesh = m_mesher.GetMesh(actor.Name, pbs, size, clod, true, convex);
+
+            if (mesh == null)
+            {
+                if (pbs.SculptEntry)
+                {
+                    if (pbs.SculptTexture != null && pbs.SculptTexture != UUID.Zero)
+                    {
+                        repData.assetID = pbs.SculptTexture;
+                        repData.meshState = MeshState.needAsset;
+                    }
+                    else
+                        repData.meshState = MeshState.MeshFailed;
+
+                    return;
+                }
+                else
+                {
+                    repData.meshState = MeshState.needMesh;
+                    mesh = m_mesher.CreateMesh(actor.Name, pbs, size, clod, true, convex, true);
+                    if (mesh == null)
+                    {
+                        repData.meshState = MeshState.MeshFailed;
+                        return;
+                    }
+                }
+            }
+
+            repData.meshState = MeshState.AssetOK;
+            repData.mesh = mesh;
+            repData.OBB = mesh.GetOBB();
+            repData.OBBOffset = mesh.GetCentroid();
+            repData.hasOBB = true;
+
+            if (pbs.SculptEntry)
+            {
+                repData.assetID = pbs.SculptTexture;
+            }
+
+            pbs.SculptData = Utils.EmptyBytes;
+            return ;
+        }
+
+        public void GetMesh(ODEPhysRepData repData)
+        {
+            PhysicsActor actor = repData.actor;
+
+            PrimitiveBaseShape pbs = repData.pbs;
+
+            repData.mesh = null;
+            repData.hasOBB = false;
+
+            if (!needsMeshing(repData))
+            {
+                repData.meshState = MeshState.noNeed;
+                return;
+            }
+
+            if (repData.meshState == MeshState.MeshFailed)
+                return;
+
+            if (pbs.SculptEntry)
+            {
+                if (repData.meshState == MeshState.AssetFailed)
+                {
+                    if (pbs.SculptTexture == repData.assetID)
+                        return;
+                }
+            }
+
+            repData.meshState = MeshState.noNeed;
+
+            IMesh mesh = null;
+            Vector3 size = repData.size;
+            byte shapetype = repData.shapetype;
+
+            bool convex;
+            int clod = (int)LevelOfDetail.High;
+            if (shapetype == 0)
+                convex = false;
+            else
+            {
+                convex = true;
+                if (pbs.SculptType != (byte)SculptType.Mesh)
+                    clod = (int)LevelOfDetail.Low;
+            }
+
+            mesh = m_mesher.CreateMesh(actor.Name, pbs, size, clod, true, convex, true);
+
+            if (mesh == null)
+            {
+                if (pbs.SculptEntry)
+                {
+                    if (pbs.SculptTexture == UUID.Zero)
+                        return;
+
+                    repData.assetID = pbs.SculptTexture;
+
+                    if (pbs.SculptData == null || pbs.SculptData.Length == 0)
+                    {
+                        repData.meshState = MeshState.needAsset;
+                        return;
+                    }
+                }
+            }
+
+            repData.mesh = mesh;
+            repData.pbs.SculptData = Utils.EmptyBytes;
+
+            if (mesh == null)
+            {
+                if (pbs.SculptEntry)
+                    repData.meshState = MeshState.AssetFailed;
+                else
+                    repData.meshState = MeshState.MeshFailed;
+
+                return;
+            }
+
+            repData.meshState = MeshState.AssetOK;
+
+            return;
+        }
+
+        private void CalculateBasicPrimVolume(ODEPhysRepData repData)
+        {
+            Vector3 _size = repData.size;
+
+            float volume = _size.X * _size.Y * _size.Z; // default
+            if(repData.isTooSmall)
+            {
+                repData.volume = volume;
+                return;
+            }
+
+            PrimitiveBaseShape _pbs = repData.pbs;
+            float tmp;
+
+            float hollowAmount = (float)_pbs.ProfileHollow * 2.0e-5f;
+            float hollowVolume = hollowAmount * hollowAmount;
+
+            switch (_pbs.ProfileShape)
+            {
+                case ProfileShape.Square:
+                    // default box
+
+                    if (_pbs.PathCurve == (byte)Extrusion.Straight)
+                    {
+                        if (hollowAmount > 0.0)
+                        {
+                            switch (_pbs.HollowShape)
+                            {
+                                case HollowShape.Square:
+                                case HollowShape.Same:
+                                    break;
+
+                                case HollowShape.Circle:
+
+                                    hollowVolume *= 0.78539816339f;
+                                    break;
+
+                                case HollowShape.Triangle:
+
+                                    hollowVolume *= (0.5f * .5f);
+                                    break;
+
+                                default:
+                                    hollowVolume = 0;
+                                    break;
+                            }
+                            volume *= (1.0f - hollowVolume);
+                        }
+                    }
+
+                    else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
+                    {
+                        //a tube
+
+                        volume *= 0.78539816339e-2f * (float)(200 - _pbs.PathScaleX);
+                        tmp = 1.0f - 2.0e-2f * (float)(200 - _pbs.PathScaleY);
+                        volume -= volume * tmp * tmp;
+
+                        if (hollowAmount > 0.0)
+                        {
+                            hollowVolume *= hollowAmount;
+
+                            switch (_pbs.HollowShape)
+                            {
+                                case HollowShape.Square:
+                                case HollowShape.Same:
+                                    break;
+
+                                case HollowShape.Circle:
+                                    hollowVolume *= 0.78539816339f;
+                                    break;
+
+                                case HollowShape.Triangle:
+                                    hollowVolume *= 0.5f * 0.5f;
+                                    break;
+                                default:
+                                    hollowVolume = 0;
+                                    break;
+                            }
+                            volume *= (1.0f - hollowVolume);
+                        }
+                    }
+
+                    break;
+
+                case ProfileShape.Circle:
+
+                    if (_pbs.PathCurve == (byte)Extrusion.Straight)
+                    {
+                        volume *= 0.78539816339f; // elipse base
+
+                        if (hollowAmount > 0.0)
+                        {
+                            switch (_pbs.HollowShape)
+                            {
+                                case HollowShape.Same:
+                                case HollowShape.Circle:
+                                    break;
+
+                                case HollowShape.Square:
+                                    hollowVolume *= 0.5f * 2.5984480504799f;
+                                    break;
+
+                                case HollowShape.Triangle:
+                                    hollowVolume *= .5f * 1.27323954473516f;
+                                    break;
+
+                                default:
+                                    hollowVolume = 0;
+                                    break;
+                            }
+                            volume *= (1.0f - hollowVolume);
+                        }
+                    }
+
+                    else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
+                    {
+                        volume *= 0.61685027506808491367715568749226e-2f * (float)(200 - _pbs.PathScaleX);
+                        tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY);
+                        volume *= (1.0f - tmp * tmp);
+
+                        if (hollowAmount > 0.0)
+                        {
+
+                            // calculate the hollow volume by it's shape compared to the prim shape
+                            hollowVolume *= hollowAmount;
+
+                            switch (_pbs.HollowShape)
+                            {
+                                case HollowShape.Same:
+                                case HollowShape.Circle:
+                                    break;
+
+                                case HollowShape.Square:
+                                    hollowVolume *= 0.5f * 2.5984480504799f;
+                                    break;
+
+                                case HollowShape.Triangle:
+                                    hollowVolume *= .5f * 1.27323954473516f;
+                                    break;
+
+                                default:
+                                    hollowVolume = 0;
+                                    break;
+                            }
+                            volume *= (1.0f - hollowVolume);
+                        }
+                    }
+                    break;
+
+                case ProfileShape.HalfCircle:
+                    if (_pbs.PathCurve == (byte)Extrusion.Curve1)
+                    {
+                        volume *= 0.5236f;
+
+                        if (hollowAmount > 0.0)
+                        {
+                            hollowVolume *= hollowAmount;
+
+                            switch (_pbs.HollowShape)
+                            {
+                                case HollowShape.Circle:
+                                case HollowShape.Triangle:  // diference in sl is minor and odd
+                                case HollowShape.Same:
+                                    break;
+
+                                case HollowShape.Square:
+                                    hollowVolume *= 0.909f;
+                                    break;
+
+                                //                                case HollowShape.Triangle:
+                                //                                    hollowVolume *= .827f;
+                                //                                    break;
+                                default:
+                                    hollowVolume = 0;
+                                    break;
+                            }
+                            volume *= (1.0f - hollowVolume);
+                        }
+
+                    }
+                    break;
+
+                case ProfileShape.EquilateralTriangle:
+
+                    if (_pbs.PathCurve == (byte)Extrusion.Straight)
+                    {
+                        volume *= 0.32475953f;
+
+                        if (hollowAmount > 0.0)
+                        {
+
+                            // calculate the hollow volume by it's shape compared to the prim shape
+                            switch (_pbs.HollowShape)
+                            {
+                                case HollowShape.Same:
+                                case HollowShape.Triangle:
+                                    hollowVolume *= .25f;
+                                    break;
+
+                                case HollowShape.Square:
+                                    hollowVolume *= 0.499849f * 3.07920140172638f;
+                                    break;
+
+                                case HollowShape.Circle:
+                                    // Hollow shape is a perfect cyllinder in respect to the cube's scale
+                                    // Cyllinder hollow volume calculation
+
+                                    hollowVolume *= 0.1963495f * 3.07920140172638f;
+                                    break;
+
+                                default:
+                                    hollowVolume = 0;
+                                    break;
+                            }
+                            volume *= (1.0f - hollowVolume);
+                        }
+                    }
+                    else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
+                    {
+                        volume *= 0.32475953f;
+                        volume *= 0.01f * (float)(200 - _pbs.PathScaleX);
+                        tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY);
+                        volume *= (1.0f - tmp * tmp);
+
+                        if (hollowAmount > 0.0)
+                        {
+
+                            hollowVolume *= hollowAmount;
+
+                            switch (_pbs.HollowShape)
+                            {
+                                case HollowShape.Same:
+                                case HollowShape.Triangle:
+                                    hollowVolume *= .25f;
+                                    break;
+
+                                case HollowShape.Square:
+                                    hollowVolume *= 0.499849f * 3.07920140172638f;
+                                    break;
+
+                                case HollowShape.Circle:
+
+                                    hollowVolume *= 0.1963495f * 3.07920140172638f;
+                                    break;
+
+                                default:
+                                    hollowVolume = 0;
+                                    break;
+                            }
+                            volume *= (1.0f - hollowVolume);
+                        }
+                    }
+                    break;
+
+                default:
+                    break;
+            }
+
+            float taperX1;
+            float taperY1;
+            float taperX;
+            float taperY;
+            float pathBegin;
+            float pathEnd;
+            float profileBegin;
+            float profileEnd;
+
+            if (_pbs.PathCurve == (byte)Extrusion.Straight || _pbs.PathCurve == (byte)Extrusion.Flexible)
+            {
+                taperX1 = _pbs.PathScaleX * 0.01f;
+                if (taperX1 > 1.0f)
+                    taperX1 = 2.0f - taperX1;
+                taperX = 1.0f - taperX1;
+
+                taperY1 = _pbs.PathScaleY * 0.01f;
+                if (taperY1 > 1.0f)
+                    taperY1 = 2.0f - taperY1;
+                taperY = 1.0f - taperY1;
+            }
+            else
+            {
+                taperX = _pbs.PathTaperX * 0.01f;
+                if (taperX < 0.0f)
+                    taperX = -taperX;
+                taperX1 = 1.0f - taperX;
+
+                taperY = _pbs.PathTaperY * 0.01f;
+                if (taperY < 0.0f)
+                    taperY = -taperY;
+                taperY1 = 1.0f - taperY;
+            }
+
+            volume *= (taperX1 * taperY1 + 0.5f * (taperX1 * taperY + taperX * taperY1) + 0.3333333333f * taperX * taperY);
+
+            pathBegin = (float)_pbs.PathBegin * 2.0e-5f;
+            pathEnd = 1.0f - (float)_pbs.PathEnd * 2.0e-5f;
+            volume *= (pathEnd - pathBegin);
+
+            // this is crude aproximation
+            profileBegin = (float)_pbs.ProfileBegin * 2.0e-5f;
+            profileEnd = 1.0f - (float)_pbs.ProfileEnd * 2.0e-5f;
+            volume *= (profileEnd - profileBegin);
+
+            repData.volume = volume;
+        }
+
+        private void CalcVolumeData(ODEPhysRepData repData)
+        {
+            if (repData.hasOBB)
+            {
+                Vector3 OBB = repData.OBB;
+            }
+            else
+            {
+                Vector3 OBB = repData.size;
+                OBB.X *= 0.5f;
+                OBB.Y *= 0.5f;
+                OBB.Z *= 0.5f;
+
+                repData.OBB = OBB;
+                repData.OBBOffset = Vector3.Zero;
+            }
+
+            CalculateBasicPrimVolume(repData);
+        }
+    }
+
+    public class ODEAssetRequest
+    {
+        ODEMeshWorker m_worker;
+        private ILog m_log;
+        ODEPhysRepData repData;
+
+        public ODEAssetRequest(ODEMeshWorker pWorker, RequestAssetDelegate provider,
+            ODEPhysRepData pRepData, ILog plog)
+        {
+            m_worker = pWorker;
+            m_log = plog;
+            repData = pRepData;
+
+            repData.meshState = MeshState.AssetFailed;
+            if (provider == null)
+                return;
+
+            if (repData.assetID == null)
+                return;
+
+            UUID assetID = (UUID) repData.assetID;
+            if (assetID == UUID.Zero)
+                return;
+
+            repData.meshState = MeshState.loadingAsset;
+            provider(assetID, ODEassetReceived);
+        }
+
+        void ODEassetReceived(AssetBase asset)
+        {
+            repData.meshState = MeshState.AssetFailed;
+            if (asset != null)
+            {
+                if (asset.Data != null && asset.Data.Length > 0)
+                {
+                    repData.meshState = MeshState.noNeed;
+
+                    if (!repData.pbs.SculptEntry)
+                        return;
+                    if (repData.pbs.SculptTexture != repData.assetID)
+                        return;
+
+//                    repData.pbs.SculptData = new byte[asset.Data.Length];
+//                    asset.Data.CopyTo(repData.pbs.SculptData,0);
+                    repData.pbs.SculptData = asset.Data;
+                    repData.meshState = MeshState.AssetOK;
+                    m_worker.AssetLoaded(repData);
+                }
+                else
+                    m_log.WarnFormat("[PHYSICS]: asset provider returned invalid mesh data for prim {0} asset UUID {1}.",
+                        repData.actor.Name, asset.ID.ToString());
+            }
+            else
+                m_log.WarnFormat("[PHYSICS]: asset provider returned null asset for mesh of prim {0}.",
+                    repData.actor.Name);
+        }
+    }
+}
diff --git a/OpenSim/Region/PhysicsModules/ubOde/ODEModule.cs b/OpenSim/Region/PhysicsModules/ubOde/ODEModule.cs
new file mode 100644
index 0000000..4cb1736
--- /dev/null
+++ b/OpenSim/Region/PhysicsModules/ubOde/ODEModule.cs
@@ -0,0 +1,112 @@
+using System;
+using System.Collections.Generic;
+using System.Reflection;
+using log4net;
+using Nini.Config;
+using Mono.Addins;
+using OdeAPI;
+using OpenSim.Framework;
+using OpenSim.Region.Framework.Scenes;
+using OpenSim.Region.Framework.Interfaces;
+
+namespace OpenSim.Region.PhysicsModule.ubOde
+{
+    [Extension(Path = "/OpenSim/RegionModules", NodeName = "RegionModule", Id = "ubODEPhysicsScene")]
+    class ubOdeModule : INonSharedRegionModule
+    {
+        private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
+
+        private static Dictionary<Scene, ODEScene> m_scenes = new Dictionary<Scene, ODEScene>();
+        private bool m_Enabled = false;
+        private IConfigSource m_config;
+        private bool OSOdeLib;
+
+
+       #region INonSharedRegionModule
+
+        public string Name
+        {
+            get { return "ubODE"; }
+        }
+
+        public string Version
+        {
+            get { return "1.0"; }
+        }
+
+        public Type ReplaceableInterface
+        {
+            get { return null; }
+        }
+
+        public void Initialise(IConfigSource source)
+        {
+            IConfig config = source.Configs["Startup"];
+            if (config != null)
+            {
+                string physics = config.GetString("physics", string.Empty);
+                if (physics == Name)
+                {
+                    m_config = source;
+                    m_Enabled = true;
+
+                    if (Util.IsWindows())
+                        Util.LoadArchSpecificWindowsDll("ode.dll");
+
+                    d.InitODE();
+
+                    string ode_config = d.GetConfiguration();
+                    if (ode_config != null && ode_config != "")
+                    {
+                        m_log.InfoFormat("[ubODE] ode library configuration: {0}", ode_config);
+
+                        if (ode_config.Contains("ODE_OPENSIM"))
+                        {
+                            OSOdeLib = true;
+                        }
+                    }
+                }
+            }
+        }
+
+        public void Close()
+        {
+        }
+
+        public void AddRegion(Scene scene)
+        {
+            if (!m_Enabled)
+                return;
+
+            if(m_scenes.ContainsKey(scene)) // ???
+                return;
+            ODEScene newodescene = new ODEScene(scene, m_config, Name, Version, OSOdeLib);
+            m_scenes[scene] = newodescene;
+        }
+
+        public void RemoveRegion(Scene scene)
+        {
+            if (!m_Enabled)
+                return;
+
+            // a odescene.dispose is called later directly by scene.cs
+            // since it is seen as a module interface
+
+            if(m_scenes.ContainsKey(scene))
+                m_scenes.Remove(scene);
+        }
+
+        public void RegionLoaded(Scene scene)
+        {
+            if (!m_Enabled)
+                return;
+
+            if(m_scenes.ContainsKey(scene))
+            {
+                m_scenes[scene].RegionLoaded();
+            }
+
+        }
+        #endregion
+    }
+}
diff --git a/OpenSim/Region/PhysicsModules/ubOde/ODEPrim.cs b/OpenSim/Region/PhysicsModules/ubOde/ODEPrim.cs
new file mode 100644
index 0000000..aa208e2
--- /dev/null
+++ b/OpenSim/Region/PhysicsModules/ubOde/ODEPrim.cs
@@ -0,0 +1,4256 @@
+/*
+ * Copyright (c) Contributors, http://opensimulator.org/
+ * See CONTRIBUTORS.TXT for a full list of copyright holders.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the OpenSimulator Project nor the
+ *       names of its contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/*
+ * Revised August 26 2009 by Kitto Flora. ODEDynamics.cs replaces
+ * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised:
+ * ODEPrim.cs contains methods dealing with Prim editing, Prim
+ * characteristics and Kinetic motion.
+ * ODEDynamics.cs contains methods dealing with Prim Physical motion
+ * (dynamics) and the associated settings. Old Linear and angular
+ * motors for dynamic motion have been replace with  MoveLinear()
+ * and MoveAngular(); 'Physical' is used only to switch ODE dynamic
+ * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_<other> is to
+ * switch between 'VEHICLE' parameter use and general dynamics
+ * settings use.
+ */
+
+//#define SPAM
+
+using System;
+using System.Collections.Generic;
+using System.Reflection;
+using System.Runtime.InteropServices;
+using System.Threading;
+using log4net;
+using OpenMetaverse;
+using OdeAPI;
+using OpenSim.Framework;
+using OpenSim.Region.PhysicsModules.SharedBase;
+
+namespace OpenSim.Region.PhysicsModule.ubOde
+{
+    public class OdePrim : PhysicsActor
+    {
+        private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
+
+        private bool m_isphysical;
+        private bool m_fakeisphysical;
+        private bool m_isphantom;
+        private bool m_fakeisphantom;
+        internal bool m_isVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively
+        private bool m_fakeisVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively
+
+        internal bool m_building;
+        protected bool m_forcePosOrRotation;
+        private bool m_iscolliding;
+
+        internal bool m_isSelected;
+        private bool m_delaySelect;
+        private bool m_lastdoneSelected;
+        internal bool m_outbounds;
+
+        private byte m_angularlocks = 0;
+
+        private Quaternion m_lastorientation;
+        private Quaternion _orientation;
+
+        private Vector3 _position;
+        private Vector3 _velocity;
+        private Vector3 m_lastVelocity;
+        private Vector3 m_lastposition;
+        private Vector3 m_rotationalVelocity;
+        private Vector3 _size;
+        private Vector3 m_acceleration;
+        private IntPtr Amotor;
+
+        internal Vector3 m_force;
+        internal Vector3 m_forceacc;
+        internal Vector3 m_torque;
+        internal Vector3 m_angularForceacc;
+
+        private float m_invTimeStep;
+        private float m_timeStep;
+
+        private Vector3 m_PIDTarget;
+        private float m_PIDTau;
+        private bool m_usePID;
+
+        private float m_PIDHoverHeight;
+        private float m_PIDHoverTau;
+        private bool m_useHoverPID;
+        private PIDHoverType m_PIDHoverType;
+        private float m_targetHoverHeight;
+        private float m_groundHeight;
+        private float m_waterHeight;
+        private float m_buoyancy;                //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle.
+
+        private int m_body_autodisable_frames;
+        public int m_bodydisablecontrol = 0;
+        private float m_gravmod = 1.0f;
+
+        // Default we're a Geometry
+        private CollisionCategories m_collisionCategories = (CollisionCategories.Geom);
+        // Default colide nonphysical don't try to colide with anything
+        private const CollisionCategories m_default_collisionFlagsNotPhysical = 0;
+
+        private const CollisionCategories m_default_collisionFlagsPhysical = (CollisionCategories.Geom |
+                                        CollisionCategories.Character |
+                                        CollisionCategories.Land |
+                                        CollisionCategories.VolumeDtc);
+
+//        private bool m_collidesLand = true;
+        private bool m_collidesWater;
+//        public bool m_returnCollisions;
+
+        private bool m_NoColide;  // for now only for internal use for bad meshs
+
+
+        // Default, Collide with Other Geometries, spaces and Bodies
+        private CollisionCategories m_collisionFlags = m_default_collisionFlagsNotPhysical;
+
+        public bool m_disabled;
+
+        private uint m_localID;
+
+        private IMesh m_mesh;
+        private object m_meshlock = new object();
+        private PrimitiveBaseShape _pbs;
+
+        private UUID? m_assetID;
+        private MeshState m_meshState;
+
+        public ODEScene _parent_scene;
+
+        /// <summary>
+        /// The physics space which contains prim geometry
+        /// </summary>
+        public IntPtr m_targetSpace;
+
+        public IntPtr prim_geom;
+        public IntPtr _triMeshData;
+
+        private PhysicsActor _parent;
+
+        private List<OdePrim> childrenPrim = new List<OdePrim>();
+
+        public float m_collisionscore;
+        private int m_colliderfilter = 0;
+
+        public IntPtr collide_geom; // for objects: geom if single prim space it linkset
+
+        private float m_density;
+        private byte m_shapetype;
+        private byte m_fakeShapetype;
+        public bool _zeroFlag;
+        private bool m_lastUpdateSent;
+
+        public IntPtr Body;
+
+        private Vector3 _target_velocity;
+
+        public Vector3 m_OBBOffset;
+        public Vector3 m_OBB;
+        public float primOOBradiusSQ;
+
+        private bool m_hasOBB = true;
+
+        private float m_physCost;
+        private float m_streamCost;
+
+        public d.Mass primdMass; // prim inertia information on it's own referencial
+        private PhysicsInertiaData m_InertiaOverride;
+        float primMass; // prim own mass
+        float primVolume; // prim own volume;
+        float m_mass; // object mass acording to case
+
+        public int givefakepos;
+        private Vector3 fakepos;
+        public int givefakeori;
+        private Quaternion fakeori;
+        private PhysicsInertiaData m_fakeInertiaOverride;
+
+        private int m_eventsubscription;
+        private int m_cureventsubscription;
+        private CollisionEventUpdate CollisionEventsThisFrame = null;
+        private CollisionEventUpdate CollisionVDTCEventsThisFrame = null;
+        private bool SentEmptyCollisionsEvent;
+
+        public volatile bool childPrim;
+
+        public ODEDynamics m_vehicle;
+
+        internal int m_material = (int)Material.Wood;
+        private float mu;
+        private float bounce;
+
+        /// <summary>
+        /// Is this prim subject to physics?  Even if not, it's still solid for collision purposes.
+        /// </summary>
+        public override bool IsPhysical  // this is not reliable for internal use
+        {
+            get { return m_fakeisphysical; }
+            set
+            {
+                m_fakeisphysical = value; // we show imediatly to outside that we changed physical
+                // and also to stop imediatly some updates
+                // but real change will only happen in taintprocessing
+
+                if (!value) // Zero the remembered last velocity
+                    m_lastVelocity = Vector3.Zero;
+                AddChange(changes.Physical, value);
+            }
+        }
+
+        public override bool IsVolumeDtc
+        {
+            get { return m_fakeisVolumeDetect; }
+            set
+            {
+                m_fakeisVolumeDetect = value;
+                AddChange(changes.VolumeDtc, value);
+            }
+        }
+
+        public override bool Phantom  // this is not reliable for internal use
+        {
+            get { return m_fakeisphantom; }
+            set
+            {
+                m_fakeisphantom = value;
+                AddChange(changes.Phantom, value);
+            }
+        }
+
+        public override bool Building  // this is not reliable for internal use
+        {
+            get { return m_building; }
+            set
+            {
+//                if (value)
+//                    m_building = true;
+                AddChange(changes.building, value);
+            }
+        }
+
+        public override void getContactData(ref ContactData cdata)
+        {
+            cdata.mu = mu;
+            cdata.bounce = bounce;
+
+            //            cdata.softcolide = m_softcolide;
+            cdata.softcolide = false;
+
+            if (m_isphysical)
+            {
+                ODEDynamics veh;
+                if (_parent != null)
+                    veh = ((OdePrim)_parent).m_vehicle;
+                else
+                    veh = m_vehicle;
+
+                if (veh != null && veh.Type != Vehicle.TYPE_NONE)
+                    cdata.mu *= veh.FrictionFactor;
+//                    cdata.mu *= 0;
+            }
+        }
+
+        public override float PhysicsCost
+        {
+            get
+            {
+                return m_physCost;
+            }
+        }
+
+        public override float StreamCost
+        {
+            get
+            {
+                return m_streamCost;
+            }
+        }
+
+        public override int PhysicsActorType
+        {
+            get { return (int)ActorTypes.Prim; }
+            set { return; }
+        }
+
+        public override bool SetAlwaysRun
+        {
+            get { return false; }
+            set { return; }
+        }
+
+        public override uint LocalID
+        {
+            get { return m_localID; }
+            set
+            {
+                uint oldid = m_localID;
+                m_localID = value;
+                _parent_scene.changePrimID(this, oldid);
+            }
+        }
+
+        public override PhysicsActor ParentActor
+        {
+            get
+            {
+                if (childPrim)
+                    return _parent;
+                else
+                    return (PhysicsActor)this;
+            }
+        }
+
+        public override bool Grabbed
+        {
+            set { return; }
+        }
+
+        public override bool Selected
+        {
+            set
+            {
+                if (value)
+                    m_isSelected = value; // if true set imediatly to stop moves etc
+                AddChange(changes.Selected, value);
+            }
+        }
+
+        public override bool Flying
+        {
+            // no flying prims for you
+            get { return false; }
+            set { }
+        }
+
+        public override bool IsColliding
+        {
+            get { return m_iscolliding; }
+            set
+            {
+                if (value)
+                {
+                    m_colliderfilter += 2;
+                    if (m_colliderfilter > 2)
+                        m_colliderfilter = 2;
+                }
+                else
+                {
+                    m_colliderfilter--;
+                    if (m_colliderfilter < 0)
+                        m_colliderfilter = 0;
+                }
+
+                if (m_colliderfilter == 0)
+                    m_iscolliding = false;
+                else
+                    m_iscolliding = true;
+            }
+        }
+
+        public override bool CollidingGround
+        {
+            get { return false; }
+            set { return; }
+        }
+
+        public override bool CollidingObj
+        {
+            get { return false; }
+            set { return; }
+        }
+
+
+        public override bool ThrottleUpdates {get;set;}
+
+        public override bool Stopped
+        {
+            get { return _zeroFlag; }
+        }
+
+        public override Vector3 Position
+        {
+            get
+            {
+                if (givefakepos > 0)
+                    return fakepos;
+                else
+                    return _position;
+            }
+
+            set
+            {
+                fakepos = value;
+                givefakepos++;
+                AddChange(changes.Position, value);
+            }
+        }
+
+        public override Vector3 Size
+        {
+            get { return _size; }
+            set
+            {
+                if (value.IsFinite())
+                {
+                     _parent_scene.m_meshWorker.ChangeActorPhysRep(this, _pbs, value, m_fakeShapetype);
+                }
+                else
+                {
+                    m_log.WarnFormat("[PHYSICS]: Got NaN Size on object {0}", Name);
+                }
+            }
+        }
+
+        public override float Mass
+        {
+            get { return primMass; }
+        }
+
+        public override Vector3 Force
+        {
+            get { return m_force; }
+            set
+            {
+                if (value.IsFinite())
+                {
+                    AddChange(changes.Force, value);
+                }
+                else
+                {
+                    m_log.WarnFormat("[PHYSICS]: NaN in Force Applied to an Object {0}", Name);
+                }
+            }
+        }
+
+        public override void SetVolumeDetect(int param)
+        {
+            m_fakeisVolumeDetect = (param != 0);
+            AddChange(changes.VolumeDtc, m_fakeisVolumeDetect);
+        }
+
+        public override Vector3 GeometricCenter
+        {
+            // this is not real geometric center but a average of positions relative to root prim acording to
+            // http://wiki.secondlife.com/wiki/llGetGeometricCenter
+            // ignoring tortured prims details since sl also seems to ignore
+            // so no real use in doing it on physics
+            get
+            {
+                return Vector3.Zero;
+            }
+        }
+
+        public override PhysicsInertiaData GetInertiaData()
+        {
+            PhysicsInertiaData inertia;
+            if(childPrim)
+            {
+                if(_parent != null)
+                    return _parent.GetInertiaData();
+                else
+                {
+                    inertia = new PhysicsInertiaData();
+                    inertia.TotalMass = -1;
+                    return inertia;
+                }
+            }
+
+            inertia = new PhysicsInertiaData();
+
+            // double buffering
+            if(m_fakeInertiaOverride != null)
+            { 
+                d.Mass objdmass = new d.Mass();
+                objdmass.I.M00 = m_fakeInertiaOverride.Inertia.X;
+                objdmass.I.M11 = m_fakeInertiaOverride.Inertia.Y;
+                objdmass.I.M22 = m_fakeInertiaOverride.Inertia.Z;
+
+                objdmass.mass = m_fakeInertiaOverride.TotalMass;
+                    
+                if(Math.Abs(m_fakeInertiaOverride.InertiaRotation.W) < 0.999)
+                {
+                    d.Matrix3 inertiarotmat = new d.Matrix3();
+                    d.Quaternion inertiarot = new d.Quaternion();
+
+                    inertiarot.X = m_fakeInertiaOverride.InertiaRotation.X;
+                    inertiarot.Y = m_fakeInertiaOverride.InertiaRotation.Y;
+                    inertiarot.Z = m_fakeInertiaOverride.InertiaRotation.Z;
+                    inertiarot.W = m_fakeInertiaOverride.InertiaRotation.W;
+                    d.RfromQ(out inertiarotmat, ref inertiarot);
+                    d.MassRotate(ref objdmass, ref inertiarotmat);
+                }
+
+                inertia.TotalMass = m_fakeInertiaOverride.TotalMass;
+                inertia.CenterOfMass = m_fakeInertiaOverride.CenterOfMass;
+                inertia.Inertia.X = objdmass.I.M00;
+                inertia.Inertia.Y = objdmass.I.M11;
+                inertia.Inertia.Z = objdmass.I.M22;
+                inertia.InertiaRotation.X =  objdmass.I.M01;
+                inertia.InertiaRotation.Y =  objdmass.I.M02;
+                inertia.InertiaRotation.Z =  objdmass.I.M12;
+                return inertia;
+            }
+
+            inertia.TotalMass = m_mass;
+
+            if(Body == IntPtr.Zero || prim_geom == IntPtr.Zero)
+            {
+                inertia.CenterOfMass = Vector3.Zero;
+                inertia.Inertia = Vector3.Zero;
+                inertia.InertiaRotation =  Vector4.Zero;
+                return inertia;
+            }
+
+            d.Vector3 dtmp;
+            d.Mass m = new d.Mass();
+            lock(_parent_scene.OdeLock)
+            {
+                d.AllocateODEDataForThread(0);
+                dtmp = d.GeomGetOffsetPosition(prim_geom);
+                d.BodyGetMass(Body, out m);
+            }
+
+            Vector3 cm = new Vector3(-dtmp.X, -dtmp.Y, -dtmp.Z);
+            inertia.CenterOfMass = cm;
+            inertia.Inertia = new Vector3(m.I.M00, m.I.M11, m.I.M22);
+            inertia.InertiaRotation = new Vector4(m.I.M01, m.I.M02 , m.I.M12, 0);
+
+            return inertia;
+        }
+
+        public override void SetInertiaData(PhysicsInertiaData inertia)
+        {
+            if(childPrim)
+            {
+                if(_parent != null)
+                    _parent.SetInertiaData(inertia);
+                return;
+            }
+
+            if(inertia.TotalMass > 0)
+                m_fakeInertiaOverride = new PhysicsInertiaData(inertia);
+            else
+                m_fakeInertiaOverride = null;
+
+            if (inertia.TotalMass > _parent_scene.maximumMassObject)
+                inertia.TotalMass = _parent_scene.maximumMassObject;
+            AddChange(changes.SetInertia,(object)m_fakeInertiaOverride);
+        }
+
+        public override Vector3 CenterOfMass
+        {
+            get
+            {
+                lock (_parent_scene.OdeLock)
+                {
+                    d.AllocateODEDataForThread(0);
+
+                    d.Vector3 dtmp;
+                    if (!childPrim && Body != IntPtr.Zero)
+                    {
+                        dtmp = d.BodyGetPosition(Body);
+                        return new Vector3(dtmp.X, dtmp.Y, dtmp.Z);
+                    }
+                    else if (prim_geom != IntPtr.Zero)
+                    {
+                        d.Quaternion dq;
+                        d.GeomCopyQuaternion(prim_geom, out dq);
+                        Quaternion q;
+                        q.X = dq.X;
+                        q.Y = dq.Y;
+                        q.Z = dq.Z;
+                        q.W = dq.W;
+
+                        Vector3 Ptot = m_OBBOffset * q;
+                        dtmp = d.GeomGetPosition(prim_geom);
+                        Ptot.X += dtmp.X;
+                        Ptot.Y += dtmp.Y;
+                        Ptot.Z += dtmp.Z;
+
+                        //                    if(childPrim)  we only know about physical linksets
+                        return Ptot;
+/*
+                        float tmass = _mass;
+                        Ptot *= tmass;
+
+                        float m;
+
+                        foreach (OdePrim prm in childrenPrim)
+                        {
+                            m = prm._mass;
+                            Ptot += prm.CenterOfMass * m;
+                            tmass += m;
+                        }
+
+                        if (tmass == 0)
+                            tmass = 0;
+                        else
+                            tmass = 1.0f / tmass;
+
+                        Ptot *= tmass;
+                        return Ptot;
+*/
+                    }
+                    else
+                        return _position;
+                }
+            }
+        }
+
+        public override PrimitiveBaseShape Shape
+        {
+            set
+            {
+//                AddChange(changes.Shape, value);
+                _parent_scene.m_meshWorker.ChangeActorPhysRep(this, value, _size, m_fakeShapetype);
+            }
+        }
+
+        public override byte PhysicsShapeType
+        {
+            get
+            {
+                return m_fakeShapetype;
+            }
+            set
+            {
+                m_fakeShapetype = value;
+               _parent_scene.m_meshWorker.ChangeActorPhysRep(this, _pbs, _size, value);
+            }
+        }
+
+        public override Vector3 rootVelocity
+        {
+            get
+            {
+                if(_parent != null)
+                    return ((OdePrim)_parent).Velocity;
+                return Velocity;
+            }
+        }
+
+        public override Vector3 Velocity
+        {
+            get
+            {
+                if (_zeroFlag)
+                    return Vector3.Zero;
+                return _velocity;
+            }
+            set
+            {
+                if (value.IsFinite())
+                {
+                    if(m_outbounds)
+                        _velocity = value;
+                    else
+                        AddChange(changes.Velocity, value);
+                }
+                else
+                {
+                    m_log.WarnFormat("[PHYSICS]: Got NaN Velocity in Object {0}", Name);
+                }
+            }
+        }
+
+        public override Vector3 Torque
+        {
+            get
+            {
+                if (!IsPhysical || Body == IntPtr.Zero)
+                    return Vector3.Zero;
+
+                return m_torque;
+            }
+
+            set
+            {
+                if (value.IsFinite())
+                {
+                    AddChange(changes.Torque, value);
+                }
+                else
+                {
+                    m_log.WarnFormat("[PHYSICS]: Got NaN Torque in Object {0}", Name);
+                }
+            }
+        }
+
+        public override float CollisionScore
+        {
+            get { return m_collisionscore; }
+            set { m_collisionscore = value; }
+        }
+
+        public override bool Kinematic
+        {
+            get { return false; }
+            set { }
+        }
+
+        public override Quaternion Orientation
+        {
+            get
+            {
+                if (givefakeori > 0)
+                    return fakeori;
+                else
+
+                    return _orientation;
+            }
+            set
+            {
+                if (QuaternionIsFinite(value))
+                {
+                    fakeori = value;
+                    givefakeori++;
+
+                    value.Normalize();
+
+                    AddChange(changes.Orientation, value);
+                }
+                else
+                    m_log.WarnFormat("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object {0}", Name);
+
+            }
+        }
+
+        public override Vector3 Acceleration
+        {
+            get { return m_acceleration; }
+            set
+            {
+                if(m_outbounds)
+                    m_acceleration = value;
+            }
+        }
+
+        public override Vector3 RotationalVelocity
+        {
+            get
+            {
+                Vector3 pv = Vector3.Zero;
+                if (_zeroFlag)
+                    return pv;
+
+                if (m_rotationalVelocity.ApproxEquals(pv, 0.0001f))
+                    return pv;
+
+                return m_rotationalVelocity;
+            }
+            set
+            {
+                if (value.IsFinite())
+                {
+                    if(m_outbounds)
+                        m_rotationalVelocity = value;
+                    else
+                        AddChange(changes.AngVelocity, value);
+                }
+                else
+                {
+                    m_log.WarnFormat("[PHYSICS]: Got NaN RotationalVelocity in Object {0}", Name);
+                }
+            }
+        }
+
+        public override float Buoyancy
+        {
+            get { return m_buoyancy; }
+            set
+            {
+                 AddChange(changes.Buoyancy,value);
+            }
+        }
+
+        public override bool FloatOnWater
+        {
+            set
+            {
+                AddChange(changes.CollidesWater, value);
+            }
+        }
+
+        public override Vector3 PIDTarget
+        {
+            set
+            {
+                if (value.IsFinite())
+                {
+                    AddChange(changes.PIDTarget,value);
+                }
+                else
+                    m_log.WarnFormat("[PHYSICS]: Got NaN PIDTarget from Scene on Object {0}", Name);
+            }
+        }
+
+        public override bool PIDActive
+        {
+            get
+            {
+                return m_usePID;
+            }
+            set
+            {
+                AddChange(changes.PIDActive,value);
+            }
+        }
+
+        public override float PIDTau
+        {
+            set
+            {
+                float tmp = 0;
+                if (value > 0)
+                {
+                    float mint = (0.05f > m_timeStep ? 0.05f : m_timeStep);
+                    if (value < mint)
+                        tmp = mint;
+                    else
+                        tmp = value;
+                }
+                AddChange(changes.PIDTau,tmp);
+            }
+        }
+
+        public override float PIDHoverHeight
+        {
+            set
+            {
+                AddChange(changes.PIDHoverHeight,value);
+            }
+        }
+        public override bool PIDHoverActive
+        {
+            get
+            {
+                return m_useHoverPID;
+            }
+            set
+            {
+                AddChange(changes.PIDHoverActive, value);
+            }
+        }
+
+        public override PIDHoverType PIDHoverType
+        {
+            set
+            {
+                AddChange(changes.PIDHoverType,value);
+            }
+        }
+
+        public override float PIDHoverTau
+        {
+            set
+            {
+                float tmp =0;
+                if (value > 0)
+                {
+                    float mint = (0.05f > m_timeStep ? 0.05f : m_timeStep);
+                    if (value < mint)
+                        tmp = mint;
+                    else
+                        tmp = value;
+                }
+                AddChange(changes.PIDHoverTau, tmp);
+            }
+        }
+
+        public override Quaternion APIDTarget { set { return; } }
+
+        public override bool APIDActive { set { return; } }
+
+        public override float APIDStrength { set { return; } }
+
+        public override float APIDDamping { set { return; } }
+
+        public override int VehicleType
+        {
+            // we may need to put a fake on this
+            get
+            {
+                if (m_vehicle == null)
+                    return (int)Vehicle.TYPE_NONE;
+                else
+                    return (int)m_vehicle.Type;
+            }
+            set
+            {
+                AddChange(changes.VehicleType, value);
+            }
+        }
+
+        public override void VehicleFloatParam(int param, float value)
+        {
+            strVehicleFloatParam fp = new strVehicleFloatParam();
+            fp.param = param;
+            fp.value = value;
+            AddChange(changes.VehicleFloatParam, fp);
+        }
+
+        public override void VehicleVectorParam(int param, Vector3 value)
+        {
+            strVehicleVectorParam fp = new strVehicleVectorParam();
+            fp.param = param;
+            fp.value = value;
+            AddChange(changes.VehicleVectorParam, fp);
+        }
+
+        public override void VehicleRotationParam(int param, Quaternion value)
+        {
+            strVehicleQuatParam fp = new strVehicleQuatParam();
+            fp.param = param;
+            fp.value = value;
+            AddChange(changes.VehicleRotationParam, fp);
+        }
+
+        public override void VehicleFlags(int param, bool value)
+        {
+            strVehicleBoolParam bp = new strVehicleBoolParam();
+            bp.param = param;
+            bp.value = value;
+            AddChange(changes.VehicleFlags, bp);
+        }
+
+        public override void SetVehicle(object vdata)
+        {
+            AddChange(changes.SetVehicle, vdata);
+        }
+        public void SetAcceleration(Vector3 accel)
+        {
+            m_acceleration = accel;
+        }
+
+        public override void AddForce(Vector3 force, bool pushforce)
+        {
+            if (force.IsFinite())
+            {
+                if(pushforce)
+                    AddChange(changes.AddForce, force);
+                else // a impulse
+                    AddChange(changes.AddForce, force * m_invTimeStep);
+            }
+            else
+            {
+                m_log.WarnFormat("[PHYSICS]: Got Invalid linear force vector from Scene in Object {0}", Name);
+            }
+            //m_log.Info("[PHYSICS]: Added Force:" + force.ToString() +  " to prim at " + Position.ToString());
+        }
+
+        public override void AddAngularForce(Vector3 force, bool pushforce)
+        {
+            if (force.IsFinite())
+            {
+//                if(pushforce)  for now applyrotationimpulse seems more happy applied as a force
+                    AddChange(changes.AddAngForce, force);
+//                else // a impulse
+//                    AddChange(changes.AddAngForce, force * m_invTimeStep);
+            }
+            else
+            {
+                m_log.WarnFormat("[PHYSICS]: Got Invalid Angular force vector from Scene in Object {0}", Name);
+            }
+        }
+
+        public override void CrossingFailure()
+        {
+            lock(_parent_scene.OdeLock)
+            {
+                if (m_outbounds)
+                {
+                    _position.X = Util.Clip(_position.X, 0.5f, _parent_scene.WorldExtents.X - 0.5f);
+                    _position.Y = Util.Clip(_position.Y, 0.5f, _parent_scene.WorldExtents.Y - 0.5f);
+                    _position.Z = Util.Clip(_position.Z + 0.2f, -100f, 50000f);
+
+                    m_lastposition = _position;
+                    _velocity.X = 0;
+                    _velocity.Y = 0;
+                    _velocity.Z = 0;
+
+                    d.AllocateODEDataForThread(0);
+
+                    m_lastVelocity = _velocity;
+                    if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE)
+                        m_vehicle.Stop();
+
+                    if(Body != IntPtr.Zero)
+                        d.BodySetLinearVel(Body, 0, 0, 0); // stop it
+                    if (prim_geom != IntPtr.Zero)
+                        d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
+
+                    m_outbounds = false;
+                    changeDisable(false);
+                    base.RequestPhysicsterseUpdate();
+                }
+            }
+        }
+
+        public override void CrossingStart()
+        {
+            lock(_parent_scene.OdeLock)
+            {
+                if (m_outbounds || childPrim)
+                    return;
+
+                m_outbounds = true;
+
+                m_lastposition = _position;
+                m_lastorientation = _orientation;
+
+                d.AllocateODEDataForThread(0);
+                if(Body != IntPtr.Zero)
+                {
+                    d.Vector3 dtmp = d.BodyGetAngularVel(Body);
+                    m_rotationalVelocity.X = dtmp.X;
+                    m_rotationalVelocity.Y = dtmp.Y;
+                    m_rotationalVelocity.Z = dtmp.Z;
+
+                    dtmp = d.BodyGetLinearVel(Body);
+                    _velocity.X = dtmp.X;
+                    _velocity.Y = dtmp.Y;
+                    _velocity.Z = dtmp.Z;
+
+                    d.BodySetLinearVel(Body, 0, 0, 0); // stop it
+                    d.BodySetAngularVel(Body, 0, 0, 0);
+                }
+                if(prim_geom != IntPtr.Zero)
+                    d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
+                disableBodySoft(); // stop collisions
+                UnSubscribeEvents();
+            }
+        }
+
+        public override void SetMomentum(Vector3 momentum)
+        {
+        }
+
+        public override void SetMaterial(int pMaterial)
+        {
+            m_material = pMaterial;
+            mu = _parent_scene.m_materialContactsData[pMaterial].mu;
+            bounce = _parent_scene.m_materialContactsData[pMaterial].bounce;
+        }
+
+        public override float Density
+        {
+            get
+            {
+                return m_density * 100f;
+            }
+            set
+            {
+                float old = m_density;
+                m_density = value / 100f;
+ //               if(m_density != old)
+ //                   UpdatePrimBodyData();
+            }
+        }
+        public override float GravModifier
+        {
+            get
+            {
+                return m_gravmod;
+            }
+            set
+            {
+                m_gravmod = value;
+                if (m_vehicle != null)
+                    m_vehicle.GravMod = m_gravmod;
+            }
+        }
+        public override float Friction
+        {
+            get
+            {
+                return mu;
+            }
+            set
+            {
+                mu = value;
+            }
+        }
+
+        public override float Restitution
+        {
+            get
+            {
+                return bounce;
+            }
+            set
+            {
+                bounce = value;
+            }
+        }
+
+        public void setPrimForRemoval()
+        {
+            AddChange(changes.Remove, null);
+        }
+
+        public override void link(PhysicsActor obj)
+        {
+            AddChange(changes.Link, obj);
+        }
+
+        public override void delink()
+        {
+            AddChange(changes.DeLink, null);
+        }
+
+        public override void LockAngularMotion(byte axislock)
+        {
+//                m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z);
+            AddChange(changes.AngLock, axislock);
+
+        }
+
+        public override void SubscribeEvents(int ms)
+        {
+            m_eventsubscription = ms;
+            m_cureventsubscription = 0;
+            if (CollisionEventsThisFrame == null)
+                CollisionEventsThisFrame = new CollisionEventUpdate();
+            if (CollisionVDTCEventsThisFrame == null)
+                CollisionVDTCEventsThisFrame = new CollisionEventUpdate();
+            SentEmptyCollisionsEvent = false;
+        }
+
+        public override void UnSubscribeEvents()
+        {
+            if (CollisionVDTCEventsThisFrame != null)
+            {
+                CollisionVDTCEventsThisFrame.Clear();
+                CollisionVDTCEventsThisFrame = null;
+            }
+            if (CollisionEventsThisFrame != null)
+            {
+                CollisionEventsThisFrame.Clear();
+                CollisionEventsThisFrame = null;
+            }
+            m_eventsubscription = 0;
+           _parent_scene.RemoveCollisionEventReporting(this);
+        }
+
+        public override void AddCollisionEvent(uint CollidedWith, ContactPoint contact)
+        {
+            if (CollisionEventsThisFrame == null)
+                CollisionEventsThisFrame = new CollisionEventUpdate();
+
+            CollisionEventsThisFrame.AddCollider(CollidedWith, contact);
+            _parent_scene.AddCollisionEventReporting(this);
+        }
+
+        public override void AddVDTCCollisionEvent(uint CollidedWith, ContactPoint contact)
+        {
+            if (CollisionVDTCEventsThisFrame == null)
+                CollisionVDTCEventsThisFrame = new CollisionEventUpdate();
+
+            CollisionVDTCEventsThisFrame.AddCollider(CollidedWith, contact);
+            _parent_scene.AddCollisionEventReporting(this);
+        }
+
+        internal void SleeperAddCollisionEvents()
+        {
+            if(CollisionEventsThisFrame != null && CollisionEventsThisFrame.m_objCollisionList.Count != 0)
+            {
+                foreach(KeyValuePair<uint,ContactPoint> kvp in CollisionEventsThisFrame.m_objCollisionList)
+                {
+                    if(kvp.Key == 0)
+                        continue;
+                    OdePrim other = _parent_scene.getPrim(kvp.Key);
+                    if(other == null)
+                        continue;
+                    ContactPoint cp = kvp.Value;
+                    cp.SurfaceNormal = - cp.SurfaceNormal;
+                    cp.RelativeSpeed = -cp.RelativeSpeed;
+                    other.AddCollisionEvent(ParentActor.LocalID,cp);
+                }
+            }
+            if(CollisionVDTCEventsThisFrame != null && CollisionVDTCEventsThisFrame.m_objCollisionList.Count != 0)
+            {
+                foreach(KeyValuePair<uint,ContactPoint> kvp in CollisionVDTCEventsThisFrame.m_objCollisionList)
+                {
+                    OdePrim other = _parent_scene.getPrim(kvp.Key);
+                    if(other == null)
+                        continue;
+                    ContactPoint cp = kvp.Value;
+                    cp.SurfaceNormal = - cp.SurfaceNormal;
+                    cp.RelativeSpeed = -cp.RelativeSpeed;
+                    other.AddCollisionEvent(ParentActor.LocalID,cp);
+                }
+            }
+        }
+
+        internal void clearSleeperCollisions()
+        {
+            if(CollisionVDTCEventsThisFrame != null && CollisionVDTCEventsThisFrame.Count >0 )
+                CollisionVDTCEventsThisFrame.Clear();
+        }
+
+        public void SendCollisions(int timestep)
+        {
+            if (m_cureventsubscription < 50000)
+                m_cureventsubscription += timestep;
+
+
+            if (m_cureventsubscription < m_eventsubscription)
+                return;
+
+            if (CollisionEventsThisFrame == null)
+                return;
+
+            int ncolisions = CollisionEventsThisFrame.m_objCollisionList.Count;
+
+            if (!SentEmptyCollisionsEvent || ncolisions > 0)
+            {
+                base.SendCollisionUpdate(CollisionEventsThisFrame);
+                m_cureventsubscription = 0;
+
+                if (ncolisions == 0)
+                {
+                    SentEmptyCollisionsEvent = true;
+//                    _parent_scene.RemoveCollisionEventReporting(this);
+                }
+                else if(Body == IntPtr.Zero || (d.BodyIsEnabled(Body) && m_bodydisablecontrol >= 0 ))
+                {
+                    SentEmptyCollisionsEvent = false;
+                    CollisionEventsThisFrame.Clear();
+                }
+            }
+        }
+
+        public override bool SubscribedEvents()
+        {
+            if (m_eventsubscription > 0)
+                return true;
+            return false;
+        }
+
+        public OdePrim(String primName, ODEScene parent_scene, Vector3 pos, Vector3 size,
+                       Quaternion rotation, PrimitiveBaseShape pbs, bool pisPhysical,bool pisPhantom,byte _shapeType,uint plocalID)
+        {
+            _parent_scene = parent_scene;
+
+            Name = primName;
+            m_localID = plocalID;
+
+            m_vehicle = null;
+
+            if (!pos.IsFinite())
+            {
+                pos = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f),
+                    parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f);
+                m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Position for {0}", Name);
+            }
+            _position = pos;
+            givefakepos = 0;
+
+            m_timeStep = parent_scene.ODE_STEPSIZE;
+            m_invTimeStep = 1f / m_timeStep;
+
+            m_density = parent_scene.geomDefaultDensity;
+            m_body_autodisable_frames = parent_scene.bodyFramesAutoDisable;
+
+            prim_geom = IntPtr.Zero;
+            collide_geom = IntPtr.Zero;
+            Body = IntPtr.Zero;
+
+            if (!size.IsFinite())
+            {
+                size = new Vector3(0.5f, 0.5f, 0.5f);
+                m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Size for {0}", Name);
+            }
+
+            if (size.X <= 0) size.X = 0.01f;
+            if (size.Y <= 0) size.Y = 0.01f;
+            if (size.Z <= 0) size.Z = 0.01f;
+
+            _size = size;
+
+            if (!QuaternionIsFinite(rotation))
+            {
+                rotation = Quaternion.Identity;
+                m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Rotation for {0}", Name);
+            }
+
+            _orientation = rotation;
+            givefakeori = 0;
+
+            _pbs = pbs;
+
+            m_targetSpace = IntPtr.Zero;
+
+            if (pos.Z < 0)
+            {
+                m_isphysical = false;
+            }
+            else
+            {
+                m_isphysical = pisPhysical;
+            }
+            m_fakeisphysical = m_isphysical;
+
+            m_isVolumeDetect = false;
+            m_fakeisVolumeDetect = false;
+
+            m_force = Vector3.Zero;
+
+            m_iscolliding = false;
+            m_colliderfilter = 0;
+            m_NoColide = false;
+
+            _triMeshData = IntPtr.Zero;
+
+            m_fakeShapetype = _shapeType;
+
+            m_lastdoneSelected = false;
+            m_isSelected = false;
+            m_delaySelect = false;
+
+            m_isphantom = pisPhantom;
+            m_fakeisphantom = pisPhantom;
+
+            mu = parent_scene.m_materialContactsData[(int)Material.Wood].mu;
+            bounce = parent_scene.m_materialContactsData[(int)Material.Wood].bounce;
+
+            m_building = true; // control must set this to false when done
+
+            AddChange(changes.Add, null);
+
+            // get basic mass parameters
+            ODEPhysRepData repData = _parent_scene.m_meshWorker.NewActorPhysRep(this, _pbs, _size, _shapeType);
+
+            primVolume = repData.volume;
+            m_OBB = repData.OBB;
+            m_OBBOffset = repData.OBBOffset;
+
+            UpdatePrimBodyData();
+        }
+
+        private void resetCollisionAccounting()
+        {
+            m_collisionscore = 0;
+        }
+
+        private void UpdateCollisionCatFlags()
+        {
+            if(m_isphysical && m_disabled)
+            {
+                m_collisionCategories = 0;
+                m_collisionFlags = 0;
+            }
+
+            else if (m_isSelected)
+            {
+                m_collisionCategories = CollisionCategories.Selected;
+                m_collisionFlags = 0;
+            }
+
+            else if (m_isVolumeDetect)
+            {
+                m_collisionCategories = CollisionCategories.VolumeDtc;
+                if (m_isphysical)
+                    m_collisionFlags = CollisionCategories.Geom | CollisionCategories.Character;
+                else
+                    m_collisionFlags = 0;
+            }
+            else if (m_isphantom)
+            {
+                m_collisionCategories = CollisionCategories.Phantom;
+                if (m_isphysical)
+                    m_collisionFlags = CollisionCategories.Land;
+                else
+                    m_collisionFlags = 0;
+            }
+            else
+            {
+                m_collisionCategories = CollisionCategories.Geom;
+                if (m_isphysical)
+                    m_collisionFlags = m_default_collisionFlagsPhysical;
+                else
+                    m_collisionFlags = m_default_collisionFlagsNotPhysical;
+            }
+        }
+
+        private void ApplyCollisionCatFlags()
+        {
+            if (prim_geom != IntPtr.Zero)
+            {
+                if (!childPrim && childrenPrim.Count > 0)
+                {
+                    foreach (OdePrim prm in childrenPrim)
+                    {
+                        if (m_isphysical && m_disabled)
+                        {
+                            prm.m_collisionCategories = 0;
+                            prm.m_collisionFlags = 0;
+                        }
+                        else
+                        {
+                            // preserve some
+                            if (prm.m_isSelected)
+                            {
+                                prm.m_collisionCategories = CollisionCategories.Selected;
+                                prm.m_collisionFlags = 0;
+                            }
+                            else if (prm.m_isVolumeDetect)
+                            {
+                                prm.m_collisionCategories = CollisionCategories.VolumeDtc;
+                                if (m_isphysical)
+                                    prm.m_collisionFlags = CollisionCategories.Geom | CollisionCategories.Character;
+                                else
+                                    prm.m_collisionFlags = 0;
+                            }
+                            else if (prm.m_isphantom)
+                            {
+                                prm.m_collisionCategories = CollisionCategories.Phantom;
+                                if (m_isphysical)
+                                    prm.m_collisionFlags = CollisionCategories.Land;
+                                else
+                                    prm.m_collisionFlags = 0;
+                            }
+                            else
+                            {
+                                prm.m_collisionCategories = m_collisionCategories;
+                                prm.m_collisionFlags = m_collisionFlags;
+                            }
+                        }
+
+                        if (prm.prim_geom != IntPtr.Zero)
+                        {
+                            if (prm.m_NoColide)
+                            {
+                                d.GeomSetCategoryBits(prm.prim_geom, 0);
+                                if (m_isphysical)
+                                    d.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land);
+                                else
+                                    d.GeomSetCollideBits(prm.prim_geom, 0);
+                            }
+                            else
+                            {
+                                d.GeomSetCategoryBits(prm.prim_geom, (uint)prm.m_collisionCategories);
+                                d.GeomSetCollideBits(prm.prim_geom, (uint)prm.m_collisionFlags);
+                            }
+                        }
+                    }
+                }
+
+                if (m_NoColide)
+                {
+                    d.GeomSetCategoryBits(prim_geom, 0);
+                    d.GeomSetCollideBits(prim_geom, (uint)CollisionCategories.Land);
+                    if (collide_geom != prim_geom && collide_geom != IntPtr.Zero)
+                    {
+                        d.GeomSetCategoryBits(collide_geom, 0);
+                        d.GeomSetCollideBits(collide_geom, (uint)CollisionCategories.Land);
+                    }
+                }
+                else
+                {
+                    d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories);
+                    d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags);
+                    if (collide_geom != prim_geom && collide_geom != IntPtr.Zero)
+                    {
+                        d.GeomSetCategoryBits(collide_geom, (uint)m_collisionCategories);
+                        d.GeomSetCollideBits(collide_geom, (uint)m_collisionFlags);
+                    }
+                }
+            }
+        }
+
+        private void createAMotor(byte axislock)
+        {
+            if (Body == IntPtr.Zero)
+                return;
+
+            if (Amotor != IntPtr.Zero)
+            {
+                d.JointDestroy(Amotor);
+                Amotor = IntPtr.Zero;
+            }
+
+            int axisnum = 0;
+            bool axisX = false;
+            bool axisY = false;
+            bool axisZ = false;
+            if((axislock & 0x02) != 0)
+                {
+                axisnum++;
+                axisX = true;
+                }
+            if((axislock & 0x04) != 0)
+                {
+                axisnum++;
+                axisY = true;
+                }
+            if((axislock & 0x08) != 0)
+                {
+                axisnum++;
+                axisZ = true;
+                }
+
+            if(axisnum == 0)
+                return;
+            // stop it
+            d.BodySetTorque(Body, 0, 0, 0);
+            d.BodySetAngularVel(Body, 0, 0, 0);
+
+            Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero);
+            d.JointAttach(Amotor, Body, IntPtr.Zero);
+
+            d.JointSetAMotorMode(Amotor, 0);
+
+            d.JointSetAMotorNumAxes(Amotor, axisnum);
+
+            // get current orientation to lock
+
+            d.Quaternion dcur = d.BodyGetQuaternion(Body);
+            Quaternion curr; // crap convertion between identical things
+            curr.X = dcur.X;
+            curr.Y = dcur.Y;
+            curr.Z = dcur.Z;
+            curr.W = dcur.W;
+            Vector3 ax;
+
+            int i = 0;
+            int j = 0;
+            if (axisX)
+            {
+                ax = (new Vector3(1, 0, 0)) * curr; // rotate world X to current local X
+                d.JointSetAMotorAxis(Amotor, 0, 0, ax.X, ax.Y, ax.Z);
+                d.JointSetAMotorAngle(Amotor, 0, 0);
+                d.JointSetAMotorParam(Amotor, (int)d.JointParam.LoStop, 0f);
+                d.JointSetAMotorParam(Amotor, (int)d.JointParam.HiStop, 0f);
+                d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel, 0);
+                d.JointSetAMotorParam(Amotor, (int)d.JointParam.FudgeFactor, 0.0001f);
+                d.JointSetAMotorParam(Amotor, (int)d.JointParam.Bounce, 0f);
+                d.JointSetAMotorParam(Amotor, (int)d.JointParam.CFM, 0f);
+                d.JointSetAMotorParam(Amotor, (int)d.JointParam.FMax, 5e8f);
+                d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopCFM, 0f);
+                d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopERP, 0.8f);
+                i++;
+                j = 256; // move to next axis set
+            }
+
+            if (axisY)
+            {
+                ax = (new Vector3(0, 1, 0)) * curr;
+                d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z);
+                d.JointSetAMotorAngle(Amotor, i, 0);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, 0f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Vel, 0);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.CFM, 0f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopCFM, 0f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f);
+                i++;
+                j += 256;
+            }
+
+            if (axisZ)
+            {
+                ax = (new Vector3(0, 0, 1)) * curr;
+                d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z);
+                d.JointSetAMotorAngle(Amotor, i, 0);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, 0f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Vel, 0);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.CFM, 0f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopCFM, 0f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f);
+            }
+        }
+
+
+        private void SetGeom(IntPtr geom)
+        {
+            prim_geom = geom;
+            //Console.WriteLine("SetGeom to " + prim_geom + " for " + Name);
+            if (prim_geom != IntPtr.Zero)
+            {
+
+                if (m_NoColide)
+                {
+                    d.GeomSetCategoryBits(prim_geom, 0);
+                    if (m_isphysical)
+                    {
+                        d.GeomSetCollideBits(prim_geom, (uint)CollisionCategories.Land);
+                    }
+                    else
+                    {
+                        d.GeomSetCollideBits(prim_geom, 0);
+                        d.GeomDisable(prim_geom);
+                    }
+                }
+                else
+                {
+                    d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories);
+                    d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags);
+                }
+
+                UpdatePrimBodyData();
+                _parent_scene.actor_name_map[prim_geom] = this;
+
+/*
+// debug
+                d.AABB aabb;
+                d.GeomGetAABB(prim_geom, out aabb);
+                float x = aabb.MaxX - aabb.MinX;
+                float y = aabb.MaxY - aabb.MinY;
+                float z = aabb.MaxZ - aabb.MinZ;
+                if( x > 60.0f || y > 60.0f || z > 60.0f)
+                    m_log.WarnFormat("[PHYSICS]: large prim geo {0},size {1}, AABBsize <{2},{3},{4}, mesh {5} at {6}",
+                        Name, _size.ToString(), x, y, z, _pbs.SculptEntry ? _pbs.SculptTexture.ToString() : "primMesh", _position.ToString());
+                else if (x < 0.001f || y < 0.001f || z < 0.001f)
+                    m_log.WarnFormat("[PHYSICS]: small prim geo {0},size {1}, AABBsize <{2},{3},{4}, mesh {5} at {6}",
+                        Name, _size.ToString(), x, y, z, _pbs.SculptEntry ? _pbs.SculptTexture.ToString() : "primMesh", _position.ToString());
+*/
+
+            }
+            else
+                m_log.Warn("Setting bad Geom");
+        }
+
+        private bool GetMeshGeom()
+        {
+            IntPtr vertices, indices;
+            int vertexCount, indexCount;
+            int vertexStride, triStride;
+
+            IMesh mesh = m_mesh;
+
+            if (mesh == null)
+                return false;
+
+            mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount);
+            mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount);
+
+            if (vertexCount == 0 || indexCount == 0)
+            {
+                m_log.WarnFormat("[PHYSICS]: Invalid mesh data on OdePrim {0}, mesh {1} at {2}",
+                    Name, _pbs.SculptEntry ? _pbs.SculptTexture.ToString() : "primMesh",_position.ToString());
+
+                m_hasOBB = false;
+                m_OBBOffset = Vector3.Zero;
+                m_OBB = _size * 0.5f;
+
+                m_physCost = 0.1f;
+                m_streamCost = 1.0f;
+
+                _parent_scene.mesher.ReleaseMesh(mesh);
+                m_meshState = MeshState.MeshFailed;
+                m_mesh = null;
+                return false;
+            }
+
+            if (vertexCount > 64000 || indexCount > 64000)
+            {
+                m_log.WarnFormat("[PHYSICS]: large mesh data on OdePrim {0}, mesh {1} at {2}, {3} vertices, {4} indexes",
+                    Name, _pbs.SculptEntry ? _pbs.SculptTexture.ToString() : "primMesh",
+                    _position.ToString() ,vertexCount , indexCount );
+            }
+            IntPtr geo = IntPtr.Zero;
+
+            try
+            {
+                _triMeshData = d.GeomTriMeshDataCreate();
+
+                d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride);
+                d.GeomTriMeshDataPreprocess(_triMeshData);
+
+                geo = d.CreateTriMesh(m_targetSpace, _triMeshData, null, null, null);
+            }
+
+            catch (Exception e)
+            {
+                m_log.ErrorFormat("[PHYSICS]: SetGeom Mesh failed for {0} exception: {1}", Name, e);
+                if (_triMeshData != IntPtr.Zero)
+                {
+                    try
+                    {
+                        d.GeomTriMeshDataDestroy(_triMeshData);
+                    }
+                    catch
+                    {
+                    }
+                }
+                _triMeshData = IntPtr.Zero;
+
+                m_hasOBB = false;
+                m_OBBOffset = Vector3.Zero;
+                m_OBB = _size * 0.5f;
+                m_physCost = 0.1f;
+                m_streamCost = 1.0f;
+
+                _parent_scene.mesher.ReleaseMesh(mesh);
+                m_meshState = MeshState.MeshFailed;
+                m_mesh = null;
+                return false;
+            }
+
+            m_physCost = 0.0013f * (float)indexCount;
+            // todo
+            m_streamCost = 1.0f;
+
+            SetGeom(geo);
+
+            return true;
+        }
+
+        private void CreateGeom(bool OverrideToBox)
+        {
+            bool hasMesh = false;
+
+            m_NoColide = false;
+
+            if ((m_meshState & MeshState.MeshNoColide) != 0)
+                m_NoColide = true;
+
+            else if(!OverrideToBox && m_mesh != null)
+            {
+                if (GetMeshGeom())
+                    hasMesh = true;
+                else
+                    m_NoColide = true;
+            }
+
+
+            if (!hasMesh)
+            {
+                IntPtr geo = IntPtr.Zero;
+
+                if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1
+                    && _size.X == _size.Y && _size.Y == _size.Z)
+                { // it's a sphere
+                    try
+                    {
+                        geo = d.CreateSphere(m_targetSpace, _size.X * 0.5f);
+                    }
+                    catch (Exception e)
+                    {
+                        m_log.WarnFormat("[PHYSICS]: Create sphere failed: {0}", e);
+                        return;
+                    }
+                }
+                else
+                {// do it as a box
+                    try
+                    {
+                        geo = d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z);
+                    }
+                    catch (Exception e)
+                    {
+                        m_log.Warn("[PHYSICS]: Create box failed: {0}", e);
+                        return;
+                    }
+                }
+                m_physCost = 0.1f;
+                m_streamCost = 1.0f;
+                SetGeom(geo);
+            }
+        }
+
+        private void RemoveGeom()
+        {
+            if (prim_geom != IntPtr.Zero)
+            {
+                _parent_scene.actor_name_map.Remove(prim_geom);
+
+                try
+                {
+                    d.GeomDestroy(prim_geom);
+                    if (_triMeshData != IntPtr.Zero)
+                    {
+                        d.GeomTriMeshDataDestroy(_triMeshData);
+                        _triMeshData = IntPtr.Zero;
+                    }
+                }
+                catch (Exception e)
+                {
+                    m_log.ErrorFormat("[PHYSICS]: PrimGeom destruction failed for {0} exception {1}", Name, e);
+                }
+
+                prim_geom = IntPtr.Zero;
+                collide_geom = IntPtr.Zero;
+                m_targetSpace = IntPtr.Zero;
+            }
+            else
+            {
+                m_log.ErrorFormat("[PHYSICS]: PrimGeom destruction BAD {0}", Name);
+            }
+
+            lock (m_meshlock)
+            {
+                if (m_mesh != null)
+                {
+                    _parent_scene.mesher.ReleaseMesh(m_mesh);
+                    m_mesh = null;
+                }
+            }
+
+            Body = IntPtr.Zero;
+            m_hasOBB = false;
+        }
+
+        //sets non physical prim m_targetSpace to right space in spaces grid for static prims
+        // should only be called for non physical prims unless they are becoming non physical
+        private void SetInStaticSpace(OdePrim prim)
+        {
+            IntPtr targetSpace = _parent_scene.MoveGeomToStaticSpace(prim.prim_geom, prim._position, prim.m_targetSpace);
+            prim.m_targetSpace = targetSpace;
+            collide_geom = IntPtr.Zero;
+        }
+
+        public void enableBodySoft()
+        {
+            m_disabled = false;
+            if (!childPrim && !m_isSelected)
+            {
+                if (m_isphysical && Body != IntPtr.Zero)
+                {
+                    UpdateCollisionCatFlags();
+                    ApplyCollisionCatFlags();
+
+                    _zeroFlag = true;
+                    d.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
+                    d.BodyEnable(Body);
+                }
+            }
+            resetCollisionAccounting();
+        }
+
+        private void disableBodySoft()
+        {
+            m_disabled = true;
+            if (!childPrim)
+            {
+                if (m_isphysical && Body != IntPtr.Zero)
+                {
+                    if (m_isSelected)
+                        m_collisionFlags = CollisionCategories.Selected;
+                    else
+                        m_collisionCategories = 0;
+                    m_collisionFlags = 0;
+                    ApplyCollisionCatFlags();
+                    d.BodyDisable(Body);
+                }
+            }
+        }
+
+        private void MakeBody()
+        {
+            if (!m_isphysical) // only physical get bodies
+                return;
+
+            if (childPrim)  // child prims don't get bodies;
+                return;
+
+            if (m_building)
+                return;
+
+            if (prim_geom == IntPtr.Zero)
+            {
+                m_log.Warn("[PHYSICS]: Unable to link the linkset.  Root has no geom yet");
+                return;
+            }
+
+            if (Body != IntPtr.Zero)
+            {
+                DestroyBody();
+                m_log.Warn("[PHYSICS]: MakeBody called having a body");
+            }
+
+            if (d.GeomGetBody(prim_geom) != IntPtr.Zero)
+            {
+                d.GeomSetBody(prim_geom, IntPtr.Zero);
+                m_log.Warn("[PHYSICS]: MakeBody root geom already had a body");
+            }
+
+            bool noInertiaOverride = (m_InertiaOverride == null);
+
+            Body = d.BodyCreate(_parent_scene.world);
+
+            d.Matrix3 mymat = new d.Matrix3();
+            d.Quaternion myrot = new d.Quaternion();
+            d.Mass objdmass = new d.Mass { };
+
+            myrot.X = _orientation.X;
+            myrot.Y = _orientation.Y;
+            myrot.Z = _orientation.Z;
+            myrot.W = _orientation.W;
+            d.RfromQ(out mymat, ref myrot);
+
+            // set the body rotation
+            d.BodySetRotation(Body, ref mymat);
+
+            if(noInertiaOverride)
+            {
+                objdmass = primdMass;
+                d.MassRotate(ref objdmass, ref mymat);
+            }
+    
+            // recompute full object inertia if needed
+            if (childrenPrim.Count > 0)
+            {
+                d.Matrix3 mat = new d.Matrix3();
+                d.Quaternion quat = new d.Quaternion();
+                d.Mass tmpdmass = new d.Mass { };
+                Vector3 rcm;
+
+                rcm.X = _position.X;
+                rcm.Y = _position.Y;
+                rcm.Z = _position.Z;
+
+                lock (childrenPrim)
+                {
+                    foreach (OdePrim prm in childrenPrim)
+                    {
+                        if (prm.prim_geom == IntPtr.Zero)
+                        {
+                            m_log.Warn("[PHYSICS]: Unable to link one of the linkset elements, skipping it.  No geom yet");
+                            continue;
+                        }
+
+                        quat.X = prm._orientation.X;
+                        quat.Y = prm._orientation.Y;
+                        quat.Z = prm._orientation.Z;
+                        quat.W = prm._orientation.W;
+                        d.RfromQ(out mat, ref quat);
+
+                        // fix prim colision cats
+
+                        if (d.GeomGetBody(prm.prim_geom) != IntPtr.Zero)
+                        {
+                            d.GeomSetBody(prm.prim_geom, IntPtr.Zero);
+                            m_log.Warn("[PHYSICS]: MakeBody child geom already had a body");
+                        }
+
+                        d.GeomClearOffset(prm.prim_geom);
+                        d.GeomSetBody(prm.prim_geom, Body);
+                        prm.Body = Body;
+                        d.GeomSetOffsetWorldRotation(prm.prim_geom, ref mat); // set relative rotation
+
+                        if(noInertiaOverride)
+                        {
+                            tmpdmass = prm.primdMass;
+
+                            d.MassRotate(ref tmpdmass, ref mat);
+                            Vector3 ppos = prm._position;
+                            ppos.X -= rcm.X;
+                            ppos.Y -= rcm.Y;
+                            ppos.Z -= rcm.Z;
+                            // refer inertia to root prim center of mass position
+                            d.MassTranslate(ref tmpdmass,
+                                ppos.X,
+                                ppos.Y,
+                                ppos.Z);
+
+                            d.MassAdd(ref objdmass, ref tmpdmass); // add to total object inertia
+                        }
+                    }
+                }
+            }
+
+            d.GeomClearOffset(prim_geom); // make sure we don't have a hidden offset
+            // associate root geom with body
+            d.GeomSetBody(prim_geom, Body);
+
+            if(noInertiaOverride)
+                d.BodySetPosition(Body, _position.X + objdmass.c.X, _position.Y + objdmass.c.Y, _position.Z + objdmass.c.Z);
+            else
+            {
+                Vector3 ncm =  m_InertiaOverride.CenterOfMass * _orientation;
+                d.BodySetPosition(Body,
+                    _position.X + ncm.X,
+                    _position.Y + ncm.Y,
+                    _position.Z + ncm.Z);
+            }
+
+            d.GeomSetOffsetWorldPosition(prim_geom, _position.X, _position.Y, _position.Z);
+
+            if(noInertiaOverride)
+            {
+                d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body
+                myrot.X = -myrot.X;
+                myrot.Y = -myrot.Y;
+                myrot.Z = -myrot.Z;
+
+                d.RfromQ(out mymat, ref myrot);
+                d.MassRotate(ref objdmass, ref mymat);
+
+                d.BodySetMass(Body, ref objdmass);
+                m_mass = objdmass.mass;
+            }
+            else
+            {
+                objdmass.c.X = 0;
+                objdmass.c.Y = 0;
+                objdmass.c.Z = 0;
+
+                objdmass.I.M00 = m_InertiaOverride.Inertia.X;
+                objdmass.I.M11 = m_InertiaOverride.Inertia.Y;
+                objdmass.I.M22 = m_InertiaOverride.Inertia.Z;
+
+                objdmass.mass = m_InertiaOverride.TotalMass;
+
+                if(Math.Abs(m_InertiaOverride.InertiaRotation.W) < 0.999)
+                {
+                    d.Matrix3 inertiarotmat = new d.Matrix3();
+                    d.Quaternion inertiarot = new d.Quaternion();
+
+                    inertiarot.X = m_InertiaOverride.InertiaRotation.X;
+                    inertiarot.Y = m_InertiaOverride.InertiaRotation.Y;
+                    inertiarot.Z = m_InertiaOverride.InertiaRotation.Z;
+                    inertiarot.W = m_InertiaOverride.InertiaRotation.W;
+                    d.RfromQ(out inertiarotmat, ref inertiarot);
+                    d.MassRotate(ref objdmass, ref inertiarotmat);
+                }
+                d.BodySetMass(Body, ref objdmass);
+
+                m_mass = objdmass.mass;
+            }
+
+            // disconnect from world gravity so we can apply buoyancy
+            d.BodySetGravityMode(Body, false);
+
+            d.BodySetAutoDisableFlag(Body, true);
+            d.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
+            d.BodySetAutoDisableAngularThreshold(Body, 0.05f);
+            d.BodySetAutoDisableLinearThreshold(Body, 0.05f);
+            d.BodySetDamping(Body, .004f, .001f);
+
+            if (m_targetSpace != IntPtr.Zero)
+            {
+                _parent_scene.waitForSpaceUnlock(m_targetSpace);
+                if (d.SpaceQuery(m_targetSpace, prim_geom))
+                    d.SpaceRemove(m_targetSpace, prim_geom);
+            }
+
+            if (childrenPrim.Count == 0)
+            {
+                collide_geom = prim_geom;
+                m_targetSpace = _parent_scene.ActiveSpace;
+            }
+            else
+            {
+                m_targetSpace = d.HashSpaceCreate(_parent_scene.ActiveSpace);
+                d.HashSpaceSetLevels(m_targetSpace, -2, 8);
+                d.SpaceSetSublevel(m_targetSpace, 3);
+                d.SpaceSetCleanup(m_targetSpace, false);
+
+                d.GeomSetCategoryBits(m_targetSpace, (uint)(CollisionCategories.Space |
+                                                            CollisionCategories.Geom |
+                                                            CollisionCategories.Phantom |
+                                                            CollisionCategories.VolumeDtc
+                                                            ));
+                d.GeomSetCollideBits(m_targetSpace, 0);
+                collide_geom = m_targetSpace;
+            }
+
+            d.SpaceAdd(m_targetSpace, prim_geom);
+
+            if (m_delaySelect)
+            {
+                m_isSelected = true;
+                m_delaySelect = false;
+            }
+
+            m_collisionscore = 0;
+
+            UpdateCollisionCatFlags();
+            ApplyCollisionCatFlags();
+
+            _parent_scene.addActivePrim(this);
+
+            lock (childrenPrim)
+            {
+                foreach (OdePrim prm in childrenPrim)
+                {
+                    if (prm.prim_geom == IntPtr.Zero)
+                        continue;
+
+                    Vector3 ppos = prm._position;
+                    d.GeomSetOffsetWorldPosition(prm.prim_geom, ppos.X, ppos.Y, ppos.Z); // set relative position
+
+                    if (prm.m_targetSpace != m_targetSpace)
+                    {
+                        if (prm.m_targetSpace != IntPtr.Zero)
+                        {
+                            _parent_scene.waitForSpaceUnlock(prm.m_targetSpace);
+                            if (d.SpaceQuery(prm.m_targetSpace, prm.prim_geom))
+                                d.SpaceRemove(prm.m_targetSpace, prm.prim_geom);
+                        }
+                        prm.m_targetSpace = m_targetSpace;
+                        d.SpaceAdd(m_targetSpace, prm.prim_geom);
+                    }
+
+                    prm.m_collisionscore = 0;
+
+                    if(!m_disabled)
+                        prm.m_disabled = false;
+
+                    _parent_scene.addActivePrim(prm);
+                }
+            }
+
+            // The body doesn't already have a finite rotation mode set here
+            if (m_angularlocks != 0 && _parent == null)
+            {
+                createAMotor(m_angularlocks);
+            }
+
+            if (m_isSelected || m_disabled)
+            {
+                d.BodyDisable(Body);
+                _zeroFlag = true;
+            }
+            else
+            {
+                d.BodySetAngularVel(Body, m_rotationalVelocity.X, m_rotationalVelocity.Y, m_rotationalVelocity.Z);
+                d.BodySetLinearVel(Body, _velocity.X, _velocity.Y, _velocity.Z);
+
+                _zeroFlag = false;
+                m_bodydisablecontrol = 0;
+            }
+            _parent_scene.addActiveGroups(this);
+        }
+
+        private void DestroyBody()
+        {
+            if (Body != IntPtr.Zero)
+            {
+                _parent_scene.remActivePrim(this);
+
+                collide_geom = IntPtr.Zero;
+
+                if (m_disabled)
+                    m_collisionCategories = 0;
+                else if (m_isSelected)
+                    m_collisionCategories = CollisionCategories.Selected;
+                else if (m_isVolumeDetect)
+                    m_collisionCategories = CollisionCategories.VolumeDtc;
+                else if (m_isphantom)
+                    m_collisionCategories = CollisionCategories.Phantom;
+                else
+                    m_collisionCategories = CollisionCategories.Geom;
+
+                m_collisionFlags = 0;
+
+                if (prim_geom != IntPtr.Zero)
+                {
+                    if (m_NoColide)
+                    {
+                        d.GeomSetCategoryBits(prim_geom, 0);
+                        d.GeomSetCollideBits(prim_geom, 0);
+                    }
+                    else
+                    {
+                        d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories);
+                        d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags);
+                    }
+                    UpdateDataFromGeom();
+                    d.GeomSetBody(prim_geom, IntPtr.Zero);
+                    SetInStaticSpace(this);
+                }
+
+                if (!childPrim)
+                {
+                    lock (childrenPrim)
+                    {
+                        foreach (OdePrim prm in childrenPrim)
+                        {
+                            _parent_scene.remActivePrim(prm);
+
+                            if (prm.m_isSelected)
+                                prm.m_collisionCategories = CollisionCategories.Selected;
+                            else if (prm.m_isVolumeDetect)
+                                prm.m_collisionCategories = CollisionCategories.VolumeDtc;
+                            else if (prm.m_isphantom)
+                                prm.m_collisionCategories = CollisionCategories.Phantom;
+                            else
+                                prm.m_collisionCategories = CollisionCategories.Geom;
+
+                            prm.m_collisionFlags = 0;
+
+                            if (prm.prim_geom != IntPtr.Zero)
+                            {
+                                if (prm.m_NoColide)
+                                {
+                                    d.GeomSetCategoryBits(prm.prim_geom, 0);
+                                    d.GeomSetCollideBits(prm.prim_geom, 0);
+                                }
+                                else
+                                {
+                                    d.GeomSetCategoryBits(prm.prim_geom, (uint)prm.m_collisionCategories);
+                                    d.GeomSetCollideBits(prm.prim_geom, (uint)prm.m_collisionFlags);
+                                }
+                                prm.UpdateDataFromGeom();
+                                SetInStaticSpace(prm);
+                            }
+                            prm.Body = IntPtr.Zero;
+                            prm.m_mass = prm.primMass;
+                            prm.m_collisionscore = 0;
+                        }
+                    }
+                    if (Amotor != IntPtr.Zero)
+                    {
+                        d.JointDestroy(Amotor);
+                        Amotor = IntPtr.Zero;
+                    }
+                    _parent_scene.remActiveGroup(this);
+                    d.BodyDestroy(Body);
+                }
+                Body = IntPtr.Zero;
+            }
+            m_mass = primMass;
+            m_collisionscore = 0;
+        }
+
+        private void FixInertia(Vector3 NewPos,Quaternion newrot)
+        {
+            d.Matrix3 mat = new d.Matrix3();
+            d.Quaternion quat = new d.Quaternion();
+
+            d.Mass tmpdmass = new d.Mass { };
+            d.Mass objdmass = new d.Mass { };
+
+            d.BodyGetMass(Body, out tmpdmass);
+            objdmass = tmpdmass;
+
+            d.Vector3 dobjpos;
+            d.Vector3 thispos;
+
+            // get current object position and rotation
+            dobjpos = d.BodyGetPosition(Body);
+
+            // get prim own inertia in its local frame
+            tmpdmass = primdMass;
+
+            // transform to object frame
+            mat = d.GeomGetOffsetRotation(prim_geom);
+            d.MassRotate(ref tmpdmass, ref mat);
+
+            thispos = d.GeomGetOffsetPosition(prim_geom);
+            d.MassTranslate(ref tmpdmass,
+                            thispos.X,
+                            thispos.Y,
+                            thispos.Z);
+
+            // subtract current prim inertia from object
+            DMassSubPartFromObj(ref tmpdmass, ref objdmass);
+
+            // back prim own inertia
+            tmpdmass = primdMass;
+
+            // update to new position and orientation
+            _position = NewPos;
+            d.GeomSetOffsetWorldPosition(prim_geom, NewPos.X, NewPos.Y, NewPos.Z);
+            _orientation = newrot;
+            quat.X = newrot.X;
+            quat.Y = newrot.Y;
+            quat.Z = newrot.Z;
+            quat.W = newrot.W;
+            d.GeomSetOffsetWorldQuaternion(prim_geom, ref quat);
+
+            mat = d.GeomGetOffsetRotation(prim_geom);
+            d.MassRotate(ref tmpdmass, ref mat);
+
+            thispos = d.GeomGetOffsetPosition(prim_geom);
+            d.MassTranslate(ref tmpdmass,
+                            thispos.X,
+                            thispos.Y,
+                            thispos.Z);
+
+            d.MassAdd(ref objdmass, ref tmpdmass);
+
+            // fix all positions
+            IntPtr g = d.BodyGetFirstGeom(Body);
+            while (g != IntPtr.Zero)
+            {
+                thispos = d.GeomGetOffsetPosition(g);
+                thispos.X -= objdmass.c.X;
+                thispos.Y -= objdmass.c.Y;
+                thispos.Z -= objdmass.c.Z;
+                d.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z);
+                g = d.dBodyGetNextGeom(g);
+            }
+            d.BodyVectorToWorld(Body,objdmass.c.X, objdmass.c.Y, objdmass.c.Z,out thispos);
+
+            d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z);
+            d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body
+            d.BodySetMass(Body, ref objdmass);
+            m_mass = objdmass.mass;
+        }
+
+        private void FixInertia(Vector3 NewPos)
+        {
+            d.Matrix3 primmat = new d.Matrix3();
+            d.Mass tmpdmass = new d.Mass { };
+            d.Mass objdmass = new d.Mass { };
+            d.Mass primmass = new d.Mass { };
+
+            d.Vector3 dobjpos;
+            d.Vector3 thispos;
+
+            d.BodyGetMass(Body, out objdmass);
+
+            // get prim own inertia in its local frame
+            primmass = primdMass;
+            // transform to object frame
+            primmat = d.GeomGetOffsetRotation(prim_geom);
+            d.MassRotate(ref primmass, ref primmat);
+
+            tmpdmass = primmass;
+
+            thispos = d.GeomGetOffsetPosition(prim_geom);
+            d.MassTranslate(ref tmpdmass,
+                            thispos.X,
+                            thispos.Y,
+                            thispos.Z);
+
+            // subtract current prim inertia from object
+            DMassSubPartFromObj(ref tmpdmass, ref objdmass);
+
+            // update to new position
+            _position = NewPos;
+            d.GeomSetOffsetWorldPosition(prim_geom, NewPos.X, NewPos.Y, NewPos.Z);
+
+            thispos = d.GeomGetOffsetPosition(prim_geom);
+            d.MassTranslate(ref primmass,
+                            thispos.X,
+                            thispos.Y,
+                            thispos.Z);
+
+            d.MassAdd(ref objdmass, ref primmass);
+
+            // fix all positions
+            IntPtr g = d.BodyGetFirstGeom(Body);
+            while (g != IntPtr.Zero)
+            {
+                thispos = d.GeomGetOffsetPosition(g);
+                thispos.X -= objdmass.c.X;
+                thispos.Y -= objdmass.c.Y;
+                thispos.Z -= objdmass.c.Z;
+                d.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z);
+                g = d.dBodyGetNextGeom(g);
+            }
+
+            d.BodyVectorToWorld(Body, objdmass.c.X, objdmass.c.Y, objdmass.c.Z, out thispos);
+
+            // get current object position and rotation
+            dobjpos = d.BodyGetPosition(Body);
+
+            d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z);
+            d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body
+            d.BodySetMass(Body, ref objdmass);
+            m_mass = objdmass.mass;
+        }
+
+        private void FixInertia(Quaternion newrot)
+        {
+            d.Matrix3 mat = new d.Matrix3();
+            d.Quaternion quat = new d.Quaternion();
+
+            d.Mass tmpdmass = new d.Mass { };
+            d.Mass objdmass = new d.Mass { };
+            d.Vector3 dobjpos;
+            d.Vector3 thispos;
+
+            d.BodyGetMass(Body, out objdmass);
+
+            // get prim own inertia in its local frame
+            tmpdmass = primdMass;
+            mat = d.GeomGetOffsetRotation(prim_geom);
+            d.MassRotate(ref tmpdmass, ref mat);
+            // transform to object frame
+            thispos = d.GeomGetOffsetPosition(prim_geom);
+            d.MassTranslate(ref tmpdmass,
+                            thispos.X,
+                            thispos.Y,
+                            thispos.Z);
+
+            // subtract current prim inertia from object
+            DMassSubPartFromObj(ref tmpdmass, ref objdmass);
+
+            // update to new orientation
+            _orientation = newrot;
+            quat.X = newrot.X;
+            quat.Y = newrot.Y;
+            quat.Z = newrot.Z;
+            quat.W = newrot.W;
+            d.GeomSetOffsetWorldQuaternion(prim_geom, ref quat);
+
+            tmpdmass = primdMass;
+            mat = d.GeomGetOffsetRotation(prim_geom);
+            d.MassRotate(ref tmpdmass, ref mat);
+            d.MassTranslate(ref tmpdmass,
+                            thispos.X,
+                            thispos.Y,
+                            thispos.Z);
+
+            d.MassAdd(ref objdmass, ref tmpdmass);
+
+            // fix all positions
+            IntPtr g = d.BodyGetFirstGeom(Body);
+            while (g != IntPtr.Zero)
+            {
+                thispos = d.GeomGetOffsetPosition(g);
+                thispos.X -= objdmass.c.X;
+                thispos.Y -= objdmass.c.Y;
+                thispos.Z -= objdmass.c.Z;
+                d.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z);
+                g = d.dBodyGetNextGeom(g);
+            }
+
+            d.BodyVectorToWorld(Body, objdmass.c.X, objdmass.c.Y, objdmass.c.Z, out thispos);
+            // get current object position and rotation
+            dobjpos = d.BodyGetPosition(Body);
+
+            d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z);
+            d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body
+            d.BodySetMass(Body, ref objdmass);
+            m_mass = objdmass.mass;
+        }
+
+
+        #region Mass Calculation
+
+        private void UpdatePrimBodyData()
+        {
+            primMass = m_density * primVolume;
+
+            if (primMass <= 0)
+                primMass = 0.0001f;//ckrinke: Mass must be greater then zero.
+            if (primMass > _parent_scene.maximumMassObject)
+                primMass = _parent_scene.maximumMassObject;
+
+            m_mass = primMass; // just in case
+
+            d.MassSetBoxTotal(out primdMass, primMass, 2.0f * m_OBB.X, 2.0f * m_OBB.Y, 2.0f * m_OBB.Z);
+
+            d.MassTranslate(ref primdMass,
+                                m_OBBOffset.X,
+                                m_OBBOffset.Y,
+                                m_OBBOffset.Z);
+
+            primOOBradiusSQ = m_OBB.LengthSquared();
+
+            if (_triMeshData != IntPtr.Zero)
+            {
+                float pc = m_physCost;
+                float psf = primOOBradiusSQ;
+                psf *= 1.33f * .2f;
+                pc *= psf;
+                if (pc < 0.1f)
+                    pc = 0.1f;
+
+                m_physCost = pc;
+            }
+            else
+                m_physCost = 0.1f;
+
+            m_streamCost = 1.0f;
+        }
+
+        #endregion
+
+
+        /// <summary>
+        /// Add a child prim to this parent prim.
+        /// </summary>
+        /// <param name="prim">Child prim</param>
+        // I'm the parent
+        // prim is the child
+        public void ParentPrim(OdePrim prim)
+        {
+            //Console.WriteLine("ParentPrim  " + m_primName);
+            if (this.m_localID != prim.m_localID)
+            {
+                DestroyBody();  // for now we need to rebuil entire object on link change
+
+                lock (childrenPrim)
+                {
+                    // adopt the prim
+                    if (!childrenPrim.Contains(prim))
+                        childrenPrim.Add(prim);
+
+                    // see if this prim has kids and adopt them also
+                    // should not happen for now
+                    foreach (OdePrim prm in prim.childrenPrim)
+                    {
+                        if (!childrenPrim.Contains(prm))
+                        {
+                            if (prm.Body != IntPtr.Zero)
+                            {
+                                if (prm.prim_geom != IntPtr.Zero)
+                                    d.GeomSetBody(prm.prim_geom, IntPtr.Zero);
+                                if (prm.Body != prim.Body)
+                                    prm.DestroyBody(); // don't loose bodies around
+                                prm.Body = IntPtr.Zero;
+                            }
+
+                            childrenPrim.Add(prm);
+                            prm._parent = this;
+                        }
+                    }
+                }
+                //Remove old children from the prim
+                prim.childrenPrim.Clear();
+
+                if (prim.Body != IntPtr.Zero)
+                {
+                    if (prim.prim_geom != IntPtr.Zero)
+                        d.GeomSetBody(prim.prim_geom, IntPtr.Zero);
+                    prim.DestroyBody(); // don't loose bodies around
+                    prim.Body = IntPtr.Zero;
+                }
+
+                prim.childPrim = true;
+                prim._parent = this;
+
+                MakeBody(); // full nasty reconstruction
+            }
+        }
+
+        private void UpdateChildsfromgeom()
+        {
+            if (childrenPrim.Count > 0)
+            {
+                foreach (OdePrim prm in childrenPrim)
+                    prm.UpdateDataFromGeom();
+            }
+        }
+
+        private void UpdateDataFromGeom()
+        {
+            if (prim_geom != IntPtr.Zero)
+            {
+                d.Quaternion qtmp;
+                d.GeomCopyQuaternion(prim_geom, out qtmp);
+                _orientation.X = qtmp.X;
+                _orientation.Y = qtmp.Y;
+                _orientation.Z = qtmp.Z;
+                _orientation.W = qtmp.W;
+/*
+// Debug
+                float qlen = _orientation.Length();
+                if (qlen > 1.01f || qlen < 0.99)
+                    m_log.WarnFormat("[PHYSICS]: Got nonnorm quaternion from geom in Object {0} norm {1}", Name, qlen);
+//
+*/
+                _orientation.Normalize();
+
+                d.Vector3 lpos = d.GeomGetPosition(prim_geom);
+                _position.X = lpos.X;
+                _position.Y = lpos.Y;
+                _position.Z = lpos.Z;
+            }
+        }
+
+        private void ChildDelink(OdePrim odePrim, bool remakebodies)
+        {
+            // Okay, we have a delinked child.. destroy all body and remake
+            if (odePrim != this && !childrenPrim.Contains(odePrim))
+                return;
+
+            DestroyBody();
+
+            if (odePrim == this) // delinking the root prim
+            {
+                OdePrim newroot = null;
+                lock (childrenPrim)
+                {
+                    if (childrenPrim.Count > 0)
+                    {
+                        newroot = childrenPrim[0];
+                        childrenPrim.RemoveAt(0);
+                        foreach (OdePrim prm in childrenPrim)
+                        {
+                            newroot.childrenPrim.Add(prm);
+                        }
+                        childrenPrim.Clear();
+                    }
+                    if (newroot != null)
+                    {
+                        newroot.childPrim = false;
+                        newroot._parent = null;
+                        if (remakebodies)
+                            newroot.MakeBody();
+                    }
+                }
+            }
+
+            else
+            {
+                lock (childrenPrim)
+                {
+                    childrenPrim.Remove(odePrim);
+                    odePrim.childPrim = false;
+                    odePrim._parent = null;
+                    //                    odePrim.UpdateDataFromGeom();
+                    if (remakebodies)
+                        odePrim.MakeBody();
+                }
+            }
+            if (remakebodies)
+                MakeBody();
+        }
+
+        protected void ChildRemove(OdePrim odePrim, bool reMakeBody)
+        {
+            // Okay, we have a delinked child.. destroy all body and remake
+            if (odePrim != this && !childrenPrim.Contains(odePrim))
+                return;
+
+            DestroyBody();
+
+            if (odePrim == this)
+            {
+                OdePrim newroot = null;
+                lock (childrenPrim)
+                {
+                    if (childrenPrim.Count > 0)
+                    {
+                        newroot = childrenPrim[0];
+                        childrenPrim.RemoveAt(0);
+                        foreach (OdePrim prm in childrenPrim)
+                        {
+                            newroot.childrenPrim.Add(prm);
+                        }
+                        childrenPrim.Clear();
+                    }
+                    if (newroot != null)
+                    {
+                        newroot.childPrim = false;
+                        newroot._parent = null;
+                        newroot.MakeBody();
+                    }
+                }
+                if (reMakeBody)
+                    MakeBody();
+                return;
+            }
+            else
+            {
+                lock (childrenPrim)
+                {
+                    childrenPrim.Remove(odePrim);
+                    odePrim.childPrim = false;
+                    odePrim._parent = null;
+                    if (reMakeBody)
+                        odePrim.MakeBody();
+                }
+            }
+            MakeBody();
+        }
+
+
+        #region changes
+
+        private void changeadd()
+        {
+            _parent_scene.addToPrims(this);
+        }
+
+        private void changeAngularLock(byte newLocks)
+        {
+            // do we have a Physical object?
+            if (Body != IntPtr.Zero)
+            {
+                //Check that we have a Parent
+                //If we have a parent then we're not authorative here
+                if (_parent == null)
+                {
+                    if (newLocks != 0)
+                    {
+                        createAMotor(newLocks);
+                    }
+                    else
+                    {
+                        if (Amotor != IntPtr.Zero)
+                        {
+                            d.JointDestroy(Amotor);
+                            Amotor = IntPtr.Zero;
+                        }
+                    }
+                }
+            }
+            // Store this for later in case we get turned into a separate body
+            m_angularlocks = newLocks;
+        }
+
+        private void changeLink(OdePrim NewParent)
+        {
+            if (_parent == null && NewParent != null)
+            {
+                NewParent.ParentPrim(this);
+            }
+            else if (_parent != null)
+            {
+                if (_parent is OdePrim)
+                {
+                    if (NewParent != _parent)
+                    {
+                        (_parent as OdePrim).ChildDelink(this, false); // for now...
+                        childPrim = false;
+
+                        if (NewParent != null)
+                        {
+                            NewParent.ParentPrim(this);
+                        }
+                    }
+                }
+            }
+            _parent = NewParent;
+        }
+
+
+        private void Stop()
+        {
+            if (!childPrim)
+            {
+//                m_force = Vector3.Zero;
+                m_forceacc = Vector3.Zero;
+                m_angularForceacc = Vector3.Zero;
+//                m_torque = Vector3.Zero;
+                _velocity = Vector3.Zero;
+                m_acceleration = Vector3.Zero;
+                m_rotationalVelocity = Vector3.Zero;
+                _target_velocity = Vector3.Zero;
+                if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE)
+                    m_vehicle.Stop();
+
+                _zeroFlag = false;
+                base.RequestPhysicsterseUpdate();
+            }
+
+            if (Body != IntPtr.Zero)
+            {
+                d.BodySetForce(Body, 0f, 0f, 0f);
+                d.BodySetTorque(Body, 0f, 0f, 0f);
+                d.BodySetLinearVel(Body, 0f, 0f, 0f);
+                d.BodySetAngularVel(Body, 0f, 0f, 0f);
+            }
+        }
+
+        private void changePhantomStatus(bool newval)
+        {
+            m_isphantom = newval;
+
+            UpdateCollisionCatFlags();
+            ApplyCollisionCatFlags();
+        }
+
+/* not in use
+        internal void ChildSelectedChange(bool childSelect)
+        {
+            if(childPrim)
+                return;
+
+            if (childSelect == m_isSelected)
+                return;
+
+            if (childSelect)
+            {
+                DoSelectedStatus(true);
+            }
+
+            else
+            {
+                foreach (OdePrim prm in childrenPrim)
+                {
+                    if (prm.m_isSelected)
+                        return;
+                }
+                DoSelectedStatus(false);
+            }
+        }
+*/
+        private void changeSelectedStatus(bool newval)
+        {
+            if (m_lastdoneSelected == newval)
+                return;
+
+            m_lastdoneSelected = newval;
+            DoSelectedStatus(newval);
+        }
+
+        private void CheckDelaySelect()
+        {
+            if (m_delaySelect)
+            {
+                DoSelectedStatus(m_isSelected);
+            }
+        }
+
+        private void DoSelectedStatus(bool newval)
+        {
+            m_isSelected = newval;
+            Stop();
+
+            if (newval)
+            {
+                if (!childPrim && Body != IntPtr.Zero)
+                    d.BodyDisable(Body);
+
+                if (m_delaySelect || m_isphysical)
+                {
+                    m_collisionCategories = CollisionCategories.Selected;
+                    m_collisionFlags = 0;
+
+                    if (!childPrim)
+                    {
+                        foreach (OdePrim prm in childrenPrim)
+                        {
+                            prm.m_collisionCategories = m_collisionCategories;
+                            prm.m_collisionFlags = m_collisionFlags;
+
+                            if (prm.prim_geom != IntPtr.Zero)
+                            {
+
+                                if (prm.m_NoColide)
+                                {
+                                    d.GeomSetCategoryBits(prm.prim_geom, 0);
+                                    d.GeomSetCollideBits(prm.prim_geom, 0);
+                                }
+                                else
+                                {
+                                    d.GeomSetCategoryBits(prm.prim_geom, (uint)m_collisionCategories);
+                                    d.GeomSetCollideBits(prm.prim_geom, (uint)m_collisionFlags);
+                                }
+                            }
+                            prm.m_delaySelect = false;
+                        }
+                    }
+//                    else if (_parent != null)
+//                        ((OdePrim)_parent).ChildSelectedChange(true);
+
+
+                    if (prim_geom != IntPtr.Zero)
+                    {
+                        if (m_NoColide)
+                        {
+                            d.GeomSetCategoryBits(prim_geom, 0);
+                            d.GeomSetCollideBits(prim_geom, 0);
+                            if (collide_geom != prim_geom && collide_geom != IntPtr.Zero)
+                            {
+                                d.GeomSetCategoryBits(collide_geom, 0);
+                                d.GeomSetCollideBits(collide_geom, 0);
+                            }
+
+                        }
+                        else
+                        {
+                            d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories);
+                            d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags);
+                            if (collide_geom != prim_geom && collide_geom != IntPtr.Zero)
+                            {
+                                d.GeomSetCategoryBits(collide_geom, (uint)m_collisionCategories);
+                                d.GeomSetCollideBits(collide_geom, (uint)m_collisionFlags);
+                            }
+                        }
+                    }
+
+                    m_delaySelect = false;
+                }
+                else if(!m_isphysical)
+                {
+                    m_delaySelect = true;
+                }
+            }
+            else
+            {
+                if (!childPrim)
+                {
+                    if (Body != IntPtr.Zero && !m_disabled)
+                    {
+                        _zeroFlag = true;
+                        d.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
+                        d.BodyEnable(Body);
+                    }
+                }
+//                else if (_parent != null)
+//                    ((OdePrim)_parent).ChildSelectedChange(false);
+
+                UpdateCollisionCatFlags();
+                ApplyCollisionCatFlags();
+
+                m_delaySelect = false;
+            }
+
+            resetCollisionAccounting();
+        }
+
+        private void changePosition(Vector3 newPos)
+        {
+            CheckDelaySelect();
+            if (m_isphysical)
+            {
+                if (childPrim)  // inertia is messed, must rebuild
+                {
+                    if (m_building)
+                    {
+                        _position = newPos;
+                    }
+
+                    else if (m_forcePosOrRotation && _position != newPos && Body != IntPtr.Zero)
+                    {
+                        FixInertia(newPos);
+                        if (!d.BodyIsEnabled(Body))
+                        {
+                            _zeroFlag = true;
+                            d.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
+                            d.BodyEnable(Body);
+                        }
+                    }
+                }
+                else
+                {
+                    if (_position != newPos)
+                    {
+                        d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z);
+                        _position = newPos;
+                    }
+                    if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
+                    {
+                        _zeroFlag = true;
+                        d.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
+                        d.BodyEnable(Body);
+                    }
+                }
+            }
+            else
+            {
+                if (prim_geom != IntPtr.Zero)
+                {
+                    if (newPos != _position)
+                    {
+                        d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z);
+                        _position = newPos;
+
+                        m_targetSpace = _parent_scene.MoveGeomToStaticSpace(prim_geom, _position, m_targetSpace);
+                    }
+                }
+            }
+            givefakepos--;
+            if (givefakepos < 0)
+                givefakepos = 0;
+//            changeSelectedStatus();
+            resetCollisionAccounting();
+        }
+
+        private void changeOrientation(Quaternion newOri)
+        {
+            CheckDelaySelect();
+            if (m_isphysical)
+            {
+                if (childPrim)  // inertia is messed, must rebuild
+                {
+                    if (m_building)
+                    {
+                        _orientation = newOri;
+                    }
+/*
+                    else if (m_forcePosOrRotation && _orientation != newOri && Body != IntPtr.Zero)
+                    {
+                        FixInertia(_position, newOri);
+                        if (!d.BodyIsEnabled(Body))
+                            d.BodyEnable(Body);
+                    }
+*/
+                }
+                else
+                {
+                    if (newOri != _orientation)
+                    {
+                        d.Quaternion myrot = new d.Quaternion();
+                        myrot.X = newOri.X;
+                        myrot.Y = newOri.Y;
+                        myrot.Z = newOri.Z;
+                        myrot.W = newOri.W;
+                        d.GeomSetQuaternion(prim_geom, ref myrot);
+                        _orientation = newOri;
+                        
+                        if (Body != IntPtr.Zero)
+                        {
+                            if(m_angularlocks != 0)
+                                createAMotor(m_angularlocks);
+                        }
+                    }
+                    if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
+                    {
+                        _zeroFlag = true;
+                        d.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
+                        d.BodyEnable(Body);
+                    }
+                }
+            }
+            else
+            {
+                if (prim_geom != IntPtr.Zero)
+                {
+                    if (newOri != _orientation)
+                    {
+                        d.Quaternion myrot = new d.Quaternion();
+                        myrot.X = newOri.X;
+                        myrot.Y = newOri.Y;
+                        myrot.Z = newOri.Z;
+                        myrot.W = newOri.W;
+                        d.GeomSetQuaternion(prim_geom, ref myrot);
+                        _orientation = newOri;
+                    }
+                }
+            }
+            givefakeori--;
+            if (givefakeori < 0)
+                givefakeori = 0;
+            resetCollisionAccounting();
+        }
+
+        private void changePositionAndOrientation(Vector3 newPos, Quaternion newOri)
+        {
+            CheckDelaySelect();
+            if (m_isphysical)
+            {
+                if (childPrim && m_building)  // inertia is messed, must rebuild
+                {
+                    _position = newPos;
+                    _orientation = newOri;
+                }
+                else
+                {
+                    if (newOri != _orientation)
+                    {
+                        d.Quaternion myrot = new d.Quaternion();
+                        myrot.X = newOri.X;
+                        myrot.Y = newOri.Y;
+                        myrot.Z = newOri.Z;
+                        myrot.W = newOri.W;
+                        d.GeomSetQuaternion(prim_geom, ref myrot);
+                        _orientation = newOri;
+                        if (Body != IntPtr.Zero && m_angularlocks != 0)
+                            createAMotor(m_angularlocks);
+                    }
+                    if (_position != newPos)
+                    {
+                        d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z);
+                        _position = newPos;
+                    }
+                    if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
+                    {
+                        _zeroFlag = true;
+                        d.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
+                        d.BodyEnable(Body);
+                    }
+                }
+            }
+            else
+            {
+                // string primScenAvatarIn = _parent_scene.whichspaceamIin(_position);
+                // int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position);
+
+                if (prim_geom != IntPtr.Zero)
+                {
+                    if (newOri != _orientation)
+                    {
+                        d.Quaternion myrot = new d.Quaternion();
+                        myrot.X = newOri.X;
+                        myrot.Y = newOri.Y;
+                        myrot.Z = newOri.Z;
+                        myrot.W = newOri.W;
+                        d.GeomSetQuaternion(prim_geom, ref myrot);
+                        _orientation = newOri;
+                    }
+
+                    if (newPos != _position)
+                    {
+                        d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z);
+                        _position = newPos;
+
+                        m_targetSpace = _parent_scene.MoveGeomToStaticSpace(prim_geom, _position, m_targetSpace);
+                    }
+                }
+            }
+            givefakepos--;
+            if (givefakepos < 0)
+                givefakepos = 0;
+            givefakeori--;
+            if (givefakeori < 0)
+                givefakeori = 0;
+            resetCollisionAccounting();
+        }
+
+        private void changeDisable(bool disable)
+        {
+            if (disable)
+            {
+                if (!m_disabled)
+                    disableBodySoft();
+            }
+            else
+            {
+                if (m_disabled)
+                    enableBodySoft();
+            }
+        }
+
+        private void changePhysicsStatus(bool NewStatus)
+        {
+            CheckDelaySelect();
+
+            m_isphysical = NewStatus;
+
+            if (!childPrim)
+            {
+                if (NewStatus)
+                {
+                    if (Body == IntPtr.Zero)
+                        MakeBody();
+                }
+                else
+                {
+                    if (Body != IntPtr.Zero)
+                    {
+                        DestroyBody();
+                    }
+                    Stop();
+                }
+            }
+
+            resetCollisionAccounting();
+        }
+
+        private void changeSize(Vector3 newSize)
+        {
+        }
+
+        private void changeShape(PrimitiveBaseShape newShape)
+        {
+        }
+
+        private void changeAddPhysRep(ODEPhysRepData repData)
+        {
+            _size = repData.size; //??
+            _pbs = repData.pbs;
+
+            m_mesh = repData.mesh;
+
+            m_assetID = repData.assetID;
+            m_meshState = repData.meshState;
+
+            m_hasOBB = repData.hasOBB;
+            m_OBBOffset = repData.OBBOffset;
+            m_OBB = repData.OBB;
+
+            primVolume = repData.volume;
+
+            CreateGeom(repData.isTooSmall);
+
+            if (prim_geom != IntPtr.Zero)
+            {
+                d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
+                d.Quaternion myrot = new d.Quaternion();
+                myrot.X = _orientation.X;
+                myrot.Y = _orientation.Y;
+                myrot.Z = _orientation.Z;
+                myrot.W = _orientation.W;
+                d.GeomSetQuaternion(prim_geom, ref myrot);
+            }
+
+            if (!m_isphysical)
+            {
+                SetInStaticSpace(this);
+                UpdateCollisionCatFlags();
+                ApplyCollisionCatFlags();
+            }
+            else
+                MakeBody();
+
+            if ((m_meshState & MeshState.NeedMask) != 0)
+            {
+                repData.size = _size;
+                repData.pbs = _pbs;
+                repData.shapetype = m_fakeShapetype;
+                _parent_scene.m_meshWorker.RequestMesh(repData);
+            }
+            else
+                m_shapetype = repData.shapetype;
+        }
+
+        private void changePhysRepData(ODEPhysRepData repData)
+        {
+            if(_size == repData.size &&
+                    _pbs == repData.pbs &&
+                    m_shapetype == repData.shapetype &&
+                    m_mesh == repData.mesh &&
+                    primVolume == repData.volume)
+                return;
+
+            CheckDelaySelect();
+
+            OdePrim parent = (OdePrim)_parent;
+
+            bool chp = childPrim;
+
+            if (chp)
+            {
+                if (parent != null)
+                {
+                    parent.DestroyBody();
+                }
+            }
+            else
+            {
+                DestroyBody();
+            }
+
+            RemoveGeom();
+
+            _size = repData.size;
+            _pbs = repData.pbs;
+
+            m_mesh = repData.mesh;
+
+            m_assetID = repData.assetID;
+            m_meshState = repData.meshState;
+
+            m_hasOBB = repData.hasOBB;
+            m_OBBOffset = repData.OBBOffset;
+            m_OBB = repData.OBB;
+
+            primVolume = repData.volume;
+
+            CreateGeom(repData.isTooSmall);
+
+            if (prim_geom != IntPtr.Zero)
+            {
+                d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
+                d.Quaternion myrot = new d.Quaternion();
+                myrot.X = _orientation.X;
+                myrot.Y = _orientation.Y;
+                myrot.Z = _orientation.Z;
+                myrot.W = _orientation.W;
+                d.GeomSetQuaternion(prim_geom, ref myrot);
+            }
+
+            if (m_isphysical)
+            {
+                if (chp)
+                {
+                    if (parent != null)
+                    {
+                        parent.MakeBody();
+                    }
+                }
+                else
+                    MakeBody();
+            }
+            else
+            {
+                SetInStaticSpace(this);
+                UpdateCollisionCatFlags();
+                ApplyCollisionCatFlags();
+            }
+
+            resetCollisionAccounting();
+
+            if ((m_meshState & MeshState.NeedMask) != 0)
+            {
+                repData.size = _size;
+                repData.pbs = _pbs;
+                repData.shapetype = m_fakeShapetype;
+                _parent_scene.m_meshWorker.RequestMesh(repData);
+            }
+            else
+                m_shapetype = repData.shapetype;
+        }
+
+        private void changeFloatOnWater(bool newval)
+        {
+            m_collidesWater = newval;
+
+            UpdateCollisionCatFlags();
+            ApplyCollisionCatFlags();
+        }
+
+        private void changeSetTorque(Vector3 newtorque)
+        {
+            if (!m_isSelected && !m_outbounds)
+            {
+                if (m_isphysical && Body != IntPtr.Zero)
+                {
+                    if (m_disabled)
+                        enableBodySoft();
+                    else if (!d.BodyIsEnabled(Body))
+                    {
+                        d.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
+                        d.BodyEnable(Body);
+                    }
+                }
+                m_torque = newtorque;
+            }
+        }
+
+        private void changeForce(Vector3 force)
+        {
+            m_force = force;
+            if (!m_isSelected && !m_outbounds && Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
+            {
+                d.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
+                d.BodyEnable(Body);
+            }
+        }
+
+        private void changeAddForce(Vector3 theforce)
+        {
+            m_forceacc += theforce;
+            if (!m_isSelected && !m_outbounds)
+            {
+                lock (this)
+                {
+                    //m_log.Info("[PHYSICS]: dequeing forcelist");
+                    if (m_isphysical && Body != IntPtr.Zero)
+                    {
+                        if (m_disabled)
+                            enableBodySoft();
+                        else if (!d.BodyIsEnabled(Body))
+                        {
+                            d.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
+                            d.BodyEnable(Body);
+                        }
+                    }
+                }
+                m_collisionscore = 0;
+            }
+        }
+
+        // actually angular impulse
+        private void changeAddAngularImpulse(Vector3 aimpulse)
+        {
+            m_angularForceacc += aimpulse * m_invTimeStep;
+            if (!m_isSelected && !m_outbounds)
+            {
+                lock (this)
+                {
+                    if (m_isphysical && Body != IntPtr.Zero)
+                    {
+                        if (m_disabled)
+                            enableBodySoft();
+                        else if (!d.BodyIsEnabled(Body))
+                        {
+                            d.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
+                            d.BodyEnable(Body);
+                        }                            
+                    }
+                }
+                m_collisionscore = 0;
+            }
+        }
+
+        private void changevelocity(Vector3 newVel)
+        {
+            float len = newVel.LengthSquared();
+            if (len > 100000.0f) // limit to 100m/s
+            {
+                len = 100.0f / (float)Math.Sqrt(len);
+                newVel *= len;
+            }
+
+            if (!m_isSelected && !m_outbounds)
+            {
+                if (Body != IntPtr.Zero)
+                {
+                    if (m_disabled)
+                        enableBodySoft();
+                    else if (!d.BodyIsEnabled(Body))
+                    {
+                        d.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
+                        d.BodyEnable(Body);
+                    }
+                    d.BodySetLinearVel(Body, newVel.X, newVel.Y, newVel.Z);
+                }
+                //resetCollisionAccounting();
+            }
+            _velocity = newVel;
+        }
+
+        private void changeangvelocity(Vector3 newAngVel)
+        {
+            float len = newAngVel.LengthSquared();
+            if (len > _parent_scene.maxAngVelocitySQ)
+            {
+                len = _parent_scene.maximumAngularVelocity / (float)Math.Sqrt(len);
+                newAngVel *= len;
+            }
+
+            if (!m_isSelected && !m_outbounds)
+            {
+                if (Body != IntPtr.Zero)
+                {
+                    if (m_disabled)
+                        enableBodySoft();
+                    else if (!d.BodyIsEnabled(Body))
+                    {
+                        d.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
+                        d.BodyEnable(Body);
+                    }
+                    d.BodySetAngularVel(Body, newAngVel.X, newAngVel.Y, newAngVel.Z);
+                }
+                //resetCollisionAccounting();
+            }
+            m_rotationalVelocity = newAngVel;
+        }
+
+        private void changeVolumedetetion(bool newVolDtc)
+        {
+            m_isVolumeDetect = newVolDtc;
+            m_fakeisVolumeDetect = newVolDtc;
+            UpdateCollisionCatFlags();
+            ApplyCollisionCatFlags();
+        }
+
+        protected void changeBuilding(bool newbuilding)
+        {
+            // Check if we need to do anything
+            if (newbuilding == m_building)
+                return;
+
+            if ((bool)newbuilding)
+            {
+                m_building = true;
+                if (!childPrim)
+                    DestroyBody();
+            }
+            else
+            {
+                m_building = false;
+                CheckDelaySelect();
+                if (!childPrim)
+                    MakeBody();
+            }
+            if (!childPrim && childrenPrim.Count > 0)
+            {
+                foreach (OdePrim prm in childrenPrim)
+                    prm.changeBuilding(m_building); // call directly
+            }
+        }
+
+       public void changeSetVehicle(VehicleData vdata)
+        {
+            if (m_vehicle == null)
+                m_vehicle = new ODEDynamics(this);
+            m_vehicle.DoSetVehicle(vdata);
+        }
+
+        private void changeVehicleType(int value)
+        {
+            if (value == (int)Vehicle.TYPE_NONE)
+            {
+                if (m_vehicle != null)
+                    m_vehicle = null;
+            }
+            else
+            {
+                if (m_vehicle == null)
+                    m_vehicle = new ODEDynamics(this);
+
+                m_vehicle.ProcessTypeChange((Vehicle)value);
+            }
+        }
+
+        private void changeVehicleFloatParam(strVehicleFloatParam fp)
+        {
+            if (m_vehicle == null)
+                return;
+
+            m_vehicle.ProcessFloatVehicleParam((Vehicle)fp.param, fp.value);
+        }
+
+        private void changeVehicleVectorParam(strVehicleVectorParam vp)
+        {
+            if (m_vehicle == null)
+                return;
+            m_vehicle.ProcessVectorVehicleParam((Vehicle)vp.param, vp.value);
+        }
+
+        private void changeVehicleRotationParam(strVehicleQuatParam qp)
+        {
+            if (m_vehicle == null)
+                return;
+            m_vehicle.ProcessRotationVehicleParam((Vehicle)qp.param, qp.value);
+        }
+
+        private void changeVehicleFlags(strVehicleBoolParam bp)
+        {
+            if (m_vehicle == null)
+                return;
+            m_vehicle.ProcessVehicleFlags(bp.param, bp.value);
+        }
+
+        private void changeBuoyancy(float b)
+        {
+            m_buoyancy = b;
+        }
+
+        private void changePIDTarget(Vector3 trg)
+        {
+            m_PIDTarget = trg;
+        }
+
+        private void changePIDTau(float tau)
+        {
+            m_PIDTau = tau;
+        }
+
+        private void changePIDActive(bool val)
+        {
+            m_usePID = val;
+        }
+
+        private void changePIDHoverHeight(float val)
+        {
+          m_PIDHoverHeight = val;
+          if (val == 0)
+            m_useHoverPID = false;
+        }
+
+        private void changePIDHoverType(PIDHoverType type)
+        {
+            m_PIDHoverType = type;
+        }
+
+        private void changePIDHoverTau(float tau)
+        {
+            m_PIDHoverTau = tau;
+        }
+
+        private void changePIDHoverActive(bool active)
+        {
+            m_useHoverPID = active;
+        }
+
+        private void changeInertia(PhysicsInertiaData inertia)
+        {
+            m_InertiaOverride = inertia;
+
+            if (Body != IntPtr.Zero)
+                DestroyBody();
+            MakeBody();
+        }
+
+        #endregion
+
+        public void Move()
+        {
+            if (!childPrim && m_isphysical && Body != IntPtr.Zero &&
+                !m_disabled && !m_isSelected && !m_building && !m_outbounds)
+            {
+                if (!d.BodyIsEnabled(Body))
+                {
+                    // let vehicles sleep
+                    if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE)
+                        return;
+
+                    if (++m_bodydisablecontrol < 50)
+                        return;
+
+                    // clear residuals
+                    d.BodySetAngularVel(Body,0f,0f,0f);
+                    d.BodySetLinearVel(Body,0f,0f,0f);
+                    _zeroFlag = true;
+                    d.BodySetAutoDisableSteps(Body, 1);
+                    d.BodyEnable(Body);
+                    m_bodydisablecontrol = -3;
+                }
+
+                if(m_bodydisablecontrol < 0)
+                    m_bodydisablecontrol++;
+
+                d.Vector3 lpos = d.GeomGetPosition(prim_geom); // root position that is seem by rest of simulator
+
+                if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE)
+                {
+                    // 'VEHICLES' are dealt with in ODEDynamics.cs
+                    m_vehicle.Step();
+                    return;
+                }
+
+                float fx = 0;
+                float fy = 0;
+                float fz = 0;
+
+                float mass = m_mass;
+
+                if (m_usePID && m_PIDTau > 0)
+                {
+                    // for now position error
+                    _target_velocity =
+                        new Vector3(
+                            (m_PIDTarget.X - lpos.X),
+                            (m_PIDTarget.Y - lpos.Y),
+                            (m_PIDTarget.Z - lpos.Z)
+                            );
+
+                    if (_target_velocity.ApproxEquals(Vector3.Zero, 0.02f))
+                    {
+                        d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);
+                        d.BodySetLinearVel(Body, 0, 0, 0);
+                        return;
+                    }
+                    else
+                    {
+                        _zeroFlag = false;
+
+                        float tmp = 1 / m_PIDTau;
+                        _target_velocity *= tmp;
+
+                        // apply limits
+                        tmp = _target_velocity.Length();
+                        if (tmp > 50.0f)
+                        {
+                            tmp = 50 / tmp;
+                            _target_velocity *= tmp;
+                        }
+                        else if (tmp < 0.05f)
+                        {
+                            tmp = 0.05f / tmp;
+                            _target_velocity *= tmp;
+                        }
+
+                        d.Vector3 vel = d.BodyGetLinearVel(Body);
+                        fx = (_target_velocity.X - vel.X) * m_invTimeStep;
+                        fy = (_target_velocity.Y - vel.Y) * m_invTimeStep;
+                        fz = (_target_velocity.Z - vel.Z) * m_invTimeStep;
+//                        d.BodySetLinearVel(Body, _target_velocity.X, _target_velocity.Y, _target_velocity.Z);
+                    }
+                }        // end if (m_usePID)
+
+                // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller
+                else if (m_useHoverPID && m_PIDHoverTau != 0 && m_PIDHoverHeight != 0)
+                {
+
+                    //    Non-Vehicles have a limited set of Hover options.
+                    // determine what our target height really is based on HoverType
+
+                    m_groundHeight = _parent_scene.GetTerrainHeightAtXY(lpos.X, lpos.Y);
+
+                    switch (m_PIDHoverType)
+                    {
+                        case PIDHoverType.Ground:
+                            m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
+                            break;
+
+                        case PIDHoverType.GroundAndWater:
+                            m_waterHeight = _parent_scene.GetWaterLevel();
+                            if (m_groundHeight > m_waterHeight)
+                                m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
+                            else
+                                m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
+                            break;
+                    }     // end switch (m_PIDHoverType)
+
+                    // don't go underground unless volumedetector
+
+                    if (m_targetHoverHeight > m_groundHeight || m_isVolumeDetect)
+                    {
+                        d.Vector3 vel = d.BodyGetLinearVel(Body);
+
+                        fz = (m_targetHoverHeight - lpos.Z);
+
+                        //  if error is zero, use position control; otherwise, velocity control
+                        if (Math.Abs(fz) < 0.01f)
+                        {
+                            d.BodySetPosition(Body, lpos.X, lpos.Y, m_targetHoverHeight);
+                            d.BodySetLinearVel(Body, vel.X, vel.Y, 0);
+                        }
+                        else
+                        {
+                            _zeroFlag = false;
+                            fz /= m_PIDHoverTau;
+
+                            float tmp = Math.Abs(fz);
+                            if (tmp > 50)
+                                fz = 50 * Math.Sign(fz);
+                            else if (tmp < 0.1)
+                                fz = 0.1f * Math.Sign(fz);
+
+                            fz = ((fz - vel.Z) * m_invTimeStep);
+                        }
+                    }
+                }
+                else
+                {
+                    float b = (1.0f - m_buoyancy) * m_gravmod;
+                    fx = _parent_scene.gravityx * b;
+                    fy = _parent_scene.gravityy * b;
+                    fz = _parent_scene.gravityz * b;
+                }
+
+                fx *= mass;
+                fy *= mass;
+                fz *= mass;
+
+                // constant force
+                fx += m_force.X;
+                fy += m_force.Y;
+                fz += m_force.Z;
+
+                fx += m_forceacc.X;
+                fy += m_forceacc.Y;
+                fz += m_forceacc.Z;
+
+                m_forceacc = Vector3.Zero;
+
+                //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString());
+                if (fx != 0 || fy != 0 || fz != 0)
+                {
+                    d.BodyAddForce(Body, fx, fy, fz);
+                    //Console.WriteLine("AddForce " + fx + "," + fy + "," + fz);
+                }
+
+                Vector3 trq;
+
+                trq = m_torque;
+                trq += m_angularForceacc;
+                m_angularForceacc = Vector3.Zero;
+                if (trq.X != 0 || trq.Y != 0 || trq.Z != 0)
+                {
+                    d.BodyAddTorque(Body, trq.X, trq.Y, trq.Z);
+                }
+            }
+            else
+            {    // is not physical, or is not a body or is selected
+                //  _zeroPosition = d.BodyGetPosition(Body);
+                return;
+                //Console.WriteLine("Nothing " +  Name);
+
+            }
+        }
+
+        public void UpdatePositionAndVelocity(int frame)
+        {
+            if (_parent == null && !m_isSelected && !m_disabled && !m_building && !m_outbounds && Body != IntPtr.Zero)
+            {
+                if(m_bodydisablecontrol < 0)
+                    return;
+
+                bool bodyenabled = d.BodyIsEnabled(Body);
+                if (bodyenabled || !_zeroFlag)
+                {
+                    bool lastZeroFlag = _zeroFlag;
+
+                    d.Vector3 lpos = d.GeomGetPosition(prim_geom);
+
+                    // check outside region
+                    if (lpos.Z < -100 || lpos.Z > 100000f)
+                    {
+                        m_outbounds = true;
+
+                        lpos.Z = Util.Clip(lpos.Z, -100f, 100000f);
+                        m_acceleration.X = 0;
+                        m_acceleration.Y = 0;
+                        m_acceleration.Z = 0;
+
+                        _velocity.X = 0;
+                        _velocity.Y = 0;
+                        _velocity.Z = 0;
+                        m_rotationalVelocity.X = 0;
+                        m_rotationalVelocity.Y = 0;
+                        m_rotationalVelocity.Z = 0;
+
+                        d.BodySetLinearVel(Body, 0, 0, 0); // stop it
+                        d.BodySetAngularVel(Body, 0, 0, 0); // stop it
+                        d.BodySetPosition(Body, lpos.X, lpos.Y, lpos.Z); // put it somewhere
+                        m_lastposition = _position;
+                        m_lastorientation = _orientation;
+
+                        base.RequestPhysicsterseUpdate();
+
+//                        throttleCounter = 0;
+                        _zeroFlag = true;
+
+                        disableBodySoft(); // disable it and colisions
+                        base.RaiseOutOfBounds(_position);
+                        return;
+                    }
+
+                    if (lpos.X < 0f)
+                    {
+                        _position.X = Util.Clip(lpos.X, -2f, -0.1f);
+                        m_outbounds = true;
+                    }
+                    else if (lpos.X > _parent_scene.WorldExtents.X)
+                    {
+                        _position.X = Util.Clip(lpos.X, _parent_scene.WorldExtents.X + 0.1f, _parent_scene.WorldExtents.X + 2f);
+                        m_outbounds = true;
+                    }
+                    if (lpos.Y < 0f)
+                    {
+                        _position.Y = Util.Clip(lpos.Y, -2f, -0.1f);
+                        m_outbounds = true;
+                    }
+                    else if (lpos.Y > _parent_scene.WorldExtents.Y)
+                    {
+                        _position.Y = Util.Clip(lpos.Y, _parent_scene.WorldExtents.Y + 0.1f, _parent_scene.WorldExtents.Y + 2f);
+                        m_outbounds = true;
+                    }
+
+                    if (m_outbounds)
+                    {
+                        m_lastposition = _position;
+                        m_lastorientation = _orientation;
+
+                        d.Vector3 dtmp = d.BodyGetAngularVel(Body);
+                        m_rotationalVelocity.X = dtmp.X;
+                        m_rotationalVelocity.Y = dtmp.Y;
+                        m_rotationalVelocity.Z = dtmp.Z;
+
+                        dtmp = d.BodyGetLinearVel(Body);
+                        _velocity.X = dtmp.X;
+                        _velocity.Y = dtmp.Y;
+                        _velocity.Z = dtmp.Z;
+
+                        d.BodySetLinearVel(Body, 0, 0, 0); // stop it
+                        d.BodySetAngularVel(Body, 0, 0, 0);
+                        d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
+                        disableBodySoft(); // stop collisions
+                        UnSubscribeEvents();
+
+                        base.RequestPhysicsterseUpdate();
+                        return;
+                    }
+
+                    d.Quaternion ori;
+                    d.GeomCopyQuaternion(prim_geom, out ori);
+
+                    // decide if moving
+                    // use positions since this are integrated quantities
+                    // tolerance values depende a lot on simulation noise...
+                    // use simple math.abs since we dont need to be exact
+                    if(!bodyenabled)
+                    {
+                        _zeroFlag = true;
+                    }
+                    else
+                    {
+                        float poserror;
+                        float angerror;
+                        if(_zeroFlag)
+                        {
+                            poserror = 0.01f;
+                            angerror = 0.001f;
+                        }
+                        else
+                        {
+                            poserror = 0.005f;
+                            angerror = 0.0005f;
+                        }
+
+                        if (
+                            (Math.Abs(_position.X - lpos.X) < poserror)
+                            && (Math.Abs(_position.Y - lpos.Y) < poserror)
+                            && (Math.Abs(_position.Z - lpos.Z) < poserror)
+                            && (Math.Abs(_orientation.X - ori.X) < angerror)
+                            && (Math.Abs(_orientation.Y - ori.Y) < angerror)
+                            && (Math.Abs(_orientation.Z - ori.Z) < angerror)  // ignore W
+                            )
+                            _zeroFlag = true;
+                        else
+                            _zeroFlag = false;
+                    }
+
+                    // update position
+                    if (!(_zeroFlag && lastZeroFlag))
+                    {
+                        _position.X = lpos.X;
+                        _position.Y = lpos.Y;
+                        _position.Z = lpos.Z;
+
+                        _orientation.X = ori.X;
+                        _orientation.Y = ori.Y;
+                        _orientation.Z = ori.Z;
+                        _orientation.W = ori.W;
+                    }
+
+                    // update velocities and acceleration
+                    if (_zeroFlag || lastZeroFlag)
+                    {
+                         // disable interpolators
+                        _velocity = Vector3.Zero;
+                        m_acceleration = Vector3.Zero;
+                        m_rotationalVelocity = Vector3.Zero;
+                    }
+                    else
+                    {
+                        d.Vector3 vel = d.BodyGetLinearVel(Body);
+
+                        m_acceleration = _velocity;
+
+                        if ((Math.Abs(vel.X) < 0.005f) &&
+                            (Math.Abs(vel.Y) < 0.005f) &&
+                            (Math.Abs(vel.Z) < 0.005f))
+                        {
+                            _velocity = Vector3.Zero;
+                            float t = -m_invTimeStep;
+                            m_acceleration = m_acceleration * t;
+                        }
+                        else
+                        {
+                            _velocity.X = vel.X;
+                            _velocity.Y = vel.Y;
+                            _velocity.Z = vel.Z;
+                            m_acceleration = (_velocity - m_acceleration) * m_invTimeStep;
+                        }
+
+                        if ((Math.Abs(m_acceleration.X) < 0.01f) &&
+                            (Math.Abs(m_acceleration.Y) < 0.01f) &&
+                            (Math.Abs(m_acceleration.Z) < 0.01f))
+                        {
+                            m_acceleration = Vector3.Zero;
+                        }
+
+                        vel = d.BodyGetAngularVel(Body);
+                        if ((Math.Abs(vel.X) < 0.0001) &&
+                            (Math.Abs(vel.Y) < 0.0001) &&
+                            (Math.Abs(vel.Z) < 0.0001)
+                            )
+                        {
+                            m_rotationalVelocity = Vector3.Zero;
+                        }
+                        else
+                        {
+                            m_rotationalVelocity.X = vel.X;
+                            m_rotationalVelocity.Y = vel.Y;
+                            m_rotationalVelocity.Z = vel.Z;
+                        }
+                    }
+
+                    if (_zeroFlag)
+                    {
+                        if (!m_lastUpdateSent)
+                        {
+                            base.RequestPhysicsterseUpdate();
+                            if (lastZeroFlag)
+                                m_lastUpdateSent = true;
+                        }
+                        return;
+                    }
+
+                    base.RequestPhysicsterseUpdate();
+                    m_lastUpdateSent = false;
+                }
+            }
+        }
+
+        internal static bool QuaternionIsFinite(Quaternion q)
+        {
+            if (Single.IsNaN(q.X) || Single.IsInfinity(q.X))
+                return false;
+            if (Single.IsNaN(q.Y) || Single.IsInfinity(q.Y))
+                return false;
+            if (Single.IsNaN(q.Z) || Single.IsInfinity(q.Z))
+                return false;
+            if (Single.IsNaN(q.W) || Single.IsInfinity(q.W))
+                return false;
+            return true;
+        }
+
+        internal static void DMassSubPartFromObj(ref d.Mass part, ref d.Mass theobj)
+        {
+            // assumes object center of mass is zero
+            float smass = part.mass;
+            theobj.mass -= smass;
+
+            smass *= 1.0f / (theobj.mass); ;
+
+            theobj.c.X -= part.c.X * smass;
+            theobj.c.Y -= part.c.Y * smass;
+            theobj.c.Z -= part.c.Z * smass;
+
+            theobj.I.M00 -= part.I.M00;
+            theobj.I.M01 -= part.I.M01;
+            theobj.I.M02 -= part.I.M02;
+            theobj.I.M10 -= part.I.M10;
+            theobj.I.M11 -= part.I.M11;
+            theobj.I.M12 -= part.I.M12;
+            theobj.I.M20 -= part.I.M20;
+            theobj.I.M21 -= part.I.M21;
+            theobj.I.M22 -= part.I.M22;
+        }
+
+        private void donullchange()
+        {
+        }
+
+        public bool DoAChange(changes what, object arg)
+        {
+            if (prim_geom == IntPtr.Zero && what != changes.Add && what != changes.AddPhysRep && what != changes.Remove)
+            {
+                return false;
+            }
+
+            // nasty switch
+            switch (what)
+            {
+                case changes.Add:
+                    changeadd();
+                    break;
+
+                case changes.AddPhysRep:
+                    changeAddPhysRep((ODEPhysRepData)arg);
+                    break;
+
+                case changes.Remove:
+                    //If its being removed, we don't want to rebuild the physical rep at all, so ignore this stuff...
+                    //When we return true, it destroys all of the prims in the linkset anyway
+                    if (_parent != null)
+                    {
+                        OdePrim parent = (OdePrim)_parent;
+                        parent.ChildRemove(this, false);
+                    }
+                    else
+                        ChildRemove(this, false);
+
+                    m_vehicle = null;
+                    RemoveGeom();
+                    m_targetSpace = IntPtr.Zero;
+                    UnSubscribeEvents();
+                    return true;
+
+                case changes.Link:
+                    OdePrim tmp = (OdePrim)arg;
+                    changeLink(tmp);
+                    break;
+
+                case changes.DeLink:
+                    changeLink(null);
+                    break;
+
+                case changes.Position:
+                    changePosition((Vector3)arg);
+                    break;
+
+                case changes.Orientation:
+                    changeOrientation((Quaternion)arg);
+                    break;
+
+                case changes.PosOffset:
+                    donullchange();
+                    break;
+
+                case changes.OriOffset:
+                    donullchange();
+                    break;
+
+                case changes.Velocity:
+                    changevelocity((Vector3)arg);
+                    break;
+
+                case changes.TargetVelocity:
+                    break;
+
+//                case changes.Acceleration:
+//                    changeacceleration((Vector3)arg);
+//                    break;
+
+                case changes.AngVelocity:
+                    changeangvelocity((Vector3)arg);
+                    break;
+
+                case changes.Force:
+                    changeForce((Vector3)arg);
+                    break;
+
+                case changes.Torque:
+                    changeSetTorque((Vector3)arg);
+                    break;
+
+                case changes.AddForce:
+                    changeAddForce((Vector3)arg);
+                    break;
+
+                case changes.AddAngForce:
+                    changeAddAngularImpulse((Vector3)arg);
+                    break;
+
+                case changes.AngLock:
+                    changeAngularLock((byte)arg);
+                    break;
+
+                case changes.Size:
+                    changeSize((Vector3)arg);
+                    break;
+
+                case changes.Shape:
+                    changeShape((PrimitiveBaseShape)arg);
+                    break;
+
+                case changes.PhysRepData:
+                    changePhysRepData((ODEPhysRepData) arg);
+                    break;
+
+                case changes.CollidesWater:
+                    changeFloatOnWater((bool)arg);
+                    break;
+
+                case changes.VolumeDtc:
+                    changeVolumedetetion((bool)arg);
+                    break;
+
+                case changes.Phantom:
+                    changePhantomStatus((bool)arg);
+                    break;
+
+                case changes.Physical:
+                    changePhysicsStatus((bool)arg);
+                    break;
+
+                case changes.Selected:
+                    changeSelectedStatus((bool)arg);
+                    break;
+
+                case changes.disabled:
+                    changeDisable((bool)arg);
+                    break;
+
+                case changes.building:
+                    changeBuilding((bool)arg);
+                    break;
+
+                case changes.VehicleType:
+                    changeVehicleType((int)arg);
+                    break;
+
+                case changes.VehicleFlags:
+                    changeVehicleFlags((strVehicleBoolParam) arg);
+                    break;
+
+                case changes.VehicleFloatParam:
+                    changeVehicleFloatParam((strVehicleFloatParam) arg);
+                    break;
+
+                case changes.VehicleVectorParam:
+                    changeVehicleVectorParam((strVehicleVectorParam) arg);
+                    break;
+
+                case changes.VehicleRotationParam:
+                    changeVehicleRotationParam((strVehicleQuatParam) arg);
+                    break;
+
+                case changes.SetVehicle:
+                    changeSetVehicle((VehicleData) arg);
+                    break;
+
+                case changes.Buoyancy:
+                    changeBuoyancy((float)arg);
+                    break;
+
+                case changes.PIDTarget:
+                    changePIDTarget((Vector3)arg);
+                    break;
+
+                case changes.PIDTau:
+                    changePIDTau((float)arg);
+                    break;
+
+                case changes.PIDActive:
+                    changePIDActive((bool)arg);
+                    break;
+
+                case changes.PIDHoverHeight:
+                    changePIDHoverHeight((float)arg);
+                    break;
+
+                case changes.PIDHoverType:
+                    changePIDHoverType((PIDHoverType)arg);
+                    break;
+
+                case changes.PIDHoverTau:
+                    changePIDHoverTau((float)arg);
+                    break;
+
+                case changes.PIDHoverActive:
+                    changePIDHoverActive((bool)arg);
+                    break;
+
+                case changes.SetInertia:
+                    changeInertia((PhysicsInertiaData) arg);
+                    break;
+
+                case changes.Null:
+                    donullchange();
+                    break;
+
+                default:
+                    donullchange();
+                    break;
+            }
+            return false;
+        }
+
+        public void AddChange(changes what, object arg)
+        {
+            _parent_scene.AddChange((PhysicsActor) this, what, arg);
+        }
+
+        private struct strVehicleBoolParam
+        {
+            public int param;
+            public bool value;
+        }
+
+        private struct strVehicleFloatParam
+        {
+            public int param;
+            public float value;
+        }
+
+        private struct strVehicleQuatParam
+        {
+            public int param;
+            public Quaternion value;
+        }
+
+        private struct strVehicleVectorParam
+        {
+            public int param;
+            public Vector3 value;
+        }
+    }
+}
diff --git a/OpenSim/Region/PhysicsModules/ubOde/ODERayCastRequestManager.cs b/OpenSim/Region/PhysicsModules/ubOde/ODERayCastRequestManager.cs
new file mode 100644
index 0000000..f671722
--- /dev/null
+++ b/OpenSim/Region/PhysicsModules/ubOde/ODERayCastRequestManager.cs
@@ -0,0 +1,689 @@
+/*
+ * Copyright (c) Contributors, http://opensimulator.org/
+ * See CONTRIBUTORS.TXT for a full list of copyright holders.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the OpenSimulator Project nor the
+ *       names of its contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+using System;
+using System.Collections.Generic;
+using System.Reflection;
+using System.Runtime.InteropServices;
+using System.Text;
+using OpenSim.Framework;
+using OpenSim.Region.PhysicsModules.SharedBase;
+using OdeAPI;
+using log4net;
+using OpenMetaverse;
+
+namespace OpenSim.Region.PhysicsModule.ubOde
+{
+    /// <summary>
+    /// Processes raycast requests as ODE is in a state to be able to do them.
+    /// This ensures that it's thread safe and there will be no conflicts.
+    /// Requests get returned by a different thread then they were requested by.
+    /// </summary>
+    public class ODERayCastRequestManager
+    {
+        /// <summary>
+        /// Pending ray requests
+        /// </summary>
+        protected OpenSim.Framework.LocklessQueue<ODERayRequest> m_PendingRequests = new OpenSim.Framework.LocklessQueue<ODERayRequest>();
+
+        /// <summary>
+        /// Scene that created this object.
+        /// </summary>
+        private ODEScene m_scene;
+
+        IntPtr ray; // the ray. we only need one for our lifetime
+        IntPtr Sphere;
+        IntPtr Box;
+        IntPtr Plane;
+
+        private int CollisionContactGeomsPerTest = 25;
+        private const int DefaultMaxCount = 25;
+        private const int MaxTimePerCallMS = 30;
+
+        /// <summary>
+        /// ODE near callback delegate
+        /// </summary>
+        private d.NearCallback nearCallback;
+        private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
+        private List<ContactResult> m_contactResults = new List<ContactResult>();
+        private RayFilterFlags CurrentRayFilter;
+        private int CurrentMaxCount;
+
+        public ODERayCastRequestManager(ODEScene pScene)
+        {
+            m_scene = pScene;
+            nearCallback = near;
+            ray = d.CreateRay(IntPtr.Zero, 1.0f);
+            d.GeomSetCategoryBits(ray, 0);
+            Box = d.CreateBox(IntPtr.Zero, 1.0f, 1.0f, 1.0f);
+            d.GeomSetCategoryBits(Box, 0);
+            Sphere = d.CreateSphere(IntPtr.Zero,1.0f);
+            d.GeomSetCategoryBits(Sphere, 0);
+            Plane = d.CreatePlane(IntPtr.Zero, 0f,0f,1f,1f);
+            d.GeomSetCategoryBits(Sphere, 0);
+        }
+
+        public void QueueRequest(ODERayRequest req)
+        {
+            if (req.Count == 0)
+                req.Count = DefaultMaxCount;
+
+            m_PendingRequests.Enqueue(req);
+        }
+
+        /// <summary>
+        /// Process all queued raycast requests
+        /// </summary>
+        /// <returns>Time in MS the raycasts took to process.</returns>
+        public int ProcessQueuedRequests()
+        {
+
+            if (m_PendingRequests.Count <= 0)
+                return 0;
+
+            if (m_scene.ContactgeomsArray == IntPtr.Zero || ray == IntPtr.Zero)
+                // oops something got wrong or scene isn't ready still
+            {
+                m_PendingRequests.Clear();
+                return 0;
+            }
+
+            int time = Util.EnvironmentTickCount();
+
+            ODERayRequest req;
+            int closestHit;
+            int backfacecull;
+            CollisionCategories catflags;
+
+            while (m_PendingRequests.Dequeue(out req))
+            {
+                if (req.callbackMethod != null)
+                {
+                    IntPtr geom = IntPtr.Zero;
+                    if (req.actor != null)
+                    {
+                        if (m_scene.haveActor(req.actor))
+                        {
+                            if (req.actor is OdePrim)
+                                geom = ((OdePrim)req.actor).prim_geom;
+                            else if (req.actor is OdeCharacter)
+                                geom = ((OdePrim)req.actor).prim_geom;
+                        }
+                        if (geom == IntPtr.Zero)
+                        {
+                            NoContacts(req);
+                            continue;
+                        }
+                    }
+
+                    CurrentRayFilter = req.filter;
+                    CurrentMaxCount = req.Count;
+
+                    CollisionContactGeomsPerTest = req.Count & 0xffff;
+
+                    closestHit = ((CurrentRayFilter & RayFilterFlags.ClosestHit) == 0 ? 0 : 1);
+                    backfacecull = ((CurrentRayFilter & RayFilterFlags.BackFaceCull) == 0 ? 0 : 1);
+
+                    if (req.callbackMethod is ProbeBoxCallback)
+                    {
+                        if (CollisionContactGeomsPerTest > 80)
+                            CollisionContactGeomsPerTest = 80;
+                        d.GeomBoxSetLengths(Box, req.Normal.X, req.Normal.Y, req.Normal.Z);
+                        d.GeomSetPosition(Box, req.Origin.X, req.Origin.Y, req.Origin.Z);
+                        d.Quaternion qtmp;
+                        qtmp.X = req.orientation.X;
+                        qtmp.Y = req.orientation.Y;
+                        qtmp.Z = req.orientation.Z;
+                        qtmp.W = req.orientation.W;
+                        d.GeomSetQuaternion(Box, ref qtmp);
+                    }
+                    else if (req.callbackMethod is ProbeSphereCallback)
+                    {
+                        if (CollisionContactGeomsPerTest > 80)
+                            CollisionContactGeomsPerTest = 80;
+
+                        d.GeomSphereSetRadius(Sphere, req.length);
+                        d.GeomSetPosition(Sphere, req.Origin.X, req.Origin.Y, req.Origin.Z);
+                    }
+                    else if (req.callbackMethod is ProbePlaneCallback)
+                    {
+                        if (CollisionContactGeomsPerTest > 80)
+                            CollisionContactGeomsPerTest = 80;
+
+                        d.GeomPlaneSetParams(Plane, req.Normal.X, req.Normal.Y, req.Normal.Z, req.length);
+                    }
+
+                    else
+                    {
+                        if (CollisionContactGeomsPerTest > 25)
+                            CollisionContactGeomsPerTest = 25;
+
+                        d.GeomRaySetLength(ray, req.length);
+                        d.GeomRaySet(ray, req.Origin.X, req.Origin.Y, req.Origin.Z, req.Normal.X, req.Normal.Y, req.Normal.Z);
+                        d.GeomRaySetParams(ray, 0, backfacecull);
+
+                        if (req.callbackMethod is RaycastCallback)
+                        {
+                            // if we only want one get only one per Collision pair saving memory
+                            CurrentRayFilter |= RayFilterFlags.ClosestHit;
+                            d.GeomRaySetClosestHit(ray, 1);
+                        }
+                        else
+                            d.GeomRaySetClosestHit(ray, closestHit);
+                    }
+
+                    if ((CurrentRayFilter & RayFilterFlags.ContactsUnImportant) != 0)
+                        unchecked
+                        {
+                            CollisionContactGeomsPerTest |= (int)d.CONTACTS_UNIMPORTANT;
+                        }
+
+                    if (geom == IntPtr.Zero)
+                    {
+                        // translate ray filter to Collision flags
+                        catflags = 0;
+                        if ((CurrentRayFilter & RayFilterFlags.volumedtc) != 0)
+                            catflags |= CollisionCategories.VolumeDtc;
+                        if ((CurrentRayFilter & RayFilterFlags.phantom) != 0)
+                            catflags |= CollisionCategories.Phantom;
+                        if ((CurrentRayFilter & RayFilterFlags.agent) != 0)
+                            catflags |= CollisionCategories.Character;
+                        if ((CurrentRayFilter & RayFilterFlags.PrimsNonPhantom) != 0)
+                            catflags |= CollisionCategories.Geom;
+                        if ((CurrentRayFilter & RayFilterFlags.land) != 0)
+                            catflags |= CollisionCategories.Land;
+                        if ((CurrentRayFilter & RayFilterFlags.water) != 0)
+                            catflags |= CollisionCategories.Water;
+
+                        if (catflags != 0)
+                        {
+                            if (req.callbackMethod is ProbeBoxCallback)
+                            {
+                                catflags |= CollisionCategories.Space;
+                                d.GeomSetCollideBits(Box, (uint)catflags);
+                                d.GeomSetCategoryBits(Box, (uint)catflags);
+                                doProbe(req, Box);
+                            }
+                            else if (req.callbackMethod is ProbeSphereCallback)
+                            {
+                                catflags |= CollisionCategories.Space;
+                                d.GeomSetCollideBits(Sphere, (uint)catflags);
+                                d.GeomSetCategoryBits(Sphere, (uint)catflags);
+                                doProbe(req, Sphere);
+                            }
+                            else if (req.callbackMethod is ProbePlaneCallback)
+                            {
+                                catflags |= CollisionCategories.Space;
+                                d.GeomSetCollideBits(Plane, (uint)catflags);
+                                d.GeomSetCategoryBits(Plane, (uint)catflags);
+                                doPlane(req,IntPtr.Zero);
+                            }
+                            else
+                            {
+                                d.GeomSetCollideBits(ray, (uint)catflags);
+                                doSpaceRay(req);
+                            }
+                        }
+                    }
+                    else
+                    {
+                        // if we select a geom don't use filters
+
+                        if (req.callbackMethod is ProbePlaneCallback)
+                        {
+                            d.GeomSetCollideBits(Plane, (uint)CollisionCategories.All);
+                            doPlane(req,geom);
+                        }
+                        else
+                        {
+                            d.GeomSetCollideBits(ray, (uint)CollisionCategories.All);
+                            doGeomRay(req,geom);
+                        }
+                    }
+                }
+
+                if (Util.EnvironmentTickCountSubtract(time) > MaxTimePerCallMS)
+                    break;
+            }
+
+            lock (m_contactResults)
+                m_contactResults.Clear();
+
+            return Util.EnvironmentTickCountSubtract(time);
+        }
+        /// <summary>
+        /// Method that actually initiates the raycast with spaces
+        /// </summary>
+        /// <param name="req"></param>
+        ///
+
+        private void NoContacts(ODERayRequest req)
+        {
+            if (req.callbackMethod is RaycastCallback)
+            {
+                ((RaycastCallback)req.callbackMethod)(false, Vector3.Zero, 0, 0, Vector3.Zero);
+                return;
+            }
+            List<ContactResult> cresult = new List<ContactResult>();
+
+            if (req.callbackMethod is RayCallback)
+                ((RayCallback)req.callbackMethod)(cresult);
+            else if (req.callbackMethod is ProbeBoxCallback)
+                ((ProbeBoxCallback)req.callbackMethod)(cresult);
+            else if (req.callbackMethod is ProbeSphereCallback)
+                ((ProbeSphereCallback)req.callbackMethod)(cresult);
+        }
+
+        private const RayFilterFlags FilterActiveSpace = RayFilterFlags.agent | RayFilterFlags.physical | RayFilterFlags.LSLPhantom;
+//        private const RayFilterFlags FilterStaticSpace = RayFilterFlags.water | RayFilterFlags.land | RayFilterFlags.nonphysical | RayFilterFlags.LSLPhanton;
+        private const RayFilterFlags FilterStaticSpace = RayFilterFlags.water | RayFilterFlags.nonphysical | RayFilterFlags.LSLPhantom;
+
+        private void doSpaceRay(ODERayRequest req)
+        {
+            // Collide tests
+            if ((CurrentRayFilter & FilterActiveSpace) != 0)
+            {
+                d.SpaceCollide2(ray, m_scene.ActiveSpace, IntPtr.Zero, nearCallback);
+                d.SpaceCollide2(ray, m_scene.CharsSpace, IntPtr.Zero, nearCallback);
+            }
+            if ((CurrentRayFilter & FilterStaticSpace) != 0 && (m_contactResults.Count < CurrentMaxCount))
+                d.SpaceCollide2(ray, m_scene.StaticSpace, IntPtr.Zero, nearCallback);
+            if ((CurrentRayFilter & RayFilterFlags.land) != 0 && (m_contactResults.Count < CurrentMaxCount))
+            {
+                // current ode land to ray collisions is very bad
+                // so for now limit its range badly
+                if (req.length > 60.0f)
+                {
+                    Vector3 t = req.Normal * req.length;
+                    float tmp = t.X * t.X + t.Y * t.Y;
+                    if(tmp > 2500)
+                    {
+                        float tmp2 = req.length * req.length - tmp + 2500;
+                        tmp2 = (float)Math.Sqrt(tmp2);
+                        d.GeomRaySetLength(ray, tmp2);
+                    }
+
+                }
+                d.SpaceCollide2(ray, m_scene.GroundSpace, IntPtr.Zero, nearCallback);
+            }
+
+            if (req.callbackMethod is RaycastCallback)
+            {
+                // Define default results
+                bool hitYN = false;
+                uint hitConsumerID = 0;
+                float distance = float.MaxValue;
+                Vector3 closestcontact = Vector3.Zero;
+                Vector3 snormal = Vector3.Zero;
+
+                // Find closest contact and object.
+                lock (m_contactResults)
+                {
+                    foreach (ContactResult cResult in m_contactResults)
+                    {
+                        if(cResult.Depth < distance)
+                        {
+                            closestcontact = cResult.Pos;
+                            hitConsumerID = cResult.ConsumerID;
+                            distance = cResult.Depth;
+                            snormal = cResult.Normal;
+                        }
+                    }
+                    m_contactResults.Clear();
+                }
+
+                if (distance > 0 && distance < float.MaxValue)
+                    hitYN = true;
+                ((RaycastCallback)req.callbackMethod)(hitYN, closestcontact, hitConsumerID, distance, snormal);
+            }
+            else
+            {
+                List<ContactResult> cresult = new List<ContactResult>(m_contactResults.Count);
+                lock (m_PendingRequests)
+                {
+                    cresult.AddRange(m_contactResults);
+                    m_contactResults.Clear();
+                }
+                ((RayCallback)req.callbackMethod)(cresult);
+            }
+        }
+
+        private void doProbe(ODERayRequest req, IntPtr probe)
+        {
+            // Collide tests
+            if ((CurrentRayFilter & FilterActiveSpace) != 0)
+            {
+                d.SpaceCollide2(probe, m_scene.ActiveSpace, IntPtr.Zero, nearCallback);
+                d.SpaceCollide2(probe, m_scene.CharsSpace, IntPtr.Zero, nearCallback);
+            }
+            if ((CurrentRayFilter & FilterStaticSpace) != 0 && (m_contactResults.Count < CurrentMaxCount))
+                d.SpaceCollide2(probe, m_scene.StaticSpace, IntPtr.Zero, nearCallback);
+            if ((CurrentRayFilter & RayFilterFlags.land) != 0 && (m_contactResults.Count < CurrentMaxCount))
+                d.SpaceCollide2(probe, m_scene.GroundSpace, IntPtr.Zero, nearCallback);
+
+            List<ContactResult> cresult = new List<ContactResult>(m_contactResults.Count);
+            lock (m_PendingRequests)
+            {
+                cresult.AddRange(m_contactResults);
+                m_contactResults.Clear();
+            }
+            if (req.callbackMethod is ProbeBoxCallback)
+                ((ProbeBoxCallback)req.callbackMethod)(cresult);
+            else if (req.callbackMethod is ProbeSphereCallback)
+                ((ProbeSphereCallback)req.callbackMethod)(cresult);
+        }
+
+        private void doPlane(ODERayRequest req,IntPtr geom)
+        {
+            // Collide tests
+            if (geom == IntPtr.Zero)
+            {
+                if ((CurrentRayFilter & FilterActiveSpace) != 0)
+                {
+                    d.SpaceCollide2(Plane, m_scene.ActiveSpace, IntPtr.Zero, nearCallback);
+                    d.SpaceCollide2(Plane, m_scene.CharsSpace, IntPtr.Zero, nearCallback);
+                }
+                if ((CurrentRayFilter & FilterStaticSpace) != 0 && (m_contactResults.Count < CurrentMaxCount))
+                    d.SpaceCollide2(Plane, m_scene.StaticSpace, IntPtr.Zero, nearCallback);
+                if ((CurrentRayFilter & RayFilterFlags.land) != 0 && (m_contactResults.Count < CurrentMaxCount))
+                    d.SpaceCollide2(Plane, m_scene.GroundSpace, IntPtr.Zero, nearCallback);
+            }
+            else
+            {
+                d.SpaceCollide2(Plane, geom, IntPtr.Zero, nearCallback);
+            }
+
+            List<ContactResult> cresult = new List<ContactResult>(m_contactResults.Count);
+            lock (m_PendingRequests)
+            {
+                cresult.AddRange(m_contactResults);
+                m_contactResults.Clear();
+            }
+
+            ((ProbePlaneCallback)req.callbackMethod)(cresult);
+        }
+
+        /// <summary>
+        /// Method that actually initiates the raycast with a geom
+        /// </summary>
+        /// <param name="req"></param>
+        private void doGeomRay(ODERayRequest req, IntPtr geom)
+        {
+            // Collide test
+            d.SpaceCollide2(ray, geom, IntPtr.Zero, nearCallback); // still do this to have full AABB pre test
+
+            if (req.callbackMethod is RaycastCallback)
+            {
+                // Define default results
+                bool hitYN = false;
+                uint hitConsumerID = 0;
+                float distance = float.MaxValue;
+                Vector3 closestcontact = Vector3.Zero;
+                Vector3 snormal = Vector3.Zero;
+
+                // Find closest contact and object.
+                lock (m_contactResults)
+                {
+                    foreach (ContactResult cResult in m_contactResults)
+                    {
+                        if(cResult.Depth < distance )
+                        {
+                            closestcontact = cResult.Pos;
+                            hitConsumerID = cResult.ConsumerID;
+                            distance = cResult.Depth;
+                            snormal = cResult.Normal;
+                        }
+                    }
+                    m_contactResults.Clear();
+                }
+
+                if (distance > 0 && distance < float.MaxValue)
+                    hitYN = true;
+
+                ((RaycastCallback)req.callbackMethod)(hitYN, closestcontact, hitConsumerID, distance, snormal);
+            }
+            else
+            {
+                List<ContactResult> cresult = new List<ContactResult>(m_contactResults.Count);
+                lock (m_PendingRequests)
+                {
+                    cresult.AddRange(m_contactResults);
+                    m_contactResults.Clear();
+                }
+                ((RayCallback)req.callbackMethod)(cresult);
+            }
+        }
+
+        private bool GetCurContactGeom(int index, ref d.ContactGeom newcontactgeom)
+        {
+            IntPtr ContactgeomsArray = m_scene.ContactgeomsArray;
+            if (ContactgeomsArray == IntPtr.Zero || index >= CollisionContactGeomsPerTest)
+                return false;
+
+            IntPtr contactptr = new IntPtr(ContactgeomsArray.ToInt64() + (Int64)(index * d.ContactGeom.unmanagedSizeOf));
+            newcontactgeom = (d.ContactGeom)Marshal.PtrToStructure(contactptr, typeof(d.ContactGeom));
+            return true;
+        }
+
+        // This is the standard Near. g1 is the ray
+        private void near(IntPtr space, IntPtr g1, IntPtr g2)
+        {
+             if (g2 == IntPtr.Zero || g1 == g2)
+                return;
+
+             if (m_contactResults.Count >= CurrentMaxCount)
+                return;
+
+            if (d.GeomIsSpace(g2))
+            {
+                try
+                {
+                    d.SpaceCollide2(g1, g2, IntPtr.Zero, nearCallback);
+                }
+                catch (Exception e)
+                {
+                    m_log.WarnFormat("[PHYSICS Ray]: Unable to Space collide test an object: {0}", e.Message);
+                }
+                return;
+            }
+
+            int count = 0;
+            try
+            {
+                count = d.CollidePtr(g1, g2, CollisionContactGeomsPerTest, m_scene.ContactgeomsArray, d.ContactGeom.unmanagedSizeOf);
+            }
+            catch (Exception e)
+            {
+                m_log.WarnFormat("[PHYSICS Ray]: Unable to collide test an object: {0}", e.Message);
+                return;
+            }
+
+            if (count == 0)
+                return;
+/*
+            uint cat1 = d.GeomGetCategoryBits(g1);
+            uint cat2 = d.GeomGetCategoryBits(g2);
+            uint col1 = d.GeomGetCollideBits(g1);
+            uint col2 = d.GeomGetCollideBits(g2);
+*/
+
+            uint ID = 0;
+            PhysicsActor p2 = null;
+
+            m_scene.actor_name_map.TryGetValue(g2, out p2);
+
+            if (p2 == null)
+                return;
+
+            switch (p2.PhysicsActorType)
+            {
+                case (int)ActorTypes.Prim:
+
+                    RayFilterFlags thisFlags;
+
+                    if (p2.IsPhysical)
+                        thisFlags = RayFilterFlags.physical;
+                    else
+                        thisFlags = RayFilterFlags.nonphysical;
+
+                    if (p2.Phantom)
+                        thisFlags |= RayFilterFlags.phantom;
+
+                    if (p2.IsVolumeDtc)
+                        thisFlags |= RayFilterFlags.volumedtc;
+
+                    if ((thisFlags & CurrentRayFilter) == 0)
+                        return;
+
+                    ID = ((OdePrim)p2).LocalID;
+                    break;
+
+                case (int)ActorTypes.Agent:
+
+                    if ((CurrentRayFilter & RayFilterFlags.agent) == 0)
+                        return;
+                    else
+                        ID = ((OdeCharacter)p2).LocalID;
+                    break;
+
+                case (int)ActorTypes.Ground:
+
+                    if ((CurrentRayFilter & RayFilterFlags.land) == 0)
+                        return;
+                    break;
+
+                case (int)ActorTypes.Water:
+
+                    if ((CurrentRayFilter & RayFilterFlags.water) == 0)
+                        return;
+                    break;
+
+                default:
+                    break;
+            }
+
+            d.ContactGeom curcontact = new d.ContactGeom();
+
+            // closestHit for now only works for meshs, so must do it for others
+            if ((CurrentRayFilter & RayFilterFlags.ClosestHit) == 0)
+            {
+                // Loop all contacts, build results.
+                for (int i = 0; i < count; i++)
+                {
+                    if (!GetCurContactGeom(i, ref curcontact))
+                        break;
+
+                    ContactResult collisionresult = new ContactResult();
+                    collisionresult.ConsumerID = ID;
+                    collisionresult.Pos.X = curcontact.pos.X;
+                    collisionresult.Pos.Y = curcontact.pos.Y;
+                    collisionresult.Pos.Z = curcontact.pos.Z;
+                    collisionresult.Depth = curcontact.depth;
+                    collisionresult.Normal.X = curcontact.normal.X;
+                    collisionresult.Normal.Y = curcontact.normal.Y;
+                    collisionresult.Normal.Z = curcontact.normal.Z;
+                    lock (m_contactResults)
+                    {
+                        m_contactResults.Add(collisionresult);
+                        if (m_contactResults.Count >= CurrentMaxCount)
+                            return;
+                    }
+                }
+            }
+            else
+            {
+                // keep only closest contact
+                ContactResult collisionresult = new ContactResult();
+                collisionresult.ConsumerID = ID;
+                collisionresult.Depth = float.MaxValue;
+
+                for (int i = 0; i < count; i++)
+                {
+                    if (!GetCurContactGeom(i, ref curcontact))
+                        break;
+
+                    if (curcontact.depth < collisionresult.Depth)
+                    {
+                        collisionresult.Pos.X = curcontact.pos.X;
+                        collisionresult.Pos.Y = curcontact.pos.Y;
+                        collisionresult.Pos.Z = curcontact.pos.Z;
+                        collisionresult.Depth = curcontact.depth;
+                        collisionresult.Normal.X = curcontact.normal.X;
+                        collisionresult.Normal.Y = curcontact.normal.Y;
+                        collisionresult.Normal.Z = curcontact.normal.Z;
+                    }
+                }
+
+                if (collisionresult.Depth != float.MaxValue)
+                {
+                    lock (m_contactResults)
+                        m_contactResults.Add(collisionresult);
+                }
+            }
+        }
+
+        /// <summary>
+        /// Dereference the creator scene so that it can be garbage collected if needed.
+        /// </summary>
+        internal void Dispose()
+        {
+            m_scene = null;
+            if (ray != IntPtr.Zero)
+            {
+                d.GeomDestroy(ray);
+                ray = IntPtr.Zero;
+            }
+            if (Box != IntPtr.Zero)
+            {
+                d.GeomDestroy(Box);
+                Box = IntPtr.Zero;
+            }
+            if (Sphere != IntPtr.Zero)
+            {
+                d.GeomDestroy(Sphere);
+                Sphere = IntPtr.Zero;
+            }
+            if (Plane != IntPtr.Zero)
+            {
+                d.GeomDestroy(Plane);
+                Plane = IntPtr.Zero;
+            }
+        }
+    }
+
+    public struct ODERayRequest
+    {
+        public PhysicsActor actor;
+        public Vector3 Origin;
+        public Vector3 Normal;
+        public int Count;
+        public float length;
+        public object callbackMethod;
+        public RayFilterFlags filter;
+        public Quaternion orientation;
+    }
+}
diff --git a/OpenSim/Region/PhysicsModules/ubOde/ODEScene.cs b/OpenSim/Region/PhysicsModules/ubOde/ODEScene.cs
new file mode 100644
index 0000000..0b51820
--- /dev/null
+++ b/OpenSim/Region/PhysicsModules/ubOde/ODEScene.cs
@@ -0,0 +1,2824 @@
+/*
+ * Copyright (c) Contributors, http://opensimulator.org/
+ * See CONTRIBUTORS.TXT for a full list of copyright holders.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the OpenSimulator Project nor the
+ *       names of its contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+// Revision 2011/12/13 by Ubit Umarov
+//#define SPAM
+
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Reflection;
+using System.Runtime.InteropServices;
+using System.Threading;
+using System.IO;
+using System.Diagnostics;
+using log4net;
+using Nini.Config;
+using Mono.Addins;
+using OdeAPI;
+using OpenSim.Framework;
+using OpenSim.Region.Framework.Scenes;
+using OpenSim.Region.Framework.Interfaces;
+using OpenSim.Region.PhysicsModules.SharedBase;
+using OpenMetaverse;
+
+namespace OpenSim.Region.PhysicsModule.ubOde
+{
+     // colision flags of things others can colide with
+    // rays, sensors, probes removed since can't  be colided with
+    // The top space where things are placed provided further selection
+    // ie physical are in active space nonphysical in static
+    // this should be exclusive as possible
+
+    [Flags]
+    public enum CollisionCategories : uint
+    {
+        Disabled = 0,
+        //by 'things' types
+        Space =         0x01,
+        Geom =          0x02, // aka prim/part
+        Character =     0x04,
+        Land =          0x08,
+        Water =         0x010,
+
+        // by state
+        Phantom =       0x01000,
+        VolumeDtc =     0x02000,
+        Selected =      0x04000,
+        NoShape =       0x08000,
+
+
+        All =           0xffffffff
+    }
+
+    /// <summary>
+    /// Material type for a primitive
+    /// </summary>
+    public enum Material : int
+    {
+        /// <summary></summary>
+        Stone = 0,
+        /// <summary></summary>
+        Metal = 1,
+        /// <summary></summary>
+        Glass = 2,
+        /// <summary></summary>
+        Wood = 3,
+        /// <summary></summary>
+        Flesh = 4,
+        /// <summary></summary>
+        Plastic = 5,
+        /// <summary></summary>
+        Rubber = 6,
+
+        light = 7 // compatibility with old viewers
+    }
+
+    public enum changes : int
+    {
+        Add = 0,                // arg null. finishs the prim creation. should be used internally only ( to remove later ?)
+        Remove,
+        Link,               // arg AuroraODEPrim new parent prim or null to delink. Makes the prim part of a object with prim parent as root
+        //  or removes from a object if arg is null
+        DeLink,
+        Position,           // arg Vector3 new position in world coords. Changes prim position. Prim must know if it is root or child
+        Orientation,        // arg Quaternion new orientation in world coords. Changes prim position. Prim must know it it is root or child
+        PosOffset,          // not in use
+        // arg Vector3 new position in local coords. Changes prim position in object
+        OriOffset,          // not in use
+        // arg Vector3 new position in local coords. Changes prim position in object
+        Velocity,
+        TargetVelocity,
+        AngVelocity,
+        Acceleration,
+        Force,
+        Torque,
+        Momentum,
+
+        AddForce,
+        AddAngForce,
+        AngLock,
+
+        Buoyancy,
+
+        PIDTarget,
+        PIDTau,
+        PIDActive,
+
+        PIDHoverHeight,
+        PIDHoverType,
+        PIDHoverTau,
+        PIDHoverActive,
+
+        Size,
+        AvatarSize,
+        Shape,
+        PhysRepData,
+        AddPhysRep,
+
+        CollidesWater,
+        VolumeDtc,
+
+        Physical,
+        Phantom,
+        Selected,
+        disabled,
+        building,
+
+        VehicleType,
+        VehicleFloatParam,
+        VehicleVectorParam,
+        VehicleRotationParam,
+        VehicleFlags,
+        SetVehicle,
+        SetInertia,
+
+        Null             //keep this last used do dim the methods array. does nothing but pulsing the prim
+    }
+
+    public struct ODEchangeitem
+    {
+        public PhysicsActor actor;
+        public OdeCharacter character;
+        public changes what;
+        public Object arg;
+    }
+
+    public class ODEScene : PhysicsScene
+    {
+        private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
+
+        public bool m_OSOdeLib = false;
+        public Scene m_frameWorkScene = null;
+
+//        private int threadid = 0;
+
+//        const d.ContactFlags comumContactFlags = d.ContactFlags.SoftERP | d.ContactFlags.SoftCFM |d.ContactFlags.Approx1 | d.ContactFlags.Bounce;
+
+        const d.ContactFlags comumContactFlags = d.ContactFlags.Bounce | d.ContactFlags.Approx1 | d.ContactFlags.Slip1 | d.ContactFlags.Slip2;
+        const float comumContactERP = 0.75f;
+        const float comumContactCFM = 0.0001f;
+        const float comumContactSLIP = 0f;
+
+        float frictionMovementMult = 0.8f;
+
+        float TerrainBounce = 0.001f;
+        float TerrainFriction = 0.3f;
+
+        public float AvatarFriction = 0;// 0.9f * 0.5f;
+
+        // this netx dimensions are only relevant for terrain partition (mega regions)
+        // WorldExtents below has the simulation dimensions
+        // they should be identical except on mega regions
+        private uint m_regionWidth = Constants.RegionSize;
+        private uint m_regionHeight = Constants.RegionSize;
+
+        public float ODE_STEPSIZE = 0.020f;
+        public float HalfOdeStep = 0.01f;
+        public int odetimestepMS = 20; // rounded
+        private float metersInSpace = 25.6f;
+        private float m_timeDilation = 1.0f;
+
+        private double m_lastframe;
+        private double m_lastMeshExpire;
+
+        public float gravityx = 0f;
+        public float gravityy = 0f;
+        public float gravityz = -9.8f;
+
+        private float waterlevel = 0f;
+        private int framecount = 0;
+
+        private float avDensity = 80f;
+        private float avMovementDivisorWalk = 1.3f;
+        private float avMovementDivisorRun = 0.8f;
+        private float minimumGroundFlightOffset = 3f;
+        public float maximumMassObject = 10000.01f;
+        public float geomDefaultDensity = 10.0f;
+
+        public float maximumAngularVelocity = 12.0f; // default 12rad/s
+        public float maxAngVelocitySQ = 144f;   // squared value
+
+        public float bodyPIDD = 35f;
+        public float bodyPIDG = 25;
+
+        public int bodyFramesAutoDisable = 5;
+
+        private d.NearCallback nearCallback;
+
+        private Dictionary<uint,OdePrim> _prims = new Dictionary<uint,OdePrim>();
+        private HashSet<OdeCharacter> _characters = new HashSet<OdeCharacter>();
+        private HashSet<OdePrim> _activeprims = new HashSet<OdePrim>();
+        private HashSet<OdePrim> _activegroups = new HashSet<OdePrim>();
+
+        public OpenSim.Framework.LocklessQueue<ODEchangeitem> ChangesQueue = new OpenSim.Framework.LocklessQueue<ODEchangeitem>();
+
+        /// <summary>
+        /// A list of actors that should receive collision events.
+        /// </summary>
+        private List<PhysicsActor> _collisionEventPrim = new List<PhysicsActor>();
+        private List<PhysicsActor> _collisionEventPrimRemove = new List<PhysicsActor>();
+
+        private HashSet<OdeCharacter> _badCharacter = new HashSet<OdeCharacter>();
+        public Dictionary<IntPtr, PhysicsActor> actor_name_map = new Dictionary<IntPtr, PhysicsActor>();
+
+        private float contactsurfacelayer = 0.002f;
+
+        private int contactsPerCollision = 80;
+        internal IntPtr ContactgeomsArray = IntPtr.Zero;
+        private IntPtr GlobalContactsArray = IntPtr.Zero;
+        private d.Contact SharedTmpcontact = new d.Contact();
+
+        const int maxContactsbeforedeath = 6000;
+        private volatile int m_global_contactcount = 0;
+
+        private IntPtr contactgroup;
+
+        public ContactData[] m_materialContactsData = new ContactData[8];
+
+        private IntPtr TerrainGeom;
+        private float[] TerrainHeightFieldHeight;
+        private GCHandle TerrainHeightFieldHeightsHandler = new GCHandle();
+
+        private int m_physicsiterations = 15;
+        private const float m_SkipFramesAtms = 0.40f; // Drop frames gracefully at a 400 ms lag
+//        private PhysicsActor PANull = new NullPhysicsActor();
+        private float step_time = 0.0f;
+
+        public IntPtr world;
+
+        // split the spaces acording to contents type
+        // ActiveSpace contains characters and active prims
+        // StaticSpace contains land and other that is mostly static in enviroment
+        // this can contain subspaces, like the grid in staticspace
+        // as now space only contains this 2 top spaces
+
+        public IntPtr TopSpace; // the global space
+        public IntPtr ActiveSpace; // space for active prims
+        public IntPtr CharsSpace; // space for active prims
+        public IntPtr StaticSpace; // space for the static things around
+        public IntPtr GroundSpace; // space for ground
+
+        // some speedup variables
+        private int spaceGridMaxX;
+        private int spaceGridMaxY;
+        private float spacesPerMeterX;
+        private float spacesPerMeterY;
+
+        // split static geometry collision into a grid as before
+        private IntPtr[,] staticPrimspace;
+        private IntPtr[] staticPrimspaceOffRegion;
+
+        public Object OdeLock;
+        public static Object SimulationLock;
+
+        public IMesher mesher;
+
+        public IConfigSource m_config;
+
+        public bool physics_logging = false;
+        public int physics_logging_interval = 0;
+        public bool physics_logging_append_existing_logfile = false;
+
+        public Vector2 WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize);
+
+        private ODERayCastRequestManager m_rayCastManager;
+        public ODEMeshWorker m_meshWorker;
+
+        /* maybe needed if ode uses tls
+                private void checkThread()
+                {
+
+                    int th = Thread.CurrentThread.ManagedThreadId;
+                    if(th != threadid)
+                    {
+                        threadid = th;
+                        d.AllocateODEDataForThread(~0U);
+                    }
+                }
+         */
+
+        IConfig physicsconfig = null;
+
+        public ODEScene(Scene pscene, IConfigSource psourceconfig, string pname, string pversion, bool pOSOdeLib)
+        {
+            OdeLock = new Object();
+
+            EngineType = pname;
+            PhysicsSceneName = EngineType + "/" + pscene.RegionInfo.RegionName;
+            EngineName = pname + " " + pversion;
+            m_config = psourceconfig;
+            m_OSOdeLib = pOSOdeLib;
+
+//            m_OSOdeLib = false; //debug
+
+            m_frameWorkScene = pscene;
+
+            m_frameWorkScene.RegisterModuleInterface<PhysicsScene>(this);
+
+            Initialization();
+
+            base.Initialise(m_frameWorkScene.PhysicsRequestAsset,
+                (m_frameWorkScene.Heightmap != null ? m_frameWorkScene.Heightmap.GetFloatsSerialised() : new float[m_frameWorkScene.RegionInfo.RegionSizeX * m_frameWorkScene.RegionInfo.RegionSizeY]),
+                (float)m_frameWorkScene.RegionInfo.RegionSettings.WaterHeight);
+        }
+
+        public void RegionLoaded()
+        {
+            mesher = m_frameWorkScene.RequestModuleInterface<IMesher>();
+            if (mesher == null)
+            {
+                m_log.ErrorFormat("[ubOde] No mesher. module disabled");
+                return;
+            }
+
+            m_meshWorker = new ODEMeshWorker(this, m_log, mesher, physicsconfig);
+            m_frameWorkScene.PhysicsEnabled = true;
+        }
+        /// <summary>
+        /// Initiailizes the scene
+        /// Sets many properties that ODE requires to be stable
+        /// These settings need to be tweaked 'exactly' right or weird stuff happens.
+        /// </summary>
+        private void Initialization()
+        {
+            d.AllocateODEDataForThread(~0U);
+
+            SimulationLock = new Object();
+
+            nearCallback = near;
+
+            m_rayCastManager = new ODERayCastRequestManager(this);
+
+            WorldExtents.X = m_frameWorkScene.RegionInfo.RegionSizeX;
+            m_regionWidth = (uint)WorldExtents.X;
+            WorldExtents.Y = m_frameWorkScene.RegionInfo.RegionSizeY;
+            m_regionHeight = (uint)WorldExtents.Y;
+
+            lock (OdeLock)
+            {
+                // Create the world and the first space
+                try
+                {
+                    world = d.WorldCreate();
+                    TopSpace = d.HashSpaceCreate(IntPtr.Zero);
+
+                    // now the major subspaces
+                    ActiveSpace = d.HashSpaceCreate(TopSpace);
+                    CharsSpace = d.HashSpaceCreate(TopSpace);
+                    StaticSpace = d.HashSpaceCreate(TopSpace);
+                    GroundSpace = d.HashSpaceCreate(TopSpace);
+                }
+                catch
+                {
+                    // i must RtC#FM
+                    // i did!
+                }
+
+                d.HashSpaceSetLevels(TopSpace, -5, 12);
+                d.HashSpaceSetLevels(ActiveSpace, -5, 10);
+                d.HashSpaceSetLevels(CharsSpace, -4, 3);
+                d.HashSpaceSetLevels(StaticSpace, -5, 12);
+                d.HashSpaceSetLevels(GroundSpace, 0, 8);
+
+                // demote to second level
+                d.SpaceSetSublevel(ActiveSpace, 1);
+                d.SpaceSetSublevel(CharsSpace, 1);
+                d.SpaceSetSublevel(StaticSpace, 1);
+                d.SpaceSetSublevel(GroundSpace, 1);
+
+                d.GeomSetCategoryBits(ActiveSpace, (uint)(CollisionCategories.Space |
+                                                        CollisionCategories.Geom |
+                                                        CollisionCategories.Character |
+                                                        CollisionCategories.Phantom |
+                                                        CollisionCategories.VolumeDtc
+                                                        ));
+                d.GeomSetCollideBits(ActiveSpace, (uint)(CollisionCategories.Space |
+                                                        CollisionCategories.Geom |
+                                                        CollisionCategories.Character |
+                                                        CollisionCategories.Phantom |
+                                                        CollisionCategories.VolumeDtc
+                                                        ));
+                d.GeomSetCategoryBits(CharsSpace, (uint)(CollisionCategories.Space |
+                                        CollisionCategories.Geom |
+                                        CollisionCategories.Character |
+                                        CollisionCategories.Phantom |
+                                        CollisionCategories.VolumeDtc
+                                        ));
+                d.GeomSetCollideBits(CharsSpace, 0);
+
+                d.GeomSetCategoryBits(StaticSpace, (uint)(CollisionCategories.Space |
+                                                        CollisionCategories.Geom |
+                                                        //                                                        CollisionCategories.Land |
+                                                        //                                                        CollisionCategories.Water |
+                                                        CollisionCategories.Phantom |
+                                                        CollisionCategories.VolumeDtc
+                                                        ));
+                d.GeomSetCollideBits(StaticSpace, 0);
+
+                d.GeomSetCategoryBits(GroundSpace, (uint)(CollisionCategories.Land));
+                d.GeomSetCollideBits(GroundSpace, 0);
+
+                contactgroup = d.JointGroupCreate(maxContactsbeforedeath + 1);
+                //contactgroup
+
+                d.WorldSetAutoDisableFlag(world, false);
+            }
+
+
+            //  checkThread();
+
+
+            // Defaults
+
+            int contactsPerCollision = 80;
+
+            physicsconfig = null;
+
+            if (m_config != null)
+            {
+                physicsconfig = m_config.Configs["ODEPhysicsSettings"];
+                if (physicsconfig != null)
+                {
+                    gravityx = physicsconfig.GetFloat("world_gravityx", gravityx);
+                    gravityy = physicsconfig.GetFloat("world_gravityy", gravityy);
+                    gravityz = physicsconfig.GetFloat("world_gravityz", gravityz);
+
+                    metersInSpace = physicsconfig.GetFloat("meters_in_small_space", metersInSpace);
+
+                    //                    contactsurfacelayer = physicsconfig.GetFloat("world_contact_surface_layer", contactsurfacelayer);
+
+                    ODE_STEPSIZE = physicsconfig.GetFloat("world_stepsize", ODE_STEPSIZE);
+
+                    avDensity = physicsconfig.GetFloat("av_density", avDensity);
+                    avMovementDivisorWalk = physicsconfig.GetFloat("av_movement_divisor_walk", avMovementDivisorWalk);
+                    avMovementDivisorRun = physicsconfig.GetFloat("av_movement_divisor_run", avMovementDivisorRun);
+
+                    contactsPerCollision = physicsconfig.GetInt("contacts_per_collision", contactsPerCollision);
+
+                    geomDefaultDensity = physicsconfig.GetFloat("geometry_default_density", geomDefaultDensity);
+//                    bodyFramesAutoDisable = physicsconfig.GetInt("body_frames_auto_disable", bodyFramesAutoDisable);
+
+                    physics_logging = physicsconfig.GetBoolean("physics_logging", false);
+                    physics_logging_interval = physicsconfig.GetInt("physics_logging_interval", 0);
+                    physics_logging_append_existing_logfile = physicsconfig.GetBoolean("physics_logging_append_existing_logfile", false);
+
+                    minimumGroundFlightOffset = physicsconfig.GetFloat("minimum_ground_flight_offset", minimumGroundFlightOffset);
+                    maximumMassObject = physicsconfig.GetFloat("maximum_mass_object", maximumMassObject);
+
+                    avDensity *= 3f / 80f;  // scale other engines density option to this
+                }
+            }
+
+            float heartbeat = 1/m_frameWorkScene.FrameTime;
+            maximumAngularVelocity = 0.49f * heartbeat *(float)Math.PI;
+            maxAngVelocitySQ = maximumAngularVelocity * maximumAngularVelocity;
+
+            d.WorldSetCFM(world, comumContactCFM);
+            d.WorldSetERP(world, comumContactERP);
+
+            d.WorldSetGravity(world, gravityx, gravityy, gravityz);
+
+            d.WorldSetLinearDamping(world, 0.001f);
+            d.WorldSetAngularDamping(world, 0.002f);
+            d.WorldSetAngularDampingThreshold(world, 0f);
+            d.WorldSetLinearDampingThreshold(world, 0f);
+            d.WorldSetMaxAngularSpeed(world, maximumAngularVelocity);
+
+            d.WorldSetQuickStepNumIterations(world, m_physicsiterations);
+
+            d.WorldSetContactSurfaceLayer(world, contactsurfacelayer);
+            d.WorldSetContactMaxCorrectingVel(world, 60.0f);
+
+            HalfOdeStep = ODE_STEPSIZE * 0.5f;
+            odetimestepMS = (int)(1000.0f * ODE_STEPSIZE + 0.5f);
+
+            ContactgeomsArray = Marshal.AllocHGlobal(contactsPerCollision * d.ContactGeom.unmanagedSizeOf);
+            GlobalContactsArray = Marshal.AllocHGlobal((maxContactsbeforedeath + 100) * d.Contact.unmanagedSizeOf);
+
+            SharedTmpcontact.geom.g1 = IntPtr.Zero;
+            SharedTmpcontact.geom.g2 = IntPtr.Zero;
+
+            SharedTmpcontact.geom.side1 = -1;
+            SharedTmpcontact.geom.side2 = -1;
+
+            SharedTmpcontact.surface.mode = comumContactFlags;
+            SharedTmpcontact.surface.mu = 0;
+            SharedTmpcontact.surface.bounce = 0;
+            SharedTmpcontact.surface.bounce_vel = 1.5f;
+            SharedTmpcontact.surface.soft_cfm = comumContactCFM;
+            SharedTmpcontact.surface.soft_erp = comumContactERP;
+            SharedTmpcontact.surface.slip1 = comumContactSLIP;
+            SharedTmpcontact.surface.slip2 = comumContactSLIP;
+
+            m_materialContactsData[(int)Material.Stone].mu = 0.8f;
+            m_materialContactsData[(int)Material.Stone].bounce = 0.4f;
+
+            m_materialContactsData[(int)Material.Metal].mu = 0.3f;
+            m_materialContactsData[(int)Material.Metal].bounce = 0.4f;
+
+            m_materialContactsData[(int)Material.Glass].mu = 0.2f;
+            m_materialContactsData[(int)Material.Glass].bounce = 0.7f;
+
+            m_materialContactsData[(int)Material.Wood].mu = 0.6f;
+            m_materialContactsData[(int)Material.Wood].bounce = 0.5f;
+
+            m_materialContactsData[(int)Material.Flesh].mu = 0.9f;
+            m_materialContactsData[(int)Material.Flesh].bounce = 0.3f;
+
+            m_materialContactsData[(int)Material.Plastic].mu = 0.4f;
+            m_materialContactsData[(int)Material.Plastic].bounce = 0.7f;
+
+            m_materialContactsData[(int)Material.Rubber].mu = 0.9f;
+            m_materialContactsData[(int)Material.Rubber].bounce = 0.95f;
+
+            m_materialContactsData[(int)Material.light].mu = 0.0f;
+            m_materialContactsData[(int)Material.light].bounce = 0.0f;
+
+
+            spacesPerMeterX = 1.0f / metersInSpace;
+            spacesPerMeterY = spacesPerMeterX;
+            spaceGridMaxX = (int)(WorldExtents.X * spacesPerMeterX);
+            spaceGridMaxY = (int)(WorldExtents.Y * spacesPerMeterY);
+
+            if (spaceGridMaxX > 24)
+            {
+                spaceGridMaxX = 24;
+                spacesPerMeterX = spaceGridMaxX / WorldExtents.X;
+            }
+
+            if (spaceGridMaxY > 24)
+            {
+                spaceGridMaxY = 24;
+                spacesPerMeterY = spaceGridMaxY / WorldExtents.Y;
+            }
+
+            staticPrimspace = new IntPtr[spaceGridMaxX, spaceGridMaxY];
+
+            // create all spaces now
+            int i, j;
+            IntPtr newspace;
+
+            for (i = 0; i < spaceGridMaxX; i++)
+                for (j = 0; j < spaceGridMaxY; j++)
+                {
+                    newspace = d.HashSpaceCreate(StaticSpace);
+                    d.GeomSetCategoryBits(newspace, (int)CollisionCategories.Space);
+                    waitForSpaceUnlock(newspace);
+                    d.SpaceSetSublevel(newspace, 2);
+                    d.HashSpaceSetLevels(newspace, -2, 8);
+                    d.GeomSetCategoryBits(newspace, (uint)(CollisionCategories.Space |
+                                        CollisionCategories.Geom |
+                                        CollisionCategories.Land |
+                                        CollisionCategories.Water |
+                                        CollisionCategories.Phantom |
+                                        CollisionCategories.VolumeDtc
+                                        ));
+                    d.GeomSetCollideBits(newspace, 0);
+
+                    staticPrimspace[i, j] = newspace;
+                }
+
+            // let this now be index limit
+            spaceGridMaxX--;
+            spaceGridMaxY--;
+
+            // create 4 off world spaces (x<0,x>max,y<0,y>max)
+            staticPrimspaceOffRegion = new IntPtr[4];
+
+            for (i = 0; i < 4; i++)
+            {
+                newspace = d.HashSpaceCreate(StaticSpace);
+                d.GeomSetCategoryBits(newspace, (int)CollisionCategories.Space);
+                waitForSpaceUnlock(newspace);
+                d.SpaceSetSublevel(newspace, 2);
+                d.HashSpaceSetLevels(newspace, -2, 8);
+                d.GeomSetCategoryBits(newspace, (uint)(CollisionCategories.Space |
+                                    CollisionCategories.Geom |
+                                    CollisionCategories.Land |
+                                    CollisionCategories.Water |
+                                    CollisionCategories.Phantom |
+                                    CollisionCategories.VolumeDtc
+                                    ));
+                d.GeomSetCollideBits(newspace, 0);
+
+                staticPrimspaceOffRegion[i] = newspace;
+            }
+
+            m_lastframe = Util.GetTimeStamp();
+            m_lastMeshExpire = m_lastframe;
+            step_time = -1;
+        }
+
+        internal void waitForSpaceUnlock(IntPtr space)
+        {
+            //if (space != IntPtr.Zero)
+                //while (d.SpaceLockQuery(space)) { } // Wait and do nothing
+        }
+
+        #region Collision Detection
+
+        // sets a global contact for a joint for contactgeom , and base contact description)
+        private IntPtr CreateContacJoint(ref d.ContactGeom contactGeom,bool smooth)
+        {
+            if (m_global_contactcount >= maxContactsbeforedeath)
+                return IntPtr.Zero;
+
+            m_global_contactcount++;
+            if(smooth)
+                SharedTmpcontact.geom.depth = contactGeom.depth * 0.05f;
+            else
+                SharedTmpcontact.geom.depth = contactGeom.depth;
+            SharedTmpcontact.geom.pos = contactGeom.pos;
+            SharedTmpcontact.geom.normal = contactGeom.normal;
+
+            IntPtr contact = new IntPtr(GlobalContactsArray.ToInt64() + (Int64)(m_global_contactcount * d.Contact.unmanagedSizeOf));
+            Marshal.StructureToPtr(SharedTmpcontact, contact, true);
+            return d.JointCreateContactPtr(world, contactgroup, contact);
+        }
+
+        private bool GetCurContactGeom(int index, ref d.ContactGeom newcontactgeom)
+        {
+            if (ContactgeomsArray == IntPtr.Zero || index >= contactsPerCollision)
+                return false;
+
+            IntPtr contactptr = new IntPtr(ContactgeomsArray.ToInt64() + (Int64)(index * d.ContactGeom.unmanagedSizeOf));
+            newcontactgeom = (d.ContactGeom)Marshal.PtrToStructure(contactptr, typeof(d.ContactGeom));
+            return true;
+        }
+
+        /// <summary>
+        /// This is our near callback.  A geometry is near a body
+        /// </summary>
+        /// <param name="space">The space that contains the geoms.  Remember, spaces are also geoms</param>
+        /// <param name="g1">a geometry or space</param>
+        /// <param name="g2">another geometry or space</param>
+        ///
+
+        private void near(IntPtr space, IntPtr g1, IntPtr g2)
+        {
+            //  no lock here!  It's invoked from within Simulate(), which is thread-locked
+
+            if (m_global_contactcount >= maxContactsbeforedeath)
+                return;
+
+            // Test if we're colliding a geom with a space.
+            // If so we have to drill down into the space recursively
+
+            if (g1 == IntPtr.Zero || g2 == IntPtr.Zero)
+                return;
+
+            if (d.GeomIsSpace(g1) || d.GeomIsSpace(g2))
+            {
+                // We'll be calling near recursivly if one
+                // of them is a space to find all of the
+                // contact points in the space
+                try
+                {
+                    d.SpaceCollide2(g1, g2, IntPtr.Zero, nearCallback);
+                }
+                catch (AccessViolationException)
+                {
+                    m_log.Warn("[PHYSICS]: Unable to collide test a space");
+                    return;
+                }
+                //here one should check collisions of geoms inside a space
+                // but on each space we only should have geoms that not colide amoung each other
+                // so we don't dig inside spaces
+                return;
+            }
+
+            // get geom bodies to check if we already a joint contact
+            // guess this shouldn't happen now
+            IntPtr b1 = d.GeomGetBody(g1);
+            IntPtr b2 = d.GeomGetBody(g2);
+
+            // d.GeomClassID id = d.GeomGetClass(g1);
+
+            // Figure out how many contact points we have
+            int count = 0;
+            try
+            {
+                // Colliding Geom To Geom
+                // This portion of the function 'was' blatantly ripped off from BoxStack.cs
+
+                if (g1 == g2)
+                    return; // Can't collide with yourself
+
+//                if (b1 != IntPtr.Zero && b2 != IntPtr.Zero && d.AreConnectedExcluding(b1, b2, d.JointType.Contact))
+//                    return;
+                /*
+                // debug
+                                PhysicsActor dp2;
+                                if (d.GeomGetClass(g1) == d.GeomClassID.HeightfieldClass)
+                                {
+                                    d.AABB aabb;
+                                    d.GeomGetAABB(g2, out aabb);
+                                    float x = aabb.MaxX - aabb.MinX;
+                                    float y = aabb.MaxY - aabb.MinY;
+                                    float z = aabb.MaxZ - aabb.MinZ;
+                                    if (x > 60.0f || y > 60.0f || z > 60.0f)
+                                    {
+                                        if (!actor_name_map.TryGetValue(g2, out dp2))
+                                            m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 2");
+                                        else
+                                            m_log.WarnFormat("[PHYSICS]: land versus large prim geo {0},size {1}, AABBsize <{2},{3},{4}>, at {5} ori {6},({7})",
+                                                dp2.Name, dp2.Size, x, y, z,
+                                                dp2.Position.ToString(),
+                                                dp2.Orientation.ToString(),
+                                                dp2.Orientation.Length());
+                                        return;
+                                    }
+                                }
+                //
+                */
+
+
+                if (d.GeomGetCategoryBits(g1) == (uint)CollisionCategories.VolumeDtc ||
+                    d.GeomGetCategoryBits(g2) == (uint)CollisionCategories.VolumeDtc)
+                {
+                    int cflags;
+                    unchecked
+                    {
+                        cflags = (int)(1 | d.CONTACTS_UNIMPORTANT);
+                    }
+                    count = d.CollidePtr(g1, g2, cflags, ContactgeomsArray, d.ContactGeom.unmanagedSizeOf);
+                }
+                else
+                    count = d.CollidePtr(g1, g2, (contactsPerCollision & 0xffff), ContactgeomsArray, d.ContactGeom.unmanagedSizeOf);
+            }
+            catch (SEHException)
+            {
+                m_log.Error("[PHYSICS]: The Operating system shut down ODE because of corrupt memory.  This could be a result of really irregular terrain.  If this repeats continuously, restart using Basic Physics and terrain fill your terrain.  Restarting the sim.");
+                //                ode.drelease(world);
+                base.TriggerPhysicsBasedRestart();
+            }
+            catch (Exception e)
+            {
+                m_log.WarnFormat("[PHYSICS]: Unable to collide test an object: {0}", e.Message);
+                return;
+            }
+
+            // contacts done
+            if (count == 0)
+                return;
+
+            // try get physical actors
+            PhysicsActor p1;
+            PhysicsActor p2;
+
+            if (!actor_name_map.TryGetValue(g1, out p1))
+            {
+                m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 1");
+                return;
+            }
+
+            if (!actor_name_map.TryGetValue(g2, out p2))
+            {
+                m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 2");
+                return;
+            }
+
+
+            // get first contact
+            d.ContactGeom curContact = new d.ContactGeom();
+
+            if (!GetCurContactGeom(0, ref curContact))
+                return;
+
+            ContactPoint maxDepthContact = new ContactPoint();
+
+            // do volume detection case
+            if ((p1.IsVolumeDtc || p2.IsVolumeDtc))
+            {
+                maxDepthContact = new ContactPoint(
+                    new Vector3(curContact.pos.X, curContact.pos.Y, curContact.pos.Z),
+                    new Vector3(curContact.normal.X, curContact.normal.Y, curContact.normal.Z),
+                    curContact.depth, false
+                    );
+
+                collision_accounting_events(p1, p2, maxDepthContact);
+                return;
+            }
+
+            // big messy collision analises
+
+            float mu = 0;
+            float bounce = 0;
+//            bool IgnoreNegSides = false;
+
+            ContactData contactdata1 = new ContactData(0, 0, false);
+            ContactData contactdata2 = new ContactData(0, 0, false);
+
+            bool dop1ava = false;
+            bool dop2ava = false;
+            bool ignore = false;
+            bool smoothMesh = false;
+
+            switch (p1.PhysicsActorType)
+            {
+                case (int)ActorTypes.Agent:
+                    {
+                        dop1ava = true;
+                        switch (p2.PhysicsActorType)
+                        {
+                            case (int)ActorTypes.Agent:
+                            case (int)ActorTypes.Prim:
+                                break;
+
+                            default:
+                                ignore = true; // avatar to terrain and water ignored
+                                break;
+                        }
+                        break;
+                    }
+
+                case (int)ActorTypes.Prim:
+                    {
+                        switch (p2.PhysicsActorType)
+                        {
+                            case (int)ActorTypes.Agent:
+                                dop2ava = true;
+                                break;
+
+                            case (int)ActorTypes.Prim:
+                                Vector3 relV = p1.rootVelocity - p2.rootVelocity;
+                                float relVlenSQ = relV.LengthSquared();
+                                if (relVlenSQ > 0.0001f)
+                                {
+                                    p1.CollidingObj = true;
+                                    p2.CollidingObj = true;
+                                }
+                                p1.getContactData(ref contactdata1);
+                                p2.getContactData(ref contactdata2);
+                                bounce = contactdata1.bounce * contactdata2.bounce;
+                                mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu);
+
+                                if (relVlenSQ > 0.01f)
+                                    mu *= frictionMovementMult;
+
+                                if(d.GeomGetClass(g2) == d.GeomClassID.TriMeshClass &&
+                                    d.GeomGetClass(g1) == d.GeomClassID.TriMeshClass)
+                                    smoothMesh = true;
+                                break;
+
+                            case (int)ActorTypes.Ground:
+                                p1.getContactData(ref contactdata1);
+                                bounce = contactdata1.bounce * TerrainBounce;
+                                mu = (float)Math.Sqrt(contactdata1.mu * TerrainFriction);
+
+                                Vector3 v1 = p1.rootVelocity;
+                                if (Math.Abs(v1.X) > 0.1f || Math.Abs(v1.Y) > 0.1f)
+                                    mu *= frictionMovementMult;
+                                p1.CollidingGround = true;
+
+                                if(d.GeomGetClass(g1) == d.GeomClassID.TriMeshClass)
+                                    smoothMesh = true;
+                                break;
+
+                            case (int)ActorTypes.Water:
+                            default:
+                                ignore = true;
+                                break;
+                        }
+                    }
+                    break;
+
+                case (int)ActorTypes.Ground:
+                    if (p2.PhysicsActorType == (int)ActorTypes.Prim)
+                    {
+                        p2.CollidingGround = true;
+                        p2.getContactData(ref contactdata2);
+                        bounce = contactdata2.bounce * TerrainBounce;
+                        mu = (float)Math.Sqrt(contactdata2.mu * TerrainFriction);
+
+//                        if (curContact.side1 > 0) // should be 2 ?
+//                            IgnoreNegSides = true;
+                        Vector3 v2 = p2.rootVelocity;
+                        if (Math.Abs(v2.X) > 0.1f || Math.Abs(v2.Y) > 0.1f)
+                            mu *= frictionMovementMult;
+
+                        if(d.GeomGetClass(g2) == d.GeomClassID.TriMeshClass)
+                            smoothMesh = true;
+                    }
+                    else
+                        ignore = true;
+                    break;
+
+                case (int)ActorTypes.Water:
+                default:
+                    break;
+            }
+
+            if (ignore)
+                return;
+
+            IntPtr Joint;
+            bool FeetCollision = false;
+            int ncontacts = 0;
+
+            int i = 0;
+
+            maxDepthContact = new ContactPoint();
+            maxDepthContact.PenetrationDepth = float.MinValue;
+            ContactPoint minDepthContact = new ContactPoint();
+            minDepthContact.PenetrationDepth = float.MaxValue;
+
+            SharedTmpcontact.geom.depth = 0;
+            SharedTmpcontact.surface.mu = mu;
+            SharedTmpcontact.surface.bounce = bounce;
+
+            d.ContactGeom altContact = new d.ContactGeom();
+            bool useAltcontact;
+            bool noskip;
+
+            if(dop1ava || dop2ava)
+                smoothMesh = false;
+
+            while (true)
+            {
+                noskip = true;
+                useAltcontact = false;
+
+                if (dop1ava)
+                {
+                    if ((((OdeCharacter)p1).Collide(g1, g2, false, ref curContact, ref altContact , ref useAltcontact, ref FeetCollision)))
+                    {
+                        if (p2.PhysicsActorType == (int)ActorTypes.Agent)
+                        {
+                            p1.CollidingObj = true;
+                            p2.CollidingObj = true;
+                        }
+                        else if (p2.rootVelocity.LengthSquared() > 0.0f)
+                            p2.CollidingObj = true;
+                    }
+                    else
+                        noskip = false;
+                }
+                else if (dop2ava)
+                {
+                if ((((OdeCharacter)p2).Collide(g2, g1, true, ref curContact, ref altContact , ref useAltcontact, ref FeetCollision)))
+                    {
+                    if (p1.PhysicsActorType == (int)ActorTypes.Agent)
+                        {
+                            p1.CollidingObj = true;
+                            p2.CollidingObj = true;
+                        }
+                    else if (p1.rootVelocity.LengthSquared() > 0.0f)
+                        p1.CollidingObj = true;
+                    }
+                else
+                    noskip = false;
+                }
+
+                if (noskip)
+                {
+                    if(useAltcontact)
+                        Joint = CreateContacJoint(ref altContact,smoothMesh);
+                    else
+                        Joint = CreateContacJoint(ref curContact,smoothMesh);
+                    if (Joint == IntPtr.Zero)
+                        break;
+
+                    d.JointAttach(Joint, b1, b2);
+
+                    ncontacts++;
+
+                    if (curContact.depth > maxDepthContact.PenetrationDepth)
+                    {
+                        maxDepthContact.Position.X = curContact.pos.X;
+                        maxDepthContact.Position.Y = curContact.pos.Y;
+                        maxDepthContact.Position.Z = curContact.pos.Z;
+                        maxDepthContact.PenetrationDepth = curContact.depth;
+                        maxDepthContact.CharacterFeet = FeetCollision;
+                    }
+
+                    if (curContact.depth < minDepthContact.PenetrationDepth)
+                    {
+                        minDepthContact.PenetrationDepth = curContact.depth;
+                        minDepthContact.SurfaceNormal.X = curContact.normal.X;
+                        minDepthContact.SurfaceNormal.Y = curContact.normal.Y;
+                        minDepthContact.SurfaceNormal.Z = curContact.normal.Z;
+                    }
+                }
+
+                if (++i >= count)
+                    break;
+
+                if (!GetCurContactGeom(i, ref curContact))
+                    break;
+            }
+
+            if (ncontacts > 0)
+            {
+                maxDepthContact.SurfaceNormal.X = minDepthContact.SurfaceNormal.X;
+                maxDepthContact.SurfaceNormal.Y = minDepthContact.SurfaceNormal.Y;
+                maxDepthContact.SurfaceNormal.Z = minDepthContact.SurfaceNormal.Z;
+
+                collision_accounting_events(p1, p2, maxDepthContact);
+            }
+        }
+
+        private void collision_accounting_events(PhysicsActor p1, PhysicsActor p2, ContactPoint contact)
+        {
+            uint obj2LocalID = 0;
+
+            // update actors collision score
+            if (p1.CollisionScore < float.MaxValue)
+                p1.CollisionScore += 1.0f;
+            if (p2.CollisionScore < float.MaxValue)
+                p2.CollisionScore += 1.0f;
+
+            bool p1events = p1.SubscribedEvents();
+            bool p2events = p2.SubscribedEvents();
+
+            if (p1.IsVolumeDtc)
+                p2events = false;
+            if (p2.IsVolumeDtc)
+                p1events = false;
+
+            if (!p2events && !p1events)
+                return;
+
+            Vector3 vel = Vector3.Zero;
+            if (p2 != null && p2.IsPhysical)
+                vel = p2.rootVelocity;
+
+            if (p1 != null && p1.IsPhysical)
+                vel -= p1.rootVelocity;
+
+            contact.RelativeSpeed = Vector3.Dot(vel, contact.SurfaceNormal);
+
+            switch ((ActorTypes)p1.PhysicsActorType)
+            {
+                case ActorTypes.Agent:
+                case ActorTypes.Prim:
+                {
+                    switch ((ActorTypes)p2.PhysicsActorType)
+                    {
+                        case ActorTypes.Agent:
+                        case ActorTypes.Prim:
+                            if (p2events)
+                            {
+                                //AddCollisionEventReporting(p2);
+                                p2.AddCollisionEvent(p1.ParentActor.LocalID, contact);
+                            }
+                            else if(p1.IsVolumeDtc)
+                                p2.AddVDTCCollisionEvent(p1.ParentActor.LocalID, contact);
+
+                            obj2LocalID = p2.ParentActor.LocalID;
+                            break;
+
+                        case ActorTypes.Ground:
+                        case ActorTypes.Unknown:
+                        default:
+                            obj2LocalID = 0;
+                            break;
+                    }
+                    if (p1events)
+                    {
+                        contact.SurfaceNormal = -contact.SurfaceNormal;
+                        contact.RelativeSpeed = -contact.RelativeSpeed;
+                        //AddCollisionEventReporting(p1);
+                        p1.AddCollisionEvent(obj2LocalID, contact);
+                    }
+                    else if(p2.IsVolumeDtc)
+                    {
+                        contact.SurfaceNormal = -contact.SurfaceNormal;
+                        contact.RelativeSpeed = -contact.RelativeSpeed;
+                        //AddCollisionEventReporting(p1);
+                        p1.AddVDTCCollisionEvent(obj2LocalID, contact);
+                    }
+                    break;
+                }
+                case ActorTypes.Ground:
+                case ActorTypes.Unknown:
+                default:
+                {
+                    if (p2events && !p2.IsVolumeDtc)
+                    {
+                        //AddCollisionEventReporting(p2);
+                        p2.AddCollisionEvent(0, contact);
+                    }
+                    break;
+                }
+            }
+        }
+
+        /// <summary>
+        /// This is our collision testing routine in ODE
+        /// </summary>
+        /// <param name="timeStep"></param>
+        private void collision_optimized()
+        {
+            lock (_characters)
+                {
+                try
+                {
+                    foreach (OdeCharacter chr in _characters)
+                    {
+                        if (chr == null)
+                            continue;
+
+                        chr.IsColliding = false;
+                        // chr.CollidingGround = false; not done here
+                        chr.CollidingObj = false;
+
+                        if(chr.Body == IntPtr.Zero || chr.collider == IntPtr.Zero )
+                            continue;
+
+                        // do colisions with static space
+                        d.SpaceCollide2(chr.collider, StaticSpace, IntPtr.Zero, nearCallback);
+
+                        // no coll with gnd
+                    }
+                    // chars with chars
+                    d.SpaceCollide(CharsSpace, IntPtr.Zero, nearCallback);
+
+                }
+                catch (AccessViolationException)
+                {
+                    m_log.Warn("[PHYSICS]: Unable to collide Character to static space");
+                }
+
+            }
+
+            lock (_activeprims)
+            {
+                foreach (OdePrim aprim in _activeprims)
+                {
+                    aprim.CollisionScore = 0;
+                    aprim.IsColliding = false;
+                    if(!aprim.m_outbounds && d.BodyIsEnabled(aprim.Body))
+                        aprim.clearSleeperCollisions();
+                }
+            }
+
+            lock (_activegroups)
+            {
+                try
+                {
+                    foreach (OdePrim aprim in _activegroups)
+                    {
+                        if(!aprim.m_outbounds && d.BodyIsEnabled(aprim.Body) &&
+                                aprim.collide_geom != IntPtr.Zero)
+                        {
+                            d.SpaceCollide2(StaticSpace, aprim.collide_geom, IntPtr.Zero, nearCallback);
+                            d.SpaceCollide2(GroundSpace, aprim.collide_geom, IntPtr.Zero, nearCallback);
+                        }
+                    }
+                }
+                catch (Exception e)
+                {
+                    m_log.Warn("[PHYSICS]: Unable to collide Active to Static: " + e.Message);
+                }
+            }
+
+            // colide active amoung them
+            try
+            {
+                d.SpaceCollide(ActiveSpace, IntPtr.Zero, nearCallback);
+            }
+            catch (Exception e)
+            {
+                    m_log.Warn("[PHYSICS]: Unable to collide in Active: " + e.Message);
+            }
+
+            // and with chars
+            try
+            {
+                d.SpaceCollide2(CharsSpace,ActiveSpace, IntPtr.Zero, nearCallback);
+            }
+            catch (Exception e)
+            {
+                    m_log.Warn("[PHYSICS]: Unable to collide Active to Character: " + e.Message);
+            }
+        }
+
+        #endregion
+        /// <summary>
+        /// Add actor to the list that should receive collision events in the simulate loop.
+        /// </summary>
+        /// <param name="obj"></param>
+        public void AddCollisionEventReporting(PhysicsActor obj)
+        {
+            if (!_collisionEventPrim.Contains(obj))
+                _collisionEventPrim.Add(obj);
+        }
+
+        /// <summary>
+        /// Remove actor from the list that should receive collision events in the simulate loop.
+        /// </summary>
+        /// <param name="obj"></param>
+        public void RemoveCollisionEventReporting(PhysicsActor obj)
+        {
+            lock(_collisionEventPrimRemove)
+            {
+               if (_collisionEventPrim.Contains(obj) && !_collisionEventPrimRemove.Contains(obj))
+                    _collisionEventPrimRemove.Add(obj);
+            }
+        }
+
+        public override float TimeDilation
+        {
+            get { return m_timeDilation; }
+        }
+
+        public override bool SupportsNINJAJoints
+        {
+            get { return false; }
+        }
+
+        #region Add/Remove Entities
+
+        public override PhysicsActor AddAvatar(string avName, Vector3 position, Vector3 velocity, Vector3 size, bool isFlying)
+        {
+            return null;
+        }
+
+        public override PhysicsActor AddAvatar(uint localID, string avName, Vector3 position, Vector3 size, float feetOffset, bool isFlying)
+        {
+             OdeCharacter newAv = new OdeCharacter(localID, avName, this, position,
+                size, feetOffset, avDensity, avMovementDivisorWalk, avMovementDivisorRun);
+            newAv.Flying = isFlying;
+            newAv.MinimumGroundFlightOffset = minimumGroundFlightOffset;
+
+            return newAv;
+        }
+
+        public void AddCharacter(OdeCharacter chr)
+        {
+            lock (_characters)
+            {
+                if (!_characters.Contains(chr))
+                {
+                    _characters.Add(chr);
+                    if (chr.bad)
+                        m_log.DebugFormat("[PHYSICS] Added BAD actor {0} to characters list", chr.m_uuid);
+                }
+            }
+        }
+
+        public void RemoveCharacter(OdeCharacter chr)
+        {
+            lock (_characters)
+            {
+                if (_characters.Contains(chr))
+                {
+                    _characters.Remove(chr);
+                }
+            }
+        }
+
+        public void BadCharacter(OdeCharacter chr)
+        {
+            lock (_badCharacter)
+            {
+                if (!_badCharacter.Contains(chr))
+                    _badCharacter.Add(chr);
+            }
+        }
+
+        public override void RemoveAvatar(PhysicsActor actor)
+        {
+            //m_log.Debug("[PHYSICS]:ODELOCK");
+            lock (OdeLock)
+            {
+                d.AllocateODEDataForThread(0);
+                ((OdeCharacter) actor).Destroy();
+            }
+        }
+
+
+        public void addActivePrim(OdePrim activatePrim)
+        {
+            // adds active prim..
+            lock (_activeprims)
+            {
+                if (!_activeprims.Contains(activatePrim))
+                    _activeprims.Add(activatePrim);
+            }
+        }
+
+        public void addActiveGroups(OdePrim activatePrim)
+        {
+            lock (_activegroups)
+            {
+                if (!_activegroups.Contains(activatePrim))
+                    _activegroups.Add(activatePrim);
+            }
+        }
+
+        private PhysicsActor AddPrim(String name, Vector3 position, Vector3 size, Quaternion rotation,
+                                     PrimitiveBaseShape pbs, bool isphysical, bool isPhantom, byte shapeType, uint localID)
+        {
+            OdePrim newPrim;
+            lock (OdeLock)
+            {
+
+                newPrim = new OdePrim(name, this, position, size, rotation, pbs, isphysical, isPhantom, shapeType, localID);
+            }
+            return newPrim;
+        }
+
+        public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
+                                                  Vector3 size, Quaternion rotation, bool isPhysical, bool isPhantom, uint localid)
+        {
+            return AddPrim(primName, position, size, rotation, pbs, isPhysical, isPhantom, 0 , localid);
+        }
+
+
+        public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
+                                                  Vector3 size, Quaternion rotation, bool isPhysical, uint localid)
+        {
+            return AddPrim(primName, position, size, rotation, pbs, isPhysical,false, 0, localid);
+        }
+
+        public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
+                                                  Vector3 size, Quaternion rotation, bool isPhysical, bool isPhantom, byte shapeType, uint localid)
+        {
+            return AddPrim(primName, position, size, rotation, pbs, isPhysical,isPhantom, shapeType, localid);
+        }
+
+        public void remActivePrim(OdePrim deactivatePrim)
+        {
+            lock (_activeprims)
+            {
+                _activeprims.Remove(deactivatePrim);
+            }
+        }
+        public void remActiveGroup(OdePrim deactivatePrim)
+        {
+            lock (_activegroups)
+            {
+                _activegroups.Remove(deactivatePrim);
+            }
+        }
+
+        public override void RemovePrim(PhysicsActor prim)
+        {
+            // As with all ODE physics operations, we don't remove the prim immediately but signal that it should be
+            // removed in the next physics simulate pass.
+            if (prim is OdePrim)
+            {
+//                lock (OdeLock)
+                {
+
+                    OdePrim p = (OdePrim)prim;
+                    p.setPrimForRemoval();
+                }
+            }
+        }
+
+        public void RemovePrimThreadLocked(OdePrim prim)
+        {
+            //Console.WriteLine("RemovePrimThreadLocked " +  prim.m_primName);
+            lock (prim)
+            {
+//                RemoveCollisionEventReporting(prim);
+                lock (_prims)
+                    _prims.Remove(prim.LocalID);
+            }
+
+        }
+
+        public void addToPrims(OdePrim prim)
+        {
+            lock (_prims)
+                _prims[prim.LocalID] = prim;
+        }
+
+        public OdePrim getPrim(uint id)
+        {
+            lock (_prims)
+            {
+                if(_prims.ContainsKey(id))
+                    return _prims[id];
+                else
+                    return null;
+            }
+        }
+
+        public bool havePrim(OdePrim prm)
+        {
+            lock (_prims)
+                return _prims.ContainsKey(prm.LocalID);
+        }
+
+        public void changePrimID(OdePrim prim,uint oldID)
+        {
+            lock (_prims)
+            {
+                if(_prims.ContainsKey(oldID))
+                    _prims.Remove(oldID);
+                _prims[prim.LocalID] = prim;
+            }
+        }
+
+        public bool haveActor(PhysicsActor actor)
+        {
+            if (actor is OdePrim)
+            {
+                lock (_prims)
+                    return _prims.ContainsKey(((OdePrim)actor).LocalID);
+            }
+            else if (actor is OdeCharacter)
+            {
+                lock (_characters)
+                    return _characters.Contains((OdeCharacter)actor);
+            }
+            return false;
+        }
+
+        #endregion
+
+        #region Space Separation Calculation
+
+        /// <summary>
+        /// Called when a static prim moves or becomes static
+        /// Places the prim in a space one the static sub-spaces grid
+        /// </summary>
+        /// <param name="geom">the pointer to the geom that moved</param>
+        /// <param name="pos">the position that the geom moved to</param>
+        /// <param name="currentspace">a pointer to the space it was in before it was moved.</param>
+        /// <returns>a pointer to the new space it's in</returns>
+        public IntPtr MoveGeomToStaticSpace(IntPtr geom, Vector3 pos, IntPtr currentspace)
+        {
+            // moves a prim into another static sub-space or from another space into a static sub-space
+
+            // Called ODEPrim so
+            // it's already in locked space.
+
+            if (geom == IntPtr.Zero) // shouldn't happen
+                return IntPtr.Zero;
+
+            // get the static sub-space for current position
+            IntPtr newspace = calculateSpaceForGeom(pos);
+
+            if (newspace == currentspace) // if we are there all done
+                return newspace;
+
+            // else remove it from its current space
+            if (currentspace != IntPtr.Zero && d.SpaceQuery(currentspace, geom))
+            {
+                if (d.GeomIsSpace(currentspace))
+                {
+                    waitForSpaceUnlock(currentspace);
+                    d.SpaceRemove(currentspace, geom);
+
+                    if (d.SpaceGetSublevel(currentspace) > 2 && d.SpaceGetNumGeoms(currentspace) == 0)
+                    {
+                        d.SpaceDestroy(currentspace);
+                    }
+                }
+                else
+                {
+                    m_log.Info("[Physics]: Invalid or empty Space passed to 'MoveGeomToStaticSpace':" + currentspace +
+                                   " Geom:" + geom);
+                }
+            }
+            else // odd currentspace is null or doesn't contain the geom? lets try the geom ideia of current space
+            {
+                currentspace = d.GeomGetSpace(geom);
+                if (currentspace != IntPtr.Zero)
+                {
+                    if (d.GeomIsSpace(currentspace))
+                    {
+                        waitForSpaceUnlock(currentspace);
+                        d.SpaceRemove(currentspace, geom);
+
+                        if (d.SpaceGetSublevel(currentspace) > 2 && d.SpaceGetNumGeoms(currentspace) == 0)
+                        {
+                            d.SpaceDestroy(currentspace);
+                        }
+
+                    }
+                }
+            }
+
+            // put the geom in the newspace
+            waitForSpaceUnlock(newspace);
+            d.SpaceAdd(newspace, geom);
+
+            // let caller know this newspace
+            return newspace;
+        }
+
+        /// <summary>
+        /// Calculates the space the prim should be in by its position
+        /// </summary>
+        /// <param name="pos"></param>
+        /// <returns>a pointer to the space. This could be a new space or reused space.</returns>
+        public IntPtr calculateSpaceForGeom(Vector3 pos)
+        {
+            int x, y;
+
+            if (pos.X < 0)
+                return staticPrimspaceOffRegion[0];
+
+            if (pos.Y < 0)
+                return staticPrimspaceOffRegion[2];
+
+            x = (int)(pos.X * spacesPerMeterX);
+            if (x > spaceGridMaxX)
+                return staticPrimspaceOffRegion[1];
+
+            y = (int)(pos.Y * spacesPerMeterY);
+            if (y > spaceGridMaxY)
+                return staticPrimspaceOffRegion[3];
+
+            return staticPrimspace[x, y];
+        }
+
+        #endregion
+
+
+        /// <summary>
+        /// Called to queue a change to a actor
+        /// to use in place of old taint mechanism so changes do have a time sequence
+        /// </summary>
+
+        public void AddChange(PhysicsActor actor, changes what, Object arg)
+        {
+            ODEchangeitem item = new ODEchangeitem();
+            item.actor = actor;
+            item.what = what;
+            item.arg = arg;
+            ChangesQueue.Enqueue(item);
+        }
+
+        /// <summary>
+        /// Called after our prim properties are set Scale, position etc.
+        /// We use this event queue like method to keep changes to the physical scene occuring in the threadlocked mutex
+        /// This assures us that we have no race conditions
+        /// </summary>
+        /// <param name="prim"></param>
+        public override void AddPhysicsActorTaint(PhysicsActor prim)
+        {
+        }
+
+        // does all pending changes generated during region load process
+        public override void ProcessPreSimulation()
+        {
+            lock (OdeLock)
+            {
+                if (world == IntPtr.Zero)
+                {
+                    ChangesQueue.Clear();
+                    return;
+                }
+
+                d.AllocateODEDataForThread(~0U);
+
+                ODEchangeitem item;
+
+                int donechanges = 0;
+                if (ChangesQueue.Count > 0)
+                {
+                    m_log.InfoFormat("[ubOde] start processing pending actor operations");
+                    int tstart = Util.EnvironmentTickCount();
+
+                    while (ChangesQueue.Dequeue(out item))
+                    {
+                        if (item.actor != null)
+                        {
+                            try
+                            {
+                                if (item.actor is OdeCharacter)
+                                    ((OdeCharacter)item.actor).DoAChange(item.what, item.arg);
+                                else if (((OdePrim)item.actor).DoAChange(item.what, item.arg))
+                                    RemovePrimThreadLocked((OdePrim)item.actor);
+                            }
+                            catch
+                            {
+                                m_log.WarnFormat("[PHYSICS]: Operation failed for a actor {0} {1}",
+                                    item.actor.Name, item.what.ToString());
+                            }
+                        }
+                        donechanges++;
+                    }
+                    int time = Util.EnvironmentTickCountSubtract(tstart);
+                    m_log.InfoFormat("[ubOde] finished {0} operations in {1}ms", donechanges, time);
+                }
+                m_log.InfoFormat("[ubOde] {0} prim actors loaded",_prims.Count);
+            }
+            m_lastframe = Util.GetTimeStamp() + 0.5;
+            step_time = -0.5f;
+        }
+
+        /// <summary>
+        /// This is our main simulate loop
+        /// It's thread locked by a Mutex in the scene.
+        /// It holds Collisions, it instructs ODE to step through the physical reactions
+        /// It moves the objects around in memory
+        /// It calls the methods that report back to the object owners.. (scenepresence, SceneObjectGroup)
+        /// </summary>
+        /// <param name="timeStep"></param>
+        /// <returns></returns>
+        public override float Simulate(float reqTimeStep)
+        {
+            double now = Util.GetTimeStamp();
+            double timeStep = now - m_lastframe;
+            m_lastframe = now;
+
+            // acumulate time so we can reduce error
+            step_time += (float)timeStep;
+
+            if (step_time < HalfOdeStep)
+                return 0;
+
+            if (framecount < 0)
+                framecount = 0;
+
+            framecount++;
+
+//            checkThread();
+            int nodeframes = 0;
+            float fps = 0;
+
+            lock (SimulationLock)
+                lock(OdeLock)
+            {
+                if (world == IntPtr.Zero)
+                {
+                    ChangesQueue.Clear();
+                    return 0;
+                }
+
+                ODEchangeitem item;
+
+//                d.WorldSetQuickStepNumIterations(world, curphysiteractions);
+
+                double loopstartMS = Util.GetTimeStampMS();
+                double looptimeMS = 0;
+                double changestimeMS = 0;
+                double maxChangestime = (int)(reqTimeStep * 500f); // half the time
+                double maxLoopTime = (int)(reqTimeStep * 1200f); // 1.2 the time
+
+//                double collisionTime = 0;
+//                double qstepTIme = 0;
+//                double tmpTime = 0;
+
+                d.AllocateODEDataForThread(~0U);
+
+                if (ChangesQueue.Count > 0)
+                {
+                    while (ChangesQueue.Dequeue(out item))
+                    {
+                        if (item.actor != null)
+                        {
+                            try
+                            {
+                                if (item.actor is OdeCharacter)
+                                    ((OdeCharacter)item.actor).DoAChange(item.what, item.arg);
+                                else if (((OdePrim)item.actor).DoAChange(item.what, item.arg))
+                                    RemovePrimThreadLocked((OdePrim)item.actor);
+                            }
+                            catch
+                            {
+                                m_log.WarnFormat("[PHYSICS]: doChange failed for a actor {0} {1}",
+                                    item.actor.Name, item.what.ToString());
+                            }
+                        }
+                        changestimeMS = Util.GetTimeStampMS() - loopstartMS;
+                        if (changestimeMS > maxChangestime)
+                            break;
+                    }
+                }
+
+                // do simulation taking at most 150ms total time including changes
+                while (step_time > HalfOdeStep)
+                {
+                    try
+                    {
+                        // clear pointer/counter to contacts to pass into joints
+                        m_global_contactcount = 0;
+
+
+                        // Move characters
+                        lock (_characters)
+                        {
+                            List<OdeCharacter> defects = new List<OdeCharacter>();
+                            foreach (OdeCharacter actor in _characters)
+                            {
+                                if (actor != null)
+                                    actor.Move(defects);
+                            }
+                            if (defects.Count != 0)
+                            {
+                                foreach (OdeCharacter defect in defects)
+                                {
+                                    RemoveCharacter(defect);
+                                }
+                                defects.Clear();
+                            }
+                        }
+
+                        // Move other active objects
+                        lock (_activegroups)
+                        {
+                            foreach (OdePrim aprim in _activegroups)
+                            {
+                                aprim.Move();
+                            }
+                        }
+
+                        m_rayCastManager.ProcessQueuedRequests();
+
+//                        tmpTime =  Util.GetTimeStampMS();
+                        collision_optimized();
+//                        collisionTime += Util.GetTimeStampMS() - tmpTime;
+
+                        lock(_collisionEventPrimRemove)
+                        {
+                            foreach (PhysicsActor obj in _collisionEventPrimRemove)
+                                _collisionEventPrim.Remove(obj);
+
+                            _collisionEventPrimRemove.Clear();
+                        }
+
+                        List<OdePrim> sleepers = new List<OdePrim>();
+                        foreach (PhysicsActor obj in _collisionEventPrim)
+                        {
+                            if (obj == null)
+                                continue;
+
+                            switch ((ActorTypes)obj.PhysicsActorType)
+                            {
+                                case ActorTypes.Agent:
+                                    OdeCharacter cobj = (OdeCharacter)obj;
+                                    cobj.SendCollisions((int)(odetimestepMS));
+                                    break;
+
+                                case ActorTypes.Prim:
+                                    OdePrim pobj = (OdePrim)obj;
+                                    if (!pobj.m_outbounds)
+                                    {
+                                        pobj.SendCollisions((int)(odetimestepMS));
+                                        if(pobj.Body != IntPtr.Zero && !pobj.m_isSelected &&
+                                            !pobj.m_disabled && !pobj.m_building &&
+                                            !d.BodyIsEnabled(pobj.Body))
+                                        sleepers.Add(pobj);
+                                    }
+                                    break;
+                            }
+                        }
+
+                        foreach(OdePrim prm in sleepers)
+                            prm.SleeperAddCollisionEvents();
+                        sleepers.Clear();
+ 
+                        // do a ode simulation step
+//                        tmpTime =  Util.GetTimeStampMS();
+                        d.WorldQuickStep(world, ODE_STEPSIZE);
+                        d.JointGroupEmpty(contactgroup);
+//                        qstepTIme += Util.GetTimeStampMS() - tmpTime;
+
+                        // update managed ideia of physical data and do updates to core
+        /*
+                        lock (_characters)
+                        {
+                            foreach (OdeCharacter actor in _characters)
+                            {
+                                if (actor != null)
+                                {
+                                    if (actor.bad)
+                                        m_log.WarnFormat("[PHYSICS]: BAD Actor {0} in _characters list was not removed?", actor.m_uuid);
+
+                                    actor.UpdatePositionAndVelocity();
+                                }
+                            }
+                        }
+        */
+
+                        lock (_activegroups)
+                        {
+                            {
+                                foreach (OdePrim actor in _activegroups)
+                                {
+                                    if (actor.IsPhysical)
+                                    {
+                                        actor.UpdatePositionAndVelocity(framecount);
+                                    }
+                                }
+                            }
+                        }
+                    }
+                    catch (Exception e)
+                    {
+                        m_log.ErrorFormat("[PHYSICS]: {0}, {1}, {2}", e.Message, e.TargetSite, e);
+//                        ode.dunlock(world);
+                    }
+
+                    step_time -= ODE_STEPSIZE;
+                    nodeframes++;
+
+                    looptimeMS = Util.GetTimeStampMS() - loopstartMS;
+                    if (looptimeMS > maxLoopTime)
+                        break;
+                }
+
+                lock (_badCharacter)
+                {
+                    if (_badCharacter.Count > 0)
+                    {
+                        foreach (OdeCharacter chr in _badCharacter)
+                        {
+                            RemoveCharacter(chr);
+                        }
+
+                        _badCharacter.Clear();
+                    }
+                }
+
+// information block for in debug breakpoint only
+/*
+                int ntopactivegeoms = d.SpaceGetNumGeoms(ActiveSpace);
+                int ntopstaticgeoms = d.SpaceGetNumGeoms(StaticSpace);
+                int ngroundgeoms = d.SpaceGetNumGeoms(GroundSpace);
+
+                int nactivegeoms = 0;
+                int nactivespaces = 0;
+
+                int nstaticgeoms = 0;
+                int nstaticspaces = 0;
+                IntPtr sp;
+
+                for (int i = 0; i < ntopactivegeoms; i++)
+                {
+                    sp = d.SpaceGetGeom(ActiveSpace, i);
+                    if (d.GeomIsSpace(sp))
+                    {
+                        nactivespaces++;
+                        nactivegeoms += d.SpaceGetNumGeoms(sp);
+                    }
+                    else
+                        nactivegeoms++;
+                }
+
+                for (int i = 0; i < ntopstaticgeoms; i++)
+                {
+                    sp = d.SpaceGetGeom(StaticSpace, i);
+                    if (d.GeomIsSpace(sp))
+                    {
+                        nstaticspaces++;
+                        nstaticgeoms += d.SpaceGetNumGeoms(sp);
+                    }
+                    else
+                        nstaticgeoms++;
+                }
+
+                int ntopgeoms = d.SpaceGetNumGeoms(TopSpace);
+
+                int totgeoms = nstaticgeoms + nactivegeoms + ngroundgeoms + 1; // one ray
+                int nbodies = d.NTotalBodies;
+                int ngeoms = d.NTotalGeoms;
+*/
+/*
+                looptimeMS /= nodeframes;
+                if(looptimeMS > 0.080)
+                {
+                    collisionTime /= nodeframes;
+                    qstepTIme /= nodeframes;    
+                }
+*/
+                // Finished with all sim stepping. If requested, dump world state to file for debugging.
+                // TODO: This call to the export function is already inside lock (OdeLock) - but is an extra lock needed?
+                // TODO: This overwrites all dump files in-place. Should this be a growing logfile, or separate snapshots?
+                if (physics_logging && (physics_logging_interval > 0) && (framecount % physics_logging_interval == 0))
+                {
+                    string fname = "state-" + world.ToString() + ".DIF"; // give each physics world a separate filename
+                    string prefix = "world" + world.ToString(); // prefix for variable names in exported .DIF file
+
+                    if (physics_logging_append_existing_logfile)
+                    {
+                        string header = "-------------- START OF PHYSICS FRAME " + framecount.ToString() + " --------------";
+                        TextWriter fwriter = File.AppendText(fname);
+                        fwriter.WriteLine(header);
+                        fwriter.Close();
+                    }
+
+                    d.WorldExportDIF(world, fname, physics_logging_append_existing_logfile, prefix);
+                }
+
+                fps = (float)nodeframes * ODE_STEPSIZE / reqTimeStep;
+
+                if(step_time < HalfOdeStep)
+                    m_timeDilation = 1.0f;
+                else if (step_time > m_SkipFramesAtms)
+                {
+                    // if we lag too much skip frames
+                    m_timeDilation = 0.0f;
+                    step_time = 0;
+                    m_lastframe = Util.GetTimeStamp(); // skip also the time lost
+                }
+                else
+                {
+                    m_timeDilation = ODE_STEPSIZE / step_time;
+                    if (m_timeDilation > 1)
+                        m_timeDilation = 1;
+                }
+
+                if (m_timeDilation == 1 && now - m_lastMeshExpire > 30)
+                {
+                    mesher.ExpireReleaseMeshs();
+                    m_lastMeshExpire = now;
+                }
+
+
+            }
+
+            return fps;
+        }
+
+        /// <summary>
+        public override void GetResults()
+        {
+        }
+
+        public override bool IsThreaded
+        {
+            // for now we won't be multithreaded
+            get { return (false); }
+        }
+
+        public float GetTerrainHeightAtXY(float x, float y)
+        {
+            if (TerrainGeom == IntPtr.Zero)
+                return 0f;
+
+            if (TerrainHeightFieldHeight == null || TerrainHeightFieldHeight.Length == 0)
+                return 0f;
+
+            // TerrainHeightField for ODE as offset 1m
+            x += 1f;
+            y += 1f;
+
+            // make position fit into array
+            if (x < 0)
+                x = 0;
+            if (y < 0)
+                y = 0;
+
+            // integer indexs
+            int ix;
+            int iy;
+            //  interpolators offset
+            float dx;
+            float dy;
+
+            int regsizeX = (int)m_regionWidth + 3; // map size see setterrain number of samples
+            int regsizeY = (int)m_regionHeight + 3; // map size see setterrain number of samples
+            int regsize = regsizeX;
+
+            if (m_OSOdeLib)
+            {
+                if (x < regsizeX - 1)
+                {
+                    ix = (int)x;
+                    dx = x - (float)ix;
+                }
+                else // out world use external height
+                {
+                    ix = regsizeX - 2;
+                    dx = 0;
+                }
+                if (y < regsizeY - 1)
+                {
+                    iy = (int)y;
+                    dy = y - (float)iy;
+                }
+                else
+                {
+                    iy = regsizeY - 2;
+                    dy = 0;
+                }
+            }
+            else
+            {
+                // we  still have square fixed size regions
+                // also flip x and y because of how map is done for ODE fliped axis
+                // so ix,iy,dx and dy are inter exchanged
+
+                regsize = regsizeY;
+
+                if (x < regsizeX - 1)
+                {
+                    iy = (int)x;
+                    dy = x - (float)iy;
+                }
+                else // out world use external height
+                {
+                    iy = regsizeX - 2;
+                    dy = 0;
+                }
+                if (y < regsizeY - 1)
+                {
+                    ix = (int)y;
+                    dx = y - (float)ix;
+                }
+                else
+                {
+                    ix = regsizeY - 2;
+                    dx = 0;
+                }
+            }
+
+            float h0;
+            float h1;
+            float h2;
+
+            iy *= regsize;
+            iy += ix; // all indexes have iy + ix
+
+            float[] heights = TerrainHeightFieldHeight;
+            /*
+                        if ((dx + dy) <= 1.0f)
+                        {
+                            h0 = ((float)heights[iy]); // 0,0 vertice
+                            h1 = (((float)heights[iy + 1]) - h0) * dx; // 1,0 vertice minus 0,0
+                            h2 = (((float)heights[iy + regsize]) - h0) * dy; // 0,1 vertice minus 0,0
+                        }
+                        else
+                        {
+                            h0 = ((float)heights[iy + regsize + 1]); // 1,1 vertice
+                            h1 = (((float)heights[iy + 1]) - h0) * (1 - dy); // 1,1 vertice minus 1,0
+                            h2 = (((float)heights[iy + regsize]) - h0) * (1 - dx); // 1,1 vertice minus 0,1
+                        }
+            */
+            h0 = ((float)heights[iy]); // 0,0 vertice
+
+            if (dy>dx)
+            {
+                iy += regsize;
+                h2 = (float)heights[iy]; // 0,1 vertice
+                h1 = (h2 - h0) * dy; // 0,1 vertice minus 0,0
+                h2 = ((float)heights[iy + 1] - h2) * dx; // 1,1 vertice minus 0,1
+            }
+            else
+            {
+                iy++;
+                h2 = (float)heights[iy]; // vertice 1,0
+                h1 = (h2 - h0) * dx; // 1,0 vertice minus 0,0
+                h2 = (((float)heights[iy + regsize]) - h2) * dy; // 1,1 vertice minus 1,0
+            }
+
+            return h0 + h1 + h2;
+        }
+
+        public Vector3 GetTerrainNormalAtXY(float x, float y)
+        {
+            Vector3 norm = new Vector3(0, 0, 1);
+
+            if (TerrainGeom == IntPtr.Zero)
+                return norm;
+
+            if (TerrainHeightFieldHeight == null || TerrainHeightFieldHeight.Length == 0)
+                return norm;
+
+            // TerrainHeightField for ODE as offset 1m
+            x += 1f;
+            y += 1f;
+
+            // make position fit into array
+            if (x < 0)
+                x = 0;
+            if (y < 0)
+                y = 0;
+
+            // integer indexs
+            int ix;
+            int iy;
+            //  interpolators offset
+            float dx;
+            float dy;
+
+            int regsizeX = (int)m_regionWidth + 3; // map size see setterrain number of samples
+            int regsizeY = (int)m_regionHeight + 3; // map size see setterrain number of samples
+            int regsize = regsizeX;
+
+            int xstep = 1;
+            int ystep = regsizeX;
+            bool firstTri = false;
+
+            if (m_OSOdeLib)
+            {
+                if (x < regsizeX - 1)
+                {
+                    ix = (int)x;
+                    dx = x - (float)ix;
+                }
+                else // out world use external height
+                {
+                    ix = regsizeX - 2;
+                    dx = 0;
+                }
+                if (y < regsizeY - 1)
+                {
+                    iy = (int)y;
+                    dy = y - (float)iy;
+                }
+                else
+                {
+                    iy = regsizeY - 2;
+                    dy = 0;
+                }
+                firstTri = dy > dx;
+            }
+
+            else
+            {
+                xstep = regsizeY;
+                ystep = 1;
+                regsize = regsizeY;
+
+                // we  still have square fixed size regions
+                // also flip x and y because of how map is done for ODE fliped axis
+                // so ix,iy,dx and dy are inter exchanged
+                if (x < regsizeX - 1)
+                {
+                    iy = (int)x;
+                    dy = x - (float)iy;
+                }
+                else // out world use external height
+                {
+                    iy = regsizeX - 2;
+                    dy = 0;
+                }
+                if (y < regsizeY - 1)
+                {
+                    ix = (int)y;
+                    dx = y - (float)ix;
+                }
+                else
+                {
+                    ix = regsizeY - 2;
+                    dx = 0;
+                }
+                firstTri = dx > dy;
+            }
+
+            float h0;
+            float h1;
+            float h2;
+
+            iy *= regsize;
+            iy += ix; // all indexes have iy + ix
+
+            float[] heights = TerrainHeightFieldHeight;
+
+            if (firstTri)
+            {
+                h1 = ((float)heights[iy]); // 0,0 vertice
+                iy += ystep;
+                h0 = (float)heights[iy]; // 0,1
+                h2 = (float)heights[iy+xstep]; // 1,1 vertice
+                norm.X = h0 - h2;
+                norm.Y = h1 - h0;
+            }
+            else
+            {
+                h2 = ((float)heights[iy]); // 0,0 vertice
+                iy += xstep;
+                h0 = ((float)heights[iy]); // 1,0 vertice
+                h1 = (float)heights[iy+ystep]; // vertice 1,1
+                norm.X = h2 - h0;
+                norm.Y = h0 - h1;
+            }
+            norm.Z = 1;
+            norm.Normalize();
+            return norm;
+        }
+
+        public override void SetTerrain(float[] heightMap)
+        {
+            if (m_OSOdeLib)
+                OSSetTerrain(heightMap);
+            else
+                OriSetTerrain(heightMap);
+        }
+
+        public void OriSetTerrain(float[] heightMap)
+        {
+            // assumes 1m size grid and constante size square regions
+            // needs to know about sims around in future
+
+            float[] _heightmap;
+
+            uint regionsizeX = m_regionWidth;
+            uint regionsizeY = m_regionHeight;
+
+            // map is rotated
+            uint heightmapWidth = regionsizeY + 2;
+            uint heightmapHeight = regionsizeX + 2;
+
+            uint heightmapWidthSamples = heightmapWidth + 1;
+            uint heightmapHeightSamples = heightmapHeight + 1;
+
+            _heightmap = new float[heightmapWidthSamples * heightmapHeightSamples];
+
+            const float scale = 1.0f;
+            const float offset = 0.0f;
+            const float thickness = 10f;
+            const int wrap = 0;
+
+
+            float hfmin = float.MaxValue;
+            float hfmax = float.MinValue;
+            float val;
+            uint xx;
+            uint yy;
+
+            uint maxXX = regionsizeX - 1;
+            uint maxYY = regionsizeY - 1;
+            // flipping map adding one margin all around so things don't fall in edges
+
+            uint xt = 0;
+            xx = 0;
+
+            for (uint x = 0; x < heightmapWidthSamples; x++)
+            {
+                if (x > 1 && xx < maxXX)
+                    xx++;
+                yy = 0;
+                for (uint y = 0; y < heightmapHeightSamples; y++)
+                {
+                    if (y > 1 && y < maxYY)
+                        yy += regionsizeX;
+
+                    val = heightMap[yy + xx];
+                    if (val < 0.0f)
+                        val = 0.0f; // no neg terrain as in chode
+                    _heightmap[xt + y] = val;
+
+                    if (hfmin > val)
+                        hfmin = val;
+                    if (hfmax < val)
+                        hfmax = val;
+                }
+                xt += heightmapHeightSamples;
+            }
+
+            lock (OdeLock)
+            {
+                d.AllocateODEDataForThread(~0U);
+
+                if (TerrainGeom != IntPtr.Zero)
+                {
+                    actor_name_map.Remove(TerrainGeom);
+                    d.GeomDestroy(TerrainGeom);
+
+                }
+
+                if (TerrainHeightFieldHeightsHandler.IsAllocated)
+                    TerrainHeightFieldHeightsHandler.Free();
+
+                IntPtr HeightmapData = d.GeomHeightfieldDataCreate();
+
+                TerrainHeightFieldHeightsHandler = GCHandle.Alloc(_heightmap, GCHandleType.Pinned);
+
+                d.GeomHeightfieldDataBuildSingle(HeightmapData, TerrainHeightFieldHeightsHandler.AddrOfPinnedObject(), 0,
+                                                heightmapHeight, heightmapWidth ,
+                                                 (int)heightmapHeightSamples, (int)heightmapWidthSamples, scale,
+                                                offset, thickness, wrap);
+
+                d.GeomHeightfieldDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1);
+
+                TerrainGeom = d.CreateHeightfield(GroundSpace, HeightmapData, 1);
+
+                if (TerrainGeom != IntPtr.Zero)
+                {
+                    d.GeomSetCategoryBits(TerrainGeom, (uint)(CollisionCategories.Land));
+                    d.GeomSetCollideBits(TerrainGeom, 0);
+
+                    PhysicsActor pa = new NullPhysicsActor();
+                    pa.Name = "Terrain";
+                    pa.PhysicsActorType = (int)ActorTypes.Ground;
+                    actor_name_map[TerrainGeom] = pa;
+
+//                    geom_name_map[GroundGeom] = "Terrain";
+
+                    d.Quaternion q = new d.Quaternion();
+                    q.X = 0.5f;
+                    q.Y = 0.5f;
+                    q.Z = 0.5f;
+                    q.W = 0.5f;
+
+                    d.GeomSetQuaternion(TerrainGeom, ref q);
+                    d.GeomSetPosition(TerrainGeom, m_regionWidth * 0.5f, m_regionHeight * 0.5f, 0.0f);
+                    TerrainHeightFieldHeight = _heightmap;
+                }
+                else
+                    TerrainHeightFieldHeightsHandler.Free();
+            }
+        }
+
+        public void OSSetTerrain(float[] heightMap)
+        {
+            // assumes 1m size grid and constante size square regions
+            // needs to know about sims around in future
+
+            float[] _heightmap;
+
+            uint regionsizeX = m_regionWidth;
+            uint regionsizeY = m_regionHeight;
+
+            uint heightmapWidth = regionsizeX + 2;
+            uint heightmapHeight = regionsizeY + 2;
+
+            uint heightmapWidthSamples = heightmapWidth + 1;
+            uint heightmapHeightSamples = heightmapHeight + 1;
+
+            _heightmap = new float[heightmapWidthSamples * heightmapHeightSamples];
+
+
+            float hfmin = float.MaxValue;
+//            float hfmax = float.MinValue;
+            float val;
+
+
+            uint maxXX = regionsizeX + 1;
+            uint maxYY = regionsizeY + 1;
+            // adding one margin all around so things don't fall in edges
+
+            uint xx;
+            uint yy = 0;
+            uint yt = 0;
+
+            for (uint y = 0; y < heightmapHeightSamples; y++)
+            {
+                if (y > 1 && y < maxYY)
+                    yy += regionsizeX;
+                xx = 0;
+                for (uint x = 0; x < heightmapWidthSamples; x++)
+                {
+                    if (x > 1 && x < maxXX)
+                        xx++;
+
+                    val = heightMap[yy + xx];
+                    if (val < 0.0f)
+                        val = 0.0f; // no neg terrain as in chode
+                    _heightmap[yt + x] = val;
+
+                    if (hfmin > val)
+                        hfmin = val;
+//                    if (hfmax < val)
+//                        hfmax = val;
+                }
+                yt += heightmapWidthSamples;
+            }
+
+            lock (OdeLock)
+            {
+                if (TerrainGeom != IntPtr.Zero)
+                {
+                    actor_name_map.Remove(TerrainGeom);
+                    d.GeomDestroy(TerrainGeom);
+                }
+
+                if (TerrainHeightFieldHeightsHandler.IsAllocated)
+                            TerrainHeightFieldHeightsHandler.Free();
+
+                TerrainHeightFieldHeight = null;
+
+                IntPtr HeightmapData = d.GeomOSTerrainDataCreate();
+
+                const int wrap = 0;
+                float thickness = hfmin;
+                if (thickness < 0)
+                    thickness = 1;
+
+                TerrainHeightFieldHeightsHandler = GCHandle.Alloc(_heightmap, GCHandleType.Pinned);
+
+                d.GeomOSTerrainDataBuild(HeightmapData, TerrainHeightFieldHeightsHandler.AddrOfPinnedObject(), 0, 1.0f,
+                                                 (int)heightmapWidthSamples, (int)heightmapHeightSamples,
+                                                 thickness, wrap);
+
+//                d.GeomOSTerrainDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1);
+                TerrainGeom = d.CreateOSTerrain(GroundSpace, HeightmapData, 1);
+                if (TerrainGeom != IntPtr.Zero)
+                {
+                    d.GeomSetCategoryBits(TerrainGeom, (uint)(CollisionCategories.Land));
+                    d.GeomSetCollideBits(TerrainGeom, 0);
+
+                    PhysicsActor pa = new NullPhysicsActor();
+                    pa.Name = "Terrain";
+                    pa.PhysicsActorType = (int)ActorTypes.Ground;
+                    actor_name_map[TerrainGeom] = pa;
+
+//                    geom_name_map[GroundGeom] = "Terrain";
+
+                    d.GeomSetPosition(TerrainGeom, m_regionWidth * 0.5f, m_regionHeight * 0.5f, 0.0f);
+                    TerrainHeightFieldHeight = _heightmap;
+                 }
+                 else
+                    TerrainHeightFieldHeightsHandler.Free();
+            }
+        }
+
+        public override void DeleteTerrain()
+        {
+        }
+
+        public float GetWaterLevel()
+        {
+            return waterlevel;
+        }
+
+        public override void SetWaterLevel(float baseheight)
+        {
+            waterlevel = baseheight;
+        }
+
+        public override void Dispose()
+        {
+            lock (OdeLock)
+            {
+
+                if (world == IntPtr.Zero)
+                    return;
+
+                d.AllocateODEDataForThread(~0U);
+
+                if (m_meshWorker != null)
+                    m_meshWorker.Stop();
+
+                if (m_rayCastManager != null)
+                {
+                    m_rayCastManager.Dispose();
+                    m_rayCastManager = null;
+                }
+
+                lock (_prims)
+                {
+                    ChangesQueue.Clear();
+                    foreach (OdePrim prm in _prims.Values)
+                    {
+                        prm.DoAChange(changes.Remove, null);
+                        _collisionEventPrim.Remove(prm);
+                    }
+                    _prims.Clear();
+                }
+
+                OdeCharacter[] chtorem;
+                lock (_characters)
+                {
+                    chtorem = new OdeCharacter[_characters.Count];
+                    _characters.CopyTo(chtorem);
+                }
+
+                ChangesQueue.Clear();
+                foreach (OdeCharacter ch in chtorem)
+                    ch.DoAChange(changes.Remove, null);
+
+                if (TerrainGeom != IntPtr.Zero)
+                        d.GeomDestroy(TerrainGeom);
+                TerrainGeom = IntPtr.Zero;
+
+                if (TerrainHeightFieldHeightsHandler.IsAllocated)
+                    TerrainHeightFieldHeightsHandler.Free();
+
+                TerrainHeightFieldHeight = null;
+
+                if (ContactgeomsArray != IntPtr.Zero)
+                {
+                    Marshal.FreeHGlobal(ContactgeomsArray);
+                    ContactgeomsArray = IntPtr.Zero;
+                }
+                if (GlobalContactsArray != IntPtr.Zero)
+                {
+                    Marshal.FreeHGlobal(GlobalContactsArray);
+                    GlobalContactsArray = IntPtr.Zero;
+                }
+
+                d.WorldDestroy(world);
+                world = IntPtr.Zero;
+                //d.CloseODE();
+            }
+        }
+
+        private int compareByCollisionsDesc(OdePrim A, OdePrim B)
+        {
+            return -A.CollisionScore.CompareTo(B.CollisionScore);
+        }
+
+        public override Dictionary<uint, float> GetTopColliders()
+        {
+            Dictionary<uint, float> topColliders;
+            List<OdePrim> orderedPrims;
+            lock (_activeprims)
+                orderedPrims = new List<OdePrim>(_activeprims);
+
+            orderedPrims.Sort(compareByCollisionsDesc);
+            topColliders = orderedPrims.Take(25).ToDictionary(p => p.LocalID, p => p.CollisionScore);
+
+            return topColliders;
+        }
+
+        public override bool SupportsRayCast()
+        {
+            return true;
+        }
+
+        public override void RaycastWorld(Vector3 position, Vector3 direction, float length, RaycastCallback retMethod)
+        {
+            if (retMethod != null)
+            {
+                ODERayRequest req = new ODERayRequest();
+                req.actor = null;
+                req.callbackMethod = retMethod;
+                req.length = length;
+                req.Normal = direction;
+                req.Origin = position;
+                req.Count = 0;
+                req.filter = RayFilterFlags.AllPrims;
+
+                m_rayCastManager.QueueRequest(req);
+            }
+        }
+
+        public override void RaycastWorld(Vector3 position, Vector3 direction, float length, int Count, RayCallback retMethod)
+        {
+            if (retMethod != null)
+            {
+                ODERayRequest req = new ODERayRequest();
+                req.actor = null;
+                req.callbackMethod = retMethod;
+                req.length = length;
+                req.Normal = direction;
+                req.Origin = position;
+                req.Count = Count;
+                req.filter = RayFilterFlags.AllPrims;
+
+                m_rayCastManager.QueueRequest(req);
+            }
+        }
+
+
+        public override List<ContactResult> RaycastWorld(Vector3 position, Vector3 direction, float length, int Count)
+        {
+            List<ContactResult> ourresults = new List<ContactResult>();
+            object SyncObject = new object();
+
+            RayCallback retMethod = delegate(List<ContactResult> results)
+            {
+                lock (SyncObject)
+                {
+                    ourresults = results;
+                    Monitor.PulseAll(SyncObject);
+                }
+            };
+
+            ODERayRequest req = new ODERayRequest();
+            req.actor = null;
+            req.callbackMethod = retMethod;
+            req.length = length;
+            req.Normal = direction;
+            req.Origin = position;
+            req.Count = Count;
+            req.filter = RayFilterFlags.AllPrims;
+
+            lock (SyncObject)
+            {
+                m_rayCastManager.QueueRequest(req);
+                if (!Monitor.Wait(SyncObject, 500))
+                    return null;
+                else
+                    return ourresults;
+            }
+        }
+
+        public override bool SupportsRaycastWorldFiltered()
+        {
+            return true;
+        }
+
+        public override object RaycastWorld(Vector3 position, Vector3 direction, float length, int Count, RayFilterFlags filter)
+        {
+            object SyncObject = new object();
+            List<ContactResult> ourresults = new List<ContactResult>();
+
+            RayCallback retMethod = delegate(List<ContactResult> results)
+            {
+                lock (SyncObject)
+                {
+                    ourresults = results;
+                    Monitor.PulseAll(SyncObject);
+                }
+            };
+
+            ODERayRequest req = new ODERayRequest();
+            req.actor = null;
+            req.callbackMethod = retMethod;
+            req.length = length;
+            req.Normal = direction;
+            req.Origin = position;
+            req.Count = Count;
+            req.filter = filter;
+
+            lock (SyncObject)
+            {
+                m_rayCastManager.QueueRequest(req);
+                if (!Monitor.Wait(SyncObject, 500))
+                    return null;
+                else
+                    return ourresults;
+            }
+        }
+
+        public override List<ContactResult> RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count, RayFilterFlags flags)
+        {
+            if (actor == null)
+                return new List<ContactResult>();
+
+            IntPtr geom;
+            if (actor is OdePrim)
+                geom = ((OdePrim)actor).prim_geom;
+            else if (actor is OdeCharacter)
+                geom = ((OdePrim)actor).prim_geom;
+            else
+                return new List<ContactResult>();
+
+            if (geom == IntPtr.Zero)
+                return new List<ContactResult>();
+
+            List<ContactResult> ourResults = null;
+            object SyncObject = new object();
+
+            RayCallback retMethod = delegate(List<ContactResult> results)
+            {
+                lock (SyncObject)
+                {
+                    ourResults = results;
+                    Monitor.PulseAll(SyncObject);
+                }
+            };
+
+            ODERayRequest req = new ODERayRequest();
+            req.actor = actor;
+            req.callbackMethod = retMethod;
+            req.length = length;
+            req.Normal = direction;
+            req.Origin = position;
+            req.Count = Count;
+            req.filter = flags;
+
+            lock (SyncObject)
+            {
+                m_rayCastManager.QueueRequest(req);
+                if (!Monitor.Wait(SyncObject, 500))
+                    return new List<ContactResult>();
+            }
+
+            if (ourResults == null)
+                return new List<ContactResult>();
+            return ourResults;
+        }
+
+        public override List<ContactResult> BoxProbe(Vector3 position, Vector3 size, Quaternion orientation, int Count, RayFilterFlags flags)
+        {
+            List<ContactResult> ourResults = null;
+            object SyncObject = new object();
+
+            ProbeBoxCallback retMethod = delegate(List<ContactResult> results)
+            {
+                lock (SyncObject)
+                {
+                    ourResults = results;
+                    Monitor.PulseAll(SyncObject);
+                }
+            };
+
+            ODERayRequest req = new ODERayRequest();
+            req.actor = null;
+            req.callbackMethod = retMethod;
+            req.Normal = size;
+            req.Origin = position;
+            req.orientation = orientation;
+            req.Count = Count;
+            req.filter = flags;
+
+            lock (SyncObject)
+            {
+                m_rayCastManager.QueueRequest(req);
+                if (!Monitor.Wait(SyncObject, 500))
+                    return new List<ContactResult>();
+            }
+
+            if (ourResults == null)
+                return new List<ContactResult>();
+            return ourResults;
+        }
+
+        public override List<ContactResult> SphereProbe(Vector3 position, float radius, int Count, RayFilterFlags flags)
+        {
+            List<ContactResult> ourResults = null;
+            object SyncObject = new object();
+
+            ProbeSphereCallback retMethod = delegate(List<ContactResult> results)
+            {
+                ourResults = results;
+                Monitor.PulseAll(SyncObject);
+            };
+
+            ODERayRequest req = new ODERayRequest();
+            req.actor = null;
+            req.callbackMethod = retMethod;
+            req.length = radius;
+            req.Origin = position;
+            req.Count = Count;
+            req.filter = flags;
+
+
+            lock (SyncObject)
+            {
+                m_rayCastManager.QueueRequest(req);
+                if (!Monitor.Wait(SyncObject, 500))
+                    return new List<ContactResult>();
+            }
+
+            if (ourResults == null)
+                return new List<ContactResult>();
+            return ourResults;
+        }
+
+        public override List<ContactResult> PlaneProbe(PhysicsActor actor, Vector4 plane, int Count, RayFilterFlags flags)
+        {
+            IntPtr geom = IntPtr.Zero;;
+
+            if (actor != null)
+            {
+                if (actor is OdePrim)
+                    geom = ((OdePrim)actor).prim_geom;
+                else if (actor is OdeCharacter)
+                    geom = ((OdePrim)actor).prim_geom;
+            }
+
+            List<ContactResult> ourResults = null;
+            object SyncObject = new object();
+
+            ProbePlaneCallback retMethod = delegate(List<ContactResult> results)
+            {
+                ourResults = results;
+                Monitor.PulseAll(SyncObject);
+            };
+
+            ODERayRequest req = new ODERayRequest();
+            req.actor = null;
+            req.callbackMethod = retMethod;
+            req.length = plane.W;
+            req.Normal.X = plane.X;
+            req.Normal.Y = plane.Y;
+            req.Normal.Z = plane.Z;
+            req.Count = Count;
+            req.filter = flags;
+
+            lock (SyncObject)
+            {
+                m_rayCastManager.QueueRequest(req);
+                if (!Monitor.Wait(SyncObject, 500))
+                    return new List<ContactResult>();
+            }
+
+            if (ourResults == null)
+                return new List<ContactResult>();
+            return ourResults;
+        }
+
+        public override int SitAvatar(PhysicsActor actor, Vector3 AbsolutePosition, Vector3 CameraPosition, Vector3 offset, Vector3 AvatarSize, SitAvatarCallback PhysicsSitResponse)
+        {
+            Util.FireAndForget( delegate
+            {
+                ODESitAvatar sitAvatar = new ODESitAvatar(this, m_rayCastManager);
+                if(sitAvatar != null)
+                    sitAvatar.Sit(actor, AbsolutePosition, CameraPosition, offset, AvatarSize, PhysicsSitResponse);
+            });
+            return 1;
+        }
+
+    }
+}
diff --git a/OpenSim/Region/PhysicsModules/ubOde/ODESitAvatar.cs b/OpenSim/Region/PhysicsModules/ubOde/ODESitAvatar.cs
new file mode 100644
index 0000000..214205d
--- /dev/null
+++ b/OpenSim/Region/PhysicsModules/ubOde/ODESitAvatar.cs
@@ -0,0 +1,356 @@
+/*
+ * Copyright (c) Contributors, http://opensimulator.org/
+ * See CONTRIBUTORS.TXT for a full list of copyright holders.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the OpenSimulator Project nor the
+ *       names of its contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+// Ubit Umarov 2012
+using System;
+using System.Collections.Generic;
+using System.Reflection;
+using System.Runtime.InteropServices;
+using System.Text;
+using OpenSim.Framework;
+using OpenSim.Region.PhysicsModules.SharedBase;
+using OdeAPI;
+using log4net;
+using OpenMetaverse;
+
+namespace OpenSim.Region.PhysicsModule.ubOde
+{
+    /// <summary>
+    /// </summary>
+    public class ODESitAvatar
+    {
+        private ODEScene m_scene;
+        private ODERayCastRequestManager m_raymanager;
+
+        public ODESitAvatar(ODEScene pScene, ODERayCastRequestManager raymanager)
+        {
+            m_scene = pScene;
+            m_raymanager = raymanager;
+        }
+
+        private static Vector3 SitAjust = new Vector3(0, 0, 0.4f);
+        private const RayFilterFlags RaySitFlags = RayFilterFlags.AllPrims | RayFilterFlags.ClosestHit;
+
+        private void RotAroundZ(float x, float y, ref Quaternion ori)
+        {
+            double ang = Math.Atan2(y, x);
+            ang *= 0.5d;
+            float s = (float)Math.Sin(ang);
+            float c = (float)Math.Cos(ang);
+
+            ori.X = 0;
+            ori.Y = 0;
+            ori.Z = s;
+            ori.W = c;
+        }
+
+
+        public void Sit(PhysicsActor actor, Vector3 avPos, Vector3 avCameraPosition, Vector3 offset, Vector3 avOffset, SitAvatarCallback PhysicsSitResponse)
+        {
+            if (!m_scene.haveActor(actor) || !(actor is OdePrim) || ((OdePrim)actor).prim_geom == IntPtr.Zero)
+            {
+                PhysicsSitResponse(-1, actor.LocalID, offset, Quaternion.Identity);
+                return;
+            }
+
+            IntPtr geom = ((OdePrim)actor).prim_geom;
+
+            Vector3 geopos = d.GeomGetPositionOMV(geom);
+            Quaternion geomOri = d.GeomGetQuaternionOMV(geom);
+
+//            Vector3 geopos = actor.Position;
+//            Quaternion geomOri = actor.Orientation;
+
+            Quaternion geomInvOri = Quaternion.Conjugate(geomOri);
+
+            Quaternion ori = Quaternion.Identity;
+
+            Vector3 rayDir = geopos + offset - avCameraPosition;
+
+            float raylen = rayDir.Length();
+            if (raylen < 0.001f)
+            {
+                PhysicsSitResponse(-1, actor.LocalID, offset, Quaternion.Identity);
+                return;
+            }
+            float t = 1 / raylen;
+            rayDir.X *= t;
+            rayDir.Y *= t;
+            rayDir.Z *= t;
+
+            raylen += 30f; // focal point may be far
+            List<ContactResult> rayResults;
+
+            rayResults = m_scene.RaycastActor(actor, avCameraPosition, rayDir, raylen, 1, RaySitFlags);
+            if (rayResults.Count == 0)
+            {
+/* if this fundamental ray failed, then just fail so user can try another spot and not be sitted far on a big prim
+                d.AABB aabb;
+                d.GeomGetAABB(geom, out aabb);
+                offset = new Vector3(avOffset.X, 0, aabb.MaxZ + avOffset.Z - geopos.Z);
+                ori = geomInvOri;
+                offset *= geomInvOri;
+                PhysicsSitResponse(1, actor.LocalID, offset, ori);
+*/
+                PhysicsSitResponse(0, actor.LocalID, offset, ori);
+                return;
+            }
+
+            int status = 1;
+
+            offset = rayResults[0].Pos - geopos;
+
+            d.GeomClassID geoclass = d.GeomGetClass(geom);
+
+            if (geoclass == d.GeomClassID.SphereClass)
+            {
+                float r = d.GeomSphereGetRadius(geom);
+
+                offset.Normalize();
+                offset *= r;
+
+                RotAroundZ(offset.X, offset.Y, ref ori);
+
+                if (r < 0.4f)
+                {
+                    offset = new Vector3(0, 0, r);
+                }
+                else
+                {
+                    if (offset.Z < 0.4f)
+                    {
+                        t = offset.Z;
+                        float rsq = r * r;
+
+                        t = 1.0f / (rsq - t * t);
+                        offset.X *= t;
+                        offset.Y *= t;
+                        offset.Z = 0.4f;
+                        t = rsq - 0.16f;
+                        offset.X *= t;
+                        offset.Y *= t;
+                    }
+                    else if (r > 0.8f && offset.Z > 0.8f * r)
+                    {
+                        status = 3;
+                        avOffset.X = -avOffset.X;
+                        avOffset.Z *= 1.6f;
+                    }
+                }
+
+                offset += avOffset * ori;
+
+                ori = geomInvOri * ori;
+                offset *= geomInvOri;
+
+                PhysicsSitResponse(status, actor.LocalID, offset, ori);
+                return;
+            }
+
+            Vector3 norm = rayResults[0].Normal;
+
+            if (norm.Z < -0.4f)
+            {
+                PhysicsSitResponse(0, actor.LocalID, offset, Quaternion.Identity);
+                return;
+            }
+
+
+            float SitNormX = -rayDir.X;
+            float SitNormY = -rayDir.Y;
+
+            Vector3 pivot = geopos + offset;
+
+            float edgeNormalX = norm.X;
+            float edgeNormalY = norm.Y;
+            float edgeDirX = -rayDir.X;
+            float edgeDirY = -rayDir.Y;
+            Vector3 edgePos = rayResults[0].Pos;
+            float edgeDist = float.MaxValue;
+
+            bool foundEdge = false;
+
+            if (norm.Z < 0.5f)
+            {
+                float rayDist = 4.0f;
+
+                for (int i = 0; i < 6; i++)
+                {
+                    pivot.X -= 0.01f * norm.X;
+                    pivot.Y -= 0.01f * norm.Y;
+                    pivot.Z -= 0.01f * norm.Z;
+
+                    rayDir.X = -norm.X * norm.Z;
+                    rayDir.Y = -norm.Y * norm.Z;
+                    rayDir.Z = 1.0f - norm.Z * norm.Z;
+                    rayDir.Normalize();
+
+                    rayResults = m_scene.RaycastActor(actor, pivot, rayDir, rayDist, 1, RayFilterFlags.AllPrims);
+                    if (rayResults.Count == 0)
+                        break;
+
+                    if (Math.Abs(rayResults[0].Normal.Z) < 0.7f)
+                    {
+                        rayDist -= rayResults[0].Depth;
+                        if (rayDist < 0f)
+                            break;
+
+                        pivot = rayResults[0].Pos;
+                        norm = rayResults[0].Normal;
+                        edgeNormalX = norm.X;
+                        edgeNormalY = norm.Y;
+                        edgeDirX = -rayDir.X;
+                        edgeDirY = -rayDir.Y;
+                    }
+                    else
+                    {
+                        foundEdge = true;
+                        edgePos = rayResults[0].Pos;
+                        break;
+                    }
+                }
+
+                if (!foundEdge)
+                {
+                    PhysicsSitResponse(0, actor.LocalID, offset, ori);
+                    return;
+                }
+                avOffset.X *= 0.5f;
+            }
+
+            else if (norm.Z > 0.866f)
+            {
+                float toCamBaseX = avCameraPosition.X - pivot.X;
+                float toCamBaseY = avCameraPosition.Y - pivot.Y;
+                float toCamX = toCamBaseX;
+                float toCamY = toCamBaseY;
+
+                for (int j = 0; j < 4; j++)
+                {
+                    float rayDist = 1.0f;
+                    float curEdgeDist = 0.0f;
+
+                    for (int i = 0; i < 3; i++)
+                    {
+                        pivot.Z -= 0.01f;
+                        rayDir.X = toCamX;
+                        rayDir.Y = toCamY;
+                        rayDir.Z = (-toCamX * norm.X - toCamY * norm.Y) / norm.Z;
+                        rayDir.Normalize();
+
+                        rayResults = m_scene.RaycastActor(actor, pivot, rayDir, rayDist, 1, RayFilterFlags.AllPrims);
+                        if (rayResults.Count == 0)
+                            break;
+
+                        curEdgeDist += rayResults[0].Depth;
+
+                        if (rayResults[0].Normal.Z > 0.5f)
+                        {
+                            rayDist -= rayResults[0].Depth;
+                            if (rayDist < 0f)
+                                break;
+
+                            pivot = rayResults[0].Pos;
+                            norm = rayResults[0].Normal;
+                        }
+                        else
+                        {
+                            foundEdge = true;
+                            if (curEdgeDist < edgeDist)
+                            {
+                                edgeDist = curEdgeDist;
+                                edgeNormalX = rayResults[0].Normal.X;
+                                edgeNormalY = rayResults[0].Normal.Y;
+                                edgeDirX = rayDir.X;
+                                edgeDirY = rayDir.Y;
+                                edgePos = rayResults[0].Pos;
+                            }
+                            break;
+                        }
+                    }
+                    if (foundEdge && edgeDist < 0.2f)
+                        break;
+
+                    pivot = geopos + offset;
+
+                    switch (j)
+                    {
+                        case 0:
+                            toCamX = -toCamBaseY;
+                            toCamY = toCamBaseX;
+                            break;
+                        case 1:
+                            toCamX = toCamBaseY;
+                            toCamY = -toCamBaseX;
+                            break;
+                        case 2:
+                            toCamX = -toCamBaseX;
+                            toCamY = -toCamBaseY;
+                            break;
+                        default:
+                            break;
+                    }
+                }
+
+                if (!foundEdge)
+                {
+                    avOffset.X = -avOffset.X;
+                    avOffset.Z *= 1.6f;
+
+                    RotAroundZ(SitNormX, SitNormY, ref ori);
+
+                    offset += avOffset * ori;
+
+                    ori = geomInvOri * ori;
+                    offset *= geomInvOri;
+
+                    PhysicsSitResponse(3, actor.LocalID, offset, ori);
+                    return;
+                }
+                avOffset.X *= 0.5f;
+            }
+
+            SitNormX = edgeNormalX;
+            SitNormY = edgeNormalY;
+            if (edgeDirX * SitNormX + edgeDirY * SitNormY < 0)
+            {
+                SitNormX = -SitNormX;
+                SitNormY = -SitNormY;
+            }
+
+            RotAroundZ(SitNormX, SitNormY, ref ori);
+
+            offset = edgePos + avOffset * ori;
+            offset -= geopos;
+
+            ori = geomInvOri * ori;
+            offset *= geomInvOri;
+
+            PhysicsSitResponse(1, actor.LocalID, offset, ori);
+            return;
+        }
+    }
+}
diff --git a/OpenSim/Region/PhysicsModules/ubOde/Properties/AssemblyInfo.cs b/OpenSim/Region/PhysicsModules/ubOde/Properties/AssemblyInfo.cs
new file mode 100644
index 0000000..5baab9f
--- /dev/null
+++ b/OpenSim/Region/PhysicsModules/ubOde/Properties/AssemblyInfo.cs
@@ -0,0 +1,61 @@
+/*
+ * Copyright (c) Contributors, http://opensimulator.org/
+ * See CONTRIBUTORS.TXT for a full list of copyright holders.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the OpenSimulator Project nor the
+ *       names of its contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+using System.Reflection;
+using System.Runtime.InteropServices;
+using Mono.Addins;
+
+// Information about this assembly is defined by the following
+// attributes.
+//
+// change them to the information which is associated with the assembly
+// you compile.
+
+[assembly : AssemblyTitle("OpenSim.Region.PhysicsModule.ubOde")]
+[assembly : AssemblyDescription("Ubit Variation of ODE")]
+[assembly : AssemblyConfiguration("")]
+[assembly : AssemblyCompany("http://opensimulator.org")]
+[assembly : AssemblyProduct("ubOde")]
+[assembly : AssemblyCopyright("Copyright (c) OpenSimulator.org Developers 2007-2009")]
+[assembly : AssemblyTrademark("")]
+[assembly : AssemblyCulture("")]
+
+// This sets the default COM visibility of types in the assembly to invisible.
+// If you need to expose a type to COM, use [ComVisible(true)] on that type.
+
+[assembly : ComVisible(false)]
+
+// The assembly version has following format :
+//
+// Major.Minor.Build.Revision
+//
+// You can specify all values by your own or you can build default build and revision
+// numbers with the '*' character (the default):
+
+[assembly : AssemblyVersion(OpenSim.VersionInfo.AssemblyVersionNumber)]
+[assembly: Addin("OpenSim.Region.PhysicsModule.ubOde", OpenSim.VersionInfo.VersionNumber)]
+[assembly: AddinDependency("OpenSim.Region.Framework", OpenSim.VersionInfo.VersionNumber)]
\ No newline at end of file
diff --git a/OpenSim/Region/PhysicsModules/ubOdeMeshing/HelperTypes.cs b/OpenSim/Region/PhysicsModules/ubOdeMeshing/HelperTypes.cs
new file mode 100644
index 0000000..ea37301
--- /dev/null
+++ b/OpenSim/Region/PhysicsModules/ubOdeMeshing/HelperTypes.cs
@@ -0,0 +1,340 @@
+/*
+ * Copyright (c) Contributors, http://opensimulator.org/
+ * See CONTRIBUTORS.TXT for a full list of copyright holders.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the OpenSimulator Project nor the
+ *       names of its contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+using System;
+using System.Collections.Generic;
+using System.Diagnostics;
+using System.Globalization;
+using OpenMetaverse;
+using OpenSim.Region.PhysicsModules.SharedBase;
+using OpenSim.Region.PhysicsModule.ubODEMeshing;
+
+public class Vertex : IComparable<Vertex>
+{
+    Vector3 vector;
+
+    public float X
+    {
+        get { return vector.X; }
+        set { vector.X = value; }
+    }
+
+    public float Y
+    {
+        get { return vector.Y; }
+        set { vector.Y = value; }
+    }
+
+    public float Z
+    {
+        get { return vector.Z; }
+        set { vector.Z = value; }
+    }
+
+    public Vertex(float x, float y, float z)
+    {
+        vector.X = x;
+        vector.Y = y;
+        vector.Z = z;
+    }
+
+    public Vertex normalize()
+    {
+        float tlength = vector.Length();
+        if (tlength != 0f)
+        {
+            float mul = 1.0f / tlength;
+            return new Vertex(vector.X * mul, vector.Y * mul, vector.Z * mul);
+        }
+        else
+        {
+            return new Vertex(0f, 0f, 0f);
+        }
+    }
+
+    public Vertex cross(Vertex v)
+    {
+        return new Vertex(vector.Y * v.Z - vector.Z * v.Y, vector.Z * v.X - vector.X * v.Z, vector.X * v.Y - vector.Y * v.X);
+    }
+
+    // disable warning: mono compiler moans about overloading
+    // operators hiding base operator but should not according to C#
+    // language spec
+#pragma warning disable 0108
+    public static Vertex operator *(Vertex v, Quaternion q)
+    {
+        // From http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/
+
+        Vertex v2 = new Vertex(0f, 0f, 0f);
+
+        v2.X =   q.W * q.W * v.X +
+            2f * q.Y * q.W * v.Z -
+            2f * q.Z * q.W * v.Y +
+                 q.X * q.X * v.X +
+            2f * q.Y * q.X * v.Y +
+            2f * q.Z * q.X * v.Z -
+                 q.Z * q.Z * v.X -
+                 q.Y * q.Y * v.X;
+
+        v2.Y =
+            2f * q.X * q.Y * v.X +
+                 q.Y * q.Y * v.Y +
+            2f * q.Z * q.Y * v.Z +
+            2f * q.W * q.Z * v.X -
+                 q.Z * q.Z * v.Y +
+                 q.W * q.W * v.Y -
+            2f * q.X * q.W * v.Z -
+                 q.X * q.X * v.Y;
+
+        v2.Z =
+            2f * q.X * q.Z * v.X +
+            2f * q.Y * q.Z * v.Y +
+                 q.Z * q.Z * v.Z -
+            2f * q.W * q.Y * v.X -
+                 q.Y * q.Y * v.Z +
+            2f * q.W * q.X * v.Y -
+                 q.X * q.X * v.Z +
+                 q.W * q.W * v.Z;
+
+        return v2;
+    }
+
+    public static Vertex operator +(Vertex v1, Vertex v2)
+    {
+        return new Vertex(v1.X + v2.X, v1.Y + v2.Y, v1.Z + v2.Z);
+    }
+
+    public static Vertex operator -(Vertex v1, Vertex v2)
+    {
+        return new Vertex(v1.X - v2.X, v1.Y - v2.Y, v1.Z - v2.Z);
+    }
+
+    public static Vertex operator *(Vertex v1, Vertex v2)
+    {
+        return new Vertex(v1.X * v2.X, v1.Y * v2.Y, v1.Z * v2.Z);
+    }
+
+    public static Vertex operator +(Vertex v1, float am)
+    {
+        v1.X += am;
+        v1.Y += am;
+        v1.Z += am;
+        return v1;
+    }
+
+    public static Vertex operator -(Vertex v1, float am)
+    {
+        v1.X -= am;
+        v1.Y -= am;
+        v1.Z -= am;
+        return v1;
+    }
+
+    public static Vertex operator *(Vertex v1, float am)
+    {
+        v1.X *= am;
+        v1.Y *= am;
+        v1.Z *= am;
+        return v1;
+    }
+
+    public static Vertex operator /(Vertex v1, float am)
+    {
+        if (am == 0f)
+        {
+            return new Vertex(0f,0f,0f);
+        }
+        float mul = 1.0f / am;
+        v1.X *= mul;
+        v1.Y *= mul;
+        v1.Z *= mul;
+        return v1;
+    }
+#pragma warning restore 0108
+
+
+    public float dot(Vertex v)
+    {
+        return X * v.X + Y * v.Y + Z * v.Z;
+    }
+
+    public Vertex(Vector3 v)
+    {
+        vector = v;
+    }
+
+    public Vertex Clone()
+    {
+        return new Vertex(X, Y, Z);
+    }
+
+    public static Vertex FromAngle(double angle)
+    {
+        return new Vertex((float) Math.Cos(angle), (float) Math.Sin(angle), 0.0f);
+    }
+
+    public float Length()
+    {
+        return vector.Length();
+    }
+
+    public virtual bool Equals(Vertex v, float tolerance)
+    {
+        Vertex diff = this - v;
+        float d = diff.Length();
+        if (d < tolerance)
+            return true;
+
+        return false;
+    }
+
+
+    public int CompareTo(Vertex other)
+    {
+        if (X < other.X)
+            return -1;
+
+        if (X > other.X)
+            return 1;
+
+        if (Y < other.Y)
+            return -1;
+
+        if (Y > other.Y)
+            return 1;
+
+        if (Z < other.Z)
+            return -1;
+
+        if (Z > other.Z)
+            return 1;
+
+        return 0;
+    }
+
+    public static bool operator >(Vertex me, Vertex other)
+    {
+        return me.CompareTo(other) > 0;
+    }
+
+    public static bool operator <(Vertex me, Vertex other)
+    {
+        return me.CompareTo(other) < 0;
+    }
+
+    public String ToRaw()
+    {
+        // Why this stuff with the number formatter?
+        // Well, the raw format uses the english/US notation of numbers
+        // where the "," separates groups of 1000 while the "." marks the border between 1 and 10E-1.
+        // The german notation uses these characters exactly vice versa!
+        // The Float.ToString() routine is a localized one, giving different results depending on the country
+        // settings your machine works with. Unusable for a machine readable file format :-(
+        NumberFormatInfo nfi = new NumberFormatInfo();
+        nfi.NumberDecimalSeparator = ".";
+        nfi.NumberDecimalDigits = 6;
+
+        String s1 = X.ToString(nfi) + " " + Y.ToString(nfi) + " " + Z.ToString(nfi);
+
+        return s1;
+    }
+}
+
+public class Triangle
+{
+    public Vertex v1;
+    public Vertex v2;
+    public Vertex v3;
+
+    public Triangle(Vertex _v1, Vertex _v2, Vertex _v3)
+    {
+        v1 = _v1;
+        v2 = _v2;
+        v3 = _v3;
+    }
+
+    public Triangle(float _v1x,float _v1y,float _v1z,
+                    float _v2x,float _v2y,float _v2z,
+                    float _v3x,float _v3y,float _v3z)
+    {
+        v1 = new Vertex(_v1x, _v1y, _v1z);
+        v2 = new Vertex(_v2x, _v2y, _v2z);
+        v3 = new Vertex(_v3x, _v3y, _v3z);
+    }
+
+    public override String ToString()
+    {
+        NumberFormatInfo nfi = new NumberFormatInfo();
+        nfi.CurrencyDecimalDigits = 2;
+        nfi.CurrencyDecimalSeparator = ".";
+
+        String s1 = "<" + v1.X.ToString(nfi) + "," + v1.Y.ToString(nfi) + "," + v1.Z.ToString(nfi) + ">";
+        String s2 = "<" + v2.X.ToString(nfi) + "," + v2.Y.ToString(nfi) + "," + v2.Z.ToString(nfi) + ">";
+        String s3 = "<" + v3.X.ToString(nfi) + "," + v3.Y.ToString(nfi) + "," + v3.Z.ToString(nfi) + ">";
+
+        return s1 + ";" + s2 + ";" + s3;
+    }
+
+    public Vector3 getNormal()
+    {
+        // Vertices
+
+        // Vectors for edges
+        Vector3 e1;
+        Vector3 e2;
+
+        e1 = new Vector3(v1.X - v2.X, v1.Y - v2.Y, v1.Z - v2.Z);
+        e2 = new Vector3(v1.X - v3.X, v1.Y - v3.Y, v1.Z - v3.Z);
+
+        // Cross product for normal
+        Vector3 n = Vector3.Cross(e1, e2);
+
+        // Length
+        float l = n.Length();
+
+        // Normalized "normal"
+        n = n/l;
+
+        return n;
+    }
+
+    public void invertNormal()
+    {
+        Vertex vt;
+        vt = v1;
+        v1 = v2;
+        v2 = vt;
+    }
+
+    // Dumps a triangle in the "raw faces" format, blender can import. This is for visualisation and
+    // debugging purposes
+    public String ToStringRaw()
+    {
+        String output = v1.ToRaw() + " " + v2.ToRaw() + " " + v3.ToRaw();
+        return output;
+    }
+}
diff --git a/OpenSim/Region/PhysicsModules/ubOdeMeshing/Mesh.cs b/OpenSim/Region/PhysicsModules/ubOdeMeshing/Mesh.cs
new file mode 100644
index 0000000..5d2b1f7
--- /dev/null
+++ b/OpenSim/Region/PhysicsModules/ubOdeMeshing/Mesh.cs
@@ -0,0 +1,636 @@
+/*
+ * Copyright (c) Contributors, http://opensimulator.org/
+ * See CONTRIBUTORS.TXT for a full list of copyright holders.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the OpenSimulator Project nor the
+ *       names of its contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+using System;
+using System.Collections.Generic;
+using System.IO;
+using System.Runtime.InteropServices;
+using OpenSim.Region.PhysicsModules.SharedBase;
+using PrimMesher;
+using OpenMetaverse;
+using System.Runtime.Serialization;
+using System.Runtime.Serialization.Formatters.Binary;
+
+namespace OpenSim.Region.PhysicsModule.ubODEMeshing
+{
+    public class MeshBuildingData
+    {
+        private class vertexcomp : IEqualityComparer<Vertex>
+        {
+            public bool Equals(Vertex v1, Vertex v2)
+            {
+                if (v1.X == v2.X && v1.Y == v2.Y && v1.Z == v2.Z)
+                    return true;
+                else
+                    return false;
+            }
+            public int GetHashCode(Vertex v)
+            {
+                int a = v.X.GetHashCode();
+                int b = v.Y.GetHashCode();
+                int c = v.Z.GetHashCode();
+                return (a << 16) ^ (b << 8) ^ c;
+            }
+        }
+
+        public Dictionary<Vertex, int> m_vertices;
+        public List<Triangle> m_triangles;
+        public float m_obbXmin;
+        public float m_obbXmax;
+        public float m_obbYmin;
+        public float m_obbYmax;
+        public float m_obbZmin;
+        public float m_obbZmax;
+        public Vector3 m_centroid;
+        public int m_centroidDiv;
+
+        public  MeshBuildingData()
+        {
+            vertexcomp vcomp = new vertexcomp();
+            m_vertices = new Dictionary<Vertex, int>(vcomp);
+            m_triangles = new List<Triangle>();
+            m_centroid = Vector3.Zero;
+            m_centroidDiv = 0;
+            m_obbXmin = float.MaxValue;
+            m_obbXmax = float.MinValue;
+            m_obbYmin = float.MaxValue;
+            m_obbYmax = float.MinValue;
+            m_obbZmin = float.MaxValue;
+            m_obbZmax = float.MinValue;
+        }
+    }
+
+    [Serializable()]
+    public class Mesh : IMesh
+    {
+        float[] vertices;
+        int[] indexes;
+        Vector3 m_obb;
+        Vector3 m_obboffset;
+        [NonSerialized()]
+        MeshBuildingData m_bdata;
+        [NonSerialized()]
+        GCHandle vhandler;
+        [NonSerialized()]
+        GCHandle ihandler;
+        [NonSerialized()]
+        IntPtr m_verticesPtr = IntPtr.Zero;
+        [NonSerialized()]
+        IntPtr m_indicesPtr = IntPtr.Zero;
+        [NonSerialized()]
+        int m_vertexCount = 0;
+        [NonSerialized()]
+        int m_indexCount = 0;
+
+        public int RefCount { get; set; }
+        public AMeshKey Key { get; set; }
+
+        public Mesh(bool forbuild)
+        {
+            if(forbuild)
+                m_bdata = new MeshBuildingData();
+            m_obb = new Vector3(0.5f, 0.5f, 0.5f);
+            m_obboffset = Vector3.Zero;
+        }
+
+        public Mesh Scale(Vector3 scale)
+        {
+            if (m_verticesPtr == null || m_indicesPtr == null)
+                return null;
+
+            Mesh result = new Mesh(false);
+
+            float x = scale.X;
+            float y = scale.Y;
+            float z = scale.Z;
+
+            float tmp;
+            tmp = m_obb.X * x;
+            if(tmp < 0.0005f)
+                tmp = 0.0005f;
+            result.m_obb.X = tmp;
+
+            tmp =  m_obb.Y * y;
+            if(tmp < 0.0005f)
+                tmp = 0.0005f;
+            result.m_obb.Y = tmp;
+
+            tmp =  m_obb.Z * z;
+            if(tmp < 0.0005f)
+                tmp = 0.0005f;
+            result.m_obb.Z = tmp;
+
+            result.m_obboffset.X = m_obboffset.X * x;
+            result.m_obboffset.Y = m_obboffset.Y * y;
+            result.m_obboffset.Z = m_obboffset.Z * z;
+
+            result.vertices = new float[vertices.Length];
+            int j = 0;
+            for (int i = 0; i < m_vertexCount; i++)
+            {
+                result.vertices[j] = vertices[j] * x;
+                j++;
+                result.vertices[j] = vertices[j] * y;
+                j++;
+                result.vertices[j] = vertices[j] * z;
+                j++;
+            }
+
+            result.indexes = new int[indexes.Length];
+            indexes.CopyTo(result.indexes,0);
+
+            result.pinMemory();
+
+            return result;
+        }
+
+        public Mesh Clone()
+        {
+            Mesh result = new Mesh(false);
+
+            if (m_bdata != null)
+            {
+                result.m_bdata = new MeshBuildingData();
+                foreach (Triangle t in m_bdata.m_triangles)
+                {
+                    result.Add(new Triangle(t.v1.Clone(), t.v2.Clone(), t.v3.Clone()));
+                }
+                result.m_bdata.m_centroid = m_bdata.m_centroid;
+                result.m_bdata.m_centroidDiv = m_bdata.m_centroidDiv;
+                result.m_bdata.m_obbXmin = m_bdata.m_obbXmin;
+                result.m_bdata.m_obbXmax = m_bdata.m_obbXmax;
+                result.m_bdata.m_obbYmin = m_bdata.m_obbYmin;
+                result.m_bdata.m_obbYmax = m_bdata.m_obbYmax;
+                result.m_bdata.m_obbZmin = m_bdata.m_obbZmin;
+                result.m_bdata.m_obbZmax = m_bdata.m_obbZmax;
+            }
+            result.m_obb = m_obb;
+            result.m_obboffset = m_obboffset;
+            return result;
+        }
+
+        public void addVertexLStats(Vertex v)
+        {
+            float x = v.X;
+            float y = v.Y;
+            float z = v.Z;
+
+            m_bdata.m_centroid.X += x;
+            m_bdata.m_centroid.Y += y;
+            m_bdata.m_centroid.Z += z;
+            m_bdata.m_centroidDiv++;
+
+            if (x > m_bdata.m_obbXmax)
+                m_bdata.m_obbXmax = x;
+            if (x < m_bdata.m_obbXmin)
+                m_bdata.m_obbXmin = x;
+
+            if (y > m_bdata.m_obbYmax)
+                m_bdata.m_obbYmax = y;
+            if (y < m_bdata.m_obbYmin)
+                m_bdata.m_obbYmin = y;
+
+            if (z > m_bdata.m_obbZmax)
+                m_bdata.m_obbZmax = z;
+            if (z < m_bdata.m_obbZmin)
+                m_bdata.m_obbZmin = z;
+
+        }
+
+        public void Add(Triangle triangle)
+        {
+            if (m_indicesPtr != IntPtr.Zero || m_verticesPtr != IntPtr.Zero)
+                throw new NotSupportedException("Attempt to Add to a pinned Mesh");
+
+
+            triangle.v1.X = (float)Math.Round(triangle.v1.X, 6);
+            triangle.v1.Y = (float)Math.Round(triangle.v1.Y, 6);
+            triangle.v1.Z = (float)Math.Round(triangle.v1.Z, 6);
+            triangle.v2.X = (float)Math.Round(triangle.v2.X, 6);
+            triangle.v2.Y = (float)Math.Round(triangle.v2.Y, 6);
+            triangle.v2.Z = (float)Math.Round(triangle.v2.Z, 6);
+            triangle.v3.X = (float)Math.Round(triangle.v3.X, 6);
+            triangle.v3.Y = (float)Math.Round(triangle.v3.Y, 6);
+            triangle.v3.Z = (float)Math.Round(triangle.v3.Z, 6);
+
+            if ((triangle.v1.X == triangle.v2.X && triangle.v1.Y == triangle.v2.Y && triangle.v1.Z ==
+                    triangle.v2.Z)
+                || (triangle.v1.X == triangle.v3.X && triangle.v1.Y == triangle.v3.Y && triangle.v1.Z ==
+                    triangle.v3.Z)
+                || (triangle.v2.X == triangle.v3.X && triangle.v2.Y == triangle.v3.Y && triangle.v2.Z ==
+                    triangle.v3.Z)
+                )
+            {
+                return;
+            }
+
+            if (m_bdata.m_vertices.Count == 0)
+            {
+                m_bdata.m_centroidDiv = 0;
+                m_bdata.m_centroid = Vector3.Zero;
+            }
+
+            if (!m_bdata.m_vertices.ContainsKey(triangle.v1))
+            {
+                m_bdata.m_vertices[triangle.v1] = m_bdata.m_vertices.Count;
+                addVertexLStats(triangle.v1);
+            }
+            if (!m_bdata.m_vertices.ContainsKey(triangle.v2))
+            {
+                m_bdata.m_vertices[triangle.v2] = m_bdata.m_vertices.Count;
+                addVertexLStats(triangle.v2);
+            }
+            if (!m_bdata.m_vertices.ContainsKey(triangle.v3))
+            {
+                m_bdata.m_vertices[triangle.v3] = m_bdata.m_vertices.Count;
+                addVertexLStats(triangle.v3);
+            }
+            m_bdata.m_triangles.Add(triangle);
+        }
+
+        public Vector3 GetCentroid()
+        {
+            return m_obboffset;
+
+        }
+
+        public Vector3 GetOBB()
+        {
+            return m_obb;
+/*
+            float x, y, z;
+            if (m_bdata.m_centroidDiv > 0)
+            {
+                x = (m_bdata.m_obbXmax - m_bdata.m_obbXmin) * 0.5f;
+                y = (m_bdata.m_obbYmax - m_bdata.m_obbYmin) * 0.5f;
+                z = (m_bdata.m_obbZmax - m_bdata.m_obbZmin) * 0.5f;
+            }
+            else // ??
+            {
+                x = 0.5f;
+                y = 0.5f;
+                z = 0.5f;
+            }
+            return new Vector3(x, y, z);
+*/
+        }
+
+        public int numberVertices()
+        {
+            return m_bdata.m_vertices.Count;
+        }
+
+        public int numberTriangles()
+        {
+            return m_bdata.m_triangles.Count;
+        }
+
+        public List<Vector3> getVertexList()
+        {
+            List<Vector3> result = new List<Vector3>();
+            foreach (Vertex v in m_bdata.m_vertices.Keys)
+            {
+                result.Add(new Vector3(v.X, v.Y, v.Z));
+            }
+            return result;
+        }
+
+        public float[] getVertexListAsFloat()
+        {
+            if (m_bdata.m_vertices == null)
+                throw new NotSupportedException();
+            float[] result = new float[m_bdata.m_vertices.Count * 3];
+            foreach (KeyValuePair<Vertex, int> kvp in m_bdata.m_vertices)
+            {
+                Vertex v = kvp.Key;
+                int i = kvp.Value;
+                result[3 * i + 0] = v.X;
+                result[3 * i + 1] = v.Y;
+                result[3 * i + 2] = v.Z;
+            }
+            return result;
+        }
+
+        public float[] getVertexListAsFloatLocked()
+        {
+            return null;
+        }
+
+        public void getVertexListAsPtrToFloatArray(out IntPtr _vertices, out int vertexStride, out int vertexCount)
+        {
+            // A vertex is 3 floats
+            vertexStride = 3 * sizeof(float);
+
+            // If there isn't an unmanaged array allocated yet, do it now
+            if (m_verticesPtr == IntPtr.Zero && m_bdata != null)
+            {
+                vertices = getVertexListAsFloat();
+                // Each vertex is 3 elements (floats)
+                m_vertexCount = vertices.Length / 3;
+                vhandler = GCHandle.Alloc(vertices, GCHandleType.Pinned);
+                m_verticesPtr = vhandler.AddrOfPinnedObject();
+                GC.AddMemoryPressure(Buffer.ByteLength(vertices));
+            }
+            _vertices = m_verticesPtr;
+            vertexCount = m_vertexCount;
+        }
+
+        public int[] getIndexListAsInt()
+        {
+            if (m_bdata.m_triangles == null)
+                throw new NotSupportedException();
+            int[] result = new int[m_bdata.m_triangles.Count * 3];
+            for (int i = 0; i < m_bdata.m_triangles.Count; i++)
+            {
+                Triangle t = m_bdata.m_triangles[i];
+                result[3 * i + 0] = m_bdata.m_vertices[t.v1];
+                result[3 * i + 1] = m_bdata.m_vertices[t.v2];
+                result[3 * i + 2] = m_bdata.m_vertices[t.v3];
+            }
+            return result;
+        }
+
+        /// <summary>
+        /// creates a list of index values that defines triangle faces. THIS METHOD FREES ALL NON-PINNED MESH DATA
+        /// </summary>
+        /// <returns></returns>
+        public int[] getIndexListAsIntLocked()
+        {
+            return null;
+        }
+
+        public void getIndexListAsPtrToIntArray(out IntPtr indices, out int triStride, out int indexCount)
+        {
+            // If there isn't an unmanaged array allocated yet, do it now
+            if (m_indicesPtr == IntPtr.Zero && m_bdata != null)
+            {
+                indexes = getIndexListAsInt();
+                m_indexCount = indexes.Length;
+                ihandler = GCHandle.Alloc(indexes, GCHandleType.Pinned);
+                m_indicesPtr = ihandler.AddrOfPinnedObject();
+                GC.AddMemoryPressure(Buffer.ByteLength(indexes));
+            }
+            // A triangle is 3 ints (indices)
+            triStride = 3 * sizeof(int);
+            indices = m_indicesPtr;
+            indexCount = m_indexCount;
+        }
+
+        public void releasePinned()
+        {
+            if (m_verticesPtr != IntPtr.Zero)
+            {
+                vhandler.Free();
+                GC.RemoveMemoryPressure(Buffer.ByteLength(vertices));
+                vertices = null;
+                m_verticesPtr = IntPtr.Zero;
+            }
+            if (m_indicesPtr != IntPtr.Zero)
+            {
+                ihandler.Free();
+                GC.RemoveMemoryPressure(Buffer.ByteLength(indexes));
+                indexes = null;
+                m_indicesPtr = IntPtr.Zero;
+            }
+        }
+
+        /// <summary>
+        /// frees up the source mesh data to minimize memory - call this method after calling get*Locked() functions
+        /// </summary>
+        public void releaseSourceMeshData()
+        {
+            if (m_bdata != null)
+            {
+                m_bdata.m_triangles = null;
+                m_bdata.m_vertices = null;
+            }
+        }
+
+        public void releaseBuildingMeshData()
+        {
+            if (m_bdata != null)
+            {
+                m_bdata.m_triangles = null;
+                m_bdata.m_vertices = null;
+                m_bdata = null;
+            }
+        }
+
+        public void Append(IMesh newMesh)
+        {
+            if (m_indicesPtr != IntPtr.Zero || m_verticesPtr != IntPtr.Zero)
+                throw new NotSupportedException("Attempt to Append to a pinned Mesh");
+
+            if (!(newMesh is Mesh))
+                return;
+
+            foreach (Triangle t in ((Mesh)newMesh).m_bdata.m_triangles)
+                Add(t);
+        }
+
+        // Do a linear transformation of  mesh.
+        public void TransformLinear(float[,] matrix, float[] offset)
+        {
+            if (m_indicesPtr != IntPtr.Zero || m_verticesPtr != IntPtr.Zero)
+                throw new NotSupportedException("Attempt to TransformLinear a pinned Mesh");
+
+            foreach (Vertex v in m_bdata.m_vertices.Keys)
+            {
+                if (v == null)
+                    continue;
+                float x, y, z;
+                x = v.X*matrix[0, 0] + v.Y*matrix[1, 0] + v.Z*matrix[2, 0];
+                y = v.X*matrix[0, 1] + v.Y*matrix[1, 1] + v.Z*matrix[2, 1];
+                z = v.X*matrix[0, 2] + v.Y*matrix[1, 2] + v.Z*matrix[2, 2];
+                v.X = x + offset[0];
+                v.Y = y + offset[1];
+                v.Z = z + offset[2];
+            }
+        }
+
+        public void DumpRaw(String path, String name, String title)
+        {
+            if (path == null)
+                return;
+            if (m_bdata == null)
+                return;
+            String fileName = name + "_" + title + ".raw";
+            String completePath = System.IO.Path.Combine(path, fileName);
+            StreamWriter sw = new StreamWriter(completePath);
+            foreach (Triangle t in m_bdata.m_triangles)
+            {
+                String s = t.ToStringRaw();
+                sw.WriteLine(s);
+            }
+            sw.Close();
+        }
+
+        public void TrimExcess()
+        {
+            m_bdata.m_triangles.TrimExcess();
+        }
+
+        public void pinMemory()
+        {
+            m_vertexCount = vertices.Length / 3;
+            vhandler = GCHandle.Alloc(vertices, GCHandleType.Pinned);
+            m_verticesPtr = vhandler.AddrOfPinnedObject();
+            GC.AddMemoryPressure(Buffer.ByteLength(vertices));
+
+            m_indexCount = indexes.Length;
+            ihandler = GCHandle.Alloc(indexes, GCHandleType.Pinned);
+            m_indicesPtr = ihandler.AddrOfPinnedObject();
+            GC.AddMemoryPressure(Buffer.ByteLength(indexes));
+        }
+
+        public void PrepForOde()
+        {
+            // If there isn't an unmanaged array allocated yet, do it now
+            if (m_verticesPtr == IntPtr.Zero)
+                vertices = getVertexListAsFloat();
+
+            // If there isn't an unmanaged array allocated yet, do it now
+            if (m_indicesPtr == IntPtr.Zero)
+                indexes = getIndexListAsInt();
+
+            float x, y, z;
+
+            if (m_bdata.m_centroidDiv > 0)
+            {
+                m_obboffset = new Vector3(m_bdata.m_centroid.X / m_bdata.m_centroidDiv, m_bdata.m_centroid.Y / m_bdata.m_centroidDiv, m_bdata.m_centroid.Z / m_bdata.m_centroidDiv);
+                x = (m_bdata.m_obbXmax - m_bdata.m_obbXmin) * 0.5f;
+                if(x < 0.0005f)
+                    x = 0.0005f;
+                y = (m_bdata.m_obbYmax - m_bdata.m_obbYmin) * 0.5f;
+                if(y < 0.0005f)
+                    y = 0.0005f;
+                z = (m_bdata.m_obbZmax - m_bdata.m_obbZmin) * 0.5f;
+                if(z < 0.0005f)
+                    z = 0.0005f;
+            }
+
+            else
+            {
+                m_obboffset = Vector3.Zero;
+                x = 0.5f;
+                y = 0.5f;
+                z = 0.5f;
+            }
+
+            m_obb = new Vector3(x, y, z);
+
+            releaseBuildingMeshData();
+            pinMemory();
+        }
+
+        public bool ToStream(Stream st)
+        {
+            if (m_indicesPtr == IntPtr.Zero || m_verticesPtr == IntPtr.Zero)
+                return false;
+
+            bool ok = true;
+
+            try
+            {
+                using(BinaryWriter bw = new BinaryWriter(st))
+                {
+                    bw.Write(m_vertexCount);
+                    bw.Write(m_indexCount);
+
+                    for (int i = 0; i < 3 * m_vertexCount; i++)
+                        bw.Write(vertices[i]);
+                    for (int i = 0; i < m_indexCount; i++)
+                        bw.Write(indexes[i]);
+                    bw.Write(m_obb.X);
+                    bw.Write(m_obb.Y);
+                    bw.Write(m_obb.Z);
+                    bw.Write(m_obboffset.X);
+                    bw.Write(m_obboffset.Y);
+                    bw.Write(m_obboffset.Z);
+                    bw.Flush();
+                    bw.Close();
+               }
+            }
+            catch
+            {
+                ok = false;
+            }
+
+            return ok;
+        }
+
+        public static Mesh FromStream(Stream st, AMeshKey key)
+        {
+            Mesh mesh = new Mesh(false);
+
+            bool ok = true;
+            try
+            {
+                using(BinaryReader br = new BinaryReader(st))
+                {
+                    mesh.m_vertexCount = br.ReadInt32();
+                    mesh.m_indexCount = br.ReadInt32();
+
+                    int n = 3 * mesh.m_vertexCount;
+                    mesh.vertices = new float[n];
+                    for (int i = 0; i < n; i++)
+                        mesh.vertices[i] = br.ReadSingle();
+
+                    mesh.indexes = new int[mesh.m_indexCount];
+                    for (int i = 0; i < mesh.m_indexCount; i++)
+                        mesh.indexes[i] = br.ReadInt32();
+
+                    mesh.m_obb.X = br.ReadSingle();
+                    mesh.m_obb.Y = br.ReadSingle();
+                    mesh.m_obb.Z = br.ReadSingle();
+
+                    mesh.m_obboffset.X = br.ReadSingle();
+                    mesh.m_obboffset.Y = br.ReadSingle();
+                    mesh.m_obboffset.Z = br.ReadSingle();
+                }
+            }
+            catch
+            {
+                ok = false;
+            }
+
+            if (ok)
+            {
+                mesh.pinMemory();
+
+                mesh.Key = key;
+                mesh.RefCount = 1;
+
+                return mesh;
+            }
+
+            mesh.vertices = null;
+            mesh.indexes = null;
+            return null;
+        }
+    }
+}
diff --git a/OpenSim/Region/PhysicsModules/ubOdeMeshing/Meshmerizer.cs b/OpenSim/Region/PhysicsModules/ubOdeMeshing/Meshmerizer.cs
new file mode 100644
index 0000000..032d4ed
--- /dev/null
+++ b/OpenSim/Region/PhysicsModules/ubOdeMeshing/Meshmerizer.cs
@@ -0,0 +1,1602 @@
+/*
+ * Copyright (c) Contributors, http://opensimulator.org/
+ * See CONTRIBUTORS.TXT for a full list of copyright holders.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the OpenSimulator Project nor the
+ *       names of its contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+//#define SPAM
+
+using System;
+using System.Collections.Generic;
+using OpenSim.Framework;
+using OpenSim.Region.Framework.Scenes;
+using OpenSim.Region.Framework.Interfaces;
+using OpenSim.Region.PhysicsModules.SharedBase;
+using OpenSim.Region.PhysicsModules.ConvexDecompositionDotNet;
+using OpenMetaverse;
+using OpenMetaverse.StructuredData;
+using System.Drawing;
+using System.Threading;
+using System.IO.Compression;
+using PrimMesher;
+using log4net;
+using Nini.Config;
+using System.Reflection;
+using System.IO;
+
+using Mono.Addins;
+
+namespace OpenSim.Region.PhysicsModule.ubODEMeshing
+{
+    [Extension(Path = "/OpenSim/RegionModules", NodeName = "RegionModule", Id = "ubODEMeshmerizer")]
+    public class ubMeshmerizer : IMesher, INonSharedRegionModule
+    {
+        private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
+
+        // Setting baseDir to a path will enable the dumping of raw files
+        // raw files can be imported by blender so a visual inspection of the results can be done
+
+        private static string cacheControlFilename = "cntr";
+        private bool m_Enabled = false;
+
+        public static object diskLock = new object();
+
+        public bool doMeshFileCache = true;
+        public bool doCacheExpire = true;
+        public string cachePath = "MeshCache";
+        public TimeSpan CacheExpire;
+
+//        const string baseDir = "rawFiles";
+        private const string baseDir = null; //"rawFiles";
+
+        private bool useMeshiesPhysicsMesh = true;
+        private bool doConvexPrims = true;
+        private bool doConvexSculpts = true;
+
+        private Dictionary<AMeshKey, Mesh> m_uniqueMeshes = new Dictionary<AMeshKey, Mesh>();
+        private Dictionary<AMeshKey, Mesh> m_uniqueReleasedMeshes = new Dictionary<AMeshKey, Mesh>();
+
+       #region INonSharedRegionModule
+        public string Name
+        {
+            get { return "ubODEMeshmerizer"; }
+        }
+
+        public Type ReplaceableInterface
+        {
+            get { return null; }
+        }
+
+        public void Initialise(IConfigSource config)
+        {
+            IConfig start_config = config.Configs["Startup"];
+
+            string mesher = start_config.GetString("meshing", string.Empty);
+            if (mesher == Name)
+            {
+                float fcache = 48.0f;
+                //            float fcache = 0.02f;
+
+                IConfig mesh_config = config.Configs["Mesh"];
+                if (mesh_config != null)
+                {
+                    useMeshiesPhysicsMesh = mesh_config.GetBoolean("UseMeshiesPhysicsMesh", useMeshiesPhysicsMesh);
+                    doConvexPrims = mesh_config.GetBoolean("ConvexPrims",doConvexPrims);
+                    doConvexSculpts = mesh_config.GetBoolean("ConvexSculpts",doConvexPrims);
+                    doMeshFileCache = mesh_config.GetBoolean("MeshFileCache", doMeshFileCache);
+                    cachePath = mesh_config.GetString("MeshFileCachePath", cachePath);
+                    fcache = mesh_config.GetFloat("MeshFileCacheExpireHours", fcache);
+                    doCacheExpire = mesh_config.GetBoolean("MeshFileCacheDoExpire", doCacheExpire);
+
+                    m_Enabled = true;
+                }
+
+                CacheExpire = TimeSpan.FromHours(fcache);
+
+                if(String.IsNullOrEmpty(cachePath))
+                    doMeshFileCache = false;
+
+                if(doMeshFileCache)
+                {
+                    if(!checkCache())
+                    {
+                        doMeshFileCache = false;
+                        doCacheExpire = false;
+                    }
+                }
+                else
+                    doCacheExpire = false;
+            }
+        }
+
+        public void Close()
+        {
+        }
+
+        public void AddRegion(Scene scene)
+        {
+            if (!m_Enabled)
+                return;
+
+            scene.RegisterModuleInterface<IMesher>(this);
+        }
+
+        public void RemoveRegion(Scene scene)
+        {
+            if (!m_Enabled)
+                return;
+
+            scene.UnregisterModuleInterface<IMesher>(this);
+        }
+
+        public void RegionLoaded(Scene scene)
+        {
+            if (!m_Enabled)
+                return;
+        }
+
+        #endregion
+
+        private void ReportPrimError(string message, string primName, PrimMesh primMesh)
+        {
+            m_log.Error(message);
+            m_log.Error("\nPrim Name: " + primName);
+            m_log.Error("****** PrimMesh Parameters ******\n" + primMesh.ParamsToDisplayString());
+        }
+
+        /// <summary>
+        /// Add a submesh to an existing list of coords and faces.
+        /// </summary>
+        /// <param name="subMeshData"></param>
+        /// <param name="size">Size of entire object</param>
+        /// <param name="coords"></param>
+        /// <param name="faces"></param>
+        private void AddSubMesh(OSDMap subMeshData, List<Coord> coords, List<Face> faces)
+        {
+            // Console.WriteLine("subMeshMap for {0} - {1}", primName, Util.GetFormattedXml((OSD)subMeshMap));
+
+            // As per http://wiki.secondlife.com/wiki/Mesh/Mesh_Asset_Format, some Mesh Level
+            // of Detail Blocks (maps) contain just a NoGeometry key to signal there is no
+            // geometry for this submesh.
+            if (subMeshData.ContainsKey("NoGeometry") && ((OSDBoolean)subMeshData["NoGeometry"]))
+                return;
+
+            OpenMetaverse.Vector3 posMax;
+            OpenMetaverse.Vector3 posMin;
+            if (subMeshData.ContainsKey("PositionDomain"))
+            {
+                posMax = ((OSDMap)subMeshData["PositionDomain"])["Max"].AsVector3();
+                posMin = ((OSDMap)subMeshData["PositionDomain"])["Min"].AsVector3();
+            }
+            else
+            {
+                posMax = new Vector3(0.5f, 0.5f, 0.5f);
+                posMin = new Vector3(-0.5f, -0.5f, -0.5f);
+            }
+
+            ushort faceIndexOffset = (ushort)coords.Count;
+
+            byte[] posBytes = subMeshData["Position"].AsBinary();
+            for (int i = 0; i < posBytes.Length; i += 6)
+            {
+                ushort uX = Utils.BytesToUInt16(posBytes, i);
+                ushort uY = Utils.BytesToUInt16(posBytes, i + 2);
+                ushort uZ = Utils.BytesToUInt16(posBytes, i + 4);
+
+                Coord c = new Coord(
+                Utils.UInt16ToFloat(uX, posMin.X, posMax.X),
+                Utils.UInt16ToFloat(uY, posMin.Y, posMax.Y),
+                Utils.UInt16ToFloat(uZ, posMin.Z, posMax.Z));
+
+                coords.Add(c);
+            }
+
+            byte[] triangleBytes = subMeshData["TriangleList"].AsBinary();
+            for (int i = 0; i < triangleBytes.Length; i += 6)
+            {
+                ushort v1 = (ushort)(Utils.BytesToUInt16(triangleBytes, i) + faceIndexOffset);
+                ushort v2 = (ushort)(Utils.BytesToUInt16(triangleBytes, i + 2) + faceIndexOffset);
+                ushort v3 = (ushort)(Utils.BytesToUInt16(triangleBytes, i + 4) + faceIndexOffset);
+                Face f = new Face(v1, v2, v3);
+                faces.Add(f);
+            }
+        }
+
+        /// <summary>
+        /// Create a physics mesh from data that comes with the prim.  The actual data used depends on the prim type.
+        /// </summary>
+        /// <param name="primName"></param>
+        /// <param name="primShape"></param>
+        /// <param name="size"></param>
+        /// <param name="lod"></param>
+        /// <returns></returns>
+        private Mesh CreateMeshFromPrimMesher(string primName, PrimitiveBaseShape primShape, float lod, bool convex)
+        {
+//            m_log.DebugFormat(
+//                "[MESH]: Creating physics proxy for {0}, shape {1}",
+//                primName, (OpenMetaverse.SculptType)primShape.SculptType);
+
+            List<Coord> coords;
+            List<Face> faces;
+            bool needsConvexProcessing = convex;
+
+            if (primShape.SculptEntry)
+            {
+                if (((OpenMetaverse.SculptType)primShape.SculptType) == SculptType.Mesh)
+                {
+                    if (!useMeshiesPhysicsMesh)
+                        return null;
+
+                    if (!GenerateCoordsAndFacesFromPrimMeshData(primName, primShape, out coords, out faces, convex))
+                        return null;
+                    needsConvexProcessing = false;
+                }
+                else
+                {
+                    if (!GenerateCoordsAndFacesFromPrimSculptData(primName, primShape, lod, out coords, out faces))
+                        return null;
+                    needsConvexProcessing &= doConvexSculpts;
+                }
+            }
+            else
+            {
+                if (!GenerateCoordsAndFacesFromPrimShapeData(primName, primShape, lod, convex, out coords, out faces))
+                    return null;
+                 needsConvexProcessing &= doConvexPrims;
+            }
+
+            int numCoords = coords.Count;
+            int numFaces = faces.Count;
+
+            if(numCoords < 3 || (!needsConvexProcessing && numFaces < 1))
+            {
+                m_log.ErrorFormat("[MESH]: invalid degenerated mesh for prim {0} ignored", primName);
+                return null;
+            }
+
+            if(needsConvexProcessing)
+            {
+                List<Coord> convexcoords;
+                List<Face> convexfaces;
+                if(CreateBoundingHull(coords, out convexcoords, out convexfaces) && convexcoords != null && convexfaces != null)
+                {
+                    coords.Clear();
+                    coords = convexcoords;
+                    numCoords = coords.Count;
+
+                    faces.Clear();
+                    faces = convexfaces;
+                    numFaces = faces.Count;
+                }
+                else
+                     m_log.ErrorFormat("[ubMESH]: failed to create convex for {0} using normal mesh", primName);
+            }
+
+            Mesh mesh = new Mesh(true);
+            // Add the corresponding triangles to the mesh
+            for (int i = 0; i < numFaces; i++)
+            {
+                Face f = faces[i];
+                mesh.Add(new Triangle(coords[f.v1].X, coords[f.v1].Y, coords[f.v1].Z,
+                                      coords[f.v2].X, coords[f.v2].Y, coords[f.v2].Z,
+                                      coords[f.v3].X, coords[f.v3].Y, coords[f.v3].Z));
+            }
+
+            coords.Clear();
+            faces.Clear();
+
+            if(mesh.numberVertices() < 3 || mesh.numberTriangles() < 1)
+            {
+                m_log.ErrorFormat("[MESH]: invalid degenerated mesh for prim {0} ignored", primName);
+                return null;
+            }
+
+            primShape.SculptData = Utils.EmptyBytes;
+
+            return mesh;
+        }
+
+        /// <summary>
+        /// Generate the co-ords and faces necessary to construct a mesh from the mesh data the accompanies a prim.
+        /// </summary>
+        /// <param name="primName"></param>
+        /// <param name="primShape"></param>
+        /// <param name="size"></param>
+        /// <param name="coords">Coords are added to this list by the method.</param>
+        /// <param name="faces">Faces are added to this list by the method.</param>
+        /// <returns>true if coords and faces were successfully generated, false if not</returns>
+        private bool GenerateCoordsAndFacesFromPrimMeshData(
+            string primName, PrimitiveBaseShape primShape, out List<Coord> coords, out List<Face> faces, bool convex)
+        {
+//            m_log.DebugFormat("[MESH]: experimental mesh proxy generation for {0}", primName);
+
+
+            // for ubOde we have a diferent mesh use priority
+            // priority is to use full mesh then decomposition
+            // SL does the oposite
+            bool usemesh = false;
+
+            coords = new List<Coord>();
+            faces = new List<Face>();
+            OSD meshOsd = null;
+
+            if (primShape.SculptData == null || primShape.SculptData.Length <= 0)
+            {
+//                m_log.InfoFormat("[MESH]: asset data for {0} is zero length", primName);
+                return false;
+            }
+
+            long start = 0;
+            using (MemoryStream data = new MemoryStream(primShape.SculptData))
+            {
+                try
+                {
+                    OSD osd = OSDParser.DeserializeLLSDBinary(data);
+                    if (osd is OSDMap)
+                        meshOsd = (OSDMap)osd;
+                    else
+                    {
+                        m_log.WarnFormat("[Mesh}: unable to cast mesh asset to OSDMap prim: {0}",primName);
+                        return false;
+                    }
+                }
+                catch (Exception e)
+                {
+                    m_log.Error("[MESH]: Exception deserializing mesh asset header:" + e.ToString());
+                    return false;
+                }
+
+                start = data.Position;
+            }
+
+            if (meshOsd is OSDMap)
+            {
+                OSDMap physicsParms = null;
+                OSDMap map = (OSDMap)meshOsd;
+
+                if (!convex)
+                {
+                    if (map.ContainsKey("physics_shape"))
+                        physicsParms = (OSDMap)map["physics_shape"]; // old asset format
+                    else if (map.ContainsKey("physics_mesh"))
+                        physicsParms = (OSDMap)map["physics_mesh"]; // new asset format
+
+                    if (physicsParms != null)
+                        usemesh = true;
+                }
+
+                if(!usemesh && (map.ContainsKey("physics_convex")))
+                        physicsParms = (OSDMap)map["physics_convex"];
+
+                if (physicsParms == null)
+                {
+                    //m_log.WarnFormat("[MESH]: unknown mesh type for prim {0}",primName);
+                    return false;
+                }
+
+                int physOffset = physicsParms["offset"].AsInteger() + (int)start;
+                int physSize = physicsParms["size"].AsInteger();
+
+                if (physOffset < 0 || physSize == 0)
+                    return false; // no mesh data in asset
+
+                OSD decodedMeshOsd = new OSD();
+                byte[] meshBytes = new byte[physSize];
+                System.Buffer.BlockCopy(primShape.SculptData, physOffset, meshBytes, 0, physSize);
+
+                try
+                {
+                    using (MemoryStream inMs = new MemoryStream(meshBytes))
+                    {
+                        using (MemoryStream outMs = new MemoryStream())
+                        {
+                            using (DeflateStream decompressionStream = new DeflateStream(inMs, CompressionMode.Decompress))
+                            {
+                                byte[] readBuffer = new byte[2048];
+                                inMs.Read(readBuffer, 0, 2); // skip first 2 bytes in header
+                                int readLen = 0;
+
+                                while ((readLen = decompressionStream.Read(readBuffer, 0, readBuffer.Length)) > 0)
+                                    outMs.Write(readBuffer, 0, readLen);
+
+                                outMs.Flush();
+                                outMs.Seek(0, SeekOrigin.Begin);
+
+                                byte[] decompressedBuf = outMs.GetBuffer();
+
+                                decodedMeshOsd = OSDParser.DeserializeLLSDBinary(decompressedBuf);
+                            }
+                        }
+                    }
+                }
+                catch (Exception e)
+                {
+                    m_log.Error("[MESH]: exception decoding physical mesh prim " + primName +" : " + e.ToString());
+                    return false;
+                }
+
+                if (usemesh)
+                {
+                    OSDArray decodedMeshOsdArray = null;
+
+                    // physics_shape is an array of OSDMaps, one for each submesh
+                    if (decodedMeshOsd is OSDArray)
+                    {
+//                      Console.WriteLine("decodedMeshOsd for {0} - {1}", primName, Util.GetFormattedXml(decodedMeshOsd));
+
+                        decodedMeshOsdArray = (OSDArray)decodedMeshOsd;
+                        foreach (OSD subMeshOsd in decodedMeshOsdArray)
+                        {
+                            if (subMeshOsd is OSDMap)
+                                AddSubMesh(subMeshOsd as OSDMap, coords, faces);
+                        }
+                    }
+                }
+                else
+                {
+                    OSDMap cmap = (OSDMap)decodedMeshOsd;
+                    if (cmap == null)
+                        return false;
+
+                    byte[] data;
+
+                    List<float3> vs = new List<float3>();
+                    PHullResult hullr = new PHullResult();
+                    float3 f3;
+                    Coord c;
+                    Face f;
+                    Vector3 range;
+                    Vector3 min;
+
+                    const float invMaxU16 = 1.0f / 65535f;
+                    int t1;
+                    int t2;
+                    int t3;
+                    int i;
+                    int nverts;
+                    int nindexs;
+
+                    if (cmap.ContainsKey("Max"))
+                        range = cmap["Max"].AsVector3();
+                    else
+                        range = new Vector3(0.5f, 0.5f, 0.5f);
+
+                    if (cmap.ContainsKey("Min"))
+                        min = cmap["Min"].AsVector3();
+                    else
+                        min = new Vector3(-0.5f, -0.5f, -0.5f);
+
+                    range = range - min;
+                    range *= invMaxU16;
+
+                    if(!convex)
+                    {
+                        // if mesh data not present and not convex then we need convex decomposition data
+                        if (cmap.ContainsKey("HullList") && cmap.ContainsKey("Positions"))
+                        {
+                            List<int> hsizes = new List<int>();
+                            int totalpoints = 0;
+                            data = cmap["HullList"].AsBinary();
+                            for (i = 0; i < data.Length; i++)
+                            {
+                                t1 = data[i];
+                                if (t1 == 0)
+                                    t1 = 256;
+                                totalpoints += t1;
+                                hsizes.Add(t1);
+                            }
+
+                            data = cmap["Positions"].AsBinary();
+                            int ptr = 0;
+                            int vertsoffset = 0;
+
+                            if (totalpoints == data.Length / 6) // 2 bytes per coord, 3 coords per point
+                            {
+                                foreach (int hullsize in hsizes)
+                                {
+                                    for (i = 0; i < hullsize; i++ )
+                                    {
+                                        t1 = data[ptr++];
+                                        t1 += data[ptr++] << 8;
+                                        t2 = data[ptr++];
+                                        t2 += data[ptr++] << 8;
+                                        t3 = data[ptr++];
+                                        t3 += data[ptr++] << 8;
+
+                                        f3 = new float3((t1 * range.X + min.X),
+                                                  (t2 * range.Y + min.Y),
+                                                  (t3 * range.Z + min.Z));
+                                        vs.Add(f3);
+                                    }
+
+                                    if(hullsize <3)
+                                    {
+                                        vs.Clear();
+                                        continue;
+                                    }
+
+                                    if (hullsize <5)
+                                    {
+                                        foreach (float3 point in vs)
+                                        {
+                                            c.X = point.x;
+                                            c.Y = point.y;
+                                            c.Z = point.z;
+                                            coords.Add(c);
+                                        }
+                                        f = new Face(vertsoffset, vertsoffset + 1, vertsoffset + 2);
+                                        faces.Add(f);
+
+                                        if (hullsize == 4)
+                                        {
+                                            // not sure about orientation..
+                                            f = new Face(vertsoffset, vertsoffset + 2, vertsoffset + 3);
+                                            faces.Add(f);
+                                            f = new Face(vertsoffset, vertsoffset + 3, vertsoffset + 1);
+                                            faces.Add(f);
+                                            f = new Face(vertsoffset + 3, vertsoffset + 2, vertsoffset + 1);
+                                            faces.Add(f);
+                                        }
+                                        vertsoffset += vs.Count;
+                                        vs.Clear();
+                                        continue;
+                                    }
+
+                                    List<int> indices;
+                                    if (!HullUtils.ComputeHull(vs, out indices))
+                                    {
+                                        vs.Clear();
+                                        continue;
+                                    }
+
+                                    nverts = vs.Count;
+                                    nindexs = indices.Count;
+
+                                    if (nindexs % 3 != 0)
+                                    {
+                                        vs.Clear();
+                                        continue;
+                                    }
+
+                                    for (i = 0; i < nverts; i++)
+                                    {
+                                        c.X = vs[i].x;
+                                        c.Y = vs[i].y;
+                                        c.Z = vs[i].z;
+                                        coords.Add(c);
+                                    }
+
+                                    for (i = 0; i < nindexs; i += 3)
+                                    {
+                                        t1 = indices[i];
+                                        if (t1 > nverts)
+                                            break;
+                                        t2 = indices[i + 1];
+                                        if (t2 > nverts)
+                                            break;
+                                        t3 = indices[i + 2];
+                                        if (t3 > nverts)
+                                            break;
+                                        f = new Face(vertsoffset + t1, vertsoffset + t2, vertsoffset + t3);
+                                        faces.Add(f);
+                                    }
+                                    vertsoffset += nverts;
+                                    vs.Clear();
+                                }
+                            }
+                            if (coords.Count > 0 && faces.Count > 0)
+                                return true;
+                        }
+                        else
+                        {
+                            // if neither mesh or decomposition present, warn and use convex
+                            //m_log.WarnFormat("[MESH]: Data for PRIM shape type ( mesh or decomposition) not found for prim {0}",primName);
+                        }
+                    }
+                    vs.Clear();
+
+                    if (cmap.ContainsKey("BoundingVerts"))
+                    {
+                        data = cmap["BoundingVerts"].AsBinary();
+
+                        for (i = 0; i < data.Length; )
+                        {
+                            t1 = data[i++];
+                            t1 += data[i++] << 8;
+                            t2 = data[i++];
+                            t2 += data[i++] << 8;
+                            t3 = data[i++];
+                            t3 += data[i++] << 8;
+
+                            f3 = new float3((t1 * range.X + min.X),
+                                      (t2 * range.Y + min.Y),
+                                      (t3 * range.Z + min.Z));
+                            vs.Add(f3);
+                        }
+
+                        nverts = vs.Count;
+
+                        if (nverts < 3)
+                        {
+                            vs.Clear();
+                            return false;
+                        }
+
+                        if (nverts < 5)
+                        {
+                            foreach (float3 point in vs)
+                            {
+                                c.X = point.x;
+                                c.Y = point.y;
+                                c.Z = point.z;
+                                coords.Add(c);
+                            }
+
+                            f = new Face(0, 1, 2);
+                            faces.Add(f);
+
+                            if (nverts == 4)
+                            {
+                                f = new Face(0, 2, 3);
+                                faces.Add(f);
+                                f = new Face(0, 3, 1);
+                                faces.Add(f);
+                                f = new Face( 3, 2, 1);
+                                faces.Add(f);
+                            }
+                            vs.Clear();
+                            return true;
+                        }
+
+                        List<int> indices;
+                        if (!HullUtils.ComputeHull(vs, out indices))
+                            return false;
+
+                        nindexs = indices.Count;
+
+                        if (nindexs % 3 != 0)
+                            return false;
+
+                        for (i = 0; i < nverts; i++)
+                        {
+                            c.X = vs[i].x;
+                            c.Y = vs[i].y;
+                            c.Z = vs[i].z;
+                            coords.Add(c);
+                        }
+                        for (i = 0; i < nindexs; i += 3)
+                        {
+                            t1 = indices[i];
+                            if (t1 > nverts)
+                                break;
+                            t2 = indices[i + 1];
+                            if (t2 > nverts)
+                                break;
+                            t3 = indices[i + 2];
+                            if (t3 > nverts)
+                                break;
+                            f = new Face(t1, t2, t3);
+                            faces.Add(f);
+                        }
+                        vs.Clear();
+                        if (coords.Count > 0 && faces.Count > 0)
+                            return true;
+                    }
+                    else
+                        return false;
+                }
+            }
+
+            return true;
+        }
+
+        /// <summary>
+        /// Generate the co-ords and faces necessary to construct a mesh from the sculpt data the accompanies a prim.
+        /// </summary>
+        /// <param name="primName"></param>
+        /// <param name="primShape"></param>
+        /// <param name="size"></param>
+        /// <param name="lod"></param>
+        /// <param name="coords">Coords are added to this list by the method.</param>
+        /// <param name="faces">Faces are added to this list by the method.</param>
+        /// <returns>true if coords and faces were successfully generated, false if not</returns>
+        private bool GenerateCoordsAndFacesFromPrimSculptData(
+            string primName, PrimitiveBaseShape primShape, float lod, out List<Coord> coords, out List<Face> faces)
+        {
+            coords = new List<Coord>();
+            faces = new List<Face>();
+            PrimMesher.SculptMesh sculptMesh;
+            Image idata = null;
+
+                if (primShape.SculptData == null || primShape.SculptData.Length == 0)
+                    return false;
+
+                try
+                {
+                    OpenMetaverse.Imaging.ManagedImage unusedData;
+                    OpenMetaverse.Imaging.OpenJPEG.DecodeToImage(primShape.SculptData, out unusedData, out idata);
+
+                    unusedData = null;
+
+                    if (idata == null)
+                    {
+                        // In some cases it seems that the decode can return a null bitmap without throwing
+                        // an exception
+                        m_log.WarnFormat("[PHYSICS]: OpenJPEG decoded sculpt data for {0} to a null bitmap.  Ignoring.", primName);
+                        return false;
+                    }
+                }
+                catch (DllNotFoundException)
+                {
+                    m_log.Error("[PHYSICS]: OpenJpeg is not installed correctly on this system. Physics Proxy generation failed.  Often times this is because of an old version of GLIBC.  You must have version 2.4 or above!");
+                    return false;
+                }
+                catch (IndexOutOfRangeException)
+                {
+                    m_log.Error("[PHYSICS]: OpenJpeg was unable to decode this. Physics Proxy generation failed");
+                    return false;
+                }
+                catch (Exception ex)
+                {
+                    m_log.Error("[PHYSICS]: Unable to generate a Sculpty physics proxy. Sculpty texture decode failed: " + ex.Message);
+                    return false;
+                }
+
+            PrimMesher.SculptMesh.SculptType sculptType;
+            // remove mirror and invert bits
+            OpenMetaverse.SculptType pbsSculptType = ((OpenMetaverse.SculptType)(primShape.SculptType & 0x3f));
+            switch (pbsSculptType)
+            {
+                case OpenMetaverse.SculptType.Cylinder:
+                    sculptType = PrimMesher.SculptMesh.SculptType.cylinder;
+                    break;
+                case OpenMetaverse.SculptType.Plane:
+                    sculptType = PrimMesher.SculptMesh.SculptType.plane;
+                    break;
+                case OpenMetaverse.SculptType.Torus:
+                    sculptType = PrimMesher.SculptMesh.SculptType.torus;
+                    break;
+                case OpenMetaverse.SculptType.Sphere:
+                    sculptType = PrimMesher.SculptMesh.SculptType.sphere;
+                    break;
+                default:
+                    sculptType = PrimMesher.SculptMesh.SculptType.plane;
+                    break;
+            }
+
+            bool mirror = ((primShape.SculptType & 128) != 0);
+            bool invert = ((primShape.SculptType & 64) != 0);
+
+            sculptMesh = new PrimMesher.SculptMesh((Bitmap)idata, sculptType, (int)lod, mirror, invert);
+
+            idata.Dispose();
+
+//            sculptMesh.DumpRaw(baseDir, primName, "primMesh");
+
+            coords = sculptMesh.coords;
+            faces = sculptMesh.faces;
+
+            return true;
+        }
+
+        /// <summary>
+        /// Generate the co-ords and faces necessary to construct a mesh from the shape data the accompanies a prim.
+        /// </summary>
+        /// <param name="primName"></param>
+        /// <param name="primShape"></param>
+        /// <param name="size"></param>
+        /// <param name="coords">Coords are added to this list by the method.</param>
+        /// <param name="faces">Faces are added to this list by the method.</param>
+        /// <returns>true if coords and faces were successfully generated, false if not</returns>
+        private bool GenerateCoordsAndFacesFromPrimShapeData(
+                string primName, PrimitiveBaseShape primShape, float lod, bool convex,
+                out List<Coord> coords, out List<Face> faces)
+        {
+            PrimMesh primMesh;
+            coords = new List<Coord>();
+            faces = new List<Face>();
+
+            float pathShearX = primShape.PathShearX < 128 ? (float)primShape.PathShearX * 0.01f : (float)(primShape.PathShearX - 256) * 0.01f;
+            float pathShearY = primShape.PathShearY < 128 ? (float)primShape.PathShearY * 0.01f : (float)(primShape.PathShearY - 256) * 0.01f;
+            float pathBegin = (float)primShape.PathBegin * 2.0e-5f;
+            float pathEnd = 1.0f - (float)primShape.PathEnd * 2.0e-5f;
+            float pathScaleX = (float)(primShape.PathScaleX - 100) * 0.01f;
+            float pathScaleY = (float)(primShape.PathScaleY - 100) * 0.01f;
+
+            float profileBegin = (float)primShape.ProfileBegin * 2.0e-5f;
+            float profileEnd = 1.0f - (float)primShape.ProfileEnd * 2.0e-5f;
+
+            if (profileBegin < 0.0f)
+                profileBegin = 0.0f;
+
+            if (profileEnd < 0.02f)
+                profileEnd = 0.02f;
+            else if (profileEnd > 1.0f)
+                profileEnd = 1.0f;
+
+            if (profileBegin >= profileEnd)
+                profileBegin = profileEnd - 0.02f;
+
+            float profileHollow = (float)primShape.ProfileHollow * 2.0e-5f;
+            if(convex)
+                profileHollow = 0.0f;
+            else if (profileHollow > 0.95f)
+                profileHollow = 0.95f;
+
+            int sides = 4;
+            LevelOfDetail iLOD = (LevelOfDetail)lod;
+            byte profshape = (byte)(primShape.ProfileCurve & 0x07);
+
+            if (profshape == (byte)ProfileShape.EquilateralTriangle
+                || profshape == (byte)ProfileShape.IsometricTriangle
+                || profshape == (byte)ProfileShape.RightTriangle)
+                sides = 3;
+            else if (profshape == (byte)ProfileShape.Circle)
+            {
+                switch (iLOD)
+                {
+                    case LevelOfDetail.High:    sides = 24;     break;
+                    case LevelOfDetail.Medium:  sides = 12;     break;
+                    case LevelOfDetail.Low:     sides = 6;      break;
+                    case LevelOfDetail.VeryLow: sides = 3;      break;
+                    default:                    sides = 24;     break;
+                }
+            }
+            else if (profshape == (byte)ProfileShape.HalfCircle)
+            { // half circle, prim is a sphere
+                switch (iLOD)
+                {
+                    case LevelOfDetail.High:    sides = 24;     break;
+                    case LevelOfDetail.Medium:  sides = 12;     break;
+                    case LevelOfDetail.Low:     sides = 6;      break;
+                    case LevelOfDetail.VeryLow: sides = 3;      break;
+                    default:                    sides = 24;     break;
+                }
+
+                profileBegin = 0.5f * profileBegin + 0.5f;
+                profileEnd = 0.5f * profileEnd + 0.5f;
+            }
+
+            int hollowSides = sides;
+            if (primShape.HollowShape == HollowShape.Circle)
+            {
+                switch (iLOD)
+                {
+                    case LevelOfDetail.High:    hollowSides = 24;     break;
+                    case LevelOfDetail.Medium:  hollowSides = 12;     break;
+                    case LevelOfDetail.Low:     hollowSides = 6;      break;
+                    case LevelOfDetail.VeryLow: hollowSides = 3;      break;
+                    default:                    hollowSides = 24;     break;
+                }
+            }
+            else if (primShape.HollowShape == HollowShape.Square)
+                hollowSides = 4;
+            else if (primShape.HollowShape == HollowShape.Triangle)
+            {
+                if (profshape == (byte)ProfileShape.HalfCircle)
+                    hollowSides = 6;
+                else
+                    hollowSides = 3;
+            }
+
+            primMesh = new PrimMesh(sides, profileBegin, profileEnd, profileHollow, hollowSides);
+
+            if (primMesh.errorMessage != null)
+                if (primMesh.errorMessage.Length > 0)
+                    m_log.Error("[ERROR] " + primMesh.errorMessage);
+
+            primMesh.topShearX = pathShearX;
+            primMesh.topShearY = pathShearY;
+            primMesh.pathCutBegin = pathBegin;
+            primMesh.pathCutEnd = pathEnd;
+
+            if (primShape.PathCurve == (byte)Extrusion.Straight || primShape.PathCurve == (byte) Extrusion.Flexible)
+            {
+                primMesh.twistBegin = (primShape.PathTwistBegin * 18) / 10;
+                primMesh.twistEnd = (primShape.PathTwist * 18) / 10;
+                primMesh.taperX = pathScaleX;
+                primMesh.taperY = pathScaleY;
+
+#if SPAM
+            m_log.Debug("****** PrimMesh Parameters (Linear) ******\n" + primMesh.ParamsToDisplayString());
+#endif
+                try
+                {
+                    primMesh.ExtrudeLinear();
+                }
+                catch (Exception ex)
+                {
+                    ReportPrimError("Extrusion failure: exception: " + ex.ToString(), primName, primMesh);
+                    return false;
+                }
+            }
+            else
+            {
+                primMesh.holeSizeX = (200 - primShape.PathScaleX) * 0.01f;
+                primMesh.holeSizeY = (200 - primShape.PathScaleY) * 0.01f;
+                primMesh.radius = 0.01f * primShape.PathRadiusOffset;
+                primMesh.revolutions = 1.0f + 0.015f * primShape.PathRevolutions;
+                primMesh.skew = 0.01f * primShape.PathSkew;
+                primMesh.twistBegin = (primShape.PathTwistBegin * 36) / 10;
+                primMesh.twistEnd = (primShape.PathTwist * 36) / 10;
+                primMesh.taperX = primShape.PathTaperX * 0.01f;
+                primMesh.taperY = primShape.PathTaperY * 0.01f;
+
+                if(profshape == (byte)ProfileShape.HalfCircle)
+                {
+                    if(primMesh.holeSizeY < 0.01f)
+                        primMesh.holeSizeY = 0.01f;
+                    else if(primMesh.holeSizeY > 1.0f)
+                        primMesh.holeSizeY = 1.0f;
+                }
+
+#if SPAM
+            m_log.Debug("****** PrimMesh Parameters (Circular) ******\n" + primMesh.ParamsToDisplayString());
+#endif
+                try
+                {
+                    primMesh.ExtrudeCircular();
+                }
+                catch (Exception ex)
+                {
+                    ReportPrimError("Extrusion failure: exception: " + ex.ToString(), primName, primMesh);
+                    return false;
+                }
+            }
+
+//            primMesh.DumpRaw(baseDir, primName, "primMesh");
+
+            coords = primMesh.coords;
+            faces = primMesh.faces;
+
+            return true;
+        }
+
+        public AMeshKey GetMeshUniqueKey(PrimitiveBaseShape primShape, Vector3 size, byte lod, bool convex)
+        {
+            AMeshKey key = new AMeshKey();
+            Byte[] someBytes;
+
+            key.hashB = 5181;
+            key.hashC = 5181;
+            ulong hash = 5381;
+
+            if (primShape.SculptEntry)
+            {
+                key.uuid = primShape.SculptTexture;
+                key.hashC = mdjb2(key.hashC, primShape.SculptType);
+                key.hashC = mdjb2(key.hashC, primShape.PCode);
+            }
+            else
+            {
+                hash = mdjb2(hash, primShape.PathCurve);
+                hash = mdjb2(hash, (byte)primShape.HollowShape);
+                hash = mdjb2(hash, (byte)primShape.ProfileShape);
+                hash = mdjb2(hash, primShape.PathBegin);
+                hash = mdjb2(hash, primShape.PathEnd);
+                hash = mdjb2(hash, primShape.PathScaleX);
+                hash = mdjb2(hash, primShape.PathScaleY);
+                hash = mdjb2(hash, primShape.PathShearX);
+                key.hashA = hash;
+                hash = key.hashB;
+                hash = mdjb2(hash, primShape.PathShearY);
+                hash = mdjb2(hash, (byte)primShape.PathTwist);
+                hash = mdjb2(hash, (byte)primShape.PathTwistBegin);
+                hash = mdjb2(hash, (byte)primShape.PathRadiusOffset);
+                hash = mdjb2(hash, (byte)primShape.PathTaperX);
+                hash = mdjb2(hash, (byte)primShape.PathTaperY);
+                hash = mdjb2(hash, primShape.PathRevolutions);
+                hash = mdjb2(hash, (byte)primShape.PathSkew);
+                hash = mdjb2(hash, primShape.ProfileBegin);
+                hash = mdjb2(hash, primShape.ProfileEnd);
+                hash = mdjb2(hash, primShape.ProfileHollow);
+                hash = mdjb2(hash, primShape.PCode);
+                key.hashB = hash;
+            }
+
+            hash = key.hashC;
+
+            hash = mdjb2(hash, lod);
+
+            if (size == m_MeshUnitSize)
+            {
+                hash = hash << 8;
+                hash |= 8;
+            }
+            else
+            {
+                someBytes = size.GetBytes();
+                for (int i = 0; i < someBytes.Length; i++)
+                    hash = mdjb2(hash, someBytes[i]);
+                hash = hash << 8;
+            }
+
+            if (convex)
+                hash |= 4;
+
+            if (primShape.SculptEntry)
+            {
+                hash |= 1;
+                if (primShape.SculptType == (byte)SculptType.Mesh)
+                    hash |= 2;
+            }
+
+            key.hashC = hash;
+
+            return key;
+        }
+
+        private ulong mdjb2(ulong hash, byte c)
+        {
+            return ((hash << 5) + hash) + (ulong)c;
+        }
+
+        private ulong mdjb2(ulong hash, ushort c)
+        {
+            hash = ((hash << 5) + hash) + (ulong)((byte)c);
+            return ((hash << 5) + hash) + (ulong)(c >> 8);
+        }
+
+        public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod)
+        {
+            return CreateMesh(primName, primShape, size, lod, false,false,false);
+        }
+
+        public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical)
+        {
+            return CreateMesh(primName, primShape, size, lod, false,false,false);
+        }
+
+        public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool shouldCache, bool convex, bool forOde)
+        {
+            return CreateMesh(primName, primShape, size, lod, false, false, false);
+        }
+
+        public IMesh GetMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool convex)
+        {
+            Mesh mesh = null;
+
+            if (size.X < 0.01f) size.X = 0.01f;
+            if (size.Y < 0.01f) size.Y = 0.01f;
+            if (size.Z < 0.01f) size.Z = 0.01f;
+
+            AMeshKey key = GetMeshUniqueKey(primShape, size, (byte)lod, convex);
+            lock (m_uniqueMeshes)
+            {
+                m_uniqueMeshes.TryGetValue(key, out mesh);
+
+                if (mesh != null)
+                {
+                    mesh.RefCount++;
+                    return mesh;
+                }
+
+                // try to find a identical mesh on meshs recently released
+                lock (m_uniqueReleasedMeshes)
+                {
+                    m_uniqueReleasedMeshes.TryGetValue(key, out mesh);
+                    if (mesh != null)
+                    {
+                        m_uniqueReleasedMeshes.Remove(key);
+                        try
+                        {
+                            m_uniqueMeshes.Add(key, mesh);
+                        }
+                        catch { }
+                        mesh.RefCount = 1;
+                        return mesh;
+                    }
+                }
+            }
+            return null;
+        }
+
+        private static Vector3 m_MeshUnitSize = new Vector3(1.0f, 1.0f, 1.0f);
+
+        public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool convex, bool forOde)
+        {
+#if SPAM
+            m_log.DebugFormat("[MESH]: Creating mesh for {0}", primName);
+#endif
+
+            Mesh mesh = null;
+
+            if (size.X < 0.01f) size.X = 0.01f;
+            if (size.Y < 0.01f) size.Y = 0.01f;
+            if (size.Z < 0.01f) size.Z = 0.01f;
+
+            // try to find a identical mesh on meshs in use
+
+            AMeshKey key = GetMeshUniqueKey(primShape,size,(byte)lod, convex);
+
+            lock (m_uniqueMeshes)
+            {
+                m_uniqueMeshes.TryGetValue(key, out mesh);
+
+                if (mesh != null)
+                {
+                    mesh.RefCount++;
+                    return mesh;
+                }
+
+                // try to find a identical mesh on meshs recently released
+                lock (m_uniqueReleasedMeshes)
+                {
+                    m_uniqueReleasedMeshes.TryGetValue(key, out mesh);
+                    if (mesh != null)
+                    {
+                        m_uniqueReleasedMeshes.Remove(key);
+                        try
+                        {
+                            m_uniqueMeshes.Add(key, mesh);
+                        }
+                        catch { }
+                        mesh.RefCount = 1;
+                        return mesh;
+                    }
+                }
+            }
+
+            Mesh UnitMesh = null;
+            AMeshKey unitKey = GetMeshUniqueKey(primShape, m_MeshUnitSize, (byte)lod, convex);
+
+            lock (m_uniqueReleasedMeshes)
+            {
+                m_uniqueReleasedMeshes.TryGetValue(unitKey, out UnitMesh);
+                if (UnitMesh != null)
+                {
+                    UnitMesh.RefCount = 1;
+                }
+            }
+
+            if (UnitMesh == null && primShape.SculptEntry && doMeshFileCache)
+                UnitMesh = GetFromFileCache(unitKey);
+
+            if (UnitMesh == null)
+            {
+                UnitMesh = CreateMeshFromPrimMesher(primName, primShape, lod, convex);
+
+                if (UnitMesh == null)
+                    return null;
+
+                UnitMesh.DumpRaw(baseDir, unitKey.ToString(), "Z");
+
+                if (forOde)
+                {
+                    // force pinned mem allocation
+                    UnitMesh.PrepForOde();
+                }
+                else
+                    UnitMesh.TrimExcess();
+
+                UnitMesh.Key = unitKey;
+                UnitMesh.RefCount = 1;
+
+                if (doMeshFileCache && primShape.SculptEntry)
+                    StoreToFileCache(unitKey, UnitMesh);
+
+                lock (m_uniqueReleasedMeshes)
+                {
+                    try
+                    {
+                        m_uniqueReleasedMeshes.Add(unitKey, UnitMesh);
+                    }
+                    catch { }
+                }
+            }
+
+            mesh = UnitMesh.Scale(size);
+            mesh.Key = key;
+            mesh.RefCount = 1;
+            lock (m_uniqueMeshes)
+            {
+                try
+                {
+                    m_uniqueMeshes.Add(key, mesh);
+                }
+                catch { }
+            }
+
+            return mesh;
+        }
+
+        public void ReleaseMesh(IMesh imesh)
+        {
+            if (imesh == null)
+                return;
+
+            Mesh mesh = (Mesh)imesh;
+
+            lock (m_uniqueMeshes)
+            {
+                int curRefCount = mesh.RefCount;
+                curRefCount--;
+
+                if (curRefCount > 0)
+                {
+                    mesh.RefCount = curRefCount;
+                    return;
+                }
+
+                mesh.RefCount = 0;
+                m_uniqueMeshes.Remove(mesh.Key);
+                lock (m_uniqueReleasedMeshes)
+                {
+                    try
+                    {
+                        m_uniqueReleasedMeshes.Add(mesh.Key, mesh);
+                    }
+                    catch { }
+                }
+            }
+        }
+
+        public void ExpireReleaseMeshs()
+        {
+            if (m_uniqueReleasedMeshes.Count == 0)
+                return;
+
+            List<Mesh> meshstodelete = new List<Mesh>();
+            int refcntr;
+
+            lock (m_uniqueReleasedMeshes)
+            {
+                foreach (Mesh m in m_uniqueReleasedMeshes.Values)
+                {
+                    refcntr = m.RefCount;
+                    refcntr--;
+                    if (refcntr > -6)
+                        m.RefCount = refcntr;
+                    else
+                        meshstodelete.Add(m);
+                }
+
+                foreach (Mesh m in meshstodelete)
+                {
+                    m_uniqueReleasedMeshes.Remove(m.Key);
+                    m.releaseBuildingMeshData();
+                    m.releasePinned();
+                }
+            }
+        }
+
+        public void FileNames(AMeshKey key, out string dir, out string fullFileName)
+        {
+            string id = key.ToString();
+            string init = id.Substring(0, 1);
+            dir = System.IO.Path.Combine(cachePath, init);
+            fullFileName = System.IO.Path.Combine(dir, id);
+        }
+
+        public string FullFileName(AMeshKey key)
+        {
+            string id = key.ToString();
+            string init = id.Substring(0,1);
+            id = System.IO.Path.Combine(init, id);
+            id = System.IO.Path.Combine(cachePath, id);
+            return id;
+        }
+
+        private Mesh GetFromFileCache(AMeshKey key)
+        {
+            Mesh mesh = null;
+            string filename = FullFileName(key);
+            bool ok = true;
+
+            lock (diskLock)
+            {
+                if (File.Exists(filename))
+                {
+                    try
+                    {
+                        using(FileStream stream = File.Open(filename, FileMode.Open, FileAccess.Read, FileShare.Read))
+                        {
+//                            BinaryFormatter bformatter = new BinaryFormatter();
+                            mesh = Mesh.FromStream(stream,key);
+                        }
+
+                    }
+                    catch (Exception e)
+                    {
+                        ok = false;
+                        m_log.ErrorFormat(
+                            "[MESH CACHE]: Failed to get file {0}.  Exception {1} {2}",
+                            filename, e.Message, e.StackTrace);
+                    }
+
+                    try
+                    {
+                        if (mesh == null || !ok)
+                            File.Delete(filename);
+                        else
+                            File.SetLastAccessTimeUtc(filename, DateTime.UtcNow);
+                    }
+                    catch
+                    {
+                    }
+
+                }
+            }
+
+            return mesh;
+        }
+
+        private void StoreToFileCache(AMeshKey key, Mesh mesh)
+        {
+            bool ok = false;
+
+            // Make sure the target cache directory exists
+            string dir = String.Empty;
+            string filename = String.Empty;
+
+            FileNames(key, out dir, out filename);
+
+            lock (diskLock)
+            {
+                Stream stream = null;
+                try
+                {
+                    if (!Directory.Exists(dir))
+                    {
+                        Directory.CreateDirectory(dir);
+                    }
+
+                    stream = File.Open(filename, FileMode.Create);
+                    ok = mesh.ToStream(stream);
+                }
+                catch (IOException e)
+                {
+                    m_log.ErrorFormat(
+                        "[MESH CACHE]: Failed to write file {0}.  Exception {1} {2}.",
+                        filename, e.Message, e.StackTrace);
+                    ok = false;
+                }
+                finally
+                {
+                    if(stream != null)
+                        stream.Dispose();
+                }
+
+                if (!ok && File.Exists(filename))
+                {
+                    try
+                    {
+                        File.Delete(filename);
+                    }
+                    catch (IOException e)
+                    {
+                         m_log.ErrorFormat(
+                        "[MESH CACHE]: Failed to delete file {0}",filename);
+                    }
+                }
+            }
+        }
+
+        public void ExpireFileCache()
+        {
+            if (!doCacheExpire)
+                return;
+
+            string controlfile = System.IO.Path.Combine(cachePath, cacheControlFilename);
+
+            lock (diskLock)
+            {
+                try
+                {
+                    if (File.Exists(controlfile))
+                    {
+                        int ndeleted = 0;
+                        int totalfiles = 0;
+                        int ndirs = 0;
+                        DateTime OlderTime = File.GetLastAccessTimeUtc(controlfile) - CacheExpire;
+                        File.SetLastAccessTimeUtc(controlfile, DateTime.UtcNow);
+
+                        foreach (string dir in Directory.GetDirectories(cachePath))
+                        {
+                            try
+                            {
+                                foreach (string file in Directory.GetFiles(dir))
+                                {
+                                    try
+                                    {
+                                        if (File.GetLastAccessTimeUtc(file) < OlderTime)
+                                        {
+                                            File.Delete(file);
+                                            ndeleted++;
+                                        }
+                                    }
+                                    catch { }
+                                    totalfiles++;
+                                }
+                            }
+                            catch { }
+                            ndirs++;
+                        }
+
+                        if (ndeleted == 0)
+                            m_log.InfoFormat("[MESH CACHE]: {0} Files in {1} cache folders, no expires",
+                                totalfiles,ndirs);
+                        else
+                            m_log.InfoFormat("[MESH CACHE]: {0} Files in {1} cache folders, expired {2} files accessed before {3}",
+                                totalfiles,ndirs, ndeleted, OlderTime.ToString());
+                    }
+                    else
+                    {
+                        m_log.Info("[MESH CACHE]: Expire delayed to next startup");
+                        FileStream fs = File.Create(controlfile,4096,FileOptions.WriteThrough);
+                        fs.Close();
+                    }
+                }
+                catch { }
+            }
+        }
+
+        public bool checkCache()
+        {
+            string controlfile = System.IO.Path.Combine(cachePath, cacheControlFilename);
+            lock (diskLock)
+            {
+                try
+                {
+                    if (!Directory.Exists(cachePath))
+                    {
+                        Directory.CreateDirectory(cachePath);
+                        Thread.Sleep(100);
+                        FileStream fs = File.Create(controlfile, 4096, FileOptions.WriteThrough);
+                        fs.Close();
+                        return true;
+                    }
+                }
+                catch
+                {
+                    doMeshFileCache = false;
+                    doCacheExpire = false;
+                    return false;
+                }
+                finally {}
+
+                if (File.Exists(controlfile))
+                    return true;
+
+                try
+                {
+                    Directory.Delete(cachePath, true);
+                    while(Directory.Exists(cachePath))
+                        Thread.Sleep(100);
+                }
+                catch(Exception e)
+                {
+                    m_log.Error("[MESH CACHE]: failed to delete old version of the cache: " + e.Message);
+                    doMeshFileCache = false;
+                    doCacheExpire = false;
+                    return false;
+                } 
+                finally {}
+                try
+                {
+                    Directory.CreateDirectory(cachePath);
+                    while(!Directory.Exists(cachePath))
+                        Thread.Sleep(100);
+                }
+                catch(Exception e)
+                {
+                    m_log.Error("[MESH CACHE]: failed to create new cache folder: " + e.Message);
+                    doMeshFileCache = false;
+                    doCacheExpire = false;
+                    return false;
+                } 
+                finally {}
+
+                try
+                {
+                    FileStream fs = File.Create(controlfile, 4096, FileOptions.WriteThrough);
+                    fs.Close();
+                }
+                catch(Exception e)
+                {
+                    m_log.Error("[MESH CACHE]: failed to create new control file: " + e.Message);
+                    doMeshFileCache = false;
+                    doCacheExpire = false;
+                    return false;
+                } 
+                finally {}
+            
+                return true;
+            }
+        }
+
+        public bool CreateBoundingHull(List<Coord> inputVertices, out List<Coord> convexcoords, out List<Face> newfaces)
+        {
+            convexcoords = null;
+            newfaces = null;
+            HullDesc desc = new HullDesc();
+            HullResult result = new HullResult();
+
+            int nInputVerts = inputVertices.Count;
+            int i;
+
+            List<float3> vs = new List<float3>(nInputVerts);
+            float3 f3;
+
+            //useless copy
+            for(i = 0 ; i < nInputVerts; i++)
+            {
+                f3 = new float3(inputVertices[i].X, inputVertices[i].Y, inputVertices[i].Z);
+                vs.Add(f3);
+            }
+
+            desc.Vertices = vs;
+            desc.Flags = HullFlag.QF_TRIANGLES;
+            desc.MaxVertices = 256;
+
+            try
+            {
+                HullError ret = HullUtils.CreateConvexHull(desc, ref result);
+                if (ret != HullError.QE_OK)
+                    return false;
+                int nverts = result.OutputVertices.Count;
+                int nindx = result.Indices.Count;
+                if(nverts < 3 || nindx< 3)
+                    return false;
+                if(nindx % 3 != 0)
+                    return false;
+
+                convexcoords = new List<Coord>(nverts);
+                Coord c;
+                vs = result.OutputVertices;
+
+                for(i = 0 ; i < nverts; i++)
+                {
+                    c = new Coord(vs[i].x, vs[i].y, vs[i].z);
+                    convexcoords.Add(c);
+                }
+
+                newfaces = new List<Face>(nindx / 3);
+                List<int> indxs = result.Indices;
+                int k, l, m;
+                Face f;
+                for(i = 0 ; i < nindx;)
+                {
+                    k = indxs[i++];
+                    l = indxs[i++];
+                    m = indxs[i++];
+                    if(k > nInputVerts)
+                        continue;
+                    if(l > nInputVerts)
+                        continue;
+                    if(m > nInputVerts)
+                        continue;
+                    f = new Face(k,l,m);
+                    newfaces.Add(f);
+                }
+                return true;
+            }
+            catch
+            {
+
+                return false;
+            }
+            return false;
+        }
+    }
+}
diff --git a/OpenSim/Region/PhysicsModules/ubOdeMeshing/PrimMesher.cs b/OpenSim/Region/PhysicsModules/ubOdeMeshing/PrimMesher.cs
new file mode 100644
index 0000000..e93175f
--- /dev/null
+++ b/OpenSim/Region/PhysicsModules/ubOdeMeshing/PrimMesher.cs
@@ -0,0 +1,1707 @@
+/*
+ * Copyright (c) Contributors
+ * See CONTRIBUTORS.TXT for a full list of copyright holders.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the OpenSimulator Project nor the
+ *       names of its contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+using System;
+using System.Collections.Generic;
+using System.Text;
+using System.IO;
+
+namespace PrimMesher
+{
+    public struct Quat
+    {
+        /// <summary>X value</summary>
+        public float X;
+        /// <summary>Y value</summary>
+        public float Y;
+        /// <summary>Z value</summary>
+        public float Z;
+        /// <summary>W value</summary>
+        public float W;
+
+        public Quat(float x, float y, float z, float w)
+        {
+            X = x;
+            Y = y;
+            Z = z;
+            W = w;
+        }
+
+        public Quat(Coord axis, float angle)
+        {
+            axis = axis.Normalize();
+
+            angle *= 0.5f;
+            float c = (float)Math.Cos(angle);
+            float s = (float)Math.Sin(angle);
+
+            X = axis.X * s;
+            Y = axis.Y * s;
+            Z = axis.Z * s;
+            W = c;
+
+            Normalize();
+        }
+
+        public float Length()
+        {
+            return (float)Math.Sqrt(X * X + Y * Y + Z * Z + W * W);
+        }
+
+        public Quat Normalize()
+        {
+            const float MAG_THRESHOLD = 0.0000001f;
+            float mag = Length();
+
+            // Catch very small rounding errors when normalizing
+            if (mag > MAG_THRESHOLD)
+            {
+                float oomag = 1f / mag;
+                X *= oomag;
+                Y *= oomag;
+                Z *= oomag;
+                W *= oomag;
+            }
+            else
+            {
+                X = 0f;
+                Y = 0f;
+                Z = 0f;
+                W = 1f;
+            }
+
+            return this;
+        }
+
+        public static Quat operator *(Quat q1, Quat q2)
+        {
+            float x = q1.W * q2.X + q1.X * q2.W + q1.Y * q2.Z - q1.Z * q2.Y;
+            float y = q1.W * q2.Y - q1.X * q2.Z + q1.Y * q2.W + q1.Z * q2.X;
+            float z = q1.W * q2.Z + q1.X * q2.Y - q1.Y * q2.X + q1.Z * q2.W;
+            float w = q1.W * q2.W - q1.X * q2.X - q1.Y * q2.Y - q1.Z * q2.Z;
+            return new Quat(x, y, z, w);
+        }
+
+        public override string ToString()
+        {
+            return "< X: " + this.X.ToString() + ", Y: " + this.Y.ToString() + ", Z: " + this.Z.ToString() + ", W: " + this.W.ToString() + ">";
+        }
+    }
+
+    public struct Coord
+    {
+        public float X;
+        public float Y;
+        public float Z;
+
+        public Coord(float x, float y, float z)
+        {
+            this.X = x;
+            this.Y = y;
+            this.Z = z;
+        }
+
+        public float Length()
+        {
+            return (float)Math.Sqrt(this.X * this.X + this.Y * this.Y + this.Z * this.Z);
+        }
+
+        public Coord Invert()
+        {
+            this.X = -this.X;
+            this.Y = -this.Y;
+            this.Z = -this.Z;
+
+            return this;
+        }
+
+        public Coord Normalize()
+        {
+            const float MAG_THRESHOLD = 0.0000001f;
+            float mag = Length();
+
+            // Catch very small rounding errors when normalizing
+            if (mag > MAG_THRESHOLD)
+            {
+                float oomag = 1.0f / mag;
+                this.X *= oomag;
+                this.Y *= oomag;
+                this.Z *= oomag;
+            }
+            else
+            {
+                this.X = 0.0f;
+                this.Y = 0.0f;
+                this.Z = 0.0f;
+            }
+
+            return this;
+        }
+
+        public override string ToString()
+        {
+            return this.X.ToString() + " " + this.Y.ToString() + " " + this.Z.ToString();
+        }
+
+        public static Coord Cross(Coord c1, Coord c2)
+        {
+            return new Coord(
+                c1.Y * c2.Z - c2.Y * c1.Z,
+                c1.Z * c2.X - c2.Z * c1.X,
+                c1.X * c2.Y - c2.X * c1.Y
+                );
+        }
+
+        public static Coord operator +(Coord v, Coord a)
+        {
+            return new Coord(v.X + a.X, v.Y + a.Y, v.Z + a.Z);
+        }
+
+        public static Coord operator *(Coord v, Coord m)
+        {
+            return new Coord(v.X * m.X, v.Y * m.Y, v.Z * m.Z);
+        }
+
+        public static Coord operator *(Coord v, Quat q)
+        {
+            // From http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/
+
+            Coord c2 = new Coord(0.0f, 0.0f, 0.0f);
+
+            c2.X = q.W * q.W * v.X +
+                2f * q.Y * q.W * v.Z -
+                2f * q.Z * q.W * v.Y +
+                     q.X * q.X * v.X +
+                2f * q.Y * q.X * v.Y +
+                2f * q.Z * q.X * v.Z -
+                     q.Z * q.Z * v.X -
+                     q.Y * q.Y * v.X;
+
+            c2.Y =
+                2f * q.X * q.Y * v.X +
+                     q.Y * q.Y * v.Y +
+                2f * q.Z * q.Y * v.Z +
+                2f * q.W * q.Z * v.X -
+                     q.Z * q.Z * v.Y +
+                     q.W * q.W * v.Y -
+                2f * q.X * q.W * v.Z -
+                     q.X * q.X * v.Y;
+
+            c2.Z =
+                2f * q.X * q.Z * v.X +
+                2f * q.Y * q.Z * v.Y +
+                     q.Z * q.Z * v.Z -
+                2f * q.W * q.Y * v.X -
+                     q.Y * q.Y * v.Z +
+                2f * q.W * q.X * v.Y -
+                     q.X * q.X * v.Z +
+                     q.W * q.W * v.Z;
+
+            return c2;
+        }
+    }
+
+    public struct Face
+    {
+        public int primFace;
+
+        // vertices
+        public int v1;
+        public int v2;
+        public int v3;
+
+        public Face(int v1, int v2, int v3)
+        {
+            primFace = 0;
+
+            this.v1 = v1;
+            this.v2 = v2;
+            this.v3 = v3;
+        }
+
+        public Coord SurfaceNormal(List<Coord> coordList)
+        {
+            Coord c1 = coordList[this.v1];
+            Coord c2 = coordList[this.v2];
+            Coord c3 = coordList[this.v3];
+
+            Coord edge1 = new Coord(c2.X - c1.X, c2.Y - c1.Y, c2.Z - c1.Z);
+            Coord edge2 = new Coord(c3.X - c1.X, c3.Y - c1.Y, c3.Z - c1.Z);
+
+            return Coord.Cross(edge1, edge2).Normalize();
+        }
+    }
+
+    internal struct Angle
+    {
+        internal float angle;
+        internal float X;
+        internal float Y;
+
+        internal Angle(float angle, float x, float y)
+        {
+            this.angle = angle;
+            this.X = x;
+            this.Y = y;
+        }
+    }
+
+    internal class AngleList
+    {
+        private float iX, iY; // intersection point
+
+        private static Angle[] angles3 =
+        {
+            new Angle(0.0f, 1.0f, 0.0f),
+            new Angle(0.33333333333333333f, -0.5f, 0.86602540378443871f),
+            new Angle(0.66666666666666667f, -0.5f, -0.86602540378443837f),
+            new Angle(1.0f, 1.0f, 0.0f)
+        };
+
+        private static Angle[] angles4 =
+        {
+            new Angle(0.0f, 1.0f, 0.0f),
+            new Angle(0.25f, 0.0f, 1.0f),
+            new Angle(0.5f, -1.0f, 0.0f),
+            new Angle(0.75f, 0.0f, -1.0f),
+            new Angle(1.0f, 1.0f, 0.0f)
+        };
+
+        private static Angle[] angles6 =
+        {
+            new Angle(0.0f, 1.0f, 0.0f),
+            new Angle(0.16666666666666667f, 0.5f, 0.8660254037844386f),
+            new Angle(0.33333333333333333f, -0.5f, 0.86602540378443871f),
+            new Angle(0.5f, -1.0f, 0.0f),
+            new Angle(0.66666666666666667f, -0.5f, -0.86602540378443837f),
+            new Angle(0.83333333333333326f, 0.5f, -0.86602540378443904f),
+            new Angle(1.0f, 1.0f, 0.0f)
+        };
+
+        private static Angle[] angles12 =
+        {
+            new Angle(0.0f, 1.0f, 0.0f),
+            new Angle(0.083333333333333329f, 0.86602540378443871f, 0.5f),
+            new Angle(0.16666666666666667f, 0.5f, 0.8660254037844386f),
+            new Angle(0.25f, 0.0f, 1.0f),
+            new Angle(0.33333333333333333f, -0.5f, 0.86602540378443871f),
+            new Angle(0.41666666666666663f, -0.86602540378443849f, 0.5f),
+            new Angle(0.5f, -1.0f, 0.0f),
+            new Angle(0.58333333333333326f, -0.86602540378443882f, -0.5f),
+            new Angle(0.66666666666666667f, -0.5f, -0.86602540378443837f),
+            new Angle(0.75f, 0.0f, -1.0f),
+            new Angle(0.83333333333333326f, 0.5f, -0.86602540378443904f),
+            new Angle(0.91666666666666663f, 0.86602540378443837f, -0.5f),
+            new Angle(1.0f, 1.0f, 0.0f)
+        };
+
+        private static Angle[] angles24 =
+        {
+            new Angle(0.0f, 1.0f, 0.0f),
+            new Angle(0.041666666666666664f, 0.96592582628906831f, 0.25881904510252074f),
+            new Angle(0.083333333333333329f, 0.86602540378443871f, 0.5f),
+            new Angle(0.125f, 0.70710678118654757f, 0.70710678118654746f),
+            new Angle(0.16666666666666667f, 0.5f, 0.8660254037844386f),
+            new Angle(0.20833333333333331f, 0.25881904510252096f, 0.9659258262890682f),
+            new Angle(0.25f, 0.0f, 1.0f),
+            new Angle(0.29166666666666663f, -0.25881904510252063f, 0.96592582628906831f),
+            new Angle(0.33333333333333333f, -0.5f, 0.86602540378443871f),
+            new Angle(0.375f, -0.70710678118654746f, 0.70710678118654757f),
+            new Angle(0.41666666666666663f, -0.86602540378443849f, 0.5f),
+            new Angle(0.45833333333333331f, -0.9659258262890682f, 0.25881904510252102f),
+            new Angle(0.5f, -1.0f, 0.0f),
+            new Angle(0.54166666666666663f, -0.96592582628906842f, -0.25881904510252035f),
+            new Angle(0.58333333333333326f, -0.86602540378443882f, -0.5f),
+            new Angle(0.62499999999999989f, -0.70710678118654791f, -0.70710678118654713f),
+            new Angle(0.66666666666666667f, -0.5f, -0.86602540378443837f),
+            new Angle(0.70833333333333326f, -0.25881904510252152f, -0.96592582628906809f),
+            new Angle(0.75f, 0.0f, -1.0f),
+            new Angle(0.79166666666666663f, 0.2588190451025203f, -0.96592582628906842f),
+            new Angle(0.83333333333333326f, 0.5f, -0.86602540378443904f),
+            new Angle(0.875f, 0.70710678118654735f, -0.70710678118654768f),
+            new Angle(0.91666666666666663f, 0.86602540378443837f, -0.5f),
+            new Angle(0.95833333333333326f, 0.96592582628906809f, -0.25881904510252157f),
+            new Angle(1.0f, 1.0f, 0.0f)
+        };
+
+        private Angle interpolatePoints(float newPoint, Angle p1, Angle p2)
+        {
+            float m = (newPoint - p1.angle) / (p2.angle - p1.angle);
+            return new Angle(newPoint, p1.X + m * (p2.X - p1.X), p1.Y + m * (p2.Y - p1.Y));
+        }
+
+        private void intersection(double x1, double y1, double x2, double y2, double x3, double y3, double x4, double y4)
+        { // ref: http://local.wasp.uwa.edu.au/~pbourke/geometry/lineline2d/
+            double denom = (y4 - y3) * (x2 - x1) - (x4 - x3) * (y2 - y1);
+            double uaNumerator = (x4 - x3) * (y1 - y3) - (y4 - y3) * (x1 - x3);
+
+            if (denom != 0.0)
+            {
+                double ua = uaNumerator / denom;
+                iX = (float)(x1 + ua * (x2 - x1));
+                iY = (float)(y1 + ua * (y2 - y1));
+            }
+        }
+
+        internal List<Angle> angles;
+
+        internal void makeAngles(int sides, float startAngle, float stopAngle, bool hasCut)
+        {
+            angles = new List<Angle>();
+
+            const double twoPi = System.Math.PI * 2.0;
+            const float twoPiInv = (float)(1.0d / twoPi);
+
+            if (sides < 1)
+                throw new Exception("number of sides not greater than zero");
+            if (stopAngle <= startAngle)
+                throw new Exception("stopAngle not greater than startAngle");
+
+            if ((sides == 3 || sides == 4 || sides == 6 || sides == 12 || sides == 24))
+            {
+                startAngle *= twoPiInv;
+                stopAngle *= twoPiInv;
+
+                Angle[] sourceAngles;
+                switch (sides)
+                {
+                    case 3:
+                        sourceAngles = angles3;
+                        break;
+                    case 4:
+                        sourceAngles = angles4;
+                        break;
+                    case 6:
+                        sourceAngles = angles6;
+                        break;
+                    case 12:
+                        sourceAngles = angles12;
+                        break;
+                    default:
+                        sourceAngles = angles24;
+                        break;
+                }
+
+                int startAngleIndex = (int)(startAngle * sides);
+                int endAngleIndex = sourceAngles.Length - 1;
+
+                if (hasCut)
+                {
+                    if (stopAngle < 1.0f)
+                        endAngleIndex = (int)(stopAngle * sides) + 1;
+                    if (endAngleIndex == startAngleIndex)
+                        endAngleIndex++;
+
+                    for (int angleIndex = startAngleIndex; angleIndex < endAngleIndex + 1; angleIndex++)
+                    {
+                        angles.Add(sourceAngles[angleIndex]);
+                    }
+
+                    if (startAngle > 0.0f)
+                        angles[0] = interpolatePoints(startAngle, angles[0], angles[1]);
+
+                    if (stopAngle < 1.0f)
+                    {
+                        int lastAngleIndex = angles.Count - 1;
+                        angles[lastAngleIndex] = interpolatePoints(stopAngle, angles[lastAngleIndex - 1], angles[lastAngleIndex]);
+                    }
+                }
+                else
+                {
+                    for (int angleIndex = startAngleIndex; angleIndex < endAngleIndex; angleIndex++)
+                        angles.Add(sourceAngles[angleIndex]);
+                }
+            }
+            else
+            {
+                double stepSize = twoPi / sides;
+
+                int startStep = (int)(startAngle / stepSize);
+                double angle = stepSize * startStep;
+                int step = startStep;
+                double stopAngleTest = stopAngle;
+                if (stopAngle < twoPi)
+                {
+                    stopAngleTest = stepSize * ((int)(stopAngle / stepSize) + 1);
+                    if (stopAngleTest < stopAngle)
+                        stopAngleTest += stepSize;
+                    if (stopAngleTest > twoPi)
+                        stopAngleTest = twoPi;
+                }
+
+                while (angle <= stopAngleTest)
+                {
+                    Angle newAngle;
+                    newAngle.angle = (float)angle;
+                    newAngle.X = (float)System.Math.Cos(angle);
+                    newAngle.Y = (float)System.Math.Sin(angle);
+                    angles.Add(newAngle);
+                    step += 1;
+                    angle = stepSize * step;
+                }
+
+                if (startAngle > angles[0].angle)
+                {
+                    Angle newAngle;
+                    intersection(angles[0].X, angles[0].Y, angles[1].X, angles[1].Y, 0.0f, 0.0f, (float)Math.Cos(startAngle), (float)Math.Sin(startAngle));
+                    newAngle.angle = startAngle;
+                    newAngle.X = iX;
+                    newAngle.Y = iY;
+                    angles[0] = newAngle;
+                }
+
+                int index = angles.Count - 1;
+                if (stopAngle < angles[index].angle)
+                {
+                    Angle newAngle;
+                    intersection(angles[index - 1].X, angles[index - 1].Y, angles[index].X, angles[index].Y, 0.0f, 0.0f, (float)Math.Cos(stopAngle), (float)Math.Sin(stopAngle));
+                    newAngle.angle = stopAngle;
+                    newAngle.X = iX;
+                    newAngle.Y = iY;
+                    angles[index] = newAngle;
+                }
+            }
+        }
+    }
+
+    /// <summary>
+    /// generates a profile for extrusion
+    /// </summary>
+    public class Profile
+    {
+        private const float twoPi = 2.0f * (float)Math.PI;
+
+        public string errorMessage = null;
+
+        public List<Coord> coords;
+        public List<Face> faces;
+
+        // use these for making individual meshes for each prim face
+        public List<int> outerCoordIndices = null;
+        public List<int> hollowCoordIndices = null;
+
+        public int numOuterVerts = 0;
+        public int numHollowVerts = 0;
+
+        public int outerFaceNumber = -1;
+        public int hollowFaceNumber = -1;
+
+        public int bottomFaceNumber = 0;
+        public int numPrimFaces = 0;
+
+        public Profile()
+        {
+            coords = new List<Coord>();
+            faces = new List<Face>();
+        }
+
+        public Profile(int sides, float profileStart, float profileEnd, float hollow, int hollowSides, bool hasProfileCut, bool createFaces)
+        {
+            const float halfSqr2 = 0.7071067811866f;
+
+            coords = new List<Coord>();
+            faces = new List<Face>();
+
+            List<Coord> hollowCoords = new List<Coord>();
+
+            bool hasHollow = (hollow > 0.0f);
+
+            AngleList angles = new AngleList();
+            AngleList hollowAngles = new AngleList();
+
+            float xScale = 0.5f;
+            float yScale = 0.5f;
+            if (sides == 4)  // corners of a square are sqrt(2) from center
+            {
+                xScale = halfSqr2;
+                yScale = halfSqr2;
+            }
+
+            float startAngle = profileStart * twoPi;
+            float stopAngle = profileEnd * twoPi;
+
+            try { angles.makeAngles(sides, startAngle, stopAngle,hasProfileCut); }
+            catch (Exception ex)
+            {
+
+                errorMessage = "makeAngles failed: Exception: " + ex.ToString()
+                + "\nsides: " + sides.ToString() + " startAngle: " + startAngle.ToString() + " stopAngle: " + stopAngle.ToString();
+
+                return;
+            }
+
+            numOuterVerts = angles.angles.Count;
+
+            Angle angle;
+            Coord newVert = new Coord();
+
+            // flag to create as few triangles as possible for 3 or 4 side profile
+            bool simpleFace = (sides < 5 && !hasHollow && !hasProfileCut);
+
+            if (hasHollow)
+            {
+                if (sides == hollowSides)
+                    hollowAngles = angles;
+                else
+                {
+                    try { hollowAngles.makeAngles(hollowSides, startAngle, stopAngle, hasProfileCut); }
+                    catch (Exception ex)
+                    {
+                        errorMessage = "makeAngles failed: Exception: " + ex.ToString()
+                        + "\nsides: " + sides.ToString() + " startAngle: " + startAngle.ToString() + " stopAngle: " + stopAngle.ToString();
+
+                        return;
+                    }
+
+                    int numHollowAngles = hollowAngles.angles.Count;
+                    for (int i = 0; i < numHollowAngles; i++)
+                    {
+                        angle = hollowAngles.angles[i];
+                        newVert.X = hollow * xScale * angle.X;
+                        newVert.Y = hollow * yScale * angle.Y;
+                        newVert.Z = 0.0f;
+
+                        hollowCoords.Add(newVert);
+                    }
+                }
+                numHollowVerts = hollowAngles.angles.Count;
+            }
+            else if (!simpleFace)
+            {
+                Coord center = new Coord(0.0f, 0.0f, 0.0f);
+                this.coords.Add(center);
+            }
+
+            int numAngles = angles.angles.Count;
+            bool hollowsame = (hasHollow && hollowSides == sides);
+
+            for (int i = 0; i < numAngles; i++)
+            {
+                angle = angles.angles[i];
+                newVert.X = angle.X * xScale;
+                newVert.Y = angle.Y * yScale;
+                newVert.Z = 0.0f;
+                coords.Add(newVert);
+                if (hollowsame)
+                {
+                    newVert.X *= hollow;
+                    newVert.Y *= hollow;
+                    hollowCoords.Add(newVert);
+                }
+            }
+
+            if (hasHollow)
+            {
+                hollowCoords.Reverse();
+                coords.AddRange(hollowCoords);
+
+                if (createFaces)
+                {
+                    int numTotalVerts = numOuterVerts + numHollowVerts;
+
+                    if (numOuterVerts == numHollowVerts)
+                    {
+                        Face newFace = new Face();
+
+                        for (int coordIndex = 0; coordIndex < numOuterVerts - 1; coordIndex++)
+                        {
+                            newFace.v1 = coordIndex;
+                            newFace.v2 = coordIndex + 1;
+                            newFace.v3 = numTotalVerts - coordIndex - 1;
+                            faces.Add(newFace);
+
+                            newFace.v1 = coordIndex + 1;
+                            newFace.v2 = numTotalVerts - coordIndex - 2;
+                            newFace.v3 = numTotalVerts - coordIndex - 1;
+                            faces.Add(newFace);
+                        }
+                        if (!hasProfileCut)
+                        {
+                            newFace.v1 = numOuterVerts - 1;
+                            newFace.v2 = 0;
+                            newFace.v3 = numOuterVerts;
+                            faces.Add(newFace);
+
+                            newFace.v1 = 0;
+                            newFace.v2 = numTotalVerts - 1;
+                            newFace.v3 = numOuterVerts;
+                            faces.Add(newFace);
+                        }
+                    }
+                    else if (numOuterVerts < numHollowVerts)
+                    {
+                        Face newFace = new Face();
+                        int j = 0; // j is the index for outer vertices
+                        int i;
+                        int maxJ = numOuterVerts - 1;
+                        float curHollowAngle = 0;
+                        for (i = 0; i < numHollowVerts; i++) // i is the index for inner vertices
+                        {
+                            curHollowAngle = hollowAngles.angles[i].angle;
+                            if (j < maxJ)
+                            {
+                                if (angles.angles[j + 1].angle - curHollowAngle < curHollowAngle - angles.angles[j].angle + 0.000001f)
+                                {
+                                    newFace.v1 = numTotalVerts - i - 1;
+                                    newFace.v2 = j;
+                                    newFace.v3 = j + 1;
+                                    faces.Add(newFace);
+                                    j++;
+                                }
+                            }
+                            else
+                            {
+                                if (1.0f - curHollowAngle < curHollowAngle - angles.angles[j].angle + 0.000001f)
+                                    break;
+                            }
+
+                            newFace.v1 = j;
+                            newFace.v2 = numTotalVerts - i - 2;
+                            newFace.v3 = numTotalVerts - i - 1;
+
+                            faces.Add(newFace);
+                        }
+
+                        if (!hasProfileCut)
+                        {
+                            if (i == numHollowVerts)
+                            {
+                                newFace.v1 = numTotalVerts - numHollowVerts;
+                                newFace.v2 = maxJ;
+                                newFace.v3 = 0;
+
+                                faces.Add(newFace);
+                            }
+                            else
+                            {
+                                if (1.0f - curHollowAngle < curHollowAngle - angles.angles[maxJ].angle + 0.000001f)
+                                {
+                                    newFace.v1 = numTotalVerts - i - 1;
+                                    newFace.v2 = maxJ;
+                                    newFace.v3 = 0;
+
+                                    faces.Add(newFace);
+                                }
+
+                                for (; i < numHollowVerts - 1; i++)
+                                {
+                                    newFace.v1 = 0;
+                                    newFace.v2 = numTotalVerts - i - 2;
+                                    newFace.v3 = numTotalVerts - i - 1;
+
+                                    faces.Add(newFace);
+                                }
+                            }
+
+                            newFace.v1 = 0;
+                            newFace.v2 = numTotalVerts - 1;
+                            newFace.v3 = numTotalVerts - numHollowVerts;
+                            faces.Add(newFace);
+                        }
+                    }
+                    else // numHollowVerts < numOuterVerts
+                    {
+                        Face newFace = new Face();
+                        int j = 0; // j is the index for inner vertices
+                        int maxJ = numHollowVerts - 1;
+                        for (int i = 0; i < numOuterVerts; i++)
+                        {
+                            if (j < maxJ)
+                                if (hollowAngles.angles[j + 1].angle - angles.angles[i].angle < angles.angles[i].angle - hollowAngles.angles[j].angle + 0.000001f)
+                                {
+                                    newFace.v1 = i;
+                                    newFace.v2 = numTotalVerts - j - 2;
+                                    newFace.v3 = numTotalVerts - j - 1;
+
+                                    faces.Add(newFace);
+                                    j += 1;
+                                }
+
+                            newFace.v1 = numTotalVerts - j - 1;
+                            newFace.v2 = i;
+                            newFace.v3 = i + 1;
+
+                            faces.Add(newFace);
+                        }
+
+                        if (!hasProfileCut)
+                        {
+                            int i = numOuterVerts - 1;
+
+                            if (hollowAngles.angles[0].angle - angles.angles[i].angle < angles.angles[i].angle - hollowAngles.angles[maxJ].angle + 0.000001f)
+                            {
+                                newFace.v1 = 0;
+                                newFace.v2 = numTotalVerts - 1;
+                                newFace.v3 = numTotalVerts - maxJ - 1;
+
+                                faces.Add(newFace);
+                            }
+
+                            newFace.v1 = numTotalVerts - maxJ - 1;
+                            newFace.v2 = i;
+                            newFace.v3 = 0;
+
+                            faces.Add(newFace);
+                        }
+                    }
+                }
+
+            }
+
+            else if (createFaces)
+            {
+                if (simpleFace)
+                {
+                    if (sides == 3)
+                        faces.Add(new Face(0, 1, 2));
+                    else if (sides == 4)
+                    {
+                        faces.Add(new Face(0, 1, 2));
+                        faces.Add(new Face(0, 2, 3));
+                    }
+                }
+                else
+                {
+                    for (int i = 1; i < numAngles ; i++)
+                    {
+                        Face newFace = new Face();
+                        newFace.v1 = 0;
+                        newFace.v2 = i;
+                        newFace.v3 = i + 1;
+                        faces.Add(newFace);
+                    }
+                    if (!hasProfileCut)
+                    {
+                        Face newFace = new Face();
+                        newFace.v1 = 0;
+                        newFace.v2 = numAngles;
+                        newFace.v3 = 1;
+                        faces.Add(newFace);
+                    }
+                }
+            }
+
+
+            hollowCoords = null;
+        }
+
+
+        public Profile Copy()
+        {
+            return Copy(true);
+        }
+
+        public Profile Copy(bool needFaces)
+        {
+            Profile copy = new Profile();
+
+            copy.coords.AddRange(coords);
+
+            if (needFaces)
+                copy.faces.AddRange(faces);
+
+            copy.numOuterVerts = numOuterVerts;
+            copy.numHollowVerts = numHollowVerts;
+
+            return copy;
+        }
+
+        public void AddPos(Coord v)
+        {
+            this.AddPos(v.X, v.Y, v.Z);
+        }
+
+        public void AddPos(float x, float y, float z)
+        {
+            int i;
+            int numVerts = coords.Count;
+            Coord vert;
+
+            for (i = 0; i < numVerts; i++)
+            {
+                vert = coords[i];
+                vert.X += x;
+                vert.Y += y;
+                vert.Z += z;
+                this.coords[i] = vert;
+            }
+        }
+
+        public void AddRot(Quat q)
+        {
+            int i;
+            int numVerts = coords.Count;
+
+            for (i = 0; i < numVerts; i++)
+                coords[i] *= q;
+        }
+
+        public void Scale(float x, float y)
+        {
+            int i;
+            int numVerts = coords.Count;
+            Coord vert;
+
+            for (i = 0; i < numVerts; i++)
+            {
+                vert = coords[i];
+                vert.X *= x;
+                vert.X = (float)Math.Round(vert.X,5);
+                vert.Y *= y;
+                vert.Y = (float)Math.Round(vert.Y,5);
+                coords[i] = vert;
+            }
+
+            if(x == 0f || y == 0f)
+                faces = new List<Face>();
+        }
+
+        /// <summary>
+        /// Changes order of the vertex indices and negates the center vertex normal. Does not alter vertex normals of radial vertices
+        /// </summary>
+        public void FlipNormals()
+        {
+            int numFaces = faces.Count;
+            if(numFaces == 0)
+                return;
+
+            int i;
+            Face tmpFace;
+            int tmp;
+
+            for (i = 0; i < numFaces; i++)
+            {
+                tmpFace = faces[i];
+                tmp = tmpFace.v3;
+                tmpFace.v3 = tmpFace.v1;
+                tmpFace.v1 = tmp;
+                faces[i] = tmpFace;
+            }
+        }
+
+        public void AddValue2FaceVertexIndices(int num)
+        {
+            int numFaces = faces.Count;
+            if(numFaces == 0)
+                return;
+
+            Face tmpFace;
+
+            for (int i = 0; i < numFaces; i++)
+            {
+                tmpFace = faces[i];
+                tmpFace.v1 += num;
+                tmpFace.v2 += num;
+                tmpFace.v3 += num;
+
+                faces[i] = tmpFace;
+            }
+        }
+
+         public void DumpRaw(String path, String name, String title)
+        {
+            if (path == null)
+                return;
+            String fileName = name + "_" + title + ".raw";
+            String completePath = System.IO.Path.Combine(path, fileName);
+            StreamWriter sw = new StreamWriter(completePath);
+
+            for (int i = 0; i < faces.Count; i++)
+            {
+                string s = coords[faces[i].v1].ToString();
+                s += " " + coords[faces[i].v2].ToString();
+                s += " " + coords[faces[i].v3].ToString();
+
+                sw.WriteLine(s);
+            }
+
+            sw.Close();
+        }
+    }
+
+    public struct PathNode
+    {
+        public Coord position;
+        public Quat rotation;
+        public float xScale;
+        public float yScale;
+        public float percentOfPath;
+    }
+
+    public enum PathType { Linear = 0, Circular = 1, Flexible = 2 }
+
+    public class Path
+    {
+        public List<PathNode> pathNodes = new List<PathNode>();
+
+        public float twistBegin = 0.0f;
+        public float twistEnd = 0.0f;
+        public float topShearX = 0.0f;
+        public float topShearY = 0.0f;
+        public float pathCutBegin = 0.0f;
+        public float pathCutEnd = 1.0f;
+        public float dimpleBegin = 0.0f;
+        public float dimpleEnd = 1.0f;
+        public float skew = 0.0f;
+        public float holeSizeX = 1.0f; // called pathScaleX in pbs
+        public float holeSizeY = 0.25f;
+        public float taperX = 0.0f;
+        public float taperY = 0.0f;
+        public float radius = 0.0f;
+        public float revolutions = 1.0f;
+        public int stepsPerRevolution = 24;
+
+        private const float twoPi = 2.0f * (float)Math.PI;
+
+        public void Create(PathType pathType, int steps)
+        {
+            if (taperX > .9999f)
+                taperX = 1.0f;
+            else if (taperX < -.9999f)
+                taperX = -1.0f;
+            if (taperY > .9999f)
+                taperY = 1.0f;
+            else if (taperY < -.9999f)
+                taperY = -1.0f;
+
+            if (pathType == PathType.Linear || pathType == PathType.Flexible)
+            {
+                int step = 0;
+
+                float length = pathCutEnd - pathCutBegin;
+                float twistTotal = twistEnd - twistBegin;
+                float twistTotalAbs = Math.Abs(twistTotal);
+                if (twistTotalAbs > 0.01f)
+                    steps += (int)(twistTotalAbs * 3.66); //  dahlia's magic number
+
+                float start = -0.5f;
+                float stepSize = length / (float)steps;
+                float percentOfPathMultiplier = stepSize * 0.999999f;
+                float xOffset = topShearX * pathCutBegin;
+                float yOffset = topShearY * pathCutBegin;
+                float zOffset = start;
+                float xOffsetStepIncrement = topShearX * length / steps;
+                float yOffsetStepIncrement = topShearY * length / steps;
+
+                float percentOfPath = pathCutBegin;
+                zOffset += percentOfPath;
+
+                // sanity checks
+
+                bool done = false;
+
+                while (!done)
+                {
+                    PathNode newNode = new PathNode();
+
+                    newNode.xScale = 1.0f;
+                    if (taperX > 0.0f)
+                        newNode.xScale -= percentOfPath * taperX;
+                    else if(taperX < 0.0f)
+                        newNode.xScale += (1.0f - percentOfPath) * taperX;
+
+                    newNode.yScale = 1.0f;
+                    if (taperY > 0.0f)
+                        newNode.yScale -= percentOfPath * taperY;
+                    else if(taperY < 0.0f)
+                        newNode.yScale += (1.0f - percentOfPath) * taperY;
+
+                    float twist = twistBegin + twistTotal * percentOfPath;
+
+                    newNode.rotation = new Quat(new Coord(0.0f, 0.0f, 1.0f), twist);
+                    newNode.position = new Coord(xOffset, yOffset, zOffset);
+                    newNode.percentOfPath = percentOfPath;
+
+                    pathNodes.Add(newNode);
+
+                    if (step < steps)
+                    {
+                        step += 1;
+                        percentOfPath += percentOfPathMultiplier;
+                        xOffset += xOffsetStepIncrement;
+                        yOffset += yOffsetStepIncrement;
+                        zOffset += stepSize;
+                        if (percentOfPath > pathCutEnd)
+                            done = true;
+                    }
+                    else done = true;
+                }
+            } // end of linear path code
+
+            else // pathType == Circular
+            {
+                float twistTotal = twistEnd - twistBegin;
+
+                // if the profile has a lot of twist, add more layers otherwise the layers may overlap
+                // and the resulting mesh may be quite inaccurate. This method is arbitrary and doesn't
+                // accurately match the viewer
+                float twistTotalAbs = Math.Abs(twistTotal);
+                if (twistTotalAbs > 0.01f)
+                {
+                    if (twistTotalAbs > Math.PI * 1.5f)
+                        steps *= 2;
+                    if (twistTotalAbs > Math.PI * 3.0f)
+                        steps *= 2;
+                }
+
+                float yPathScale = holeSizeY * 0.5f;
+                float pathLength = pathCutEnd - pathCutBegin;
+                float totalSkew = skew * 2.0f * pathLength;
+                float skewStart = pathCutBegin * 2.0f * skew - skew;
+                float xOffsetTopShearXFactor = topShearX * (0.25f + 0.5f * (0.5f - holeSizeY));
+                float yShearCompensation = 1.0f + Math.Abs(topShearY) * 0.25f;
+
+                // It's not quite clear what pushY (Y top shear) does, but subtracting it from the start and end
+                // angles appears to approximate it's effects on path cut. Likewise, adding it to the angle used
+                // to calculate the sine for generating the path radius appears to approximate it's effects there
+                // too, but there are some subtle differences in the radius which are noticeable as the prim size
+                // increases and it may affect megaprims quite a bit. The effect of the Y top shear parameter on
+                // the meshes generated with this technique appear nearly identical in shape to the same prims when
+                // displayed by the viewer.
+
+                float startAngle = (twoPi * pathCutBegin * revolutions) - topShearY * 0.9f;
+                float endAngle = (twoPi * pathCutEnd * revolutions) - topShearY * 0.9f;
+                float stepSize = twoPi / stepsPerRevolution;
+
+                int step = (int)(startAngle / stepSize);
+                float angle = startAngle;
+
+                bool done = false;
+                while (!done) // loop through the length of the path and add the layers
+                {
+                    PathNode newNode = new PathNode();
+
+                    float xProfileScale = (1.0f - Math.Abs(skew)) * holeSizeX;
+                    float yProfileScale = holeSizeY;
+
+                    float percentOfPath = angle / (twoPi * revolutions);
+                    float percentOfAngles = (angle - startAngle) / (endAngle - startAngle);
+
+                    if (taperX > 0.01f)
+                        xProfileScale *= 1.0f - percentOfPath * taperX;
+                    else if (taperX < -0.01f)
+                        xProfileScale *= 1.0f + (1.0f - percentOfPath) * taperX;
+
+                    if (taperY > 0.01f)
+                        yProfileScale *= 1.0f - percentOfPath * taperY;
+                    else if (taperY < -0.01f)
+                        yProfileScale *= 1.0f + (1.0f - percentOfPath) * taperY;
+
+                    newNode.xScale = xProfileScale;
+                    newNode.yScale = yProfileScale;
+
+                    float radiusScale = 1.0f;
+                    if (radius > 0.001f)
+                        radiusScale = 1.0f - radius * percentOfPath;
+                    else if (radius < 0.001f)
+                        radiusScale = 1.0f + radius * (1.0f - percentOfPath);
+
+                    float twist = twistBegin + twistTotal * percentOfPath;
+
+                    float xOffset = 0.5f * (skewStart + totalSkew * percentOfAngles);
+                    xOffset += (float)Math.Sin(angle) * xOffsetTopShearXFactor;
+
+                    float yOffset = yShearCompensation * (float)Math.Cos(angle) * (0.5f - yPathScale) * radiusScale;
+
+                    float zOffset = (float)Math.Sin(angle + topShearY) * (0.5f - yPathScale) * radiusScale;
+
+                    newNode.position = new Coord(xOffset, yOffset, zOffset);
+
+                    // now orient the rotation of the profile layer relative to it's position on the path
+                    // adding taperY to the angle used to generate the quat appears to approximate the viewer
+
+                    newNode.rotation = new Quat(new Coord(1.0f, 0.0f, 0.0f), angle + topShearY);
+
+                    // next apply twist rotation to the profile layer
+                    if (twistTotal != 0.0f || twistBegin != 0.0f)
+                        newNode.rotation *= new Quat(new Coord(0.0f, 0.0f, 1.0f), twist);
+
+                    newNode.percentOfPath = percentOfPath;
+
+                    pathNodes.Add(newNode);
+
+                    // calculate terms for next iteration
+                    // calculate the angle for the next iteration of the loop
+
+                    if (angle >= endAngle - 0.01)
+                        done = true;
+                    else
+                    {
+                        step += 1;
+                        angle = stepSize * step;
+                        if (angle > endAngle)
+                            angle = endAngle;
+                    }
+                }
+            }
+        }
+    }
+
+    public class PrimMesh
+    {
+        public string errorMessage = "";
+        private const float twoPi = 2.0f * (float)Math.PI;
+
+        public List<Coord> coords;
+//        public List<Coord> normals;
+        public List<Face> faces;
+
+        private int sides = 4;
+        private int hollowSides = 4;
+        private float profileStart = 0.0f;
+        private float profileEnd = 1.0f;
+        private float hollow = 0.0f;
+        public int twistBegin = 0;
+        public int twistEnd = 0;
+        public float topShearX = 0.0f;
+        public float topShearY = 0.0f;
+        public float pathCutBegin = 0.0f;
+        public float pathCutEnd = 1.0f;
+        public float dimpleBegin = 0.0f;
+        public float dimpleEnd = 1.0f;
+        public float skew = 0.0f;
+        public float holeSizeX = 1.0f; // called pathScaleX in pbs
+        public float holeSizeY = 0.25f;
+        public float taperX = 0.0f;
+        public float taperY = 0.0f;
+        public float radius = 0.0f;
+        public float revolutions = 1.0f;
+        public int stepsPerRevolution = 24;
+
+        private bool hasProfileCut = false;
+        private bool hasHollow = false;
+
+        public int numPrimFaces = 0;
+
+        /// <summary>
+        /// Human readable string representation of the parameters used to create a mesh.
+        /// </summary>
+        /// <returns></returns>
+        public string ParamsToDisplayString()
+        {
+            string s = "";
+            s += "sides..................: " + this.sides.ToString();
+            s += "\nhollowSides..........: " + this.hollowSides.ToString();
+            s += "\nprofileStart.........: " + this.profileStart.ToString();
+            s += "\nprofileEnd...........: " + this.profileEnd.ToString();
+            s += "\nhollow...............: " + this.hollow.ToString();
+            s += "\ntwistBegin...........: " + this.twistBegin.ToString();
+            s += "\ntwistEnd.............: " + this.twistEnd.ToString();
+            s += "\ntopShearX............: " + this.topShearX.ToString();
+            s += "\ntopShearY............: " + this.topShearY.ToString();
+            s += "\npathCutBegin.........: " + this.pathCutBegin.ToString();
+            s += "\npathCutEnd...........: " + this.pathCutEnd.ToString();
+            s += "\ndimpleBegin..........: " + this.dimpleBegin.ToString();
+            s += "\ndimpleEnd............: " + this.dimpleEnd.ToString();
+            s += "\nskew.................: " + this.skew.ToString();
+            s += "\nholeSizeX............: " + this.holeSizeX.ToString();
+            s += "\nholeSizeY............: " + this.holeSizeY.ToString();
+            s += "\ntaperX...............: " + this.taperX.ToString();
+            s += "\ntaperY...............: " + this.taperY.ToString();
+            s += "\nradius...............: " + this.radius.ToString();
+            s += "\nrevolutions..........: " + this.revolutions.ToString();
+            s += "\nstepsPerRevolution...: " + this.stepsPerRevolution.ToString();
+            s += "\nhasProfileCut........: " + this.hasProfileCut.ToString();
+            s += "\nhasHollow............: " + this.hasHollow.ToString();
+
+            return s;
+        }
+
+        public bool HasProfileCut
+        {
+            get { return hasProfileCut; }
+            set { hasProfileCut = value; }
+        }
+
+        public bool HasHollow
+        {
+            get { return hasHollow; }
+        }
+
+
+        /// <summary>
+        /// Constructs a PrimMesh object and creates the profile for extrusion.
+        /// </summary>
+        /// <param name="sides"></param>
+        /// <param name="profileStart"></param>
+        /// <param name="profileEnd"></param>
+        /// <param name="hollow"></param>
+        /// <param name="hollowSides"></param>
+        /// <param name="sphereMode"></param>
+        public PrimMesh(int _sides, float _profileStart, float _profileEnd, float _hollow, int _hollowSides)
+        {
+            coords = new List<Coord>();
+            faces = new List<Face>();
+
+            sides = _sides;
+            profileStart = _profileStart;
+            profileEnd = _profileEnd;
+            hollow = _hollow;
+            hollowSides = _hollowSides;
+
+            if (sides < 3)
+                sides = 3;
+            if (hollowSides < 3)
+                hollowSides = 3;
+            if (profileStart < 0.0f)
+                profileStart = 0.0f;
+            if (profileEnd > 1.0f)
+                profileEnd = 1.0f;
+            if (profileEnd < 0.02f)
+                profileEnd = 0.02f;
+            if (profileStart >= profileEnd)
+                profileStart = profileEnd - 0.02f;
+            if (hollow > 0.99f)
+                hollow = 0.99f;
+            if (hollow < 0.0f)
+                hollow = 0.0f;
+        }
+
+        /// <summary>
+        /// Extrudes a profile along a path.
+        /// </summary>
+        public void Extrude(PathType pathType)
+        {
+            bool needEndFaces = false;
+
+            coords = new List<Coord>();
+            faces = new List<Face>();
+
+            int steps = 1;
+
+            float length = pathCutEnd - pathCutBegin;
+
+            hasProfileCut = this.profileEnd - this.profileStart < 0.9999f;
+
+            hasHollow = (this.hollow > 0.001f);
+
+            float twistBegin = this.twistBegin / 360.0f * twoPi;
+            float twistEnd = this.twistEnd / 360.0f * twoPi;
+            float twistTotal = twistEnd - twistBegin;
+            float twistTotalAbs = Math.Abs(twistTotal);
+            if (twistTotalAbs > 0.01f)
+                steps += (int)(twistTotalAbs * 3.66); //  dahlia's magic number
+
+            float hollow = this.hollow;
+            float initialProfileRot = 0.0f;
+
+            if (pathType == PathType.Circular)
+            {
+                needEndFaces = false;
+                if (pathCutBegin != 0.0f || pathCutEnd != 1.0f)
+                    needEndFaces = true;
+                else if (taperX != 0.0f || taperY != 0.0f)
+                    needEndFaces = true;
+                else if (skew != 0.0f)
+                    needEndFaces = true;
+                else if (twistTotal != 0.0f)
+                    needEndFaces = true;
+                else if (radius != 0.0f)
+                    needEndFaces = true;
+            }
+            else needEndFaces = true;
+
+            if (pathType == PathType.Circular)
+            {
+                if (sides == 3)
+                {
+                    initialProfileRot = (float)Math.PI;
+                    if (hollowSides == 4)
+                    {
+                        if (hollow > 0.7f)
+                            hollow = 0.7f;
+                        hollow *= 0.707f;
+                    }
+                    else hollow *= 0.5f;
+                }
+                else if (sides == 4)
+                {
+                    initialProfileRot = 0.25f * (float)Math.PI;
+                    if (hollowSides != 4)
+                        hollow *= 0.707f;
+                }
+                else if (sides > 4)
+                {
+                    initialProfileRot = (float)Math.PI;
+                    if (hollowSides == 4)
+                    {
+                        if (hollow > 0.7f)
+                            hollow = 0.7f;
+                        hollow /= 0.7f;
+                    }
+                }
+            }
+            else
+            {
+                if (sides == 3)
+                {
+                    if (hollowSides == 4)
+                    {
+                        if (hollow > 0.7f)
+                            hollow = 0.7f;
+                        hollow *= 0.707f;
+                    }
+                    else hollow *= 0.5f;
+                }
+                else if (sides == 4)
+                {
+                    initialProfileRot = 1.25f * (float)Math.PI;
+                    if (hollowSides != 4)
+                        hollow *= 0.707f;
+                }
+                else if (sides == 24 && hollowSides == 4)
+                    hollow *= 1.414f;
+            }
+
+            Profile profile = new Profile(sides, profileStart, profileEnd, hollow, hollowSides,
+                                HasProfileCut,true);
+            errorMessage = profile.errorMessage;
+
+            numPrimFaces = profile.numPrimFaces;
+
+            if (initialProfileRot != 0.0f)
+            {
+                profile.AddRot(new Quat(new Coord(0.0f, 0.0f, 1.0f), initialProfileRot));
+            }
+
+            Path path = new Path();
+            path.twistBegin = twistBegin;
+            path.twistEnd = twistEnd;
+            path.topShearX = topShearX;
+            path.topShearY = topShearY;
+            path.pathCutBegin = pathCutBegin;
+            path.pathCutEnd = pathCutEnd;
+            path.dimpleBegin = dimpleBegin;
+            path.dimpleEnd = dimpleEnd;
+            path.skew = skew;
+            path.holeSizeX = holeSizeX;
+            path.holeSizeY = holeSizeY;
+            path.taperX = taperX;
+            path.taperY = taperY;
+            path.radius = radius;
+            path.revolutions = revolutions;
+            path.stepsPerRevolution = stepsPerRevolution;
+
+            path.Create(pathType, steps);
+
+            int lastNode = path.pathNodes.Count - 1;
+
+            for (int nodeIndex = 0; nodeIndex < path.pathNodes.Count; nodeIndex++)
+            {
+                PathNode node = path.pathNodes[nodeIndex];
+                Profile newLayer = profile.Copy();
+
+                newLayer.Scale(node.xScale, node.yScale);
+                newLayer.AddRot(node.rotation);
+                newLayer.AddPos(node.position);
+
+                // append this layer
+                int coordsStart = coords.Count;
+                coords.AddRange(newLayer.coords);
+
+                if (needEndFaces && nodeIndex == 0 && newLayer.faces.Count > 0)
+                {
+                    newLayer.AddValue2FaceVertexIndices(coordsStart);
+                    newLayer.FlipNormals();
+                    faces.AddRange(newLayer.faces);
+                }
+
+                // fill faces between layers
+
+                List<Face> linkfaces = new List<Face>();
+                int numVerts = newLayer.coords.Count;
+                Face newFace1 = new Face();
+                Face newFace2 = new Face();
+
+                if (nodeIndex > 0)
+                {
+                    int startVert = coordsStart;
+                    int endVert = coords.Count;
+                    if (!hasProfileCut)
+                    {
+                        if(numVerts > 5 && !hasHollow)
+                            startVert++;
+                        int i = startVert;
+                        for (int l = 0; l < profile.numOuterVerts - 1; l++)
+                        {
+                            newFace1.v1 = i;
+                            newFace1.v2 = i - numVerts;
+                            newFace1.v3 = i + 1;
+                            linkfaces.Add(newFace1);
+
+                            newFace2.v1 = i + 1;
+                            newFace2.v2 = i - numVerts;
+                            newFace2.v3 = i + 1 - numVerts;
+                            linkfaces.Add(newFace2);
+                            i++;
+                        }
+
+                        newFace1.v1 = i;
+                        newFace1.v2 = i - numVerts;
+                        newFace1.v3 = startVert;
+                        linkfaces.Add(newFace1);
+
+                        newFace2.v1 = startVert;
+                        newFace2.v2 = i - numVerts;
+                        newFace2.v3 = startVert - numVerts;
+                        linkfaces.Add(newFace2);
+
+                        if (hasHollow)
+                        {
+                            startVert = ++i;
+                            for (int l = 0; l < profile.numHollowVerts - 1; l++)
+                            {
+                                newFace1.v1 = i;
+                                newFace1.v2 = i - numVerts;
+                                newFace1.v3 = i + 1;
+                                linkfaces.Add(newFace1);
+
+                                newFace2.v1 = i + 1;
+                                newFace2.v2 = i - numVerts;
+                                newFace2.v3 = i + 1 - numVerts;
+                                linkfaces.Add(newFace2);
+                                i++;
+                            }
+
+                            newFace1.v1 = i;
+                            newFace1.v2 = i - numVerts;
+                            newFace1.v3 = startVert;
+                            linkfaces.Add(newFace1);
+
+                            newFace2.v1 = startVert;
+                            newFace2.v2 = i - numVerts;
+                            newFace2.v3 = startVert - numVerts;
+                            linkfaces.Add(newFace2);
+                        }
+                    }
+                    else
+                    {
+                        for (int i = startVert; i < endVert; i++)
+                        {
+                            int iNext = i + 1;
+                            if (i == endVert - 1)
+                                iNext = startVert;
+
+                            newFace1.v1 = i;
+                            newFace1.v2 = i - numVerts;
+                            newFace1.v3 = iNext;
+                            linkfaces.Add(newFace1);
+
+                            newFace2.v1 = iNext;
+                            newFace2.v2 = i - numVerts;
+                            newFace2.v3 = iNext - numVerts;
+                            linkfaces.Add(newFace2);
+                        }
+                    }
+                }
+
+                if(linkfaces.Count > 0)
+                    faces.AddRange(linkfaces);
+
+                if (needEndFaces && nodeIndex == lastNode && newLayer.faces.Count > 0)
+                {
+                    newLayer.AddValue2FaceVertexIndices(coordsStart);
+                    faces.AddRange(newLayer.faces);
+                }
+
+            } // for (int nodeIndex = 0; nodeIndex < path.pathNodes.Count; nodeIndex++)
+        // more cleanup will be done at Meshmerizer.cs
+        }
+
+
+        /// <summary>
+        /// DEPRICATED - use Extrude(PathType.Linear) instead
+        /// Extrudes a profile along a straight line path. Used for prim types box, cylinder, and prism.
+        /// </summary>
+        ///
+        public void ExtrudeLinear()
+        {
+            Extrude(PathType.Linear);
+        }
+
+
+        /// <summary>
+        /// DEPRICATED - use Extrude(PathType.Circular) instead
+        /// Extrude a profile into a circular path prim mesh. Used for prim types torus, tube, and ring.
+        /// </summary>
+        ///
+        public void ExtrudeCircular()
+        {
+            Extrude(PathType.Circular);
+        }
+
+
+        private Coord SurfaceNormal(Coord c1, Coord c2, Coord c3)
+        {
+            Coord edge1 = new Coord(c2.X - c1.X, c2.Y - c1.Y, c2.Z - c1.Z);
+            Coord edge2 = new Coord(c3.X - c1.X, c3.Y - c1.Y, c3.Z - c1.Z);
+
+            Coord normal = Coord.Cross(edge1, edge2);
+
+            normal.Normalize();
+
+            return normal;
+        }
+
+        private Coord SurfaceNormal(Face face)
+        {
+            return SurfaceNormal(this.coords[face.v1], this.coords[face.v2], this.coords[face.v3]);
+        }
+
+        /// <summary>
+        /// Calculate the surface normal for a face in the list of faces
+        /// </summary>
+        /// <param name="faceIndex"></param>
+        /// <returns></returns>
+        public Coord SurfaceNormal(int faceIndex)
+        {
+            int numFaces = this.faces.Count;
+            if (faceIndex < 0 || faceIndex >= numFaces)
+                throw new Exception("faceIndex out of range");
+
+            return SurfaceNormal(this.faces[faceIndex]);
+        }
+
+        /// <summary>
+        /// Duplicates a PrimMesh object. All object properties are copied by value, including lists.
+        /// </summary>
+        /// <returns></returns>
+        public PrimMesh Copy()
+        {
+            PrimMesh copy = new PrimMesh(this.sides, this.profileStart, this.profileEnd, this.hollow, this.hollowSides);
+            copy.twistBegin = this.twistBegin;
+            copy.twistEnd = this.twistEnd;
+            copy.topShearX = this.topShearX;
+            copy.topShearY = this.topShearY;
+            copy.pathCutBegin = this.pathCutBegin;
+            copy.pathCutEnd = this.pathCutEnd;
+            copy.dimpleBegin = this.dimpleBegin;
+            copy.dimpleEnd = this.dimpleEnd;
+            copy.skew = this.skew;
+            copy.holeSizeX = this.holeSizeX;
+            copy.holeSizeY = this.holeSizeY;
+            copy.taperX = this.taperX;
+            copy.taperY = this.taperY;
+            copy.radius = this.radius;
+            copy.revolutions = this.revolutions;
+            copy.stepsPerRevolution = this.stepsPerRevolution;
+
+            copy.numPrimFaces = this.numPrimFaces;
+            copy.errorMessage = this.errorMessage;
+
+            copy.coords = new List<Coord>(this.coords);
+            copy.faces = new List<Face>(this.faces);
+
+            return copy;
+        }
+
+        /// <summary>
+        /// Adds a value to each XYZ vertex coordinate in the mesh
+        /// </summary>
+        /// <param name="x"></param>
+        /// <param name="y"></param>
+        /// <param name="z"></param>
+        public void AddPos(float x, float y, float z)
+        {
+            int i;
+            int numVerts = this.coords.Count;
+            Coord vert;
+
+            for (i = 0; i < numVerts; i++)
+            {
+                vert = this.coords[i];
+                vert.X += x;
+                vert.Y += y;
+                vert.Z += z;
+                this.coords[i] = vert;
+            }
+        }
+
+        /// <summary>
+        /// Rotates the mesh
+        /// </summary>
+        /// <param name="q"></param>
+        public void AddRot(Quat q)
+        {
+            int i;
+            int numVerts = this.coords.Count;
+
+            for (i = 0; i < numVerts; i++)
+                this.coords[i] *= q;
+        }
+
+#if VERTEX_INDEXER
+        public VertexIndexer GetVertexIndexer()
+        {
+            return null;
+        }
+#endif
+
+        /// <summary>
+        /// Scales the mesh
+        /// </summary>
+        /// <param name="x"></param>
+        /// <param name="y"></param>
+        /// <param name="z"></param>
+        public void Scale(float x, float y, float z)
+        {
+            int i;
+            int numVerts = this.coords.Count;
+            //Coord vert;
+
+            Coord m = new Coord(x, y, z);
+            for (i = 0; i < numVerts; i++)
+                this.coords[i] *= m;
+        }
+
+        /// <summary>
+        /// Dumps the mesh to a Blender compatible "Raw" format file
+        /// </summary>
+        /// <param name="path"></param>
+        /// <param name="name"></param>
+        /// <param name="title"></param>
+        public void DumpRaw(String path, String name, String title)
+        {
+            if (path == null)
+                return;
+            String fileName = name + "_" + title + ".raw";
+            String completePath = System.IO.Path.Combine(path, fileName);
+            StreamWriter sw = new StreamWriter(completePath);
+
+            for (int i = 0; i < this.faces.Count; i++)
+            {
+                string s = this.coords[this.faces[i].v1].ToString();
+                s += " " + this.coords[this.faces[i].v2].ToString();
+                s += " " + this.coords[this.faces[i].v3].ToString();
+
+                sw.WriteLine(s);
+            }
+
+            sw.Close();
+        }
+    }
+}
diff --git a/OpenSim/Region/PhysicsModules/ubOdeMeshing/Properties/AssemblyInfo.cs b/OpenSim/Region/PhysicsModules/ubOdeMeshing/Properties/AssemblyInfo.cs
new file mode 100644
index 0000000..6503e35
--- /dev/null
+++ b/OpenSim/Region/PhysicsModules/ubOdeMeshing/Properties/AssemblyInfo.cs
@@ -0,0 +1,36 @@
+using System.Reflection;
+using System.Runtime.CompilerServices;
+using System.Runtime.InteropServices;
+using Mono.Addins;
+
+// General Information about an assembly is controlled through the following
+// set of attributes. Change these attribute values to modify the information
+// associated with an assembly.
+[assembly: AssemblyTitle("OpenSim.Region.PhysicsModule.ubODEMeshing")]
+[assembly: AssemblyDescription("Mesher for ubODE")]
+[assembly: AssemblyConfiguration("")]
+[assembly: AssemblyCompany("http://opensimulator.org")]
+[assembly: AssemblyProduct("OpenSim")]
+[assembly: AssemblyCopyright("OpenSimulator developers")]
+[assembly: AssemblyTrademark("")]
+[assembly: AssemblyCulture("")]
+
+// Setting ComVisible to false makes the types in this assembly not visible
+// to COM components.  If you need to access a type in this assembly from
+// COM, set the ComVisible attribute to true on that type.
+[assembly: ComVisible(false)]
+
+// The following GUID is for the ID of the typelib if this project is exposed to COM
+[assembly: Guid("4b7e35c2-a9dd-4b10-b778-eb417f4f6884")]
+
+// Version information for an assembly consists of the following four values:
+//
+//      Major Version
+//      Minor Version
+//      Build Number
+//      Revision
+//
+[assembly: AssemblyVersion(OpenSim.VersionInfo.AssemblyVersionNumber)]
+
+[assembly: Addin("OpenSim.Region.PhysicsModule.ubOdeMeshing", OpenSim.VersionInfo.VersionNumber)]
+[assembly: AddinDependency("OpenSim.Region.Framework", OpenSim.VersionInfo.VersionNumber)]
diff --git a/OpenSim/Region/PhysicsModules/ubOdeMeshing/SculptMap.cs b/OpenSim/Region/PhysicsModules/ubOdeMeshing/SculptMap.cs
new file mode 100644
index 0000000..ebe2523
--- /dev/null
+++ b/OpenSim/Region/PhysicsModules/ubOdeMeshing/SculptMap.cs
@@ -0,0 +1,238 @@
+/*
+ * Copyright (c) Contributors
+ * See CONTRIBUTORS.TXT for a full list of copyright holders.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the OpenSimulator Project nor the
+ *       names of its contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+using System;
+using System.Collections.Generic;
+using System.Text;
+
+using System.Drawing;
+using System.Drawing.Imaging;
+
+namespace PrimMesher
+{
+    public class SculptMap
+    {
+        public int width;
+        public int height;
+        public byte[] redBytes;
+        public byte[] greenBytes;
+        public byte[] blueBytes;
+
+        public SculptMap()
+        {
+        }
+
+        public SculptMap(Bitmap bm, int lod)
+        {
+            int bmW = bm.Width;
+            int bmH = bm.Height;
+
+            if (bmW == 0 || bmH == 0)
+                throw new Exception("SculptMap: bitmap has no data");
+
+            int numLodPixels = lod * lod;  // (32 * 2)^2  = 64^2 pixels for default sculpt map image
+
+            bool needsScaling = false;
+            bool smallMap = false;
+
+            width = bmW;
+            height = bmH;
+
+            while (width * height > numLodPixels * 4)
+            {
+                width >>= 1;
+                height >>= 1;
+                needsScaling = true;
+            }
+
+            if (needsScaling)
+                bm = ScaleImage(bm, width, height);
+
+            if (width * height > numLodPixels)
+            {
+                smallMap = false;
+                width >>= 1;
+                height >>= 1;
+            }
+            else
+                smallMap = true;
+
+            int numBytes = (width + 1) * (height + 1);
+            redBytes = new byte[numBytes];
+            greenBytes = new byte[numBytes];
+            blueBytes = new byte[numBytes];
+
+            int byteNdx = 0;
+            Color c;
+
+            try
+            {
+                for (int y = 0; y <= height; y++)
+                {
+                    for (int x = 0; x < width; x++)
+                    {
+                        if (smallMap)
+                            c = bm.GetPixel(x,  y < height ? y : y - 1);
+                        else
+                            c = bm.GetPixel(x * 2, y < height ? y * 2 : y * 2 - 1);
+
+                        redBytes[byteNdx] = c.R;
+                        greenBytes[byteNdx] = c.G;
+                        blueBytes[byteNdx] = c.B;
+
+                        ++byteNdx;
+                    }
+
+                    if (smallMap)
+                        c = bm.GetPixel(width - 1, y < height ? y : y - 1);
+                    else
+                        c = bm.GetPixel(width * 2 - 1, y < height ? y * 2 : y * 2 - 1);
+
+                    redBytes[byteNdx] = c.R;
+                    greenBytes[byteNdx] = c.G;
+                    blueBytes[byteNdx] = c.B;
+
+                    ++byteNdx;
+                }
+            }
+            catch (Exception e)
+            {
+                if (needsScaling)
+                    bm.Dispose();
+                throw new Exception("Caught exception processing byte arrays in SculptMap(): e: " + e.ToString());
+            }
+
+            width++;
+            height++;
+            if(needsScaling)
+                bm.Dispose();
+        }
+
+        public List<List<Coord>> ToRows(bool mirror)
+        {
+            int numRows = height;
+            int numCols = width;
+
+            List<List<Coord>> rows = new List<List<Coord>>(numRows);
+
+            float pixScale = 1.0f / 255;
+
+            int rowNdx, colNdx;
+            int smNdx = 0;
+
+            for (rowNdx = 0; rowNdx < numRows; rowNdx++)
+            {
+                List<Coord> row = new List<Coord>(numCols);
+                for (colNdx = 0; colNdx < numCols; colNdx++)
+                {
+
+                    if (mirror)
+                        row.Add(new Coord(-((float)redBytes[smNdx] * pixScale - 0.5f), ((float)greenBytes[smNdx] * pixScale - 0.5f), (float)blueBytes[smNdx] * pixScale - 0.5f));
+                    else
+                        row.Add(new Coord((float)redBytes[smNdx] * pixScale - 0.5f, (float)greenBytes[smNdx] * pixScale - 0.5f, (float)blueBytes[smNdx] * pixScale - 0.5f));
+
+                    ++smNdx;
+                }
+                rows.Add(row);
+            }
+            return rows;
+        }
+
+        private Bitmap ScaleImage(Bitmap srcImage, int destWidth, int destHeight)
+        {
+            Bitmap scaledImage = new Bitmap(destWidth, destHeight, PixelFormat.Format24bppRgb);
+
+            Color c;
+
+            // will let last step to be eventually diferent, as seems to be in sl
+
+            float xscale = (float)srcImage.Width / (float)destWidth;
+            float yscale = (float)srcImage.Height / (float)destHeight;
+
+            int lastsx = srcImage.Width - 1;
+            int lastsy = srcImage.Height - 1;
+            int lastdx = destWidth - 1;
+            int lastdy = destHeight - 1;
+
+            float sy = 0.5f;
+            float sx;
+
+            for (int y = 0; y < lastdy; y++)
+            {
+                sx = 0.5f;
+                for (int x = 0; x < lastdx; x++)
+                {
+                    try
+                    {
+                        c = srcImage.GetPixel((int)(sx), (int)(sy));
+                        scaledImage.SetPixel(x, y, Color.FromArgb(c.R, c.G, c.B));
+                    }
+                    catch (IndexOutOfRangeException)
+                    {
+                    }
+                    sx += xscale;
+                }
+                try
+                {
+                    c = srcImage.GetPixel(lastsx, (int)(sy));
+                    scaledImage.SetPixel(lastdx, y, Color.FromArgb(c.R, c.G, c.B));
+                }
+                catch (IndexOutOfRangeException)
+                {
+                }
+
+                sy += yscale;
+            }
+
+            sx = 0.5f;
+            for (int x = 0; x < lastdx; x++)
+            {
+                try
+                {
+                    c = srcImage.GetPixel((int)(sx), lastsy);
+                    scaledImage.SetPixel(x, lastdy, Color.FromArgb(c.R, c.G, c.B));
+                }
+                catch (IndexOutOfRangeException)
+                {
+                }
+
+                sx += xscale;
+            }
+            try
+            {
+                c = srcImage.GetPixel(lastsx, lastsy);
+                scaledImage.SetPixel(lastdx, lastdy, Color.FromArgb(c.R, c.G, c.B));
+            }
+            catch (IndexOutOfRangeException)
+            {
+            }
+
+            srcImage.Dispose();
+            return scaledImage;
+        }
+    }
+}
\ No newline at end of file
diff --git a/OpenSim/Region/PhysicsModules/ubOdeMeshing/SculptMesh.cs b/OpenSim/Region/PhysicsModules/ubOdeMeshing/SculptMesh.cs
new file mode 100644
index 0000000..bc1375b
--- /dev/null
+++ b/OpenSim/Region/PhysicsModules/ubOdeMeshing/SculptMesh.cs
@@ -0,0 +1,220 @@
+/*
+ * Copyright (c) Contributors
+ * See CONTRIBUTORS.TXT for a full list of copyright holders.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the OpenSimulator Project nor the
+ *       names of its contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+using System;
+using System.Collections.Generic;
+using System.Text;
+using System.IO;
+
+using System.Drawing;
+using System.Drawing.Imaging;
+
+namespace PrimMesher
+{
+
+    public class SculptMesh
+    {
+        public List<Coord> coords;
+        public List<Face> faces;
+
+        public enum SculptType { sphere = 1, torus = 2, plane = 3, cylinder = 4 };
+
+
+        public SculptMesh(Bitmap sculptBitmap, SculptType sculptType, int lod, bool mirror, bool invert)
+        {
+            if (mirror)
+                invert = !invert;
+
+            SculptMap smap = new SculptMap(sculptBitmap, lod);
+
+            List<List<Coord>> rows = smap.ToRows(mirror);
+
+            _SculptMesh(rows, sculptType, invert);
+        }
+
+        private void _SculptMesh(List<List<Coord>> rows, SculptType sculptType, bool invert)
+        {
+            coords = new List<Coord>();
+            faces = new List<Face>();
+
+            sculptType = (SculptType)(((int)sculptType) & 0x07);
+
+            int width = rows[0].Count;
+
+            int p1, p2, p3, p4;
+
+            int imageX, imageY;
+
+            if (sculptType != SculptType.plane)
+            {
+                if (rows.Count % 2 == 0)
+                {
+                    for (int rowNdx = 0; rowNdx < rows.Count; rowNdx++)
+                        rows[rowNdx].Add(rows[rowNdx][0]);
+                }
+                else
+                {
+                    int lastIndex = rows[0].Count - 1;
+
+                    for (int i = 0; i < rows.Count; i++)
+                        rows[i][0] = rows[i][lastIndex];
+                }
+            }
+
+            Coord topPole = rows[0][width / 2];
+            Coord bottomPole = rows[rows.Count - 1][width / 2];
+
+            if (sculptType == SculptType.sphere)
+            {
+                if (rows.Count % 2 == 0)
+                {
+                    int count = rows[0].Count;
+                    List<Coord> topPoleRow = new List<Coord>(count);
+                    List<Coord> bottomPoleRow = new List<Coord>(count);
+
+                    for (int i = 0; i < count; i++)
+                    {
+                        topPoleRow.Add(topPole);
+                        bottomPoleRow.Add(bottomPole);
+                    }
+                    rows.Insert(0, topPoleRow);
+                    rows.Add(bottomPoleRow);
+                }
+                else
+                {
+                    int count = rows[0].Count;
+
+                    List<Coord> topPoleRow = rows[0];
+                    List<Coord> bottomPoleRow = rows[rows.Count - 1];
+
+                    for (int i = 0; i < count; i++)
+                    {
+                        topPoleRow[i] = topPole;
+                        bottomPoleRow[i] = bottomPole;
+                    }
+                }
+            }
+
+            if (sculptType == SculptType.torus)
+                rows.Add(rows[0]);
+
+            int coordsDown = rows.Count;
+            int coordsAcross = rows[0].Count;
+
+            float widthUnit = 1.0f / (coordsAcross - 1);
+            float heightUnit = 1.0f / (coordsDown - 1);
+
+            for (imageY = 0; imageY < coordsDown; imageY++)
+            {
+                int rowOffset = imageY * coordsAcross;
+
+                for (imageX = 0; imageX < coordsAcross; imageX++)
+                {
+                    /*
+                    *   p1-----p2
+                    *   | \ f2 |
+                    *   |   \  |
+                    *   | f1  \|
+                    *   p3-----p4
+                    */
+
+                    p4 = rowOffset + imageX;
+                    p3 = p4 - 1;
+
+                    p2 = p4 - coordsAcross;
+                    p1 = p3 - coordsAcross;
+
+                    this.coords.Add(rows[imageY][imageX]);
+
+                    if (imageY > 0 && imageX > 0)
+                    {
+                        Face f1, f2;
+
+                            if (invert)
+                            {
+                                f1 = new Face(p1, p4, p3);
+                                f2 = new Face(p1, p2, p4);
+                            }
+                            else
+                            {
+                                f1 = new Face(p1, p3, p4);
+                                f2 = new Face(p1, p4, p2);
+                            }
+
+                        this.faces.Add(f1);
+                        this.faces.Add(f2);
+                    }
+                }
+            }
+        }
+
+        /// <summary>
+        /// Duplicates a SculptMesh object. All object properties are copied by value, including lists.
+        /// </summary>
+        /// <returns></returns>
+        public SculptMesh Copy()
+        {
+            return new SculptMesh(this);
+        }
+
+        public SculptMesh(SculptMesh sm)
+        {
+            coords = new List<Coord>(sm.coords);
+            faces = new List<Face>(sm.faces);
+        }
+
+        public void Scale(float x, float y, float z)
+        {
+            int i;
+            int numVerts = this.coords.Count;
+
+            Coord m = new Coord(x, y, z);
+            for (i = 0; i < numVerts; i++)
+                this.coords[i] *= m;
+        }
+
+        public void DumpRaw(String path, String name, String title)
+        {
+            if (path == null)
+                return;
+            String fileName = name + "_" + title + ".raw";
+            String completePath = System.IO.Path.Combine(path, fileName);
+            StreamWriter sw = new StreamWriter(completePath);
+
+            for (int i = 0; i < this.faces.Count; i++)
+            {
+                string s = this.coords[this.faces[i].v1].ToString();
+                s += " " + this.coords[this.faces[i].v2].ToString();
+                s += " " + this.coords[this.faces[i].v3].ToString();
+
+                sw.WriteLine(s);
+            }
+
+            sw.Close();
+        }
+    }
+}