18 files changed, 12694 insertions, 1071 deletions

diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
index a68dcb7..80c1277 100644
--- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
+++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
@@ -22,11 +22,13 @@
  * (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 March 5th 2010 by Kitto Flora. ODEDynamics.cs 
+ * Revised March 5th 2010 by Kitto Flora. ODEDynamics.cs
+ * Ubit 2012
  * rolled into ODEPrim.cs
  */
 
 using System;
+using System.IO;
 using System.Collections.Generic;
 using System.Reflection;
 using System.Runtime.InteropServices;
@@ -37,7 +39,6 @@ using Ode.NET;
 using OpenSim.Framework;
 using OpenSim.Region.Physics.Manager;
 
-
 namespace OpenSim.Region.Physics.OdePlugin
 {
     /// <summary>
@@ -48,6 +49,11 @@ namespace OpenSim.Region.Physics.OdePlugin
     {
         private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
 
+        public class SerialControl
+        {
+            public object alock = new object();
+            public byte[] data = new byte[0];
+        }
         private Vector3 _position;
         private Vector3 _velocity;
         private Vector3 _torque;
@@ -80,12 +86,12 @@ namespace OpenSim.Region.Physics.OdePlugin
         private float PID_D = 35f;
         private float PID_G = 25f;
         private bool m_usePID = false;
-        
+
         private Quaternion m_APIDTarget = new Quaternion();
         private float m_APIDStrength = 0.5f;
         private float m_APIDDamping = 0.5f;
         private bool m_useAPID = false;
-                private float m_APIDdamper = 1.0f;
+        private float m_APIDdamper = 1.0f;
 
         // These next 7 params apply to llSetHoverHeight(float height, integer water, float tau),
         // do not confuse with VEHICLE HOVER
@@ -112,7 +118,7 @@ namespace OpenSim.Region.Physics.OdePlugin
         private bool m_taintPhysics;
         private bool m_collidesLand = true;
         private bool m_collidesWater;
-        public bool m_returnCollisions;
+        //        public bool m_returnCollisions;
 
         // Default we're a Geometry
         private CollisionCategories m_collisionCategories = (CollisionCategories.Geom);
@@ -144,7 +150,7 @@ namespace OpenSim.Region.Physics.OdePlugin
         private OdeScene _parent_scene;
         public IntPtr m_targetSpace = IntPtr.Zero;
         public IntPtr prim_geom;
-//        public IntPtr prev_geom;
+        //        public IntPtr prev_geom;
         public IntPtr _triMeshData;
 
         private IntPtr _linkJointGroup = IntPtr.Zero;
@@ -163,8 +169,8 @@ namespace OpenSim.Region.Physics.OdePlugin
         private int throttleCounter;
         public int m_interpenetrationcount;
         public float m_collisionscore;
-        public int m_roundsUnderMotionThreshold;
-        private int m_crossingfailures;
+        //        public int m_roundsUnderMotionThreshold;
+        //        private int m_crossingfailures;
 
         public bool m_outofBounds;
         private float m_density = 10.000006836f; // Aluminum g/cm3;
@@ -185,75 +191,525 @@ namespace OpenSim.Region.Physics.OdePlugin
         public volatile bool childPrim;
 
         internal int m_material = (int)Material.Wood;
-        
-        private int frcount = 0;                                                                                // Used to limit dynamics debug output to 
-        private int revcount = 0;                                                                               // Reverse motion while > 0
 
         private IntPtr m_body = IntPtr.Zero;
-        
+
         // Vehicle properties ============================================================================================
         private Vehicle m_type = Vehicle.TYPE_NONE;                                             // If a 'VEHICLE', and what kind
         // private Quaternion m_referenceFrame = Quaternion.Identity;   // Axis modifier
-        private VehicleFlag m_flags = (VehicleFlag) 0;                                  // Bit settings:
-                                                                                                                                                // HOVER_TERRAIN_ONLY
-                                                                                                                                                // HOVER_GLOBAL_HEIGHT
-                                                                                                                                                // NO_DEFLECTION_UP
-                                                                                                                                                // HOVER_WATER_ONLY
-                                                                                                                                                // HOVER_UP_ONLY
-                                                                                                                                                // LIMIT_MOTOR_UP
-                                                                                                                                                // LIMIT_ROLL_ONLY
-        
+        private VehicleFlag m_flags = (VehicleFlag)0;                                   // Bit settings:
+        // HOVER_TERRAIN_ONLY
+        // HOVER_GLOBAL_HEIGHT
+        // NO_DEFLECTION_UP
+        // HOVER_WATER_ONLY
+        // HOVER_UP_ONLY
+        // LIMIT_MOTOR_UP
+        // LIMIT_ROLL_ONLY
+
         // Linear properties
         private Vector3 m_linearMotorDirection = Vector3.Zero;                  // (was m_linearMotorDirectionLASTSET) the (local) Velocity 
-                                                                                                                                                                //requested by LSL
-        private float   m_linearMotorTimescale = 0;                                             // Motor Attack rate set by LSL
-        private float   m_linearMotorDecayTimescale = 0;                                // Motor Decay rate set by LSL
+        //requested by LSL
+        private float m_linearMotorTimescale = 0;                                               // Motor Attack rate set by LSL
+        private float m_linearMotorDecayTimescale = 0;                          // Motor Decay rate set by LSL
         private Vector3 m_linearFrictionTimescale = Vector3.Zero;               // General Friction set by LSL
-        
-                private Vector3 m_lLinMotorDVel = Vector3.Zero;                                 // decayed motor
-                private Vector3 m_lLinObjectVel = Vector3.Zero;                                 // local frame object velocity
-                private Vector3 m_wLinObjectVel = Vector3.Zero;                                 // world frame object velocity
-        
+
+        private Vector3 m_lLinMotorDVel = Vector3.Zero;                                 // decayed motor
+        private Vector3 m_lLinObjectVel = Vector3.Zero;                                 // local frame object velocity
+        private Vector3 m_wLinObjectVel = Vector3.Zero;                                 // world frame object velocity
+
         //Angular properties
         private Vector3 m_angularMotorDirection = Vector3.Zero;                 // angular velocity requested by LSL motor 
-        
+
         private float m_angularMotorTimescale = 0;                                              // motor angular Attack rate set by LSL
         private float m_angularMotorDecayTimescale = 0;                                 // motor angular Decay rate set by LSL
         private Vector3 m_angularFrictionTimescale = Vector3.Zero;              // body angular Friction set by LSL
 
         private Vector3 m_angularMotorDVel = Vector3.Zero;                              // decayed angular motor
-//        private Vector3 m_angObjectVel = Vector3.Zero;                                        // current body angular velocity
+        //        private Vector3 m_angObjectVel = Vector3.Zero;                                        // current body angular velocity
         private Vector3 m_lastAngularVelocity = Vector3.Zero;                   // what was last applied to body
 
-                //Deflection properties        
+        //Deflection properties        
         // private float m_angularDeflectionEfficiency = 0;
         // private float m_angularDeflectionTimescale = 0;
         // private float m_linearDeflectionEfficiency = 0;
         // private float m_linearDeflectionTimescale = 0;
-        
+
         //Banking properties
         // private float m_bankingEfficiency = 0;
         // private float m_bankingMix = 0;
         // private float m_bankingTimescale = 0;
-        
+
         //Hover and Buoyancy properties
         private float m_VhoverHeight = 0f;
-//        private float m_VhoverEfficiency = 0f;
+        //        private float m_VhoverEfficiency = 0f;
         private float m_VhoverTimescale = 0f;
         private float m_VhoverTargetHeight = -1.0f;     // if <0 then no hover, else its the current target height 
         private float m_VehicleBuoyancy = 0f;                   // 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
+        // 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 = 500f;                     // Timescale > 300  means no vert attractor.
-        
-        
+
+        SerialControl m_taintserial = null;
+        object m_taintvehicledata = null;
+
+        public void DoSetVehicle()
+        {
+            VehicleData vd = (VehicleData)m_taintvehicledata;
+
+        m_type = vd.m_type;
+        m_flags = vd.m_flags;
+
+        // Linear properties
+        m_linearMotorDirection = vd.m_linearMotorDirection;
+        m_linearFrictionTimescale = vd.m_linearFrictionTimescale;
+        m_linearMotorDecayTimescale = vd.m_linearMotorDecayTimescale;
+        m_linearMotorTimescale = vd.m_linearMotorTimescale;
+//        m_linearMotorOffset = vd.m_linearMotorOffset;
+
+        //Angular properties
+        m_angularMotorDirection = vd.m_angularMotorDirection;
+        m_angularMotorTimescale = vd.m_angularMotorTimescale;
+        m_angularMotorDecayTimescale = vd.m_angularMotorDecayTimescale;
+        m_angularFrictionTimescale = vd.m_angularFrictionTimescale;
+
+        //Deflection properties
+//        m_angularDeflectionEfficiency = vd.m_angularDeflectionEfficiency;
+//        m_angularDeflectionTimescale = vd.m_angularDeflectionTimescale;
+//        m_linearDeflectionEfficiency = vd.m_linearDeflectionEfficiency;
+//        m_linearDeflectionTimescale = vd.m_linearDeflectionTimescale;
+
+        //Banking properties
+//        m_bankingEfficiency = vd.m_bankingEfficiency;
+//        m_bankingMix = vd.m_bankingMix;
+//        m_bankingTimescale = vd.m_bankingTimescale;
+
+        //Hover and Buoyancy properties
+        m_VhoverHeight = vd.m_VhoverHeight;
+//        m_VhoverEfficiency = vd.m_VhoverEfficiency;
+        m_VhoverTimescale = vd.m_VhoverTimescale;
+        m_VehicleBuoyancy = vd.m_VehicleBuoyancy;
+
+        //Attractor properties
+        m_verticalAttractionEfficiency = vd.m_verticalAttractionEfficiency;
+        m_verticalAttractionTimescale = vd.m_verticalAttractionTimescale;
+
+        // Axis
+//        m_referenceFrame = vd.m_referenceFrame;
 
 
+            m_taintvehicledata = null;
+        }
+
+        public override void SetVehicle(object vdata)
+        {
+            m_taintvehicledata = vdata;
+            _parent_scene.AddPhysicsActorTaint(this);
+        }
+
+        public override byte[] Serialize(bool PhysIsRunning)
+        {
+            SerialControl sc = new SerialControl();
+
+            lock (sc.alock)
+            {
+                if (PhysIsRunning)
+                {
+                    m_taintserial = sc;
+                
+                    if (!Monitor.Wait(sc.alock, 1000))
+                    {
+                        m_log.Error("[chOde] prim data serialization timed out");
+                        m_taintserial = null;
+                        return new byte[0];
+                    }
+                }
+                else
+                    DoSerialize(sc);
+            }
+
+            return sc.data;
+        }
+
+        public void DoSerialize(SerialControl sc)
+        {
+            wstreamer st = new wstreamer();
+            Vector3 vtmp;
+
+            ushort version = 2;
+            if (!BitConverter.IsLittleEndian)
+                version |= 1;
+            st.Wushort(version); //version lower bit codes endian type for future use
+
+            // compact booleans in a ushort
+            ushort flags = 0;
+
+            if (m_isphysical) // this should be true for now
+                flags |= 1;
+            if (m_isSelected)
+                flags |= 2;
+            if (m_isVolumeDetect)
+                flags |= 4;
+            if (m_disabled)
+                flags |= 8;
+            if (m_collidesWater)
+                flags |= 16;
+            if (m_collidesLand)
+                flags |= 32;
+            if (m_usePID)
+                flags |= 64;
+            if (m_useAPID)
+                flags |= 128;
+            if (m_useHoverPID)
+                flags |= 256;
+            if (m_throttleUpdates)
+                flags |= 512;
+
+            st.Wushort(flags);
+
+            st.Wvector3(_size);
+            st.Wint(m_material);
+            st.Wfloat(m_density);
+            st.Wfloat(0); // future gravity mod V3
+            st.Wfloat(0); // future friction V3
+            st.Wfloat(0); // future bounce V3
+
+//            st.Wuint((uint)m_collisionCategories);
+//            st.Wuint((uint)m_collisionFlags);
+
+            if (_parent == null)
+            {
+                st.Wvector3(_position); // ??
+                st.Wquat(_orientation);
+            }
+            else // for childs save offsets
+            {
+                Quaternion to;
+                Quaternion ipo = Quaternion.Inverse(_parent.Orientation);
+
+                if (m_isphysical && prim_geom != IntPtr.Zero)
+                {
+                    d.Vector3 dvt;
+                    d.GeomCopyPosition(prim_geom, out dvt);
 
+                    vtmp.X = dvt.X;
+                    vtmp.Y = dvt.Y;
+                    vtmp.Z = dvt.Z;
+
+                    d.Quaternion dqt;
+                    d.GeomCopyQuaternion(prim_geom, out dqt);
+
+                    to.X = dqt.X;
+                    to.Y = dqt.Y;
+                    to.Z = dqt.Z;
+                    to.W = dqt.W; // rotation in world
+                }
+                else
+                {
+                    vtmp = _position;
+                    to = _orientation;
+                }
+
+                vtmp -= _parent.Position; // offset in world
+                vtmp *= ipo; // offset in local
+                st.Wvector3(vtmp);
+
+                ipo *= to; // own rotation
+                st.Wquat(ipo);
+            }
+
+            st.Wvector3(_velocity);
+            st.Wvector3(m_rotationalVelocity);
+            st.Wvector3(_acceleration);
+            st.Wvector3(m_rotateEnable);
+
+            vtmp = Vector3.Zero;
+            for (int i = 0; i < m_forcelist.Count; i++)
+                {
+
+                    vtmp += (m_forcelist[i] * 100);
+                }
+
+            st.Wvector3(vtmp); // force acc
+
+            vtmp = Vector3.Zero;
+            for (int i = 0; i < m_angularforcelist.Count; i++)
+            {
+                vtmp += (m_angularforcelist[i] * 100);
+            }
+
+            st.Wvector3(vtmp); // angular force acc
+
+            st.Wvector3(m_PIDTarget);
+            st.Wfloat(m_PIDTau);
+            st.Wfloat(PID_D);
+            st.Wfloat(PID_G);
+            st.Wquat(m_APIDTarget);
+            st.Wfloat(m_APIDStrength);
+            st.Wfloat(m_APIDDamping);
+            st.Wfloat(m_APIDdamper);
+
+            st.Wint((int)m_PIDHoverType);
+            st.Wfloat(m_PIDHoverHeight);
+            st.Wfloat(m_PIDHoverTau);
+            st.Wfloat(m_targetHoverHeight);
+
+            st.Wfloat(m_groundHeight);
+            st.Wfloat(m_waterHeight);
+
+            st.Wfloat(m_buoyancy);
+
+            // this must be last since type none ends stream
+            if (m_type == Vehicle.TYPE_NONE)
+                st.Wint((int)Vehicle.TYPE_NONE);
+            else
+            {
+                st.Wint((int)m_type);
+
+                st.Wquat(Quaternion.Identity); //m_referenceFrame
+
+                st.Wint((int)m_flags);
+
+                st.Wvector3(m_linearMotorDirection);
+                st.Wfloat(
+                    (float)Math.Sqrt(m_lLinMotorDVel.LengthSquared() / m_linearMotorDirection.LengthSquared()));
+
+                st.Wvector3(m_linearFrictionTimescale);
+                st.Wfloat(m_linearMotorDecayTimescale);
+                st.Wfloat(m_linearMotorTimescale);
+                st.Wvector3(new Vector3(0, 0, 0)); //m_linearMotorOffset);
+
+                st.Wvector3(m_angularMotorDirection);
+                st.Wfloat((float)Math.Sqrt(m_angularMotorDVel.LengthSquared() / m_angularMotorDirection.LengthSquared()));
+
+                st.Wvector3(m_angularFrictionTimescale);
+                st.Wfloat(m_angularMotorDecayTimescale);
+                st.Wfloat(m_angularMotorTimescale);
+
+                st.Wfloat(0); //m_linearDeflectionEfficiency);
+                st.Wfloat(1000); //m_linearDeflectionTimescale);
+
+                st.Wfloat(0); //m_angularDeflectionEfficiency);
+                st.Wfloat(120);   //m_angularDeflectionTimescale);
+
+                st.Wfloat(0); // m_bankingEfficiency);
+                st.Wfloat(0); //m_bankingMix);
+                st.Wfloat(1000);  //m_bankingTimescale);
+
+                st.Wfloat(m_VhoverHeight);
+                st.Wfloat(0.5f);   //m_VhoverEfficiency);
+                st.Wfloat(m_VhoverTimescale);
+
+                st.Wfloat(m_VehicleBuoyancy);
+
+                st.Wfloat(m_verticalAttractionEfficiency);
+                st.Wfloat(m_verticalAttractionTimescale);
+            }
+            sc.data = st.close();
+            m_taintserial = null;
+            Monitor.PulseAll(sc.alock);
+        }
+
+        public bool DeSerialize(byte[] data)
+        {
+            rstreamer st = new rstreamer(data);
+
+            int version =st.Rushort(); //version
+
+            // merge booleans in a ushort
+            ushort flags = st.Rushort();
+            if ((flags & 1) != 0)
+                m_isphysical = true;
+            if ((flags & 2) != 0)
+                m_taintselected = true;
+            if ((flags & 4) != 0)
+                m_isVolumeDetect = true;
+            if ((flags & 8) != 0)
+                m_taintdisable = true;
+            if ((flags & 16) != 0)
+                m_taintCollidesWater = true;
+            if ((flags & 32) != 0)
+                m_collidesLand = true;
+            if ((flags & 64) != 0)
+                m_usePID = true;
+            if ((flags & 128) != 0)
+                m_useAPID = true;
+            if ((flags & 256) != 0)
+                m_useHoverPID = true;
+            if ((flags & 512) != 0)
+                m_throttleUpdates = true;
+
+            _size = st.Rvector3();
+            m_taintsize = _size;
+
+            m_material= st.Rint();
+            m_density = st.Rfloat();
+            st.Rfloat(); // future gravity mod V3
+            st.Rfloat(); // future friction V3
+            st.Rfloat(); // future bounce V3
+
+//            m_collisionCategories = (CollisionCategories)st.Ruint();
+//            m_collisionFlags = (CollisionCategories) st.Ruint();
+
+            if (m_taintparent == null)
+            {
+                st.Rvector3(); // ignore old position sop/sog as to tell the new one
+                m_taintrot = st.Rquat(); // 
+                _orientation = m_taintrot;
+            }
+            else
+            {
+                m_taintrot = _parent.Orientation;
+                m_taintposition = st.Rvector3(); // ??
+                _position = m_taintposition;
+
+                m_taintposition *= m_taintrot;
+                m_taintposition += _parent.Position;
+
+                m_taintrot *= st.Rquat(); // 
+                _orientation = m_taintrot;
+            }
+            
+            m_taintVelocity = st.Rvector3();
+            m_rotationalVelocity = st.Rvector3();
+            
+            _acceleration = st.Rvector3();
+            m_rotateEnableRequest = st.Rvector3();
+            m_rotateEnableUpdate = true;
+
+            Vector3 vtmp;
+
+            vtmp = st.Rvector3(); // forces acc
+            m_forcelist.Add(vtmp);
+            m_taintforce = true;
+
+            vtmp = st.Rvector3(); // angular forces acc
+            m_angularforcelist.Add(vtmp);
+            m_taintaddangularforce = true;
+
+            m_PIDTarget = st.Rvector3();
+            m_PIDTau = st.Rfloat();
+            PID_D = st.Rfloat();
+            PID_G = st.Rfloat();
+
+            m_APIDTarget = st.Rquat();
+            m_APIDStrength = st.Rfloat();
+            m_APIDDamping = st.Rfloat();
+            m_APIDdamper = st.Rfloat();
+
+            m_PIDHoverType = (PIDHoverType) st.Rint();
+            m_PIDHoverHeight = st.Rfloat();
+            m_PIDHoverTau = st.Rfloat();
+            m_targetHoverHeight = st.Rfloat();
+
+            m_groundHeight = st.Rfloat();
+            m_waterHeight = st.Rfloat();
+
+            m_buoyancy = st.Rfloat();
+
+
+            // this must be last since type none ends stream
+
+            m_type = (Vehicle) st.Rint();
+
+            if (m_type != Vehicle.TYPE_NONE)
+            {
+                float ftmp;
+
+                st.Rquat(); //m_referenceFrame
+
+                m_flags = (VehicleFlag) st.Rint();
+
+                m_linearMotorDirection = st.Rvector3();
+
+                ftmp = st.Rfloat();
+                m_lLinMotorDVel = m_linearMotorDirection * ftmp;
+
+                m_linearFrictionTimescale = st.Rvector3();
+                m_linearMotorDecayTimescale = st.Rfloat();
+                m_linearMotorTimescale = st.Rfloat();
+                st.Rvector3(); //m_linearMotorOffset);
+
+                m_angularMotorDirection = st.Rvector3();
+                ftmp = st.Rfloat();
+                m_angularMotorDVel = m_angularMotorDirection * ftmp;
+
+                m_angularFrictionTimescale = st.Rvector3();
+                m_angularMotorDecayTimescale = st.Rfloat();
+                m_angularMotorTimescale = st.Rfloat();
+
+                st.Rfloat(); //m_linearDeflectionEfficiency);
+                st.Rfloat(); //m_linearDeflectionTimescale);
+
+                st.Rfloat(); //m_angularDeflectionEfficiency);
+                st.Rfloat();   //m_angularDeflectionTimescale);
+
+                st.Rfloat(); // m_bankingEfficiency);
+                st.Rfloat(); //m_bankingMix);
+                st.Rfloat();  //m_bankingTimescale);
+
+                m_VhoverHeight = st.Rfloat();
+                st.Rfloat();   //m_VhoverEfficiency);
+                m_VhoverTimescale = st.Rfloat();
+
+                m_VehicleBuoyancy = st.Rfloat();
+
+                m_verticalAttractionEfficiency = st.Rfloat();
+                m_verticalAttractionTimescale = st.Rfloat();
+            }
+            st.close();
+            return true;
+        }
+
+        public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, PhysicsActor parent,
+                       PrimitiveBaseShape pbs, CollisionLocker dode, uint localid, byte[] sdata)
+        {
+            m_localID = localid;
+            ode = dode;
+
+            if (parent == null)
+            {
+                m_taintparent = 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.Warn("[PHYSICS]: Got nonFinite Object create Position");
+                }
+
+                _position = pos;
+                m_taintposition = pos;
+            }
+            else
+                m_taintparent = parent;
+
+            body_autodisable_frames = parent_scene.bodyFramesAutoDisable;
+
+            prim_geom = IntPtr.Zero;
+
+            _mesh = null;
+            m_meshfailed = false;
+            _pbs = pbs;
+
+            _parent_scene = parent_scene;
+            m_targetSpace = (IntPtr)0;
+
+            if(sdata != null && sdata.Length > 1)
+                DeSerialize(sdata);
+
+            if (m_isphysical)
+                m_targetSpace = _parent_scene.space;
+
+            m_primName = primName;
+            m_taintserial = null;
+            m_taintadd = true;
+            _parent_scene.AddPhysicsActorTaint(this);
+            //  don't do .add() here; old geoms get recycled with the same hash
+        }
 
         public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size,
                        Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical, CollisionLocker dode, uint localid)
@@ -266,7 +722,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                     parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f);
                 m_log.Warn("[PHYSICS]: Got nonFinite Object create Position");
             }
-            
+
             _position = pos;
             m_taintposition = pos;
             PID_D = parent_scene.bodyPIDD;
@@ -275,9 +731,8 @@ namespace OpenSim.Region.Physics.OdePlugin
             // m_tensor = parent_scene.bodyMotorJointMaxforceTensor;
             body_autodisable_frames = parent_scene.bodyFramesAutoDisable;
 
-
             prim_geom = IntPtr.Zero;
-//            prev_geom = IntPtr.Zero;
+            //            prev_geom = IntPtr.Zero;
 
             if (!pos.IsFinite())
             {
@@ -306,7 +761,7 @@ namespace OpenSim.Region.Physics.OdePlugin
             _parent_scene = parent_scene;
             m_targetSpace = (IntPtr)0;
 
-//            if (pos.Z < 0)
+            //            if (pos.Z < 0)
             if (pos.Z < parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y))
                 m_isphysical = false;
             else
@@ -317,6 +772,8 @@ namespace OpenSim.Region.Physics.OdePlugin
                 if (m_isphysical)
                     m_targetSpace = _parent_scene.space;
             }
+
+            m_taintserial = null;
             m_primName = primName;
             m_taintadd = true;
             _parent_scene.AddPhysicsActorTaint(this);
@@ -325,7 +782,7 @@ namespace OpenSim.Region.Physics.OdePlugin
 
         public override int PhysicsActorType
         {
-            get { return (int) ActorTypes.Prim; }
+            get { return (int)ActorTypes.Prim; }
             set { return; }
         }
 
@@ -337,9 +794,11 @@ namespace OpenSim.Region.Physics.OdePlugin
 
         public override uint LocalID
         {
-            set {
+            set
+            {
                 //m_log.Info("[PHYSICS]: Setting TrackerID: " + value);
-                m_localID = value; }
+                m_localID = value;
+            }
         }
 
         public override bool Grabbed
@@ -349,9 +808,10 @@ namespace OpenSim.Region.Physics.OdePlugin
 
         public override bool Selected
         {
-            set {
-        
-//Console.WriteLine("Sel {0}  {1}  {2}", m_primName, value,   m_isphysical);        
+            set
+            {
+
+                //Console.WriteLine("Sel {0}  {1}  {2}", m_primName, value,   m_isphysical);        
                 // This only makes the object not collidable if the object
                 // is physical or the object is modified somehow *IN THE FUTURE*
                 // without this, if an avatar selects prim, they can walk right
@@ -367,21 +827,21 @@ namespace OpenSim.Region.Physics.OdePlugin
                     m_taintselected = value;
                     m_isSelected = value;
                 }
-                if(m_isSelected) disableBodySoft();         
+                if (m_isSelected) disableBodySoft();
             }
         }
 
         public override bool IsPhysical
         {
             get { return m_isphysical; }
-            set 
-            { 
-                  m_isphysical = value;
-                  if (!m_isphysical)
-                  {             // Zero the remembered last velocity
-                                m_lastVelocity = Vector3.Zero;
-                            if (m_type != Vehicle.TYPE_NONE) Halt();
-                  }
+            set
+            {
+                m_isphysical = value;
+                if (!m_isphysical)
+                {               // Zero the remembered last velocity
+                    m_lastVelocity = Vector3.Zero;
+                    if (m_type != Vehicle.TYPE_NONE) Halt();
+                }
             }
         }
 
@@ -430,7 +890,9 @@ namespace OpenSim.Region.Physics.OdePlugin
         {
             get { return _position; }
 
-            set { _position = value;
+            set
+            {
+                _position = value;
                 //m_log.Info("[PHYSICS]: " + _position.ToString());
             }
         }
@@ -481,29 +943,29 @@ namespace OpenSim.Region.Physics.OdePlugin
 
         public override void VehicleFloatParam(int param, float value)
         {
-            ProcessFloatVehicleParam((Vehicle) param, value);
+            ProcessFloatVehicleParam((Vehicle)param, value);
         }
 
         public override void VehicleVectorParam(int param, Vector3 value)
         {
-            ProcessVectorVehicleParam((Vehicle) param, value);
+            ProcessVectorVehicleParam((Vehicle)param, value);
         }
 
         public override void VehicleRotationParam(int param, Quaternion rotation)
         {
-            ProcessRotationVehicleParam((Vehicle) param, rotation);
+            ProcessRotationVehicleParam((Vehicle)param, rotation);
         }
 
         public override void VehicleFlags(int param, bool remove)
         {
             ProcessVehicleFlags(param, remove);
         }
-        
+
         public override void SetVolumeDetect(int param)
         {
             lock (_parent_scene.OdeLock)
             {
-                m_isVolumeDetect = (param!=0);
+                m_isVolumeDetect = (param != 0);
             }
         }
 
@@ -536,9 +998,9 @@ namespace OpenSim.Region.Physics.OdePlugin
                     return Vector3.Zero;
 
                 Vector3 returnVelocity = Vector3.Zero;
-                returnVelocity.X = (m_lastVelocity.X + _velocity.X)/2;
-                returnVelocity.Y = (m_lastVelocity.Y + _velocity.Y)/2;
-                returnVelocity.Z = (m_lastVelocity.Z + _velocity.Z)/2;
+                returnVelocity.X = (m_lastVelocity.X + _velocity.X) / 2;
+                returnVelocity.Y = (m_lastVelocity.Y + _velocity.Y) / 2;
+                returnVelocity.Z = (m_lastVelocity.Z + _velocity.Z) / 2;
                 return returnVelocity;
             }
             set
@@ -546,8 +1008,8 @@ namespace OpenSim.Region.Physics.OdePlugin
                 if (value.IsFinite())
                 {
                     _velocity = value;
-                    if (_velocity.ApproxEquals(Vector3.Zero,0.001f))
-                                _acceleration = Vector3.Zero;   
+                    if (_velocity.ApproxEquals(Vector3.Zero, 0.001f))
+                        _acceleration = Vector3.Zero;
 
                     m_taintVelocity = value;
                     _parent_scene.AddPhysicsActorTaint(this);
@@ -604,17 +1066,18 @@ namespace OpenSim.Region.Physics.OdePlugin
                 if (QuaternionIsFinite(value))
                 {
                     _orientation = value;
-                                }
+                }
                 else
                     m_log.Warn("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object");
 
             }
         }
-        
-        
-                public override bool FloatOnWater
+
+
+        public override bool FloatOnWater
         {
-            set {
+            set
+            {
                 m_taintCollidesWater = value;
                 _parent_scene.AddPhysicsActorTaint(this);
             }
@@ -624,8 +1087,8 @@ namespace OpenSim.Region.Physics.OdePlugin
         {
         }
 
-        public override Vector3 PIDTarget 
-        { 
+        public override Vector3 PIDTarget
+        {
             set
             {
                 if (value.IsFinite())
@@ -634,16 +1097,16 @@ namespace OpenSim.Region.Physics.OdePlugin
                 }
                 else
                     m_log.Warn("[PHYSICS]: Got NaN PIDTarget from Scene on Object");
-            } 
+            }
         }
         public override bool PIDActive { set { m_usePID = value; } }
         public override float PIDTau { set { m_PIDTau = value; } }
-        
-                // For RotLookAt        
-        public  override Quaternion APIDTarget { set { m_APIDTarget = value; } }
-        public  override bool APIDActive { set { m_useAPID = value; } }
-        public  override float APIDStrength { set { m_APIDStrength = value; } }
-        public  override float APIDDamping { set { m_APIDDamping = value; } }
+
+        // For RotLookAt        
+        public override Quaternion APIDTarget { set { m_APIDTarget = value; } }
+        public override bool APIDActive { set { m_useAPID = value; } }
+        public override float APIDStrength { set { m_APIDStrength = value; } }
+        public override float APIDDamping { set { m_APIDDamping = value; } }
 
         public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } }
         public override bool PIDHoverActive { set { m_useHoverPID = value; } }
@@ -665,13 +1128,13 @@ namespace OpenSim.Region.Physics.OdePlugin
 
         public override Vector3 Acceleration            // client updates read data via here
         {
-            get 
-            { 
+            get
+            {
                 if (_zeroFlag)
                 {
                     return Vector3.Zero;
                 }
-                return _acceleration; 
+                return _acceleration;
             }
             set { _acceleration = value; }
         }
@@ -752,18 +1215,18 @@ namespace OpenSim.Region.Physics.OdePlugin
                 base.RequestPhysicsterseUpdate();
                 m_outofBounds = false;
             }
-/*
-            int tmp = Interlocked.Increment(ref m_crossingfailures);
-            if (tmp > _parent_scene.geomCrossingFailuresBeforeOutofbounds)
-            {
-                base.RaiseOutOfBounds(_position);
-                return;
-            }
-            else if (tmp == _parent_scene.geomCrossingFailuresBeforeOutofbounds)
-            {
-                m_log.Warn("[PHYSICS]: Too many crossing failures for: " + m_primName);
-            }
- */
+            /*
+                        int tmp = Interlocked.Increment(ref m_crossingfailures);
+                        if (tmp > _parent_scene.geomCrossingFailuresBeforeOutofbounds)
+                        {
+                            base.RaiseOutOfBounds(_position);
+                            return;
+                        }
+                        else if (tmp == _parent_scene.geomCrossingFailuresBeforeOutofbounds)
+                        {
+                            m_log.Warn("[PHYSICS]: Too many crossing failures for: " + m_primName);
+                        }
+             */
         }
 
         public override float Buoyancy
@@ -806,26 +1269,26 @@ namespace OpenSim.Region.Physics.OdePlugin
 
         public void SetGeom(IntPtr geom)
         {
-            if(prim_geom != IntPtr.Zero)
-            {
-                // Remove any old entries
-//string tPA;
-//_parent_scene.geom_name_map.TryGetValue(prim_geom, out tPA);
-//Console.WriteLine("**** Remove {0}", tPA);
-                if(_parent_scene.geom_name_map.ContainsKey(prim_geom)) _parent_scene.geom_name_map.Remove(prim_geom);
-                if(_parent_scene.actor_name_map.ContainsKey(prim_geom)) _parent_scene.actor_name_map.Remove(prim_geom);
+            if (prim_geom != IntPtr.Zero)
+            {
+                // Remove any old entries
+                //string tPA;
+                //_parent_scene.geom_name_map.TryGetValue(prim_geom, out tPA);
+                //Console.WriteLine("**** Remove {0}", tPA);
+                if (_parent_scene.geom_name_map.ContainsKey(prim_geom)) _parent_scene.geom_name_map.Remove(prim_geom);
+                if (_parent_scene.actor_name_map.ContainsKey(prim_geom)) _parent_scene.actor_name_map.Remove(prim_geom);
                 d.GeomDestroy(prim_geom);
-                }
+            }
 
             prim_geom = geom;
-//Console.WriteLine("SetGeom to " + prim_geom + " for " + m_primName);     
+            //Console.WriteLine("SetGeom to " + prim_geom + " for " + m_primName);     
             if (prim_geom != IntPtr.Zero)
             {
                 _parent_scene.geom_name_map[prim_geom] = this.m_primName;
                 _parent_scene.actor_name_map[prim_geom] = (PhysicsActor)this;
                 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
                 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
-//Console.WriteLine("**** Create {2}    Dicts: actor={0}   name={1}", _parent_scene.actor_name_map.Count, _parent_scene.geom_name_map.Count, this.m_primName);
+                //Console.WriteLine("**** Create {2}    Dicts: actor={0}   name={1}", _parent_scene.actor_name_map.Count, _parent_scene.geom_name_map.Count, this.m_primName);
             }
 
             if (childPrim)
@@ -833,7 +1296,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                 if (_parent != null && _parent is OdePrim)
                 {
                     OdePrim parent = (OdePrim)_parent;
-//Console.WriteLine("SetGeom calls ChildSetGeom");                    
+                    //Console.WriteLine("SetGeom calls ChildSetGeom");                    
                     parent.ChildSetGeom(this);
                 }
             }
@@ -848,7 +1311,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                 {
                     d.BodyEnable(Body);
                     if (m_type != Vehicle.TYPE_NONE)
-                            Enable(Body, _parent_scene);
+                        Enable(Body, _parent_scene);
                 }
 
                 m_disabled = false;
@@ -892,9 +1355,9 @@ namespace OpenSim.Region.Physics.OdePlugin
 
                 d.BodySetAutoDisableFlag(Body, true);
                 d.BodySetAutoDisableSteps(Body, body_autodisable_frames);
-                
+
                 // disconnect from world gravity so we can apply buoyancy
-                                d.BodySetGravityMode (Body, false);
+                d.BodySetGravityMode(Body, false);
 
                 m_interpenetrationcount = 0;
                 m_collisionscore = 0;
@@ -918,19 +1381,19 @@ namespace OpenSim.Region.Physics.OdePlugin
 
             float returnMass = 0;
             float hollowAmount = (float)_pbs.ProfileHollow * 2.0e-5f;
-            float hollowVolume = hollowAmount * hollowAmount; 
-            
+            float hollowVolume = hollowAmount * hollowAmount;
+
             switch (_pbs.ProfileShape)
             {
                 case ProfileShape.Square:
                     // default box
 
                     if (_pbs.PathCurve == (byte)Extrusion.Straight)
-                        {
+                    {
                         if (hollowAmount > 0.0)
-                            {
+                        {
                             switch (_pbs.HollowShape)
-                                {
+                            {
                                 case HollowShape.Square:
                                 case HollowShape.Same:
                                     break;
@@ -948,31 +1411,31 @@ namespace OpenSim.Region.Physics.OdePlugin
                                 default:
                                     hollowVolume = 0;
                                     break;
-                                }
-                            volume *= (1.0f - hollowVolume);
                             }
+                            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;
-                        
+                        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;;
+                                    hollowVolume *= 0.78539816339f; ;
                                     break;
 
                                 case HollowShape.Triangle:
@@ -981,23 +1444,23 @@ namespace OpenSim.Region.Physics.OdePlugin
                                 default:
                                     hollowVolume = 0;
                                     break;
-                                }
-                            volume *= (1.0f - hollowVolume);
                             }
+                            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;
@@ -1013,25 +1476,25 @@ namespace OpenSim.Region.Physics.OdePlugin
                                 default:
                                     hollowVolume = 0;
                                     break;
-                                }
-                            volume *= (1.0f - hollowVolume);
                             }
+                            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;
@@ -1047,31 +1510,31 @@ namespace OpenSim.Region.Physics.OdePlugin
                                 default:
                                     hollowVolume = 0;
                                     break;
-                                }
-                            volume *= (1.0f - hollowVolume);
                             }
+                            volume *= (1.0f - hollowVolume);
                         }
+                    }
                     break;
 
                 case ProfileShape.HalfCircle:
                     if (_pbs.PathCurve == (byte)Extrusion.Curve1)
                     {
-                    volume *= 0.52359877559829887307710723054658f;
+                        volume *= 0.52359877559829887307710723054658f;
                     }
                     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;
@@ -1091,24 +1554,24 @@ namespace OpenSim.Region.Physics.OdePlugin
                                 default:
                                     hollowVolume = 0;
                                     break;
-                                }
-                            volume *= (1.0f - hollowVolume);
                             }
+                            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;
@@ -1126,15 +1589,15 @@ namespace OpenSim.Region.Physics.OdePlugin
                                 default:
                                     hollowVolume = 0;
                                     break;
-                                }
-                            volume *= (1.0f - hollowVolume);
                             }
+                            volume *= (1.0f - hollowVolume);
                         }
-                        break;                  
+                    }
+                    break;
 
                 default:
                     break;
-                }
+            }
 
 
 
@@ -1148,7 +1611,7 @@ namespace OpenSim.Region.Physics.OdePlugin
             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;
@@ -1158,9 +1621,9 @@ namespace OpenSim.Region.Physics.OdePlugin
                 if (taperY1 > 1.0f)
                     taperY1 = 2.0f - taperY1;
                 taperY = 1.0f - taperY1;
-                }
+            }
             else
-                {
+            {
                 taperX = _pbs.PathTaperX * 0.01f;
                 if (taperX < 0.0f)
                     taperX = -taperX;
@@ -1168,10 +1631,10 @@ namespace OpenSim.Region.Physics.OdePlugin
 
                 taperY = _pbs.PathTaperY * 0.01f;
                 if (taperY < 0.0f)
-                    taperY = -taperY;               
+                    taperY = -taperY;
                 taperY1 = 1.0f - taperY;
 
-                }
+            }
 
 
             volume *= (taperX1 * taperY1 + 0.5f * (taperX1 * taperY + taperX * taperY1) + 0.3333333333f * taperX * taperY);
@@ -1180,7 +1643,7 @@ namespace OpenSim.Region.Physics.OdePlugin
             pathEnd = 1.0f - (float)_pbs.PathEnd * 2.0e-5f;
             volume *= (pathEnd - pathBegin);
 
-// this is crude aproximation
+            // this is crude aproximation
             profileBegin = (float)_pbs.ProfileBegin * 2.0e-5f;
             profileEnd = 1.0f - (float)_pbs.ProfileEnd * 2.0e-5f;
             volume *= (profileEnd - profileBegin);
@@ -1189,8 +1652,8 @@ namespace OpenSim.Region.Physics.OdePlugin
 
             if (returnMass <= 0)
                 returnMass = 0.0001f;//ckrinke: Mass must be greater then zero.
-//            else if (returnMass > _parent_scene.maximumMassObject)
-//                returnMass = _parent_scene.maximumMassObject;
+            //            else if (returnMass > _parent_scene.maximumMassObject)
+            //                returnMass = _parent_scene.maximumMassObject;
 
 
 
@@ -1230,7 +1693,7 @@ namespace OpenSim.Region.Physics.OdePlugin
 
         public void setMass()
         {
-            if (Body != (IntPtr) 0)
+            if (Body != (IntPtr)0)
             {
                 float newmass = CalculateMass();
 
@@ -1260,7 +1723,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                             d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
                         }
 
-                        
+
                         d.BodyDestroy(Body);
                         lock (childrenPrim)
                         {
@@ -1279,7 +1742,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                 else
                 {
                     _parent_scene.remActivePrim(this);
-                    
+
                     m_collisionCategories &= ~CollisionCategories.Body;
                     m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land);
 
@@ -1289,7 +1752,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                         d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
                     }
 
-                    
+
                     Body = IntPtr.Zero;
                 }
             }
@@ -1345,10 +1808,10 @@ namespace OpenSim.Region.Physics.OdePlugin
             _parent_scene.waitForSpaceUnlock(m_targetSpace);
             try
             {
-            //    if (prim_geom == IntPtr.Zero)  // setGeom takes care of phys engine recreate and prim_geom pointer 
-            //    {
-                    SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, parent_scene.triCallback, null, null));
-            //    }
+                //    if (prim_geom == IntPtr.Zero)  // setGeom takes care of phys engine recreate and prim_geom pointer 
+                //    {
+                SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, parent_scene.triCallback, null, null));
+                //    }
             }
             catch (AccessViolationException)
             {
@@ -1357,14 +1820,14 @@ namespace OpenSim.Region.Physics.OdePlugin
             }
 
 
-           // if (IsPhysical && Body == (IntPtr) 0)
-           // {
-                // Recreate the body
-          //     m_interpenetrationcount = 0;
-           //     m_collisionscore = 0;
+            // if (IsPhysical && Body == (IntPtr) 0)
+            // {
+            // Recreate the body
+            //     m_interpenetrationcount = 0;
+            //     m_collisionscore = 0;
 
-           //     enableBody();
-           // }
+            //     enableBody();
+            // }
         }
 
         public void ProcessTaints(float timestep) //=============================================================================
@@ -1373,37 +1836,37 @@ namespace OpenSim.Region.Physics.OdePlugin
             {
                 changeadd(timestep);
             }
-            
+
             if (prim_geom != IntPtr.Zero)
             {
-                         if (!_position.ApproxEquals(m_taintposition, 0f))
+                if (!_position.ApproxEquals(m_taintposition, 0f))
                 {
-                            changemove(timestep);
+                    changemove(timestep);
                 }
-                     if (m_taintrot != _orientation)
-                     {
-                        if(childPrim && IsPhysical)     // For physical child prim...
-                        {
-                            rotate(timestep);       
-                            // KF: ODE will also rotate the parent prim!
-                            // so rotate the root back to where it was
-                            OdePrim parent = (OdePrim)_parent;
-                            parent.rotate(timestep);
+                if (m_taintrot != _orientation)
+                {
+                    if (childPrim && IsPhysical)        // For physical child prim...
+                    {
+                        rotate(timestep);
+                        // KF: ODE will also rotate the parent prim!
+                        // so rotate the root back to where it was
+                        OdePrim parent = (OdePrim)_parent;
+                        parent.rotate(timestep);
                     }
                     else
                     {
-                            //Just rotate the prim
-                            rotate(timestep);
-                    }  
+                        //Just rotate the prim
+                        rotate(timestep);
+                    }
                 }
                 //
-            
+
                 if (m_taintPhysics != m_isphysical && !(m_taintparent != _parent))
                 {
                     changePhysicsStatus(timestep);
                 }//
 
-                if (!_size.ApproxEquals(m_taintsize,0f))
+                if (!_size.ApproxEquals(m_taintsize, 0f))
                     changesize(timestep);
                 //
 
@@ -1434,10 +1897,17 @@ namespace OpenSim.Region.Physics.OdePlugin
 
                 if (m_taintCollidesWater != m_collidesWater)
                     changefloatonwater(timestep);
-/* obsolete                    
-                if (!m_angularLock.ApproxEquals(m_taintAngularLock,0f))
-                    changeAngularLock(timestep);
- */
+
+                if (m_taintvehicledata != null)
+                    DoSetVehicle();
+
+                if (m_taintserial != null)
+                    DoSerialize(m_taintserial);
+
+                /* obsolete                    
+                                if (!m_angularLock.ApproxEquals(m_taintAngularLock,0f))
+                                    changeAngularLock(timestep);
+                 */
             }
             else
             {
@@ -1445,16 +1915,16 @@ namespace OpenSim.Region.Physics.OdePlugin
             }
         }
 
-/* obsolete
-        private void changeAngularLock(float timestep) 
-        {
-            if (_parent == null)
-            {
-                m_angularLock = m_taintAngularLock;
-                m_angularLockSet = true;
-            }
-        }
- */
+        /* obsolete
+                private void changeAngularLock(float timestep) 
+                {
+                    if (_parent == null)
+                    {
+                        m_angularLock = m_taintAngularLock;
+                        m_angularLockSet = true;
+                    }
+                }
+         */
         private void changelink(float timestep)
         {
             // If the newly set parent is not null
@@ -1489,7 +1959,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                     childPrim = false;
                     //_parent = null;
                 }
-                
+
                 /*
                     if (Body != (IntPtr)0 && _linkJointGroup != (IntPtr)0)
                     d.JointGroupDestroy(_linkJointGroup);
@@ -1498,7 +1968,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                     m_linkJoint = (IntPtr)0;
                 */
             }
- 
+
             _parent = m_taintparent;
             m_taintPhysics = m_isphysical;
         }
@@ -1512,8 +1982,8 @@ namespace OpenSim.Region.Physics.OdePlugin
                 if (Body == IntPtr.Zero)
                 {
                     Body = d.BodyCreate(_parent_scene.world);
-                // disconnect from world gravity so we can apply buoyancy
-                                d.BodySetGravityMode (Body, false);
+                    // disconnect from world gravity so we can apply buoyancy
+                    d.BodySetGravityMode(Body, false);
 
                     setMass();
                 }
@@ -1524,7 +1994,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                         if (!childrenPrim.Contains(prim))
                         {
                             childrenPrim.Add(prim);
-                            
+
                             foreach (OdePrim prm in childrenPrim)
                             {
                                 d.Mass m2;
@@ -1546,7 +2016,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                             }
                             foreach (OdePrim prm in childrenPrim)
                             {
-                       
+
                                 prm.m_collisionCategories |= CollisionCategories.Body;
                                 prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
 
@@ -1571,7 +2041,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                                 {
                                     d.GeomSetBody(prm.prim_geom, Body);
                                     prm.childPrim = true;
-                                    d.GeomSetOffsetWorldPosition(prm.prim_geom, prm.Position.X , prm.Position.Y, prm.Position.Z);
+                                    d.GeomSetOffsetWorldPosition(prm.prim_geom, prm.Position.X, prm.Position.Y, prm.Position.Z);
                                     //d.GeomSetOffsetPosition(prim.prim_geom,
                                     //    (Position.X - prm.Position.X) - pMass.c.X,
                                     //    (Position.Y - prm.Position.Y) - pMass.c.Y,
@@ -1668,7 +2138,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                     ParentPrim(prm);
                 }
             }
-            
+
         }
 
         private void ChildDelink(OdePrim odePrim)
@@ -1719,7 +2189,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                 // in between the disabling and the collision properties setting
                 // which would wake the physical body up from a soft disabling and potentially cause it to fall
                 // through the ground.
-                
+
                 // NOTE FOR JOINTS: this doesn't always work for jointed assemblies because if you select
                 // just one part of the assembly, the rest of the assembly is non-selected and still simulating,
                 // so that causes the selected part to wake up and continue moving.
@@ -1752,12 +2222,12 @@ namespace OpenSim.Region.Physics.OdePlugin
                 {
                     disableBodySoft();
                 }
-            if (Body != IntPtr.Zero)
+                if (Body != IntPtr.Zero)
                 {
                     d.BodySetLinearVel(Body, 0f, 0f, 0f);
                     d.BodySetForce(Body, 0f, 0f, 0f);
-                    d.BodySetAngularVel (Body, 0.0f, 0.0f, 0.0f);
-                    d.BodySetTorque (Body, 0.0f, 0.0f, 0.0f);
+                    d.BodySetAngularVel(Body, 0.0f, 0.0f, 0.0f);
+                    d.BodySetTorque(Body, 0.0f, 0.0f, 0.0f);
                 }
 
             }
@@ -1780,12 +2250,12 @@ namespace OpenSim.Region.Physics.OdePlugin
                     d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
                     d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
                 }
-            if (Body != IntPtr.Zero)
+                if (Body != IntPtr.Zero)
                 {
                     d.BodySetLinearVel(Body, 0f, 0f, 0f);
                     d.BodySetForce(Body, 0f, 0f, 0f);
-                    d.BodySetAngularVel (Body, 0.0f, 0.0f, 0.0f);
-                    d.BodySetTorque (Body, 0.0f, 0.0f, 0.0f);
+                    d.BodySetAngularVel(Body, 0.0f, 0.0f, 0.0f);
+                    d.BodySetTorque(Body, 0.0f, 0.0f, 0.0f);
                 }
 
                 if (m_isphysical)
@@ -1913,7 +2383,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                         m_meshfailed = true;
                     }
                     // createmesh returns null when it's a shape that isn't a cube.
-                   // m_log.Debug(m_localID);
+                    // m_log.Debug(m_localID);
                 }
             }
 
@@ -1948,7 +2418,7 @@ namespace OpenSim.Region.Physics.OdePlugin
         {
             if (m_isphysical)
             {
-//                if (!m_disabled && !m_taintremove && !childPrim)  After one edit m_disabled is sometimes set, disabling further edits!
+                //                if (!m_disabled && !m_taintremove && !childPrim)  After one edit m_disabled is sometimes set, disabling further edits!
                 if (!m_taintremove && !childPrim)
                 {
                     if (Body == IntPtr.Zero)
@@ -1972,8 +2442,8 @@ namespace OpenSim.Region.Physics.OdePlugin
                             OdePrim odParent = (OdePrim)_parent;
                             if (Body != (IntPtr)0 && odParent.Body != (IntPtr)0 && Body != odParent.Body)
                             {
-// KF: Fixed Joints were removed? Anyway - this Console.WriteLine does not show up, so routine is not used??
-Console.WriteLine("ODEPrim JointCreateFixed !!!");                           
+                                // KF: Fixed Joints were removed? Anyway - this Console.WriteLine does not show up, so routine is not used??
+                                Console.WriteLine("ODEPrim JointCreateFixed !!!");
                                 m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup);
                                 d.JointAttach(m_linkJoint, Body, odParent.Body);
                                 d.JointSetFixed(m_linkJoint);
@@ -1991,8 +2461,8 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
                     }
                 }
                 //else
-               // {
-                    //m_log.Debug("[BUG]: race!");
+                // {
+                //m_log.Debug("[BUG]: race!");
                 //}
             }
             else
@@ -2031,15 +2501,15 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
             myrot.W = _orientation.W;
             if (Body != IntPtr.Zero)
             {
-                    // KF: If this is a root prim do BodySet
+                // KF: If this is a root prim do BodySet
                 d.BodySetQuaternion(Body, ref myrot);
-                }
-                else
-                {
-                        // daughter prim, do Geom set
-                    d.GeomSetQuaternion(prim_geom, ref myrot);
             }
-            
+            else
+            {
+                // daughter prim, do Geom set
+                d.GeomSetQuaternion(prim_geom, ref myrot);
+            }
+
             resetCollisionAccounting();
             m_taintrot = _orientation;
         }
@@ -2111,7 +2581,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
 
         public void changesize(float timestamp)
         {
-            
+
             string oldname = _parent_scene.geom_name_map[prim_geom];
 
             if (_size.X <= 0) _size.X = 0.01f;
@@ -2170,7 +2640,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
                 //IMesh mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical);
                 CreateGeom(m_targetSpace, mesh);
 
-               
+
             }
             else
             {
@@ -2210,7 +2680,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
             m_taintsize = _size;
         }
 
-       
+
 
         public void changefloatonwater(float timestep)
         {
@@ -2398,7 +2868,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
                         }
                         d.BodyEnable(Body);
                         d.BodyAddTorque(Body, iforce.X, iforce.Y, iforce.Z);
-                        
+
                     }
                     m_angularforcelist.Clear();
                 }
@@ -2420,7 +2890,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
                     if (Body != IntPtr.Zero)
                         d.BodySetLinearVel(Body, m_taintVelocity.X, m_taintVelocity.Y, m_taintVelocity.Z);
                 }
-                
+
                 //resetCollisionAccounting();
             }
             m_taintVelocity = Vector3.Zero;
@@ -2428,9 +2898,9 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
 
         public void UpdatePositionAndVelocity()
         {
-                return;   // moved to the Move () method
+            return;   // moved to the Move () method
         }
-        
+
         public d.Mass FromMatrix4(Matrix4 pMat, ref d.Mass obj)
         {
             obj.I.M00 = pMat[0, 0];
@@ -2455,7 +2925,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
         {
             _parent_scene.remCollisionEventReporting(this);
             m_eventsubscription = 0;
-        }        
+        }
 
         public void AddCollisionEvent(uint CollidedWith, ContactPoint contact)
         {
@@ -2499,9 +2969,9 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
         public static Matrix4 Adjoint(Matrix4 pMat)
         {
             Matrix4 adjointMatrix = new Matrix4();
-            for (int i=0; i<4; i++)
+            for (int i = 0; i < 4; i++)
             {
-                for (int j=0; j<4; j++)
+                for (int j = 0; j < 4; j++)
                 {
                     Matrix4SetValue(ref adjointMatrix, i, j, (float)(Math.Pow(-1, i + j) * (determinant3x3(Minor(pMat, i, j)))));
                 }
@@ -2524,7 +2994,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
                 {
                     if (j == iCol)
                         continue;
-                    Matrix4SetValue(ref minor, m,n, matrix[i, j]);
+                    Matrix4SetValue(ref minor, m, n, matrix[i, j]);
                     n++;
                 }
                 m++;
@@ -2622,18 +3092,18 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
         private static float determinant3x3(Matrix4 pMat)
         {
             float det = 0;
-            float diag1 = pMat[0, 0]*pMat[1, 1]*pMat[2, 2];
-            float diag2 = pMat[0, 1]*pMat[2, 1]*pMat[2, 0];
-            float diag3 = pMat[0, 2]*pMat[1, 0]*pMat[2, 1];
-            float diag4 = pMat[2, 0]*pMat[1, 1]*pMat[0, 2];
-            float diag5 = pMat[2, 1]*pMat[1, 2]*pMat[0, 0];
-            float diag6 = pMat[2, 2]*pMat[1, 0]*pMat[0, 1];
+            float diag1 = pMat[0, 0] * pMat[1, 1] * pMat[2, 2];
+            float diag2 = pMat[0, 1] * pMat[2, 1] * pMat[2, 0];
+            float diag3 = pMat[0, 2] * pMat[1, 0] * pMat[2, 1];
+            float diag4 = pMat[2, 0] * pMat[1, 1] * pMat[0, 2];
+            float diag5 = pMat[2, 1] * pMat[1, 2] * pMat[0, 0];
+            float diag6 = pMat[2, 2] * pMat[1, 0] * pMat[0, 1];
 
             det = diag1 + diag2 + diag3 - (diag4 + diag5 + diag6);
             return det;
 
         }
-        
+
         private static void DMassCopy(ref d.Mass src, ref d.Mass dst)
         {
             dst.c.W = src.c.W;
@@ -2690,15 +3160,15 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
                     // m_bankingTimescale = pValue;
                     break;
                 case Vehicle.BUOYANCY:
-                        if (pValue < -1f) pValue = -1f;
-                        if (pValue > 1f) pValue = 1f;
+                    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_EFFICIENCY:
+                //                      if (pValue < 0f) pValue = 0f;
+                //                      if (pValue > 1f) pValue = 1f;
+                //                    m_VhoverEfficiency = pValue;
+                //                    break;
                 case Vehicle.HOVER_HEIGHT:
                     m_VhoverHeight = pValue;
                     break;
@@ -2731,12 +3201,12 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
                     if (pValue < 0.1f) pValue = 0.1f;
                     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 > 30f) pValue = 30f;
-                        if (pValue < 0.1f) pValue = 0.1f;
+                    if (pValue > 30f) pValue = 30f;
+                    if (pValue < 0.1f) pValue = 0.1f;
                     m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue);
                     break;
                 case Vehicle.ANGULAR_MOTOR_DIRECTION:
@@ -2744,7 +3214,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
                     UpdateAngDecay();
                     break;
                 case Vehicle.LINEAR_FRICTION_TIMESCALE:
-                        if (pValue < 0.1f) pValue = 0.1f;
+                    if (pValue < 0.1f) pValue = 0.1f;
                     m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue);
                     break;
                 case Vehicle.LINEAR_MOTOR_DIRECTION:
@@ -2756,7 +3226,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
                     break;
 
             }
-            
+
         }//end ProcessFloatVehicleParam
 
         internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue)
@@ -2764,29 +3234,29 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
             switch (pParam)
             {
                 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
-                        if (pValue.X > 30f) pValue.X = 30f;
-                        if (pValue.X < 0.1f) pValue.X = 0.1f;
-                        if (pValue.Y > 30f) pValue.Y = 30f;
-                        if (pValue.Y < 0.1f) pValue.Y = 0.1f;
-                        if (pValue.Z > 30f) pValue.Z = 30f;
-                        if (pValue.Z < 0.1f) pValue.Z = 0.1f;
+                    if (pValue.X > 30f) pValue.X = 30f;
+                    if (pValue.X < 0.1f) pValue.X = 0.1f;
+                    if (pValue.Y > 30f) pValue.Y = 30f;
+                    if (pValue.Y < 0.1f) pValue.Y = 0.1f;
+                    if (pValue.Z > 30f) pValue.Z = 30f;
+                    if (pValue.Z < 0.1f) pValue.Z = 0.1f;
                     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
-                    if(m_angularMotorDirection.X > 12.56f) m_angularMotorDirection.X = 12.56f; 
-                    if(m_angularMotorDirection.X < - 12.56f) m_angularMotorDirection.X = - 12.56f; 
-                    if(m_angularMotorDirection.Y > 12.56f) m_angularMotorDirection.Y = 12.56f; 
-                    if(m_angularMotorDirection.Y < - 12.56f) m_angularMotorDirection.Y = - 12.56f; 
-                    if(m_angularMotorDirection.Z > 12.56f) m_angularMotorDirection.Z = 12.56f; 
-                    if(m_angularMotorDirection.Z < - 12.56f) m_angularMotorDirection.Z = - 12.56f; 
+                    if (m_angularMotorDirection.X > 12.56f) m_angularMotorDirection.X = 12.56f;
+                    if (m_angularMotorDirection.X < -12.56f) m_angularMotorDirection.X = -12.56f;
+                    if (m_angularMotorDirection.Y > 12.56f) m_angularMotorDirection.Y = 12.56f;
+                    if (m_angularMotorDirection.Y < -12.56f) m_angularMotorDirection.Y = -12.56f;
+                    if (m_angularMotorDirection.Z > 12.56f) m_angularMotorDirection.Z = 12.56f;
+                    if (m_angularMotorDirection.Z < -12.56f) m_angularMotorDirection.Z = -12.56f;
                     UpdateAngDecay();
                     break;
                 case Vehicle.LINEAR_FRICTION_TIMESCALE:
-                        if (pValue.X < 0.1f) pValue.X = 0.1f;
-                        if (pValue.Y < 0.1f) pValue.Y = 0.1f;
-                        if (pValue.Z < 0.1f) pValue.Z = 0.1f;
+                    if (pValue.X < 0.1f) pValue.X = 0.1f;
+                    if (pValue.Y < 0.1f) pValue.Y = 0.1f;
+                    if (pValue.Z < 0.1f) pValue.Z = 0.1f;
                     m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
                     break;
                 case Vehicle.LINEAR_MOTOR_DIRECTION:
@@ -2797,7 +3267,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
                     // m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z);
                     break;
             }
-            
+
         }//end ProcessVectorVehicleParam
 
         internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue)
@@ -2808,31 +3278,31 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
                     // m_referenceFrame = pValue;
                     break;
             }
-            
+
         }//end ProcessRotationVehicleParam
-        
+
         internal void ProcessVehicleFlags(int pParam, bool remove)
         {
             if (remove)
             {
-                        m_flags &= ~((VehicleFlag)pParam);         
-                        }
-                        else
-                        {
-                        m_flags |= (VehicleFlag)pParam;
-                }
+                m_flags &= ~((VehicleFlag)pParam);
+            }
+            else
+            {
+                m_flags |= (VehicleFlag)pParam;
+            }
         }
-        
+
         internal void ProcessTypeChange(Vehicle pType)
         {
-                        // Set Defaults For Type
+            // Set Defaults For Type
             m_type = pType;
             switch (pType)
             {
                 case Vehicle.TYPE_SLED:
                     m_linearFrictionTimescale = new Vector3(30, 1, 1000);
                     m_angularFrictionTimescale = new Vector3(30, 30, 30);
-//                     m_lLinMotorVel = Vector3.Zero;
+                    //                     m_lLinMotorVel = Vector3.Zero;
                     m_linearMotorTimescale = 1000;
                     m_linearMotorDecayTimescale = 120;
                     m_angularMotorDirection = Vector3.Zero;
@@ -2840,7 +3310,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
                     m_angularMotorTimescale = 1000;
                     m_angularMotorDecayTimescale = 120;
                     m_VhoverHeight = 0;
-//                    m_VhoverEfficiency = 1;
+                    //                    m_VhoverEfficiency = 1;
                     m_VhoverTimescale = 10;
                     m_VehicleBuoyancy = 0;
                     // m_linearDeflectionEfficiency = 1;
@@ -2859,7 +3329,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
                 case Vehicle.TYPE_CAR:
                     m_linearFrictionTimescale = new Vector3(100, 2, 1000);
                     m_angularFrictionTimescale = new Vector3(30, 30, 30);               // was 1000, but sl max frict time is 30.
-//                     m_lLinMotorVel = Vector3.Zero;
+                    //                     m_lLinMotorVel = Vector3.Zero;
                     m_linearMotorTimescale = 1;
                     m_linearMotorDecayTimescale = 60;
                     m_angularMotorDirection = Vector3.Zero;
@@ -2867,7 +3337,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
                     m_angularMotorTimescale = 1;
                     m_angularMotorDecayTimescale = 0.8f;
                     m_VhoverHeight = 0;
-//                    m_VhoverEfficiency = 0;
+                    //                    m_VhoverEfficiency = 0;
                     m_VhoverTimescale = 1000;
                     m_VehicleBuoyancy = 0;
                     // // m_linearDeflectionEfficiency = 1;
@@ -2886,8 +3356,8 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
                     break;
                 case Vehicle.TYPE_BOAT:
                     m_linearFrictionTimescale = new Vector3(10, 3, 2);
-                    m_angularFrictionTimescale = new Vector3(10,10,10);
-//                     m_lLinMotorVel = Vector3.Zero;
+                    m_angularFrictionTimescale = new Vector3(10, 10, 10);
+                    //                     m_lLinMotorVel = Vector3.Zero;
                     m_linearMotorTimescale = 5;
                     m_linearMotorDecayTimescale = 60;
                     m_angularMotorDirection = Vector3.Zero;
@@ -2895,7 +3365,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
                     m_angularMotorTimescale = 4;
                     m_angularMotorDecayTimescale = 4;
                     m_VhoverHeight = 0;
-//                    m_VhoverEfficiency = 0.5f;
+                    //                    m_VhoverEfficiency = 0.5f;
                     m_VhoverTimescale = 2;
                     m_VehicleBuoyancy = 1;
                     // m_linearDeflectionEfficiency = 0.5f;
@@ -2908,15 +3378,15 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
                     // m_bankingMix = 0.8f;
                     // m_bankingTimescale = 1;
                     // m_referenceFrame = Quaternion.Identity;
-                    m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.LIMIT_ROLL_ONLY | 
-                                VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
+                    m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.LIMIT_ROLL_ONLY |
+                            VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
                     m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY |
                                 VehicleFlag.LIMIT_MOTOR_UP);
                     break;
                 case Vehicle.TYPE_AIRPLANE:
                     m_linearFrictionTimescale = new Vector3(200, 10, 5);
                     m_angularFrictionTimescale = new Vector3(20, 20, 20);
-//                     m_lLinMotorVel = Vector3.Zero;
+                    //                     m_lLinMotorVel = Vector3.Zero;
                     m_linearMotorTimescale = 2;
                     m_linearMotorDecayTimescale = 60;
                     m_angularMotorDirection = Vector3.Zero;
@@ -2924,7 +3394,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
                     m_angularMotorTimescale = 4;
                     m_angularMotorDecayTimescale = 4;
                     m_VhoverHeight = 0;
-//                    m_VhoverEfficiency = 0.5f;
+                    //                    m_VhoverEfficiency = 0.5f;
                     m_VhoverTimescale = 1000;
                     m_VehicleBuoyancy = 0;
                     // m_linearDeflectionEfficiency = 0.5f;
@@ -2951,7 +3421,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
                     m_angularMotorTimescale = 6;
                     m_angularMotorDecayTimescale = 10;
                     m_VhoverHeight = 5;
-//                    m_VhoverEfficiency = 0.8f;
+                    //                    m_VhoverEfficiency = 0.8f;
                     m_VhoverTimescale = 10;
                     m_VehicleBuoyancy = 1;
                     // m_linearDeflectionEfficiency = 0;
@@ -2981,63 +3451,48 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
         }
 
 
-                internal void Halt()
-                {       // Kill all motions, when non-physical
-                //      m_linearMotorDirection = Vector3.Zero;
-                        m_lLinMotorDVel = Vector3.Zero;
-                        m_lLinObjectVel = Vector3.Zero;                                         
-                        m_wLinObjectVel = Vector3.Zero;
-                        m_angularMotorDirection = Vector3.Zero;         
-                        m_lastAngularVelocity = Vector3.Zero;
-                        m_angularMotorDVel = Vector3.Zero;
-                        _acceleration = Vector3.Zero;   
-                }
-                
-                private void UpdateLinDecay()
-                {
-//                      if (Math.Abs(m_linearMotorDirection.X) > Math.Abs(m_lLinMotorDVel.X)) m_lLinMotorDVel.X = m_linearMotorDirection.X;
-//                      if (Math.Abs(m_linearMotorDirection.Y) > Math.Abs(m_lLinMotorDVel.Y)) m_lLinMotorDVel.Y = m_linearMotorDirection.Y;
-//                      if (Math.Abs(m_linearMotorDirection.Z) > Math.Abs(m_lLinMotorDVel.Z)) m_lLinMotorDVel.Z = m_linearMotorDirection.Z;
-                        m_lLinMotorDVel.X = m_linearMotorDirection.X;
-                        m_lLinMotorDVel.Y = m_linearMotorDirection.Y;
-                        m_lLinMotorDVel.Z = m_linearMotorDirection.Z;
-                } // else let the motor decay on its own
-
-                private void UpdateAngDecay()
-                {
-//                      if (Math.Abs(m_angularMotorDirection.X) > Math.Abs(m_angularMotorDVel.X)) m_angularMotorDVel.X = m_angularMotorDirection.X;
-//                      if (Math.Abs(m_angularMotorDirection.Y) > Math.Abs(m_angularMotorDVel.Y)) m_angularMotorDVel.Y = m_angularMotorDirection.Y;
-//                      if (Math.Abs(m_angularMotorDirection.Z) > Math.Abs(m_angularMotorDVel.Z)) m_angularMotorDVel.Z = m_angularMotorDirection.Z;
-                        m_angularMotorDVel.X = m_angularMotorDirection.X;
-                        m_angularMotorDVel.Y = m_angularMotorDirection.Y;
-                        m_angularMotorDVel.Z = m_angularMotorDirection.Z;
-                } // else let the motor decay on its own
-        
+        internal void Halt()
+        {       // Kill all motions, when non-physical
+            //  m_linearMotorDirection = Vector3.Zero;
+            m_lLinMotorDVel = Vector3.Zero;
+            m_lLinObjectVel = Vector3.Zero;
+            m_wLinObjectVel = Vector3.Zero;
+            m_angularMotorDirection = Vector3.Zero;
+            m_lastAngularVelocity = Vector3.Zero;
+            m_angularMotorDVel = Vector3.Zero;
+            _acceleration = Vector3.Zero;
+        }
+
+        private void UpdateLinDecay()
+        {
+            m_lLinMotorDVel.X = m_linearMotorDirection.X;
+            m_lLinMotorDVel.Y = m_linearMotorDirection.Y;
+            m_lLinMotorDVel.Z = m_linearMotorDirection.Z;
+        } // else let the motor decay on its own
+
+        private void UpdateAngDecay()
+        {
+            m_angularMotorDVel.X = m_angularMotorDirection.X;
+            m_angularMotorDVel.Y = m_angularMotorDirection.Y;
+            m_angularMotorDVel.Z = m_angularMotorDirection.Z;
+        } // else let the motor decay on its own
+
         public void Move(float timestep)
         {
             float fx = 0;
             float fy = 0;
             float fz = 0;
             Vector3 linvel;                             // velocity applied, including any reversal
-            int outside = 0;
-            
+
             // If geomCrossingFailuresBeforeOutofbounds is set to 0 in OpenSim.ini then phys objects bounce off region borders.
             // This is a temp patch until proper region crossing is developed. 
-            
-            int failureLimit = _parent_scene.geomCrossingFailuresBeforeOutofbounds;
-            float fence = _parent_scene.geomRegionFence;
-            
-                frcount++;                                      // used to limit debug comment output
-            if (frcount > 50)
-                frcount = 0;
-                
-            if(revcount > 0) revcount--;
-                     
+
+
             if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim && !m_outofBounds)           // Only move root prims.
             {
-                                //  Old public void UpdatePositionAndVelocity(), more accuratley calculated here
-                bool lastZeroFlag = _zeroFlag;  // was it stopped
-                
+                //  Old public void UpdatePositionAndVelocity(), more accuratley calculated here
+                bool lastZeroFlag = _zeroFlag;  // was it stopped
+
                 d.Vector3 vec = d.BodyGetPosition(Body);
                 Vector3 l_position = Vector3.Zero;
                 l_position.X = vec.X;
@@ -3045,95 +3500,26 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
                 l_position.Z = vec.Z;
                 m_lastposition = _position;
                 _position = l_position;
-                
+
                 d.Quaternion ori = d.BodyGetQuaternion(Body);
-         //       Quaternion l_orientation = Quaternion.Identity;
+                //       Quaternion l_orientation = Quaternion.Identity;
                 _orientation.X = ori.X;
                 _orientation.Y = ori.Y;
                 _orientation.Z = ori.Z;
                 _orientation.W = ori.W;
                 m_lastorientation = _orientation;
-                
+
                 d.Vector3 vel = d.BodyGetLinearVel(Body);
                 m_lastVelocity = _velocity;
                 _velocity.X = vel.X;
                 _velocity.Y = vel.Y;
                 _velocity.Z = vel.Z;
                 _acceleration = ((_velocity - m_lastVelocity) / timestep);
-                
+
                 d.Vector3 torque = d.BodyGetTorque(Body);
                 _torque = new Vector3(torque.X, torque.Y, torque.Z);
-                
-                
-//Console.WriteLine("Move {0}  at  {1}", m_primName, l_position);        
- /*                   
-                                // Check if outside region
-                                // In Scene.cs/CrossPrimGroupIntoNewRegion the object is checked for 0.1M from border!
-                if (l_position.X > ((float)_parent_scene.WorldExtents.X - fence))
-                {
-                    l_position.X = ((float)_parent_scene.WorldExtents.X - fence);
-                        outside = 1;
-                }
-                
-                if (l_position.X < fence)
-                {
-                                        l_position.X = fence;
-                                        outside = 2;
-                                }
-                                if (l_position.Y > ((float)_parent_scene.WorldExtents.Y - fence))
-                                {
-                    l_position.Y = ((float)_parent_scene.WorldExtents.Y - fence);
-                        outside = 3;
-                }
-                                
-                if (l_position.Y < fence)
-                {
-                                        l_position.Y = fence;
-                                        outside = 4;
-                                }
-                
-                if (outside > 0)
-                {
-        
-//Console.WriteLine("Border {0}   fence={1}", l_position, fence);               
-                    if (fence > 0.0f)           // bounce object off boundary 
-                    {
-                        if (revcount == 0)
-                            {
-                                if (outside < 3)
-                                        {
-                                                _velocity.X = -_velocity.X;
-                                        }
-                                        else
-                                        {
-                                                _velocity.Y = -_velocity.Y;
-                                        }
-                                        if (m_type != Vehicle.TYPE_NONE) Halt();
-                                        _position = l_position;
-                                    m_taintposition = _position;
-                                                m_lastVelocity = _velocity;     
-                                                        _acceleration = Vector3.Zero;   
-                            d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
-                                                d.BodySetLinearVel(Body, _velocity.X, _velocity.Y, _velocity.Z);
-                            base.RequestPhysicsterseUpdate();
-
-                                        revcount = 25;          // wait for object to move away from border
-                                    }
-                    } // else old crossing mode
-                    else if (m_crossingfailures < failureLimit)
-                    {   // keep trying to cross?
-                        _position = l_position;
-                        //_parent_scene.remActivePrim(this);
-                        if (_parent == null) base.RequestPhysicsterseUpdate();
-                        return;         // Dont process any other motion?
-                    }
-                    else
-                    {   // Too many tries
-                        if (_parent == null) base.RaiseOutOfBounds(l_position);
-                        return;         // Dont process any other motion?
-                    }  // end various methods
-                }    // end outside region horizontally
- */
+
+
                 if (_position.X < 0f || _position.X > _parent_scene.WorldExtents.X
                     || _position.Y < 0f || _position.Y > _parent_scene.WorldExtents.Y
                     )
@@ -3146,41 +3532,11 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
                     _position.Z = Util.Clip(l_position.Z, -100f, 50000f);
                     d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
                     d.BodySetLinearVel(Body, 0, 0, 0);
-                    /*
-                                        if (Interlocked.Exchange(ref m_crossingfailures, m_crossingfailures) == 0)
-                                        { // tell base code only once
-                                            Interlocked.Increment(ref m_crossingfailures);
-                                            base.RequestPhysicsterseUpdate();
-                                        }
-                     */
                     m_outofBounds = true;
                     base.RequestPhysicsterseUpdate();
                     return;
                 }
-/*
-                if (Interlocked.Exchange(ref m_crossingfailures, 0) != 0)
-                {
-                    // main simulator had a crossing failure
-                    // park it inside region
-                    _position.X = Util.Clip(l_position.X, 0.5f, _parent_scene.WorldExtents.X - 0.5f);
-                    _position.Y = Util.Clip(l_position.Y, 0.5f, _parent_scene.WorldExtents.Y - 0.5f);
-                    _position.Z = Util.Clip(l_position.Z, -100f, 50000f);
-                    d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
 
-                    m_lastposition = _position;
-
-                    _velocity = Vector3.Zero;
-                    m_lastVelocity = _velocity;
-
-
-                    if (m_type != Vehicle.TYPE_NONE)
-                        Halt();
-
-                    d.BodySetLinearVel(Body, 0, 0, 0);                   
-                    base.RequestPhysicsterseUpdate();
-                    return;
-                }
-*/
                 base.RequestPhysicsterseUpdate();
 
                 if (l_position.Z < 0)
@@ -3193,8 +3549,8 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
 
                     //IsPhysical = false;
                     if (_parent == null) base.RaiseOutOfBounds(_position);
-                    
-                    
+
+
                     _acceleration.X = 0;                        // This stuff may stop client display but it has no
                     _acceleration.Y = 0;                        // effect on the object in phys engine!
                     _acceleration.Z = 0;
@@ -3215,13 +3571,13 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
                     //outofBounds = true;
                 }  // end neg Z check
 
-                                // Is it moving?
-             /*   if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02)
-                    && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02)
-                    && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02) */
-                if ( (Vector3.Mag(_velocity) < 0.01) &&                         // moving very slowly
+                // Is it moving?
+                /*   if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02)
+                       && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02)
+                       && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02) */
+                if ((Vector3.Mag(_velocity) < 0.01) &&                         // moving very slowly
                      (Vector3.Mag(_velocity) < Vector3.Mag(m_lastVelocity)) &&  // decelerating
-                     (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, _orientation)) < 0.0001) )  // spinning very slowly
+                     (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, _orientation)) < 0.0001))  // spinning very slowly
                 {
                     _zeroFlag = true;
                     m_throttleUpdates = false;
@@ -3238,18 +3594,18 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
                 {               // Its stopped
                     _velocity.X = 0.0f;
                     _velocity.Y = 0.0f;
-            //        _velocity.Z = 0.0f;
+                    //        _velocity.Z = 0.0f;
 
                     _acceleration.X = 0;
                     _acceleration.Y = 0;
-            //        _acceleration.Z = 0;
-                    
+                    //        _acceleration.Z = 0;
+
                     m_rotationalVelocity.X = 0;
                     m_rotationalVelocity.Y = 0;
                     m_rotationalVelocity.Z = 0;
                     // Stop it in the phys engine
-                                        d.BodySetLinearVel(Body, 0.0f, 0.0f, _velocity.Z);        
-                                        d.BodySetAngularVel (Body, 0.0f, 0.0f, 0.0f);
+                    d.BodySetLinearVel(Body, 0.0f, 0.0f, _velocity.Z);
+                    d.BodySetAngularVel(Body, 0.0f, 0.0f, 0.0f);
                     d.BodySetForce(Body, 0f, 0f, 0f);
 
                     if (!m_lastUpdateSent)
@@ -3287,71 +3643,71 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
                     }
                 }
                 m_lastposition = l_position;
-             
-                                /// End UpdatePositionAndVelocity insert     
-                                
-                                
+
+                /// End UpdatePositionAndVelocity insert     
+
+
                 // Rotation lock  =====================================
-                if(m_rotateEnableUpdate)
-                {   
+                if (m_rotateEnableUpdate)
+                {
                     // Snapshot current angles, set up Amotor(s)
                     m_rotateEnableUpdate = false;
                     m_rotateEnable = m_rotateEnableRequest;
-//Console.WriteLine("RotEnable {0} = {1}",m_primName,  m_rotateEnable);
+                    //Console.WriteLine("RotEnable {0} = {1}",m_primName,  m_rotateEnable);
 
                     if (Amotor != IntPtr.Zero)
                     {
                         d.JointDestroy(Amotor);
                         Amotor = IntPtr.Zero;
-//Console.WriteLine("Old Amotor Destroyed");                        
+                        //Console.WriteLine("Old Amotor Destroyed");                        
                     }
-                    
+
                     if (!m_rotateEnable.ApproxEquals(Vector3.One, 0.003f))
                     {   // not all are enabled
                         d.Quaternion r = d.BodyGetQuaternion(Body);
                         Quaternion locrot = new Quaternion(r.X, r.Y, r.Z, r.W);
                         // extract the axes vectors
-                        Vector3 vX = new Vector3(1f,0f,0f);
-                        Vector3 vY = new Vector3(0f,1f,0f);
-                        Vector3 vZ = new Vector3(0f,0f,1f);
-                        vX = vX * locrot;    
+                        Vector3 vX = new Vector3(1f, 0f, 0f);
+                        Vector3 vY = new Vector3(0f, 1f, 0f);
+                        Vector3 vZ = new Vector3(0f, 0f, 1f);
+                        vX = vX * locrot;
                         vY = vY * locrot;
                         vZ = vZ * locrot;
                         // snapshot the current angle vectors
                         m_lockX = vX;
                         m_lockY = vY;
                         m_lockZ = vZ;
-             //           m_lockRot = locrot;
+                        //           m_lockRot = locrot;
                         Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero);
                         d.JointAttach(Amotor, Body, IntPtr.Zero);
                         d.JointSetAMotorMode(Amotor, 0);  // User mode??
-//Console.WriteLine("New Amotor Created for {0}", m_primName);                   
-                    
+                        //Console.WriteLine("New Amotor Created for {0}", m_primName);                   
+
                         float axisnum = 3;  // how many to lock
                         axisnum = (axisnum - (m_rotateEnable.X + m_rotateEnable.Y + m_rotateEnable.Z));
-                        d.JointSetAMotorNumAxes(Amotor,(int)axisnum);
-//Console.WriteLine("AxisNum={0}",(int)axisnum);                       
+                        d.JointSetAMotorNumAxes(Amotor, (int)axisnum);
+                        //Console.WriteLine("AxisNum={0}",(int)axisnum);                       
 
                         int i = 0;
 
                         if (m_rotateEnable.X == 0)
                         {
                             d.JointSetAMotorAxis(Amotor, i, 0, m_lockX.X, m_lockX.Y, m_lockX.Z);
-//Console.WriteLine("AxisX {0} set to {1}", i,  m_lockX);                           
+                            //Console.WriteLine("AxisX {0} set to {1}", i,  m_lockX);                           
                             i++;
                         }
 
                         if (m_rotateEnable.Y == 0)
                         {
                             d.JointSetAMotorAxis(Amotor, i, 0, m_lockY.X, m_lockY.Y, m_lockY.Z);
-//Console.WriteLine("AxisY {0} set to {1}", i,  m_lockY);                           
+                            //Console.WriteLine("AxisY {0} set to {1}", i,  m_lockY);                           
                             i++;
                         }
 
                         if (m_rotateEnable.Z == 0)
                         {
                             d.JointSetAMotorAxis(Amotor, i, 0, m_lockZ.X, m_lockZ.Y, m_lockZ.Z);
-//Console.WriteLine("AxisZ {0} set to {1}", i,  m_lockZ);                           
+                            //Console.WriteLine("AxisZ {0} set to {1}", i,  m_lockZ);                           
                             i++;
                         }
 
@@ -3362,519 +3718,519 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
                         d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0f);
                         d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0f);
                         d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0f);
-                        d.JointSetAMotorParam(Amotor, (int) dParam.Vel, 0f);
-                        d.JointSetAMotorParam(Amotor, (int) dParam.Vel3, 0f);
-                        d.JointSetAMotorParam(Amotor, (int) dParam.Vel2, 0f);
+                        d.JointSetAMotorParam(Amotor, (int)dParam.Vel, 0f);
+                        d.JointSetAMotorParam(Amotor, (int)dParam.Vel3, 0f);
+                        d.JointSetAMotorParam(Amotor, (int)dParam.Vel2, 0f);
                         d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM, 0f);
                         d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM3, 0f);
                         d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM2, 0f);
                     } // else none are locked
                 } // end Rotation Update  
-                
-                
+
+
                 // VEHICLE processing ==========================================            
-                if (m_type != Vehicle.TYPE_NONE)
-                {
-                        // get body attitude
-                            d.Quaternion rot = d.BodyGetQuaternion(Body);
-                                Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);   // rotq = rotation of object
-                                Quaternion irotq = Quaternion.Inverse(rotq);
-                                
-                                        // VEHICLE Linear Motion
-                                        d.Vector3 velnow = d.BodyGetLinearVel(Body);                                    // this is in world frame
-                                        Vector3 vel_now = new Vector3(velnow.X, velnow.Y, velnow.Z);
-                                m_lLinObjectVel = vel_now * irotq;
-                            if (m_linearMotorDecayTimescale < 300.0f) //setting of 300 or more disables decay rate
-                            {
-                                if ( Vector3.Mag(m_lLinMotorDVel) < 1.0f)
-                                {
-                                                float decayfactor = m_linearMotorDecayTimescale/timestep;
-                                        Vector3 decayAmount = (m_lLinMotorDVel/decayfactor);
-                                        m_lLinMotorDVel -= decayAmount;
-                                                }
-                                                else
-                                                {
-                                                float decayfactor = 3.0f - (0.57f * (float)Math.Log((double)(m_linearMotorDecayTimescale)));
-                                                        Vector3 decel = Vector3.Normalize(m_lLinMotorDVel) * decayfactor * timestep;
-                                                        m_lLinMotorDVel -= decel;
-                                                }
-                                                if (m_lLinMotorDVel.ApproxEquals(Vector3.Zero, 0.01f))
-                                                {
-                                                        m_lLinMotorDVel = Vector3.Zero;
-                                                }
-                                                
-                                        /*      else
-                                {
-                                                if (Math.Abs(m_lLinMotorDVel.X) <  Math.Abs(m_lLinObjectVel.X)) m_lLinObjectVel.X = m_lLinMotorDVel.X;
-                                            if (Math.Abs(m_lLinMotorDVel.Y) <  Math.Abs(m_lLinObjectVel.Y)) m_lLinObjectVel.Y = m_lLinMotorDVel.Y;
-                                                if (Math.Abs(m_lLinMotorDVel.Z) <  Math.Abs(m_lLinObjectVel.Z)) m_lLinObjectVel.Z = m_lLinMotorDVel.Z;  
-                                            } */
-                                        }  // end linear motor decay
-        
-                            if ( (! m_lLinMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (! m_lLinObjectVel.ApproxEquals(Vector3.Zero, 0.01f)) )
-                            {
-                                if(!d.BodyIsEnabled (Body))  d.BodyEnable (Body);
-                                if (m_linearMotorTimescale < 300.0f)
-                                {       
-                                        Vector3 attack_error = m_lLinMotorDVel - m_lLinObjectVel;       
-                                        float linfactor = m_linearMotorTimescale/timestep;
-                                        Vector3 attackAmount = (attack_error/linfactor) * 1.3f;
-                                        m_lLinObjectVel += attackAmount;
-                                }
-                                        if (m_linearFrictionTimescale.X < 300.0f)
-                                        {
-                                                float fricfactor = m_linearFrictionTimescale.X / timestep;
-                                                float fricX = m_lLinObjectVel.X / fricfactor;
-                                                m_lLinObjectVel.X -= fricX;
-                                            }
-                                        if (m_linearFrictionTimescale.Y < 300.0f)
-                                        {
-                                                float fricfactor = m_linearFrictionTimescale.Y / timestep;
-                                                float fricY = m_lLinObjectVel.Y / fricfactor;
-                                                m_lLinObjectVel.Y -= fricY;
-                                            }
-                                        if (m_linearFrictionTimescale.Z < 300.0f)
-                                        {
-                                                float fricfactor = m_linearFrictionTimescale.Z / timestep;
-                                                float fricZ = m_lLinObjectVel.Z / fricfactor;
-                                                m_lLinObjectVel.Z -= fricZ;
-                                            }
-                                        }
-                                    m_wLinObjectVel = m_lLinObjectVel * rotq;
-                                    
-                                        // Gravity and Buoyancy
-                            Vector3 grav = Vector3.Zero;
-                                        if(m_VehicleBuoyancy < 1.0f)
-                                        {
-                                                // There is some gravity, make a gravity force vector
-                                                // that is applied after object velocity.     
-                                    d.Mass objMass;
-                                    d.BodyGetMass(Body, out objMass);
-                                    // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g; 
-                                    grav.Z = _parent_scene.gravityz * objMass.mass * (1f - m_VehicleBuoyancy); // Applied later as a force
-                                } // else its 1.0, no gravity.
-                
-                                // Hovering
-                                if( (m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0)
-                                {       
-                                        // We should hover, get the target height
-                                        d.Vector3 pos = d.BodyGetPosition(Body);
-                                        if((m_flags & VehicleFlag.HOVER_WATER_ONLY) == VehicleFlag.HOVER_WATER_ONLY)
-                                        {
-                                                m_VhoverTargetHeight = _parent_scene.GetWaterLevel() + m_VhoverHeight;
-                                        }
-                                        else if((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) == VehicleFlag.HOVER_TERRAIN_ONLY)
-                                        {
-                                                m_VhoverTargetHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y) + m_VhoverHeight;
-                                        }
-                                        else if((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == VehicleFlag.HOVER_GLOBAL_HEIGHT)
-                                        {
-                                                m_VhoverTargetHeight = m_VhoverHeight;
-                                        }
-                        
-                                                if((m_flags & VehicleFlag.HOVER_UP_ONLY) == VehicleFlag.HOVER_UP_ONLY)
-                                                {
-                                                        // If body is aready heigher, use its height as target height
-                                                        if(pos.Z > m_VhoverTargetHeight) m_VhoverTargetHeight = pos.Z;
-                                                }
-                                
-//                  m_VhoverEfficiency = 0f;    // 0=boucy, 1=Crit.damped
-//                              m_VhoverTimescale = 0f;         // time to acheive height
-//                              timestep  is time since last frame,in secs 
-                                                float herr0 = pos.Z - m_VhoverTargetHeight;
-                                                // Replace Vertical speed with correction figure if significant
-                                                if(Math.Abs(herr0) > 0.01f )
-                                                {
-                                  //?          d.Mass objMass;
-                                  //?          d.BodyGetMass(Body, out objMass);
-                                                        m_wLinObjectVel.Z = - ( (herr0 * timestep * 50.0f) / m_VhoverTimescale);
-                                                        //KF: m_VhoverEfficiency is not yet implemented
-                                                }
-                                                else
-                                                {
-                                                        m_wLinObjectVel.Z = 0f;
-                                                }
-                                        }
-                                        else
-                                        {       // not hovering
-                                                if (m_wLinObjectVel.Z == 0f)
-                                                {               // Gravity rules
-                                                        m_wLinObjectVel.Z = vel_now.Z;
-                                                }  // else the motor has it
-                            }   
-                            linvel = m_wLinObjectVel;
-
-                            // Vehicle Linear  Motion done =======================================
-                                // Apply velocity
-                                        d.BodySetLinearVel(Body, linvel.X, linvel.Y, linvel.Z);        
-                            // apply gravity force
-                                        d.BodyAddForce(Body, grav.X, grav.Y, grav.Z);           
-//if(frcount == 0) Console.WriteLine("Vel={0}    Force={1}",linvel ,  grav);
-                                // end MoveLinear()
-
-                                        
-                                    // MoveAngular
-                                /*
-                                private Vector3 m_angularMotorDirection = Vector3.Zero;                 // angular velocity requested by LSL motor 
-        
-                                private float m_angularMotorTimescale = 0;                                              // motor angular Attack rate set by LSL
-                                private float m_angularMotorDecayTimescale = 0;                                 // motor angular Decay rate set by LSL
-                                private Vector3 m_angularFrictionTimescale = Vector3.Zero;              // body angular Friction set by LSL
-
-                                private Vector3 m_angularMotorDVel = Vector3.Zero;                              // decayed angular motor
-                                private Vector3 m_angObjectVel = Vector3.Zero;                                  // what was last applied to body
-                                        */
-//if(frcount == 0) Console.WriteLine("MoveAngular ");   
+                if (m_type != Vehicle.TYPE_NONE)
+                {
+                    // get body attitude
+                    d.Quaternion rot = d.BodyGetQuaternion(Body);
+                    Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);       // rotq = rotation of object
+                    Quaternion irotq = Quaternion.Inverse(rotq);
+
+                    // VEHICLE Linear Motion
+                    d.Vector3 velnow = d.BodyGetLinearVel(Body);                                        // this is in world frame
+                    Vector3 vel_now = new Vector3(velnow.X, velnow.Y, velnow.Z);
+                    m_lLinObjectVel = vel_now * irotq;
+                    if (m_linearMotorDecayTimescale < 300.0f) //setting of 300 or more disables decay rate
+                    {
+                        if (Vector3.Mag(m_lLinMotorDVel) < 1.0f)
+                        {
+                            float decayfactor = m_linearMotorDecayTimescale / timestep;
+                            Vector3 decayAmount = (m_lLinMotorDVel / decayfactor);
+                            m_lLinMotorDVel -= decayAmount;
+                        }
+                        else
+                        {
+                            float decayfactor = 3.0f - (0.57f * (float)Math.Log((double)(m_linearMotorDecayTimescale)));
+                            Vector3 decel = Vector3.Normalize(m_lLinMotorDVel) * decayfactor * timestep;
+                            m_lLinMotorDVel -= decel;
+                        }
+                        if (m_lLinMotorDVel.ApproxEquals(Vector3.Zero, 0.01f))
+                        {
+                            m_lLinMotorDVel = Vector3.Zero;
+                        }
+
+                        /*      else
+                            {
+                                if (Math.Abs(m_lLinMotorDVel.X) <  Math.Abs(m_lLinObjectVel.X)) m_lLinObjectVel.X = m_lLinMotorDVel.X;
+                                if (Math.Abs(m_lLinMotorDVel.Y) <  Math.Abs(m_lLinObjectVel.Y)) m_lLinObjectVel.Y = m_lLinMotorDVel.Y;
+                                if (Math.Abs(m_lLinMotorDVel.Z) <  Math.Abs(m_lLinObjectVel.Z)) m_lLinObjectVel.Z = m_lLinMotorDVel.Z;  
+                            } */
+                    }  // end linear motor decay
+
+                    if ((!m_lLinMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (!m_lLinObjectVel.ApproxEquals(Vector3.Zero, 0.01f)))
+                    {
+                        if (!d.BodyIsEnabled(Body)) d.BodyEnable(Body);
+                        if (m_linearMotorTimescale < 300.0f)
+                        {
+                            Vector3 attack_error = m_lLinMotorDVel - m_lLinObjectVel;
+                            float linfactor = m_linearMotorTimescale / timestep;
+                            Vector3 attackAmount = (attack_error / linfactor) * 1.3f;
+                            m_lLinObjectVel += attackAmount;
+                        }
+                        if (m_linearFrictionTimescale.X < 300.0f)
+                        {
+                            float fricfactor = m_linearFrictionTimescale.X / timestep;
+                            float fricX = m_lLinObjectVel.X / fricfactor;
+                            m_lLinObjectVel.X -= fricX;
+                        }
+                        if (m_linearFrictionTimescale.Y < 300.0f)
+                        {
+                            float fricfactor = m_linearFrictionTimescale.Y / timestep;
+                            float fricY = m_lLinObjectVel.Y / fricfactor;
+                            m_lLinObjectVel.Y -= fricY;
+                        }
+                        if (m_linearFrictionTimescale.Z < 300.0f)
+                        {
+                            float fricfactor = m_linearFrictionTimescale.Z / timestep;
+                            float fricZ = m_lLinObjectVel.Z / fricfactor;
+                            m_lLinObjectVel.Z -= fricZ;
+                        }
+                    }
+                    m_wLinObjectVel = m_lLinObjectVel * rotq;
+
+                    // Gravity and Buoyancy
+                    Vector3 grav = Vector3.Zero;
+                    if (m_VehicleBuoyancy < 1.0f)
+                    {
+                        // There is some gravity, make a gravity force vector
+                        // that is applied after object velocity.     
+                        d.Mass objMass;
+                        d.BodyGetMass(Body, out objMass);
+                        // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g; 
+                        grav.Z = _parent_scene.gravityz * objMass.mass * (1f - m_VehicleBuoyancy); // Applied later as a force
+                    } // else its 1.0, no gravity.
+
+                    // Hovering
+                    if ((m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0)
+                    {
+                        // We should hover, get the target height
+                        d.Vector3 pos = d.BodyGetPosition(Body);
+                        if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) == VehicleFlag.HOVER_WATER_ONLY)
+                        {
+                            m_VhoverTargetHeight = _parent_scene.GetWaterLevel() + m_VhoverHeight;
+                        }
+                        else if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) == VehicleFlag.HOVER_TERRAIN_ONLY)
+                        {
+                            m_VhoverTargetHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y) + m_VhoverHeight;
+                        }
+                        else if ((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == VehicleFlag.HOVER_GLOBAL_HEIGHT)
+                        {
+                            m_VhoverTargetHeight = m_VhoverHeight;
+                        }
+
+                        if ((m_flags & VehicleFlag.HOVER_UP_ONLY) == VehicleFlag.HOVER_UP_ONLY)
+                        {
+                            // If body is aready heigher, use its height as target height
+                            if (pos.Z > m_VhoverTargetHeight) m_VhoverTargetHeight = pos.Z;
+                        }
+
+                        //                  m_VhoverEfficiency = 0f;    // 0=boucy, 1=Crit.damped
+                        //                              m_VhoverTimescale = 0f;         // time to acheive height
+                        //                              timestep  is time since last frame,in secs 
+                        float herr0 = pos.Z - m_VhoverTargetHeight;
+                        // Replace Vertical speed with correction figure if significant
+                        if (Math.Abs(herr0) > 0.01f)
+                        {
+                            //?          d.Mass objMass;
+                            //?          d.BodyGetMass(Body, out objMass);
+                            m_wLinObjectVel.Z = -((herr0 * timestep * 50.0f) / m_VhoverTimescale);
+                            //KF: m_VhoverEfficiency is not yet implemented
+                        }
+                        else
+                        {
+                            m_wLinObjectVel.Z = 0f;
+                        }
+                    }
+                    else
+                    {   // not hovering
+                        if (m_wLinObjectVel.Z == 0f)
+                        {               // Gravity rules
+                            m_wLinObjectVel.Z = vel_now.Z;
+                        }  // else the motor has it
+                    }
+                    linvel = m_wLinObjectVel;
+
+                    // Vehicle Linear  Motion done =======================================
+                    // Apply velocity
+                    d.BodySetLinearVel(Body, linvel.X, linvel.Y, linvel.Z);
+                    // apply gravity force
+                    d.BodyAddForce(Body, grav.X, grav.Y, grav.Z);
+                    //if(frcount == 0) Console.WriteLine("Vel={0}    Force={1}",linvel ,  grav);
+                    // end MoveLinear()
+
+
+                    // MoveAngular
+                    /*
+                    private Vector3 m_angularMotorDirection = Vector3.Zero;                     // angular velocity requested by LSL motor 
         
-                                d.Vector3 angularObjectVel = d.BodyGetAngularVel(Body);
-                                Vector3 angObjectVel = new Vector3(angularObjectVel.X, angularObjectVel.Y, angularObjectVel.Z);
-                                angObjectVel = angObjectVel * irotq;    // ============ Converts to LOCAL rotation
-                
-//if(frcount == 0) Console.WriteLine("V0 = {0}", angObjectVel);         
-
-                                // Decay Angular Motor 1. In SL this also depends on attack rate! decay ~= 23/Attack.
-                                float atk_decayfactor = 23.0f  / (m_angularMotorTimescale * timestep); 
-                                m_angularMotorDVel -= m_angularMotorDVel / atk_decayfactor;
-                                // Decay Angular Motor 2.
-                                if (m_angularMotorDecayTimescale < 300.0f)
-                                {
-                                if ( Vector3.Mag(m_angularMotorDVel) < 1.0f)
-                                {
-                                                        float decayfactor = (m_angularMotorDecayTimescale)/timestep;
-                                            Vector3 decayAmount = (m_angularMotorDVel/decayfactor);     
-                                            m_angularMotorDVel -= decayAmount;                  
-                                                }
-                                                else
-                                                {
-                                                        Vector3 decel = Vector3.Normalize(m_angularMotorDVel) * timestep / m_angularMotorDecayTimescale;
-                                                        m_angularMotorDVel -= decel;
-                                                }
-                
-                                                if (m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f))
-                                                {
-                                                        m_angularMotorDVel = Vector3.Zero;
-                                                }
-                                                else
-                                        {
-                                                if (Math.Abs(m_angularMotorDVel.X) <  Math.Abs(angObjectVel.X)) angObjectVel.X = m_angularMotorDVel.X;
-                                            if (Math.Abs(m_angularMotorDVel.Y) <  Math.Abs(angObjectVel.Y)) angObjectVel.Y = m_angularMotorDVel.Y;
-                                                if (Math.Abs(m_angularMotorDVel.Z) <  Math.Abs(angObjectVel.Z)) angObjectVel.Z = m_angularMotorDVel.Z;
-                                            }           
-                                } // end decay angular motor
-//if(frcount == 0) Console.WriteLine("MotorDvel {0}    Obj {1}", m_angularMotorDVel, angObjectVel);
-
-//if(frcount == 0) Console.WriteLine("VA = {0}", angObjectVel);   
-
-                            if ( (! m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (! angObjectVel.ApproxEquals(Vector3.Zero, 0.01f)) )
-                            {  // if motor or object have motion
-                                if(!d.BodyIsEnabled (Body))  d.BodyEnable (Body);
-                
-                                if (m_angularMotorTimescale < 300.0f)
-                                {       
-                                        Vector3 attack_error = m_angularMotorDVel - angObjectVel;       
-                                        float angfactor = m_angularMotorTimescale/timestep;
-                                        Vector3 attackAmount = (attack_error/angfactor);
-                                        angObjectVel += attackAmount;
-//if(frcount == 0) Console.WriteLine("Accel {0}      Attk {1}",FrAaccel, attackAmount);                 
-//if(frcount == 0) Console.WriteLine("V2+= {0}", angObjectVel);         
-                                }
-                
-                                        angObjectVel.X -= angObjectVel.X / (m_angularFrictionTimescale.X * 0.7f / timestep);
-                                        angObjectVel.Y -= angObjectVel.Y / (m_angularFrictionTimescale.Y * 0.7f / timestep);
-                                        angObjectVel.Z -= angObjectVel.Z / (m_angularFrictionTimescale.Z * 0.7f / timestep);
-                                        } // else no signif. motion
-                        
-//if(frcount == 0) Console.WriteLine("Dmotor {0}      Obj {1}", m_angularMotorDVel, angObjectVel);
-                                        // Bank section tba
-                                        // Deflection section tba
-//if(frcount == 0) Console.WriteLine("V3 = {0}", angObjectVel);         
-                        
-        
-                /*                      // Rotation Axis Disables:
-                                    if (!m_angularEnable.ApproxEquals(Vector3.One, 0.003f))
-                                {
-                                if (m_angularEnable.X == 0)
-                                        angObjectVel.X = 0f;
-                                    if (m_angularEnable.Y == 0)
-                                        angObjectVel.Y = 0f;
-                                    if (m_angularEnable.Z == 0)
-                                        angObjectVel.Z = 0f;
-                                }        
-                        */        
-                                        angObjectVel = angObjectVel * rotq; // ================ Converts to WORLD rotation
-                                        
-                            // Vertical attractor section
-                                        Vector3 vertattr = Vector3.Zero;
-            
-                                        if(m_verticalAttractionTimescale < 300)
-                                        {
-                                    float VAservo = 1.0f / (m_verticalAttractionTimescale * timestep);
-                                    // make a vector pointing up
-                                                Vector3 verterr = Vector3.Zero;
-                                                verterr.Z = 1.0f;
-                                                // rotate it to Body Angle
-                                                verterr = verterr * rotq;
-                                                // verterr.X and .Y are the World error ammounts. They are 0 when there is no error (Vehicle Body is 'vertical'), and .Z will be 1.
-                                                // As the body leans to its side |.X| will increase to 1 and .Z fall to 0. As body inverts |.X| will fall and .Z will go
-                                                // negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body.
-                                
-                                                if (verterr.Z < 0.0f)
-                                                {       // Deflection from vertical exceeds 90-degrees. This method will ensure stable return to
-                                                        // vertical, BUT for some reason a z-rotation is imparted to the object. TBI.
-//Console.WriteLine("InvertFlip");      
-                                                        verterr.X = 2.0f - verterr.X;
-                                                        verterr.Y = 2.0f - verterr.Y;
-                                                }
-                                                verterr *= 0.5f;
-                                                // verterror is 0 (no error) to +/- 1 (max error at 180-deg tilt)
-                                                Vector3 xyav = angObjectVel;
-                                                xyav.Z = 0.0f;
-                                                if ((!xyav.ApproxEquals(Vector3.Zero, 0.001f)) || (verterr.Z < 0.49f))
-                                                {
-                                                        // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so 
-                                                        // Change  Body angular velocity  X based on Y, and Y based on X. Z is not changed.
-                                                        vertattr.X =    verterr.Y;
-                                                        vertattr.Y =  - verterr.X;
-                                                        vertattr.Z = 0f;
-//if(frcount == 0) Console.WriteLine("VAerr=" + verterr);       
-                        
-                                                        // scaling appears better usingsquare-law
-                                                        float damped = m_verticalAttractionEfficiency * m_verticalAttractionEfficiency;
-                                                        float bounce = 1.0f - damped;  
-                                                        // 0 = crit damp, 1 = bouncy
-                                                        float oavz = angObjectVel.Z;   // retain z velocity
-                                                        // time-scaled correction, which sums, therefore is bouncy:
-                                                        angObjectVel = (angObjectVel + (vertattr * VAservo * 0.0333f)) * bounce; 
-                                                        // damped, good @ < 90:
-                                                        angObjectVel = angObjectVel + (vertattr * VAservo *  0.0667f * damped); 
-                                                        angObjectVel.Z = oavz;
-//if(frcount == 0) Console.WriteLine("VA+");                                    
-//Console.WriteLine("VAttr {0}         OAvel {1}", vertattr, angObjectVel);
-                                                }
-                                                else
-                                                {
-                                                        // else error is very small
-                                                        angObjectVel.X = 0f;
-                                                        angObjectVel.Y = 0f;
-//if(frcount == 0) Console.WriteLine("VA0");                                    
-                                                }
-                                        } // else vertical attractor is off
-//if(frcount == 0) Console.WriteLine("V1 = {0}", angObjectVel);         
-
-                                        
-                                        m_lastAngularVelocity = angObjectVel;
-                                        // apply Angular Velocity to body
-                                        d.BodySetAngularVel (Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z);
-//if(frcount == 0) Console.WriteLine("V4 = {0}", m_lastAngularVelocity);                
-                            
-                }  // end VEHICLES
-                else
-                {
-                        // Dyamics (NON-'VEHICLES') are dealt with here ================================================================                        
-                        
-                        if(!d.BodyIsEnabled (Body))  d.BodyEnable (Body); // KF add 161009
-                        
-                                        /// Dynamics Buoyancy
-                        //KF: m_buoyancy is set by llSetBuoyancy() and is for non-vehicle.
-                        // m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up 
-                        // NB Prims in ODE are no subject to global gravity
-                        // This should only affect gravity operations 
-                        
-                        float m_mass = CalculateMass();
-                        // calculate z-force due togravity on object.
-                        fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass; // force = acceleration * mass
-                        if ((m_usePID) && (m_PIDTau > 0.0f))  // Dynamics  llMoveToTarget.
-                        {
-                            fz = 0;     // llMoveToTarget ignores gravity.
-                                        // it also ignores mass of object, and any physical resting on it.
-                                        // Vector3 m_PIDTarget is where we are going
-                                    // float m_PIDTau is time to get there
+                    private float m_angularMotorTimescale = 0;                                          // motor angular Attack rate set by LSL
+                    private float m_angularMotorDecayTimescale = 0;                                     // motor angular Decay rate set by LSL
+                    private Vector3 m_angularFrictionTimescale = Vector3.Zero;          // body angular Friction set by LSL
+
+                    private Vector3 m_angularMotorDVel = Vector3.Zero;                          // decayed angular motor
+                    private Vector3 m_angObjectVel = Vector3.Zero;                                      // what was last applied to body
+                    */
+                    //if(frcount == 0) Console.WriteLine("MoveAngular ");       
+
+                    d.Vector3 angularObjectVel = d.BodyGetAngularVel(Body);
+                    Vector3 angObjectVel = new Vector3(angularObjectVel.X, angularObjectVel.Y, angularObjectVel.Z);
+                    angObjectVel = angObjectVel * irotq;        // ============ Converts to LOCAL rotation
+
+                    //if(frcount == 0) Console.WriteLine("V0 = {0}", angObjectVel);             
+
+                    // Decay Angular Motor 1. In SL this also depends on attack rate! decay ~= 23/Attack.
+                    float atk_decayfactor = 23.0f / (m_angularMotorTimescale * timestep);
+                    m_angularMotorDVel -= m_angularMotorDVel / atk_decayfactor;
+                    // Decay Angular Motor 2.
+                    if (m_angularMotorDecayTimescale < 300.0f)
+                    {
+                        if (Vector3.Mag(m_angularMotorDVel) < 1.0f)
+                        {
+                            float decayfactor = (m_angularMotorDecayTimescale) / timestep;
+                            Vector3 decayAmount = (m_angularMotorDVel / decayfactor);
+                            m_angularMotorDVel -= decayAmount;
+                        }
+                        else
+                        {
+                            Vector3 decel = Vector3.Normalize(m_angularMotorDVel) * timestep / m_angularMotorDecayTimescale;
+                            m_angularMotorDVel -= decel;
+                        }
+
+                        if (m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f))
+                        {
+                            m_angularMotorDVel = Vector3.Zero;
+                        }
+                        else
+                        {
+                            if (Math.Abs(m_angularMotorDVel.X) < Math.Abs(angObjectVel.X)) angObjectVel.X = m_angularMotorDVel.X;
+                            if (Math.Abs(m_angularMotorDVel.Y) < Math.Abs(angObjectVel.Y)) angObjectVel.Y = m_angularMotorDVel.Y;
+                            if (Math.Abs(m_angularMotorDVel.Z) < Math.Abs(angObjectVel.Z)) angObjectVel.Z = m_angularMotorDVel.Z;
+                        }
+                    } // end decay angular motor
+                    //if(frcount == 0) Console.WriteLine("MotorDvel {0}    Obj {1}", m_angularMotorDVel, angObjectVel);
+
+                    //if(frcount == 0) Console.WriteLine("VA = {0}", angObjectVel);   
+
+                    if ((!m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (!angObjectVel.ApproxEquals(Vector3.Zero, 0.01f)))
+                    {  // if motor or object have motion
+                        if (!d.BodyIsEnabled(Body)) d.BodyEnable(Body);
+
+                        if (m_angularMotorTimescale < 300.0f)
+                        {
+                            Vector3 attack_error = m_angularMotorDVel - angObjectVel;
+                            float angfactor = m_angularMotorTimescale / timestep;
+                            Vector3 attackAmount = (attack_error / angfactor);
+                            angObjectVel += attackAmount;
+                            //if(frcount == 0) Console.WriteLine("Accel {0}      Attk {1}",FrAaccel, attackAmount);                     
+                            //if(frcount == 0) Console.WriteLine("V2+= {0}", angObjectVel);             
+                        }
+
+                        angObjectVel.X -= angObjectVel.X / (m_angularFrictionTimescale.X * 0.7f / timestep);
+                        angObjectVel.Y -= angObjectVel.Y / (m_angularFrictionTimescale.Y * 0.7f / timestep);
+                        angObjectVel.Z -= angObjectVel.Z / (m_angularFrictionTimescale.Z * 0.7f / timestep);
+                    } // else no signif. motion
+
+                    //if(frcount == 0) Console.WriteLine("Dmotor {0}      Obj {1}", m_angularMotorDVel, angObjectVel);
+                    // Bank section tba
+                    // Deflection section tba
+                    //if(frcount == 0) Console.WriteLine("V3 = {0}", angObjectVel);             
+
+
+                    /*                  // Rotation Axis Disables:
+                                if (!m_angularEnable.ApproxEquals(Vector3.One, 0.003f))
+                                {
+                                    if (m_angularEnable.X == 0)
+                                        angObjectVel.X = 0f;
+                                    if (m_angularEnable.Y == 0)
+                                        angObjectVel.Y = 0f;
+                                    if (m_angularEnable.Z == 0)
+                                        angObjectVel.Z = 0f;
+                                }        
+                        */
+                    angObjectVel = angObjectVel * rotq; // ================ Converts to WORLD rotation
+
+                    // Vertical attractor section
+                    Vector3 vertattr = Vector3.Zero;
+
+                    if (m_verticalAttractionTimescale < 300)
+                    {
+                        float VAservo = 1.0f / (m_verticalAttractionTimescale * timestep);
+                        // make a vector pointing up
+                        Vector3 verterr = Vector3.Zero;
+                        verterr.Z = 1.0f;
+                        // rotate it to Body Angle
+                        verterr = verterr * rotq;
+                        // verterr.X and .Y are the World error ammounts. They are 0 when there is no error (Vehicle Body is 'vertical'), and .Z will be 1.
+                        // As the body leans to its side |.X| will increase to 1 and .Z fall to 0. As body inverts |.X| will fall and .Z will go
+                        // negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body.
+
+                        if (verterr.Z < 0.0f)
+                        {       // Deflection from vertical exceeds 90-degrees. This method will ensure stable return to
+                            // vertical, BUT for some reason a z-rotation is imparted to the object. TBI.
+                            //Console.WriteLine("InvertFlip");  
+                            verterr.X = 2.0f - verterr.X;
+                            verterr.Y = 2.0f - verterr.Y;
+                        }
+                        verterr *= 0.5f;
+                        // verterror is 0 (no error) to +/- 1 (max error at 180-deg tilt)
+                        Vector3 xyav = angObjectVel;
+                        xyav.Z = 0.0f;
+                        if ((!xyav.ApproxEquals(Vector3.Zero, 0.001f)) || (verterr.Z < 0.49f))
+                        {
+                            // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so 
+                            // Change  Body angular velocity  X based on Y, and Y based on X. Z is not changed.
+                            vertattr.X = verterr.Y;
+                            vertattr.Y = -verterr.X;
+                            vertattr.Z = 0f;
+                            //if(frcount == 0) Console.WriteLine("VAerr=" + verterr);   
+
+                            // scaling appears better usingsquare-law
+                            float damped = m_verticalAttractionEfficiency * m_verticalAttractionEfficiency;
+                            float bounce = 1.0f - damped;
+                            // 0 = crit damp, 1 = bouncy
+                            float oavz = angObjectVel.Z;   // retain z velocity
+                            // time-scaled correction, which sums, therefore is bouncy:
+                            angObjectVel = (angObjectVel + (vertattr * VAservo * 0.0333f)) * bounce;
+                            // damped, good @ < 90:
+                            angObjectVel = angObjectVel + (vertattr * VAservo * 0.0667f * damped);
+                            angObjectVel.Z = oavz;
+                            //if(frcount == 0) Console.WriteLine("VA+");                                        
+                            //Console.WriteLine("VAttr {0}         OAvel {1}", vertattr, angObjectVel);
+                        }
+                        else
+                        {
+                            // else error is very small
+                            angObjectVel.X = 0f;
+                            angObjectVel.Y = 0f;
+                            //if(frcount == 0) Console.WriteLine("VA0");                                        
+                        }
+                    } // else vertical attractor is off
+                    //if(frcount == 0) Console.WriteLine("V1 = {0}", angObjectVel);             
+
+
+                    m_lastAngularVelocity = angObjectVel;
+                    // apply Angular Velocity to body
+                    d.BodySetAngularVel(Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z);
+                    //if(frcount == 0) Console.WriteLine("V4 = {0}", m_lastAngularVelocity);            
+
+                }  // end VEHICLES
+                else
+                {
+                    // Dyamics (NON-'VEHICLES') are dealt with here ================================================================                    
+
+                    if (!d.BodyIsEnabled(Body)) d.BodyEnable(Body); // KF add 161009
+
+                    /// Dynamics Buoyancy
+                    //KF: m_buoyancy is set by llSetBuoyancy() and is for non-vehicle.
+                    // m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up 
+                    // NB Prims in ODE are no subject to global gravity
+                    // This should only affect gravity operations 
+
+                    float m_mass = CalculateMass();
+                    // calculate z-force due togravity on object.
+                    fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass; // force = acceleration * mass
+                    if ((m_usePID) && (m_PIDTau > 0.0f))  // Dynamics  llMoveToTarget.
+                    {
+                        fz = 0;     // llMoveToTarget ignores gravity.
+                        // it also ignores mass of object, and any physical resting on it.
+                        // Vector3 m_PIDTarget is where we are going
+                        // float m_PIDTau is time to get there
                         fx = 0;
-                        fy = 0;                                    
-                            d.Vector3 pos = d.BodyGetPosition(Body);
-                            Vector3 error = new Vector3(
-                                    (m_PIDTarget.X - pos.X),
-                                    (m_PIDTarget.Y - pos.Y),
-                                    (m_PIDTarget.Z - pos.Z));
-                            if (error.ApproxEquals(Vector3.Zero,0.01f))
-                            {   // Very close, Jump there and quit move
-
-                                d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);
-                                _target_velocity = Vector3.Zero;
+                        fy = 0;
+                        d.Vector3 pos = d.BodyGetPosition(Body);
+                        Vector3 error = new Vector3(
+                                (m_PIDTarget.X - pos.X),
+                                (m_PIDTarget.Y - pos.Y),
+                                (m_PIDTarget.Z - pos.Z));
+                        if (error.ApproxEquals(Vector3.Zero, 0.01f))
+                        {   // Very close, Jump there and quit move
+
+                            d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);
+                            _target_velocity = Vector3.Zero;
                             d.BodySetLinearVel(Body, _target_velocity.X, _target_velocity.Y, _target_velocity.Z);
                             d.BodySetForce(Body, 0f, 0f, 0f);
-                            }
-                            else 
-                            {
-                            float scale =  50.0f * timestep / m_PIDTau;
-                            if ((error.ApproxEquals(Vector3.Zero,0.5f)) && (_target_velocity != Vector3.Zero))
-                            {
-                                    // Nearby, quit update of velocity
-                                }
-                                else
-                                {  // Far, calc damped velocity
-                                    _target_velocity = error * scale;
-                                }
-                            d.BodySetLinearVel(Body, _target_velocity.X, _target_velocity.Y, _target_velocity.Z);                                   
                         }
-                        } // end PID MoveToTarget
-                        
-                        
-                        /// Dynamics Hover ===================================================================================
-                        // Hover PID Controller can only run if the PIDcontroller is not in use.
-                        if (m_useHoverPID && !m_usePID)
-                        {
-//Console.WriteLine("Hover " +  m_primName);            
-                        
-                            // If we're using the PID controller, then we have no gravity
-                            fz = (-1 * _parent_scene.gravityz) * m_mass;
-
-                            //  no lock; for now it's only called from within Simulate()
-
-                            // If the PID Controller isn't active then we set our force
-                            // calculating base velocity to the current position
-
-                            if ((m_PIDTau < 1))
-                            {
-                                PID_G = PID_G / m_PIDTau;
-                            }
-
-                            if ((PID_G - m_PIDTau) <= 0)
-                            {
-                                PID_G = m_PIDTau + 1;
-                            }
-                    
-
-                            // Where are we, and where are we headed?
-                            d.Vector3 pos = d.BodyGetPosition(Body);
-//                          d.Vector3 vel = d.BodyGetLinearVel(Body);
-
-
-                                                //    Non-Vehicles have a limited set of Hover options.
-                            // determine what our target height really is based on HoverType
-                            switch (m_PIDHoverType)
-                            {
-                                case PIDHoverType.Ground:
-                                    m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
-                                    m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
-                                    break;
-                                case PIDHoverType.GroundAndWater:
-                                    m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
-                                    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)
-
-
-                            _target_velocity =
+                        else
+                        {
+                            float scale = 50.0f * timestep / m_PIDTau;
+                            if ((error.ApproxEquals(Vector3.Zero, 0.5f)) && (_target_velocity != Vector3.Zero))
+                            {
+                                // Nearby, quit update of velocity
+                            }
+                            else
+                            {  // Far, calc damped velocity
+                                _target_velocity = error * scale;
+                            }
+                            d.BodySetLinearVel(Body, _target_velocity.X, _target_velocity.Y, _target_velocity.Z);
+                        }
+                    } // end PID MoveToTarget
+
+
+                    /// Dynamics Hover ===================================================================================
+                    // Hover PID Controller can only run if the PIDcontroller is not in use.
+                    if (m_useHoverPID && !m_usePID)
+                    {
+                        //Console.WriteLine("Hover " +  m_primName);            
+
+                        // If we're using the PID controller, then we have no gravity
+                        fz = (-1 * _parent_scene.gravityz) * m_mass;
+
+                        //  no lock; for now it's only called from within Simulate()
+
+                        // If the PID Controller isn't active then we set our force
+                        // calculating base velocity to the current position
+
+                        if ((m_PIDTau < 1))
+                        {
+                            PID_G = PID_G / m_PIDTau;
+                        }
+
+                        if ((PID_G - m_PIDTau) <= 0)
+                        {
+                            PID_G = m_PIDTau + 1;
+                        }
+
+
+                        // Where are we, and where are we headed?
+                        d.Vector3 pos = d.BodyGetPosition(Body);
+                        //                          d.Vector3 vel = d.BodyGetLinearVel(Body);
+
+
+                        //    Non-Vehicles have a limited set of Hover options.
+                        // determine what our target height really is based on HoverType
+                        switch (m_PIDHoverType)
+                        {
+                            case PIDHoverType.Ground:
+                                m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
+                                m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
+                                break;
+                            case PIDHoverType.GroundAndWater:
+                                m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
+                                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)
+
+
+                        _target_velocity =
                             new Vector3(0.0f, 0.0f,
-                                (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep)
-                                );
-
-                        //  if velocity is zero, use position control; otherwise, velocity control
-
-                        if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f))
-                        {
-                            //  keep track of where we stopped.  No more slippin' & slidin'
-        
-                            // We only want to deactivate the PID Controller if we think we want to have our surrogate
-                            // react to the physics scene by moving it's position.
-                            // Avatar to Avatar collisions
-                            // Prim to avatar collisions
-                                            d.Vector3 dlinvel = vel;
-                            d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight);
-                                                d.BodySetLinearVel(Body, dlinvel.X, dlinvel.Y, dlinvel.Z);       
-                            d.BodyAddForce(Body, 0, 0, fz);
-                    //KF this prevents furthur motions         return;
-                        }
-                        else
-                        {
-                            _zeroFlag = false;
-
-                            // We're flying and colliding with something
-                            fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
-                        }
-                    } // end m_useHoverPID && !m_usePID
-                    
-                        
-                        /// Dynamics Apply Forces ===================================================================================
-                    fx *= m_mass;
-                    fy *= m_mass;
-                    //fz *= m_mass;
-                    fx += m_force.X;
-                    fy += m_force.Y;
-                    fz += m_force.Z;
-
-                    //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString());
-                    if (fx != 0 || fy != 0 || fz != 0)
-                    {
-                        //m_taintdisable = true;
-                        //base.RaiseOutOfBounds(Position);
-                        //d.BodySetLinearVel(Body, fx, fy, 0f);
-                        if (!d.BodyIsEnabled(Body))
-                        {
-                                // A physical body at rest on a surface will auto-disable after a while,
-                                // this appears to re-enable it incase the surface it is upon vanishes,
-                                // and the body should fall again. 
-                            d.BodySetLinearVel(Body, 0f, 0f, 0f);
-                            d.BodySetForce(Body, 0f, 0f, 0f);
-                            enableBodySoft();
-                        }
-
-                        // 35x10 = 350n times the mass per second applied maximum.
-                        float nmax = 35f * m_mass;
-                        float nmin = -35f * m_mass;
-
-                    
-                        if (fx > nmax)
-                            fx = nmax;
-                        if (fx < nmin)
-                            fx = nmin;
-                        if (fy > nmax)
-                            fy = nmax;
-                        if (fy < nmin)
-                            fy = nmin;
-                        d.BodyAddForce(Body, fx, fy, fz);
-                    }  // end apply forces
-                                } // end Vehicle/Dynamics
-
-                    /// RotLookAt / LookAt =================================================================================
+                                (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep)
+                                );
+
+                        //  if velocity is zero, use position control; otherwise, velocity control
+
+                        if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f))
+                        {
+                            //  keep track of where we stopped.  No more slippin' & slidin'
+
+                            // We only want to deactivate the PID Controller if we think we want to have our surrogate
+                            // react to the physics scene by moving it's position.
+                            // Avatar to Avatar collisions
+                            // Prim to avatar collisions
+                            d.Vector3 dlinvel = vel;
+                            d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight);
+                            d.BodySetLinearVel(Body, dlinvel.X, dlinvel.Y, dlinvel.Z);
+                            d.BodyAddForce(Body, 0, 0, fz);
+                            //KF this prevents furthur motions         return;
+                        }
+                        else
+                        {
+                            _zeroFlag = false;
+
+                            // We're flying and colliding with something
+                            fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
+                        }
+                    } // end m_useHoverPID && !m_usePID
+
+
+                    /// Dynamics Apply Forces ===================================================================================
+                    fx *= m_mass;
+                    fy *= m_mass;
+                    //fz *= m_mass;
+                    fx += m_force.X;
+                    fy += m_force.Y;
+                    fz += m_force.Z;
+
+                    //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString());
+                    if (fx != 0 || fy != 0 || fz != 0)
+                    {
+                        //m_taintdisable = true;
+                        //base.RaiseOutOfBounds(Position);
+                        //d.BodySetLinearVel(Body, fx, fy, 0f);
+                        if (!d.BodyIsEnabled(Body))
+                        {
+                            // A physical body at rest on a surface will auto-disable after a while,
+                            // this appears to re-enable it incase the surface it is upon vanishes,
+                            // and the body should fall again. 
+                            d.BodySetLinearVel(Body, 0f, 0f, 0f);
+                            d.BodySetForce(Body, 0f, 0f, 0f);
+                            enableBodySoft();
+                        }
+
+                        // 35x10 = 350n times the mass per second applied maximum.
+                        float nmax = 35f * m_mass;
+                        float nmin = -35f * m_mass;
+
+
+                        if (fx > nmax)
+                            fx = nmax;
+                        if (fx < nmin)
+                            fx = nmin;
+                        if (fy > nmax)
+                            fy = nmax;
+                        if (fy < nmin)
+                            fy = nmin;
+                        d.BodyAddForce(Body, fx, fy, fz);
+                    }  // end apply forces
+                } // end Vehicle/Dynamics
+
+                /// RotLookAt / LookAt =================================================================================
                 if (m_useAPID)
                 {
-                        // RotLookAt, apparently overrides all other rotation sources. Inputs:
-                        // Quaternion m_APIDTarget
-                                        // float m_APIDStrength         // From SL experiments, this is the time to get there
-                                        // float m_APIDDamping          // From SL experiments, this is damping, 1.0 = damped, 0.1 = wobbly
-                                                                                                // Also in SL the mass of the object has no effect on time to get there.
-                                        // Factors:
-                            // get present body rotation
-                            float limit = 1.0f;
-                            float rscaler = 50f;                // adjusts rotation damping time
-                            float lscaler = 10f;                // adjusts linear damping time in llLookAt
-                            float RLAservo = 0f;
+                    // RotLookAt, apparently overrides all other rotation sources. Inputs:
+                    // Quaternion m_APIDTarget
+                    // float m_APIDStrength             // From SL experiments, this is the time to get there
+                    // float m_APIDDamping              // From SL experiments, this is damping, 1.0 = damped, 0.1 = wobbly
+                    // Also in SL the mass of the object has no effect on time to get there.
+                    // Factors:
+                    // get present body rotation
+                    float limit = 1.0f;
+                    float rscaler = 50f;                // adjusts rotation damping time
+                    float lscaler = 10f;                // adjusts linear damping time in llLookAt
+                    float RLAservo = 0f;
                     Vector3 diff_axis;
                     float diff_angle;
-                            d.Quaternion rot = d.BodyGetQuaternion(Body);                                               // prim present rotation
-                            Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);
-                            Quaternion rtarget = new Quaternion();
-
-                                        if(m_APIDTarget.W == -99.9f)
-                                        {
-                                                // this is really a llLookAt(), x,y,z is the target vector
-                                                Vector3 target = new Vector3(m_APIDTarget.X, m_APIDTarget.Y, m_APIDTarget.Z);
-                        Vector3 ospin = new Vector3(1.0f, 0.0f, 0.0f) *  rotq;
+                    d.Quaternion rot = d.BodyGetQuaternion(Body);                                               // prim present rotation
+                    Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);
+                    Quaternion rtarget = new Quaternion();
+
+                    if (m_APIDTarget.W == -99.9f)
+                    {
+                        // this is really a llLookAt(), x,y,z is the target vector
+                        Vector3 target = new Vector3(m_APIDTarget.X, m_APIDTarget.Y, m_APIDTarget.Z);
+                        Vector3 ospin = new Vector3(1.0f, 0.0f, 0.0f) * rotq;
                         Vector3 error = new Vector3(0.0f, 0.0f, 0.0f);
                         float twopi = 2.0f * (float)Math.PI;
-                                                Vector3 dir = target - _position;
-                                                dir.Normalize();
+                        Vector3 dir = target - _position;
+                        dir.Normalize();
                         float tzrot = (float)Math.Atan2(dir.Y, dir.X);
                         float txy = (float)Math.Sqrt((dir.X * dir.X) + (dir.Y * dir.Y));
                         float terot = (float)Math.Atan2(dir.Z, txy);
@@ -3882,63 +4238,63 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
                         float oxy = (float)Math.Sqrt((ospin.X * ospin.X) + (ospin.Y * ospin.Y));
                         float oerot = (float)Math.Atan2(ospin.Z, oxy);
                         float ra = 2.0f * ((rotq.W * rotq.X) + (rotq.Y * rotq.Z));
-                        float rb = 1.0f - 2.0f * ((rotq.Y * rotq.Y)+(rotq.X * rotq.X));
+                        float rb = 1.0f - 2.0f * ((rotq.Y * rotq.Y) + (rotq.X * rotq.X));
                         float roll = (float)Math.Atan2(ra, rb);
                         float errorz = tzrot - ozrot;
-                        if(errorz > (float)Math.PI) errorz -= twopi;
-                        else if(errorz < -(float)Math.PI) errorz += twopi;
+                        if (errorz > (float)Math.PI) errorz -= twopi;
+                        else if (errorz < -(float)Math.PI) errorz += twopi;
                         float errory = oerot - terot;
-                        if(errory > (float)Math.PI) errory -= twopi;
-                        else if(errory < -(float)Math.PI) errory += twopi;
+                        if (errory > (float)Math.PI) errory -= twopi;
+                        else if (errory < -(float)Math.PI) errory += twopi;
                         diff_angle = Math.Abs(errorz) + Math.Abs(errory) + Math.Abs(roll);
-                                                if(diff_angle > 0.01f * m_APIDdamper)
-                                                {       
-                                                    m_APIDdamper = 1.0f;
-                            RLAservo = timestep / m_APIDStrength * rscaler;
+                        if (diff_angle > 0.01f * m_APIDdamper)
+                        {
+                            m_APIDdamper = 1.0f;
+                            RLAservo = timestep / m_APIDStrength * rscaler;
                             errorz *= RLAservo;
                             errory *= RLAservo;
                             error.X = -roll * 8.0f;
                             error.Y = errory;
                             error.Z = errorz;
                             error *= rotq;
-                                                d.BodySetAngularVel (Body,  error.X, error.Y, error.Z);
-                                                }
-                                                else
-                                                {
-                                                        d.BodySetAngularVel (Body,  0.0f, 0.0f, 0.0f);
-                                                        m_APIDdamper = 2.0f;
-                                                }
-                                        }
-                                        else
-                                        {
-                                                // this is a llRotLookAt()
-                                                rtarget = m_APIDTarget;
-
-                                        Quaternion rot_diff = Quaternion.Inverse(rotq) * rtarget;               // difference to desired rot
-                                rot_diff.GetAxisAngle(out diff_axis, out diff_angle);                           // convert to axis to point at & error angle
-//if(frcount == 0) Console.WriteLine("axis {0}   angle {1}",diff_axis * 57.3f, diff_angle);
-
-                             //   diff_axis.Normalize(); it already is!
-                                                if(diff_angle > 0.01f * m_APIDdamper)                   // diff_angle is always +ve                     // if there is enough error
-                                                {       
-                                                                m_APIDdamper = 1.0f;
-                                        Vector3 rotforce = new Vector3(diff_axis.X, diff_axis.Y, diff_axis.Z);
-                                    rotforce = rotforce * rotq;
-                                    if(diff_angle > limit) diff_angle = limit;          // cap the rotate rate
-                                    RLAservo = timestep / m_APIDStrength * lscaler;
-                                    rotforce = rotforce * RLAservo * diff_angle ;
-                                                                d.BodySetAngularVel (Body,  rotforce.X, rotforce.Y, rotforce.Z);
-//Console.WriteLine("axis= " + diff_axis + "    angle= " + diff_angle + "servo= " + RLAservo);                                                  
-                                                }
-                                                else
-                                                {       // close enough
-                                                        d.BodySetAngularVel (Body,  0.0f, 0.0f, 0.0f);
-                                                        m_APIDdamper = 2.0f;
-                                                }
-                                        } // end llLookAt/llRotLookAt
-//if(frcount == 0) Console.WriteLine("mass= " + m_mass + "  servo= " + RLAservo + "   angle= " + diff_angle);
+                            d.BodySetAngularVel(Body, error.X, error.Y, error.Z);
+                        }
+                        else
+                        {
+                            d.BodySetAngularVel(Body, 0.0f, 0.0f, 0.0f);
+                            m_APIDdamper = 2.0f;
+                        }
+                    }
+                    else
+                    {
+                        // this is a llRotLookAt()
+                        rtarget = m_APIDTarget;
+
+                        Quaternion rot_diff = Quaternion.Inverse(rotq) * rtarget;               // difference to desired rot
+                        rot_diff.GetAxisAngle(out diff_axis, out diff_angle);                           // convert to axis to point at & error angle
+                        //if(frcount == 0) Console.WriteLine("axis {0}   angle {1}",diff_axis * 57.3f, diff_angle);
+
+                        //   diff_axis.Normalize(); it already is!
+                        if (diff_angle > 0.01f * m_APIDdamper)                  // diff_angle is always +ve                     // if there is enough error
+                        {
+                            m_APIDdamper = 1.0f;
+                            Vector3 rotforce = new Vector3(diff_axis.X, diff_axis.Y, diff_axis.Z);
+                            rotforce = rotforce * rotq;
+                            if (diff_angle > limit) diff_angle = limit;         // cap the rotate rate
+                            RLAservo = timestep / m_APIDStrength * lscaler;
+                            rotforce = rotforce * RLAservo * diff_angle;
+                            d.BodySetAngularVel(Body, rotforce.X, rotforce.Y, rotforce.Z);
+                            //Console.WriteLine("axis= " + diff_axis + "    angle= " + diff_angle + "servo= " + RLAservo);                                                      
+                        }
+                        else
+                        {       // close enough
+                            d.BodySetAngularVel(Body, 0.0f, 0.0f, 0.0f);
+                            m_APIDdamper = 2.0f;
+                        }
+                    } // end llLookAt/llRotLookAt
+                    //if(frcount == 0) Console.WriteLine("mass= " + m_mass + "  servo= " + RLAservo + "   angle= " + diff_angle);
                 } // end m_useAPID
             } // end root prims
-       }  // end Move()
-        } // end class
+        }  // end Move()
+    } // end class
 }
diff --git a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs
index cf7c1d7..b5b30ee 100644
--- a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs
+++ b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs
@@ -1736,6 +1736,23 @@ namespace OpenSim.Region.Physics.OdePlugin
             return newPrim;
         }
 
+        private PhysicsActor AddPrim(String name, Vector3 position, PhysicsActor parent,
+                                     PrimitiveBaseShape pbs, uint localid, byte[] sdata)
+        {
+            Vector3 pos = position;
+
+            OdePrim newPrim;
+            lock (OdeLock)
+            {
+                newPrim = new OdePrim(name, this, pos, parent, pbs, ode, localid, sdata);               
+                lock (_prims)
+                    _prims.Add(newPrim);
+            }
+
+            return newPrim;
+        }
+
+
         public void addActivePrim(OdePrim activatePrim)
         {
             // adds active prim..   (ones that should be iterated over in collisions_optimized
@@ -1762,6 +1779,17 @@ namespace OpenSim.Region.Physics.OdePlugin
             return result;
         }
 
+        public override PhysicsActor AddPrimShape(string primName, PhysicsActor parent, PrimitiveBaseShape pbs, Vector3 position,
+                                                  uint localid, byte[] sdata)
+        {
+            PhysicsActor result;
+
+            result = AddPrim(primName, position, parent,
+                                     pbs, localid, sdata);
+
+            return result;
+        }
+
         public override float TimeDilation
         {
             get { return m_timeDilation; }
@@ -3412,15 +3440,15 @@ namespace OpenSim.Region.Physics.OdePlugin
            
             int regionsize = (int) Constants.RegionSize;                // visible region size eg. 256(M)
             
-            int heightmapWidth = regionsize + 2;                                // ODE map size 257 x 257 (Meters) (1 extra 
+            int heightmapWidth = regionsize + 2;                                // ODE map size 258 x 258 (Meters) (1 extra each side)
             int heightmapHeight = regionsize + 2;
             
-            int heightmapWidthSamples = (int)regionsize + 2;            // Sample file size, 258 x 258 samples
-            int heightmapHeightSamples = (int)regionsize + 2;
+            int heightmapWidthSamples = (int)regionsize + 3;    // to have 258m we need 259 samples
+            int heightmapHeightSamples = (int)regionsize + 3;
             
             // Array of height samples for ODE 
             float[] _heightmap;
-            _heightmap = new float[(heightmapWidthSamples * heightmapHeightSamples)];   // loaded samples 258 x 258
+            _heightmap = new float[(heightmapWidthSamples * heightmapHeightSamples)];   // loaded samples 259 x 259
 
                         // Other ODE parameters
             const float scale = 1.0f;
diff --git a/OpenSim/Region/Physics/ChOdePlugin/OdeUtils.cs b/OpenSim/Region/Physics/ChOdePlugin/OdeUtils.cs
new file mode 100644
index 0000000..e7e7bb3
--- /dev/null
+++ b/OpenSim/Region/Physics/ChOdePlugin/OdeUtils.cs
@@ -0,0 +1,353 @@
+/* Ubit 2012
+  * 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.
+*/
+
+// no endian conversion. So can't be use to pass information around diferent cpus with diferent endian
+
+using System;
+using System.IO;
+using OpenMetaverse;
+
+namespace OpenSim.Region.Physics.OdePlugin
+{
+   
+    unsafe public class wstreamer
+    {
+        byte[] buf;
+        int index;
+        byte* src;
+
+        public wstreamer()
+        {
+            buf = new byte[1024];
+            index = 0;
+        }
+        public wstreamer(int size)
+        {
+            buf = new byte[size];
+            index = 0;
+        }
+
+        public byte[] close()
+        {
+            byte[] data = new byte[index];
+            Buffer.BlockCopy(buf, 0, data, 0, index); 
+            return data;
+        }
+
+        public void Seek(int pos)
+        {
+            index = pos;
+        }
+
+        public void Seekrel(int pos)
+        {
+            index += pos;
+        }
+
+        public void Wbyte(byte value)
+        {
+            buf[index++] = value;
+        }
+        public void Wshort(short value)
+        {
+            src = (byte*)&value;
+            buf[index++] = *src++;
+            buf[index++] = *src;
+        }
+        public void Wushort(ushort value)
+        {
+            src = (byte*)&value;
+            buf[index++] = *src++;
+            buf[index++] = *src;
+        }
+        public void Wint(int value)
+        {
+            src = (byte*)&value;
+            buf[index++] = *src++;
+            buf[index++] = *src++;
+            buf[index++] = *src++;
+            buf[index++] = *src;
+        }
+        public void Wuint(uint value)
+        {
+            src = (byte*)&value;
+            buf[index++] = *src++;
+            buf[index++] = *src++;
+            buf[index++] = *src++;
+            buf[index++] = *src;
+        }
+        public void Wlong(long value)
+        {
+            src = (byte*)&value;
+            buf[index++] = *src++;
+            buf[index++] = *src++;
+            buf[index++] = *src++;
+            buf[index++] = *src++;
+            buf[index++] = *src++;
+            buf[index++] = *src++;
+            buf[index++] = *src++;
+            buf[index++] = *src;
+        }
+        public void Wulong(ulong value)
+        {
+            src = (byte*)&value;
+            buf[index++] = *src++;
+            buf[index++] = *src++;
+            buf[index++] = *src++;
+            buf[index++] = *src++;
+            buf[index++] = *src++;
+            buf[index++] = *src++;
+            buf[index++] = *src++;
+            buf[index++] = *src;
+        }
+
+        public void Wfloat(float value)
+        {
+            src = (byte*)&value;
+            buf[index++] = *src++;
+            buf[index++] = *src++;
+            buf[index++] = *src++;
+            buf[index++] = *src;
+        }
+
+        public void Wdouble(double value)
+        {
+            src = (byte*)&value;
+            buf[index++] = *src++;
+            buf[index++] = *src++;
+            buf[index++] = *src++;
+            buf[index++] = *src++;
+            buf[index++] = *src++;
+            buf[index++] = *src++;
+            buf[index++] = *src++;
+            buf[index++] = *src;
+        }
+
+        public void Wvector3(Vector3 value)
+        {
+            src = (byte*)&value.X;
+            buf[index++] = *src++;
+            buf[index++] = *src++;
+            buf[index++] = *src++;
+            buf[index++] = *src;
+            src = (byte*)&value.Y; // it may have padding ??
+            buf[index++] = *src++;
+            buf[index++] = *src++;
+            buf[index++] = *src++;
+            buf[index++] = *src;
+            src = (byte*)&value.Z;
+            buf[index++] = *src++;
+            buf[index++] = *src++;
+            buf[index++] = *src++;
+            buf[index++] = *src;
+        }
+        public void Wquat(Quaternion value)
+        {
+            src = (byte*)&value.X;
+            buf[index++] = *src++;
+            buf[index++] = *src++;
+            buf[index++] = *src++;
+            buf[index++] = *src;
+            src = (byte*)&value.Y; // it may have padding ??
+            buf[index++] = *src++;
+            buf[index++] = *src++;
+            buf[index++] = *src++;
+            buf[index++] = *src;
+            src = (byte*)&value.Z;
+            buf[index++] = *src++;
+            buf[index++] = *src++;
+            buf[index++] = *src++;
+            buf[index++] = *src;
+            src = (byte*)&value.W;
+            buf[index++] = *src++;
+            buf[index++] = *src++;
+            buf[index++] = *src++;
+            buf[index++] = *src;
+        }
+    }
+
+    unsafe public class rstreamer
+    {
+        private byte[] rbuf;
+        private int ptr;
+        private byte* dst;
+
+        public rstreamer(byte[] data)
+        {
+            rbuf = data;
+            ptr = 0;
+        }
+
+        public void close()
+        {
+        }
+
+        public void Seek(int pos)
+        {
+            ptr = pos;
+        }
+
+        public void Seekrel(int pos)
+        {
+            ptr += pos;
+        }
+
+        public byte Rbyte()
+        {
+            return (byte)rbuf[ptr++];
+        }
+
+        public short Rshort()
+        {
+            short v;
+            dst = (byte*)&v;
+            *dst++ = rbuf[ptr++];
+            *dst = rbuf[ptr++];
+            return v;
+        }
+        public ushort Rushort()
+        {
+            ushort v;
+            dst = (byte*)&v;
+            *dst++ = rbuf[ptr++];
+            *dst = rbuf[ptr++];
+            return v;
+        }
+        public int Rint()
+        {
+            int v;
+            dst = (byte*)&v;
+            *dst++ = rbuf[ptr++];
+            *dst++ = rbuf[ptr++];
+            *dst++ = rbuf[ptr++];
+            *dst = rbuf[ptr++];
+            return v;
+        }
+        public uint Ruint()
+        {
+            uint v;
+            dst = (byte*)&v;
+            *dst++ = rbuf[ptr++];
+            *dst++ = rbuf[ptr++];
+            *dst++ = rbuf[ptr++];
+            *dst = rbuf[ptr++];
+            return v;
+        }
+        public long Rlong()
+        {
+            long v;
+            dst = (byte*)&v;
+            *dst++ = rbuf[ptr++];
+            *dst++ = rbuf[ptr++];
+            *dst++ = rbuf[ptr++];
+            *dst++ = rbuf[ptr++];
+            *dst++ = rbuf[ptr++];
+            *dst++ = rbuf[ptr++];
+            *dst++ = rbuf[ptr++];
+            *dst = rbuf[ptr++];
+            return v;
+        }
+        public ulong Rulong()
+        {
+            ulong v;
+            dst = (byte*)&v;
+            *dst++ = rbuf[ptr++];
+            *dst++ = rbuf[ptr++];
+            *dst++ = rbuf[ptr++];
+            *dst++ = rbuf[ptr++];
+            *dst++ = rbuf[ptr++];
+            *dst++ = rbuf[ptr++];
+            *dst++ = rbuf[ptr++];
+            *dst = rbuf[ptr++];
+            return v;
+        }
+        public float Rfloat()
+        {
+            float v;
+            dst = (byte*)&v;
+            *dst++ = rbuf[ptr++];
+            *dst++ = rbuf[ptr++];
+            *dst++ = rbuf[ptr++];
+            *dst = rbuf[ptr++];
+            return v;
+        }
+
+        public double Rdouble()
+        {
+            double v;
+            dst = (byte*)&v;
+            *dst++ = rbuf[ptr++];
+            *dst++ = rbuf[ptr++];
+            *dst++ = rbuf[ptr++];
+            *dst++ = rbuf[ptr++];
+            *dst++ = rbuf[ptr++];
+            *dst++ = rbuf[ptr++];
+            *dst++ = rbuf[ptr++];
+            *dst = rbuf[ptr++];
+            return v;
+        }
+
+        public Vector3 Rvector3()
+        {
+            Vector3 v;
+            dst = (byte*)&v.X;
+            *dst++ = rbuf[ptr++];
+            *dst++ = rbuf[ptr++];
+            *dst++ = rbuf[ptr++];
+            *dst = rbuf[ptr++];
+
+            dst = (byte*)&v.Y;
+            *dst++ = rbuf[ptr++];
+            *dst++ = rbuf[ptr++];
+            *dst++ = rbuf[ptr++];
+            *dst = rbuf[ptr++];
+
+            dst = (byte*)&v.Z;
+            *dst++ = rbuf[ptr++];
+            *dst++ = rbuf[ptr++];
+            *dst++ = rbuf[ptr++];
+            *dst = rbuf[ptr++];
+            return v;
+        }
+
+        public Quaternion Rquat()
+        {
+            Quaternion v;
+            dst = (byte*)&v.X;
+            *dst++ = rbuf[ptr++];
+            *dst++ = rbuf[ptr++];
+            *dst++ = rbuf[ptr++];
+            *dst = rbuf[ptr++];
+
+            dst = (byte*)&v.Y;
+            *dst++ = rbuf[ptr++];
+            *dst++ = rbuf[ptr++];
+            *dst++ = rbuf[ptr++];
+            *dst = rbuf[ptr++];
+
+            dst = (byte*)&v.Z;
+            *dst++ = rbuf[ptr++];
+            *dst++ = rbuf[ptr++];
+            *dst++ = rbuf[ptr++];
+            *dst = rbuf[ptr++];
+
+            dst = (byte*)&v.W;
+            *dst++ = rbuf[ptr++];
+            *dst++ = rbuf[ptr++];
+            *dst++ = rbuf[ptr++];
+            *dst = rbuf[ptr++];
+
+            return v;
+        }
+    }
+}
diff --git a/OpenSim/Region/Physics/Manager/IMesher.cs b/OpenSim/Region/Physics/Manager/IMesher.cs
index 3a9ca1b..cc92484 100644
--- a/OpenSim/Region/Physics/Manager/IMesher.cs
+++ b/OpenSim/Region/Physics/Manager/IMesher.cs
@@ -65,5 +65,6 @@ namespace OpenSim.Region.Physics.Manager
         void releasePinned();
         void Append(IMesh newMesh);
         void TransformLinear(float[,] matrix, float[] offset);
+        Vector3 GetCentroid();
     }
 }
diff --git a/OpenSim/Region/Physics/Manager/PhysicsActor.cs b/OpenSim/Region/Physics/Manager/PhysicsActor.cs
index 0587054..f525e9e 100644
--- a/OpenSim/Region/Physics/Manager/PhysicsActor.cs
+++ b/OpenSim/Region/Physics/Manager/PhysicsActor.cs
@@ -68,6 +68,17 @@ namespace OpenSim.Region.Physics.Manager
         }
     }
 
+    public struct ContactData
+    {
+        public float mu;
+        public float bounce;
+
+        public ContactData(float _mu, float _bounce)
+        {
+            mu = _mu;
+            bounce = _bounce;
+        }
+    }
     /// <summary>
     /// Used to pass collision information to OnCollisionUpdate listeners.
     /// </summary>
@@ -135,6 +146,8 @@ namespace OpenSim.Region.Physics.Manager
         /// </summary>
         public event CollisionUpdate OnCollisionUpdate;
 
+        public virtual void SetVehicle(object vdata) { }
+
         public event OutOfBounds OnOutOfBounds;
 #pragma warning restore 67
 
@@ -142,6 +155,13 @@ namespace OpenSim.Region.Physics.Manager
         {
             get { return new NullPhysicsActor(); }
         }
+   
+        public virtual bool Building { get; set; }
+
+        public virtual ContactData ContactData
+        {
+            get { return new ContactData(0, 0); }
+        }
 
         public abstract bool Stopped { get; }
 
@@ -195,6 +215,11 @@ namespace OpenSim.Region.Physics.Manager
             }
         }
 
+        public virtual byte[] Serialize(bool PhysIsRunning)
+        {
+            return new byte[0];
+        }
+
         public virtual void RaiseOutOfBounds(Vector3 pos)
         {
             // Make a temporary copy of the event to avoid possibility of
@@ -554,5 +579,6 @@ namespace OpenSim.Region.Physics.Manager
         {
             return false;
         }
+
     }
 }
diff --git a/OpenSim/Region/Physics/Manager/PhysicsScene.cs b/OpenSim/Region/Physics/Manager/PhysicsScene.cs
index 514d9ad..0346d4e 100644
--- a/OpenSim/Region/Physics/Manager/PhysicsScene.cs
+++ b/OpenSim/Region/Physics/Manager/PhysicsScene.cs
@@ -128,6 +128,12 @@ namespace OpenSim.Region.Physics.Manager
         public abstract PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
                                                   Vector3 size, Quaternion rotation, bool isPhysical, uint localid);
 
+        public virtual PhysicsActor AddPrimShape(string primName, PhysicsActor parent, PrimitiveBaseShape pbs, Vector3 position,
+                                                  uint localid, byte[] sdata)
+        {
+            return null;
+        }
+
         public virtual float TimeDilation
         {
             get { return 1.0f; }
@@ -225,7 +231,7 @@ namespace OpenSim.Region.Physics.Manager
         }
 
         public virtual void Combine(PhysicsScene pScene, Vector3 offset, Vector3 extents) {}
-
+        public virtual void CombineTerrain(float[] heightMap, Vector3 pOffset) {}
         public virtual void UnCombine(PhysicsScene pScene) {}
 
         /// <summary>
@@ -263,5 +269,13 @@ namespace OpenSim.Region.Physics.Manager
         {
             return new List<ContactResult>();
         }
+
+        public virtual void RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, RaycastCallback retMethod){}
+        public virtual void RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count, RayCallback retMethod) { }
+        public virtual List<ContactResult> RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count)
+        {
+            return new List<ContactResult>();
+        }
+
     }
 }
diff --git a/OpenSim/Region/Physics/Manager/VehicleConstants.cs b/OpenSim/Region/Physics/Manager/VehicleConstants.cs
index f0775c1..8e24b4c 100644
--- a/OpenSim/Region/Physics/Manager/VehicleConstants.cs
+++ b/OpenSim/Region/Physics/Manager/VehicleConstants.cs
@@ -26,6 +26,7 @@
  */
 
 using System;
+using OpenMetaverse;
 
 namespace OpenSim.Region.Physics.Manager
 {
@@ -117,5 +118,47 @@ namespace OpenSim.Region.Physics.Manager
         NO_DEFLECTION = 16392,
         LOCK_ROTATION = 32784
     }
-        
+
+    public struct VehicleData
+    {
+        public Vehicle m_type;
+        public VehicleFlag m_flags;
+
+        // Linear properties
+        public Vector3 m_linearMotorDirection;
+        public Vector3 m_linearFrictionTimescale;
+        public float m_linearMotorDecayTimescale;
+        public float m_linearMotorTimescale;
+        public Vector3 m_linearMotorOffset;
+
+        //Angular properties
+        public Vector3 m_angularMotorDirection;
+        public float m_angularMotorTimescale;
+        public float m_angularMotorDecayTimescale;
+        public Vector3 m_angularFrictionTimescale;
+
+        //Deflection properties
+        public float m_angularDeflectionEfficiency;
+        public float m_angularDeflectionTimescale;
+        public float m_linearDeflectionEfficiency;
+        public float m_linearDeflectionTimescale;
+
+        //Banking properties
+        public float m_bankingEfficiency;
+        public float m_bankingMix;
+        public float m_bankingTimescale;
+
+        //Hover and Buoyancy properties
+        public float m_VhoverHeight;
+        public float m_VhoverEfficiency;
+        public float m_VhoverTimescale;
+        public float m_VehicleBuoyancy;
+
+        //Attractor properties
+        public float m_verticalAttractionEfficiency;
+        public float m_verticalAttractionTimescale;
+
+        // Axis
+        public Quaternion m_referenceFrame;
+    }
 }
diff --git a/OpenSim/Region/Physics/Meshing/Mesh.cs b/OpenSim/Region/Physics/Meshing/Mesh.cs
index f781ff9..c715642 100644
--- a/OpenSim/Region/Physics/Meshing/Mesh.cs
+++ b/OpenSim/Region/Physics/Meshing/Mesh.cs
@@ -46,11 +46,36 @@ namespace OpenSim.Region.Physics.Meshing
         IntPtr m_indicesPtr = IntPtr.Zero;
         int m_indexCount = 0;
         public float[] m_normals;
+        Vector3 _centroid;
+        int _centroidDiv;
+
+        private class vertexcomp : IEqualityComparer<Vertex>
+        {
+            public bool Equals(Vertex v1, Vertex v2)
+            {
+                if (v1.X == v2.X && v1.Y == v2.Y && v1.Z == v2.Z)
+                    return true;
+                else
+                    return false;
+            }
+            public int GetHashCode(Vertex v)
+            {
+                int a = v.X.GetHashCode();
+                int b = v.Y.GetHashCode();
+                int c = v.Z.GetHashCode();
+                return (a << 16) ^ (b << 8) ^ c;
+            }
+
+        }
 
         public Mesh()
         {
-            m_vertices = new Dictionary<Vertex, int>();
+            vertexcomp vcomp = new vertexcomp();
+
+            m_vertices = new Dictionary<Vertex, int>(vcomp);
             m_triangles = new List<Triangle>();
+            _centroid = Vector3.Zero;
+            _centroidDiv = 0;
         }
 
         public Mesh Clone()
@@ -61,7 +86,8 @@ namespace OpenSim.Region.Physics.Meshing
             {
                 result.Add(new Triangle(t.v1.Clone(), t.v2.Clone(), t.v3.Clone()));
             }
-
+            result._centroid = _centroid;
+            result._centroidDiv = _centroidDiv;
             return result;
         }
 
@@ -71,15 +97,57 @@ namespace OpenSim.Region.Physics.Meshing
                 throw new NotSupportedException("Attempt to Add to a pinned Mesh");
             // If a vertex of the triangle is not yet in the vertices list,
             // add it and set its index to the current index count
+            // vertex == seems broken
+            // skip colapsed triangles
+            if ((triangle.v1.X == triangle.v2.X && triangle.v1.Y == triangle.v2.Y && triangle.v1.Z == triangle.v2.Z)
+                || (triangle.v1.X == triangle.v3.X && triangle.v1.Y == triangle.v3.Y && triangle.v1.Z == triangle.v3.Z)
+                || (triangle.v2.X == triangle.v3.X && triangle.v2.Y == triangle.v3.Y && triangle.v2.Z == triangle.v3.Z)
+                )               
+            {               
+                return;
+            }
+
+            if (m_vertices.Count == 0)
+            {
+                _centroidDiv = 0;
+                _centroid = Vector3.Zero;
+            }
+
             if (!m_vertices.ContainsKey(triangle.v1))
+            {
                 m_vertices[triangle.v1] = m_vertices.Count;
+                _centroid.X += triangle.v1.X;
+                _centroid.Y += triangle.v1.Y;
+                _centroid.Z += triangle.v1.Z;
+                _centroidDiv++;
+            }
             if (!m_vertices.ContainsKey(triangle.v2))
+            {
                 m_vertices[triangle.v2] = m_vertices.Count;
+                _centroid.X += triangle.v2.X;
+                _centroid.Y += triangle.v2.Y;
+                _centroid.Z += triangle.v2.Z;
+                _centroidDiv++;
+            }
             if (!m_vertices.ContainsKey(triangle.v3))
+            {
                 m_vertices[triangle.v3] = m_vertices.Count;
+                _centroid.X += triangle.v3.X;
+                _centroid.Y += triangle.v3.Y;
+                _centroid.Z += triangle.v3.Z;
+                _centroidDiv++;
+            }
             m_triangles.Add(triangle);
         }
 
+        public Vector3 GetCentroid()
+        {
+            if (_centroidDiv > 0)
+                return new Vector3(_centroid.X / _centroidDiv, _centroid.Y / _centroidDiv, _centroid.Z / _centroidDiv);
+            else
+                return Vector3.Zero;
+        }
+
         public void CalcNormals()
         {
             int iTriangles = m_triangles.Count;
diff --git a/OpenSim/Region/Physics/Meshing/Meshmerizer.cs b/OpenSim/Region/Physics/Meshing/Meshmerizer.cs
index 6f6ed7f..c4b245f 100644
--- a/OpenSim/Region/Physics/Meshing/Meshmerizer.cs
+++ b/OpenSim/Region/Physics/Meshing/Meshmerizer.cs
@@ -74,6 +74,8 @@ namespace OpenSim.Region.Physics.Meshing
 #endif
 
         private bool cacheSculptMaps = true;
+        private bool cacheSculptAlphaMaps = true;
+
         private string decodedSculptMapPath = null;
         private bool useMeshiesPhysicsMesh = false;
 
@@ -87,7 +89,16 @@ namespace OpenSim.Region.Physics.Meshing
             IConfig mesh_config = config.Configs["Mesh"];
 
             decodedSculptMapPath = start_config.GetString("DecodedSculptMapPath","j2kDecodeCache");
+
             cacheSculptMaps = start_config.GetBoolean("CacheSculptMaps", cacheSculptMaps);
+
+            if (Environment.OSVersion.Platform == PlatformID.Unix)
+            {
+                cacheSculptAlphaMaps = false;
+            }
+            else
+                cacheSculptAlphaMaps = cacheSculptMaps;      
+
             if(mesh_config != null)
                 useMeshiesPhysicsMesh = mesh_config.GetBoolean("UseMeshiesPhysicsMesh", useMeshiesPhysicsMesh);
 
@@ -268,15 +279,18 @@ namespace OpenSim.Region.Physics.Meshing
                 {
                     if (!GenerateCoordsAndFacesFromPrimSculptData(primName, primShape, size, lod, out coords, out faces))
                         return null;
+                    // Remove the reference to any JPEG2000 sculpt data so it can be GCed
+                    // don't loose it
+                    //            primShape.SculptData = Utils.EmptyBytes;
                 }
+//                primShape.SculptDataLoaded = true;
             }
             else
             {
                 if (!GenerateCoordsAndFacesFromPrimShapeData(primName, primShape, size, lod, out coords, out faces))
                     return null;
             }
-
-            // Remove the reference to any JPEG2000 sculpt data so it can be GCed
+            // keep compatible
             primShape.SculptData = Utils.EmptyBytes;
 
             int numCoords = coords.Count;
@@ -482,7 +496,8 @@ namespace OpenSim.Region.Physics.Meshing
 
                     //idata = CSJ2K.J2kImage.FromBytes(primShape.SculptData);
 
-                    if (cacheSculptMaps)
+                    if (cacheSculptMaps && (cacheSculptAlphaMaps || (((ImageFlags)(idata.Flags) & ImageFlags.HasAlpha) ==0)))
+                        // don't cache images with alpha channel in linux since mono can't load them correctly)
                     {
                         try { idata.Save(decodedSculptFileName, ImageFormat.MemoryBmp); }
                         catch (Exception e) { m_log.Error("[SCULPT]: unable to cache sculpt map " + decodedSculptFileName + " " + e.Message); }
diff --git a/OpenSim/Region/Physics/Meshing/SculptMap.cs b/OpenSim/Region/Physics/Meshing/SculptMap.cs
index 740424e..b3d9cb6 100644
--- a/OpenSim/Region/Physics/Meshing/SculptMap.cs
+++ b/OpenSim/Region/Physics/Meshing/SculptMap.cs
@@ -58,28 +58,24 @@ namespace PrimMesher
             if (bmW == 0 || bmH == 0)
                 throw new Exception("SculptMap: bitmap has no data");
 
-            int numLodPixels = lod * 2 * lod * 2;  // (32 * 2)^2  = 64^2 pixels for default sculpt map image
+            int numLodPixels = lod * lod;  // (32 * 2)^2  = 64^2 pixels for default sculpt map image
 
+            bool smallMap = bmW * bmH <= numLodPixels;
             bool needsScaling = false;
 
-            bool smallMap = bmW * bmH <= lod * lod;
-
             width = bmW;
             height = bmH;
-            while (width * height > numLodPixels)
+            while (width * height > numLodPixels * 4)
             {
                 width >>= 1;
                 height >>= 1;
                 needsScaling = true;
             }
 
-
-
             try
             {
                 if (needsScaling)
-                    bm = ScaleImage(bm, width, height,
-                        System.Drawing.Drawing2D.InterpolationMode.NearestNeighbor);
+                    bm = ScaleImage(bm, width, height);
             }
 
             catch (Exception e)
@@ -87,7 +83,7 @@ namespace PrimMesher
                 throw new Exception("Exception in ScaleImage(): e: " + e.ToString());
             }
 
-            if (width * height > lod * lod)
+            if (width * height > numLodPixels)
             {
                 width >>= 1;
                 height >>= 1;
@@ -144,15 +140,17 @@ namespace PrimMesher
             int rowNdx, colNdx;
             int smNdx = 0;
 
+
             for (rowNdx = 0; rowNdx < numRows; rowNdx++)
             {
                 List<Coord> row = new List<Coord>(numCols);
                 for (colNdx = 0; colNdx < numCols; colNdx++)
                 {
+
                     if (mirror)
-                        row.Add(new Coord(-(redBytes[smNdx] * pixScale - 0.5f), (greenBytes[smNdx] * pixScale - 0.5f), blueBytes[smNdx] * pixScale - 0.5f));
+                        row.Add(new Coord(-((float)redBytes[smNdx] * pixScale - 0.5f), ((float)greenBytes[smNdx] * pixScale - 0.5f), (float)blueBytes[smNdx] * pixScale - 0.5f));
                     else
-                        row.Add(new Coord(redBytes[smNdx] * pixScale - 0.5f, greenBytes[smNdx] * pixScale - 0.5f, blueBytes[smNdx] * pixScale - 0.5f));
+                        row.Add(new Coord((float)redBytes[smNdx] * pixScale - 0.5f, (float)greenBytes[smNdx] * pixScale - 0.5f, (float)blueBytes[smNdx] * pixScale - 0.5f));
 
                     ++smNdx;
                 }
@@ -161,23 +159,39 @@ namespace PrimMesher
             return rows;
         }
 
-        private Bitmap ScaleImage(Bitmap srcImage, int destWidth, int destHeight,
-                System.Drawing.Drawing2D.InterpolationMode interpMode)
+        private Bitmap ScaleImage(Bitmap srcImage, int destWidth, int destHeight)
         {
-            Bitmap scaledImage = new Bitmap(srcImage, destWidth, destHeight);
-            scaledImage.SetResolution(96.0f, 96.0f);
-
-            Graphics grPhoto = Graphics.FromImage(scaledImage);
-            grPhoto.InterpolationMode = interpMode;
 
-            grPhoto.DrawImage(srcImage,
-                new Rectangle(0, 0, destWidth, destHeight),
-                new Rectangle(0, 0, srcImage.Width, srcImage.Height),
-                GraphicsUnit.Pixel);
+            Bitmap scaledImage = new Bitmap(destWidth, destHeight, PixelFormat.Format24bppRgb);
+            
+            Color c;
+            float xscale = srcImage.Width / destWidth;
+            float yscale = srcImage.Height / destHeight;
+            
+            float sy = 0.5f;
+            for (int y = 0; y < destHeight; y++)
+            {
+                float sx = 0.5f;
+                for (int x = 0; x < destWidth; x++)
+                {
+                    try
+                    {
+                        c = srcImage.GetPixel((int)(sx), (int)(sy));
+                        scaledImage.SetPixel(x, y, Color.FromArgb(c.R, c.G, c.B));
+                    }
+                    catch (IndexOutOfRangeException)
+                    {
+                    }
 
-            grPhoto.Dispose();
+                    sx += xscale;
+                }
+                sy += yscale;
+            }
+            srcImage.Dispose();
             return scaledImage;
         }
+
+    }        
+    
     }
-}
 #endif
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/AssemblyInfo.cs b/OpenSim/Region/Physics/UbitOdePlugin/AssemblyInfo.cs
new file mode 100644
index 0000000..d46341b
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitOdePlugin/AssemblyInfo.cs
@@ -0,0 +1,58 @@
+/*
+ * 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;
+
+// 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("OdePlugin")]
+[assembly : AssemblyDescription("Ubit Variation")]
+[assembly : AssemblyConfiguration("")]
+[assembly : AssemblyCompany("http://opensimulator.org")]
+[assembly : AssemblyProduct("OdePlugin")]
+[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("0.6.5.*")]
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs
new file mode 100644
index 0000000..793e281
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs
@@ -0,0 +1,1451 @@
+/*
+ * 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.Physics.Manager;
+using log4net;
+
+namespace OpenSim.Region.Physics.OdePlugin
+{
+    /// <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 bool _zeroFlag = false;
+        private Vector3 _velocity;
+        private Vector3 _target_velocity;
+        private Vector3 _acceleration;
+        private Vector3 m_rotationalVelocity;
+        private float m_mass = 80f;
+        public float m_density = 60f;
+        private bool m_pidControllerActive = true;
+        public float PID_D = 800.0f;
+        public float PID_P = 900.0f;
+        //private static float POSTURE_SERVO = 10000.0f;
+        public float CAPSULE_RADIUS = 0.37f;
+        public float CAPSULE_LENGTH = 2.140599f;
+        public float walkDivisor = 1.3f;
+        public float runDivisor = 0.8f;
+        private bool flying = false;
+        private bool m_iscolliding = false;
+        private bool m_iscollidingGround = false;
+        private bool m_iscollidingObj = false;
+        private bool m_alwaysRun = false;
+        private int m_requestedUpdateFrequency = 0;
+        public 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 CollisionLocker ode;
+
+        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.Geom
+                                                        | CollisionCategories.Space
+                                                        | CollisionCategories.Body
+                                                        | CollisionCategories.Character
+                                                        );
+        // we do land collisions not ode                                                       | CollisionCategories.Land);
+        public IntPtr Body = IntPtr.Zero;
+        private OdeScene _parent_scene;
+        public IntPtr Shell = IntPtr.Zero;
+        public IntPtr Amotor = IntPtr.Zero;
+        public d.Mass ShellMass;
+//        public bool collidelock = false;
+
+        private bool m_haseventsubscription = false;
+        public int m_eventsubscription = 0;
+        private CollisionEventUpdate CollisionEventsThisFrame = new CollisionEventUpdate();
+
+        // unique UUID of this character object
+        public UUID m_uuid;
+        public bool bad = false;
+
+        public ContactData AvatarContactData = new ContactData(10f, 0.3f);
+
+        public OdeCharacter(String avName, OdeScene parent_scene, Vector3 pos, Vector3 size, float pid_d, float pid_p, float capsule_radius, float density, float walk_divisor, float rundivisor)
+        {
+            m_uuid = UUID.Random();
+
+            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)_parent_scene.WorldExtents.X * 0.5f), ((float)_parent_scene.WorldExtents.Y * 0.5f), parent_scene.GetTerrainHeightAtXY(128f, 128f) + 10f);
+                m_log.Warn("[PHYSICS]: Got NaN Position on Character Create");
+            }
+
+            _parent_scene = parent_scene;
+
+            PID_D = pid_d;
+            PID_P = pid_p;
+            CAPSULE_RADIUS = capsule_radius;
+            m_density = density;
+            m_mass = 80f; // sure we have a default
+
+            AvatarContactData.mu = parent_scene.AvatarFriction;
+            AvatarContactData.bounce = parent_scene.AvatarBounce;
+
+            walkDivisor = walk_divisor;
+            runDivisor = rundivisor;
+
+            CAPSULE_LENGTH = size.Z * 1.15f - CAPSULE_RADIUS * 2.0f;
+            //m_log.Info("[SIZE]: " + CAPSULE_LENGTH.ToString());
+
+            m_isPhysical = false; // current status: no ODE information exists
+
+            m_name = avName;
+
+            AddChange(changes.Add, null);
+        }
+
+        public override int PhysicsActorType
+        {
+            get { return (int)ActorTypes.Agent; }
+            set { return; }
+        }
+
+        public override ContactData ContactData
+        {
+            get { return AvatarContactData; }
+        }
+
+        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
+        {
+            set { m_localID = value; }
+        }
+
+        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 false; }
+            set { return; }
+        }
+
+        public override bool ThrottleUpdates
+        {
+            get { return false; }
+            set { return; }
+        }
+
+        public override bool Flying
+        {
+            get { return flying; }
+            set
+            {
+                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 += 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
+                {
+//                    SetPidStatus(false);
+                    m_pidControllerActive = true;
+                    m_iscolliding = true;
+                }
+            }
+        }
+
+        /// <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 (Body == IntPtr.Zero || Shell == IntPtr.Zero)
+                {
+                    if (value.IsFinite())
+                    {
+                        if (value.Z > 9999999f)
+                        {
+                            value.Z = _parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
+                        }
+                        if (value.Z < -100f)
+                        {
+                            value.Z = _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 {
+                float d = CAPSULE_RADIUS * 2;
+                return new Vector3(d, d, (CAPSULE_LENGTH +d)/1.15f); }
+            set
+            {
+                if (value.IsFinite())
+                {
+                    AddChange(changes.Size, value);
+                }
+                else
+                {
+                    m_log.Warn("[PHYSICS]: Got a NaN Size 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
+            {
+                float AVvolume = (float)(Math.PI * CAPSULE_RADIUS * CAPSULE_RADIUS * (1.3333333333f * CAPSULE_RADIUS + CAPSULE_LENGTH));
+                return m_density * AVvolume;
+            }
+        }
+        public override void link(PhysicsActor obj)
+        {
+
+        }
+
+        public override void delink()
+        {
+
+        }
+
+        public override void LockAngularMotion(Vector3 axis)
+        {
+
+        }
+
+
+        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;
+            }
+        }
+
+        //UBit mess
+        /* for later use
+                public override Vector3 PrimOOBsize
+                    {
+                    get
+                        {
+                        Vector3 s=Size;
+                        s.X *=0.5f;
+                        s.Y *=0.5f;
+                        s.Z *=0.5f;
+                        return s;
+                        }
+                    }
+ 
+                public override Vector3 PrimOOBoffset
+                    {
+                    get
+                        {
+                        return Vector3.Zero;
+                        }
+                    }
+        */
+
+        public override PrimitiveBaseShape Shape
+        {
+            set { return; }
+        }
+
+        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 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 Quaternion.Identity; }
+            set
+            {
+            }
+        }
+
+        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 / _parent_scene.ODE_STEPSIZE / 28f);
+                }
+                else
+                {
+                    AddChange(changes.Velocity, 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)
+        {
+        }
+
+
+        // WARNING: This MUST NOT be called outside of ProcessTaints, else we can have unsynchronized access
+        // to ODE internals. ProcessTaints is called from within thread-locked Simulate(), so it is the only 
+        // place that is safe to call this routine AvatarGeomAndBodyCreation.
+        private void AvatarGeomAndBodyCreation(float npositionX, float npositionY, float npositionZ)
+        {
+            _parent_scene.waitForSpaceUnlock(_parent_scene.ActiveSpace);
+            if (CAPSULE_LENGTH <= 0)
+            {
+                m_log.Warn("[PHYSICS]: The capsule size you specified in opensim.ini is invalid!  Setting it to the smallest possible size!");
+                CAPSULE_LENGTH = 0.01f;
+
+            }
+
+            if (CAPSULE_RADIUS <= 0)
+            {
+                m_log.Warn("[PHYSICS]: The capsule size you specified in opensim.ini is invalid!  Setting it to the smallest possible size!");
+                CAPSULE_RADIUS = 0.01f;
+
+            }
+            Shell = d.CreateCapsule(_parent_scene.ActiveSpace, CAPSULE_RADIUS, CAPSULE_LENGTH);
+
+            d.GeomSetCategoryBits(Shell, (int)m_collisionCategories);
+            d.GeomSetCollideBits(Shell, (int)m_collisionFlags);
+
+            d.MassSetCapsule(out ShellMass, m_density, 3, CAPSULE_RADIUS, CAPSULE_LENGTH);
+
+            m_mass = ShellMass.mass;  // update mass
+
+            // rescale PID parameters 
+            PID_D = _parent_scene.avPIDD;
+            PID_P = _parent_scene.avPIDP;
+
+            // rescale PID parameters so that this aren't affected by mass
+            // and so don't get unstable for some masses
+            // also scale by ode time step so you don't need to refix them
+
+            PID_D /= 50 * 80; //scale to original mass of around 80 and 50 ODE fps
+            PID_D *= m_mass / _parent_scene.ODE_STEPSIZE;
+            PID_P /= 50 * 80;
+            PID_P *= m_mass / _parent_scene.ODE_STEPSIZE;
+
+            Body = d.BodyCreate(_parent_scene.world);
+
+            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(Shell, Body);
+
+            // The purpose of the AMotor here is to keep the avatar's physical
+            // surrogate from rotating while moving
+            Amotor = d.JointCreateAMotor(_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, 5e6f);
+            d.JointSetAMotorParam(Amotor, (int)dParam.FMax2, 5e6f);
+            d.JointSetAMotorParam(Amotor, (int)dParam.FMax3, 5e6f);
+        }
+
+        /// <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 Geometry
+            if (Shell != IntPtr.Zero)
+            {
+                _parent_scene.geom_name_map.Remove(Shell);
+                _parent_scene.waitForSpaceUnlock(_parent_scene.ActiveSpace);
+                d.GeomDestroy(Shell);
+                _parent_scene.geom_name_map.Remove(Shell);
+                Shell = IntPtr.Zero;
+            }
+        }
+
+        /// <summary>
+        /// Called from Simulate
+        /// This is the avatar's movement control + PID Controller
+        /// </summary>
+        /// <param name="timeStep"></param>
+        public void Move(float timeStep, List<OdeCharacter> defects)
+        {
+            //  no lock; for now it's only called from within Simulate()
+
+            // If the PID Controller isn't active then we set our force
+            // calculating base velocity to the current position
+
+            if (Body == IntPtr.Zero)
+                return;
+
+            d.Vector3 dtmp;
+            d.BodyCopyPosition(Body, out dtmp);
+            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 = 1;
+            qtmp.X = 0;
+            qtmp.Y = 0;
+            qtmp.Z = 0;
+            d.BodySetQuaternion(Body, ref qtmp);
+
+            if (m_pidControllerActive == false)
+            {
+                _zeroPosition = localpos;
+            }
+            //PidStatus = true;
+
+
+            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;
+            }
+
+            Vector3 vec = Vector3.Zero;
+            dtmp = d.BodyGetLinearVel(Body);
+            Vector3 vel = new Vector3(dtmp.X, dtmp.Y, dtmp.Z);
+
+            float movementdivisor = 1f;
+            //Ubit change divisions into multiplications below
+            if (!m_alwaysRun)
+            {
+                movementdivisor = 1 / walkDivisor;
+            }
+            else
+            {
+                movementdivisor = 1 / runDivisor;
+            }
+
+            // colide with land
+
+            d.AABB aabb;
+            d.GeomGetAABB(Shell, out aabb);
+            float chrminZ = aabb.MinZ;
+
+            Vector3 posch = localpos;
+
+            float ftmp;
+
+            if (flying)
+            {
+                ftmp = timeStep;
+                posch.X += vel.X * ftmp;
+                posch.Y += vel.Y * ftmp;
+            }
+
+            float terrainheight = _parent_scene.GetTerrainHeightAtXY(posch.X, posch.Y);
+            if (chrminZ < terrainheight)
+            {
+                float depth = terrainheight - chrminZ;
+                if (!flying)
+                {
+                    vec.Z = -vel.Z * PID_D * 1.5f + depth * PID_P * 50;
+                }
+                else
+                    vec.Z = depth * PID_P * 50;
+
+                /*
+                                Vector3 vtmp;
+                                vtmp.X = _target_velocity.X * timeStep;
+                                vtmp.Y = _target_velocity.Y * timeStep;
+                                // fake and avoid squares
+                                float k = (Math.Abs(vtmp.X) + Math.Abs(vtmp.Y));
+                                if (k > 0)
+                                    {
+                                    posch.X += vtmp.X;
+                                    posch.Y += vtmp.Y;
+                                    terrainheight -= _parent_scene.GetTerrainHeightAtXY(posch.X, posch.Y);
+                                    k = 1 + Math.Abs(terrainheight) / k;
+                                    movementdivisor /= k;
+
+                                    if (k < 1)
+                                        k = 1;
+                                    }
+                */
+
+
+                if (depth < 0.1f)
+                {
+                    m_iscolliding = true;
+                    m_colliderfilter = 2;
+                    m_iscollidingGround = true;
+
+                    ContactPoint contact = new ContactPoint();
+                    contact.PenetrationDepth = depth;
+                    contact.Position.X = localpos.X;
+                    contact.Position.Y = localpos.Y;
+                    contact.Position.Z = chrminZ;
+                    contact.SurfaceNormal.X = 0f;
+                    contact.SurfaceNormal.Y = 0f;
+                    contact.SurfaceNormal.Z = -1f;
+                    AddCollisionEvent(0, contact);
+
+                    vec.Z *= 0.5f;
+                }
+
+                else
+                    m_iscollidingGround = false;
+            }
+            else
+                m_iscollidingGround = false;
+
+
+            //  if velocity is zero, use position control; otherwise, velocity control
+            if (_target_velocity.X == 0.0f && _target_velocity.Y == 0.0f && _target_velocity.Z == 0.0f
+                && m_iscolliding)
+            {
+                //  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 + (_zeroPosition.X - localpos.X) * (PID_P * 2);
+                    vec.Y = -vel.Y * PID_D + (_zeroPosition.Y - localpos.Y) * (PID_P * 2);
+                    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 (!flying)
+                    {
+                        if (_target_velocity.Z > 0.0f)
+                        {
+                            // We're colliding with something and we're not flying but we're moving
+                            // This means we're walking or running. JUMPING
+                            vec.Z += (_target_velocity.Z - vel.Z) * PID_D * 1.2f;// +(_zeroPosition.Z - localpos.Z) * PID_P;
+                        }
+                        // We're standing on something
+                        vec.X = ((_target_velocity.X * movementdivisor) - vel.X) * (PID_D);
+                        vec.Y = ((_target_velocity.Y * movementdivisor) - vel.Y) * (PID_D);
+                    }
+                    else
+                    {
+                        // We're flying and colliding with something
+                        vec.X = ((_target_velocity.X * movementdivisor) - vel.X) * (PID_D * 0.0625f);
+                        vec.Y = ((_target_velocity.Y * movementdivisor) - vel.Y) * (PID_D * 0.0625f);
+                        vec.Z += (_target_velocity.Z - vel.Z) * (PID_D);
+                    }
+                }
+                else // ie not colliding
+                {
+                    if (flying) //(!m_iscolliding && flying)
+                    {
+                        // we're in mid air suspended
+                        vec.X = ((_target_velocity.X * movementdivisor) - vel.X) * (PID_D * 1.667f);
+                        vec.Y = ((_target_velocity.Y * movementdivisor) - vel.Y) * (PID_D * 1.667f);
+                        vec.Z += (_target_velocity.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 = (_target_velocity.X - vel.X) * PID_D * 0.833f;
+                        vec.Y = (_target_velocity.Y - vel.Y) * PID_D * 0.833f;
+                    }
+                }
+            }
+
+            if (flying)
+            {
+                vec.Z -= _parent_scene.gravityz * m_mass;
+
+                //Added for auto fly height. Kitto Flora
+                float target_altitude = _parent_scene.GetTerrainHeightAtXY(localpos.X, localpos.Y) + 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();
+            }
+        }
+
+        /// <summary>
+        /// Updates the reported position and velocity.  This essentially sends the data up to ScenePresence.
+        /// </summary>
+        public void UpdatePositionAndVelocity()
+        {
+            //  no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit!
+            if (Body == IntPtr.Zero)
+                return;
+
+            d.Vector3 vec;
+            try
+            {
+                d.BodyCopyPosition(Body, out vec);
+            }
+            catch (NullReferenceException)
+            {
+                bad = true;
+                _parent_scene.BadCharacter(this);
+                vec = new d.Vector3(_position.X, _position.Y, _position.Z);
+                base.RaiseOutOfBounds(_position); // Tells ScenePresence that there's a problem!
+                m_log.WarnFormat("[ODEPLUGIN]: Avatar Null reference for Avatar {0}, physical actor {1}", m_name, m_uuid);
+            }
+
+            _position.X = vec.X;
+            _position.Y = vec.Y;
+            _position.Z = vec.Z;
+
+            bool fixbody = false;
+
+            if (_position.X < 0.0f)
+            {
+                fixbody = true;
+                _position.X = 0.1f;
+            }
+            else if (_position.X > (int)_parent_scene.WorldExtents.X - 0.1f)
+            {
+                fixbody = true;
+                _position.X = (int)_parent_scene.WorldExtents.X - 0.1f;
+            }
+
+            if (_position.Y < 0.0f)
+            {
+                fixbody = true;
+                _position.Y = 0.1f;
+            }
+            else if (_position.Y > (int)_parent_scene.WorldExtents.Y - 0.1)
+            {
+                fixbody = true;
+                _position.Y = (int)_parent_scene.WorldExtents.Y - 0.1f;
+            }
+
+            if (fixbody)
+                d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
+
+            // Did we move last? = zeroflag
+            // This helps keep us from sliding all over
+/*
+            if (_zeroFlag)
+            {
+                _velocity.X = 0.0f;
+                _velocity.Y = 0.0f;
+                _velocity.Z = 0.0f;
+
+                // Did we send out the 'stopped' message?
+                if (!m_lastUpdateSent)
+                {
+                    m_lastUpdateSent = true;
+                    base.RequestPhysicsterseUpdate();
+                }
+            }
+            else
+            {
+                m_lastUpdateSent = false;
+ */
+                try
+                {
+                    vec = d.BodyGetLinearVel(Body);
+                }
+                catch (NullReferenceException)
+                {
+                    vec.X = _velocity.X;
+                    vec.Y = _velocity.Y;
+                    vec.Z = _velocity.Z;
+                }
+                _velocity.X = (vec.X);
+                _velocity.Y = (vec.Y);
+                _velocity.Z = (vec.Z);
+ //           }
+        }
+
+        /// <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 { set { return; } }
+        public override float PIDTau { set { return; } }
+
+        public override float PIDHoverHeight { set { return; } }
+        public override bool PIDHoverActive { set { return; } }
+        public override PIDHoverType PIDHoverType { set { return; } }
+        public override float PIDHoverTau { set { return; } }
+
+        public override Quaternion APIDTarget { set { return; } }
+
+        public override bool APIDActive { set { return; } }
+
+        public override float APIDStrength { set { return; } }
+
+        public override float APIDDamping { set { return; } }
+
+
+        public override void SubscribeEvents(int ms)
+        {
+            m_requestedUpdateFrequency = ms;
+            m_eventsubscription = ms;
+            _parent_scene.AddCollisionEventReporting(this);
+            m_haseventsubscription = true;
+        }
+
+        public override void UnSubscribeEvents()
+        {
+            m_haseventsubscription = false;
+            _parent_scene.RemoveCollisionEventReporting(this);
+            m_requestedUpdateFrequency = 0;
+            m_eventsubscription = 0;
+        }
+
+        public void AddCollisionEvent(uint CollidedWith, ContactPoint contact)
+        {
+            if (m_haseventsubscription)
+            {
+                //                m_log.DebugFormat(
+                //                    "[PHYSICS]: Adding collision event for {0}, collidedWith {1}, contact {2}", "", CollidedWith, contact);
+
+                CollisionEventsThisFrame.AddCollider(CollidedWith, contact);
+            }
+        }
+
+        public void SendCollisions()
+        {
+            if (m_haseventsubscription && m_eventsubscription > m_requestedUpdateFrequency)
+            {
+                if (CollisionEventsThisFrame != null)
+                {
+                    base.SendCollisionUpdate(CollisionEventsThisFrame);
+                }
+                CollisionEventsThisFrame = new CollisionEventUpdate();
+                m_eventsubscription = 0;
+            }
+        }
+
+        public override bool SubscribedEvents()
+        {
+            return m_haseventsubscription;
+        }
+
+        private void changePhysicsStatus(bool NewStatus)
+        {
+            if (NewStatus != m_isPhysical)
+            {
+                if (NewStatus)
+                {
+                    // Create avatar capsule and related ODE data
+                    if ((Shell != IntPtr.Zero))
+                    {
+                        // a lost shell ?
+                        m_log.Warn("[PHYSICS]: re-creating the following avatar ODE data, even though it already exists - "
+                            + (Shell != IntPtr.Zero ? "Shell " : "")
+                            + (Body != IntPtr.Zero ? "Body " : "")
+                            + (Amotor != IntPtr.Zero ? "Amotor " : ""));
+                        AvatarGeomAndBodyDestroy();
+                    }
+
+                    AvatarGeomAndBodyCreation(_position.X, _position.Y, _position.Z);
+                    _parent_scene.geom_name_map[Shell] = m_name;
+                    _parent_scene.actor_name_map[Shell] = (PhysicsActor)this;
+                    _parent_scene.AddCharacter(this);
+                }
+                else
+                {
+                    _parent_scene.RemoveCharacter(this);
+                    // destroy avatar capsule and related ODE data
+                    AvatarGeomAndBodyDestroy();
+                }
+
+                m_isPhysical = NewStatus;
+            }
+        }
+
+        private void changeAdd()
+        {
+            changePhysicsStatus(true);
+        }
+
+        private void changeRemove()
+        {
+            changePhysicsStatus(false);
+        }
+
+        private void changeShape(PrimitiveBaseShape arg)
+        {
+        }
+
+        private void changeSize(Vector3 Size)
+        {
+            if (Size.IsFinite())
+            {
+                float caplen = Size.Z;
+
+                caplen = caplen * 1.15f - CAPSULE_RADIUS * 2.0f;
+
+                if (caplen != CAPSULE_LENGTH)
+                {
+                    if (Shell != IntPtr.Zero && Body != IntPtr.Zero && Amotor != IntPtr.Zero)
+                    {
+                        AvatarGeomAndBodyDestroy();
+
+                        float prevCapsule = CAPSULE_LENGTH;
+                        CAPSULE_LENGTH = caplen;
+
+                        AvatarGeomAndBodyCreation(_position.X, _position.Y,
+                                          _position.Z + (Math.Abs(CAPSULE_LENGTH - prevCapsule) * 2));
+
+                        Velocity = Vector3.Zero;
+
+                        _parent_scene.geom_name_map[Shell] = m_name;
+                        _parent_scene.actor_name_map[Shell] = (PhysicsActor)this;
+                    }
+                    else
+                    {
+                        m_log.Warn("[PHYSICS]: trying to change capsule size, but the following ODE data is missing - "
+                            + (Shell == IntPtr.Zero ? "Shell " : "")
+                            + (Body == IntPtr.Zero ? "Body " : "")
+                            + (Amotor == IntPtr.Zero ? "Amotor " : ""));
+                    }
+                }
+
+                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;
+            }
+
+        private void changeOrientation(Quaternion newOri)
+        {
+        }
+
+        private void changeVelocity(Vector3 newVel)
+        {
+            m_pidControllerActive = true;
+            _target_velocity = newVel;
+        }
+
+        private void changeSetTorque(Vector3 newTorque)
+        {
+        }                                 
+
+        private void changeAddForce(Vector3 newForce)
+        {
+        }                                 
+
+        private void changeAddAngularForce(Vector3 arg)
+        {
+        }                                 
+
+        private void changeAngularLock(Vector3 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 changeForce(Vector3 newForce)
+        {
+            m_pidControllerActive = false;
+            if (Body != IntPtr.Zero)
+            {
+                if (newForce.X != 0f || newForce.Y != 0f || newForce.Z != 0)
+                    d.BodyAddForce(Body, newForce.X, newForce.Y, newForce.Z);
+            }
+        }
+
+        private void donullchange()
+        {
+        }
+
+        public bool DoAChange(changes what, object arg)
+        {
+            if (Shell == 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.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((Vector3)arg);
+                    break;
+
+                case changes.Size:
+                    changeSize((Vector3)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)
+        {
+            _parent_scene.AddChange((PhysicsActor)this, what, arg);
+        }
+
+
+        internal void AddCollisionFrameTime(int p)
+        {
+            // protect it from overflow crashing
+            if (m_eventsubscription + p >= int.MaxValue)
+                m_eventsubscription = 0;
+            m_eventsubscription += p;
+        }
+    }
+}
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs
new file mode 100644
index 0000000..80218e7
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs
@@ -0,0 +1,849 @@
+/*
+ * 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.
+ */
+
+// 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.Physics.Manager;
+
+namespace OpenSim.Region.Physics.OdePlugin
+{
+    public class ODEDynamics
+    {
+        public Vehicle Type
+        {
+            get { return m_type; }
+        }
+
+        private OdePrim rootPrim;
+        private OdeScene _pParentScene;
+
+        // Vehicle properties
+        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_lastLinearVelocityVector = Vector3.Zero;
+        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
+        private Vector3 m_lastAngularVelocity = Vector3.Zero;           // what was last applied to body
+
+        //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 = 0;
+
+        //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 Vector3 m_dir = Vector3.Zero;                           // velocity applied to body
+
+        private float m_lmEfect = 0;                                            // current linear motor eficiency
+        private float m_amEfect = 0;                                            // current angular motor eficiency
+
+
+        public ODEDynamics(OdePrim rootp)
+        {
+            rootPrim = rootp;
+            _pParentScene = rootPrim._parent_scene;
+        }
+
+        internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue)
+        {
+            float len;
+            float invtimestep = 1.0f / _pParentScene.ODE_STEPSIZE;
+            float timestep = _pParentScene.ODE_STEPSIZE;
+
+            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 < timestep) pValue = timestep;
+                    m_angularDeflectionTimescale = pValue;
+                    break;
+                case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE:
+                    //                    if (pValue < timestep) pValue = timestep;
+                    // try to make impulses to work a bit better
+                    if (pValue < 0.5f) pValue = 0.5f;
+                    else if (pValue > 120) pValue = 120;
+                    m_angularMotorDecayTimescale = pValue * invtimestep;
+                    break;
+                case Vehicle.ANGULAR_MOTOR_TIMESCALE:
+                    if (pValue < timestep) pValue = 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 < timestep) pValue = 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 < timestep) pValue = 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 < timestep) pValue = timestep;
+                    m_linearDeflectionTimescale = pValue;
+                    break;
+                case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE:
+                    //                    if (pValue < timestep) pValue = timestep;
+                    // try to make impulses to work a bit better
+                    if (pValue < 0.5f) pValue = 0.5f;
+                    else if (pValue > 120) pValue = 120;
+                    m_linearMotorDecayTimescale = pValue * invtimestep;
+                    break;
+                case Vehicle.LINEAR_MOTOR_TIMESCALE:
+                    if (pValue < timestep) pValue = 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 < timestep) pValue = 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 < timestep) pValue = 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
+                    break;
+                case Vehicle.LINEAR_FRICTION_TIMESCALE:
+                    if (pValue < timestep) pValue = 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 > 30.0f)
+                        m_linearMotorDirection *= (30.0f / len);
+                    m_lmEfect = 1.0f; // turn it on
+                    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;
+            float invtimestep = 1.0f / _pParentScene.ODE_STEPSIZE;
+            float timestep = _pParentScene.ODE_STEPSIZE;
+            switch (pParam)
+            {
+                case Vehicle.ANGULAR_FRICTION_TIMESCALE:
+                    if (pValue.X < timestep) pValue.X = timestep;
+                    if (pValue.Y < timestep) pValue.Y = timestep;
+                    if (pValue.Z < timestep) pValue.Z = 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
+                    break;
+                case Vehicle.LINEAR_FRICTION_TIMESCALE:
+                    if (pValue.X < timestep) pValue.X = timestep;
+                    if (pValue.Y < timestep) pValue.Y = timestep;
+                    if (pValue.Z < timestep) pValue.Z = 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 > 30.0f)
+                        m_linearMotorDirection *= (30.0f / len);
+                    m_lmEfect = 1.0f; // turn it on
+                    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);
+                    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)
+        {
+            float invtimestep = _pParentScene.ODE_STEPSIZE;
+            m_lmEfect = 0;
+            m_amEfect = 0;
+
+            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 * invtimestep;
+                    m_angularMotorTimescale = 1000;
+                    m_angularMotorDecayTimescale = 1000 * invtimestep;
+                    m_VhoverHeight = 0;
+                    m_VhoverTimescale = 1000;
+                    m_VehicleBuoyancy = 0;
+                    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 * invtimestep;
+                    m_angularMotorTimescale = 1000;
+                    m_angularMotorDecayTimescale = 120 * invtimestep;
+                    m_VhoverHeight = 0;
+                    m_VhoverEfficiency = 1;
+                    m_VhoverTimescale = 10;
+                    m_VehicleBuoyancy = 0;
+                    m_linearDeflectionEfficiency = 1;
+                    m_linearDeflectionTimescale = 1;
+                    m_angularDeflectionEfficiency = 0;
+                    m_angularDeflectionTimescale = 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 * invtimestep;
+                    m_angularMotorTimescale = 1;
+                    m_angularMotorDecayTimescale = 0.8f * 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 * invtimestep;
+                    m_angularMotorTimescale = 4;
+                    m_angularMotorDecayTimescale = 4 * 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_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 * invtimestep;
+                    m_angularMotorTimescale = 4;
+                    m_angularMotorDecayTimescale = 8 * 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 * invtimestep;
+                    m_angularMotorTimescale = 6;
+                    m_angularMotorDecayTimescale = 10 * invtimestep;
+                    m_VhoverHeight = 5;
+                    m_VhoverEfficiency = 0.8f;
+                    m_VhoverTimescale = 10;
+                    m_VehicleBuoyancy = 1;
+                    m_linearDeflectionEfficiency = 0;
+                    m_linearDeflectionTimescale = 5 * invtimestep;
+                    m_angularDeflectionEfficiency = 0;
+                    m_angularDeflectionTimescale = 5;
+                    m_verticalAttractionEfficiency = 0f;
+                    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);
+                    m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY |
+                        VehicleFlag.HOVER_GLOBAL_HEIGHT);
+                    break;
+            }
+
+        }//end SetDefaultsForType
+
+        internal void Stop()
+        {
+            m_lmEfect = 0;
+            m_amEfect = 0;
+        }
+
+        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 halfpi = 0.5f * (float)Math.PI;
+
+        public static Vector3 ubitRot2Euler(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()//float pTimestep)
+        {
+            IntPtr Body = rootPrim.Body;
+
+            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
+
+            // 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 *= rootPrim.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 *= (1.0f - 1.0f / m_linearMotorDecayTimescale);
+            }
+            else
+                m_lmEfect = 0;
+
+            // friction
+            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
+                force.X += tmpV.X;
+                force.Y += tmpV.Y;
+                force.Z += tmpV.Z;
+            }
+
+            // hover
+            if (m_VhoverTimescale < 300)
+            {
+                d.Vector3 pos = d.BodyGetPosition(Body);
+
+                // default to global
+                float perr = m_VhoverHeight - pos.Z;;
+                
+                if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0)
+                {
+                    perr += _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y);
+                }
+                else if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) != 0)
+                {
+                    perr += _pParentScene.GetWaterLevel();
+                }
+                else if ((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == 0)
+                {
+                    float t = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y);
+                    float w = _pParentScene.GetWaterLevel();
+                    if (t > w)
+                        perr += t;
+                    else
+                        perr += w;
+                }
+
+                if ((m_flags & VehicleFlag.HOVER_UP_ONLY) == 0 || perr > 0)
+                {
+                    force.Z += (perr / m_VhoverTimescale / m_VhoverTimescale - curVel.Z * m_VhoverEfficiency) / _pParentScene.ODE_STEPSIZE;
+                    force.Z += _pParentScene.gravityz * (1f - m_VehicleBuoyancy);
+                }
+                else // no buoyancy
+                    force.Z += _pParentScene.gravityz;
+            }
+            else
+            {
+                // default gravity and buoancy
+                force.Z += _pParentScene.gravityz * (1f - m_VehicleBuoyancy);
+            }
+
+            // linear deflection
+            if (m_linearDeflectionEfficiency > 0)
+            {
+                float len = curVel.Length();
+                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;
+                }
+            }
+
+            // 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;
+                torque.Y += tmpV.Y;
+                torque.Z += tmpV.Z;
+                m_amEfect *= (1 - 1.0f / m_angularMotorDecayTimescale);
+            }
+            else
+                m_amEfect = 0;
+
+            // angular friction
+            if (curLocalAngVel.X != 0 || curLocalAngVel.Y != 0 || curLocalAngVel.Z != 0)
+            {
+                torque.X -= curLocalAngVel.X / m_angularFrictionTimescale.X;
+                torque.Y -= curLocalAngVel.Y / m_angularFrictionTimescale.Y;
+                torque.Z -= curLocalAngVel.Z / m_angularFrictionTimescale.Z;
+            }
+
+            // 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;
+                }
+            }
+
+            // vertical atractor
+            if (m_verticalAttractionTimescale < 300)
+            {
+                float roll;
+                float pitch;
+
+                GetRollPitch(irotq, out roll, out pitch);
+
+                float ftmp = 1.0f / m_verticalAttractionTimescale / m_verticalAttractionTimescale / _pParentScene.ODE_STEPSIZE;
+                float ftmp2 = m_verticalAttractionEfficiency / _pParentScene.ODE_STEPSIZE;
+
+                if (Math.Abs(roll) > 0.01) // roll
+                {
+                    torque.X -= -roll * ftmp + curLocalAngVel.X * ftmp2;
+                }
+
+                if (Math.Abs(pitch) > 0.01 && ((m_flags & VehicleFlag.LIMIT_ROLL_ONLY) == 0)) // pitch
+                {
+                    torque.Y -= -pitch * ftmp + curLocalAngVel.Y * ftmp2;
+                }
+
+                if (m_bankingEfficiency != 0 && Math.Abs(roll) > 0.01)
+                {
+                    float broll = roll * 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 - m_bankingMix) + vfact);
+                        else
+                            broll *= -((1 - m_bankingMix) + vfact);                      
+                    }
+                    broll = (broll - curLocalAngVel.Z) / m_bankingTimescale;
+                    //                    torque.Z += broll; 
+
+                    // make z rot be in world Z not local as seems to be in sl
+                    tmpV.X = 0;
+                    tmpV.Y = 0;
+                    tmpV.Z = broll;
+                    tmpV *= irotq;
+
+                    torque.X += tmpV.X;
+                    torque.Y += tmpV.Y;
+                    torque.Z += tmpV.Z;
+                }
+            }
+            
+            d.Mass dmass;
+            d.BodyGetMass(Body,out dmass);
+
+            if (force.X != 0 || force.Y != 0 || force.Z != 0)
+            {
+                force *= dmass.mass;
+                d.BodySetForce(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
+            }
+        }
+    }
+}
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs
new file mode 100644
index 0000000..3b7f562
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs
@@ -0,0 +1,3317 @@
+/*
+ * 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 by Ubit Umarov
+ *
+ * 
+ */
+
+/*
+ * 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.Physics.Manager;
+
+
+namespace OpenSim.Region.Physics.OdePlugin
+{
+    public class OdePrim : PhysicsActor
+    {
+        private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
+
+        private bool m_isphysical;
+        private bool m_fakeisphysical;
+
+        protected bool m_building;
+        private Quaternion m_lastorientation = new Quaternion();
+        private Quaternion _orientation;
+
+        private Vector3 _position;
+        private Vector3 _velocity;
+        private Vector3 _torque;
+        private Vector3 m_lastVelocity;
+        private Vector3 m_lastposition;
+        private Vector3 m_rotationalVelocity;
+        private Vector3 _size;
+        private Vector3 _acceleration;
+        private Vector3 m_angularlock = Vector3.One;
+        private IntPtr Amotor = IntPtr.Zero;
+
+        private Vector3 m_force;
+        private Vector3 m_forceacc;
+        private Vector3 m_angularForceacc;
+
+        private Vector3 m_PIDTarget;
+        private float m_PIDTau;
+        private float PID_D = 35f;
+        private float PID_G = 25f;
+        private bool m_usePID;
+
+        // KF: These next 7 params apply to llSetHoverHeight(float height, integer water, float tau),
+        // and are for non-VEHICLES only.
+
+        private float m_PIDHoverHeight;
+        private float m_PIDHoverTau;
+        private bool m_useHoverPID;
+        private PIDHoverType m_PIDHoverType = PIDHoverType.Ground;
+        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 body_autodisable_frames = 20;
+
+        private const CollisionCategories m_default_collisionFlags = (CollisionCategories.Geom
+                                                        | CollisionCategories.Space
+                                                        | CollisionCategories.Body
+                                                        | CollisionCategories.Character
+                                                        );
+        private bool m_collidesLand = true;
+        private bool m_collidesWater;
+        public bool m_returnCollisions;
+
+        // Default we're a Geometry
+        private CollisionCategories m_collisionCategories = (CollisionCategories.Geom);
+
+        // Default, Collide with Other Geometries, spaces and Bodies
+        private CollisionCategories m_collisionFlags = m_default_collisionFlags;
+
+        public bool m_disabled;
+        public bool m_taintselected;
+
+        public uint m_localID;
+
+        private PrimitiveBaseShape _pbs;
+        public OdeScene _parent_scene;
+
+        /// <summary>
+        /// The physics space which contains prim geometry
+        /// </summary>
+        public IntPtr m_targetSpace = IntPtr.Zero;
+
+        public IntPtr prim_geom;
+        public IntPtr _triMeshData;
+
+        private PhysicsActor _parent;
+
+        private List<OdePrim> childrenPrim = new List<OdePrim>();
+
+        private bool m_iscolliding;
+        private bool m_wascolliding;
+        private bool m_isSelected;
+
+        internal bool m_isVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively
+
+        private bool m_throttleUpdates;
+        private int throttleCounter;
+        public int m_interpenetrationcount;
+        public float m_collisionscore;
+        int m_colliderfilter = 0;
+        public int m_roundsUnderMotionThreshold;
+        private int m_crossingfailures;
+
+        public bool outofBounds;
+        private float m_density = 10.000006836f; // Aluminum g/cm3;
+
+        public bool _zeroFlag;
+        private bool m_lastUpdateSent;
+
+        public IntPtr Body = IntPtr.Zero;
+        public String Name { get; private set; }
+        private Vector3 _target_velocity;
+
+        public Vector3 primOOBsize; // prim real dimensions from mesh 
+        public Vector3 primOOBoffset; // is centroid out of mesh or rest aabb
+        public float primOOBradiusSQ;
+        public d.Mass primdMass; // prim inertia information on it's own referencial
+        float primMass; // prim own mass
+        float _mass; // object mass acording to case
+        public d.Mass objectpMass; // object last computed inertia
+        private bool hasOOBoffsetFromMesh = false; // if true we did compute it form mesh centroid, else from aabb
+
+        public int givefakepos = 0;
+        private Vector3 fakepos;
+        public int givefakeori = 0;
+        private Quaternion fakeori;
+
+        public int m_eventsubscription;
+        private CollisionEventUpdate CollisionEventsThisFrame = new CollisionEventUpdate();
+
+        private IntPtr m_linkJoint = IntPtr.Zero;
+        private IntPtr _linkJointGroup = IntPtr.Zero;
+
+        public volatile bool childPrim;
+
+        public ODEDynamics m_vehicle;
+
+        internal int m_material = (int)Material.Wood;
+        protected ContactData primContactData = new ContactData { mu = 0f, bounce = 0.1f };
+
+        /// <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 Building  // this is not reliable for internal use
+        {
+            get { return m_building; }
+            set
+            {
+                if (value)
+                    m_building = true;
+                AddChange(changes.building, value);
+            }
+        }
+
+        public override ContactData ContactData
+        {
+            get
+            {
+                return primContactData;
+            }
+        }
+
+        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
+            {
+                //m_log.Info("[PHYSICS]: Setting TrackerID: " + value);
+                m_localID = value;
+            }
+        }
+
+        public override bool Grabbed
+        {
+            set { return; }
+        }
+
+        public override bool Selected
+        {
+            set
+            {
+                if (value)
+                    m_isSelected = value;
+                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;
+
+                if (m_wascolliding != m_iscolliding)
+                {
+                    if (m_wascolliding && !m_isSelected && Body != IntPtr.Zero)
+                        d.BodyEnable(Body);
+                    m_wascolliding = m_iscolliding;
+                }
+            }
+        }
+
+        public override bool CollidingGround
+        {
+            get { return false; }
+            set { return; }
+        }
+
+        public override bool CollidingObj
+        {
+            get { return false; }
+            set { return; }
+        }
+
+        public override bool ThrottleUpdates
+        {
+            get { return m_throttleUpdates; }
+            set { m_throttleUpdates = value; }
+        }
+
+        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())
+                {
+                    AddChange(changes.Size, value);
+                }
+                else
+                {
+                    m_log.WarnFormat("[PHYSICS]: Got NaN Size on object {0}", Name);
+                }
+            }
+        }
+
+        public override float Mass
+        {
+            get { return _mass; }
+        }
+
+        public override Vector3 Force
+        {
+            //get { return Vector3.Zero; }
+            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)
+        {
+            AddChange(changes.VolumeDtc, (param != 0));
+        }
+
+        public override Vector3 GeometricCenter
+        {
+            get
+            {
+                return Vector3.Zero;
+            }
+        }
+
+        public override Vector3 CenterOfMass
+        {
+            get
+            {
+                d.Vector3 dtmp;
+                if (IsPhysical && !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 vtmp = primOOBoffset * q;
+                    dtmp = d.GeomGetPosition(prim_geom);
+                    return new Vector3(dtmp.X + vtmp.X, dtmp.Y + vtmp.Y, dtmp.Z + vtmp.Z);
+                }
+                else
+                    return Vector3.Zero;
+            }
+        }
+        /*
+                public override Vector3 PrimOOBsize
+                    {
+                    get
+                        {
+                        return primOOBsize;
+                        }
+                    }
+
+                public override Vector3 PrimOOBoffset
+                    {
+                    get
+                        {
+                        return primOOBoffset;
+                        }
+                    }
+
+                public override float PrimOOBRadiusSQ
+                    {
+                    get
+                        {
+                        return primOOBradiusSQ;
+                        }
+                    }
+        */
+        public override PrimitiveBaseShape Shape
+        {
+            set
+            {
+                AddChange(changes.Shape, value);
+            }
+        }
+
+        public override Vector3 Velocity
+        {
+            get
+            {
+                // Averate previous velocity with the new one so
+                // client object interpolation works a 'little' better
+                if (_zeroFlag)
+                    return Vector3.Zero;
+                return _velocity;
+            }
+            set
+            {
+                if (value.IsFinite())
+                {
+                    AddChange(changes.Velocity, value);
+                    //                    _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 _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++;
+                    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 _acceleration; }
+            set { }
+        }
+
+        public override Vector3 RotationalVelocity
+        {
+            get
+            {
+                Vector3 pv = Vector3.Zero;
+                if (_zeroFlag)
+                    return pv;
+                m_lastUpdateSent = false;
+
+                if (m_rotationalVelocity.ApproxEquals(pv, 0.0001f))
+                    return pv;
+
+                return m_rotationalVelocity;
+            }
+            set
+            {
+                if (value.IsFinite())
+                {
+                    m_rotationalVelocity = value;
+                    if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
+                        d.BodyEnable(Body);
+                }
+                else
+                {
+                    m_log.WarnFormat("[PHYSICS]: Got NaN RotationalVelocity in Object {0}", Name);
+                }
+            }
+        }
+
+
+        public override float Buoyancy
+        {
+            get { return m_buoyancy; }
+            set
+            {
+                m_buoyancy = value;
+            }
+        }
+
+        public override bool FloatOnWater
+        {
+            set
+            {
+                AddChange(changes.CollidesWater, value);
+            }
+        }
+
+        public override Vector3 PIDTarget
+        {
+            set
+            {
+                if (value.IsFinite())
+                {
+                    m_PIDTarget = value;
+                }
+                else
+                    m_log.WarnFormat("[PHYSICS]: Got NaN PIDTarget from Scene on Object {0}", Name);
+            }
+        }
+
+        public override bool PIDActive { set { m_usePID = value; } }
+        public override float PIDTau { set { m_PIDTau = value; } }
+
+        public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } }
+        public override bool PIDHoverActive { set { m_useHoverPID = value; } }
+        public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } }
+        public override float PIDHoverTau { set { m_PIDHoverTau = value; } }
+
+        public override Quaternion APIDTarget { set { return; } }
+
+        public override bool APIDActive { set { return; } }
+
+        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.VehicleVectorParam, fp);
+        }
+
+        public override void VehicleFlags(int param, bool value)
+        {
+            if (m_vehicle == null)
+                return;
+            strVehicleBoolParam bp = new strVehicleBoolParam();
+            bp.param = param;
+            bp.value = value;
+            AddChange(changes.VehicleFlags, bp);
+        }
+
+        public void SetAcceleration(Vector3 accel)
+        {
+            _acceleration = accel;
+        }
+
+        public override void AddForce(Vector3 force, bool pushforce)
+        {
+            if (force.IsFinite())
+            {
+                AddChange(changes.AddForce, force / _parent_scene.ODE_STEPSIZE);
+            }
+            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())
+            {
+                AddChange(changes.AddAngForce, force / _parent_scene.ODE_STEPSIZE);
+            }
+            else
+            {
+                m_log.WarnFormat("[PHYSICS]: Got Invalid Angular force vector from Scene in Object {0}", Name);
+            }
+        }
+
+        public override void CrossingFailure()
+        {
+            m_crossingfailures++;
+            changeDisable(false);
+        }
+
+        public override void SetMomentum(Vector3 momentum)
+        {
+        }
+
+        public override void SetMaterial(int pMaterial)
+        {
+            m_material = pMaterial;
+            primContactData.mu = _parent_scene.m_materialContactsData[pMaterial].mu;
+            primContactData.bounce = _parent_scene.m_materialContactsData[pMaterial].bounce;
+        }
+
+        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(Vector3 axis)
+        {
+            // reverse the zero/non zero values for ODE.
+            if (axis.IsFinite())
+            {
+                axis.X = (axis.X > 0) ? 1f : 0f;
+                axis.Y = (axis.Y > 0) ? 1f : 0f;
+                axis.Z = (axis.Z > 0) ? 1f : 0f;
+                m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z);
+                AddChange(changes.AngLock, axis);
+            }
+            else
+            {
+                m_log.WarnFormat("[PHYSICS]: Got NaN locking axis from Scene on Object {0}", Name);
+            }
+        }
+
+        public override void SubscribeEvents(int ms)
+        {
+            m_eventsubscription = ms;
+            _parent_scene.AddCollisionEventReporting(this);
+        }
+
+        public override void UnSubscribeEvents()
+        {
+            _parent_scene.RemoveCollisionEventReporting(this);
+            m_eventsubscription = 0;
+        }
+
+        public void AddCollisionEvent(uint CollidedWith, ContactPoint contact)
+        {
+            if (CollisionEventsThisFrame == null)
+                CollisionEventsThisFrame = new CollisionEventUpdate();
+
+            CollisionEventsThisFrame.AddCollider(CollidedWith, contact);
+        }
+
+        public void SendCollisions()
+        {
+            if (CollisionEventsThisFrame == null)
+                return;
+
+            base.SendCollisionUpdate(CollisionEventsThisFrame);
+
+            if (CollisionEventsThisFrame.m_objCollisionList.Count == 0)
+                CollisionEventsThisFrame = null;
+            else
+                CollisionEventsThisFrame = new CollisionEventUpdate();
+        }
+
+        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)
+        {
+            Name = primName;
+
+            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;
+
+            PID_D = parent_scene.bodyPIDD;
+            PID_G = parent_scene.bodyPIDG;
+            m_density = parent_scene.geomDefaultDensity;
+            // m_tensor = parent_scene.bodyMotorJointMaxforceTensor;
+            body_autodisable_frames = parent_scene.bodyFramesAutoDisable;
+
+            prim_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;
+
+            _parent_scene = parent_scene;
+            m_targetSpace = IntPtr.Zero;
+
+            if (pos.Z < 0)
+            {
+                m_isphysical = false;
+            }
+            else
+            {
+                m_isphysical = pisPhysical;
+            }
+            m_fakeisphysical = m_isphysical;
+
+            m_isVolumeDetect = false;
+
+            m_force = Vector3.Zero;
+
+            m_iscolliding = false;
+            m_wascolliding = false;
+            m_colliderfilter = 0;
+
+            hasOOBoffsetFromMesh = false;
+            _triMeshData = IntPtr.Zero;
+
+
+            primContactData.mu = parent_scene.m_materialContactsData[(int)Material.Wood].mu;
+            primContactData.bounce = parent_scene.m_materialContactsData[(int)Material.Wood].bounce;
+
+            CalcPrimBodyData();
+
+            m_building = true; // control must set this to false when done
+
+            AddChange(changes.Add, null);
+        }
+
+        private void resetCollisionAccounting()
+        {
+            m_collisionscore = 0;
+            m_interpenetrationcount = 0;
+            m_disabled = false;
+        }
+
+        private void createAMotor(Vector3 axis)
+        {
+            if (Body == IntPtr.Zero)
+                return;
+
+            if (Amotor != IntPtr.Zero)
+            {
+                d.JointDestroy(Amotor);
+                Amotor = IntPtr.Zero;
+            }
+
+            int axisnum = 3 - (int)(axis.X + axis.Y + axis.Z);
+
+            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;
+
+            const int StopERP = 7;
+            const int StopCFM = 8;
+
+            int i = 0;
+            int j = 0;
+            if (axis.X == 0)
+            {
+                ax = (new Vector3(1, 0, 0)) * curr; // rotate world X to current local X
+                // ODE should do this  with axis relative to body 1 but seems to fail
+                d.JointSetAMotorAxis(Amotor, 0, 0, ax.X, ax.Y, ax.Z);
+                d.JointSetAMotorAngle(Amotor, 0, 0);
+                d.JointSetAMotorParam(Amotor, (int)d.JointParam.LoStop, -0.000001f);
+                d.JointSetAMotorParam(Amotor, (int)d.JointParam.HiStop, 0.000001f);
+                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.FMax, 5e8f);
+                d.JointSetAMotorParam(Amotor, (int)StopCFM, 0f);
+                d.JointSetAMotorParam(Amotor, (int)StopERP, 0.8f);
+                i++;
+                j = 256; // odeplugin.cs doesn't have all parameters so this moves to next axis set
+            }
+
+            if (axis.Y == 0)
+            {
+                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, -0.000001f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0.000001f);
+                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.FMax, 5e8f);
+                d.JointSetAMotorParam(Amotor, j + (int)StopCFM, 0f);
+                d.JointSetAMotorParam(Amotor, j + (int)StopERP, 0.8f);
+                i++;
+                j += 256;
+            }
+
+            if (axis.Z == 0)
+            {
+                ax = (new Vector3(0, 0, 1)) * curr;
+                d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z);
+                d.JointSetAMotorAngle(Amotor, i, 0);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, -0.000001f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0.000001f);
+                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.FMax, 5e8f);
+                d.JointSetAMotorParam(Amotor, j + (int)StopCFM, 0f);
+                d.JointSetAMotorParam(Amotor, j + (int)StopERP, 0.8f);
+            }
+        }
+
+        private bool setMesh(OdeScene parent_scene)
+        {
+            if (Body != IntPtr.Zero)
+            {
+                if (childPrim)
+                {
+                    if (_parent != null)
+                    {
+                        OdePrim parent = (OdePrim)_parent;
+                        parent.ChildDelink(this, false);
+                    }
+                }
+                else
+                {
+                    DestroyBody();
+                }
+            }
+
+            IMesh mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, (int)LevelOfDetail.High, true);
+            if (mesh == null)
+            {
+                m_log.WarnFormat("[PHYSICS]: CreateMesh Failed on prim {0} at <{1},{2},{3}>.", Name, _position.X, _position.Y, _position.Z);
+                return false;
+            }
+
+            IntPtr vertices, indices;
+            int vertexCount, indexCount;
+            int vertexStride, triStride;
+
+            mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); // Note, that vertices are fixed in unmanaged heap
+            mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); // Also fixed, needs release after usage
+
+            if (vertexCount == 0 || indexCount == 0)
+            {
+                m_log.WarnFormat("[PHYSICS]: Got invalid mesh on prim {0} at <{1},{2},{3}>. It can be a sculp with alpha channel in map. Replacing it by a small box.", Name, _position.X, _position.Y, _position.Z);
+                _size.X = 0.01f;
+                _size.Y = 0.01f;
+                _size.Z = 0.01f;
+                return false;
+            }
+
+            primOOBoffset = mesh.GetCentroid();
+            hasOOBoffsetFromMesh = true;
+
+            _triMeshData = d.GeomTriMeshDataCreate();
+
+            d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride);
+            d.GeomTriMeshDataPreprocess(_triMeshData);
+
+            mesh.releaseSourceMeshData();
+
+            _parent_scene.waitForSpaceUnlock(m_targetSpace);
+            try
+            {
+                SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, null, null, null));
+            }
+
+            catch (Exception e)
+            {
+                m_log.ErrorFormat("[PHYSICS]: SetGeom Mesh failed for {0} exception: {1}", Name, e);
+                return false;
+            }
+            return true;
+        }
+
+        private void SetGeom(IntPtr geom)
+        {
+            prim_geom = geom;
+            //Console.WriteLine("SetGeom to " + prim_geom + " for " + Name);
+            if (prim_geom != IntPtr.Zero)
+            {
+                d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
+                d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
+
+                CalcPrimBodyData();
+
+                _parent_scene.geom_name_map[prim_geom] = Name;
+                _parent_scene.actor_name_map[prim_geom] = this;
+
+                /*
+                                if (childPrim)
+                                {
+                                    if (_parent != null && _parent is OdePrim)
+                                    {
+                                        OdePrim parent = (OdePrim)_parent;
+                                        //Console.WriteLine("SetGeom calls ChildSetGeom");
+                                        parent.ChildSetGeom(this);
+                                    }
+                                }
+                 */
+            }
+            else
+                m_log.Warn("Setting bad Geom");
+        }
+
+
+        /// <summary>
+        /// Create a geometry for the given mesh in the given target space.
+        /// </summary>
+        /// <param name="m_targetSpace"></param>
+        /// <param name="mesh">If null, then a mesh is used that is based on the profile shape data.</param>
+        private void CreateGeom()
+        {
+            if (_triMeshData != IntPtr.Zero)
+            {
+                d.GeomTriMeshDataDestroy(_triMeshData);
+                _triMeshData = IntPtr.Zero;
+            }
+
+            bool haveMesh = false;
+            hasOOBoffsetFromMesh = false;
+
+            if (_parent_scene.needsMeshing(_pbs))
+            {
+                haveMesh = setMesh(_parent_scene); // this will give a mesh to non trivial known prims
+            }
+
+            if (!haveMesh)
+            {
+                if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1
+                    && _size.X == _size.Y && _size.Y == _size.Z)
+                { // it's a sphere
+                    _parent_scene.waitForSpaceUnlock(m_targetSpace);
+                    try
+                    {
+                        SetGeom(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
+                    _parent_scene.waitForSpaceUnlock(m_targetSpace);
+                    try
+                    {
+                        //Console.WriteLine("  CreateGeom 4");
+                        SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
+                    }
+                    catch (Exception e)
+                    {
+                        m_log.Warn("[PHYSICS]: Create box failed: {0}", e);
+                        return;
+                    }
+                }
+            }
+        }
+
+        /// <summary>
+        /// Set a new geometry for this prim.
+        /// </summary>
+        /// <param name="geom"></param>
+        private void RemoveGeom()
+        {
+            if (prim_geom != IntPtr.Zero)
+            {
+                _parent_scene.geom_name_map.Remove(prim_geom);
+                _parent_scene.actor_name_map.Remove(prim_geom);
+                try
+                {
+                    d.GeomDestroy(prim_geom);
+                    if (_triMeshData != IntPtr.Zero)
+                    {
+                        d.GeomTriMeshDataDestroy(_triMeshData);
+                        _triMeshData = IntPtr.Zero;
+                    }
+                }
+                //                catch (System.AccessViolationException)
+                catch (Exception e)
+                {
+                    m_log.ErrorFormat("[PHYSICS]: PrimGeom destruction failed for {0} exception {1}", Name, e);
+                }
+
+                prim_geom = IntPtr.Zero;
+            }
+            else
+            {
+                m_log.ErrorFormat("[PHYSICS]: PrimGeom destruction BAD {0}", Name);
+            }
+            Body = IntPtr.Zero;
+            hasOOBoffsetFromMesh = false;
+            CalcPrimBodyData();
+        }
+
+        private void ChildSetGeom(OdePrim odePrim)
+        {
+            // well.. 
+            DestroyBody();
+            MakeBody();
+        }
+
+        //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;
+            d.GeomEnable(prim_geom);
+        }
+
+        public void enableBodySoft()
+        {
+            if (!childPrim)
+            {
+                if (m_isphysical && Body != IntPtr.Zero && prim_geom != IntPtr.Zero)
+                {
+                    if (m_targetSpace != _parent_scene.ActiveSpace)
+                    {
+                        m_targetSpace = _parent_scene.ActiveSpace;
+
+                        foreach (OdePrim prm in childrenPrim)
+                        {
+                            if (prm.prim_geom != IntPtr.Zero)
+                            {
+                                d.SpaceAdd(m_targetSpace, prm.prim_geom);
+                                prm.m_targetSpace = m_targetSpace;
+                            }
+                        }
+                        d.SpaceAdd(m_targetSpace, prim_geom);
+                    }
+                    d.GeomEnable(prim_geom);
+                    foreach (OdePrim prm in childrenPrim)
+                        d.GeomEnable(prm.prim_geom);
+
+                    d.BodyEnable(Body);
+                }
+            }
+            resetCollisionAccounting(); // this sets m_disable to false
+        }
+
+        private void disableBodySoft()
+        {
+            m_disabled = true;
+            if (!childPrim)
+            {
+                if (m_isphysical && Body != IntPtr.Zero && prim_geom != IntPtr.Zero)
+                {
+                    if (m_targetSpace == _parent_scene.ActiveSpace)
+                    {
+                        foreach (OdePrim prm in childrenPrim)
+                        {
+                            if (prm.m_targetSpace != IntPtr.Zero && prm.prim_geom != IntPtr.Zero)
+                            {
+                                d.SpaceRemove(prm.m_targetSpace, prm.prim_geom);
+                                prm.m_targetSpace = IntPtr.Zero;
+                            }
+                        }
+                        d.SpaceRemove(m_targetSpace, prim_geom);
+                        m_targetSpace = IntPtr.Zero;
+                    }
+                    d.GeomDisable(prim_geom);
+                    foreach (OdePrim prm in childrenPrim)
+                        d.GeomDisable(prm.prim_geom);
+                    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)
+            {
+                d.BodyDestroy(Body);
+                Body = IntPtr.Zero;
+                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");
+            }
+
+            d.Matrix3 mymat = new d.Matrix3();
+            d.Quaternion myrot = new d.Quaternion();
+            d.Mass objdmass = new d.Mass { };
+
+            Body = d.BodyCreate(_parent_scene.world);
+
+            DMassDup(ref primdMass, out objdmass);
+
+            // rotate inertia
+            myrot.X = _orientation.X;
+            myrot.Y = _orientation.Y;
+            myrot.Z = _orientation.Z;
+            myrot.W = _orientation.W;
+
+            d.RfromQ(out mymat, ref myrot);
+            d.MassRotate(ref objdmass, ref mymat);
+
+            // set the body rotation and position
+            d.BodySetRotation(Body, 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 + objdmass.c.X;
+                rcm.Y = _position.Y + objdmass.c.Y;
+                rcm.Z = _position.Z + objdmass.c.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;
+                        }
+
+
+
+                        DMassCopy(ref prm.primdMass, ref tmpdmass);
+
+                        // apply prim current rotation to inertia
+                        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);
+                        d.MassRotate(ref tmpdmass, ref mat);
+
+                        Vector3 ppos = prm._position;
+                        ppos.X += tmpdmass.c.X - rcm.X;
+                        ppos.Y += tmpdmass.c.Y - rcm.Y;
+                        ppos.Z += tmpdmass.c.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
+                        // 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
+                    }
+                }
+            }
+
+            d.GeomClearOffset(prim_geom); // make sure we don't have a hidden offset
+            // associate root geom with body
+            d.GeomSetBody(prim_geom, Body);
+
+            d.BodySetPosition(Body, _position.X + objdmass.c.X, _position.Y + objdmass.c.Y, _position.Z + objdmass.c.Z);
+            d.GeomSetOffsetWorldPosition(prim_geom, _position.X, _position.Y, _position.Z);
+
+            d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body
+            myrot.W = -myrot.W;
+            d.RfromQ(out mymat, ref myrot);
+            d.MassRotate(ref objdmass, ref mymat);
+            d.BodySetMass(Body, ref objdmass);
+            _mass = objdmass.mass;
+
+            m_collisionCategories |= CollisionCategories.Body;
+            m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
+
+            // disconnect from world gravity so we can apply buoyancy
+            d.BodySetGravityMode(Body, false);
+
+            d.BodySetAutoDisableFlag(Body, true);
+            d.BodySetAutoDisableSteps(Body, body_autodisable_frames);
+            //            d.BodySetLinearDampingThreshold(Body, 0.01f);
+            //            d.BodySetAngularDampingThreshold(Body, 0.001f);
+            d.BodySetDamping(Body, .001f, .0002f);
+
+            d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
+            d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
+
+            m_interpenetrationcount = 0;
+            m_collisionscore = 0;
+
+            m_disabled = false;
+
+            if (m_targetSpace != _parent_scene.ActiveSpace)
+            {
+                if (m_targetSpace != IntPtr.Zero)
+                {
+                    _parent_scene.waitForSpaceUnlock(m_targetSpace);
+                    if (d.SpaceQuery(m_targetSpace, prim_geom))
+                        d.SpaceRemove(m_targetSpace, prim_geom);
+                }
+
+                m_targetSpace = _parent_scene.ActiveSpace;
+                d.SpaceAdd(m_targetSpace, prim_geom);
+            }
+
+            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
+
+                    prm.m_collisionCategories |= CollisionCategories.Body;
+                    prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
+                    d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories);
+                    d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags);
+
+                    if (prm.m_targetSpace != _parent_scene.ActiveSpace)
+                    {
+                        if (prm.m_targetSpace != IntPtr.Zero)
+                        {
+                            _parent_scene.waitForSpaceUnlock(m_targetSpace);
+                            if (d.SpaceQuery(prm.m_targetSpace, prm.prim_geom))
+                                d.SpaceRemove(prm.m_targetSpace, prm.prim_geom);
+                        }
+                        prm.m_targetSpace = _parent_scene.ActiveSpace;
+                        d.SpaceAdd(m_targetSpace, prm.prim_geom);
+                    }
+                    d.GeomEnable(prm.prim_geom);
+                    prm.m_disabled = false;
+                    prm.m_interpenetrationcount = 0;
+                    prm.m_collisionscore = 0;
+                    _parent_scene.addActivePrim(prm);
+                }
+            }
+
+            // The body doesn't already have a finite rotation mode set here
+            if ((!m_angularlock.ApproxEquals(Vector3.One, 0.0f)) && _parent == null)
+            {
+                createAMotor(m_angularlock);
+            }
+
+            d.GeomEnable(prim_geom);
+            m_disabled = false;
+            _parent_scene.addActivePrim(this);
+        }
+
+        private void DestroyBody()
+        {
+            if (Body != IntPtr.Zero)
+            {
+                _parent_scene.remActivePrim(this);
+                m_collisionCategories &= ~CollisionCategories.Body;
+                m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land);
+                if (prim_geom != IntPtr.Zero)
+                {
+                    d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
+                    d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
+                    UpdateDataFromGeom();
+                    d.GeomSetBody(prim_geom, IntPtr.Zero);
+                    SetInStaticSpace(this);
+                }
+
+                if (!childPrim)
+                {
+                    lock (childrenPrim)
+                    {
+                        foreach (OdePrim prm in childrenPrim)
+                        {
+                            _parent_scene.remActivePrim(prm);
+                            prm.m_collisionCategories &= ~CollisionCategories.Body;
+                            prm.m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land);
+                            if (prm.prim_geom != IntPtr.Zero)
+                            {
+                                d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories);
+                                d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags);
+                                prm.UpdateDataFromGeom();
+                                SetInStaticSpace(prm);
+                            }
+                            prm.Body = IntPtr.Zero;
+                            prm._mass = prm.primMass;
+                            prm.m_collisionscore = 0;
+                        }
+                    }
+                    d.BodyDestroy(Body);
+                }
+                Body = IntPtr.Zero;
+            }
+            _mass = primMass;
+            m_disabled = true;
+            m_collisionscore = 0;
+        }
+
+        #region Mass Calculation
+
+        private float CalculatePrimVolume()
+        {
+            float volume = _size.X * _size.Y * _size.Z; // default
+            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.52359877559829887307710723054658f;
+                    }
+                    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);
+
+            return volume;
+        }
+
+
+        private void CalcPrimBodyData()
+        {
+            float volume;
+
+            if (prim_geom == IntPtr.Zero)
+            {
+                // Ubit let's have a initial basic OOB
+                primOOBsize.X = _size.X;
+                primOOBsize.Y = _size.Y;
+                primOOBsize.Z = _size.Z;
+                primOOBoffset = Vector3.Zero;
+            }
+            else
+            {
+                d.AABB AABB;
+                d.GeomGetAABB(prim_geom, out AABB); // get the AABB from engine geom
+
+                primOOBsize.X = (AABB.MaxX - AABB.MinX);
+                primOOBsize.Y = (AABB.MaxY - AABB.MinY);
+                primOOBsize.Z = (AABB.MaxZ - AABB.MinZ);
+                if (!hasOOBoffsetFromMesh)
+                {
+                    primOOBoffset.X = (AABB.MaxX + AABB.MinX) * 0.5f;
+                    primOOBoffset.Y = (AABB.MaxY + AABB.MinY) * 0.5f;
+                    primOOBoffset.Z = (AABB.MaxZ + AABB.MinZ) * 0.5f;
+                }
+            }
+
+            // also its own inertia and mass
+            // keep using basic shape mass for now
+            volume = CalculatePrimVolume();
+
+            primMass = m_density * volume;
+
+            if (primMass <= 0)
+                primMass = 0.0001f;//ckrinke: Mass must be greater then zero.
+            if (primMass > _parent_scene.maximumMassObject)
+                primMass = _parent_scene.maximumMassObject;
+
+            _mass = primMass; // just in case
+
+            d.MassSetBoxTotal(out primdMass, primMass, primOOBsize.X, primOOBsize.Y, primOOBsize.Z);
+
+            d.MassTranslate(ref primdMass,
+                                primOOBoffset.X,
+                                primOOBoffset.Y,
+                                primOOBoffset.Z);
+
+            primOOBsize *= 0.5f; // let obb size be a corner coords
+            primOOBradiusSQ = primOOBsize.LengthSquared();
+        }
+
+
+        #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.Vector3 lpos;
+                d.GeomCopyPosition(prim_geom, out lpos);
+                _position.X = lpos.X;
+                _position.Y = lpos.Y;
+                _position.Z = lpos.Z;
+                d.Quaternion qtmp = new d.Quaternion { };
+                d.GeomCopyQuaternion(prim_geom, out qtmp);
+                _orientation.W = qtmp.W;
+                _orientation.X = qtmp.X;
+                _orientation.Y = qtmp.Y;
+                _orientation.Z = qtmp.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()
+        {
+            CreateGeom();
+
+            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);
+                // _parent_scene.actor_name_map[prim_geom] = (PhysicsActor)this;
+                if (!m_isphysical)
+                    SetInStaticSpace(this);
+            }
+
+            if (m_isphysical && Body == IntPtr.Zero)
+            {
+                /*
+                                if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim)
+                                {
+                                    changeShape(_pbs);
+                                }
+                                else
+                                {
+                 */
+                MakeBody();
+                //                }
+            }
+        }
+
+        private void changeAngularLock(Vector3 newLock)
+        {
+            // 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 (!newLock.ApproxEquals(Vector3.One, 0f))
+                    {
+                        createAMotor(newLock);
+                    }
+                    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_angularlock = newLock;
+        }
+
+        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;
+                _torque = Vector3.Zero;
+                _velocity = Vector3.Zero;
+                _acceleration = Vector3.Zero;
+                m_rotationalVelocity = Vector3.Zero;
+                _target_velocity = Vector3.Zero;
+                if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE)
+                    m_vehicle.Stop();
+            }
+
+            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 changeSelectedStatus(bool newval)
+        {
+            m_isSelected = newval;
+            Stop();
+
+            if (newval)
+            {
+                m_collisionCategories = CollisionCategories.Selected;
+                m_collisionFlags = (CollisionCategories.Sensor | CollisionCategories.Space);
+
+                if (prim_geom != IntPtr.Zero)
+                {
+                    d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
+                    d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
+                }
+
+                disableBodySoft();
+            }
+            else
+            {
+                m_collisionCategories = CollisionCategories.Geom;
+
+                if (m_isphysical)
+                    m_collisionCategories |= CollisionCategories.Body;
+
+                m_collisionFlags = m_default_collisionFlags;
+
+                if (m_collidesLand)
+                    m_collisionFlags |= CollisionCategories.Land;
+                if (m_collidesWater)
+                    m_collisionFlags |= CollisionCategories.Water;
+
+                if (prim_geom != IntPtr.Zero)
+                {
+                    d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
+                    d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
+                }
+
+                enableBodySoft();
+            }
+
+            resetCollisionAccounting();
+        }
+
+        private void changePosition(Vector3 newPos)
+        {
+            if (m_isphysical)
+            {
+                if (childPrim)  // inertia is messed, must rebuild
+                {
+                    if (m_building)
+                    {
+                        _position = newPos;
+                    }
+                }
+                else
+                {
+                    if (_position != newPos)
+                    {
+                        d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z);
+                        _position = newPos;
+                    }
+                    if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
+                        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)
+        {
+            if (m_isphysical)
+            {
+                if (childPrim)  // inertia is messed, must rebuild
+                {
+                    if (m_building)
+                    {
+                        _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_angularlock.ApproxEquals(Vector3.One, 0f))
+                            createAMotor(m_angularlock);
+                    }
+                    if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
+                        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)
+        {
+            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_angularlock.ApproxEquals(Vector3.One, 0f))
+                            createAMotor(m_angularlock);
+                    }
+                    if (_position != newPos)
+                    {
+                        d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z);
+                        _position = newPos;
+                    }
+                    if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
+                        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)
+        {
+            m_isphysical = NewStatus;
+
+            if (!childPrim)
+            {
+                if (NewStatus)
+                {
+                    if (Body == IntPtr.Zero)
+                    {
+                        /*
+                                                if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim)
+                                                {
+                                                    changeShape(_pbs);
+                                                }
+                                                else
+                         */
+                        {
+                            MakeBody();
+                        }
+                    }
+                }
+                else
+                {
+                    if (Body != IntPtr.Zero)
+                    {
+                        //                        UpdateChildsfromgeom();
+                        /*                        if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim)
+                                                {
+                                                    changeShape(_pbs);
+                                                }
+                                                else
+                         */                       
+                        DestroyBody();
+                        Stop();
+                    }
+                }
+            }
+
+            resetCollisionAccounting();
+        }
+
+        private void changeprimsizeshape()
+        {
+            OdePrim parent = (OdePrim)_parent;
+
+            bool chp = childPrim;
+
+            if (chp)
+            {
+                if (parent != null)
+                {
+                    parent.DestroyBody();
+                }
+            }
+            else
+            {
+                DestroyBody();
+            }
+
+            RemoveGeom();
+
+            // we don't need to do space calculation because the client sends a position update also.
+            if (_size.X <= 0)
+                _size.X = 0.01f;
+            if (_size.Y <= 0)
+                _size.Y = 0.01f;
+            if (_size.Z <= 0)
+                _size.Z = 0.01f;
+            // Construction of new prim
+
+            CreateGeom();
+
+            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 (chp)
+            {
+                if (parent != null)
+                {
+                    parent.MakeBody();
+                }
+            }
+            else
+                MakeBody();
+
+            resetCollisionAccounting();
+        }
+
+        private void changeSize(Vector3 newSize)
+        {
+            _size = newSize;
+            changeprimsizeshape();
+        }
+
+        private void changeShape(PrimitiveBaseShape newShape)
+        {
+            _pbs = newShape;
+            changeprimsizeshape();
+        }
+
+        private void changeFloatOnWater(bool newval)
+        {
+            m_collidesWater = newval;
+
+            if (prim_geom != IntPtr.Zero)
+            {
+                if (m_collidesWater)
+                {
+                    m_collisionFlags |= CollisionCategories.Water;
+                }
+                else
+                {
+                    m_collisionFlags &= ~CollisionCategories.Water;
+                }
+                d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
+            }
+        }
+
+        private void changeSetTorque(Vector3 newtorque)
+        {
+            if (!m_isSelected)
+            {
+                if (m_isphysical && Body != IntPtr.Zero)
+                {
+                    if (m_disabled)
+                        enableBodySoft();
+                    else if (!d.BodyIsEnabled(Body))
+                        d.BodyEnable(Body);
+
+                }
+                _torque = newtorque;
+            }
+        }
+
+        private void changeForce(Vector3 force)
+        {
+            m_force = force;
+            if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
+                d.BodyEnable(Body);
+        }
+
+        private void changeAddForce(Vector3 force)
+        {
+            m_forceacc += force;
+            if (!m_isSelected)
+            {
+                lock (this)
+                {
+                    //m_log.Info("[PHYSICS]: dequeing forcelist");
+                    if (m_isphysical && Body != IntPtr.Zero)
+                    {
+                        if (m_disabled)
+                            enableBodySoft();
+                        else if (!d.BodyIsEnabled(Body))
+                            d.BodyEnable(Body);
+                    }
+                }
+
+                m_collisionscore = 0;
+                m_interpenetrationcount = 0;
+            }
+        }
+
+        private void changeAddAngularForce(Vector3 aforce)
+        {
+            m_angularForceacc += aforce;
+            if (!m_isSelected)
+            {
+                lock (this)
+                {
+                    if (m_isphysical && Body != IntPtr.Zero)
+                    {
+                        if (m_disabled)
+                            enableBodySoft();
+                        else if (!d.BodyIsEnabled(Body))
+                            d.BodyEnable(Body);
+                    }
+                }
+                m_collisionscore = 0;
+                m_interpenetrationcount = 0;
+            }
+        }
+
+        private void changevelocity(Vector3 newVel)
+        {
+            if (!m_isSelected)
+            {
+                if (Body != IntPtr.Zero)
+                {
+                    if (m_disabled)
+                        enableBodySoft();
+                    else if (!d.BodyIsEnabled(Body))
+                        d.BodyEnable(Body);
+
+                    d.BodySetLinearVel(Body, newVel.X, newVel.Y, newVel.Z);
+                }
+                //resetCollisionAccounting();           
+            }
+            _velocity = newVel;
+        }
+
+        private void changeVolumedetetion(bool newVolDtc)
+        {
+            m_isVolumeDetect = newVolDtc;
+        }
+
+        protected void changeBuilding(bool newbuilding)
+        {
+            if ((bool)newbuilding)
+            {
+                m_building = true;
+                DestroyBody();
+            }
+            else
+            {
+                m_building = false;
+                if (!childPrim)
+                    MakeBody();
+            }
+            if (!childPrim && childrenPrim.Count > 0)
+            {
+                foreach (OdePrim prm in childrenPrim)
+                    prm.changeBuilding(m_building); // call directly
+            }
+        }
+
+        private void changeVehicleType(int value)
+        {
+            if (m_vehicle == null)
+            {
+                if (value != (int)Vehicle.TYPE_NONE)
+                {
+                    m_vehicle = new ODEDynamics(this);
+                    m_vehicle.ProcessTypeChange((Vehicle)value);
+                }
+            }
+            else
+                m_vehicle.ProcessTypeChange((Vehicle)value);
+        }
+
+        private void changeVehicleFloatParam(strVehicleFloatParam fp)
+        {
+            if (m_vehicle == null)
+                return;
+
+            m_vehicle.ProcessFloatVehicleParam((Vehicle)fp.param, fp.value);
+            if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
+                d.BodyEnable(Body);
+        }
+
+        private void changeVehicleVectorParam(strVehicleVectorParam vp)
+        {
+            if (m_vehicle == null)
+                return;
+            m_vehicle.ProcessVectorVehicleParam((Vehicle)vp.param, vp.value);
+            if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
+                d.BodyEnable(Body);
+        }
+
+        private void changeVehicleRotationParam(strVehicleQuatParam qp)
+        {
+            if (m_vehicle == null)
+                return;
+            m_vehicle.ProcessRotationVehicleParam((Vehicle)qp.param, qp.value);
+            if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
+                d.BodyEnable(Body);
+        }
+
+        private void changeVehicleFlags(strVehicleBoolParam bp)
+        {
+            if (m_vehicle == null)
+                return;
+            m_vehicle.ProcessVehicleFlags(bp.param, bp.value);
+            if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
+                d.BodyEnable(Body);
+        }
+
+        #endregion
+
+        public void Move()
+        {
+            if (!childPrim && m_isphysical && Body != IntPtr.Zero &&
+                !m_disabled && !m_isSelected && d.BodyIsEnabled(Body) && !m_building)        // KF: Only move root prims.
+            {
+                //                if (!d.BodyIsEnabled(Body)) d.BodyEnable(Body); // KF add 161009
+
+                float timestep = _parent_scene.ODE_STEPSIZE;
+
+                float fx = 0;
+                float fy = 0;
+                float fz = 0;
+
+                if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE)
+                {
+                    // 'VEHICLES' are dealt with in ODEDynamics.cs
+                    m_vehicle.Step();
+                }
+                else
+                {
+                    float m_mass = _mass;
+
+                    //                    fz = 0f;
+                    //m_log.Info(m_collisionFlags.ToString());
+                    if (m_usePID)
+                    {
+
+                        // If the PID Controller isn't active then we set our force
+                        // calculating base velocity to the current position
+
+                        if ((m_PIDTau < 1) && (m_PIDTau != 0))
+                        {
+                            //PID_G = PID_G / m_PIDTau;
+                            m_PIDTau = 1;
+                        }
+
+                        if ((PID_G - m_PIDTau) <= 0)
+                        {
+                            PID_G = m_PIDTau + 1;
+                        }
+
+                        d.Vector3 vel = d.BodyGetLinearVel(Body);
+                        d.Vector3 pos = d.BodyGetPosition(Body);
+                        _target_velocity =
+                            new Vector3(
+                                (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep),
+                                (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep),
+                                (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep)
+                                );
+
+                        //  if velocity is zero, use position control; otherwise, velocity control
+
+                        if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f))
+                        {
+                            //  keep track of where we stopped.  No more slippin' & slidin'
+
+                            // We only want to deactivate the PID Controller if we think we want to have our surrogate
+                            // react to the physics scene by moving it's position.
+                            // Avatar to Avatar collisions
+                            // Prim to avatar collisions
+
+                            //fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2);
+                            //fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2);
+                            //fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P;
+                            d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);
+                            d.BodySetLinearVel(Body, 0, 0, 0);
+                            d.BodyAddForce(Body, 0, 0, fz);
+                            return;
+                        }
+                        else
+                        {
+                            _zeroFlag = false;
+
+                            // We're flying and colliding with something
+                            fx = ((_target_velocity.X) - vel.X) * (PID_D);
+                            fy = ((_target_velocity.Y) - vel.Y) * (PID_D);
+
+                            // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P;
+
+                            fz = ((_target_velocity.Z - vel.Z) * (PID_D));
+                        }
+                    }        // end if (m_usePID)
+
+                    // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller
+                    else if (m_useHoverPID)
+                    {
+                        //Console.WriteLine("Hover " +  Name);
+
+                        // If we're using the PID controller, then we have no gravity
+
+                        //  no lock; for now it's only called from within Simulate()
+
+                        // If the PID Controller isn't active then we set our force
+                        // calculating base velocity to the current position
+
+                        if ((m_PIDTau < 1))
+                        {
+                            PID_G = PID_G / m_PIDTau;
+                        }
+
+                        if ((PID_G - m_PIDTau) <= 0)
+                        {
+                            PID_G = m_PIDTau + 1;
+                        }
+
+                        // Where are we, and where are we headed?
+                        d.Vector3 pos = d.BodyGetPosition(Body);
+                        d.Vector3 vel = d.BodyGetLinearVel(Body);
+
+                        //    Non-Vehicles have a limited set of Hover options.
+                        // determine what our target height really is based on HoverType
+                        switch (m_PIDHoverType)
+                        {
+                            case PIDHoverType.Ground:
+                                m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
+                                m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
+                                break;
+                            case PIDHoverType.GroundAndWater:
+                                m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
+                                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)
+
+
+                        _target_velocity =
+                            new Vector3(0.0f, 0.0f,
+                                (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep)
+                                );
+
+                        //  if velocity is zero, use position control; otherwise, velocity control
+
+                        if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f))
+                        {
+                            //  keep track of where we stopped.  No more slippin' & slidin'
+
+                            // We only want to deactivate the PID Controller if we think we want to have our surrogate
+                            // react to the physics scene by moving it's position.
+                            // Avatar to Avatar collisions
+                            // Prim to avatar collisions
+
+                            d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight);
+                            d.BodySetLinearVel(Body, vel.X, vel.Y, 0);
+                            // ?                        d.BodyAddForce(Body, 0, 0, fz);
+                            return;
+                        }
+                        else
+                        {
+                            _zeroFlag = false;
+
+                            // We're flying and colliding with something
+                            fz = ((_target_velocity.Z - vel.Z) * (PID_D));
+                        }
+                    }
+                    else
+                    {
+                        float b = (1.0f - m_buoyancy);
+                        fx = _parent_scene.gravityx * b;
+                        fy = _parent_scene.gravityy * b;
+                        fz = _parent_scene.gravityz * b;
+                    }
+
+                    fx *= m_mass;
+                    fy *= m_mass;
+                    fz *= m_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 = _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(float simulatedtime)
+        {
+            //  no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit!
+            if (_parent == null && !m_disabled && !m_building)
+            {
+                if (Body != IntPtr.Zero)
+                {
+                    if (m_crossingfailures != 0 && m_crossingfailures < 5)
+                    {
+                        _position.X = Util.Clip(_position.X, 0.4f, _parent_scene.WorldExtents.X - 0.4f);
+                        _position.Y = Util.Clip(_position.Y, 0.4f, _parent_scene.WorldExtents.Y - 0.4f);
+                        _position.Z = Util.Clip(_position.Z + 0.2f, -100f, 50000f);
+
+                        float tmp = _parent_scene.GetTerrainHeightAtXY(_position.X, _position.Y);
+                        if (_position.Z < tmp)
+                            _position.Z = tmp + 0.2f;
+
+                        m_lastposition = _position;
+                        m_lastorientation = _orientation;
+                        _velocity.X = 0;
+                        _velocity.Y = 0;
+                        _velocity.Z = 0;
+
+                        m_lastVelocity = _velocity;
+                        m_rotationalVelocity = _velocity;
+                        if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE)
+                            m_vehicle.Stop();
+
+                        m_crossingfailures = 0; // do this only once
+                        d.BodySetLinearVel(Body, 0, 0, 0); // stop it
+                        d.BodySetAngularVel(Body, 0, 0, 0);
+                        d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
+                        enableBodySoft();
+                        base.RequestPhysicsterseUpdate();
+                        return;
+                    }
+
+                    else if (m_crossingfailures != 0)
+                    {
+                        return;
+                    }
+
+                    Vector3 pv = Vector3.Zero;
+                    bool lastZeroFlag = _zeroFlag;
+
+                    d.Vector3 lpos;
+                    d.GeomCopyPosition(prim_geom, out lpos); // root position that is seem by rest of simulator
+
+                    // we need to use root position since that's all the rest of scene uses
+                    if (lpos.X < 0f || lpos.X > _parent_scene.WorldExtents.X
+                        || lpos.Y < 0f || lpos.Y > _parent_scene.WorldExtents.Y
+                        )
+                    {
+                        // we are outside current region
+                        // we can't let it keeping moving and having colisions
+                        // since it can be stucked between something like terrain and edge
+                        // so lets stop and disable it until something else kicks it
+                        if (m_crossingfailures == 0)
+                        {
+
+                            _position.X = Util.Clip(lpos.X, -0.5f, _parent_scene.WorldExtents.X + 0.5f);
+                            _position.Y = Util.Clip(lpos.Y, -0.5f, _parent_scene.WorldExtents.Y + 0.5f);
+                            _position.Z = Util.Clip(lpos.Z, -100f, 50000f);
+
+                            m_lastposition = _position;
+                            m_lastorientation = _orientation;
+
+                            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
+                            m_crossingfailures++; // do this only once
+                            base.RequestPhysicsterseUpdate();
+                            return;
+                        }
+                    }
+
+                    if (lpos.Z < -100 || lpos.Z > 100000f)
+                    {
+                        lpos.Z = Util.Clip(lpos.Z, -100f, 50000f);
+
+                        _acceleration.X = 0;
+                        _acceleration.Y = 0;
+                        _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();
+
+                        m_throttleUpdates = false;
+                        throttleCounter = 0;
+                        _zeroFlag = true;
+
+                        disableBodySoft(); // disable it and colisions
+                        base.RaiseOutOfBounds(_position);
+
+                        return;
+                    }
+
+                    d.Quaternion ori;
+                    d.GeomCopyQuaternion(prim_geom, out ori);
+                    d.Vector3 vel = d.BodyGetLinearVel(Body);
+                    d.Vector3 rotvel = d.BodyGetAngularVel(Body);
+
+                    if ((Math.Abs(m_lastposition.X - lpos.X) < 0.01)
+                        && (Math.Abs(m_lastposition.Y - lpos.Y) < 0.01)
+                        && (Math.Abs(m_lastposition.Z - lpos.Z) < 0.01)
+                        && (Math.Abs(m_lastorientation.X - ori.X) < 0.0001)
+                        && (Math.Abs(m_lastorientation.Y - ori.Y) < 0.0001)
+                        && (Math.Abs(m_lastorientation.Z - ori.Z) < 0.0001)
+                        )
+                    {
+                        _zeroFlag = true;
+                        //Console.WriteLine("ZFT 2");
+                        m_throttleUpdates = false;
+                    }
+                    else
+                    {
+                        //m_log.Debug(Math.Abs(m_lastposition.X - l_position.X).ToString());
+                        _zeroFlag = false;
+                        m_lastUpdateSent = false;
+                        //m_throttleUpdates = false;
+                    }
+
+                    if (_zeroFlag)
+                    {
+                        m_lastposition = _position;
+                        m_lastorientation = _orientation;
+
+                        _velocity.X = 0.0f;
+                        _velocity.Y = 0.0f;
+                        _velocity.Z = 0.0f;
+
+                        _acceleration.X = 0;
+                        _acceleration.Y = 0;
+                        _acceleration.Z = 0;
+
+                        m_rotationalVelocity.X = 0;
+                        m_rotationalVelocity.Y = 0;
+                        m_rotationalVelocity.Z = 0;
+                        if (!m_lastUpdateSent)
+                        {
+                            m_throttleUpdates = false;
+                            throttleCounter = 0;
+                            m_rotationalVelocity = pv;
+
+                            base.RequestPhysicsterseUpdate();
+
+                            m_lastUpdateSent = true;
+                        }
+                    }
+                    else
+                    {
+                        if (lastZeroFlag != _zeroFlag)
+                        {
+                            base.RequestPhysicsterseUpdate();
+                        }
+
+                        m_lastVelocity = _velocity;
+
+                        _position.X = lpos.X;
+                        _position.Y = lpos.Y;
+                        _position.Z = lpos.Z;
+
+                        _velocity.X = vel.X;
+                        _velocity.Y = vel.Y;
+                        _velocity.Z = vel.Z;
+
+                        _orientation.X = ori.X;
+                        _orientation.Y = ori.Y;
+                        _orientation.Z = ori.Z;
+                        _orientation.W = ori.W;
+
+                        _acceleration = ((_velocity - m_lastVelocity) / simulatedtime);
+
+                        if (m_rotationalVelocity.ApproxEquals(pv, 0.0001f))
+                        {
+                            m_rotationalVelocity = pv;
+                        }
+                        else
+                        {
+                            m_rotationalVelocity.X = rotvel.X;
+                            m_rotationalVelocity.Y = rotvel.Y;
+                            m_rotationalVelocity.Z = rotvel.Z;
+                        }
+
+                        m_lastUpdateSent = false;
+                        if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate)
+                        {
+                            m_lastposition = _position;
+                            m_lastorientation = _orientation;
+                            base.RequestPhysicsterseUpdate();
+                        }
+                        else
+                        {
+                            throttleCounter++;
+                        }
+                    }
+                }
+                else if (!m_lastUpdateSent || !_zeroFlag)
+                {
+                    // Not a body..   so Make sure the client isn't interpolating
+                    _velocity.X = 0;
+                    _velocity.Y = 0;
+                    _velocity.Z = 0;
+
+                    _acceleration.X = 0;
+                    _acceleration.Y = 0;
+                    _acceleration.Z = 0;
+
+                    m_rotationalVelocity.X = 0;
+                    m_rotationalVelocity.Y = 0;
+                    m_rotationalVelocity.Z = 0;
+                    _zeroFlag = true;
+
+                    if (!m_lastUpdateSent)
+                    {
+                        m_throttleUpdates = false;
+                        throttleCounter = 0;
+
+                        base.RequestPhysicsterseUpdate();
+
+                        m_lastUpdateSent = true;
+                    }
+                }
+            }
+        }
+
+        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 DMassCopy(ref d.Mass src, ref d.Mass dst)
+        {
+            dst.c.W = src.c.W;
+            dst.c.X = src.c.X;
+            dst.c.Y = src.c.Y;
+            dst.c.Z = src.c.Z;
+            dst.mass = src.mass;
+            dst.I.M00 = src.I.M00;
+            dst.I.M01 = src.I.M01;
+            dst.I.M02 = src.I.M02;
+            dst.I.M10 = src.I.M10;
+            dst.I.M11 = src.I.M11;
+            dst.I.M12 = src.I.M12;
+            dst.I.M20 = src.I.M20;
+            dst.I.M21 = src.I.M21;
+            dst.I.M22 = src.I.M22;
+        }
+
+        private static void DMassDup(ref d.Mass src, out d.Mass dst)
+        {
+            dst = new d.Mass { };
+
+            dst.c.W = src.c.W;
+            dst.c.X = src.c.X;
+            dst.c.Y = src.c.Y;
+            dst.c.Z = src.c.Z;
+            dst.mass = src.mass;
+            dst.I.M00 = src.I.M00;
+            dst.I.M01 = src.I.M01;
+            dst.I.M02 = src.I.M02;
+            dst.I.M10 = src.I.M10;
+            dst.I.M11 = src.I.M11;
+            dst.I.M12 = src.I.M12;
+            dst.I.M20 = src.I.M20;
+            dst.I.M21 = src.I.M21;
+            dst.I.M22 = src.I.M22;
+        }
+        private void donullchange()
+        {
+        }
+
+        public bool DoAChange(changes what, object arg)
+        {
+            if (prim_geom == IntPtr.Zero && what != changes.Add && what != changes.Remove)
+            {
+                return false;
+            }
+
+            // nasty switch
+            switch (what)
+            {
+                case changes.Add:
+                    changeadd();
+                    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);
+
+                    RemoveGeom();
+                    m_targetSpace = IntPtr.Zero;
+                    if (m_eventsubscription > 0)
+                        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.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((Vector3)arg);
+                    break;
+
+                case changes.Size:
+                    changeSize((Vector3)arg);
+                    break;
+
+                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.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.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;
+        }
+    }
+}
\ No newline at end of file
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs
new file mode 100644
index 0000000..4b3f83b
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs
@@ -0,0 +1,443 @@
+/*
+ * 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 OpenMetaverse;
+using OpenSim.Region.Physics.Manager;
+using OdeAPI;
+using log4net;
+
+namespace OpenSim.Region.Physics.OdePlugin
+{
+    /// <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;
+
+        private const int ColisionContactGeomsPerTest = 5;
+
+        /// <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>();
+
+        public ODERayCastRequestManager(OdeScene pScene)
+        {
+            m_scene = pScene;
+            nearCallback = near;
+            ray = d.CreateRay(IntPtr.Zero, 1.0f);
+        }
+
+        /// <summary>
+        /// Queues a raycast
+        /// </summary>
+        /// <param name="position">Origin of Ray</param>
+        /// <param name="direction">Ray normal</param>
+        /// <param name="length">Ray length</param>
+        /// <param name="retMethod">Return method to send the results</param>
+        public void QueueRequest(Vector3 position, Vector3 direction, float length, RayCallback retMethod)
+        {
+            ODERayRequest req = new ODERayRequest();
+            req.geom = IntPtr.Zero;
+            req.callbackMethod = retMethod;
+            req.Count = 0;
+            req.length = length;
+            req.Normal = direction;
+            req.Origin = position;
+
+            m_PendingRequests.Enqueue(req);
+        }
+
+        public void QueueRequest(IntPtr geom, Vector3 position, Vector3 direction, float length, RayCallback retMethod)
+        {
+            ODERayRequest req = new ODERayRequest();
+            req.geom = geom;
+            req.callbackMethod = retMethod;
+            req.length = length;
+            req.Normal = direction;
+            req.Origin = position;
+            req.Count = 0;
+
+            m_PendingRequests.Enqueue(req);
+        }
+
+        public void QueueRequest(Vector3 position, Vector3 direction, float length, RaycastCallback retMethod)
+        {
+            ODERayRequest req = new ODERayRequest();
+            req.geom = IntPtr.Zero;
+            req.callbackMethod = retMethod;
+            req.Count = 0;
+            req.length = length;
+            req.Normal = direction;
+            req.Origin = position;
+
+            m_PendingRequests.Enqueue(req);
+        }
+
+        public void QueueRequest(IntPtr geom, Vector3 position, Vector3 direction, float length, RaycastCallback retMethod)
+        {
+            ODERayRequest req = new ODERayRequest();
+            req.geom = geom;
+            req.callbackMethod = retMethod;
+            req.length = length;
+            req.Normal = direction;
+            req.Origin = position;
+            req.Count = 0;
+
+            m_PendingRequests.Enqueue(req);
+        }
+
+        /// <summary>
+        /// Queues a raycast
+        /// </summary>
+        /// <param name="position">Origin of Ray</param>
+        /// <param name="direction">Ray normal</param>
+        /// <param name="length">Ray length</param>
+        /// <param name="count"></param>
+        /// <param name="retMethod">Return method to send the results</param>
+        public void QueueRequest(Vector3 position, Vector3 direction, float length, int count, RayCallback retMethod)
+        {
+            ODERayRequest req = new ODERayRequest();
+            req.geom = IntPtr.Zero;
+            req.callbackMethod = retMethod;
+            req.length = length;
+            req.Normal = direction;
+            req.Origin = position;
+            req.Count = count;
+
+            m_PendingRequests.Enqueue(req);
+        }
+
+        public void QueueRequest(IntPtr geom, Vector3 position, Vector3 direction, float length, int count, RayCallback retMethod)
+        {
+            ODERayRequest req = new ODERayRequest();
+            req.geom = geom;
+            req.callbackMethod = retMethod;
+            req.length = length;
+            req.Normal = direction;
+            req.Origin = position;
+            req.Count = count;
+
+            m_PendingRequests.Enqueue(req);
+        }
+
+        public void QueueRequest(Vector3 position, Vector3 direction, float length, int count, RaycastCallback retMethod)
+        {
+            ODERayRequest req = new ODERayRequest();
+            req.geom = IntPtr.Zero;
+            req.callbackMethod = retMethod;
+            req.length = length;
+            req.Normal = direction;
+            req.Origin = position;
+            req.Count = count;
+
+            m_PendingRequests.Enqueue(req);
+        }
+
+        public void QueueRequest(IntPtr geom, Vector3 position, Vector3 direction, float length, int count, RaycastCallback retMethod)
+        {
+            ODERayRequest req = new ODERayRequest();
+            req.geom = geom;
+            req.callbackMethod = retMethod;
+            req.length = length;
+            req.Normal = direction;
+            req.Origin = position;
+            req.Count = count;
+
+            m_PendingRequests.Enqueue(req);
+        }
+
+        /// <summary>
+        /// Process all queued raycast requests
+        /// </summary>
+        /// <returns>Time in MS the raycasts took to process.</returns>
+        public int ProcessQueuedRequests()
+        {
+            int time = System.Environment.TickCount;
+
+            if (m_PendingRequests.Count <= 0)
+                return 0;
+
+            if (m_scene.ContactgeomsArray == IntPtr.Zero) // oops something got wrong or scene isn't ready still
+            {
+                m_PendingRequests.Clear();
+                return 0;
+            }
+
+            ODERayRequest req;
+
+            int i = 50; // arbitary limit of processed tests per frame
+
+            while(m_PendingRequests.Dequeue(out req))
+            {
+                if (req.geom == IntPtr.Zero)
+                    doSpaceRay(req);
+                else
+                    doGeomRay(req);
+            if(--i < 0)
+                break;
+            }
+
+            lock (m_contactResults)
+                m_contactResults.Clear();
+
+            return System.Environment.TickCount - time;
+        }
+        /// <summary>
+        /// Method that actually initiates the raycast with full top space
+        /// </summary>
+        /// <param name="req"></param>
+        private void doSpaceRay(ODERayRequest req)
+        {
+            // Create the ray
+//            IntPtr ray = d.CreateRay(m_scene.TopSpace, req.length);
+            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);
+
+            // Collide test
+            d.SpaceCollide2(m_scene.TopSpace, ray, IntPtr.Zero, nearCallback);
+
+            // Remove Ray
+//            d.GeomDestroy(ray);
+
+            if (req.callbackMethod == null)
+                return;
+
+            if (req.callbackMethod is RaycastCallback)
+            {
+                // Define default results
+                bool hitYN = false;
+                uint hitConsumerID = 0;
+                float distance = 999999999999f;
+                Vector3 closestcontact = new Vector3(99999f, 99999f, 99999f);
+                Vector3 snormal = Vector3.Zero;
+
+                // Find closest contact and object.
+                lock (m_contactResults)
+                {
+                    foreach (ContactResult cResult in m_contactResults)
+                    {
+                        if (Vector3.Distance(req.Origin, cResult.Pos) < Vector3.Distance(req.Origin, closestcontact))
+                        {
+                            closestcontact = cResult.Pos;
+                            hitConsumerID = cResult.ConsumerID;
+                            distance = cResult.Depth;
+                            hitYN = true;
+                            snormal = cResult.Normal;
+                        }
+                    }
+                    m_contactResults.Clear();
+                }
+               
+                ((RaycastCallback)req.callbackMethod)(hitYN, closestcontact, hitConsumerID, distance, snormal);
+            }
+            else
+            {
+                ((RayCallback)req.callbackMethod)(m_contactResults);
+                lock (m_PendingRequests)
+                    m_contactResults.Clear();
+            }
+        }
+
+        /// <summary>
+        /// Method that actually initiates the raycast with a geom
+        /// </summary>
+        /// <param name="req"></param>
+        private void doGeomRay(ODERayRequest req)
+        {
+            // Create the ray
+//            IntPtr ray = d.CreateRay(m_scene.TopSpace, req.length);
+            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);
+
+            // Collide test
+            d.SpaceCollide2(req.geom, ray, IntPtr.Zero, nearCallback); // still do this to have full AABB pre test
+
+            // Remove Ray
+//            d.GeomDestroy(ray);
+
+            if (req.callbackMethod == null)
+                return;
+
+            if (req.callbackMethod is RaycastCallback)
+            {
+                // Define default results
+                bool hitYN = false;
+                uint hitConsumerID = 0;
+                float distance = 999999999999f;
+                Vector3 closestcontact = new Vector3(99999f, 99999f, 99999f);
+                Vector3 snormal = Vector3.Zero;
+
+                // Find closest contact and object.
+                lock (m_contactResults)
+                {
+                    foreach (ContactResult cResult in m_contactResults)
+                    {
+                        if (Vector3.Distance(req.Origin, cResult.Pos) < Vector3.Distance(req.Origin, closestcontact))
+                        {
+                            closestcontact = cResult.Pos;
+                            hitConsumerID = cResult.ConsumerID;
+                            distance = cResult.Depth;
+                            hitYN = true;
+                            snormal = cResult.Normal;
+                        }
+                    }
+                    m_contactResults.Clear();
+                }
+
+                ((RaycastCallback)req.callbackMethod)(hitYN, closestcontact, hitConsumerID, distance, snormal);
+            }
+            else
+            {
+                ((RayCallback)req.callbackMethod)(m_contactResults);
+                lock (m_PendingRequests)
+                    m_contactResults.Clear();
+            }
+        }
+
+        private bool GetCurContactGeom(int index, ref d.ContactGeom newcontactgeom)
+        {
+            IntPtr ContactgeomsArray = m_scene.ContactgeomsArray;
+            if (ContactgeomsArray == IntPtr.Zero || index >= ColisionContactGeomsPerTest)
+                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. g2 is the ray
+        private void near(IntPtr space, IntPtr g1, IntPtr g2)
+        {
+            //Don't test against heightfield Geom, or you'll be sorry!
+            // Exclude heightfield geom
+
+            if (g1 == IntPtr.Zero || g1 == g2)
+                return;
+
+            if (d.GeomGetClass(g1) == d.GeomClassID.HeightfieldClass)
+                return;
+
+            // Raytest against AABBs of spaces first, then dig into the spaces it hits for actual geoms.
+            if (d.GeomIsSpace(g1))
+            {
+                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, ColisionContactGeomsPerTest, m_scene.ContactgeomsArray, d.ContactGeom.unmanagedSizeOf);
+            }
+            catch (SEHException)
+            {
+                m_log.Error("[PHYSICS Ray]: 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.");
+            }
+            catch (Exception e)
+            {
+                m_log.WarnFormat("[PHYSICS Ray]: Unable to collide test an object: {0}", e.Message);
+                return;
+            }
+
+            if (count == 0)
+                return;
+
+            PhysicsActor p1 = null;
+
+            if (g1 != IntPtr.Zero)
+                m_scene.actor_name_map.TryGetValue(g1, out p1);
+
+            d.ContactGeom curcontact = new d.ContactGeom();
+            // Loop over contacts, build results.
+            for (int i = 0; i < count; i++)
+            {
+                if (!GetCurContactGeom(i, ref curcontact))
+                    break;
+                if (p1 != null) { 
+                    if (p1 is OdePrim)
+                    {
+                        ContactResult collisionresult = new ContactResult();
+                    
+                        collisionresult.ConsumerID = ((OdePrim)p1).m_localID;
+                        collisionresult.Pos = new Vector3(curcontact.pos.X, curcontact.pos.Y, curcontact.pos.Z);
+                        collisionresult.Depth = curcontact.depth;
+                        collisionresult.Normal = new Vector3(curcontact.normal.X, curcontact.normal.Y,
+                                                             curcontact.normal.Z);
+                        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;
+        }
+    }
+
+    public struct ODERayRequest
+    {
+        public IntPtr geom;
+        public Vector3 Origin;
+        public Vector3 Normal;
+        public int Count;
+        public float length;
+        public object callbackMethod;
+    }
+}
\ No newline at end of file
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs
new file mode 100644
index 0000000..e62746e
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs
@@ -0,0 +1,1961 @@
+
+/*
+ * 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/
+  
+ * Revision/fixs by Ubit Umarov
+ */
+
+using System;
+using System.Runtime.InteropServices;
+using System.Security;
+
+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;
+
+                #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,
+                        FirstUserClass,
+                        LastUserClass = FirstUserClass + MaxUserClasses - 1,
+                        NumClasses,
+                        MaxUserClasses = 4
+                }
+
+                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 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 = "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 int 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 int 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)); }
+                }
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetQuaternion"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomCopyQuaternion(IntPtr geom, out Quaternion q);
+
+                [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 = "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, int bits);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetCollideBits"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomSetCollideBits(IntPtr geom, int 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 string GetConfiguration(string str);
+
+                [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/Physics/UbitOdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdePlugin.cs
new file mode 100644
index 0000000..215d47a
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitOdePlugin/OdePlugin.cs
@@ -0,0 +1,86 @@
+/*
+ * 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.Threading;
+using System.IO;
+using System.Diagnostics;
+using log4net;
+using Nini.Config;
+using OdeAPI;
+using OpenSim.Framework;
+using OpenSim.Region.Physics.Manager;
+using OpenMetaverse;
+
+namespace OpenSim.Region.Physics.OdePlugin
+{
+    /// <summary>
+    /// ODE plugin
+    /// </summary>
+    public class OdePlugin : IPhysicsPlugin
+    {
+        //private static readonly log4net.ILog m_log = log4net.LogManager.GetLogger(System.Reflection.MethodBase.GetCurrentMethod().DeclaringType);
+
+        private OdeScene m_scene;
+
+        public bool Init()
+        {
+            if (d.InitODE2(0) != 0)
+            {
+                if (d.AllocateODEDataForThread(~0U) == 0)
+                {
+                    d.CloseODE();
+                    return false;
+                }
+                return true;
+            }
+            return false;
+        }
+
+        public PhysicsScene GetScene(String sceneIdentifier)
+        {
+            if (m_scene == null)
+            {
+                m_scene = new OdeScene(sceneIdentifier);
+            }
+            return (m_scene);
+        }
+
+        public string GetName()
+        {
+            return ("UbitODE");
+        }
+
+        public void Dispose()
+        {
+            d.CloseODE();
+        }
+    }
+}
\ No newline at end of file
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs
new file mode 100644
index 0000000..56f3786
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs
@@ -0,0 +1,2540 @@
+/*
+ * 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 System.Reflection;
+using System.Runtime.InteropServices;
+using System.Threading;
+using System.IO;
+using System.Diagnostics;
+using log4net;
+using Nini.Config;
+using OdeAPI;
+using OpenSim.Framework;
+using OpenSim.Region.Physics.Manager;
+using OpenMetaverse;
+
+namespace OpenSim.Region.Physics.OdePlugin
+{
+    public enum StatusIndicators : int
+    {
+        Generic = 0,
+        Start = 1,
+        End = 2
+    }
+
+    public struct sCollisionData
+    {
+        public uint ColliderLocalId;
+        public uint CollidedWithLocalId;
+        public int NumberOfCollisions;
+        public int CollisionType;
+        public int StatusIndicator;
+        public int lastframe;
+    }
+
+    [Flags]
+    public enum CollisionCategories : int
+    {
+        Disabled = 0,
+        Geom = 0x00000001,
+        Body = 0x00000002,
+        Space = 0x00000004,
+        Character = 0x00000008,
+        Land = 0x00000010,
+        Water = 0x00000020,
+        Wind = 0x00000040,
+        Sensor = 0x00000080,
+        Selected = 0x00000100
+    }
+
+    /// <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,
+        AngVelocity,
+        Acceleration,
+        Force,
+        Torque,
+
+        AddForce,
+        AddAngForce,
+        AngLock,
+
+        Size,
+        Shape,
+
+        CollidesWater,
+        VolumeDtc,
+
+        Physical,
+        Selected,
+        disabled,
+        building,
+
+        VehicleType,
+        VehicleFloatParam,
+        VehicleVectorParam,
+        VehicleRotationParam,
+        VehicleFlags,
+
+        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 readonly ILog m_log;
+        // private Dictionary<string, sCollisionData> m_storedCollisions = new Dictionary<string, sCollisionData>();
+
+        private int threadid = 0;
+        private Random fluidRandomizer = new Random(Environment.TickCount);
+
+        const d.ContactFlags comumContactFlags = d.ContactFlags.SoftERP | d.ContactFlags.SoftCFM |d.ContactFlags.Approx1 | d.ContactFlags.Bounce;
+        const float comumContactERP = 0.6f;
+        const float comumSoftContactERP = 0.1f;
+        const float comumContactCFM = 0.0001f;
+        
+        float frictionScale = 1.0f;
+        
+        float frictionMovementMult = 0.3f;
+
+        float TerrainBounce = 0.3f;
+        float TerrainFriction = 0.3f;
+
+        public float AvatarBounce = 0.3f;
+        public float AvatarFriction = 0;// 0.9f * 0.5f;
+
+        private const uint m_regionWidth = Constants.RegionSize;
+        private const uint m_regionHeight = Constants.RegionSize;
+
+        public float ODE_STEPSIZE = 0.020f;
+        private float metersInSpace = 25.6f;
+        private float m_timeDilation = 1.0f;
+
+        public float gravityx = 0f;
+        public float gravityy = 0f;
+        public float gravityz = -9.8f;
+
+        private float waterlevel = 0f;
+        private int framecount = 0;
+
+        internal IntPtr WaterGeom;
+
+        public float avPIDD = 3200f; // make it visible
+        public float avPIDP = 1400f; // make it visible
+        private float avCapRadius = 0.37f;
+        private float avDensity = 3f;
+        private float avMovementDivisorWalk = 1.3f;
+        private float avMovementDivisorRun = 0.8f;
+        private float minimumGroundFlightOffset = 3f;
+        public float maximumMassObject = 10000.01f;
+
+        public bool meshSculptedPrim = true;
+        public bool forceSimplePrimMeshing = false;
+
+        public float meshSculptLOD = 32;
+        public float MeshSculptphysicalLOD = 16;
+
+        public float geomDefaultDensity = 10.000006836f;
+
+        public int geomContactPointsStartthrottle = 3;
+        public int geomUpdatesPerThrottledUpdate = 15;
+
+        public float bodyPIDD = 35f;
+        public float bodyPIDG = 25;
+
+        public int geomCrossingFailuresBeforeOutofbounds = 6;
+
+        public int bodyFramesAutoDisable = 20;
+
+        private float[] _watermap;
+        private bool m_filterCollisions = true;
+
+        private d.NearCallback nearCallback;
+
+        private readonly HashSet<OdeCharacter> _characters = new HashSet<OdeCharacter>();
+        private readonly HashSet<OdePrim> _prims = new HashSet<OdePrim>();
+        private readonly HashSet<OdePrim> _activeprims = 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 readonly List<PhysicsActor> _collisionEventPrim = new List<PhysicsActor>();
+        
+        private readonly HashSet<OdeCharacter> _badCharacter = new HashSet<OdeCharacter>();
+        public Dictionary<IntPtr, String> geom_name_map = new Dictionary<IntPtr, String>();
+        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;
+
+        const int maxContactsbeforedeath = 4000;
+        private volatile int m_global_contactcount = 0;
+
+
+        private readonly IntPtr contactgroup;
+
+        public ContactData[] m_materialContactsData = new ContactData[8];
+
+        private readonly DoubleDictionary<Vector3, IntPtr, IntPtr> RegionTerrain = new DoubleDictionary<Vector3, IntPtr, IntPtr>();
+        private readonly Dictionary<IntPtr, float[]> TerrainHeightFieldHeights = new Dictionary<IntPtr, float[]>();
+        private readonly Dictionary<IntPtr, GCHandle> TerrainHeightFieldHeightsHandlers = new Dictionary<IntPtr, GCHandle>();
+       
+        private int m_physicsiterations = 10;
+        private const float m_SkipFramesAtms = 0.40f; // Drop frames gracefully at a 400 ms lag
+        private readonly PhysicsActor PANull = new NullPhysicsActor();
+        private float step_time = 0.0f;
+
+        public IntPtr world;
+
+        private uint obj2LocalID = 0;
+        private OdeCharacter cc1;
+        private OdePrim cp1;
+        private OdeCharacter cc2;
+        private OdePrim cp2;
+
+        // 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 StaticSpace; // space for the static things around
+
+        // some speedup variables
+        private int spaceGridMaxX;
+        private int spaceGridMaxY;
+        private float spacesPerMeter;
+
+        // split static geometry collision into a grid as before
+        private IntPtr[,] staticPrimspace;
+
+        private Object OdeLock;
+        private static Object SimulationLock;
+
+        public IMesher mesher;
+
+        private IConfigSource m_config;
+
+        public bool physics_logging = false;
+        public int physics_logging_interval = 0;
+        public bool physics_logging_append_existing_logfile = false;
+
+        private Vector3 m_worldOffset = Vector3.Zero;
+        public Vector2 WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize);
+        private PhysicsScene m_parentScene = null;
+
+        private ODERayCastRequestManager m_rayCastManager;
+
+
+/* maybe needed if ode uses tls
+        private void checkThread()
+        {
+
+            int th = Thread.CurrentThread.ManagedThreadId;
+            if(th != threadid)
+            {
+                threadid = th;
+                d.AllocateODEDataForThread(~0U);
+            }
+        }
+ */
+        /// <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>
+        public OdeScene(string sceneIdentifier)
+            {
+            m_log 
+                = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType.ToString() + "." + sceneIdentifier);
+
+//            checkThread();
+            Name = sceneIdentifier;
+
+            OdeLock = new Object();
+            SimulationLock = new Object();
+
+            nearCallback = near;
+
+            m_rayCastManager = new ODERayCastRequestManager(this);
+            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);
+                    StaticSpace = d.HashSpaceCreate(TopSpace);
+                    }
+                catch
+                    {
+                    // i must RtC#FM 
+                    }
+
+                d.HashSpaceSetLevels(TopSpace, -2, 8); // cell sizes from .25 to 256 ?? need check what this really does
+                d.HashSpaceSetLevels(ActiveSpace, -2, 8);
+                d.HashSpaceSetLevels(StaticSpace, -2, 8);
+
+                // demote to second level
+                d.SpaceSetSublevel(ActiveSpace, 1);
+                d.SpaceSetSublevel(StaticSpace, 1);
+
+                contactgroup = d.JointGroupCreate(0);
+                //contactgroup
+
+                d.WorldSetAutoDisableFlag(world, false);
+            }
+
+            _watermap = new float[258 * 258];
+        }
+
+        // Initialize the mesh plugin
+//        public override void Initialise(IMesher meshmerizer, IConfigSource config, RegionInfo region )
+        public override void Initialise(IMesher meshmerizer, IConfigSource config)
+        {
+//            checkThread();
+            mesher = meshmerizer;
+            m_config = config;
+
+//            m_log.WarnFormat("ODE configuration: {0}", d.GetConfiguration("ODE"));
+            /*
+                        if (region != null)
+                        {
+                            WorldExtents.X = region.RegionSizeX;
+                            WorldExtents.Y = region.RegionSizeY;
+                        }
+             */
+
+            // Defaults
+
+            avPIDD = 2200.0f;
+            avPIDP = 900.0f;
+
+            int contactsPerCollision = 80;
+
+            if (m_config != null)
+            {
+                IConfig physicsconfig = m_config.Configs["ODEPhysicsSettings"];
+                if (physicsconfig != null)
+                {
+                    gravityx = physicsconfig.GetFloat("world_gravityx", 0f);
+                    gravityy = physicsconfig.GetFloat("world_gravityy", 0f);
+                    gravityz = physicsconfig.GetFloat("world_gravityz", -9.8f);
+
+                    metersInSpace = physicsconfig.GetFloat("meters_in_small_space", 29.9f);
+
+                    contactsurfacelayer = physicsconfig.GetFloat("world_contact_surface_layer", contactsurfacelayer);
+
+                    ODE_STEPSIZE = physicsconfig.GetFloat("world_stepsize", 0.020f);
+                    m_physicsiterations = physicsconfig.GetInt("world_internal_steps_without_collisions", 10);
+
+                    avDensity = physicsconfig.GetFloat("av_density", avDensity);
+                    avMovementDivisorWalk = physicsconfig.GetFloat("av_movement_divisor_walk", 1.3f);
+                    avMovementDivisorRun = physicsconfig.GetFloat("av_movement_divisor_run", 0.8f);
+                    avCapRadius = physicsconfig.GetFloat("av_capsule_radius", 0.37f);
+
+                    contactsPerCollision = physicsconfig.GetInt("contacts_per_collision", 80);
+
+                    geomContactPointsStartthrottle = physicsconfig.GetInt("geom_contactpoints_start_throttling", 3);
+                    geomUpdatesPerThrottledUpdate = physicsconfig.GetInt("geom_updates_before_throttled_update", 15);
+                    geomCrossingFailuresBeforeOutofbounds = physicsconfig.GetInt("geom_crossing_failures_before_outofbounds", 5);
+
+                    geomDefaultDensity = physicsconfig.GetFloat("geometry_default_density", 10.000006836f);
+                    bodyFramesAutoDisable = physicsconfig.GetInt("body_frames_auto_disable", 20);
+
+                    bodyPIDD = physicsconfig.GetFloat("body_pid_derivative", 35f);
+                    bodyPIDG = physicsconfig.GetFloat("body_pid_gain", 25f);
+
+                    forceSimplePrimMeshing = physicsconfig.GetBoolean("force_simple_prim_meshing", forceSimplePrimMeshing);
+                    meshSculptedPrim = physicsconfig.GetBoolean("mesh_sculpted_prim", true);
+                    meshSculptLOD = physicsconfig.GetFloat("mesh_lod", 32f);
+                    MeshSculptphysicalLOD = physicsconfig.GetFloat("mesh_physical_lod", 16f);
+                    m_filterCollisions = physicsconfig.GetBoolean("filter_collisions", false);
+
+                    if (Environment.OSVersion.Platform == PlatformID.Unix)
+                    {
+                        avPIDD = physicsconfig.GetFloat("av_pid_derivative_linux", 2200.0f);
+                        avPIDP = physicsconfig.GetFloat("av_pid_proportional_linux", 900.0f);
+                    }
+                    else
+                    {
+                        avPIDD = physicsconfig.GetFloat("av_pid_derivative_win", 2200.0f);
+                        avPIDP = physicsconfig.GetFloat("av_pid_proportional_win", 900.0f);
+                    }
+
+                    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", 3f);
+                    maximumMassObject = physicsconfig.GetFloat("maximum_mass_object", 10000.01f);
+                }
+            }
+
+            ContactgeomsArray = Marshal.AllocHGlobal(contactsPerCollision * d.ContactGeom.unmanagedSizeOf);
+            GlobalContactsArray = GlobalContactsArray = Marshal.AllocHGlobal(maxContactsbeforedeath * d.Contact.unmanagedSizeOf);
+
+            m_materialContactsData[(int)Material.Stone].mu = frictionScale * 0.8f;
+            m_materialContactsData[(int)Material.Stone].bounce = 0.4f;
+
+            m_materialContactsData[(int)Material.Metal].mu = frictionScale * 0.3f;
+            m_materialContactsData[(int)Material.Metal].bounce = 0.4f;
+
+            m_materialContactsData[(int)Material.Glass].mu = frictionScale * 0.2f;
+            m_materialContactsData[(int)Material.Glass].bounce = 0.7f;
+
+            m_materialContactsData[(int)Material.Wood].mu = frictionScale * 0.6f;
+            m_materialContactsData[(int)Material.Wood].bounce = 0.5f;
+
+            m_materialContactsData[(int)Material.Flesh].mu = frictionScale * 0.9f;
+            m_materialContactsData[(int)Material.Flesh].bounce = 0.3f;
+
+            m_materialContactsData[(int)Material.Plastic].mu = frictionScale * 0.4f;
+            m_materialContactsData[(int)Material.Plastic].bounce = 0.7f;
+
+            m_materialContactsData[(int)Material.Rubber].mu = frictionScale * 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;
+
+            TerrainFriction *= frictionScale;
+//            AvatarFriction *= frictionScale;
+
+            // Set the gravity,, don't disable things automatically (we set it explicitly on some things)
+
+            d.WorldSetGravity(world, gravityx, gravityy, gravityz);
+            d.WorldSetContactSurfaceLayer(world, contactsurfacelayer);
+
+            d.WorldSetLinearDamping(world, 0.001f);
+            d.WorldSetAngularDamping(world, 0.001f);
+            d.WorldSetAngularDampingThreshold(world, 0f);
+            d.WorldSetLinearDampingThreshold(world, 0f);
+            d.WorldSetMaxAngularSpeed(world, 256f);
+
+            d.WorldSetCFM(world,1e-6f); // a bit harder than default
+            //d.WorldSetCFM(world, 1e-4f); // a bit harder than default
+            d.WorldSetERP(world, 0.6f); // higher than original
+
+            // Set how many steps we go without running collision testing
+            // This is in addition to the step size.
+            // Essentially Steps * m_physicsiterations
+            d.WorldSetQuickStepNumIterations(world, m_physicsiterations);
+            d.WorldSetContactMaxCorrectingVel(world, 100.0f);
+
+            spacesPerMeter = 1 / metersInSpace;
+            spaceGridMaxX = (int)(WorldExtents.X * spacesPerMeter);
+            spaceGridMaxY = (int)(WorldExtents.Y * spacesPerMeter);
+
+            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);
+                    staticPrimspace[i, j] = newspace;
+                }
+            // let this now be real maximum values
+            spaceGridMaxX--;
+            spaceGridMaxY--;
+        }
+
+        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, float mu, float bounce, bool softerp)
+        {
+            if (GlobalContactsArray == IntPtr.Zero || m_global_contactcount >= maxContactsbeforedeath)
+                return IntPtr.Zero;
+
+            d.Contact newcontact = new d.Contact();
+            newcontact.geom.depth = contactGeom.depth;
+            newcontact.geom.g1 = contactGeom.g1;
+            newcontact.geom.g2 = contactGeom.g2;
+            newcontact.geom.pos = contactGeom.pos;
+            newcontact.geom.normal = contactGeom.normal;
+            newcontact.geom.side1 = contactGeom.side1;
+            newcontact.geom.side2 = contactGeom.side2;
+
+            // this needs bounce also
+            newcontact.surface.mode = comumContactFlags;
+            newcontact.surface.mu = mu;
+            newcontact.surface.bounce = bounce;
+            newcontact.surface.soft_cfm = comumContactCFM;
+            if (softerp)
+                newcontact.surface.soft_erp = comumSoftContactERP;
+            else
+                newcontact.surface.soft_erp = comumContactERP;
+
+            IntPtr contact = new IntPtr(GlobalContactsArray.ToInt64() + (Int64)(m_global_contactcount * d.Contact.unmanagedSizeOf));
+            Marshal.StructureToPtr(newcontact, contact, true);
+            return d.JointCreateContactPtr(world, contactgroup, contact);
+        }
+
+
+        /// <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 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;
+        }
+
+
+
+        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;
+
+                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;
+            }
+
+            // id contacts done
+            if (count == 0)
+                return;
+
+            // try get physical actors 
+            PhysicsActor p1;
+            PhysicsActor p2;
+
+            if (!actor_name_map.TryGetValue(g1, out p1))
+            {
+                p1 = PANull;
+            }
+
+            if (!actor_name_map.TryGetValue(g2, out p2))
+            {
+                p2 = PANull;
+            }
+
+            // update actors collision score
+            if (p1.CollisionScore >= float.MaxValue - count)
+                p1.CollisionScore = 0;
+            p1.CollisionScore += count;
+
+            if (p2.CollisionScore >= float.MaxValue - count)
+                p2.CollisionScore = 0;
+            p2.CollisionScore += count;
+
+
+            // get first contact
+            d.ContactGeom curContact = new d.ContactGeom();
+            if (!GetCurContactGeom(0, ref curContact))
+                return;
+            // for now it's the one with max depth
+            ContactPoint 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
+                        );
+            // do volume detection case
+            if (
+                (p1 is OdePrim) && (((OdePrim)p1).m_isVolumeDetect) ||
+                                (p2 is OdePrim) && (((OdePrim)p2).m_isVolumeDetect))
+            {
+                collision_accounting_events(p1, p2, maxDepthContact);
+                return;
+            }
+
+            // big messy collision analises
+            float mu = 0;
+            float bounce = 0;
+            ContactData contactdata1;
+            ContactData contactdata2;
+            bool erpSoft = false;
+
+            String name = null;
+            bool dop1foot = false;
+            bool dop2foot = false;
+            bool ignore = false;
+
+            switch (p1.PhysicsActorType)
+            {
+                case (int)ActorTypes.Agent:
+                    switch (p2.PhysicsActorType)
+                    {
+                        case (int)ActorTypes.Agent:
+                            contactdata1 = p1.ContactData;
+                            contactdata2 = p2.ContactData;
+                            bounce = contactdata1.bounce * contactdata2.bounce;
+                            
+                            mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu);
+
+                            if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f))
+                                mu *= frictionMovementMult;
+
+                            p1.CollidingObj = true;
+                            p2.CollidingObj = true;
+                            break;
+                        case (int)ActorTypes.Prim:
+                            contactdata1 = p1.ContactData;
+                            contactdata2 = p2.ContactData;
+                            bounce = contactdata1.bounce * contactdata2.bounce;
+                            
+                            mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu);
+
+                            if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f))
+                                mu *= frictionMovementMult;
+                            if (p2.Velocity.LengthSquared() > 0.0f)
+                                p2.CollidingObj = true;
+                            dop1foot = true;
+                            break;
+                        default:
+                            ignore=true; // avatar to terrain and water ignored
+                            break;
+                    }
+                    break;
+
+                case (int)ActorTypes.Prim:
+                    switch (p2.PhysicsActorType)
+                    {
+                        case (int)ActorTypes.Agent:
+                            contactdata1 = p1.ContactData;
+                            contactdata2 = p2.ContactData;
+                            bounce = contactdata1.bounce * contactdata2.bounce;
+                            
+                            mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu);
+
+                            if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f))
+                                mu *= frictionMovementMult;
+
+                            dop2foot = true;
+                            if (p1.Velocity.LengthSquared() > 0.0f)
+                                p1.CollidingObj = true;
+                            break;
+                        case (int)ActorTypes.Prim:
+                            if ((p1.Velocity - p2.Velocity).LengthSquared() > 0.0f)
+                            {
+                                p1.CollidingObj = true;
+                                p2.CollidingObj = true;
+                            }
+                            contactdata1 = p1.ContactData;
+                            contactdata2 = p2.ContactData;
+                            bounce = contactdata1.bounce * contactdata2.bounce;
+                            erpSoft = true;
+                            mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu);
+
+                            if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f))
+                                mu *= frictionMovementMult;
+
+                            break;
+                        default:
+                            if (geom_name_map.TryGetValue(g2, out name))
+                            {
+                                if (name == "Terrain")
+                                {
+                                    erpSoft = true;
+                                    contactdata1 = p1.ContactData;
+                                    bounce = contactdata1.bounce * TerrainBounce;
+                                    mu = (float)Math.Sqrt(contactdata1.mu * TerrainFriction);
+                                    if (Math.Abs(p1.Velocity.X) > 0.1f || Math.Abs(p1.Velocity.Y) > 0.1f)
+                                        mu *= frictionMovementMult;
+                                    p1.CollidingGround = true;
+                                }
+                                else if (name == "Water")
+                                {
+                                    erpSoft = true;
+                                }
+                            }
+                            else
+                                ignore=true;
+                            break;
+                    }
+                    break;
+
+                default:
+                    if (geom_name_map.TryGetValue(g1, out name))
+                    {
+                        if (name == "Terrain")
+                        {
+                            if (p2.PhysicsActorType == (int)ActorTypes.Prim)
+                            {
+                                erpSoft = true;
+                                p2.CollidingGround = true;
+                                contactdata2 = p2.ContactData;
+                                bounce = contactdata2.bounce * TerrainBounce;
+                                mu = (float)Math.Sqrt(contactdata2.mu * TerrainFriction);
+
+                                if (Math.Abs(p2.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y) > 0.1f)
+                                    mu *= frictionMovementMult;
+                            }
+                            else
+                                ignore = true;
+
+                        }
+                        else if (name == "Water" &&
+                            (p2.PhysicsActorType == (int)ActorTypes.Prim || p2.PhysicsActorType == (int)ActorTypes.Agent))
+                        {
+                            erpSoft = true;
+                        }
+                    }
+                    else
+                        ignore = true;
+                    break;
+            }
+
+            if (ignore)
+                return;
+
+            IntPtr Joint;
+
+            int i = 0;
+            while(true)
+            {
+                if (dop1foot && (p1.Position.Z - curContact.pos.Z) > (p1.Size.Z - avCapRadius) * 0.5f)
+                    p1.IsColliding = true;
+                if (dop2foot && (p2.Position.Z - curContact.pos.Z) > (p2.Size.Z - avCapRadius) * 0.5f)
+                    p2.IsColliding = true;
+
+                Joint = CreateContacJoint(ref curContact, mu, bounce, erpSoft);
+                d.JointAttach(Joint, b1, b2);
+
+                if (++m_global_contactcount >= maxContactsbeforedeath)
+                    break;
+
+                if(++i >= count)
+                    break;
+
+                if (!GetCurContactGeom(i, ref curContact))
+                    break;
+
+                if (curContact.depth > maxDepthContact.PenetrationDepth)
+                {
+                    maxDepthContact.Position.X = curContact.pos.X;
+                    maxDepthContact.Position.Y = curContact.pos.Y;
+                    maxDepthContact.Position.Z = curContact.pos.Z;
+                    maxDepthContact.SurfaceNormal.X = curContact.normal.X;
+                    maxDepthContact.SurfaceNormal.Y = curContact.normal.Y;
+                    maxDepthContact.SurfaceNormal.Z = curContact.normal.Z;
+                    maxDepthContact.PenetrationDepth = curContact.depth;
+                }
+            }
+
+            collision_accounting_events(p1, p2, maxDepthContact);
+
+/*
+            if (notskipedcount > geomContactPointsStartthrottle)
+            {
+                // If there are more then 3 contact points, it's likely
+                // that we've got a pile of objects, so ...
+                // We don't want to send out hundreds of terse updates over and over again
+                // so lets throttle them and send them again after it's somewhat sorted out.
+                 this needs checking so out for now
+                                if (b1 != IntPtr.Zero)
+                                    p1.ThrottleUpdates = true;
+                                if (b2 != IntPtr.Zero)
+                                    p2.ThrottleUpdates = true;
+                
+            }
+ */
+        }            
+
+        private void collision_accounting_events(PhysicsActor p1, PhysicsActor p2, ContactPoint contact)
+            {
+            // obj1LocalID = 0;
+            //returncollisions = false;
+            obj2LocalID = 0;
+            //ctype = 0;
+            //cStartStop = 0;
+            if (!(p2.SubscribedEvents() || p1.SubscribedEvents()))
+                return;
+
+            switch ((ActorTypes)p1.PhysicsActorType)
+                {
+                case ActorTypes.Agent:
+                    cc1 = (OdeCharacter)p1;
+                    switch ((ActorTypes)p2.PhysicsActorType)
+                        {
+                        case ActorTypes.Agent:
+                            cc2 = (OdeCharacter)p2;
+                            obj2LocalID = cc2.m_localID;
+                            if (p2.SubscribedEvents())
+                                cc2.AddCollisionEvent(cc1.m_localID, contact);
+                            break;
+
+                        case ActorTypes.Prim:
+                            if (p2 is OdePrim)
+                                {
+                                cp2 = (OdePrim)p2;
+                                obj2LocalID = cp2.m_localID;
+                                if (p2.SubscribedEvents())
+                                    cp2.AddCollisionEvent(cc1.m_localID, contact);
+                                }
+                            break;
+
+                        case ActorTypes.Ground:
+                        case ActorTypes.Unknown:
+                        default:
+                            obj2LocalID = 0;
+                            break;
+                        }
+                    if (p1.SubscribedEvents())
+                        {
+                        contact.SurfaceNormal = -contact.SurfaceNormal;
+                        cc1.AddCollisionEvent(obj2LocalID, contact);
+                        }
+                    break;
+
+                case ActorTypes.Prim:
+
+                    if (p1 is OdePrim)
+                        {
+                        cp1 = (OdePrim)p1;
+
+                        // obj1LocalID = cp2.m_localID;
+                        switch ((ActorTypes)p2.PhysicsActorType)
+                            {
+                            case ActorTypes.Agent:
+                                if (p2 is OdeCharacter)
+                                    {
+                                    cc2 = (OdeCharacter)p2;
+                                    obj2LocalID = cc2.m_localID;
+                                    if (p2.SubscribedEvents())
+                                        cc2.AddCollisionEvent(cp1.m_localID, contact);
+                                    }
+                                break;
+                            case ActorTypes.Prim:
+
+                                if (p2 is OdePrim)
+                                    {
+                                    cp2 = (OdePrim)p2;
+                                    obj2LocalID = cp2.m_localID;
+                                    if (p2.SubscribedEvents())
+                                        cp2.AddCollisionEvent(cp1.m_localID, contact);
+                                    }
+                                break;
+
+                            case ActorTypes.Ground:
+                            case ActorTypes.Unknown:
+                            default:
+                                obj2LocalID = 0;
+                                break;
+                            }
+                        if (p1.SubscribedEvents())
+                            {
+                            contact.SurfaceNormal = -contact.SurfaceNormal;
+                            cp1.AddCollisionEvent(obj2LocalID, contact);
+                            }
+                        }
+                    break;
+                }
+            }
+
+        /// <summary>
+        /// This is our collision testing routine in ODE
+        /// </summary>
+        /// <param name="timeStep"></param>
+        private void collision_optimized()
+        {
+//        _perloopContact.Clear();
+// clear characts IsColliding until we do it some other way
+
+            lock (_characters)
+                {
+                foreach (OdeCharacter chr in _characters)
+                    {
+                    // this are odd checks  if they are needed something is wrong elsewhere
+                    // keep for now
+                    if (chr == null)
+                        continue;
+
+                    if (chr.Shell == IntPtr.Zero || chr.Body == IntPtr.Zero)
+                        continue;
+
+                    chr.IsColliding = false;
+                    //                    chr.CollidingGround = false; not done here
+                    chr.CollidingObj = false;
+                    }
+                }
+
+            // now let ode do its job
+            // colide active things amoung them
+
+            int st = Util.EnvironmentTickCount();
+            int ta;
+            int ts;
+            try
+                {
+                d.SpaceCollide(ActiveSpace, IntPtr.Zero, nearCallback);
+                }
+            catch (AccessViolationException)
+                {
+                m_log.Warn("[PHYSICS]: Unable to Active space collide");
+                }
+            ta = Util.EnvironmentTickCountSubtract(st);
+            // then active things with static enviroment
+            try
+                {
+                d.SpaceCollide2(ActiveSpace,StaticSpace, IntPtr.Zero, nearCallback);
+                }
+            catch (AccessViolationException)
+                {
+                m_log.Warn("[PHYSICS]: Unable to Active to static space collide");
+                }
+            ts = Util.EnvironmentTickCountSubtract(st);
+//            _perloopContact.Clear();
+        }
+
+        #endregion
+
+
+        public float GetTerrainHeightAtXY(float x, float y)
+        {
+            // assumes 1m size grid and constante size square regions
+            // region offset in mega position
+
+            int offsetX = ((int)(x / (int)Constants.RegionSize)) * (int)Constants.RegionSize;
+            int offsetY = ((int)(y / (int)Constants.RegionSize)) * (int)Constants.RegionSize;
+
+            IntPtr heightFieldGeom = IntPtr.Zero;
+
+            // get region map
+            if (!RegionTerrain.TryGetValue(new Vector3(offsetX, offsetY, 0), out heightFieldGeom))
+                return 0f;
+
+            if (heightFieldGeom == IntPtr.Zero)
+                return 0f;
+
+            if (!TerrainHeightFieldHeights.ContainsKey(heightFieldGeom))
+                return 0f;
+
+            // TerrainHeightField for ODE as offset 1m
+            x += 1f - offsetX;
+            y += 1f - offsetY;
+
+            // 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 regsize = (int)Constants.RegionSize + 2; // map size see setterrain
+
+            // 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 < regsize - 1)
+            {
+                iy = (int)x;
+                dy = x - (float)iy;
+            }
+            else // out world use external height
+            {
+                iy = regsize - 1;
+                dy = 0;
+            }
+            if (y < regsize - 1)
+            {
+                ix = (int)y;
+                dx = y - (float)ix;
+            }
+            else
+            {
+                ix = regsize - 1;
+                dx = 0;
+            }
+
+            float h0;
+            float h1;
+            float h2;
+
+            iy *= regsize;
+            iy += ix; // all indexes have iy + ix
+
+            float[] heights = TerrainHeightFieldHeights[heightFieldGeom];
+
+            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
+            }
+
+            return h0 + h1 + h2;
+        }
+
+        /// <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)
+        {
+            lock (_collisionEventPrim)
+            {
+                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 (_collisionEventPrim)
+            {
+                if (_collisionEventPrim.Contains(obj))
+                    _collisionEventPrim.Remove(obj);
+            }
+        }
+
+        #region Add/Remove Entities
+
+        public override PhysicsActor AddAvatar(string avName, Vector3 position, Vector3 size, bool isFlying)
+        {
+            Vector3 pos;
+            pos.X = position.X;
+            pos.Y = position.Y;
+            pos.Z = position.Z;
+            OdeCharacter newAv = new OdeCharacter(avName, this, pos, size, avPIDD, avPIDP, avCapRadius, 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");
+            ((OdeCharacter) actor).Destroy();
+        }
+
+        private PhysicsActor AddPrim(String name, Vector3 position, Vector3 size, Quaternion rotation,
+                                     PrimitiveBaseShape pbs, bool isphysical, uint localID)
+        {
+            Vector3 pos = position;
+            Vector3 siz = size;
+            Quaternion rot = rotation;
+
+            OdePrim newPrim;
+            lock (OdeLock)
+            {
+                newPrim = new OdePrim(name, this, pos, siz, rot, pbs, isphysical);
+
+                lock (_prims)
+                    _prims.Add(newPrim);
+            }
+            newPrim.LocalID = localID;
+            return newPrim;
+        }
+
+        public void addActivePrim(OdePrim activatePrim)
+        {
+            // adds active prim..   (ones that should be iterated over in collisions_optimized
+            lock (_activeprims)
+            {
+                if (!_activeprims.Contains(activatePrim))
+                    _activeprims.Add(activatePrim);
+                //else
+                  //  m_log.Warn("[PHYSICS]: Double Entry in _activeprims detected, potential crash immenent");
+            }
+        }
+
+        public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
+                                                  Vector3 size, Quaternion rotation, bool isPhysical, uint localid)
+        {
+#if SPAM
+            m_log.DebugFormat("[PHYSICS]: Adding physics actor to {0}", primName);
+#endif
+
+            return AddPrim(primName, position, size, rotation, pbs, isPhysical, localid);
+        }
+
+        public override float TimeDilation
+        {
+            get { return m_timeDilation; }
+        }
+
+        public override bool SupportsNINJAJoints
+        {
+            get { return false; }
+        }
+
+
+        public void remActivePrim(OdePrim deactivatePrim)
+        {
+            lock (_activeprims)
+            {
+                _activeprims.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();
+                }
+            }
+        }
+        /// <summary>
+        /// This is called from within simulate but outside the locked portion
+        /// We need to do our own locking here
+        /// (Note: As of 20110801 this no longer appears to be true - this is being called within lock (odeLock) in
+        /// Simulate() -- justincc).
+        ///
+        /// Essentially, we need to remove the prim from our space segment, whatever segment it's in.
+        ///
+        /// If there are no more prim in the segment, we need to empty (spacedestroy)the segment and reclaim memory
+        /// that the space was using.
+        /// </summary>
+        /// <param name="prim"></param>
+        public void RemovePrimThreadLocked(OdePrim prim)
+        {
+            //Console.WriteLine("RemovePrimThreadLocked " +  prim.m_primName);
+            lock (prim)
+            {
+                RemoveCollisionEventReporting(prim);
+                lock (_prims)
+                    _prims.Remove(prim);
+            }
+
+        }
+        #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);
+                }
+                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);
+                    }
+                }
+            }
+
+            // 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;
+            x = (int)(pos.X * spacesPerMeter);
+            if (x < 0)
+                x = 0;
+            else if (x > spaceGridMaxX)
+                x = spaceGridMaxX;
+
+            y = (int)(pos.Y * spacesPerMeter);
+            if (y < 0)
+                y = 0;
+            else if (y >spaceGridMaxY)
+                y = spaceGridMaxY;
+
+            IntPtr tmpSpace = staticPrimspace[x, y];
+            return tmpSpace;
+        }
+ 
+        #endregion
+
+        /// <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(PrimitiveBaseShape pbs)
+        {
+            // most of this is redundant now as the mesher will return null if it cant mesh a prim
+            // but we still need to check for sculptie meshing being enabled so this is the most
+            // convenient place to do it for now...
+
+        //    //if (pbs.PathCurve == (byte)Primitive.PathCurve.Circle && pbs.ProfileCurve == (byte)Primitive.ProfileCurve.Circle && pbs.PathScaleY <= 0.75f)
+        //    //m_log.Debug("needsMeshing: " + " pathCurve: " + pbs.PathCurve.ToString() + " profileCurve: " + pbs.ProfileCurve.ToString() + " pathScaleY: " + Primitive.UnpackPathScale(pbs.PathScaleY).ToString());
+            int iPropertiesNotSupportedDefault = 0;
+
+            if (pbs.SculptEntry)
+            {
+                if(!meshSculptedPrim)
+                    return false;
+            }
+
+            // 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 (!forceSimplePrimMeshing && !pbs.SculptEntry)
+            {
+                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
+                        && pbs.ProfileHollow == 0
+                        && pbs.PathTwist == 0 && pbs.PathTwistBegin == 0
+                        && pbs.PathBegin == 0 && pbs.PathEnd == 0
+                        && pbs.PathTaperX == 0 && pbs.PathTaperY == 0
+                        && pbs.PathScaleX == 100 && pbs.PathScaleY == 100
+                        && pbs.PathShearX == 0 && pbs.PathShearY == 0)
+                    {
+#if SPAM
+                    m_log.Warn("NonMesh");
+#endif
+                        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
+
+            if (forceSimplePrimMeshing)
+                return true;
+
+            if (pbs.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 (pbs.SculptEntry && meshSculptedPrim)
+                iPropertiesNotSupportedDefault++;
+
+            if (iPropertiesNotSupportedDefault == 0)
+            {
+#if SPAM
+                m_log.Warn("NonMesh");
+#endif
+                return false;
+            }
+#if SPAM
+            m_log.Debug("Mesh");
+#endif
+            return true; 
+        }
+
+        /// <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)
+            {
+            }
+
+        /// <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 timeStep)
+        {
+            int statstart;
+            int statchanges = 0;
+            int statchmove = 0;
+            int statactmove = 0;
+            int statray = 0;
+            int statcol = 0;
+            int statstep = 0;
+            int statmovchar = 0;
+            int statmovprim;
+            int totjcontact = 0;
+
+            // acumulate time so we can reduce error
+            step_time += timeStep;
+
+            if (step_time < ODE_STEPSIZE)
+                return 0;
+
+            if (framecount >= int.MaxValue)
+                framecount = 0;
+
+            framecount++;
+
+            int curphysiteractions = m_physicsiterations;
+
+            if (step_time >= m_SkipFramesAtms)
+            {
+                // if in trouble reduce step resolution
+                curphysiteractions /= 2;
+            }
+
+            int nodeframes = 0;
+
+//            checkThread();
+
+            lock (SimulationLock)
+            {
+                // adjust number of iterations per step
+                try
+                {
+                    d.WorldSetQuickStepNumIterations(world, curphysiteractions);
+                }
+                catch (StackOverflowException)
+                {
+                    m_log.Error("[PHYSICS]: The operating system wasn't able to allocate enough memory for the simulation.  Restarting the sim.");
+//                    ode.drelease(world);
+                    base.TriggerPhysicsBasedRestart();
+                }
+
+
+                while (step_time >= ODE_STEPSIZE && nodeframes < 10) //limit number of steps so we don't say here for ever
+                {
+                    try
+                    {
+                        statstart = Util.EnvironmentTickCount();
+
+                        // clear pointer/counter to contacts to pass into joints
+                        m_global_contactcount = 0;
+
+                        ODEchangeitem item;
+                        
+                        if(ChangesQueue.Count >0)
+                        {
+                            int ttmpstart = Util.EnvironmentTickCount();
+                            int ttmp;
+                            int ttmp2;
+
+                            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.Warn("[PHYSICS]: doChange failed for a actor");
+                                    };
+                                }
+                                ttmp = Util.EnvironmentTickCountSubtract(ttmpstart);
+                                if (ttmp > 20)
+                                    break;
+                            }
+
+                            ttmp2 = Util.EnvironmentTickCountSubtract(ttmpstart);
+                            if (ttmp2 > 50)
+                                ttmp2 = 0;
+
+                        }
+
+                        statchanges += Util.EnvironmentTickCountSubtract(statstart);
+
+                        // Move characters
+                        lock (_characters)
+                        {
+                            List<OdeCharacter> defects = new List<OdeCharacter>();
+                            foreach (OdeCharacter actor in _characters)
+                            {
+                                if (actor != null)
+                                    actor.Move(ODE_STEPSIZE, defects);
+                            }
+                            if (defects.Count != 0)
+                            {
+                                foreach (OdeCharacter defect in defects)
+                                {
+                                    RemoveCharacter(defect);
+                                }
+                            }
+                        }
+                        statchmove += Util.EnvironmentTickCountSubtract(statstart);
+
+                        // Move other active objects
+                        lock (_activeprims)
+                        {
+                            foreach (OdePrim aprim in _activeprims)
+                            {
+                                aprim.CollisionScore = 0;
+                                aprim.IsColliding = false;
+                                aprim.Move();
+                            }
+                        }
+
+                        statactmove += Util.EnvironmentTickCountSubtract(statstart);
+                        //if ((framecount % m_randomizeWater) == 0)
+                        // randomizeWater(waterlevel);
+
+                        m_rayCastManager.ProcessQueuedRequests();
+
+                        statray += Util.EnvironmentTickCountSubtract(statstart);
+                        collision_optimized();
+                        statcol += Util.EnvironmentTickCountSubtract(statstart);
+
+                        lock (_collisionEventPrim)
+                        {
+                            foreach (PhysicsActor obj in _collisionEventPrim)
+                            {
+                                if (obj == null)
+                                    continue;
+
+                                switch ((ActorTypes)obj.PhysicsActorType)
+                                {
+                                    case ActorTypes.Agent:
+                                        OdeCharacter cobj = (OdeCharacter)obj;
+                                        cobj.AddCollisionFrameTime((int)(ODE_STEPSIZE*1000.0f));
+                                        cobj.SendCollisions();
+                                        break;
+
+                                    case ActorTypes.Prim:
+                                        OdePrim pobj = (OdePrim)obj;
+                                        pobj.SendCollisions();
+                                        break;
+                                }
+                            }
+                        }
+
+                        d.WorldQuickStep(world, ODE_STEPSIZE);
+                        statstep += Util.EnvironmentTickCountSubtract(statstart);
+                        d.JointGroupEmpty(contactgroup);
+                        totjcontact += m_global_contactcount;
+                        //ode.dunlock(world);
+                    }
+                    catch (Exception e)
+                    {
+                        m_log.ErrorFormat("[PHYSICS]: {0}, {1}, {2}", e.Message, e.TargetSite, e);
+//                        ode.dunlock(world);
+                    }
+
+                    step_time -= ODE_STEPSIZE;
+                    nodeframes++;
+                }
+
+                statstart = Util.EnvironmentTickCount();
+
+                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 (_badCharacter)
+                {
+                    if (_badCharacter.Count > 0)
+                    {
+                        foreach (OdeCharacter chr in _badCharacter)
+                        {
+                            RemoveCharacter(chr);
+                        }
+
+                        _badCharacter.Clear();
+                    }
+                }
+                statmovchar = Util.EnvironmentTickCountSubtract(statstart);
+
+                lock (_activeprims)
+                {
+                    {
+                        foreach (OdePrim actor in _activeprims)
+                        {
+                            if (actor.IsPhysical)
+                            {
+                                actor.UpdatePositionAndVelocity((float)nodeframes * ODE_STEPSIZE);
+                            }
+                        }
+                    }
+                }
+
+                statmovprim = Util.EnvironmentTickCountSubtract(statstart);
+
+                int nactivegeoms = d.SpaceGetNumGeoms(ActiveSpace);
+                int nstaticgeoms = d.SpaceGetNumGeoms(StaticSpace);
+                int ntopgeoms = d.SpaceGetNumGeoms(TopSpace);
+                int nbodies = d.NTotalBodies;
+                int ngeoms = d.NTotalGeoms;
+
+                // 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);
+                }
+                
+                // think time dilation is not a physics issue alone..  but ok let's fake something
+                if (step_time < ODE_STEPSIZE) // we did the required loops
+                    m_timeDilation = 1.0f;
+                else
+                { // we didn't forget the lost ones and let user know something
+                    m_timeDilation = 1 - step_time / timeStep;
+                    if (m_timeDilation < 0)
+                        m_timeDilation = 0;
+                    step_time = 0;
+                }
+            }
+
+//            return nodeframes * ODE_STEPSIZE; // return real simulated time
+            return 1000 * nodeframes; // return steps for now * 1000 to keep core happy
+        }
+
+        /// <summary>
+        public override void GetResults()
+        {
+        }
+
+        public override bool IsThreaded
+        {
+            // for now we won't be multithreaded
+            get { return (false); }
+        }
+
+        #region ODE Specific Terrain Fixes
+        public float[] ResizeTerrain512NearestNeighbour(float[] heightMap)
+        {
+            float[] returnarr = new float[262144];
+            float[,] resultarr = new float[(int)WorldExtents.X, (int)WorldExtents.Y];
+
+            // Filling out the array into its multi-dimensional components
+            for (int y = 0; y < WorldExtents.Y; y++)
+            {
+                for (int x = 0; x < WorldExtents.X; x++)
+                {
+                    resultarr[y, x] = heightMap[y * (int)WorldExtents.Y + x];
+                }
+            }
+
+            // Resize using Nearest Neighbour
+
+            // This particular way is quick but it only works on a multiple of the original
+
+            // The idea behind this method can be described with the following diagrams
+            // second pass and third pass happen in the same loop really..  just separated
+            // them to show what this does.
+
+            // First Pass
+            // ResultArr:
+            // 1,1,1,1,1,1
+            // 1,1,1,1,1,1
+            // 1,1,1,1,1,1
+            // 1,1,1,1,1,1
+            // 1,1,1,1,1,1
+            // 1,1,1,1,1,1
+
+            // Second Pass
+            // ResultArr2:
+            // 1,,1,,1,,1,,1,,1,
+            // ,,,,,,,,,,
+            // 1,,1,,1,,1,,1,,1,
+            // ,,,,,,,,,,
+            // 1,,1,,1,,1,,1,,1,
+            // ,,,,,,,,,,
+            // 1,,1,,1,,1,,1,,1,
+            // ,,,,,,,,,,
+            // 1,,1,,1,,1,,1,,1,
+            // ,,,,,,,,,,
+            // 1,,1,,1,,1,,1,,1,
+
+            // Third pass fills in the blanks
+            // ResultArr2:
+            // 1,1,1,1,1,1,1,1,1,1,1,1
+            // 1,1,1,1,1,1,1,1,1,1,1,1
+            // 1,1,1,1,1,1,1,1,1,1,1,1
+            // 1,1,1,1,1,1,1,1,1,1,1,1
+            // 1,1,1,1,1,1,1,1,1,1,1,1
+            // 1,1,1,1,1,1,1,1,1,1,1,1
+            // 1,1,1,1,1,1,1,1,1,1,1,1
+            // 1,1,1,1,1,1,1,1,1,1,1,1
+            // 1,1,1,1,1,1,1,1,1,1,1,1
+            // 1,1,1,1,1,1,1,1,1,1,1,1
+            // 1,1,1,1,1,1,1,1,1,1,1,1
+
+            // X,Y = .
+            // X+1,y = ^
+            // X,Y+1 = *
+            // X+1,Y+1 = #
+
+            // Filling in like this;
+            // .*
+            // ^#
+            // 1st .
+            // 2nd *
+            // 3rd ^
+            // 4th #
+            // on single loop.
+
+            float[,] resultarr2 = new float[512, 512];
+            for (int y = 0; y < WorldExtents.Y; y++)
+            {
+                for (int x = 0; x < WorldExtents.X; x++)
+                {
+                    resultarr2[y * 2, x * 2] = resultarr[y, x];
+
+                    if (y < WorldExtents.Y)
+                    {
+                        resultarr2[(y * 2) + 1, x * 2] = resultarr[y, x];
+                    }
+                    if (x < WorldExtents.X)
+                    {
+                        resultarr2[y * 2, (x * 2) + 1] = resultarr[y, x];
+                    }
+                    if (x < WorldExtents.X && y < WorldExtents.Y)
+                    {
+                        resultarr2[(y * 2) + 1, (x * 2) + 1] = resultarr[y, x];
+                    }
+                }
+            }
+
+            //Flatten out the array
+            int i = 0;
+            for (int y = 0; y < 512; y++)
+            {
+                for (int x = 0; x < 512; x++)
+                {
+                    if (resultarr2[y, x] <= 0)
+                        returnarr[i] = 0.0000001f;
+                    else
+                        returnarr[i] = resultarr2[y, x];
+
+                    i++;
+                }
+            }
+
+            return returnarr;
+        }
+
+        public float[] ResizeTerrain512Interpolation(float[] heightMap)
+        {
+            float[] returnarr = new float[262144];
+            float[,] resultarr = new float[512,512];
+
+            // Filling out the array into its multi-dimensional components
+            for (int y = 0; y < 256; y++)
+            {
+                for (int x = 0; x < 256; x++)
+                {
+                    resultarr[y, x] = heightMap[y * 256 + x];
+                }
+            }
+
+            // Resize using interpolation
+
+            // This particular way is quick but it only works on a multiple of the original
+
+            // The idea behind this method can be described with the following diagrams
+            // second pass and third pass happen in the same loop really..  just separated
+            // them to show what this does.
+
+            // First Pass
+            // ResultArr:
+            // 1,1,1,1,1,1
+            // 1,1,1,1,1,1
+            // 1,1,1,1,1,1
+            // 1,1,1,1,1,1
+            // 1,1,1,1,1,1
+            // 1,1,1,1,1,1
+
+            // Second Pass
+            // ResultArr2:
+            // 1,,1,,1,,1,,1,,1,
+            // ,,,,,,,,,,
+            // 1,,1,,1,,1,,1,,1,
+            // ,,,,,,,,,,
+            // 1,,1,,1,,1,,1,,1,
+            // ,,,,,,,,,,
+            // 1,,1,,1,,1,,1,,1,
+            // ,,,,,,,,,,
+            // 1,,1,,1,,1,,1,,1,
+            // ,,,,,,,,,,
+            // 1,,1,,1,,1,,1,,1,
+
+            // Third pass fills in the blanks
+            // ResultArr2:
+            // 1,1,1,1,1,1,1,1,1,1,1,1
+            // 1,1,1,1,1,1,1,1,1,1,1,1
+            // 1,1,1,1,1,1,1,1,1,1,1,1
+            // 1,1,1,1,1,1,1,1,1,1,1,1
+            // 1,1,1,1,1,1,1,1,1,1,1,1
+            // 1,1,1,1,1,1,1,1,1,1,1,1
+            // 1,1,1,1,1,1,1,1,1,1,1,1
+            // 1,1,1,1,1,1,1,1,1,1,1,1
+            // 1,1,1,1,1,1,1,1,1,1,1,1
+            // 1,1,1,1,1,1,1,1,1,1,1,1
+            // 1,1,1,1,1,1,1,1,1,1,1,1
+
+            // X,Y = .
+            // X+1,y = ^
+            // X,Y+1 = *
+            // X+1,Y+1 = #
+
+            // Filling in like this;
+            // .*
+            // ^#
+            // 1st .
+            // 2nd *
+            // 3rd ^
+            // 4th #
+            // on single loop.
+
+            float[,] resultarr2 = new float[512,512];
+            for (int y = 0; y < (int)Constants.RegionSize; y++)
+            {
+                for (int x = 0; x < (int)Constants.RegionSize; x++)
+                {
+                    resultarr2[y*2, x*2] = resultarr[y, x];
+
+                    if (y < (int)Constants.RegionSize)
+                    {
+                        if (y + 1 < (int)Constants.RegionSize)
+                        {
+                            if (x + 1 < (int)Constants.RegionSize)
+                            {
+                                resultarr2[(y*2) + 1, x*2] = ((resultarr[y, x] + resultarr[y + 1, x] +
+                                                               resultarr[y, x + 1] + resultarr[y + 1, x + 1])/4);
+                            }
+                            else
+                            {
+                                resultarr2[(y*2) + 1, x*2] = ((resultarr[y, x] + resultarr[y + 1, x])/2);
+                            }
+                        }
+                        else
+                        {
+                            resultarr2[(y*2) + 1, x*2] = resultarr[y, x];
+                        }
+                    }
+                    if (x < (int)Constants.RegionSize)
+                    {
+                        if (x + 1 < (int)Constants.RegionSize)
+                        {
+                            if (y + 1 < (int)Constants.RegionSize)
+                            {
+                                resultarr2[y*2, (x*2) + 1] = ((resultarr[y, x] + resultarr[y + 1, x] +
+                                                               resultarr[y, x + 1] + resultarr[y + 1, x + 1])/4);
+                            }
+                            else
+                            {
+                                resultarr2[y*2, (x*2) + 1] = ((resultarr[y, x] + resultarr[y, x + 1])/2);
+                            }
+                        }
+                        else
+                        {
+                            resultarr2[y*2, (x*2) + 1] = resultarr[y, x];
+                        }
+                    }
+                    if (x < (int)Constants.RegionSize && y < (int)Constants.RegionSize)
+                    {
+                        if ((x + 1 < (int)Constants.RegionSize) && (y + 1 < (int)Constants.RegionSize))
+                        {
+                            resultarr2[(y*2) + 1, (x*2) + 1] = ((resultarr[y, x] + resultarr[y + 1, x] +
+                                                                 resultarr[y, x + 1] + resultarr[y + 1, x + 1])/4);
+                        }
+                        else
+                        {
+                            resultarr2[(y*2) + 1, (x*2) + 1] = resultarr[y, x];
+                        }
+                    }
+                }
+            }
+            //Flatten out the array
+            int i = 0;
+            for (int y = 0; y < 512; y++)
+            {
+                for (int x = 0; x < 512; x++)
+                {
+                    if (Single.IsNaN(resultarr2[y, x]) || Single.IsInfinity(resultarr2[y, x]))
+                    {
+                        m_log.Warn("[PHYSICS]: Non finite heightfield element detected.  Setting it to 0");
+                        resultarr2[y, x] = 0;
+                    }
+                    returnarr[i] = resultarr2[y, x];
+                    i++;
+                }
+            }
+
+            return returnarr;
+        }
+
+        #endregion
+
+        public override void SetTerrain(float[] heightMap)
+        {
+            if (m_worldOffset != Vector3.Zero && m_parentScene != null)
+            {
+                if (m_parentScene is OdeScene)
+                {
+                    ((OdeScene)m_parentScene).SetTerrain(heightMap, m_worldOffset);
+                }
+            }
+            else
+            {
+                SetTerrain(heightMap, m_worldOffset);
+            }
+        }
+
+        public override void CombineTerrain(float[] heightMap, Vector3 pOffset)
+        {
+            SetTerrain(heightMap, pOffset);
+        }
+
+        public void SetTerrain(float[] heightMap, Vector3 pOffset)
+            {
+
+            float[] _heightmap;
+            _heightmap = new float[(((int)Constants.RegionSize + 2) * ((int)Constants.RegionSize + 2))];
+
+            uint heightmapWidth = Constants.RegionSize + 2;
+            uint heightmapHeight = Constants.RegionSize + 2;
+
+            uint heightmapWidthSamples;
+
+            uint heightmapHeightSamples;
+ 
+            heightmapWidthSamples = (uint)Constants.RegionSize + 2;
+            heightmapHeightSamples = (uint)Constants.RegionSize + 2;
+
+            const float scale = 1.0f;
+            const float offset = 0.0f;
+            const float thickness = 10f;
+            const int wrap = 0;
+
+            int regionsize = (int) Constants.RegionSize + 2;
+ 
+            float hfmin = float.MaxValue;
+            float hfmax = float.MinValue;
+            float val;
+            int xx;
+            int yy;
+
+            int maxXXYY = regionsize - 3;
+            // flipping map adding one margin all around so things don't fall in edges
+
+            int xt = 0;
+            xx = 0;
+
+            for (int x = 0; x < heightmapWidthSamples; x++)
+            {
+                if (x > 1 && xx < maxXXYY)
+                    xx++;
+                yy = 0;
+                for (int y = 0; y < heightmapHeightSamples; y++)
+                {
+                    if (y > 1 && y < maxXXYY)
+                        yy += (int)Constants.RegionSize;
+
+                    val = heightMap[yy + xx];
+                    _heightmap[xt + y] = val;
+
+                    if (hfmin > val)
+                        hfmin = val;
+                    if (hfmax < val)
+                        hfmax = val;
+
+                }
+
+                xt += regionsize;
+            }
+            lock (OdeLock)
+            {
+                IntPtr GroundGeom = IntPtr.Zero;
+                if (RegionTerrain.TryGetValue(pOffset, out GroundGeom))
+                {
+                    RegionTerrain.Remove(pOffset);
+                    if (GroundGeom != IntPtr.Zero)
+                    {
+                        if (TerrainHeightFieldHeights.ContainsKey(GroundGeom))
+                            {
+                            TerrainHeightFieldHeightsHandlers[GroundGeom].Free();
+                            TerrainHeightFieldHeightsHandlers.Remove(GroundGeom);
+                            TerrainHeightFieldHeights.Remove(GroundGeom);
+                            }
+                        d.SpaceRemove(StaticSpace, GroundGeom);
+                        d.GeomDestroy(GroundGeom);
+                    }
+                }
+                IntPtr HeightmapData = d.GeomHeightfieldDataCreate();
+
+                GCHandle _heightmaphandler = GCHandle.Alloc(_heightmap, GCHandleType.Pinned);
+
+                d.GeomHeightfieldDataBuildSingle(HeightmapData, _heightmaphandler.AddrOfPinnedObject(), 0, heightmapWidth , heightmapHeight,
+                                                 (int)heightmapWidthSamples, (int)heightmapHeightSamples, scale,
+                                                 offset, thickness, wrap);
+
+                d.GeomHeightfieldDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1);
+                GroundGeom = d.CreateHeightfield(StaticSpace, HeightmapData, 1);
+                if (GroundGeom != IntPtr.Zero)
+                {
+                    d.GeomSetCategoryBits(GroundGeom, (int)(CollisionCategories.Land));
+                    d.GeomSetCollideBits(GroundGeom, (int)(CollisionCategories.Space));
+
+                }
+                geom_name_map[GroundGeom] = "Terrain";
+
+                d.Matrix3 R = new d.Matrix3();
+
+                Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f);
+                Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f);
+                
+
+                q1 = q1 * q2;
+                
+                Vector3 v3;
+                float angle;
+                q1.GetAxisAngle(out v3, out angle);
+
+                d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle);
+                d.GeomSetRotation(GroundGeom, ref R);
+                d.GeomSetPosition(GroundGeom, pOffset.X + (float)Constants.RegionSize * 0.5f - 0.5f, pOffset.Y + (float)Constants.RegionSize * 0.5f - 0.5f, 0);
+                IntPtr testGround = IntPtr.Zero;
+                if (RegionTerrain.TryGetValue(pOffset, out testGround))
+                {
+                    RegionTerrain.Remove(pOffset);
+                }
+                RegionTerrain.Add(pOffset, GroundGeom, GroundGeom);
+//                TerrainHeightFieldHeights.Add(GroundGeom, ODElandMap);
+                TerrainHeightFieldHeights.Add(GroundGeom, _heightmap);
+                TerrainHeightFieldHeightsHandlers.Add(GroundGeom, _heightmaphandler);
+               
+            }
+        }
+
+        public override void DeleteTerrain()
+        {
+        }
+
+        public float GetWaterLevel()
+        {
+            return waterlevel;
+        }
+
+        public override bool SupportsCombining()
+        {
+            return true;
+        }
+/*
+        public override void UnCombine(PhysicsScene pScene)
+        {
+            IntPtr localGround = IntPtr.Zero;
+//            float[] localHeightfield;
+            bool proceed = false;
+            List<IntPtr> geomDestroyList = new List<IntPtr>();
+
+            lock (OdeLock)
+            {
+                if (RegionTerrain.TryGetValue(Vector3.Zero, out localGround))
+                {
+                    foreach (IntPtr geom in TerrainHeightFieldHeights.Keys)
+                    {
+                        if (geom == localGround)
+                        {
+//                            localHeightfield = TerrainHeightFieldHeights[geom];
+                            proceed = true;
+                        }
+                        else
+                        {
+                            geomDestroyList.Add(geom);
+                        }
+                    }
+
+                    if (proceed)
+                    {
+                        m_worldOffset = Vector3.Zero;
+                        WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize);
+                        m_parentScene = null;
+
+                        foreach (IntPtr g in geomDestroyList)
+                        {
+                            // removingHeightField needs to be done or the garbage collector will
+                            // collect the terrain data before we tell ODE to destroy it causing 
+                            // memory corruption
+                            if (TerrainHeightFieldHeights.ContainsKey(g))
+                            {
+//                                float[] removingHeightField = TerrainHeightFieldHeights[g];
+                                TerrainHeightFieldHeights.Remove(g);
+
+                                if (RegionTerrain.ContainsKey(g))
+                                {
+                                    RegionTerrain.Remove(g);
+                                }
+
+                                d.GeomDestroy(g);
+                                //removingHeightField = new float[0];
+                            }
+                        }
+
+                    }
+                    else
+                    {
+                        m_log.Warn("[PHYSICS]: Couldn't proceed with UnCombine.  Region has inconsistant data.");
+                    }
+                }
+            }
+        }
+*/
+        public override void SetWaterLevel(float baseheight)
+        {
+            waterlevel = baseheight;
+            randomizeWater(waterlevel);
+        }
+
+        public void randomizeWater(float baseheight)
+        {
+            const uint heightmapWidth = m_regionWidth + 2;
+            const uint heightmapHeight = m_regionHeight + 2;
+            const uint heightmapWidthSamples = m_regionWidth + 2;
+            const uint heightmapHeightSamples = m_regionHeight + 2;
+            const float scale = 1.0f;
+            const float offset = 0.0f;
+            const float thickness = 2.9f;
+            const int wrap = 0;
+
+            for (int i = 0; i < (258 * 258); i++)
+            {
+                _watermap[i] = (baseheight-0.1f) + ((float)fluidRandomizer.Next(1,9) / 10f);
+               // m_log.Info((baseheight - 0.1f) + ((float)fluidRandomizer.Next(1, 9) / 10f));
+            }
+
+            lock (OdeLock)
+            {
+                if (WaterGeom != IntPtr.Zero)
+                {
+                d.SpaceRemove(StaticSpace, WaterGeom);
+                }
+                IntPtr HeightmapData = d.GeomHeightfieldDataCreate();
+                d.GeomHeightfieldDataBuildSingle(HeightmapData, _watermap, 0, heightmapWidth, heightmapHeight,
+                                                 (int)heightmapWidthSamples, (int)heightmapHeightSamples, scale,
+                                                 offset, thickness, wrap);
+                d.GeomHeightfieldDataSetBounds(HeightmapData, m_regionWidth, m_regionHeight);
+                WaterGeom = d.CreateHeightfield(StaticSpace, HeightmapData, 1);
+                if (WaterGeom != IntPtr.Zero)
+                {
+                    d.GeomSetCategoryBits(WaterGeom, (int)(CollisionCategories.Water));
+                    d.GeomSetCollideBits(WaterGeom, (int)(CollisionCategories.Space));
+
+                }
+                geom_name_map[WaterGeom] = "Water";
+
+                d.Matrix3 R = new d.Matrix3();
+
+                Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f);
+                Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f);
+
+                q1 = q1 * q2;
+                Vector3 v3;
+                float angle;
+                q1.GetAxisAngle(out v3, out angle);
+
+                d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle);
+                d.GeomSetRotation(WaterGeom, ref R);
+                d.GeomSetPosition(WaterGeom, 128, 128, 0);
+                
+            }
+
+        }
+
+        public override void Dispose()
+        {
+            m_rayCastManager.Dispose();
+            m_rayCastManager = null;
+
+            lock (OdeLock)
+            {
+                lock (_prims)
+                {
+                    foreach (OdePrim prm in _prims)
+                    {
+                        RemovePrim(prm);
+                    }
+                }
+
+                if (ContactgeomsArray != IntPtr.Zero)
+                    Marshal.FreeHGlobal(ContactgeomsArray);
+                if (GlobalContactsArray != IntPtr.Zero)
+                    Marshal.FreeHGlobal(GlobalContactsArray);
+
+                d.WorldDestroy(world);
+                //d.CloseODE();
+            }
+        }
+
+        public override Dictionary<uint, float> GetTopColliders()
+        {
+            Dictionary<uint, float> returncolliders = new Dictionary<uint, float>();
+            int cnt = 0;
+            lock (_prims)
+            {
+                foreach (OdePrim prm in _prims)
+                {
+                    if (prm.CollisionScore > 0)
+                    {
+                        returncolliders.Add(prm.m_localID, prm.CollisionScore);
+                        cnt++;
+                        prm.CollisionScore = 0f;
+                        if (cnt > 25)
+                        {
+                            break;
+                        }
+                    }
+                }
+            }
+            return returncolliders;
+        }
+
+        public override bool SupportsRayCast()
+        {
+            return true;
+        }
+
+        public override void RaycastWorld(Vector3 position, Vector3 direction, float length, RaycastCallback retMethod)
+        {
+            if (retMethod != null)
+            {
+                m_rayCastManager.QueueRequest(position, direction, length, retMethod);
+            }
+        }
+
+        public override void RaycastWorld(Vector3 position, Vector3 direction, float length, int Count, RayCallback retMethod)
+        {
+            if (retMethod != null)
+            {
+                m_rayCastManager.QueueRequest(position, direction, length, Count, retMethod);
+            }
+        }
+
+        // don't like this
+        public override List<ContactResult> RaycastWorld(Vector3 position, Vector3 direction, float length, int Count)
+        {
+            ContactResult[] ourResults = null;
+            RayCallback retMethod = delegate(List<ContactResult> results)
+            {
+                ourResults = new ContactResult[results.Count];
+                results.CopyTo(ourResults, 0);
+            };
+            int waitTime = 0;
+            m_rayCastManager.QueueRequest(position, direction, length, Count, retMethod);
+            while (ourResults == null && waitTime < 1000)
+            {
+                Thread.Sleep(1);
+                waitTime++;
+            }
+            if (ourResults == null)
+                return new List<ContactResult>();
+            return new List<ContactResult>(ourResults);
+        }
+
+        public override void RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, RaycastCallback retMethod)
+        {
+            if (retMethod != null && actor !=null)
+            {
+                IntPtr geom;
+                if (actor is OdePrim)
+                    geom = ((OdePrim)actor).prim_geom;
+                else if (actor is OdeCharacter)
+                    geom = ((OdePrim)actor).prim_geom;
+                else
+                    return;
+                if (geom == IntPtr.Zero)
+                    return;
+                m_rayCastManager.QueueRequest(geom, position, direction, length, retMethod);
+            }
+        }
+
+        public override void RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count, RayCallback retMethod)
+        {
+            if (retMethod != null && actor != null)
+            {
+                IntPtr geom;
+                if (actor is OdePrim)
+                    geom = ((OdePrim)actor).prim_geom;
+                else if (actor is OdeCharacter)
+                    geom = ((OdePrim)actor).prim_geom;
+                else
+                    return;
+                if (geom == IntPtr.Zero)
+                    return;
+
+                m_rayCastManager.QueueRequest(geom,position, direction, length, Count, retMethod);
+            }
+        }
+
+        // don't like this
+        public override List<ContactResult> RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count)
+        {
+            if (actor != null)
+            {
+                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>();
+
+                ContactResult[] ourResults = null;
+                RayCallback retMethod = delegate(List<ContactResult> results)
+                {
+                    ourResults = new ContactResult[results.Count];
+                    results.CopyTo(ourResults, 0);
+                };
+                int waitTime = 0;
+                m_rayCastManager.QueueRequest(geom,position, direction, length, Count, retMethod);
+                while (ourResults == null && waitTime < 1000)
+                {
+                    Thread.Sleep(1);
+                    waitTime++;
+                }
+                if (ourResults == null)
+                    return new List<ContactResult>();
+                return new List<ContactResult>(ourResults);
+            }
+            return new List<ContactResult>();
+        }
+    }
+}