10 files changed, 13997 insertions, 0 deletions

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..f7e4c1c
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs
@@ -0,0 +1,1950 @@
+/*
+ * 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 Vector3 _velocity;
+        private Vector3 _target_velocity;
+        private Vector3 _acceleration;
+        private Vector3 m_rotationalVelocity;
+        private Vector3 m_size;
+        private Quaternion m_orientation;
+        private Quaternion m_orientation2D;
+        private float m_mass = 80f;
+        public float m_density = 60f;
+        private bool m_pidControllerActive = true;
+
+        const float basePID_D = 0.55f; // scaled for unit mass unit time (2200 /(50*80))
+        const float basePID_P = 0.225f; // scaled for unit mass unit time (900 /(50*80))
+        public float PID_D;
+        public float PID_P;
+
+        private float timeStep;
+        private float invtimeStep;
+
+        private float m_feetOffset = 0;
+        private float feetOff = 0;
+        private float feetSZ = 0.5f;
+        const float feetScale = 0.8f;
+        private float boneOff = 0;
+        private float m_lastVelocitySqr = 0;
+
+        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 bool _zeroFlag = false;
+
+        private int m_requestedUpdateFrequency = 0;
+        private uint m_localID = 0;
+        public bool m_returnCollisions = false;
+        // taints and their non-tainted counterparts
+        public bool m_isPhysical = false; // the current physical status
+        public float MinimumGroundFlightOffset = 3f;
+
+        private float m_buoyancy = 0f;
+
+        private bool m_freemove = false;
+        // private CollisionLocker ode;
+
+//        private string m_name = String.Empty;
+        // other filter control
+        int m_colliderfilter = 0;
+        int m_colliderGroundfilter = 0;
+        int m_colliderObjectfilter = 0;
+        bool m_collisionException = false;
+
+        // Default we're a Character
+        private CollisionCategories m_collisionCategories = (CollisionCategories.Character);
+
+        // Default, Collide with Other Geometries, spaces, bodies and characters.
+        private CollisionCategories m_collisionFlags = (CollisionCategories.Character
+                                                        | CollisionCategories.Geom
+                                                        | CollisionCategories.VolumeDtc
+                                                        );
+        // we do land collisions not ode                | CollisionCategories.Land);
+        public IntPtr Body = IntPtr.Zero;
+        private OdeScene _parent_scene;
+        private IntPtr topbox = IntPtr.Zero;
+        private IntPtr midbox = IntPtr.Zero;
+        private IntPtr feetbox = IntPtr.Zero;
+        private IntPtr bbox = IntPtr.Zero;
+        public IntPtr collider = IntPtr.Zero;
+
+        public IntPtr Amotor = IntPtr.Zero;
+
+        public d.Mass ShellMass;
+
+
+
+
+        public int m_eventsubscription = 0;
+        private int m_cureventsubscription = 0;
+        private CollisionEventUpdate CollisionEventsThisFrame = null;
+        private bool SentEmptyCollisionsEvent;
+
+        // unique UUID of this character object
+        public UUID m_uuid;
+        public bool bad = false;
+
+        float mu;
+
+        
+
+        public OdeCharacter(uint localID, String avName, OdeScene parent_scene, Vector3 pos, Vector3 pSize, float pfeetOffset, float density, float walk_divisor, float rundivisor)
+        {
+            m_uuid = UUID.Random();
+            m_localID = localID;
+
+            timeStep = parent_scene.ODE_STEPSIZE;
+            invtimeStep = 1 / timeStep;
+
+            if (pos.IsFinite())
+            {
+                if (pos.Z > 99999f)
+                {
+                    pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
+                }
+                if (pos.Z < -100f) // shouldn't this be 0 ?
+                {
+                    pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
+                }
+                _position = pos;
+            }
+            else
+            {
+                _position = new Vector3(((float)_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;
+
+
+            m_size.X = pSize.X;
+            m_size.Y = pSize.Y;
+            m_size.Z = pSize.Z;
+
+            if(m_size.X <0.01f)
+                m_size.X = 0.01f;
+            if(m_size.Y <0.01f)
+                m_size.Y = 0.01f;
+            if(m_size.Z <0.01f)
+                m_size.Z = 0.01f;
+
+            m_feetOffset = pfeetOffset;
+            m_orientation = Quaternion.Identity;
+            m_orientation2D = Quaternion.Identity;
+            m_density = density;
+
+            // force lower density for testing
+            m_density = 3.0f;
+
+            m_density *= 1.4f; // scale to have mass similar to capsule
+
+            mu = parent_scene.AvatarFriction;
+
+            walkDivisor = walk_divisor;
+            runDivisor = rundivisor;
+
+            m_mass = m_density * m_size.X * m_size.Y * m_size.Z; ; // sure we have a default           
+
+            PID_D = basePID_D * m_mass * invtimeStep;
+            PID_P = basePID_P * m_mass * invtimeStep;
+
+            m_isPhysical = false; // current status: no ODE information exists
+
+            Name = avName;
+
+            AddChange(changes.Add, null);
+        }
+
+        public override int PhysicsActorType
+        {
+            get { return (int)ActorTypes.Agent; }
+            set { return; }
+        }
+
+        public override void getContactData(ref ContactData cdata)
+        {
+            cdata.mu = mu;
+            cdata.bounce = 0;
+            cdata.softcolide = false;
+        }
+
+        public override bool Building { get; set; }
+
+        /// <summary>
+        /// If this is set, the avatar will move faster
+        /// </summary>
+        public override bool SetAlwaysRun
+        {
+            get { return m_alwaysRun; }
+            set { m_alwaysRun = value; }
+        }
+
+        public override uint LocalID
+        {
+            get { return m_localID; }     
+            set { m_localID = value; }
+        }
+
+        public override PhysicsActor ParentActor
+        {
+            get { return (PhysicsActor)this; }
+        }
+
+        public override bool Grabbed
+        {
+            set { return; }
+        }
+
+        public override bool Selected
+        {
+            set { return; }
+        }
+
+        public override float Buoyancy
+        {
+            get { return m_buoyancy; }
+            set { m_buoyancy = value; }
+        }
+
+        public override bool FloatOnWater
+        {
+            set { return; }
+        }
+
+        public override bool IsPhysical
+        {
+            get { return m_isPhysical; }
+            set { return; }
+        }
+
+        public override bool ThrottleUpdates
+        {
+            get { return false; }
+            set { return; }
+        }
+
+        public override bool Flying
+        {
+            get { return 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
+                {
+                    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 (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
+            {
+                return m_size;
+            }
+            set
+            {
+                if (value.IsFinite())
+                {
+                    if(value.X <0.01f)
+                        value.X = 0.01f;
+                    if(value.Y <0.01f)
+                        value.Y = 0.01f;
+                    if(value.Z <0.01f)
+                        value.Z = 0.01f;
+
+                    AddChange(changes.Size, value);
+                }
+                else
+                {
+                    m_log.Warn("[PHYSICS]: Got a NaN Size from Scene on a Character");
+                }
+            }
+        }
+
+        public override void setAvatarSize(Vector3 size, float feetOffset)
+        {
+            if (size.IsFinite())
+            {
+                if (size.X < 0.01f)
+                    size.X = 0.01f;
+                if (size.Y < 0.01f)
+                    size.Y = 0.01f;
+                if (size.Z < 0.01f)
+                    size.Z = 0.01f;
+
+                strAvatarSize st = new strAvatarSize();
+                st.size = size;
+                st.offset = feetOffset;
+                AddChange(changes.AvatarSize, st);
+            }
+            else
+            {
+                m_log.Warn("[PHYSICS]: Got a NaN AvatarSize from Scene on a Character");
+            }
+            
+        }
+        /// <summary>
+        /// This creates the Avatar's physical Surrogate at the position supplied
+        /// </summary>
+        /// <param name="npositionX"></param>
+        /// <param name="npositionY"></param>
+        /// <param name="npositionZ"></param>
+
+        //
+        /// <summary>
+        /// Uses the capped cyllinder volume formula to calculate the avatar's mass.
+        /// This may be used in calculations in the scene/scenepresence
+        /// </summary>
+        public override float Mass
+        {
+            get
+            {
+                return m_mass;
+            }
+        }
+        public override void link(PhysicsActor obj)
+        {
+
+        }
+
+        public override void delink()
+        {
+
+        }
+
+        public override void LockAngularMotion(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;
+            }
+        }
+
+        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 m_orientation; }
+            set
+            {
+//                fakeori = value;
+//                givefakeori++;
+                value.Normalize();
+                AddChange(changes.Orientation, value);
+            }
+        }
+
+        public override Vector3 Acceleration
+        {
+            get { return _acceleration; }
+            set { }
+        }
+
+        public void SetAcceleration(Vector3 accel)
+        {
+            m_pidControllerActive = true;
+            _acceleration = accel;
+        }
+
+        /// <summary>
+        /// Adds the force supplied to the Target Velocity
+        /// The PID controller takes this target velocity and tries to make it a reality
+        /// </summary>
+        /// <param name="force"></param>
+        public override void AddForce(Vector3 force, bool pushforce)
+        {
+            if (force.IsFinite())
+            {
+                if (pushforce)
+                {
+                    AddChange(changes.Force, force * m_density / (_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)
+        {
+            if (momentum.IsFinite())
+                AddChange(changes.Momentum, momentum);
+        }
+
+        private void ajustCollider()
+        {
+            float vq = _velocity.LengthSquared();
+            if (m_lastVelocitySqr != vq)
+            {
+                m_lastVelocitySqr = vq;
+                if (vq > 100.0f)
+                {
+                    Vector3 off = _velocity;
+                    float t = 0.5f * timeStep;
+                    off = off * t;
+                    d.Quaternion qtmp;
+                    d.GeomCopyQuaternion(bbox, out qtmp);
+                    Quaternion q;
+                    q.X = qtmp.X;
+                    q.Y = qtmp.Y;
+                    q.Z = qtmp.Z;
+                    q.W = qtmp.W;
+                    off *= Quaternion.Conjugate(q);
+
+                    d.GeomSetOffsetPosition(bbox, off.X, off.Y, off.Z);
+
+                    off.X = 2.0f * (m_size.X + Math.Abs(off.X));
+                    off.Y = 2.0f * (m_size.Y + Math.Abs(off.Y));
+                    off.Z = m_size.Z + 2.0f * Math.Abs(off.Z);
+                    d.GeomBoxSetLengths(bbox, off.X, off.Y, off.Z);
+
+                    d.GeomSetCategoryBits(bbox, (uint)m_collisionCategories);
+                    d.GeomSetCollideBits(bbox, (uint)m_collisionFlags);
+                    d.GeomSetCategoryBits(topbox, 0);
+                    d.GeomSetCollideBits(topbox, 0);
+                    d.GeomSetCategoryBits(midbox, 0);
+                    d.GeomSetCollideBits(midbox, 0);
+                    d.GeomSetCategoryBits(feetbox, 0);
+                    d.GeomSetCollideBits(feetbox, 0);
+                }
+                else
+                {
+                    d.GeomSetCategoryBits(bbox, 0);
+                    d.GeomSetCollideBits(bbox, 0);
+                    d.GeomSetCategoryBits(topbox, (uint)m_collisionCategories);
+                    d.GeomSetCollideBits(topbox, (uint)m_collisionFlags);
+                    d.GeomSetCategoryBits(midbox, (uint)m_collisionCategories);
+                    d.GeomSetCollideBits(midbox, (uint)m_collisionFlags);
+                    d.GeomSetCategoryBits(feetbox, (uint)m_collisionCategories);
+                    d.GeomSetCollideBits(feetbox, (uint)m_collisionFlags);
+                }
+                uint cat1 = d.GeomGetCategoryBits(bbox);
+                uint col1 = d.GeomGetCollideBits(bbox);
+
+            }
+        }
+
+        private void AvatarGeomAndBodyCreation(float npositionX, float npositionY, float npositionZ)
+        {
+            // sizes  one day should came from visual parameters
+            float sx = m_size.X;
+            float sy = m_size.Y;
+            float sz = m_size.Z;
+
+            float topsx = sx * 0.9f;
+            float midsx = sx;
+            float feetsx = sx * feetScale;
+            float bonesx = sx * 0.2f;
+
+            float topsy = sy * 0.4f;
+            float midsy = sy;
+            float feetsy = sy * feetScale * 0.8f;
+            float bonesy = feetsy * 0.2f;
+
+            float topsz = sz * 0.15f;
+            float feetsz = sz * 0.45f;
+            if (feetsz > 0.6f)
+                feetsz = 0.6f;
+
+            float midsz = sz - topsz - feetsz;
+            float bonesz = sz;
+
+            float bot = -sz * 0.5f + m_feetOffset;
+
+            boneOff = bot + 0.3f;
+
+            float feetz = bot + feetsz * 0.5f;
+            bot += feetsz;
+
+            feetOff = bot;
+            feetSZ = feetsz;
+
+            float midz = bot + midsz * 0.5f;
+            bot += midsz;
+            float topz = bot + topsz * 0.5f;
+
+            _parent_scene.waitForSpaceUnlock(_parent_scene.CharsSpace);
+
+            collider = d.HashSpaceCreate(_parent_scene.CharsSpace);
+            d.HashSpaceSetLevels(collider, -4, 3);
+            d.SpaceSetSublevel(collider, 3);
+            d.SpaceSetCleanup(collider, false);
+            d.GeomSetCategoryBits(collider, (uint)m_collisionCategories);
+            d.GeomSetCollideBits(collider, (uint)m_collisionFlags);
+
+            feetbox = d.CreateBox(collider, feetsx, feetsy, feetsz);
+            midbox = d.CreateBox(collider, midsx, midsy, midsz);
+            topbox = d.CreateBox(collider, topsx, topsy, topsz);
+            bbox = d.CreateBox(collider, m_size.X, m_size.Y, m_size.Z);
+
+            m_mass = m_density * m_size.X * m_size.Y * m_size.Z;  // update mass
+
+            d.MassSetBoxTotal(out ShellMass, m_mass, m_size.X, m_size.Y, m_size.Z);
+
+            PID_D = basePID_D * m_mass / _parent_scene.ODE_STEPSIZE;
+            PID_P = basePID_P * m_mass / _parent_scene.ODE_STEPSIZE;
+            
+            Body = d.BodyCreate(_parent_scene.world);
+
+            _zeroFlag = false;
+            m_collisionException = false;
+            m_pidControllerActive = true;
+            m_freemove = false;
+
+            _velocity = Vector3.Zero;
+            m_lastVelocitySqr = 0;
+
+            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(feetbox, Body);
+            d.GeomSetBody(midbox, Body);
+            d.GeomSetBody(topbox, Body);
+            d.GeomSetBody(bbox, Body);
+
+            d.GeomSetOffsetPosition(feetbox, 0, 0, feetz);
+            d.GeomSetOffsetPosition(midbox, 0, 0, midz);
+            d.GeomSetOffsetPosition(topbox, 0, 0, topz);
+
+            ajustCollider();
+
+
+            // 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, 5e8f);
+            d.JointSetAMotorParam(Amotor, (int)dParam.FMax2, 5e8f);
+            d.JointSetAMotorParam(Amotor, (int)dParam.FMax3, 5e8f);
+        }
+
+        /// <summary>
+        /// Destroys the avatar body and geom
+
+        private void AvatarGeomAndBodyDestroy()
+        {
+            // Kill the Amotor
+            if (Amotor != IntPtr.Zero)
+            {
+                d.JointDestroy(Amotor);
+                Amotor = IntPtr.Zero;
+            }
+
+            if (Body != IntPtr.Zero)
+            {
+                //kill the body
+                d.BodyDestroy(Body);
+                Body = IntPtr.Zero;
+            }
+
+            //kill the Geoms
+            if (topbox != IntPtr.Zero)
+            {
+                _parent_scene.actor_name_map.Remove(topbox);
+                _parent_scene.waitForSpaceUnlock(collider);
+                d.GeomDestroy(topbox);
+                topbox = IntPtr.Zero;
+            }
+            if (midbox != IntPtr.Zero)
+            {
+                _parent_scene.actor_name_map.Remove(midbox);
+                _parent_scene.waitForSpaceUnlock(collider);
+                d.GeomDestroy(midbox);
+                midbox = IntPtr.Zero;
+            }
+            if (feetbox != IntPtr.Zero)
+            {
+                _parent_scene.actor_name_map.Remove(feetbox);
+                _parent_scene.waitForSpaceUnlock(collider);
+                d.GeomDestroy(feetbox);
+                feetbox = IntPtr.Zero;
+            }
+
+            if (bbox != IntPtr.Zero)
+            {
+                _parent_scene.actor_name_map.Remove(bbox);
+                _parent_scene.waitForSpaceUnlock(collider);
+                d.GeomDestroy(bbox);
+                bbox = IntPtr.Zero;
+            }
+
+            if (collider != IntPtr.Zero)
+            {
+                d.SpaceDestroy(collider);
+                collider = IntPtr.Zero;
+            }
+
+        }
+
+        //in place 2D rotation around Z assuming rot is normalised and is a rotation around Z
+        public void RotateXYonZ(ref float x, ref float y, ref Quaternion rot)
+        {
+            float sin = 2.0f * rot.Z * rot.W;
+            float cos = rot.W * rot.W - rot.Z * rot.Z;
+            float tx = x;
+
+            x = tx * cos - y * sin;
+            y = tx * sin + y * cos;
+        }
+        public void RotateXYonZ(ref float x, ref float y, ref float sin, ref float cos)
+        {
+            float tx = x;
+            x = tx * cos - y * sin;
+            y = tx * sin + y * cos;
+        }
+        public void invRotateXYonZ(ref float x, ref float y, ref float sin, ref float cos)
+        {
+            float tx = x;
+            x = tx * cos + y * sin;
+            y = -tx * sin + y * cos;
+        }
+
+        public void invRotateXYonZ(ref float x, ref float y, ref Quaternion rot)
+        {
+            float sin = - 2.0f * rot.Z * rot.W;
+            float cos = rot.W * rot.W - rot.Z * rot.Z;
+            float tx = x;
+
+            x = tx * cos - y * sin;
+            y = tx * sin + y * cos;
+        }
+
+
+        public bool Collide(IntPtr me, bool reverse, ref d.ContactGeom contact, ref bool feetcollision)
+        {
+            feetcollision = false;
+            if (m_collisionException)
+                return false;
+
+            Vector3 offset;
+
+            if (me == bbox) // if moving fast
+            {
+                // force a full inelastic collision
+                m_collisionException = true;
+
+                offset = m_size * m_orientation2D;
+
+                offset.X = (float)Math.Abs(offset.X) * 0.5f + contact.depth;
+                offset.Y = (float)Math.Abs(offset.Y) * 0.5f + contact.depth;
+                offset.Z = (float)Math.Abs(offset.Z) * 0.5f + contact.depth;
+
+                if (reverse)
+                {
+                    offset.X *= -contact.normal.X;
+                    offset.Y *= -contact.normal.Y;
+                    offset.Z *= -contact.normal.Z;
+                }
+                else
+                {
+                    offset.X *= contact.normal.X;
+                    offset.Y *= contact.normal.Y;
+                    offset.Z *= contact.normal.Z;
+                }
+
+                offset.X += contact.pos.X;
+                offset.Y += contact.pos.Y;
+                offset.Z += contact.pos.Z;
+
+                _position = offset;
+                return false;
+            }
+
+            offset.X = contact.pos.X - _position.X;
+            offset.Y = contact.pos.Y - _position.Y;
+
+            if (me == topbox)
+            {
+                offset.Z = contact.pos.Z - _position.Z;
+
+                offset.Normalize();
+
+                if (reverse)
+                {
+                    contact.normal.X = offset.X;
+                    contact.normal.Y = offset.Y;
+                    contact.normal.Z = offset.Z;
+                }
+                else
+                {
+                    contact.normal.X = -offset.X;
+                    contact.normal.Y = -offset.Y;
+                    contact.normal.Z = -offset.Z;
+                }
+                return true;
+            }
+
+            if (me == midbox)
+            {
+                if (Math.Abs(contact.normal.Z) > 0.95f)
+                    offset.Z = contact.pos.Z - _position.Z;
+                else
+                    offset.Z = contact.normal.Z;
+
+                offset.Normalize();
+
+                if (reverse)
+                {
+                    contact.normal.X = offset.X;
+                    contact.normal.Y = offset.Y;
+                    contact.normal.Z = offset.Z;
+                }
+                else
+                {
+                    contact.normal.X = -offset.X;
+                    contact.normal.Y = -offset.Y;
+                    contact.normal.Z = -offset.Z;
+                }
+
+                return true;
+            }
+
+            else if (me == feetbox)
+            {
+                float h = contact.pos.Z - _position.Z;
+
+                if (Math.Abs(contact.normal.Z) > 0.95f)
+                {
+                    if (contact.normal.Z > 0)
+                        contact.normal.Z = 1.0f;
+                    else
+                        contact.normal.Z = -1.0f;
+                    contact.normal.X = 0.0f;
+                    contact.normal.Y = 0.0f;
+                    feetcollision = true;
+                    if (h < boneOff)
+                        IsColliding = true;
+                    return true;
+                }
+
+                offset.Z = h - feetOff; // distance from top of feetbox
+
+                if (offset.Z > 0)
+                    return false;
+
+                if (offset.Z > -0.01)
+                {
+                    offset.X = 0;
+                    offset.Y = 0;
+                    offset.Z = -1.0f;
+                }
+                else
+                {
+                    offset.Normalize();
+                }
+
+                if (reverse)
+                {
+                    contact.normal.X = offset.X;
+                    contact.normal.Y = offset.Y;
+                    contact.normal.Z = offset.Z;
+                }
+                else
+                {
+                    contact.normal.X = -offset.X;
+                    contact.normal.Y = -offset.Y;
+                    contact.normal.Z = -offset.Z;
+                }
+                feetcollision = true;
+                if (h < boneOff)
+                    IsColliding = true;
+            }
+            else
+                return false;
+
+            return true;
+        }
+
+        /// <summary>
+        /// Called from Simulate
+        /// This is the avatar's movement control + PID Controller
+        /// </summary>
+        /// <param name="timeStep"></param>
+        public void Move(List<OdeCharacter> defects)
+        {
+            if (Body == IntPtr.Zero)
+                return;
+
+            if (m_collisionException)
+            {
+                d.BodySetPosition(Body,_position.X, _position.Y, _position.Z);
+                d.BodySetLinearVel(Body, 0, 0, 0);
+
+                float v = _velocity.Length();
+                if (v != 0)
+                {
+                    v = 5.0f / v;
+                    _velocity = _velocity * v;
+                    d.BodySetLinearVel(Body, _velocity.X, _velocity.Y, _velocity.Z);
+                }
+                ajustCollider();
+                m_collisionException = false;
+                return;
+            }
+
+            d.Vector3 dtmp = d.BodyGetPosition(Body);
+            Vector3 localpos = new Vector3(dtmp.X, dtmp.Y, dtmp.Z);
+
+            // the Amotor still lets avatar rotation to drift during colisions
+            // so force it back to identity
+
+            d.Quaternion qtmp;
+            qtmp.W = m_orientation2D.W;
+            qtmp.X = m_orientation2D.X;
+            qtmp.Y = m_orientation2D.Y;
+            qtmp.Z = m_orientation2D.Z;
+            d.BodySetQuaternion(Body, ref qtmp);
+
+            if (m_pidControllerActive == false)
+            {
+                _zeroPosition = localpos;
+            }
+
+            if (!localpos.IsFinite())
+            {
+                m_log.Warn("[PHYSICS]: Avatar Position is non-finite!");
+                defects.Add(this);
+                // _parent_scene.RemoveCharacter(this);
+
+                // destroy avatar capsule and related ODE data
+                AvatarGeomAndBodyDestroy();
+                return;
+            }
+
+            // check outbounds forcing to be in world
+            bool fixbody = false;
+            if (localpos.X < 0.0f)
+            {
+                fixbody = true;
+                localpos.X = 0.1f;
+            }
+            else if (localpos.X > _parent_scene.WorldExtents.X - 0.1f)
+            {
+                fixbody = true;
+                localpos.X = _parent_scene.WorldExtents.X - 0.1f;
+            }
+            if (localpos.Y < 0.0f)
+            {
+                fixbody = true;
+                localpos.Y = 0.1f;
+            }
+            else if (localpos.Y > _parent_scene.WorldExtents.Y - 0.1)
+            {
+                fixbody = true;
+                localpos.Y = _parent_scene.WorldExtents.Y - 0.1f;
+            }
+            if (fixbody)
+            {
+                m_freemove = false;
+                d.BodySetPosition(Body, localpos.X, localpos.Y, localpos.Z);
+            }
+
+            float breakfactor;
+
+            Vector3 vec = Vector3.Zero;
+            dtmp = d.BodyGetLinearVel(Body);
+            Vector3 vel = new Vector3(dtmp.X, dtmp.Y, dtmp.Z);
+            float velLengthSquared = vel.LengthSquared();
+
+            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(feetbox, out aabb);
+            float chrminZ = aabb.MinZ; ; // move up a bit
+            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;
+
+                if (depth < 0.1f)
+                {
+                    m_colliderGroundfilter++;
+                    if (m_colliderGroundfilter > 2)
+                    {
+                        m_iscolliding = true;
+                        m_colliderfilter = 2;
+
+                        if (m_colliderGroundfilter > 10)
+                        {
+                            m_colliderGroundfilter = 10;
+                            m_freemove = false;
+                        }
+
+                        m_iscollidingGround = true;
+
+                        ContactPoint contact = new ContactPoint();
+                        contact.PenetrationDepth = depth;
+                        contact.Position.X = localpos.X;
+                        contact.Position.Y = localpos.Y;
+                        contact.Position.Z = terrainheight;
+                        contact.SurfaceNormal.X = 0.0f;
+                        contact.SurfaceNormal.Y = 0.0f;
+                        contact.SurfaceNormal.Z = -1f;
+                        contact.RelativeSpeed = -vel.Z;
+                        contact.CharacterFeet = true;
+                        AddCollisionEvent(0, contact);
+
+                        vec.Z *= 0.5f;
+                    }
+                }
+
+                else
+                {
+                    m_colliderGroundfilter = 0;
+                    m_iscollidingGround = false;
+                }
+            }
+            else
+            {
+                m_colliderGroundfilter = 0;
+                m_iscollidingGround = false;
+            }
+
+            
+            //******************************************
+
+            bool tviszero = (_target_velocity.X == 0.0f && _target_velocity.Y == 0.0f && _target_velocity.Z == 0.0f);
+
+            //            if (!tviszero || m_iscolliding || velLengthSquared <0.01)
+            if (!tviszero)
+                m_freemove = false;
+
+            if (!m_freemove)
+            {
+
+                //  if velocity is zero, use position control; otherwise, velocity control
+                if (tviszero && 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 (velLengthSquared > 2500.0f) // 50m/s apply breaks
+                {
+                    breakfactor = 0.16f * m_mass;
+                    vec.X -= breakfactor * vel.X;
+                    vec.Y -= breakfactor * vel.Y;
+                    vec.Z -= breakfactor * vel.Z;
+                }
+            }
+            else
+            {
+                breakfactor = m_mass;
+                vec.X -= breakfactor * vel.X;
+                vec.Y -= breakfactor * vel.Y;
+                if (flying)
+                    vec.Z -= breakfactor * vel.Z;
+                else
+                    vec.Z -= .5f* m_mass * vel.Z;
+            }
+
+            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();
+                return;
+            }
+
+            // update our local ideia of position velocity and aceleration
+            //            _position = localpos;
+            _position = localpos;
+
+            if (_zeroFlag)
+            {
+                _velocity = Vector3.Zero;
+                _acceleration = Vector3.Zero;
+                m_rotationalVelocity = Vector3.Zero;
+            }
+            else
+            {
+                Vector3 a =_velocity; // previus velocity
+                SetSmooth(ref _velocity, ref vel, 2);
+                a = (_velocity - a) * invtimeStep;
+                SetSmooth(ref _acceleration, ref a, 2);
+
+                dtmp = d.BodyGetAngularVel(Body);
+                m_rotationalVelocity.X = 0f;
+                m_rotationalVelocity.Y = 0f;
+                m_rotationalVelocity.Z = dtmp.Z;
+                Math.Round(m_rotationalVelocity.Z,3);
+            }
+            ajustCollider();
+        }
+
+        public void round(ref Vector3 v, int digits)
+        {
+            v.X = (float)Math.Round(v.X, digits);
+            v.Y = (float)Math.Round(v.Y, digits);
+            v.Z = (float)Math.Round(v.Z, digits);
+         }
+
+        public void SetSmooth(ref Vector3 dst, ref Vector3 value)
+        {
+            dst.X = 0.1f * dst.X + 0.9f * value.X;
+            dst.Y = 0.1f * dst.Y + 0.9f * value.Y;
+            dst.Z = 0.1f * dst.Z + 0.9f * value.Z;
+        }
+
+        public void SetSmooth(ref Vector3 dst, ref Vector3 value, int rounddigits)
+        {
+            dst.X = 0.4f * dst.X + 0.6f * value.X;
+            dst.X = (float)Math.Round(dst.X, rounddigits);
+
+            dst.Y = 0.4f * dst.Y + 0.6f * value.Y;
+            dst.Y = (float)Math.Round(dst.Y, rounddigits);
+
+            dst.Z = 0.4f * dst.Z + 0.6f * value.Z;
+            dst.Z = (float)Math.Round(dst.Z, rounddigits);
+        }
+
+
+        /// <summary>
+        /// Updates the reported position and velocity.
+        /// Used to copy variables from unmanaged space at heartbeat rate and also trigger scene updates acording
+        /// also outbounds checking
+        /// copy and outbounds now done in move(..) at ode rate
+        /// 
+        /// </summary>
+        public void UpdatePositionAndVelocity()
+        {
+            return;
+
+//            if (Body == IntPtr.Zero)
+//                return;
+
+        }
+
+        /// <summary>
+        /// Cleanup the things we use in the scene.
+        /// </summary>
+        public void Destroy()
+        {
+            AddChange(changes.Remove, null);
+        }
+
+        public override void CrossingFailure()
+        {
+        }
+
+        public override Vector3 PIDTarget { set { return; } }
+        public override bool PIDActive { 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_eventsubscription = ms;
+            m_cureventsubscription = 0;
+            if (CollisionEventsThisFrame == null)
+                CollisionEventsThisFrame = new CollisionEventUpdate();
+            SentEmptyCollisionsEvent = false;
+        }
+
+        public override void UnSubscribeEvents()
+        {
+            if (CollisionEventsThisFrame != null)
+            {
+                lock (CollisionEventsThisFrame)
+                {
+                    CollisionEventsThisFrame.Clear();
+                    CollisionEventsThisFrame = null;
+                }
+            }
+            m_eventsubscription = 0;
+        }
+
+        public override void AddCollisionEvent(uint CollidedWith, ContactPoint contact)
+        {
+            if (CollisionEventsThisFrame == null)
+                CollisionEventsThisFrame = new CollisionEventUpdate();
+            lock (CollisionEventsThisFrame)
+            {
+                CollisionEventsThisFrame.AddCollider(CollidedWith, contact);
+                _parent_scene.AddCollisionEventReporting(this);
+            }
+        }
+
+        public void SendCollisions()
+        {
+            if (CollisionEventsThisFrame == null)
+                return;
+
+            lock (CollisionEventsThisFrame)
+            {
+                if (m_cureventsubscription < m_eventsubscription)
+                    return;
+
+                m_cureventsubscription = 0;
+
+                int ncolisions = CollisionEventsThisFrame.m_objCollisionList.Count;
+
+                if (!SentEmptyCollisionsEvent || ncolisions > 0)
+                {
+                    base.SendCollisionUpdate(CollisionEventsThisFrame);
+
+                    if (ncolisions == 0)
+                    {
+                        SentEmptyCollisionsEvent = true;
+                        _parent_scene.RemoveCollisionEventReporting(this);
+                    }
+                    else
+                    {
+                        SentEmptyCollisionsEvent = false;
+                        CollisionEventsThisFrame.Clear();
+                    }
+                }
+            }           
+        }
+
+        internal void AddCollisionFrameTime(int t)
+        {
+            // protect it from overflow crashing
+            if (m_cureventsubscription < 50000)
+                m_cureventsubscription += t;
+        }
+
+        public override bool SubscribedEvents()
+        {
+            if (m_eventsubscription > 0)
+                return true;
+            return false;
+        }
+
+        private void changePhysicsStatus(bool NewStatus)
+        {
+            if (NewStatus != m_isPhysical)
+            {
+                if (NewStatus)
+                {
+                    AvatarGeomAndBodyDestroy();
+
+                    AvatarGeomAndBodyCreation(_position.X, _position.Y, _position.Z);
+
+                    _parent_scene.actor_name_map[collider] = (PhysicsActor)this;
+                    _parent_scene.actor_name_map[feetbox] = (PhysicsActor)this;
+                    _parent_scene.actor_name_map[midbox] = (PhysicsActor)this;
+                    _parent_scene.actor_name_map[topbox] = (PhysicsActor)this;
+                    _parent_scene.actor_name_map[bbox] = (PhysicsActor)this;
+                    _parent_scene.AddCharacter(this);
+                }
+                else
+                {
+                    _parent_scene.RemoveCollisionEventReporting(this);
+                    _parent_scene.RemoveCharacter(this);
+                    // destroy avatar capsule and related ODE data
+                    AvatarGeomAndBodyDestroy();
+                }
+                m_freemove = false;
+                m_isPhysical = NewStatus;
+            }
+        }
+
+        private void changeAdd()
+        {
+            changePhysicsStatus(true);
+        }
+
+        private void changeRemove()
+        {
+            changePhysicsStatus(false);
+        }
+
+        private void changeShape(PrimitiveBaseShape arg)
+        {
+        }
+
+        private void changeAvatarSize(strAvatarSize st)
+        {
+            m_feetOffset = st.offset;
+            changeSize(st.size);
+        }
+
+        private void changeSize(Vector3 pSize)
+        {
+            if (pSize.IsFinite())
+            {
+                // for now only look to Z changes since viewers also don't change X and Y
+                if (pSize.Z != m_size.Z)
+                {
+                    AvatarGeomAndBodyDestroy();
+
+
+                    float oldsz = m_size.Z;
+                    m_size = pSize;
+
+
+                    AvatarGeomAndBodyCreation(_position.X, _position.Y,
+                                      _position.Z + (m_size.Z - oldsz) * 0.5f);
+
+                    Velocity = Vector3.Zero;
+                    
+
+                    _parent_scene.actor_name_map[collider] = (PhysicsActor)this;
+                    _parent_scene.actor_name_map[feetbox] = (PhysicsActor)this;
+                    _parent_scene.actor_name_map[midbox] = (PhysicsActor)this;
+                    _parent_scene.actor_name_map[topbox] = (PhysicsActor)this;
+                    _parent_scene.actor_name_map[bbox] = (PhysicsActor)this;
+                }
+                m_freemove = false;
+                m_collisionException = false;
+                m_pidControllerActive = true;
+            }
+            else
+            {
+                m_log.Warn("[PHYSICS]: Got a NaN Size from Scene on a Character");
+            }
+        }
+
+        private void changePosition( Vector3 newPos)
+            {
+                if (Body != IntPtr.Zero)
+                    d.BodySetPosition(Body, newPos.X, newPos.Y, newPos.Z);
+                _position = newPos;
+                m_freemove = false;
+                m_pidControllerActive = true;               
+            }
+
+        private void changeOrientation(Quaternion newOri)
+        {
+            if (m_orientation != newOri)
+            {
+                m_orientation = newOri; // keep a copy for core use
+                // but only use rotations around Z
+
+                m_orientation2D.W = newOri.W;
+                m_orientation2D.Z = newOri.Z;
+
+                float t = m_orientation2D.W * m_orientation2D.W + m_orientation2D.Z * m_orientation2D.Z;
+                if (t > 0)
+                {
+                    t = 1.0f / (float)Math.Sqrt(t);
+                    m_orientation2D.W *= t;
+                    m_orientation2D.Z *= t;
+                }
+                else
+                {
+                    m_orientation2D.W = 1.0f;
+                    m_orientation2D.Z = 0f;
+                }
+                m_orientation2D.Y = 0f;
+                m_orientation2D.X = 0f;
+
+                d.Quaternion myrot = new d.Quaternion();
+                myrot.X = m_orientation2D.X;
+                myrot.Y = m_orientation2D.Y;
+                myrot.Z = m_orientation2D.Z;
+                myrot.W = m_orientation2D.W;
+                d.BodySetQuaternion(Body, ref myrot);
+            }
+        }
+
+        private void changeVelocity(Vector3 newVel)
+        {
+            m_pidControllerActive = true;
+            m_freemove = false;
+            _target_velocity = newVel;
+        }
+
+        private void changeSetTorque(Vector3 newTorque)
+        {
+        }                                 
+
+        private void changeAddForce(Vector3 newForce)
+        {
+        }                                 
+
+        private void changeAddAngularForce(Vector3 arg)
+        {
+        }                                 
+
+        private void changeAngularLock(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 setFreeMove()
+        {
+            m_pidControllerActive = true;
+            _zeroFlag = false;
+            _target_velocity = Vector3.Zero;
+            m_freemove = true;
+            m_colliderfilter = -2;
+            m_colliderObjectfilter = -2;
+            m_colliderGroundfilter = -2;
+
+            m_iscolliding = false;
+            m_iscollidingGround = false;
+            m_iscollidingObj = false;
+
+            CollisionEventsThisFrame.Clear();
+        }
+
+        private void changeForce(Vector3 newForce)
+        {
+            setFreeMove();
+
+            if (Body != IntPtr.Zero)
+            {
+                if (newForce.X != 0f || newForce.Y != 0f || newForce.Z != 0)
+                    d.BodyAddForce(Body, newForce.X, newForce.Y, newForce.Z);
+            }
+        }
+
+        // for now momentum is actually velocity
+        private void changeMomentum(Vector3 newmomentum)
+        {
+            _velocity = newmomentum;
+            setFreeMove();
+
+            if (Body != IntPtr.Zero)
+                d.BodySetLinearVel(Body, newmomentum.X, newmomentum.Y, newmomentum.Z);
+            ajustCollider();
+        }
+
+        private void donullchange()
+        {
+        }
+
+        public bool DoAChange(changes what, object arg)
+        {
+            if (collider == IntPtr.Zero && what != changes.Add && what != changes.Remove)
+            {
+                return false;
+            }
+
+            // nasty switch
+            switch (what)
+            {
+                case changes.Add:
+                    changeAdd();
+                    break;
+                case changes.Remove:
+                    changeRemove();
+                    break;
+
+                case changes.Position:
+                    changePosition((Vector3)arg);
+                    break;
+
+                case changes.Orientation:
+                    changeOrientation((Quaternion)arg);
+                    break;
+
+                case changes.PosOffset:
+                    donullchange();
+                    break;
+
+                case changes.OriOffset:
+                    donullchange();
+                    break;
+
+                case changes.Velocity:
+                    changeVelocity((Vector3)arg);
+                    break;
+
+                //                case changes.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.AvatarSize:
+                    changeAvatarSize((strAvatarSize)arg);
+                    break;
+
+                case changes.Momentum:
+                    changeMomentum((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);
+        }
+
+        private struct strAvatarSize
+        {
+            public Vector3 size;
+            public float offset;
+        }
+
+    }
+}
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs
new file mode 100644
index 0000000..a7dda7a
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs
@@ -0,0 +1,1087 @@
+/*
+ * Copyright (c) Contributors, http://opensimulator.org/
+ * See CONTRIBUTORS.TXT for a full list of copyright holders.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the OpenSimulator Project nor the
+ *       names of its contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/* Revised Aug, Sept 2009 by Kitto Flora. ODEDynamics.cs replaces
+ * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised:
+ * ODEPrim.cs contains methods dealing with Prim editing, Prim
+ * characteristics and Kinetic motion.
+ * ODEDynamics.cs contains methods dealing with Prim Physical motion
+ * (dynamics) and the associated settings. Old Linear and angular
+ * motors for dynamic motion have been replace with  MoveLinear()
+ * and MoveAngular(); 'Physical' is used only to switch ODE dynamic
+ * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_<other> is to
+ * switch between 'VEHICLE' parameter use and general dynamics
+ * settings use.
+ */
+
+// Extensive change Ubit 2012
+
+using System;
+using System.Collections.Generic;
+using System.Reflection;
+using System.Runtime.InteropServices;
+using log4net;
+using OpenMetaverse;
+using OdeAPI;
+using OpenSim.Framework;
+using OpenSim.Region.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
+        // WARNING this are working copies for internel use
+        // their values may not be the corresponding parameter
+
+        private Quaternion m_referenceFrame = Quaternion.Identity;      // Axis modifier
+        private Quaternion m_RollreferenceFrame = Quaternion.Identity;  // what hell is this ?
+
+        private Vehicle m_type = Vehicle.TYPE_NONE;                     // If a 'VEHICLE', and what kind
+
+        private VehicleFlag m_flags = (VehicleFlag) 0;                  // Boolean settings:
+                                                                        // HOVER_TERRAIN_ONLY
+                                                                        // HOVER_GLOBAL_HEIGHT
+                                                                        // NO_DEFLECTION_UP
+                                                                        // HOVER_WATER_ONLY
+                                                                        // HOVER_UP_ONLY
+                                                                        // LIMIT_MOTOR_UP
+                                                                        // LIMIT_ROLL_ONLY
+        private Vector3 m_BlockingEndPoint = Vector3.Zero;              // not sl
+
+        // Linear properties
+        private Vector3 m_linearMotorDirection = Vector3.Zero;          // velocity requested by LSL, decayed by time
+        private Vector3 m_linearFrictionTimescale = new Vector3(1000, 1000, 1000);
+        private float m_linearMotorDecayTimescale = 120;
+        private float m_linearMotorTimescale = 1000;
+        private Vector3 m_linearMotorOffset = Vector3.Zero;
+
+        //Angular properties
+        private Vector3 m_angularMotorDirection = Vector3.Zero;         // angular velocity requested by LSL motor
+        private float m_angularMotorTimescale = 1000;                      // motor angular velocity ramp up rate
+        private float m_angularMotorDecayTimescale = 120;                 // motor angular velocity decay rate
+        private Vector3 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);      // body angular velocity  decay rate
+
+        //Deflection properties
+        private float m_angularDeflectionEfficiency = 0;
+        private float m_angularDeflectionTimescale = 1000;
+        private float m_linearDeflectionEfficiency = 0;
+        private float m_linearDeflectionTimescale = 1000;
+
+        //Banking properties
+        private float m_bankingEfficiency = 0;
+        private float m_bankingMix = 0;
+        private float m_bankingTimescale = 1000;
+
+        //Hover and Buoyancy properties
+        private float m_VhoverHeight = 0f;
+        private float m_VhoverEfficiency = 0f;
+        private float m_VhoverTimescale = 1000f;
+        private float m_VehicleBuoyancy = 0f;           //KF: m_VehicleBuoyancy is set by VEHICLE_BUOYANCY for a vehicle.
+                    // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity)
+                    // KF: So far I have found no good method to combine a script-requested .Z velocity and gravity.
+                    // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity.
+
+        //Attractor properties
+        private float m_verticalAttractionEfficiency = 1.0f;        // damped
+        private float m_verticalAttractionTimescale = 1000f;        // Timescale > 300  means no vert attractor.
+
+
+        // auxiliar
+        private float m_lmEfect = 0f;                                            // current linear motor eficiency
+        private float m_lmDecay = 0f;                                            // current linear decay
+
+        private float m_amEfect = 0;                                            // current angular motor eficiency
+        private float m_amDecay = 0f;                                            // current linear decay
+
+        private float m_ffactor = 1.0f;
+
+        private float m_timestep = 0.02f;
+        private float m_invtimestep = 50;
+
+
+        float m_ampwr;
+        float m_amdampX;
+        float m_amdampY;
+        float m_amdampZ;
+
+
+        public float FrictionFactor
+        {
+            get
+            {
+                return m_ffactor;
+            }
+        }
+
+
+        public ODEDynamics(OdePrim rootp)
+        {
+            rootPrim = rootp;
+            _pParentScene = rootPrim._parent_scene;
+            m_timestep = _pParentScene.ODE_STEPSIZE;
+            m_invtimestep = 1.0f / m_timestep;
+        }
+
+        public void DoSetVehicle(VehicleData vd)
+        {
+            m_type = vd.m_type;
+            m_flags = vd.m_flags;
+
+            
+            // Linear properties
+            m_linearMotorDirection = vd.m_linearMotorDirection;
+
+            m_linearFrictionTimescale = vd.m_linearFrictionTimescale;
+            if (m_linearFrictionTimescale.X < m_timestep) m_linearFrictionTimescale.X = m_timestep;
+            if (m_linearFrictionTimescale.Y < m_timestep) m_linearFrictionTimescale.Y = m_timestep;
+            if (m_linearFrictionTimescale.Z < m_timestep) m_linearFrictionTimescale.Z = m_timestep;
+
+            m_linearMotorDecayTimescale = vd.m_linearMotorDecayTimescale;
+            if (m_linearMotorDecayTimescale < m_timestep) m_linearMotorDecayTimescale = m_timestep;
+            m_linearMotorDecayTimescale += 0.2f;
+            m_linearMotorDecayTimescale *= m_invtimestep;
+
+            m_linearMotorTimescale = vd.m_linearMotorTimescale;
+            if (m_linearMotorTimescale < m_timestep) m_linearMotorTimescale = m_timestep;
+
+            m_linearMotorOffset = vd.m_linearMotorOffset;
+
+            //Angular properties
+            m_angularMotorDirection = vd.m_angularMotorDirection;
+            m_angularMotorTimescale = vd.m_angularMotorTimescale;
+            if (m_angularMotorTimescale < m_timestep) m_angularMotorTimescale = m_timestep;
+
+            m_angularMotorDecayTimescale = vd.m_angularMotorDecayTimescale;
+            if (m_angularMotorDecayTimescale < m_timestep) m_angularMotorDecayTimescale = m_timestep;
+            m_angularMotorDecayTimescale *= m_invtimestep;
+
+            m_angularFrictionTimescale = vd.m_angularFrictionTimescale;
+            if (m_angularFrictionTimescale.X < m_timestep) m_angularFrictionTimescale.X = m_timestep;
+            if (m_angularFrictionTimescale.Y < m_timestep) m_angularFrictionTimescale.Y = m_timestep;
+            if (m_angularFrictionTimescale.Z < m_timestep) m_angularFrictionTimescale.Z = m_timestep;
+
+            //Deflection properties
+            m_angularDeflectionEfficiency = vd.m_angularDeflectionEfficiency;
+            m_angularDeflectionTimescale = vd.m_angularDeflectionTimescale;
+            if (m_angularDeflectionTimescale < m_timestep) m_angularDeflectionTimescale = m_timestep;
+
+            m_linearDeflectionEfficiency = vd.m_linearDeflectionEfficiency;
+            m_linearDeflectionTimescale = vd.m_linearDeflectionTimescale;
+            if (m_linearDeflectionTimescale < m_timestep) m_linearDeflectionTimescale = m_timestep;
+
+            //Banking properties
+            m_bankingEfficiency = vd.m_bankingEfficiency;
+            m_bankingMix = vd.m_bankingMix;
+            m_bankingTimescale = vd.m_bankingTimescale;
+            if (m_bankingTimescale < m_timestep) m_bankingTimescale = m_timestep;
+
+            //Hover and Buoyancy properties
+            m_VhoverHeight = vd.m_VhoverHeight;
+            m_VhoverEfficiency = vd.m_VhoverEfficiency;
+            m_VhoverTimescale = vd.m_VhoverTimescale;
+            if (m_VhoverTimescale < m_timestep) m_VhoverTimescale = m_timestep;
+
+            m_VehicleBuoyancy = vd.m_VehicleBuoyancy;
+
+            //Attractor properties
+            m_verticalAttractionEfficiency = vd.m_verticalAttractionEfficiency;
+            m_verticalAttractionTimescale = vd.m_verticalAttractionTimescale;
+            if (m_verticalAttractionTimescale < m_timestep) m_verticalAttractionTimescale = m_timestep;
+
+            // Axis
+            m_referenceFrame = vd.m_referenceFrame;
+
+            m_lmEfect = 0;
+            m_lmDecay = (1.0f - 1.0f / m_linearMotorDecayTimescale);
+            m_amEfect = 0;
+            m_ffactor = 1.0f;
+        }
+
+        internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue)
+        {
+            float len;
+
+            switch (pParam)
+            {
+                case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY:
+                    if (pValue < 0f) pValue = 0f;
+                    if (pValue > 1f) pValue = 1f;
+                    m_angularDeflectionEfficiency = pValue;
+                    break;
+                case Vehicle.ANGULAR_DEFLECTION_TIMESCALE:
+                    if (pValue < m_timestep) pValue = m_timestep;
+                    m_angularDeflectionTimescale = pValue;
+                    break;
+                case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE:
+                    if (pValue < m_timestep) pValue = m_timestep;
+                    else if (pValue > 120) pValue = 120;
+                    m_angularMotorDecayTimescale = pValue * m_invtimestep;
+                    m_amDecay = 1.0f - 1.0f / m_angularMotorDecayTimescale;
+                    break;
+                case Vehicle.ANGULAR_MOTOR_TIMESCALE:
+                    if (pValue < m_timestep) pValue = m_timestep;
+                    m_angularMotorTimescale = pValue;
+                    break;
+                case Vehicle.BANKING_EFFICIENCY:
+                    if (pValue < -1f) pValue = -1f;
+                    if (pValue > 1f) pValue = 1f;
+                    m_bankingEfficiency = pValue;
+                    break;
+                case Vehicle.BANKING_MIX:
+                    if (pValue < 0f) pValue = 0f;
+                    if (pValue > 1f) pValue = 1f;
+                    m_bankingMix = pValue;
+                    break;
+                case Vehicle.BANKING_TIMESCALE:
+                    if (pValue < m_timestep) pValue = m_timestep;
+                    m_bankingTimescale = pValue;
+                    break;
+                case Vehicle.BUOYANCY:
+                    if (pValue < -1f) pValue = -1f;
+                    if (pValue > 1f) pValue = 1f;
+                    m_VehicleBuoyancy = pValue;
+                    break;
+                case Vehicle.HOVER_EFFICIENCY:
+                    if (pValue < 0f) pValue = 0f;
+                    if (pValue > 1f) pValue = 1f;
+                    m_VhoverEfficiency = pValue;
+                    break;
+                case Vehicle.HOVER_HEIGHT:
+                    m_VhoverHeight = pValue;
+                    break;
+                case Vehicle.HOVER_TIMESCALE:
+                    if (pValue < m_timestep) pValue = m_timestep;
+                    m_VhoverTimescale = pValue;
+                    break;
+                case Vehicle.LINEAR_DEFLECTION_EFFICIENCY:
+                    if (pValue < 0f) pValue = 0f;
+                    if (pValue > 1f) pValue = 1f;
+                    m_linearDeflectionEfficiency = pValue;
+                    break;
+                case Vehicle.LINEAR_DEFLECTION_TIMESCALE:
+                    if (pValue < m_timestep) pValue = m_timestep;
+                    m_linearDeflectionTimescale = pValue;
+                    break;
+                case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE:
+                    if (pValue < m_timestep) pValue = m_timestep;
+                    else if (pValue > 120) pValue = 120;
+                    m_linearMotorDecayTimescale = (0.2f +pValue) * m_invtimestep;
+                    m_lmDecay = (1.0f - 1.0f / m_linearMotorDecayTimescale);
+                    break;
+                case Vehicle.LINEAR_MOTOR_TIMESCALE:
+                    if (pValue < m_timestep) pValue = m_timestep;
+                    m_linearMotorTimescale = pValue;
+                    break;
+                case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY:
+                    if (pValue < 0f) pValue = 0f;
+                    if (pValue > 1f) pValue = 1f;
+                    m_verticalAttractionEfficiency = pValue;
+                    break;
+                case Vehicle.VERTICAL_ATTRACTION_TIMESCALE:
+                    if (pValue < m_timestep) pValue = m_timestep;
+                    m_verticalAttractionTimescale = pValue;
+                    break;
+
+                // These are vector properties but the engine lets you use a single float value to
+                // set all of the components to the same value
+                case Vehicle.ANGULAR_FRICTION_TIMESCALE:
+                    if (pValue < m_timestep) pValue = m_timestep;
+                    m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue);
+                    break;
+                case Vehicle.ANGULAR_MOTOR_DIRECTION:
+                    m_angularMotorDirection = new Vector3(pValue, pValue, pValue);
+                    len = m_angularMotorDirection.Length();
+                    if (len > 12.566f)
+                        m_angularMotorDirection *= (12.566f / len);
+
+                    m_amEfect = 1.0f ; // turn it on
+                    m_amDecay = 1.0f - 1.0f / m_angularMotorDecayTimescale;
+
+                    if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body)
+                            && !rootPrim.m_isSelected && !rootPrim.m_disabled)
+                        d.BodyEnable(rootPrim.Body);
+                    break;
+                case Vehicle.LINEAR_FRICTION_TIMESCALE:
+                    if (pValue < m_timestep) pValue = m_timestep;
+                    m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue);
+                    break;
+                case Vehicle.LINEAR_MOTOR_DIRECTION:
+                    m_linearMotorDirection = new Vector3(pValue, pValue, pValue);
+                    len = m_linearMotorDirection.Length();
+                    if (len > 100.0f)
+                        m_linearMotorDirection *= (100.0f / len);
+
+                    m_lmDecay = 1.0f - 1.0f / m_linearMotorDecayTimescale;
+                    m_lmEfect = 1.0f; // turn it on
+
+                    m_ffactor = 0.0f;
+                    if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body)
+                            && !rootPrim.m_isSelected && !rootPrim.m_disabled)
+                        d.BodyEnable(rootPrim.Body);
+                    break;
+                case Vehicle.LINEAR_MOTOR_OFFSET:
+                    m_linearMotorOffset = new Vector3(pValue, pValue, pValue);
+                    len = m_linearMotorOffset.Length();
+                    if (len > 100.0f)
+                        m_linearMotorOffset *= (100.0f / len);
+                    break;
+            }
+        }//end ProcessFloatVehicleParam
+
+        internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue)
+        {
+            float len;
+
+            switch (pParam)
+            {
+                case Vehicle.ANGULAR_FRICTION_TIMESCALE:
+                    if (pValue.X < m_timestep) pValue.X = m_timestep;
+                    if (pValue.Y < m_timestep) pValue.Y = m_timestep;
+                    if (pValue.Z < m_timestep) pValue.Z = m_timestep;
+
+                    m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
+                    break;
+                case Vehicle.ANGULAR_MOTOR_DIRECTION:
+                    m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
+                    // Limit requested angular speed to 2 rps= 4 pi rads/sec
+                    len = m_angularMotorDirection.Length();
+                    if (len > 12.566f)
+                        m_angularMotorDirection *= (12.566f / len);
+
+                    m_amEfect = 1.0f; // turn it on
+                    m_amDecay = 1.0f - 1.0f / m_angularMotorDecayTimescale;
+
+                    if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body)
+                            && !rootPrim.m_isSelected && !rootPrim.m_disabled)
+                        d.BodyEnable(rootPrim.Body);
+                    break;
+                case Vehicle.LINEAR_FRICTION_TIMESCALE:
+                    if (pValue.X < m_timestep) pValue.X = m_timestep;
+                    if (pValue.Y < m_timestep) pValue.Y = m_timestep;
+                    if (pValue.Z < m_timestep) pValue.Z = m_timestep;
+                    m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
+                    break;
+                case Vehicle.LINEAR_MOTOR_DIRECTION:
+                    m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
+                    len = m_linearMotorDirection.Length();
+                    if (len > 100.0f)
+                        m_linearMotorDirection *= (100.0f / len);
+
+                    m_lmEfect = 1.0f; // turn it on
+                    m_lmDecay = 1.0f - 1.0f / m_linearMotorDecayTimescale;
+
+                    m_ffactor = 0.0f;
+                    if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body)
+                            && !rootPrim.m_isSelected && !rootPrim.m_disabled)
+                        d.BodyEnable(rootPrim.Body);
+                    break;
+                case Vehicle.LINEAR_MOTOR_OFFSET:
+                    m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z);
+                    len = m_linearMotorOffset.Length();
+                    if (len > 100.0f)
+                        m_linearMotorOffset *= (100.0f / len);
+                    break;
+                case Vehicle.BLOCK_EXIT:
+                    m_BlockingEndPoint = new Vector3(pValue.X, pValue.Y, pValue.Z);
+                    break;
+            }
+        }//end ProcessVectorVehicleParam
+
+        internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue)
+        {
+            switch (pParam)
+            {
+                case Vehicle.REFERENCE_FRAME:
+                    //                    m_referenceFrame = Quaternion.Inverse(pValue);
+                    m_referenceFrame = pValue;
+                    break;
+                case Vehicle.ROLL_FRAME:
+                    m_RollreferenceFrame = pValue;
+                    break;
+            }
+        }//end ProcessRotationVehicleParam
+
+        internal void ProcessVehicleFlags(int pParam, bool remove)
+        {
+            if (remove)
+            {
+                m_flags &= ~((VehicleFlag)pParam);
+            }
+            else
+            {
+                m_flags |= (VehicleFlag)pParam;
+            }
+        }//end ProcessVehicleFlags
+
+        internal void ProcessTypeChange(Vehicle pType)
+        {
+            m_lmEfect = 0;
+
+            m_amEfect = 0;
+            m_ffactor = 1f;
+
+            m_linearMotorDirection = Vector3.Zero;
+            m_angularMotorDirection = Vector3.Zero;
+
+            m_BlockingEndPoint = Vector3.Zero;
+            m_RollreferenceFrame = Quaternion.Identity;
+            m_linearMotorOffset = Vector3.Zero;
+
+            m_referenceFrame = Quaternion.Identity;
+
+            // Set Defaults For Type
+            m_type = pType;
+            switch (pType)
+            {
+                case Vehicle.TYPE_NONE:
+                    m_linearFrictionTimescale = new Vector3(1000, 1000, 1000);
+                    m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
+                    m_linearMotorTimescale = 1000;
+                    m_linearMotorDecayTimescale = 120 * m_invtimestep;
+                    m_angularMotorTimescale = 1000;
+                    m_angularMotorDecayTimescale = 1000  * m_invtimestep;
+                    m_VhoverHeight = 0;
+                    m_VhoverEfficiency = 1;
+                    m_VhoverTimescale = 1000;
+                    m_VehicleBuoyancy = 0;
+                    m_linearDeflectionEfficiency = 0;
+                    m_linearDeflectionTimescale = 1000;
+                    m_angularDeflectionEfficiency = 0;
+                    m_angularDeflectionTimescale = 1000;
+                    m_bankingEfficiency = 0;
+                    m_bankingMix = 1;
+                    m_bankingTimescale = 1000;
+                    m_verticalAttractionEfficiency = 0;
+                    m_verticalAttractionTimescale = 1000;
+
+                    m_flags = (VehicleFlag)0;
+                    break;
+
+                case Vehicle.TYPE_SLED:
+                    m_linearFrictionTimescale = new Vector3(30, 1, 1000);
+                    m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
+                    m_linearMotorTimescale = 1000;
+                    m_linearMotorDecayTimescale = 120 * m_invtimestep;
+                    m_angularMotorTimescale = 1000;
+                    m_angularMotorDecayTimescale = 120 * m_invtimestep;
+                    m_VhoverHeight = 0;
+                    m_VhoverEfficiency = 1;
+                    m_VhoverTimescale = 10;
+                    m_VehicleBuoyancy = 0;
+                    m_linearDeflectionEfficiency = 1;
+                    m_linearDeflectionTimescale = 1;
+                    m_angularDeflectionEfficiency = 0;
+                    m_angularDeflectionTimescale = 10;
+                    m_verticalAttractionEfficiency = 1;
+                    m_verticalAttractionTimescale = 1000;
+                    m_bankingEfficiency = 0;
+                    m_bankingMix = 1;
+                    m_bankingTimescale = 10;
+                    m_flags &=
+                         ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
+                           VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
+                    m_flags |= (VehicleFlag.NO_DEFLECTION_UP |
+                        VehicleFlag.LIMIT_ROLL_ONLY |
+                        VehicleFlag.LIMIT_MOTOR_UP);
+                    break;
+
+                case Vehicle.TYPE_CAR:
+                    m_linearFrictionTimescale = new Vector3(100, 2, 1000);
+                    m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
+                    m_linearMotorTimescale = 1;
+                    m_linearMotorDecayTimescale = 60 * m_invtimestep;
+                    m_angularMotorTimescale = 1;
+                    m_angularMotorDecayTimescale = 0.8f * m_invtimestep;
+                    m_VhoverHeight = 0;
+                    m_VhoverEfficiency = 0;
+                    m_VhoverTimescale = 1000;
+                    m_VehicleBuoyancy = 0;
+                    m_linearDeflectionEfficiency = 1;
+                    m_linearDeflectionTimescale = 2;
+                    m_angularDeflectionEfficiency = 0;
+                    m_angularDeflectionTimescale = 10;
+                    m_verticalAttractionEfficiency = 1f;
+                    m_verticalAttractionTimescale = 10f;
+                    m_bankingEfficiency = -0.2f;
+                    m_bankingMix = 1;
+                    m_bankingTimescale = 1;
+                    m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY |
+                                VehicleFlag.HOVER_TERRAIN_ONLY |
+                                VehicleFlag.HOVER_GLOBAL_HEIGHT);
+                    m_flags |= (VehicleFlag.NO_DEFLECTION_UP |
+                                VehicleFlag.LIMIT_ROLL_ONLY |
+                                VehicleFlag.LIMIT_MOTOR_UP |
+                                VehicleFlag.HOVER_UP_ONLY);
+                    break;
+                case Vehicle.TYPE_BOAT:
+                    m_linearFrictionTimescale = new Vector3(10, 3, 2);
+                    m_angularFrictionTimescale = new Vector3(10, 10, 10);
+                    m_linearMotorTimescale = 5;
+                    m_linearMotorDecayTimescale = 60 * m_invtimestep;
+                    m_angularMotorTimescale = 4;
+                    m_angularMotorDecayTimescale = 4 * m_invtimestep;
+                    m_VhoverHeight = 0;
+                    m_VhoverEfficiency = 0.5f;
+                    m_VhoverTimescale = 2;
+                    m_VehicleBuoyancy = 1;
+                    m_linearDeflectionEfficiency = 0.5f;
+                    m_linearDeflectionTimescale = 3;
+                    m_angularDeflectionEfficiency = 0.5f;
+                    m_angularDeflectionTimescale = 5;
+                    m_verticalAttractionEfficiency = 0.5f;
+                    m_verticalAttractionTimescale = 5f;
+                    m_bankingEfficiency = -0.3f;
+                    m_bankingMix = 0.8f;
+                    m_bankingTimescale = 1;
+                    m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY |
+                            VehicleFlag.HOVER_GLOBAL_HEIGHT |
+                            VehicleFlag.HOVER_UP_ONLY); // |
+//                            VehicleFlag.LIMIT_ROLL_ONLY);
+                    m_flags |= (VehicleFlag.NO_DEFLECTION_UP |
+                                VehicleFlag.LIMIT_MOTOR_UP |
+                                VehicleFlag.HOVER_UP_ONLY |  // new sl
+                                VehicleFlag.HOVER_WATER_ONLY);
+                    break;
+
+                case Vehicle.TYPE_AIRPLANE:
+                    m_linearFrictionTimescale = new Vector3(200, 10, 5);
+                    m_angularFrictionTimescale = new Vector3(20, 20, 20);
+                    m_linearMotorTimescale = 2;
+                    m_linearMotorDecayTimescale = 60 * m_invtimestep;
+                    m_angularMotorTimescale = 4;
+                    m_angularMotorDecayTimescale = 8 * m_invtimestep;
+                    m_VhoverHeight = 0;
+                    m_VhoverEfficiency = 0.5f;
+                    m_VhoverTimescale = 1000;
+                    m_VehicleBuoyancy = 0;
+                    m_linearDeflectionEfficiency = 0.5f;
+                    m_linearDeflectionTimescale = 0.5f;
+                    m_angularDeflectionEfficiency = 1;
+                    m_angularDeflectionTimescale = 2;
+                    m_verticalAttractionEfficiency = 0.9f;
+                    m_verticalAttractionTimescale = 2f;
+                    m_bankingEfficiency = 1;
+                    m_bankingMix = 0.7f;
+                    m_bankingTimescale = 2;
+                    m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY |
+                        VehicleFlag.HOVER_TERRAIN_ONLY |
+                        VehicleFlag.HOVER_GLOBAL_HEIGHT |
+                        VehicleFlag.HOVER_UP_ONLY |
+                        VehicleFlag.NO_DEFLECTION_UP |
+                        VehicleFlag.LIMIT_MOTOR_UP);
+                    m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY);
+                    break;
+
+                case Vehicle.TYPE_BALLOON:
+                    m_linearFrictionTimescale = new Vector3(5, 5, 5);
+                    m_angularFrictionTimescale = new Vector3(10, 10, 10);
+                    m_linearMotorTimescale = 5;
+                    m_linearMotorDecayTimescale = 60 * m_invtimestep;
+                    m_angularMotorTimescale = 6;
+                    m_angularMotorDecayTimescale = 10 * m_invtimestep;
+                    m_VhoverHeight = 5;
+                    m_VhoverEfficiency = 0.8f;
+                    m_VhoverTimescale = 10;
+                    m_VehicleBuoyancy = 1;
+                    m_linearDeflectionEfficiency = 0;
+                    m_linearDeflectionTimescale = 5 * m_invtimestep;
+                    m_angularDeflectionEfficiency = 0;
+                    m_angularDeflectionTimescale = 5;
+                    m_verticalAttractionEfficiency = 1f;
+                    m_verticalAttractionTimescale = 1000f;
+                    m_bankingEfficiency = 0;
+                    m_bankingMix = 0.7f;
+                    m_bankingTimescale = 5;
+                    m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY |
+                        VehicleFlag.HOVER_TERRAIN_ONLY |
+                        VehicleFlag.HOVER_UP_ONLY |
+                        VehicleFlag.NO_DEFLECTION_UP |
+                        VehicleFlag.LIMIT_MOTOR_UP  | //);
+                        VehicleFlag.LIMIT_ROLL_ONLY | // new sl
+                        VehicleFlag.HOVER_GLOBAL_HEIGHT); // new sl
+
+//                    m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY |
+//                        VehicleFlag.HOVER_GLOBAL_HEIGHT);
+                    break;
+
+            }
+
+            m_lmDecay = (1.0f - 1.0f / m_linearMotorDecayTimescale);
+            m_amDecay = 1.0f - 1.0f / m_angularMotorDecayTimescale;
+
+        }//end SetDefaultsForType
+
+        internal void Stop()
+        {
+            m_lmEfect = 0;
+            m_lmDecay = 0f;
+            m_amEfect = 0;
+            m_amDecay = 0;
+            m_ffactor = 1f;
+        }
+
+        public static Vector3 Xrot(Quaternion rot)
+        {
+            Vector3 vec;
+            rot.Normalize(); // just in case
+            vec.X = 2 * (rot.X * rot.X + rot.W * rot.W) - 1;
+            vec.Y = 2 * (rot.X * rot.Y + rot.Z * rot.W);
+            vec.Z = 2 * (rot.X * rot.Z - rot.Y * rot.W);
+            return vec;
+        }
+
+        public static Vector3 Zrot(Quaternion rot)
+        {
+            Vector3 vec;
+            rot.Normalize(); // just in case
+            vec.X = 2 * (rot.X * rot.Z + rot.Y * rot.W);
+            vec.Y = 2 * (rot.Y * rot.Z - rot.X * rot.W);
+            vec.Z = 2 * (rot.Z * rot.Z + rot.W * rot.W) - 1;
+
+            return vec;
+        }
+
+        private const float pi = (float)Math.PI;
+        private const float halfpi = 0.5f * (float)Math.PI;
+        private const float twopi = 2.0f * pi;
+
+        public static Vector3 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()
+        {
+            IntPtr Body = rootPrim.Body;
+
+            d.Mass dmass;
+            d.BodyGetMass(Body, out dmass);
+
+            d.Quaternion rot = d.BodyGetQuaternion(Body);
+            Quaternion objrotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);    // rotq = rotation of object
+            Quaternion rotq = objrotq;    // rotq = rotation of object
+            rotq *= m_referenceFrame; // rotq is now rotation in vehicle reference frame
+            Quaternion irotq = Quaternion.Inverse(rotq);
+
+            d.Vector3 dvtmp;
+            Vector3 tmpV;
+            Vector3 curVel; // velocity in world
+            Vector3 curAngVel; // angular velocity in world
+            Vector3 force = Vector3.Zero; // actually linear aceleration until mult by mass in world frame
+            Vector3 torque = Vector3.Zero;// actually angular aceleration until mult by Inertia in vehicle frame
+            d.Vector3 dtorque = new d.Vector3();
+
+            dvtmp = d.BodyGetLinearVel(Body);
+            curVel.X = dvtmp.X;
+            curVel.Y = dvtmp.Y;
+            curVel.Z = dvtmp.Z;
+            Vector3 curLocalVel = curVel * irotq; // current velocity in  local
+
+            dvtmp = d.BodyGetAngularVel(Body);
+            curAngVel.X = dvtmp.X;
+            curAngVel.Y = dvtmp.Y;
+            curAngVel.Z = dvtmp.Z;
+            Vector3 curLocalAngVel = curAngVel * irotq; // current angular velocity in  local
+
+            float ldampZ = 0;
+            
+            // linear motor
+            if (m_lmEfect > 0.01 && m_linearMotorTimescale < 1000)
+            {
+                tmpV = m_linearMotorDirection - curLocalVel; // velocity error
+                tmpV *= m_lmEfect / m_linearMotorTimescale; // error to correct in this timestep
+                tmpV *= rotq; // to world
+
+                if ((m_flags & VehicleFlag.LIMIT_MOTOR_UP) != 0)
+                    tmpV.Z = 0;
+
+                if (m_linearMotorOffset.X != 0 || m_linearMotorOffset.Y != 0 || m_linearMotorOffset.Z != 0)
+                {
+                    // have offset, do it now
+                    tmpV *= dmass.mass;
+                    d.BodyAddForceAtRelPos(Body, tmpV.X, tmpV.Y, tmpV.Z, m_linearMotorOffset.X, m_linearMotorOffset.Y, m_linearMotorOffset.Z);
+                }
+                else
+                {
+                    force.X += tmpV.X;
+                    force.Y += tmpV.Y;
+                    force.Z += tmpV.Z;
+                }
+
+                m_lmEfect *= m_lmDecay;
+//                m_ffactor = 0.01f + 1e-4f * curVel.LengthSquared();
+                m_ffactor = 0.0f;
+            }
+            else
+            {
+                m_lmEfect = 0;
+                m_ffactor = 1f;
+            }
+
+            // hover
+            if (m_VhoverTimescale < 300 && rootPrim.prim_geom != IntPtr.Zero)
+            {
+                //                d.Vector3 pos = d.BodyGetPosition(Body);
+                d.Vector3 pos = d.GeomGetPosition(rootPrim.prim_geom);
+                pos.Z -= 0.21f; // minor offset that seems to be always there in sl
+
+                float t = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y);
+                float perr;
+
+                // default to global but don't go underground
+                perr = m_VhoverHeight - pos.Z;
+
+                if ((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == 0)
+                {
+                    if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) != 0)
+                    {
+                        perr += _pParentScene.GetWaterLevel();
+                    }
+                    else if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0)
+                    {
+                        perr += t;
+                    }
+                    else
+                    {
+                        float w = _pParentScene.GetWaterLevel();
+                        if (t > w)
+                            perr += t;
+                        else
+                            perr += w;
+                    }
+                }
+                else if (t > m_VhoverHeight)
+                        perr = t - pos.Z; ;
+
+                if ((m_flags & VehicleFlag.HOVER_UP_ONLY) == 0 || perr > -0.1)
+                {
+                    ldampZ = m_VhoverEfficiency * m_invtimestep;
+
+                    perr *= (1.0f + ldampZ) / m_VhoverTimescale;
+
+                    //                    force.Z += perr - curVel.Z * tmp;
+                    force.Z += perr;
+                    ldampZ *= -curVel.Z;
+
+                    force.Z += _pParentScene.gravityz * (1f - m_VehicleBuoyancy);
+                }
+                else // no buoyancy
+                    force.Z += _pParentScene.gravityz;
+            }
+            else
+            {
+                // default gravity and Buoyancy
+                force.Z += _pParentScene.gravityz * (1f - m_VehicleBuoyancy);
+            }
+
+            // linear deflection
+            if (m_linearDeflectionEfficiency > 0)
+            {
+                float len = curVel.Length();
+                if (len > 0.01) // if moving
+                {
+                    Vector3 atAxis;
+                    atAxis = Xrot(rotq); // where are we pointing to
+                    atAxis *= len; // make it same size as world velocity vector
+
+                    tmpV = -atAxis; // oposite direction
+                    atAxis -= curVel; // error to one direction
+                    len = atAxis.LengthSquared();
+
+                    tmpV -= curVel; // error to oposite
+                    float lens = tmpV.LengthSquared();
+
+                    if (len > 0.01 || lens > 0.01) // do nothing if close enougth
+                    {
+                        if (len < lens)
+                            tmpV = atAxis;
+
+                        tmpV *= (m_linearDeflectionEfficiency / m_linearDeflectionTimescale); // error to correct in this timestep
+                        force.X += tmpV.X;
+                        force.Y += tmpV.Y;
+                        if ((m_flags & VehicleFlag.NO_DEFLECTION_UP) == 0)
+                            force.Z += tmpV.Z;
+                    }
+                }
+            }
+
+            // linear friction/damping
+            if (curLocalVel.X != 0 || curLocalVel.Y != 0 || curLocalVel.Z != 0)
+            {
+                tmpV.X = -curLocalVel.X / m_linearFrictionTimescale.X;
+                tmpV.Y = -curLocalVel.Y / m_linearFrictionTimescale.Y;
+                tmpV.Z = -curLocalVel.Z / m_linearFrictionTimescale.Z;
+                tmpV *= rotq; // to world
+
+                if(ldampZ != 0 && Math.Abs(ldampZ) > Math.Abs(tmpV.Z))
+                    tmpV.Z = ldampZ;
+                force.X += tmpV.X;
+                force.Y += tmpV.Y;
+                force.Z += tmpV.Z;
+            }
+
+            // vertical atractor
+            if (m_verticalAttractionTimescale < 300)
+            {
+                float roll;
+                float pitch;
+
+
+
+                float ftmp = m_invtimestep / m_verticalAttractionTimescale / m_verticalAttractionTimescale;
+
+                float ftmp2;
+                ftmp2 = 0.5f * m_verticalAttractionEfficiency * m_invtimestep;
+                m_amdampX = ftmp2;
+
+                m_ampwr = 1.0f - 0.8f * m_verticalAttractionEfficiency;
+
+                GetRollPitch(irotq, out roll, out pitch);
+
+                if (roll > halfpi)
+                    roll = pi - roll;
+                else if (roll < -halfpi)
+                    roll = -pi - roll;
+
+                float effroll = pitch / halfpi;
+                effroll *= effroll;
+                effroll = 1 - effroll;
+                effroll *= roll;
+
+
+                torque.X += effroll * ftmp;
+
+                if ((m_flags & VehicleFlag.LIMIT_ROLL_ONLY) == 0)
+                {
+                    float effpitch = roll / halfpi;
+                    effpitch *= effpitch;
+                    effpitch = 1 - effpitch;
+                    effpitch *= pitch;
+
+                    torque.Y += effpitch * ftmp;
+                }
+
+                if (m_bankingEfficiency != 0 && Math.Abs(effroll) > 0.01)
+                {
+
+                    float broll = effroll;
+                    /*
+                                        if (broll > halfpi)
+                                            broll = pi - broll;
+                                        else if (broll < -halfpi)
+                                            broll = -pi - broll;                           
+                    */
+                    broll *= m_bankingEfficiency;
+                    if (m_bankingMix != 0)
+                    {
+                        float vfact = Math.Abs(curLocalVel.X) / 10.0f;
+                        if (vfact > 1.0f) vfact = 1.0f;
+
+                        if (curLocalVel.X >= 0)
+                            broll *= (1 + (vfact - 1) * m_bankingMix);
+                        else
+                            broll *= -(1 + (vfact - 1) * m_bankingMix);
+                    }
+                    // make z rot be in world Z not local as seems to be in sl
+
+                    broll = broll / m_bankingTimescale;
+
+
+                    tmpV = Zrot(irotq);
+                    tmpV *= broll;
+
+                    torque.X += tmpV.X;
+                    torque.Y += tmpV.Y;
+                    torque.Z += tmpV.Z;
+
+                    m_amdampZ = Math.Abs(m_bankingEfficiency) / m_bankingTimescale;
+                    m_amdampY = m_amdampZ;
+
+                }
+                else
+                {
+                    m_amdampZ = 1 / m_angularFrictionTimescale.Z;
+                    m_amdampY = m_amdampX;
+                }
+            }
+            else
+            {
+                m_ampwr = 1.0f;
+                m_amdampX = 1 / m_angularFrictionTimescale.X;
+                m_amdampY = 1 / m_angularFrictionTimescale.Y;
+                m_amdampZ = 1 / m_angularFrictionTimescale.Z;
+            }
+
+            // angular motor
+            if (m_amEfect > 0.01 && m_angularMotorTimescale < 1000)
+            {
+                tmpV = m_angularMotorDirection - curLocalAngVel; // velocity error
+                tmpV *= m_amEfect / m_angularMotorTimescale; // error to correct in this timestep
+                torque.X += tmpV.X * m_ampwr;
+                torque.Y += tmpV.Y * m_ampwr;
+                torque.Z += tmpV.Z;
+
+                m_amEfect *= m_amDecay;
+            }
+            else
+                m_amEfect = 0;
+
+            // angular deflection
+            if (m_angularDeflectionEfficiency > 0)
+            {
+                Vector3 dirv;
+                
+                if (curLocalVel.X > 0.01f)
+                    dirv = curLocalVel;
+                else if (curLocalVel.X < -0.01f)
+                    // use oposite 
+                    dirv = -curLocalVel;
+                else
+                {
+                    // make it fall into small positive x case
+                    dirv.X = 0.01f;
+                    dirv.Y = curLocalVel.Y;
+                    dirv.Z = curLocalVel.Z;
+                }
+
+                float ftmp = m_angularDeflectionEfficiency / m_angularDeflectionTimescale;
+
+                if (Math.Abs(dirv.Z) > 0.01)
+                {
+                    torque.Y += - (float)Math.Atan2(dirv.Z, dirv.X) * ftmp;
+                }
+
+                if (Math.Abs(dirv.Y) > 0.01)
+                {
+                    torque.Z += (float)Math.Atan2(dirv.Y, dirv.X) * ftmp;
+                }
+            }
+
+            // angular friction
+            if (curLocalAngVel.X != 0 || curLocalAngVel.Y != 0 || curLocalAngVel.Z != 0)
+            {
+                torque.X -= curLocalAngVel.X * m_amdampX;
+                torque.Y -= curLocalAngVel.Y * m_amdampY;
+                torque.Z -= curLocalAngVel.Z * m_amdampZ;
+            }
+
+            
+
+            if (force.X != 0 || force.Y != 0 || force.Z != 0)
+            {
+                force *= dmass.mass;
+                d.BodyAddForce(Body, force.X, force.Y, force.Z);
+            }
+
+            if (torque.X != 0 || torque.Y != 0 || torque.Z != 0)
+            {
+                torque *= m_referenceFrame; // to object frame
+                dtorque.X = torque.X ;
+                dtorque.Y = torque.Y;
+                dtorque.Z = torque.Z;
+
+                d.MultiplyM3V3(out dvtmp, ref dmass.I, ref dtorque);
+                d.BodyAddRelTorque(Body, dvtmp.X, dvtmp.Y, dvtmp.Z); // add torque in object frame
+            }
+        }
+    }
+}
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEMeshWorker.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEMeshWorker.cs
new file mode 100644
index 0000000..0df71eb
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEMeshWorker.cs
@@ -0,0 +1,932 @@
+/*
+ * AJLDuarte 2012
+ */
+
+using System;
+using System.Threading;
+using System.Collections.Generic;
+using System.IO;
+using System.Reflection;
+using System.Runtime.InteropServices;
+using System.Text;
+using OpenSim.Framework;
+using OpenSim.Region.Physics.Manager;
+using OdeAPI;
+using log4net;
+using Nini.Config;
+using OpenMetaverse;
+
+namespace OpenSim.Region.Physics.OdePlugin
+{
+    public enum MeshState : byte
+    {
+        noNeed = 0,
+
+        loadingAsset = 1,
+
+        AssetOK = 0x0f,     // 00001111
+
+        NeedMask = 0x30,    // 00110000
+        needMesh = 0x10,    // 00010000 
+        needAsset = 0x20,   // 00100000
+
+        FailMask = 0xC0,    // 11000000
+        AssetFailed = 0x40, // 01000000
+        MeshFailed = 0x80,   // 10000000
+
+        MeshNoColide = FailMask | needAsset
+    }
+
+    public enum meshWorkerCmnds : byte
+    {
+        nop = 0,
+        addnew,
+        changefull,
+        changesize,
+        changeshapetype,
+        getmesh,
+    }
+
+    public class ODEPhysRepData
+    {
+        public PhysicsActor actor;
+        public PrimitiveBaseShape pbs;
+        public IMesh mesh;
+
+        public Vector3 size;
+        public Vector3 OBB;
+        public Vector3 OBBOffset;
+
+        public float volume;
+
+        public byte shapetype;
+        public bool hasOBB;
+        public bool hasMeshVolume;
+        public MeshState meshState;
+        public UUID? assetID;
+        public meshWorkerCmnds comand;
+    }
+
+    public class ODEMeshWorker
+    {
+
+        private ILog m_log;
+        private OdeScene m_scene;
+        private IMesher m_mesher;
+
+        public bool meshSculptedPrim = true;
+        public bool forceSimplePrimMeshing = false;
+        public float meshSculptLOD = 32;
+        public float MeshSculptphysicalLOD = 32;
+
+
+        private OpenSim.Framework.BlockingQueue<ODEPhysRepData> createqueue = new OpenSim.Framework.BlockingQueue<ODEPhysRepData>();
+        private bool m_running;
+
+        private Thread m_thread;
+
+        public ODEMeshWorker(OdeScene pScene, ILog pLog, IMesher pMesher, IConfig pConfig)
+        {
+            m_scene = pScene;
+            m_log = pLog;
+            m_mesher = pMesher;
+
+            if (pConfig != null)
+            {
+                forceSimplePrimMeshing = pConfig.GetBoolean("force_simple_prim_meshing", forceSimplePrimMeshing);
+                meshSculptedPrim = pConfig.GetBoolean("mesh_sculpted_prim", meshSculptedPrim);
+                meshSculptLOD = pConfig.GetFloat("mesh_lod", meshSculptLOD);
+                MeshSculptphysicalLOD = pConfig.GetFloat("mesh_physical_lod", MeshSculptphysicalLOD);
+            }
+            m_running = true;
+            m_thread = new Thread(DoWork);
+            m_thread.Start();
+        }
+
+        private void DoWork()
+        {
+            m_mesher.ExpireFileCache();
+
+            while(m_running)
+            {
+                 ODEPhysRepData nextRep = createqueue.Dequeue();
+                if(!m_running)
+                    return;
+                if (nextRep == null)
+                    continue;
+                if (m_scene.haveActor(nextRep.actor))
+                {
+                    switch (nextRep.comand)
+                    {
+                        case meshWorkerCmnds.changefull:
+                        case meshWorkerCmnds.changeshapetype:
+                        case meshWorkerCmnds.changesize:
+                            GetMesh(nextRep);
+                            if (CreateActorPhysRep(nextRep) && m_scene.haveActor(nextRep.actor))
+                                m_scene.AddChange(nextRep.actor, changes.PhysRepData, nextRep);
+                            break;
+                        case meshWorkerCmnds.getmesh:
+                            DoRepDataGetMesh(nextRep);
+                            break;
+                    }
+                }
+            }
+        }
+
+        public void Stop()
+        {
+            try
+            {
+                m_thread.Abort();
+                createqueue.Clear();
+            }
+            catch
+            {
+            }
+        }
+
+        public void ChangeActorPhysRep(PhysicsActor actor, PrimitiveBaseShape pbs,
+                                        Vector3 size, byte shapetype)
+        {
+            ODEPhysRepData repData = new ODEPhysRepData();
+            repData.actor = actor;
+            repData.pbs = pbs;
+            repData.size = size;
+            repData.shapetype = shapetype;
+
+            CheckMesh(repData);
+            CalcVolumeData(repData);
+            m_scene.AddChange(actor, changes.PhysRepData, repData);
+            return;
+        }
+
+        public ODEPhysRepData NewActorPhysRep(PhysicsActor actor, PrimitiveBaseShape pbs,
+                                        Vector3 size, byte shapetype)
+        {
+            ODEPhysRepData repData = new ODEPhysRepData();
+            repData.actor = actor;
+            repData.pbs = pbs;
+            repData.size = size;
+            repData.shapetype = shapetype;
+
+            CheckMesh(repData);
+            CalcVolumeData(repData);
+            m_scene.AddChange(actor, changes.AddPhysRep, repData);
+            return repData;
+        }
+
+        public void RequestMesh(ODEPhysRepData repData)
+        {
+            repData.mesh = null;
+
+            if (repData.meshState == MeshState.needAsset)
+            {
+                PrimitiveBaseShape pbs = repData.pbs;
+
+                // check if we got outdated
+
+                if (!pbs.SculptEntry || pbs.SculptTexture == UUID.Zero)
+                {
+                    repData.meshState = MeshState.noNeed;
+                    return;
+                }
+
+                repData.assetID = pbs.SculptTexture;
+                repData.meshState = MeshState.loadingAsset;
+
+                repData.comand = meshWorkerCmnds.getmesh;
+                createqueue.Enqueue(repData);
+            }
+        }
+
+        // creates and prepares a mesh to use and calls parameters estimation
+        public bool CreateActorPhysRep(ODEPhysRepData repData)
+        {
+            IMesh mesh = repData.mesh;
+
+            if (mesh != null)
+            {
+                IntPtr vertices, indices;
+                int vertexCount, indexCount;
+                int vertexStride, triStride;
+
+                mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount);
+                mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount);
+
+                if (vertexCount == 0 || indexCount == 0)
+                {
+                    m_log.WarnFormat("[PHYSICS]: Invalid mesh data on prim {0} mesh UUID {1}",
+                        repData.actor.Name, repData.pbs.SculptTexture.ToString());
+                    repData.meshState = MeshState.MeshFailed;
+                    repData.hasOBB = false;
+                    repData.mesh = null;
+                    m_scene.mesher.ReleaseMesh(mesh);
+                }
+                else
+                {
+                    repData.OBBOffset = mesh.GetCentroid();
+                    repData.OBB = mesh.GetOBB();
+                    repData.hasOBB = true;
+                    mesh.releaseSourceMeshData();
+                }
+            }
+            CalcVolumeData(repData);
+            return true;
+        }
+
+        public void AssetLoaded(ODEPhysRepData repData)
+        {
+            if (m_scene.haveActor(repData.actor))
+            {
+                if (needsMeshing(repData.pbs)) // no need for pbs now?
+                {
+                    repData.comand = meshWorkerCmnds.changefull;
+                    createqueue.Enqueue(repData);
+                }
+            }
+            else
+                repData.pbs.SculptData = Utils.EmptyBytes;
+        }
+
+        public void DoRepDataGetMesh(ODEPhysRepData repData)
+        {
+            if (!repData.pbs.SculptEntry)
+                return;
+
+            if (repData.meshState != MeshState.loadingAsset)
+                return;
+
+            if (repData.assetID == null || repData.assetID == UUID.Zero)
+                return;
+
+            if (repData.assetID != repData.pbs.SculptTexture)
+                return;
+
+            // check if it is in cache
+            GetMesh(repData);
+            if (repData.meshState != MeshState.needAsset)
+            {
+                CreateActorPhysRep(repData);
+                m_scene.AddChange(repData.actor, changes.PhysRepData, repData);
+                return;
+            }
+
+            RequestAssetDelegate assetProvider = m_scene.RequestAssetMethod;
+            if (assetProvider == null)
+                return;
+            ODEAssetRequest asr = new ODEAssetRequest(this, assetProvider, repData, m_log);
+        }
+
+
+        /// <summary>
+        /// Routine to figure out if we need to mesh this prim with our mesher
+        /// </summary>
+        /// <param name="pbs"></param>
+        /// <returns></returns>
+        public bool needsMeshing(PrimitiveBaseShape pbs)
+        {
+            // check sculpts or meshs 
+            if (pbs.SculptEntry)
+            {
+                if (meshSculptedPrim)
+                    return true;
+
+                if (pbs.SculptType == (byte)SculptType.Mesh) // always do meshs
+                    return true;
+
+                return false;
+            }
+
+            if (forceSimplePrimMeshing)
+                return true;
+
+            // if it's a standard box or sphere with no cuts, hollows, twist or top shear, return false since ODE can use an internal representation for the prim
+
+            if ((pbs.ProfileShape == ProfileShape.Square && pbs.PathCurve == (byte)Extrusion.Straight)
+                    || (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1
+                    && pbs.Scale.X == pbs.Scale.Y && pbs.Scale.Y == pbs.Scale.Z))
+            {
+
+                if (pbs.ProfileBegin == 0 && pbs.ProfileEnd == 0
+                    && 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)
+                {
+                    return false;
+                }
+            }
+
+            //  following code doesn't give meshs to boxes and spheres ever
+            // and it's odd..  so for now just return true if asked to force meshs
+            // hopefully mesher will fail if doesn't suport so things still get basic boxes
+
+            int iPropertiesNotSupportedDefault = 0;
+
+            if (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 (iPropertiesNotSupportedDefault == 0)
+            {
+                return false;
+            }
+            return true;
+        }
+
+        // see if we need a mesh and if so if we have a cached one
+        // called with a new repData
+        public void CheckMesh(ODEPhysRepData repData)
+        {
+            PhysicsActor actor = repData.actor;
+            PrimitiveBaseShape pbs = repData.pbs;
+
+            if (!needsMeshing(pbs))
+            {
+                repData.meshState = MeshState.noNeed;
+                return;
+            }
+
+            IMesh mesh = null;
+
+            Vector3 size = repData.size;
+            byte shapetype = repData.shapetype;
+
+            bool convex;
+
+            int clod = (int)LevelOfDetail.High;
+            if (shapetype == 0)
+                convex = false;
+            else
+            {
+                convex = true;
+                if (pbs.SculptType != (byte)SculptType.Mesh)
+                    clod = (int)LevelOfDetail.Low;
+            }
+
+            mesh = m_mesher.GetMesh(actor.Name, pbs, size, clod, true, convex);
+
+            if (mesh == null)
+            {
+                if (pbs.SculptEntry)
+                {
+                    if (pbs.SculptTexture != null && pbs.SculptTexture != UUID.Zero)
+                    {
+                        repData.assetID = pbs.SculptTexture;
+                        repData.meshState = MeshState.needAsset;
+                    }
+                    else
+                        repData.meshState = MeshState.MeshFailed;
+
+                    return;
+                }
+                else
+                {
+                    repData.meshState = MeshState.needMesh;
+                    mesh = m_mesher.CreateMesh(actor.Name, pbs, size, clod, true, false, convex, true);
+                    if (mesh == null)
+                    {
+                        repData.meshState = MeshState.MeshFailed;
+                        return;
+                    }
+                }               
+            }
+
+            repData.meshState = MeshState.AssetOK;
+            repData.mesh = mesh;
+
+            if (pbs.SculptEntry)
+            {
+                repData.assetID = pbs.SculptTexture;
+            }
+
+            pbs.SculptData = Utils.EmptyBytes;
+            return ;
+        }
+
+        public void GetMesh(ODEPhysRepData repData)
+        {
+            PhysicsActor actor = repData.actor;
+
+            PrimitiveBaseShape pbs = repData.pbs;
+
+            repData.mesh = null;
+            repData.hasOBB = false;
+
+            if (!needsMeshing(pbs))
+            {
+                repData.meshState = MeshState.noNeed;
+                return;
+            }
+
+            if (repData.meshState == MeshState.MeshFailed)
+                return;
+
+            if (pbs.SculptEntry)
+            {
+                if (repData.meshState == MeshState.AssetFailed)
+                {
+                    if (pbs.SculptTexture == repData.assetID)
+                        return;
+                }
+            }
+
+            repData.meshState = MeshState.noNeed;
+
+            IMesh mesh = null;
+            Vector3 size = repData.size;
+            byte shapetype = repData.shapetype;
+
+            bool convex;
+            int clod = (int)LevelOfDetail.High;
+            if (shapetype == 0)
+                convex = false;
+            else
+            {
+                convex = true;
+                if (pbs.SculptType != (byte)SculptType.Mesh)
+                    clod = (int)LevelOfDetail.Low;
+            }
+
+            mesh = m_mesher.CreateMesh(actor.Name, pbs, size, clod, true, false, convex, true);
+   
+            if (mesh == null)
+            {
+                if (pbs.SculptEntry)
+                {
+                    if (pbs.SculptTexture == UUID.Zero)
+                        return;
+
+                    repData.assetID = pbs.SculptTexture;
+
+                    if (pbs.SculptData == null || pbs.SculptData.Length == 0)
+                    {
+                        repData.meshState = MeshState.needAsset;
+                        return;
+                    }
+                }
+            }
+
+            repData.mesh = mesh;
+            repData.pbs.SculptData = Utils.EmptyBytes;
+
+            if (mesh == null)
+            {
+                if (pbs.SculptEntry)
+                    repData.meshState = MeshState.AssetFailed;
+                else
+                    repData.meshState = MeshState.MeshFailed;
+
+                return;
+            }
+
+            repData.meshState = MeshState.AssetOK;
+
+            return;
+        }
+
+        private void CalculateBasicPrimVolume(ODEPhysRepData repData)
+        {
+            PrimitiveBaseShape _pbs = repData.pbs;
+            Vector3 _size = repData.size;
+
+            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.5236f;
+
+                        if (hollowAmount > 0.0)
+                        {
+                            hollowVolume *= hollowAmount;
+
+                            switch (_pbs.HollowShape)
+                            {
+                                case HollowShape.Circle:
+                                case HollowShape.Triangle:  // diference in sl is minor and odd
+                                case HollowShape.Same:
+                                    break;
+
+                                case HollowShape.Square:
+                                    hollowVolume *= 0.909f;
+                                    break;
+
+                                //                                case HollowShape.Triangle:
+                                //                                    hollowVolume *= .827f;
+                                //                                    break;
+                                default:
+                                    hollowVolume = 0;
+                                    break;
+                            }
+                            volume *= (1.0f - hollowVolume);
+                        }
+
+                    }
+                    break;
+
+                case ProfileShape.EquilateralTriangle:
+
+                    if (_pbs.PathCurve == (byte)Extrusion.Straight)
+                    {
+                        volume *= 0.32475953f;
+
+                        if (hollowAmount > 0.0)
+                        {
+
+                            // calculate the hollow volume by it's shape compared to the prim shape
+                            switch (_pbs.HollowShape)
+                            {
+                                case HollowShape.Same:
+                                case HollowShape.Triangle:
+                                    hollowVolume *= .25f;
+                                    break;
+
+                                case HollowShape.Square:
+                                    hollowVolume *= 0.499849f * 3.07920140172638f;
+                                    break;
+
+                                case HollowShape.Circle:
+                                    // Hollow shape is a perfect cyllinder in respect to the cube's scale
+                                    // Cyllinder hollow volume calculation
+
+                                    hollowVolume *= 0.1963495f * 3.07920140172638f;
+                                    break;
+
+                                default:
+                                    hollowVolume = 0;
+                                    break;
+                            }
+                            volume *= (1.0f - hollowVolume);
+                        }
+                    }
+                    else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
+                    {
+                        volume *= 0.32475953f;
+                        volume *= 0.01f * (float)(200 - _pbs.PathScaleX);
+                        tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY);
+                        volume *= (1.0f - tmp * tmp);
+
+                        if (hollowAmount > 0.0)
+                        {
+
+                            hollowVolume *= hollowAmount;
+
+                            switch (_pbs.HollowShape)
+                            {
+                                case HollowShape.Same:
+                                case HollowShape.Triangle:
+                                    hollowVolume *= .25f;
+                                    break;
+
+                                case HollowShape.Square:
+                                    hollowVolume *= 0.499849f * 3.07920140172638f;
+                                    break;
+
+                                case HollowShape.Circle:
+
+                                    hollowVolume *= 0.1963495f * 3.07920140172638f;
+                                    break;
+
+                                default:
+                                    hollowVolume = 0;
+                                    break;
+                            }
+                            volume *= (1.0f - hollowVolume);
+                        }
+                    }
+                    break;
+
+                default:
+                    break;
+            }
+
+            float taperX1;
+            float taperY1;
+            float taperX;
+            float taperY;
+            float pathBegin;
+            float pathEnd;
+            float profileBegin;
+            float profileEnd;
+
+            if (_pbs.PathCurve == (byte)Extrusion.Straight || _pbs.PathCurve == (byte)Extrusion.Flexible)
+            {
+                taperX1 = _pbs.PathScaleX * 0.01f;
+                if (taperX1 > 1.0f)
+                    taperX1 = 2.0f - taperX1;
+                taperX = 1.0f - taperX1;
+
+                taperY1 = _pbs.PathScaleY * 0.01f;
+                if (taperY1 > 1.0f)
+                    taperY1 = 2.0f - taperY1;
+                taperY = 1.0f - taperY1;
+            }
+            else
+            {
+                taperX = _pbs.PathTaperX * 0.01f;
+                if (taperX < 0.0f)
+                    taperX = -taperX;
+                taperX1 = 1.0f - taperX;
+
+                taperY = _pbs.PathTaperY * 0.01f;
+                if (taperY < 0.0f)
+                    taperY = -taperY;
+                taperY1 = 1.0f - taperY;
+            }
+
+            volume *= (taperX1 * taperY1 + 0.5f * (taperX1 * taperY + taperX * taperY1) + 0.3333333333f * taperX * taperY);
+
+            pathBegin = (float)_pbs.PathBegin * 2.0e-5f;
+            pathEnd = 1.0f - (float)_pbs.PathEnd * 2.0e-5f;
+            volume *= (pathEnd - pathBegin);
+
+            // this is crude aproximation
+            profileBegin = (float)_pbs.ProfileBegin * 2.0e-5f;
+            profileEnd = 1.0f - (float)_pbs.ProfileEnd * 2.0e-5f;
+            volume *= (profileEnd - profileBegin);           
+
+            repData.volume = volume;
+        }
+
+        private void CalcVolumeData(ODEPhysRepData repData)
+        {
+            if (repData.hasOBB)
+            {
+                Vector3 OBB = repData.OBB;
+            }
+            else
+            {
+                Vector3 OBB = repData.size;
+                OBB.X *= 0.5f;
+                OBB.Y *= 0.5f;
+                OBB.Z *= 0.5f;
+
+                repData.OBB = OBB;
+                repData.OBBOffset = Vector3.Zero;
+            }
+
+            CalculateBasicPrimVolume(repData);
+        }
+    }
+
+    public class ODEAssetRequest
+    {
+        ODEMeshWorker m_worker;
+        private ILog m_log;
+        ODEPhysRepData repData;
+
+        public ODEAssetRequest(ODEMeshWorker pWorker, RequestAssetDelegate provider,
+            ODEPhysRepData pRepData, ILog plog)
+        {
+            m_worker = pWorker;
+            m_log = plog;
+            repData = pRepData;
+
+            repData.meshState = MeshState.AssetFailed;
+            if (provider == null)
+                return;
+
+            if (repData.assetID == null)
+                return;
+
+            UUID assetID = (UUID) repData.assetID;
+            if (assetID == UUID.Zero)
+                return;
+
+            repData.meshState = MeshState.loadingAsset;
+            provider(assetID, ODEassetReceived);
+        }
+
+        void ODEassetReceived(AssetBase asset)
+        {
+            repData.meshState = MeshState.AssetFailed;
+            if (asset != null)
+            {
+                if (asset.Data != null && asset.Data.Length > 0)
+                {
+                    repData.meshState = MeshState.noNeed;
+
+                    if (!repData.pbs.SculptEntry)
+                        return;
+                    if (repData.pbs.SculptTexture != repData.assetID)
+                        return;
+
+//                    repData.pbs.SculptData = new byte[asset.Data.Length];
+//                    asset.Data.CopyTo(repData.pbs.SculptData,0);
+                    repData.pbs.SculptData = asset.Data;
+                    repData.meshState = MeshState.AssetOK;
+                    m_worker.AssetLoaded(repData);
+                }
+                else
+                    m_log.WarnFormat("[PHYSICS]: asset provider returned invalid mesh data for prim {0} asset UUID {1}.",
+                        repData.actor.Name, asset.ID.ToString());
+            }
+            else
+                m_log.WarnFormat("[PHYSICS]: asset provider returned null asset fo mesh of prim {0}.",
+                    repData.actor.Name);
+        }
+    }
+}
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs
new file mode 100644
index 0000000..faa9488
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs
@@ -0,0 +1,3844 @@
+/*

+ * 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;

+        private bool m_isphantom;

+        private bool m_fakeisphantom;

+        internal bool m_isVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively

+        private bool m_fakeisVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively

+

+        protected bool m_building;

+        protected bool m_forcePosOrRotation;

+        private bool m_iscolliding;

+

+        internal bool m_isSelected;

+        private bool m_delaySelect;

+        private bool m_lastdoneSelected;

+        internal bool m_outbounds;

+

+        private Quaternion m_lastorientation;

+        private Quaternion _orientation;

+

+        private Vector3 _position;

+        private Vector3 _velocity;

+        private Vector3 m_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;

+

+        private Vector3 m_force;

+        private Vector3 m_forceacc;

+        private Vector3 m_angularForceacc;

+

+        private float m_invTimeStep;

+        private float m_timeStep;

+

+        private Vector3 m_PIDTarget;

+        private float m_PIDTau;

+        private bool m_usePID;

+

+        private float m_PIDHoverHeight;

+        private float m_PIDHoverTau;

+        private bool m_useHoverPID;

+        private PIDHoverType m_PIDHoverType;

+        private float m_targetHoverHeight;

+        private float m_groundHeight;

+        private float m_waterHeight;

+        private float m_buoyancy;                //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle. 

+

+        private int body_autodisable_frames;

+        public int bodydisablecontrol;

+

+

+        // Default we're a Geometry

+        private CollisionCategories m_collisionCategories = (CollisionCategories.Geom);

+        // Default colide nonphysical don't try to colide with anything

+        private const CollisionCategories m_default_collisionFlagsNotPhysical = 0;

+

+        private const CollisionCategories m_default_collisionFlagsPhysical = (CollisionCategories.Geom |

+                                        CollisionCategories.Character |

+                                        CollisionCategories.Land |

+                                        CollisionCategories.VolumeDtc);

+

+//        private bool m_collidesLand = true;

+        private bool m_collidesWater;

+//        public bool m_returnCollisions;

+

+        private bool m_NoColide;  // for now only for internal use for bad meshs

+

+

+        // Default, Collide with Other Geometries, spaces and Bodies

+        private CollisionCategories m_collisionFlags = m_default_collisionFlagsNotPhysical;

+

+        public bool m_disabled;

+

+        private uint m_localID;

+

+        private IMesh m_mesh;

+        private object m_meshlock = new object();

+        private PrimitiveBaseShape _pbs;

+

+        private UUID? m_assetID;

+        private MeshState m_meshState;

+        

+        public OdeScene _parent_scene;

+

+        /// <summary>

+        /// The physics space which contains prim geometry

+        /// </summary>

+        public IntPtr m_targetSpace;

+

+        public IntPtr prim_geom;

+        public IntPtr _triMeshData;

+

+        private PhysicsActor _parent;

+

+        private List<OdePrim> childrenPrim = new List<OdePrim>();

+

+        public float m_collisionscore;

+        private int m_colliderfilter = 0;

+

+        public IntPtr collide_geom; // for objects: geom if single prim space it linkset

+

+        private float m_density;

+        private byte m_shapetype;

+        public bool _zeroFlag;

+        private bool m_lastUpdateSent;

+

+        public IntPtr Body;

+

+        private Vector3 _target_velocity;

+

+        public Vector3 m_OBBOffset;

+        public Vector3 m_OBB;

+        public float primOOBradiusSQ;

+

+        private bool m_hasOBB = true;

+

+        private float m_physCost;

+        private float m_streamCost;

+

+        public d.Mass primdMass; // prim inertia information on it's own referencial

+        float primMass; // prim own mass

+        float primVolume; // prim own volume;

+        float _mass; // object mass acording to case

+

+        public int givefakepos;

+        private Vector3 fakepos;

+        public int givefakeori;

+        private Quaternion fakeori;

+

+        private int m_eventsubscription;

+        private int m_cureventsubscription;

+        private CollisionEventUpdate CollisionEventsThisFrame = null;

+        private bool SentEmptyCollisionsEvent;

+

+        public volatile bool childPrim;

+

+        public ODEDynamics m_vehicle;

+

+        internal int m_material = (int)Material.Wood;

+        private float mu;

+        private float bounce;

+

+        /// <summary>

+        /// Is this prim subject to physics?  Even if not, it's still solid for collision purposes.

+        /// </summary>

+        public override bool IsPhysical  // this is not reliable for internal use

+        {

+            get { return m_fakeisphysical; }

+            set

+            {

+                m_fakeisphysical = value; // we show imediatly to outside that we changed physical

+                // and also to stop imediatly some updates

+                // but real change will only happen in taintprocessing

+

+                if (!value) // Zero the remembered last velocity

+                    m_lastVelocity = Vector3.Zero;

+                AddChange(changes.Physical, value);

+            }

+        }

+

+        public override bool IsVolumeDtc

+        {

+            get { return m_fakeisVolumeDetect; }

+            set

+            {

+                m_fakeisVolumeDetect = value;

+                AddChange(changes.VolumeDtc, value);

+            }

+        }

+

+        public override bool Phantom  // this is not reliable for internal use

+        {

+            get { return m_fakeisphantom; }

+            set

+            {

+                m_fakeisphantom = value;

+                AddChange(changes.Phantom, value);

+            }

+        }

+

+        public override bool Building  // this is not reliable for internal use

+        {

+            get { return m_building; }

+            set

+            {

+                if (value)

+                    m_building = true;

+                AddChange(changes.building, value);

+            }

+        }

+

+        public override void getContactData(ref ContactData cdata)

+        {

+            cdata.mu = mu;

+            cdata.bounce = bounce;

+

+            //            cdata.softcolide = m_softcolide;

+            cdata.softcolide = false;

+

+            if (m_isphysical)

+            {

+                ODEDynamics veh;

+                if (_parent != null)

+                    veh = ((OdePrim)_parent).m_vehicle;

+                else

+                    veh = m_vehicle;

+

+                if (veh != null && veh.Type != Vehicle.TYPE_NONE)

+                    cdata.mu *= veh.FrictionFactor;

+//                    cdata.mu *= 0;

+            }

+        }

+

+        public override float PhysicsCost

+        {

+            get

+            {

+                return m_physCost;

+            }

+        }

+

+        public override float StreamCost

+        {

+            get

+            {

+                return m_streamCost;

+            }

+        }

+

+        public override int PhysicsActorType

+        {

+            get { return (int)ActorTypes.Prim; }

+            set { return; }

+        }

+

+        public override bool SetAlwaysRun

+        {

+            get { return false; }

+            set { return; }

+        }

+

+        public override uint LocalID

+        {

+            get { return m_localID; }

+            set { m_localID = value; }

+        }

+

+        public override PhysicsActor ParentActor

+        {

+            get

+            {

+                if (childPrim)

+                    return _parent;

+                else

+                    return (PhysicsActor)this;

+            }

+        }

+

+        public override bool Grabbed

+        {

+            set { return; }

+        }

+

+        public override bool Selected

+        {

+            set

+            {

+                if (value)

+                    m_isSelected = value; // if true set imediatly to stop moves etc

+                AddChange(changes.Selected, value);

+            }

+        }

+

+        public override bool Flying

+        {

+            // no flying prims for you

+            get { return false; }

+            set { }

+        }

+

+        public override bool IsColliding

+        {

+            get { return m_iscolliding; }

+            set

+            {

+                if (value)

+                {

+                    m_colliderfilter += 2;

+                    if (m_colliderfilter > 2)

+                        m_colliderfilter = 2;

+                }

+                else

+                {

+                    m_colliderfilter--;

+                    if (m_colliderfilter < 0)

+                        m_colliderfilter = 0;

+                }

+

+                if (m_colliderfilter == 0)

+                    m_iscolliding = false;

+                else

+                    m_iscolliding = true;

+            }

+        }

+

+        public override bool CollidingGround

+        {

+            get { return false; }

+            set { return; }

+        }

+

+        public override bool CollidingObj

+        {

+            get { return false; }

+            set { return; }

+        }

+

+

+        public override bool ThrottleUpdates {get;set;}

+

+        public override bool Stopped

+        {

+            get { return _zeroFlag; }

+        }

+

+        public override Vector3 Position

+        {

+            get

+            {

+                if (givefakepos > 0)

+                    return fakepos;

+                else

+                    return _position;

+            }

+

+            set

+            {

+                fakepos = value;

+                givefakepos++;

+                AddChange(changes.Position, value);

+            }

+        }

+

+        public override Vector3 Size

+        {

+            get { return _size; }

+            set

+            {

+                if (value.IsFinite())

+                {

+                     _parent_scene.m_meshWorker.ChangeActorPhysRep(this, _pbs, value, m_shapetype);

+                }

+                else

+                {

+                    m_log.WarnFormat("[PHYSICS]: Got NaN Size on object {0}", Name);

+                }

+            }

+        }

+

+        public override float Mass

+        {

+            get { return primMass; }

+        }

+

+        public override Vector3 Force

+        {

+            get { return m_force; }

+            set

+            {

+                if (value.IsFinite())

+                {

+                    AddChange(changes.Force, value);

+                }

+                else

+                {

+                    m_log.WarnFormat("[PHYSICS]: NaN in Force Applied to an Object {0}", Name);

+                }

+            }

+        }

+

+        public override void SetVolumeDetect(int param)

+        {

+            m_fakeisVolumeDetect = (param != 0);

+            AddChange(changes.VolumeDtc, m_fakeisVolumeDetect);

+        }

+

+        public override Vector3 GeometricCenter

+        {

+            // this is not real geometric center but a average of positions relative to root prim acording to

+            // http://wiki.secondlife.com/wiki/llGetGeometricCenter

+            // ignoring tortured prims details since sl also seems to ignore

+            // so no real use in doing it on physics

+            get

+            {

+                return Vector3.Zero;

+            }

+        }

+

+        public override Vector3 CenterOfMass

+        {

+            get

+            {

+                lock (_parent_scene.OdeLock)

+                {

+                    d.Vector3 dtmp;

+                    if (!childPrim && Body != IntPtr.Zero)

+                    {

+                        dtmp = d.BodyGetPosition(Body);

+                        return new Vector3(dtmp.X, dtmp.Y, dtmp.Z);

+                    }

+                    else if (prim_geom != IntPtr.Zero)

+                    {

+                        d.Quaternion dq;

+                        d.GeomCopyQuaternion(prim_geom, out dq);

+                        Quaternion q;

+                        q.X = dq.X;

+                        q.Y = dq.Y;

+                        q.Z = dq.Z;

+                        q.W = dq.W;

+

+                        Vector3 Ptot = m_OBBOffset * q;

+                        dtmp = d.GeomGetPosition(prim_geom);

+                        Ptot.X += dtmp.X;

+                        Ptot.Y += dtmp.Y;

+                        Ptot.Z += dtmp.Z;

+

+                        //                    if(childPrim)  we only know about physical linksets

+                        return Ptot;

+/*

+                        float tmass = _mass;

+                        Ptot *= tmass;

+

+                        float m;

+

+                        foreach (OdePrim prm in childrenPrim)

+                        {

+                            m = prm._mass;

+                            Ptot += prm.CenterOfMass * m;

+                            tmass += m;

+                        }

+

+                        if (tmass == 0)

+                            tmass = 0;

+                        else

+                            tmass = 1.0f / tmass;

+

+                        Ptot *= tmass;

+                        return Ptot;

+*/

+                    }

+                    else

+                        return _position;

+                }

+            }

+        }

+

+        public override Vector3 OOBsize

+            {

+            get

+                {

+                return m_OBB;

+                }

+            }

+

+        public override Vector3 OOBoffset

+            {

+            get

+                {

+                return m_OBBOffset;

+                }

+            }

+

+        public override float OOBRadiusSQ

+            {

+            get

+                {

+                return primOOBradiusSQ;

+                }

+            }

+

+        public override PrimitiveBaseShape Shape

+        {

+            set

+            {

+//                AddChange(changes.Shape, value);

+                _parent_scene.m_meshWorker.ChangeActorPhysRep(this, value, _size, m_shapetype);

+            }

+        }

+

+        public override byte PhysicsShapeType

+        {

+            get

+            {

+                return m_shapetype;

+            }

+            set

+            {

+                m_shapetype = value;

+               _parent_scene.m_meshWorker.ChangeActorPhysRep(this, _pbs, _size, value);

+            }

+        }

+

+        public override Vector3 Velocity

+        {

+            get

+            {

+                if (_zeroFlag)

+                    return Vector3.Zero;

+                return _velocity;

+            }

+            set

+            {

+                if (value.IsFinite())

+                {

+                    AddChange(changes.Velocity, value);

+                }

+                else

+                {

+                    m_log.WarnFormat("[PHYSICS]: Got NaN Velocity in Object {0}", Name);

+                }

+

+            }

+        }

+

+        public override Vector3 Torque

+        {

+            get

+            {

+                if (!IsPhysical || Body == IntPtr.Zero)

+                    return Vector3.Zero;

+

+                return m_torque;

+            }

+

+            set

+            {

+                if (value.IsFinite())

+                {

+                    AddChange(changes.Torque, value);

+                }

+                else

+                {

+                    m_log.WarnFormat("[PHYSICS]: Got NaN Torque in Object {0}", Name);

+                }

+            }

+        }

+

+        public override float CollisionScore

+        {

+            get { return m_collisionscore; }

+            set { m_collisionscore = value; }

+        }

+

+        public override bool Kinematic

+        {

+            get { return false; }

+            set { }

+        }

+

+        public override Quaternion Orientation

+        {

+            get

+            {

+                if (givefakeori > 0)

+                    return fakeori;

+                else

+

+                    return _orientation;

+            }

+            set

+            {

+                if (QuaternionIsFinite(value))

+                {

+                    fakeori = value;

+                    givefakeori++;

+

+                    value.Normalize();

+

+                    AddChange(changes.Orientation, value);

+                }

+                else

+                    m_log.WarnFormat("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object {0}", Name);

+

+            }

+        }

+

+        public override Vector3 Acceleration

+        {

+            get { return _acceleration; }

+            set { }

+        }

+

+        public override Vector3 RotationalVelocity

+        {

+            get

+            {

+                Vector3 pv = Vector3.Zero;

+                if (_zeroFlag)

+                    return pv;

+

+                if (m_rotationalVelocity.ApproxEquals(pv, 0.0001f))

+                    return pv;

+

+                return m_rotationalVelocity;

+            }

+            set

+            {

+                if (value.IsFinite())

+                {

+                    AddChange(changes.AngVelocity, value);

+                }

+                else

+                {

+                    m_log.WarnFormat("[PHYSICS]: Got NaN RotationalVelocity in Object {0}", Name);

+                }

+            }

+        }

+

+        public override float Buoyancy

+        {

+            get { return m_buoyancy; }

+            set

+            {

+                 AddChange(changes.Buoyancy,value);

+            }

+        }

+

+        public override bool FloatOnWater

+        {

+            set

+            {

+                AddChange(changes.CollidesWater, value);

+            }

+        }

+

+        public override Vector3 PIDTarget

+        {

+            set

+            {

+                if (value.IsFinite())

+                {

+                    AddChange(changes.PIDTarget,value);

+                }

+                else

+                    m_log.WarnFormat("[PHYSICS]: Got NaN PIDTarget from Scene on Object {0}", Name);

+            }

+        }

+

+        public override bool PIDActive

+        {

+            set

+            {

+                AddChange(changes.PIDActive,value);

+            }

+        }

+

+        public override float PIDTau

+        {

+            set

+            {

+                float tmp = 0;

+                if (value > 0)

+                {

+                    float mint = (0.05f > m_timeStep ? 0.05f : m_timeStep);

+                    if (value < mint)

+                        tmp = mint;

+                    else

+                        tmp = value;

+                }

+                AddChange(changes.PIDTau,tmp);

+            }

+        }

+

+        public override float PIDHoverHeight

+        {

+            set

+            {

+                AddChange(changes.PIDHoverHeight,value);

+            }

+        }

+        public override bool PIDHoverActive

+        {

+            set

+            {

+                AddChange(changes.PIDHoverActive, value);

+            }

+        }

+

+        public override PIDHoverType PIDHoverType

+        {

+            set

+            {

+                AddChange(changes.PIDHoverType,value);

+            }

+        }

+

+        public override float PIDHoverTau

+        {

+            set

+            {

+                float tmp =0;

+                if (value > 0)

+                {

+                    float mint = (0.05f > m_timeStep ? 0.05f : m_timeStep);

+                    if (value < mint)

+                        tmp = mint;

+                    else

+                        tmp = value;

+                }

+                AddChange(changes.PIDHoverTau, tmp);

+            }

+        }

+

+        public override Quaternion APIDTarget { set { return; } }

+

+        public override bool APIDActive { set { return; } }

+

+        public override float APIDStrength { set { return; } }

+

+        public override float APIDDamping { set { return; } }

+

+        public override int VehicleType

+        {

+            // we may need to put a fake on this

+            get

+            {

+                if (m_vehicle == null)

+                    return (int)Vehicle.TYPE_NONE;

+                else

+                    return (int)m_vehicle.Type;

+            }

+            set

+            {

+                AddChange(changes.VehicleType, value);

+            }

+        }

+

+        public override void VehicleFloatParam(int param, float value)

+        {

+            strVehicleFloatParam fp = new strVehicleFloatParam();

+            fp.param = param;

+            fp.value = value;

+            AddChange(changes.VehicleFloatParam, fp);

+        }

+

+        public override void VehicleVectorParam(int param, Vector3 value)

+        {

+            strVehicleVectorParam fp = new strVehicleVectorParam();

+            fp.param = param;

+            fp.value = value;

+            AddChange(changes.VehicleVectorParam, fp);

+        }

+

+        public override void VehicleRotationParam(int param, Quaternion value)

+        {

+            strVehicleQuatParam fp = new strVehicleQuatParam();

+            fp.param = param;

+            fp.value = value;

+            AddChange(changes.VehicleRotationParam, fp);

+        }

+

+        public override void VehicleFlags(int param, bool value)

+        {

+            strVehicleBoolParam bp = new strVehicleBoolParam();

+            bp.param = param;

+            bp.value = value;

+            AddChange(changes.VehicleFlags, bp);

+        }

+

+        public override void SetVehicle(object vdata)

+        {

+            AddChange(changes.SetVehicle, vdata);

+        }

+        public void SetAcceleration(Vector3 accel)

+        {

+            _acceleration = accel;

+        }

+

+        public override void AddForce(Vector3 force, bool pushforce)

+        {

+            if (force.IsFinite())

+            {

+                if(pushforce)

+                    AddChange(changes.AddForce, force);

+                else // a impulse

+                    AddChange(changes.AddForce, force * m_invTimeStep);

+            }

+            else

+            {

+                m_log.WarnFormat("[PHYSICS]: Got Invalid linear force vector from Scene in Object {0}", Name);

+            }

+            //m_log.Info("[PHYSICS]: Added Force:" + force.ToString() +  " to prim at " + Position.ToString());

+        }

+

+        public override void AddAngularForce(Vector3 force, bool pushforce)

+        {

+            if (force.IsFinite())

+            {

+//                if(pushforce)  for now applyrotationimpulse seems more happy applied as a force

+                    AddChange(changes.AddAngForce, force);

+//                else // a impulse

+//                    AddChange(changes.AddAngForce, force * m_invTimeStep);

+            }

+            else

+            {

+                m_log.WarnFormat("[PHYSICS]: Got Invalid Angular force vector from Scene in Object {0}", Name);

+            }

+        }

+

+        public override void CrossingFailure()

+        {

+            if (m_outbounds)

+            {

+                _position.X = Util.Clip(_position.X, 0.5f, _parent_scene.WorldExtents.X - 0.5f);

+                _position.Y = Util.Clip(_position.Y, 0.5f, _parent_scene.WorldExtents.Y - 0.5f);

+                _position.Z = Util.Clip(_position.Z + 0.2f, -100f, 50000f);

+

+                m_lastposition = _position;

+                _velocity.X = 0;

+                _velocity.Y = 0;

+                _velocity.Z = 0;

+

+                m_lastVelocity = _velocity;

+                if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE)

+                    m_vehicle.Stop();

+

+                if(Body != IntPtr.Zero)

+                    d.BodySetLinearVel(Body, 0, 0, 0); // stop it

+                if (prim_geom != IntPtr.Zero)

+                    d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);

+

+                m_outbounds = false;

+                changeDisable(false);

+                base.RequestPhysicsterseUpdate();

+            }

+        }

+

+        public override void SetMomentum(Vector3 momentum)

+        {

+        }

+

+        public override void SetMaterial(int pMaterial)

+        {

+            m_material = pMaterial;

+            mu = _parent_scene.m_materialContactsData[pMaterial].mu;

+            bounce = _parent_scene.m_materialContactsData[pMaterial].bounce;

+        }

+

+        public 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;

+            m_cureventsubscription = 0;

+            if (CollisionEventsThisFrame == null)

+                CollisionEventsThisFrame = new CollisionEventUpdate();

+            SentEmptyCollisionsEvent = false;

+        }

+

+        public override void UnSubscribeEvents()

+        {

+            if (CollisionEventsThisFrame != null)

+            {

+                CollisionEventsThisFrame.Clear();

+                CollisionEventsThisFrame = null;

+            }

+            m_eventsubscription = 0;

+           _parent_scene.RemoveCollisionEventReporting(this);

+        }

+

+        public override void AddCollisionEvent(uint CollidedWith, ContactPoint contact)

+        {

+            if (CollisionEventsThisFrame == null)

+                CollisionEventsThisFrame = new CollisionEventUpdate();

+//            if(CollisionEventsThisFrame.Count < 32)

+                CollisionEventsThisFrame.AddCollider(CollidedWith, contact);

+        }

+

+        public void SendCollisions()

+        {

+            if (CollisionEventsThisFrame == null)

+                return;

+

+            if (m_cureventsubscription < m_eventsubscription)

+                return;

+

+            m_cureventsubscription = 0;

+

+            int ncolisions = CollisionEventsThisFrame.m_objCollisionList.Count;

+

+            if (!SentEmptyCollisionsEvent || ncolisions > 0)

+            {

+                base.SendCollisionUpdate(CollisionEventsThisFrame);

+

+                if (ncolisions == 0)

+                {

+                    SentEmptyCollisionsEvent = true;

+                    _parent_scene.RemoveCollisionEventReporting(this);

+                }

+                else

+                {

+                    SentEmptyCollisionsEvent = false;

+                    CollisionEventsThisFrame.Clear();

+                }

+            }           

+        }

+

+        internal void AddCollisionFrameTime(int t)

+        {

+            if (m_cureventsubscription < 50000)

+                m_cureventsubscription += t;

+        }

+

+        public override bool SubscribedEvents()

+        {

+            if (m_eventsubscription > 0)

+                return true;

+            return false;

+        }

+

+        public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size,

+                       Quaternion rotation, PrimitiveBaseShape pbs, bool pisPhysical,bool pisPhantom,byte _shapeType,uint plocalID)

+        {

+            Name = primName;

+            LocalID = plocalID;

+

+            m_vehicle = null;

+

+            if (!pos.IsFinite())

+            {

+                pos = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f),

+                    parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f);

+                m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Position for {0}", Name);

+            }

+            _position = pos;

+            givefakepos = 0;

+

+            m_timeStep = parent_scene.ODE_STEPSIZE;

+            m_invTimeStep = 1f / m_timeStep;

+

+            m_density = parent_scene.geomDefaultDensity;

+            body_autodisable_frames = parent_scene.bodyFramesAutoDisable;

+

+            prim_geom = IntPtr.Zero;

+            collide_geom = IntPtr.Zero;

+            Body = IntPtr.Zero;

+

+            if (!size.IsFinite())

+            {

+                size = new Vector3(0.5f, 0.5f, 0.5f);

+                m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Size for {0}", Name);

+            }

+

+            if (size.X <= 0) size.X = 0.01f;

+            if (size.Y <= 0) size.Y = 0.01f;

+            if (size.Z <= 0) size.Z = 0.01f;

+

+            _size = size;

+

+            if (!QuaternionIsFinite(rotation))

+            {

+                rotation = Quaternion.Identity;

+                m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Rotation for {0}", Name);

+            }

+

+            _orientation = rotation;

+            givefakeori = 0;

+

+            _pbs = pbs;

+

+            _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_fakeisVolumeDetect = false;

+

+            m_force = Vector3.Zero;

+

+            m_iscolliding = false;

+            m_colliderfilter = 0;

+            m_NoColide = false;

+

+            _triMeshData = IntPtr.Zero;

+

+            m_shapetype = _shapeType;

+

+            m_lastdoneSelected = false;

+            m_isSelected = false;

+            m_delaySelect = false;

+

+            m_isphantom = pisPhantom;

+            m_fakeisphantom = pisPhantom;

+

+            mu = parent_scene.m_materialContactsData[(int)Material.Wood].mu;

+            bounce = parent_scene.m_materialContactsData[(int)Material.Wood].bounce;

+

+            m_building = true; // control must set this to false when done

+

+            // get basic mass parameters

+            ODEPhysRepData repData = _parent_scene.m_meshWorker.NewActorPhysRep(this, _pbs, _size, m_shapetype);

+

+            primVolume = repData.volume;

+            m_OBB = repData.OBB;

+            m_OBBOffset = repData.OBBOffset;

+

+            UpdatePrimBodyData();

+        }

+

+        private void resetCollisionAccounting()

+        {

+            m_collisionscore = 0;

+        }

+

+        private void UpdateCollisionCatFlags()

+        {

+            if(m_isphysical && m_disabled)

+            {

+                m_collisionCategories = 0;

+                m_collisionFlags = 0;

+            }

+

+            else if (m_isSelected)

+            {

+                m_collisionCategories = CollisionCategories.Selected;

+                m_collisionFlags = 0;

+            }

+

+            else if (m_isVolumeDetect)

+            {

+                m_collisionCategories = CollisionCategories.VolumeDtc;

+                if (m_isphysical)

+                    m_collisionFlags = CollisionCategories.Geom | CollisionCategories.Character;

+                else

+                    m_collisionFlags = 0;

+            }

+            else if (m_isphantom)

+            {

+                m_collisionCategories = CollisionCategories.Phantom;

+                if (m_isphysical)

+                    m_collisionFlags = CollisionCategories.Land;

+                else

+                    m_collisionFlags = 0;

+            }

+            else

+            {

+                m_collisionCategories = CollisionCategories.Geom;

+                if (m_isphysical)

+                    m_collisionFlags = m_default_collisionFlagsPhysical;

+                else

+                    m_collisionFlags = m_default_collisionFlagsNotPhysical;

+            }

+        }

+

+        private void ApplyCollisionCatFlags()

+        {

+            if (prim_geom != IntPtr.Zero)

+            {

+                if (!childPrim && childrenPrim.Count > 0)

+                {

+                    foreach (OdePrim prm in childrenPrim)

+                    {

+                        if (m_isphysical && m_disabled)

+                        {

+                            prm.m_collisionCategories = 0;

+                            prm.m_collisionFlags = 0;

+                        }

+                        else

+                        {

+                            // preserve some

+                            if (prm.m_isSelected)

+                            {

+                                prm.m_collisionCategories = CollisionCategories.Selected;

+                                prm.m_collisionFlags = 0;

+                            }

+                            else if (prm.m_isVolumeDetect)

+                            {

+                                prm.m_collisionCategories = CollisionCategories.VolumeDtc;

+                                if (m_isphysical)

+                                    prm.m_collisionFlags = CollisionCategories.Geom | CollisionCategories.Character;

+                                else

+                                    prm.m_collisionFlags = 0;

+                            }

+                            else if (prm.m_isphantom)

+                            {

+                                prm.m_collisionCategories = CollisionCategories.Phantom;

+                                if (m_isphysical)

+                                    prm.m_collisionFlags = CollisionCategories.Land;

+                                else

+                                    prm.m_collisionFlags = 0;

+                            }

+                            else

+                            {

+                                prm.m_collisionCategories = m_collisionCategories;

+                                prm.m_collisionFlags = m_collisionFlags;

+                            }

+                        }

+

+                        if (prm.prim_geom != IntPtr.Zero)

+                        {

+                            if (prm.m_NoColide)

+                            {

+                                d.GeomSetCategoryBits(prm.prim_geom, 0);

+                                if (m_isphysical)

+                                    d.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land);

+                                else

+                                    d.GeomSetCollideBits(prm.prim_geom, 0);

+                            }

+                            else

+                            {

+                                d.GeomSetCategoryBits(prm.prim_geom, (uint)prm.m_collisionCategories);

+                                d.GeomSetCollideBits(prm.prim_geom, (uint)prm.m_collisionFlags);

+                            }

+                        }

+                    }

+                }

+

+                if (m_NoColide)

+                {

+                    d.GeomSetCategoryBits(prim_geom, 0);

+                    d.GeomSetCollideBits(prim_geom, (uint)CollisionCategories.Land);

+                    if (collide_geom != prim_geom && collide_geom != IntPtr.Zero)

+                    {

+                        d.GeomSetCategoryBits(collide_geom, 0);

+                        d.GeomSetCollideBits(collide_geom, (uint)CollisionCategories.Land);

+                    }

+                }

+                else

+                {

+                    d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories);

+                    d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags);

+                    if (collide_geom != prim_geom && collide_geom != IntPtr.Zero)

+                    {

+                        d.GeomSetCategoryBits(collide_geom, (uint)m_collisionCategories);

+                        d.GeomSetCollideBits(collide_geom, (uint)m_collisionFlags);

+                    }

+                }

+            }

+        }

+

+        private void createAMotor(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;

+

+            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)d.JointParam.StopCFM, 0f);

+                d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopERP, 0.8f);

+                i++;

+                j = 256; // move 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)d.JointParam.StopCFM, 0f);

+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.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)d.JointParam.StopCFM, 0f);

+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f);

+            }

+        }

+

+

+        private void SetGeom(IntPtr geom)

+        {

+            prim_geom = geom;

+            //Console.WriteLine("SetGeom to " + prim_geom + " for " + Name);

+            if (prim_geom != IntPtr.Zero)

+            {

+

+                if (m_NoColide)

+                {

+                    d.GeomSetCategoryBits(prim_geom, 0);

+                    if (m_isphysical)

+                    {

+                        d.GeomSetCollideBits(prim_geom, (uint)CollisionCategories.Land);

+                    }

+                    else

+                    {

+                        d.GeomSetCollideBits(prim_geom, 0);

+                        d.GeomDisable(prim_geom);

+                    }

+                }

+                else

+                {

+                    d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories);

+                    d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags);

+                }

+

+                UpdatePrimBodyData();

+                _parent_scene.actor_name_map[prim_geom] = this;

+

+/*

+// debug

+                d.AABB aabb;

+                d.GeomGetAABB(prim_geom, out aabb);

+                float x = aabb.MaxX - aabb.MinX;

+                float y = aabb.MaxY - aabb.MinY;

+                float z = aabb.MaxZ - aabb.MinZ;

+                if( x > 60.0f || y > 60.0f || z > 60.0f)

+                    m_log.WarnFormat("[PHYSICS]: large prim geo {0},size {1}, AABBsize <{2},{3},{4}, mesh {5} at {6}",

+                        Name, _size.ToString(), x, y, z, _pbs.SculptEntry ? _pbs.SculptTexture.ToString() : "primMesh", _position.ToString());

+                else if (x < 0.001f || y < 0.001f || z < 0.001f)

+                    m_log.WarnFormat("[PHYSICS]: small prim geo {0},size {1}, AABBsize <{2},{3},{4}, mesh {5} at {6}",

+                        Name, _size.ToString(), x, y, z, _pbs.SculptEntry ? _pbs.SculptTexture.ToString() : "primMesh", _position.ToString());

+

+//

+*/

+

+            }

+            else

+                m_log.Warn("Setting bad Geom");

+        }

+

+        private bool GetMeshGeom()

+        {

+            IntPtr vertices, indices;

+            int vertexCount, indexCount;

+            int vertexStride, triStride;

+            

+            IMesh mesh = m_mesh;

+

+            if (mesh == null)

+                return false;

+

+            mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount);

+            mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount);

+

+            if (vertexCount == 0 || indexCount == 0)

+            {

+                m_log.WarnFormat("[PHYSICS]: Invalid mesh data on OdePrim {0}, mesh {1} at {2}",

+                    Name, _pbs.SculptEntry ? _pbs.SculptTexture.ToString() : "primMesh",_position.ToString());

+

+                m_hasOBB = false;

+                m_OBBOffset = Vector3.Zero;

+                m_OBB = _size * 0.5f;

+

+                m_physCost = 0.1f;

+                m_streamCost = 1.0f;

+

+                _parent_scene.mesher.ReleaseMesh(mesh);

+                m_meshState = MeshState.MeshFailed;

+                m_mesh = null;

+                return false;

+            }

+

+            IntPtr geo = IntPtr.Zero;

+

+            try

+            {

+                _triMeshData = d.GeomTriMeshDataCreate();

+

+                d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride);

+                d.GeomTriMeshDataPreprocess(_triMeshData);

+

+                geo = d.CreateTriMesh(m_targetSpace, _triMeshData, null, null, null);

+            }

+

+            catch (Exception e)

+            {

+                m_log.ErrorFormat("[PHYSICS]: SetGeom Mesh failed for {0} exception: {1}", Name, e);

+                if (_triMeshData != IntPtr.Zero)

+                {

+                    try

+                    {

+                        d.GeomTriMeshDataDestroy(_triMeshData);

+                    }

+                    catch

+                    {

+                    }

+                }

+                _triMeshData = IntPtr.Zero;

+

+                m_hasOBB = false;

+                m_OBBOffset = Vector3.Zero;

+                m_OBB = _size * 0.5f;

+                m_physCost = 0.1f;

+                m_streamCost = 1.0f;

+              

+                _parent_scene.mesher.ReleaseMesh(mesh);

+                m_meshState = MeshState.MeshFailed;

+                m_mesh = null;

+                return false;

+            }

+

+            m_physCost = 0.0013f * (float)indexCount;

+            // todo

+            m_streamCost = 1.0f;

+

+            SetGeom(geo);

+

+            return true;

+        }

+

+        private void CreateGeom()

+        {

+            bool hasMesh = false;

+

+            m_NoColide = false;

+

+            if ((m_meshState & MeshState.MeshNoColide) != 0)

+                m_NoColide = true;

+

+            else if(m_mesh != null)

+            {

+                if (GetMeshGeom())

+                    hasMesh = true;

+                else

+                    m_NoColide = true;

+            }

+

+

+            if (!hasMesh)

+            {

+                IntPtr geo = IntPtr.Zero;

+

+                if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1

+                    && _size.X == _size.Y && _size.Y == _size.Z)

+                { // it's a sphere

+                    try

+                    {

+                        geo = d.CreateSphere(m_targetSpace, _size.X * 0.5f);

+                    }

+                    catch (Exception e)

+                    {

+                        m_log.WarnFormat("[PHYSICS]: Create sphere failed: {0}", e);

+                        return;

+                    }

+                }

+                else

+                {// do it as a box

+                    try

+                    {

+                        geo = d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z);

+                    }

+                    catch (Exception e)

+                    {

+                        m_log.Warn("[PHYSICS]: Create box failed: {0}", e);

+                        return;

+                    }

+                }

+                m_physCost = 0.1f;

+                m_streamCost = 1.0f;

+                SetGeom(geo);

+            }

+        }

+

+        private void RemoveGeom()

+        {

+            if (prim_geom != IntPtr.Zero)

+            {

+                _parent_scene.actor_name_map.Remove(prim_geom);

+

+                try

+                {

+                    d.GeomDestroy(prim_geom);

+                    if (_triMeshData != IntPtr.Zero)

+                    {

+                        d.GeomTriMeshDataDestroy(_triMeshData);

+                        _triMeshData = IntPtr.Zero;

+                    }

+                }

+                catch (Exception e)

+                {

+                    m_log.ErrorFormat("[PHYSICS]: PrimGeom destruction failed for {0} exception {1}", Name, e);

+                }

+

+                prim_geom = IntPtr.Zero;

+                collide_geom = IntPtr.Zero;

+                m_targetSpace = IntPtr.Zero;

+            }

+            else

+            {

+                m_log.ErrorFormat("[PHYSICS]: PrimGeom destruction BAD {0}", Name);

+            }

+

+            lock (m_meshlock)

+            {

+                if (m_mesh != null)

+                {

+                    _parent_scene.mesher.ReleaseMesh(m_mesh);

+                    m_mesh = null;

+                }

+            }

+

+            Body = IntPtr.Zero;

+            m_hasOBB = false;

+        }

+

+        //sets non physical prim m_targetSpace to right space in spaces grid for static prims

+        // should only be called for non physical prims unless they are becoming non physical

+        private void SetInStaticSpace(OdePrim prim)

+        {

+            IntPtr targetSpace = _parent_scene.MoveGeomToStaticSpace(prim.prim_geom, prim._position, prim.m_targetSpace);

+            prim.m_targetSpace = targetSpace;

+            collide_geom = IntPtr.Zero;

+        }

+

+        public void enableBodySoft()

+        {

+            m_disabled = false;

+            if (!childPrim && !m_isSelected)

+            {

+                if (m_isphysical && Body != IntPtr.Zero)

+                {

+                    UpdateCollisionCatFlags();

+                    ApplyCollisionCatFlags();

+

+                    d.BodyEnable(Body);

+                }

+            }

+            resetCollisionAccounting();

+        }

+

+        private void disableBodySoft()

+        {

+            m_disabled = true;

+            if (!childPrim)

+            {

+                if (m_isphysical && Body != IntPtr.Zero)

+                {

+                    if (m_isSelected)

+                        m_collisionFlags = CollisionCategories.Selected;

+                    else

+                        m_collisionCategories = 0;

+                    m_collisionFlags = 0;

+                    ApplyCollisionCatFlags();

+                    d.BodyDisable(Body);

+                }

+            }

+        }

+

+        private void MakeBody()

+        {

+            if (!m_isphysical) // only physical get bodies

+                return;

+

+            if (childPrim)  // child prims don't get bodies;

+                return;

+

+            if (m_building)

+                return;

+

+            if (prim_geom == IntPtr.Zero)

+            {

+                m_log.Warn("[PHYSICS]: Unable to link the linkset.  Root has no geom yet");

+                return;

+            }

+

+            if (Body != IntPtr.Zero)

+            {

+                DestroyBody();

+                m_log.Warn("[PHYSICS]: MakeBody called having a body");

+            }

+

+            if (d.GeomGetBody(prim_geom) != IntPtr.Zero)

+            {

+                d.GeomSetBody(prim_geom, IntPtr.Zero);

+                m_log.Warn("[PHYSICS]: MakeBody root geom already had a body");

+            }

+

+            d.Matrix3 mymat = new d.Matrix3();

+            d.Quaternion myrot = new d.Quaternion();

+            d.Mass objdmass = new d.Mass { };

+

+            Body = d.BodyCreate(_parent_scene.world);

+

+            objdmass = primdMass;

+

+            // 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

+            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;

+                rcm.Y = _position.Y;

+                rcm.Z = _position.Z;

+

+                lock (childrenPrim)

+                {

+                    foreach (OdePrim prm in childrenPrim)

+                    {

+                        if (prm.prim_geom == IntPtr.Zero)

+                        {

+                            m_log.Warn("[PHYSICS]: Unable to link one of the linkset elements, skipping it.  No geom yet");

+                            continue;

+                        }

+

+                        tmpdmass = prm.primdMass;

+

+                        // 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 -= rcm.X;

+                        ppos.Y -= rcm.Y;

+                        ppos.Z -= rcm.Z;

+                        // refer inertia to root prim center of mass position

+                        d.MassTranslate(ref tmpdmass,

+                            ppos.X,

+                            ppos.Y,

+                            ppos.Z);

+

+                        d.MassAdd(ref objdmass, ref tmpdmass); // add to total object inertia

+                        // 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.X = -myrot.X;

+            myrot.Y = -myrot.Y;

+            myrot.Z = -myrot.Z;

+

+            d.RfromQ(out mymat, ref myrot);

+            d.MassRotate(ref objdmass, ref mymat);

+

+            d.BodySetMass(Body, ref objdmass);

+            _mass = objdmass.mass;

+

+            // disconnect from world gravity so we can apply buoyancy

+            d.BodySetGravityMode(Body, false);

+

+            d.BodySetAutoDisableFlag(Body, true);

+            d.BodySetAutoDisableSteps(Body, body_autodisable_frames);

+            d.BodySetDamping(Body, .005f, .005f);

+

+            if (m_targetSpace != IntPtr.Zero)

+            {

+                _parent_scene.waitForSpaceUnlock(m_targetSpace);

+                if (d.SpaceQuery(m_targetSpace, prim_geom))

+                    d.SpaceRemove(m_targetSpace, prim_geom);

+            }

+

+            if (childrenPrim.Count == 0)

+            {

+                collide_geom = prim_geom;

+                m_targetSpace = _parent_scene.ActiveSpace;               

+            }

+            else

+            {

+                m_targetSpace = d.HashSpaceCreate(_parent_scene.ActiveSpace);

+                d.HashSpaceSetLevels(m_targetSpace, -2, 8);

+                d.SpaceSetSublevel(m_targetSpace, 3);

+                d.SpaceSetCleanup(m_targetSpace, false);

+

+                d.GeomSetCategoryBits(m_targetSpace, (uint)(CollisionCategories.Space |

+                                                            CollisionCategories.Geom |

+                                                            CollisionCategories.Phantom |

+                                                            CollisionCategories.VolumeDtc

+                                                            ));

+                d.GeomSetCollideBits(m_targetSpace, 0);

+                collide_geom = m_targetSpace;

+            }

+

+            d.SpaceAdd(m_targetSpace, prim_geom);

+

+            if (m_delaySelect)

+            {

+                m_isSelected = true;

+                m_delaySelect = false;

+            }

+

+            m_collisionscore = 0;

+

+            UpdateCollisionCatFlags();

+            ApplyCollisionCatFlags();

+

+            _parent_scene.addActivePrim(this);

+

+            lock (childrenPrim)

+            {

+                foreach (OdePrim prm in childrenPrim)

+                {

+                    if (prm.prim_geom == IntPtr.Zero)

+                        continue;

+

+                    Vector3 ppos = prm._position;

+                    d.GeomSetOffsetWorldPosition(prm.prim_geom, ppos.X, ppos.Y, ppos.Z); // set relative position

+

+                    if (prm.m_targetSpace != m_targetSpace)

+                    {

+                        if (prm.m_targetSpace != IntPtr.Zero)

+                        {

+                            _parent_scene.waitForSpaceUnlock(prm.m_targetSpace);

+                            if (d.SpaceQuery(prm.m_targetSpace, prm.prim_geom))

+                                d.SpaceRemove(prm.m_targetSpace, prm.prim_geom);

+                        }

+                        prm.m_targetSpace = m_targetSpace;

+                        d.SpaceAdd(m_targetSpace, prm.prim_geom);

+                    }

+

+                    prm.m_collisionscore = 0;

+

+                    if(!m_disabled)

+                        prm.m_disabled = false;

+

+                    _parent_scene.addActivePrim(prm);

+                }

+            }

+

+            // The body doesn't already have a finite rotation mode set here

+            if ((!m_angularlock.ApproxEquals(Vector3.One, 0.0f)) && _parent == null)

+            {

+                createAMotor(m_angularlock);

+            }

+

+

+            if (m_isSelected || m_disabled)

+            {

+                d.BodyDisable(Body);

+            }

+            else

+            {

+                d.BodySetAngularVel(Body, m_rotationalVelocity.X, m_rotationalVelocity.Y, m_rotationalVelocity.Z);

+                d.BodySetLinearVel(Body, _velocity.X, _velocity.Y, _velocity.Z);

+            }               

+            _parent_scene.addActiveGroups(this);

+        }

+

+        private void DestroyBody()

+        {

+            if (Body != IntPtr.Zero)

+            {

+                _parent_scene.remActivePrim(this);

+

+                collide_geom = IntPtr.Zero;

+

+                if (m_disabled)

+                    m_collisionCategories = 0;

+                else if (m_isSelected)

+                    m_collisionCategories = CollisionCategories.Selected;

+                else if (m_isVolumeDetect)

+                    m_collisionCategories = CollisionCategories.VolumeDtc;

+                else if (m_isphantom)

+                    m_collisionCategories = CollisionCategories.Phantom;

+                else

+                    m_collisionCategories = CollisionCategories.Geom;

+

+                m_collisionFlags = 0;

+

+                if (prim_geom != IntPtr.Zero)

+                {

+                    if (m_NoColide)

+                    {

+                        d.GeomSetCategoryBits(prim_geom, 0);

+                        d.GeomSetCollideBits(prim_geom, 0);

+                    }

+                    else

+                    {

+                        d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories);

+                        d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags);

+                    }

+                    UpdateDataFromGeom();

+                    d.GeomSetBody(prim_geom, IntPtr.Zero);

+                    SetInStaticSpace(this);

+                }

+

+                if (!childPrim)

+                {

+                    lock (childrenPrim)

+                    {

+                        foreach (OdePrim prm in childrenPrim)

+                        {

+                            _parent_scene.remActivePrim(prm);

+

+                            if (prm.m_isSelected)

+                                prm.m_collisionCategories = CollisionCategories.Selected;

+                            else if (prm.m_isVolumeDetect)

+                                prm.m_collisionCategories = CollisionCategories.VolumeDtc;

+                            else if (prm.m_isphantom)

+                                prm.m_collisionCategories = CollisionCategories.Phantom;

+                            else

+                                prm.m_collisionCategories = CollisionCategories.Geom;

+

+                            prm.m_collisionFlags = 0;

+

+                            if (prm.prim_geom != IntPtr.Zero)

+                            {

+                                if (prm.m_NoColide)

+                                {

+                                    d.GeomSetCategoryBits(prm.prim_geom, 0);

+                                    d.GeomSetCollideBits(prm.prim_geom, 0);

+                                }

+                                else

+                                {

+                                    d.GeomSetCategoryBits(prm.prim_geom, (uint)prm.m_collisionCategories);

+                                    d.GeomSetCollideBits(prm.prim_geom, (uint)prm.m_collisionFlags);

+                                }

+                                prm.UpdateDataFromGeom();

+                                SetInStaticSpace(prm);

+                            }

+                            prm.Body = IntPtr.Zero;

+                            prm._mass = prm.primMass;

+                            prm.m_collisionscore = 0;

+                        }

+                    }

+                    if (Amotor != IntPtr.Zero)

+                    {

+                        d.JointDestroy(Amotor);

+                        Amotor = IntPtr.Zero;

+                    }

+                    _parent_scene.remActiveGroup(this);

+                    d.BodyDestroy(Body);

+                }

+                Body = IntPtr.Zero;

+            }

+            _mass = primMass;

+            m_collisionscore = 0;

+        }

+

+        private void FixInertia(Vector3 NewPos,Quaternion newrot)

+        {

+            d.Matrix3 mat = new d.Matrix3();

+            d.Quaternion quat = new d.Quaternion();

+

+            d.Mass tmpdmass = new d.Mass { };         

+            d.Mass objdmass = new d.Mass { };

+

+            d.BodyGetMass(Body, out tmpdmass);

+            objdmass = tmpdmass;

+

+            d.Vector3 dobjpos;

+            d.Vector3 thispos;

+

+            // get current object position and rotation

+            dobjpos = d.BodyGetPosition(Body);

+

+            // get prim own inertia in its local frame

+            tmpdmass = primdMass;

+

+            // transform to object frame

+            mat = d.GeomGetOffsetRotation(prim_geom);

+            d.MassRotate(ref tmpdmass, ref mat);

+

+            thispos = d.GeomGetOffsetPosition(prim_geom);

+            d.MassTranslate(ref tmpdmass,

+                            thispos.X,

+                            thispos.Y,

+                            thispos.Z);

+

+            // subtract current prim inertia from object

+            DMassSubPartFromObj(ref tmpdmass, ref objdmass);

+

+            // back prim own inertia 

+            tmpdmass = primdMass;

+

+            // update to new position and orientation

+            _position = NewPos;

+            d.GeomSetOffsetWorldPosition(prim_geom, NewPos.X, NewPos.Y, NewPos.Z);

+            _orientation = newrot;

+            quat.X = newrot.X;

+            quat.Y = newrot.Y;

+            quat.Z = newrot.Z;

+            quat.W = newrot.W;

+            d.GeomSetOffsetWorldQuaternion(prim_geom, ref quat);

+

+            mat = d.GeomGetOffsetRotation(prim_geom);

+            d.MassRotate(ref tmpdmass, ref mat);

+

+            thispos = d.GeomGetOffsetPosition(prim_geom);

+            d.MassTranslate(ref tmpdmass,

+                            thispos.X,

+                            thispos.Y,

+                            thispos.Z);

+

+            d.MassAdd(ref objdmass, ref tmpdmass);

+

+            // fix all positions

+            IntPtr g = d.BodyGetFirstGeom(Body);

+            while (g != IntPtr.Zero)

+            {

+                thispos = d.GeomGetOffsetPosition(g);

+                thispos.X -= objdmass.c.X;

+                thispos.Y -= objdmass.c.Y;

+                thispos.Z -= objdmass.c.Z;

+                d.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z);

+                g = d.dBodyGetNextGeom(g);

+            }

+            d.BodyVectorToWorld(Body,objdmass.c.X, objdmass.c.Y, objdmass.c.Z,out thispos);

+

+            d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z);

+            d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body

+            d.BodySetMass(Body, ref objdmass);

+            _mass = objdmass.mass;

+        }

+

+

+

+        private void FixInertia(Vector3 NewPos)

+        {

+            d.Matrix3 primmat = new d.Matrix3();

+            d.Mass tmpdmass = new d.Mass { };

+            d.Mass objdmass = new d.Mass { };

+            d.Mass primmass = new d.Mass { };

+

+            d.Vector3 dobjpos;

+            d.Vector3 thispos;

+

+            d.BodyGetMass(Body, out objdmass);

+

+            // get prim own inertia in its local frame

+            primmass = primdMass;

+            // transform to object frame

+            primmat = d.GeomGetOffsetRotation(prim_geom);

+            d.MassRotate(ref primmass, ref primmat);

+

+            tmpdmass = primmass;

+

+            thispos = d.GeomGetOffsetPosition(prim_geom);

+            d.MassTranslate(ref tmpdmass,

+                            thispos.X,

+                            thispos.Y,

+                            thispos.Z);

+

+            // subtract current prim inertia from object

+            DMassSubPartFromObj(ref tmpdmass, ref objdmass);

+

+            // update to new position

+            _position = NewPos;

+            d.GeomSetOffsetWorldPosition(prim_geom, NewPos.X, NewPos.Y, NewPos.Z);

+

+            thispos = d.GeomGetOffsetPosition(prim_geom);

+            d.MassTranslate(ref primmass,

+                            thispos.X,

+                            thispos.Y,

+                            thispos.Z);

+

+            d.MassAdd(ref objdmass, ref primmass);

+

+            // fix all positions

+            IntPtr g = d.BodyGetFirstGeom(Body);

+            while (g != IntPtr.Zero)

+            {

+                thispos = d.GeomGetOffsetPosition(g);

+                thispos.X -= objdmass.c.X;

+                thispos.Y -= objdmass.c.Y;

+                thispos.Z -= objdmass.c.Z;

+                d.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z);

+                g = d.dBodyGetNextGeom(g);

+            }

+

+            d.BodyVectorToWorld(Body, objdmass.c.X, objdmass.c.Y, objdmass.c.Z, out thispos);

+

+            // get current object position and rotation

+            dobjpos = d.BodyGetPosition(Body);

+

+            d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z);

+            d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body

+            d.BodySetMass(Body, ref objdmass);

+            _mass = objdmass.mass;

+        }

+

+        private void FixInertia(Quaternion newrot)

+        {

+            d.Matrix3 mat = new d.Matrix3();

+            d.Quaternion quat = new d.Quaternion();

+

+            d.Mass tmpdmass = new d.Mass { };

+            d.Mass objdmass = new d.Mass { };

+            d.Vector3 dobjpos;

+            d.Vector3 thispos;

+

+            d.BodyGetMass(Body, out objdmass);

+

+            // get prim own inertia in its local frame

+            tmpdmass = primdMass;

+            mat = d.GeomGetOffsetRotation(prim_geom);

+            d.MassRotate(ref tmpdmass, ref mat);

+            // transform to object frame

+            thispos = d.GeomGetOffsetPosition(prim_geom);

+            d.MassTranslate(ref tmpdmass,

+                            thispos.X,

+                            thispos.Y,

+                            thispos.Z);

+

+            // subtract current prim inertia from object

+            DMassSubPartFromObj(ref tmpdmass, ref objdmass);

+

+            // update to new orientation

+            _orientation = newrot;

+            quat.X = newrot.X;

+            quat.Y = newrot.Y;

+            quat.Z = newrot.Z;

+            quat.W = newrot.W;

+            d.GeomSetOffsetWorldQuaternion(prim_geom, ref quat);

+

+            tmpdmass = primdMass;

+            mat = d.GeomGetOffsetRotation(prim_geom);

+            d.MassRotate(ref tmpdmass, ref mat);

+            d.MassTranslate(ref tmpdmass,

+                            thispos.X,

+                            thispos.Y,

+                            thispos.Z);

+

+            d.MassAdd(ref objdmass, ref tmpdmass);

+

+            // fix all positions

+            IntPtr g = d.BodyGetFirstGeom(Body);

+            while (g != IntPtr.Zero)

+            {

+                thispos = d.GeomGetOffsetPosition(g);

+                thispos.X -= objdmass.c.X;

+                thispos.Y -= objdmass.c.Y;

+                thispos.Z -= objdmass.c.Z;

+                d.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z);

+                g = d.dBodyGetNextGeom(g);

+            }

+

+            d.BodyVectorToWorld(Body, objdmass.c.X, objdmass.c.Y, objdmass.c.Z, out thispos);

+            // get current object position and rotation

+            dobjpos = d.BodyGetPosition(Body);

+

+            d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z);

+            d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body

+            d.BodySetMass(Body, ref objdmass);

+            _mass = objdmass.mass;

+        }

+

+

+        #region Mass Calculation

+

+        private void UpdatePrimBodyData()

+        {

+            primMass = m_density * primVolume;

+

+            if (primMass <= 0)

+                primMass = 0.0001f;//ckrinke: Mass must be greater then zero.

+            if (primMass > _parent_scene.maximumMassObject)

+                primMass = _parent_scene.maximumMassObject;

+

+            _mass = primMass; // just in case

+

+            d.MassSetBoxTotal(out primdMass, primMass, m_OBB.X, m_OBB.Y, m_OBB.Z);

+

+            d.MassTranslate(ref primdMass,

+                                m_OBBOffset.X,

+                                m_OBBOffset.Y,

+                                m_OBBOffset.Z);

+

+            primOOBradiusSQ = m_OBB.LengthSquared();

+

+            if (_triMeshData != IntPtr.Zero)

+            {

+                float pc = m_physCost;

+                float psf = primOOBradiusSQ;

+                psf *= 1.33f * .2f;

+                pc *= psf;

+                if (pc < 0.1f)

+                    pc = 0.1f;

+

+                m_physCost = pc;

+            }

+            else

+                m_physCost = 0.1f;

+

+            m_streamCost = 1.0f;

+        }

+

+        #endregion

+

+

+        /// <summary>

+        /// Add a child prim to this parent prim.

+        /// </summary>

+        /// <param name="prim">Child prim</param>

+        // I'm the parent

+        // prim is the child

+        public void ParentPrim(OdePrim prim)

+        {

+            //Console.WriteLine("ParentPrim  " + m_primName);

+            if (this.m_localID != prim.m_localID)

+            {

+                DestroyBody();  // for now we need to rebuil entire object on link change

+

+                lock (childrenPrim)

+                {

+                    // adopt the prim

+                    if (!childrenPrim.Contains(prim))

+                        childrenPrim.Add(prim);

+

+                    // see if this prim has kids and adopt them also

+                    // should not happen for now

+                    foreach (OdePrim prm in prim.childrenPrim)

+                    {

+                        if (!childrenPrim.Contains(prm))

+                        {

+                            if (prm.Body != IntPtr.Zero)

+                            {

+                                if (prm.prim_geom != IntPtr.Zero)

+                                    d.GeomSetBody(prm.prim_geom, IntPtr.Zero);

+                                if (prm.Body != prim.Body)

+                                    prm.DestroyBody(); // don't loose bodies around

+                                prm.Body = IntPtr.Zero;

+                            }

+

+                            childrenPrim.Add(prm);

+                            prm._parent = this;

+                        }

+                    }

+                }

+                //Remove old children from the prim

+                prim.childrenPrim.Clear();

+

+                if (prim.Body != IntPtr.Zero)

+                {

+                    if (prim.prim_geom != IntPtr.Zero)

+                        d.GeomSetBody(prim.prim_geom, IntPtr.Zero);

+                    prim.DestroyBody(); // don't loose bodies around

+                    prim.Body = IntPtr.Zero;

+                }

+

+                prim.childPrim = true;

+                prim._parent = this;

+

+                MakeBody(); // full nasty reconstruction

+            }

+        }

+

+        private void UpdateChildsfromgeom()

+        {

+            if (childrenPrim.Count > 0)

+            {

+                foreach (OdePrim prm in childrenPrim)

+                    prm.UpdateDataFromGeom();

+            }

+        }

+

+        private void UpdateDataFromGeom()

+        {

+            if (prim_geom != IntPtr.Zero)

+            {

+                d.Quaternion qtmp;

+                d.GeomCopyQuaternion(prim_geom, out qtmp);

+                _orientation.X = qtmp.X;

+                _orientation.Y = qtmp.Y;

+                _orientation.Z = qtmp.Z;

+                _orientation.W = qtmp.W;

+/*

+// Debug

+                float qlen = _orientation.Length();

+                if (qlen > 1.01f || qlen < 0.99)

+                    m_log.WarnFormat("[PHYSICS]: Got nonnorm quaternion from geom in Object {0} norm {1}", Name, qlen);

+//

+*/

+                _orientation.Normalize();

+

+                d.Vector3 lpos = d.GeomGetPosition(prim_geom);

+                _position.X = lpos.X;

+                _position.Y = lpos.Y;

+                _position.Z = lpos.Z;

+            }

+        }

+

+        private void ChildDelink(OdePrim odePrim, bool remakebodies)

+        {

+            // Okay, we have a delinked child.. destroy all body and remake

+            if (odePrim != this && !childrenPrim.Contains(odePrim))

+                return;

+

+            DestroyBody();

+

+            if (odePrim == this) // delinking the root prim

+            {

+                OdePrim newroot = null;

+                lock (childrenPrim)

+                {

+                    if (childrenPrim.Count > 0)

+                    {

+                        newroot = childrenPrim[0];

+                        childrenPrim.RemoveAt(0);

+                        foreach (OdePrim prm in childrenPrim)

+                        {

+                            newroot.childrenPrim.Add(prm);

+                        }

+                        childrenPrim.Clear();

+                    }

+                    if (newroot != null)

+                    {

+                        newroot.childPrim = false;

+                        newroot._parent = null;

+                        if (remakebodies)

+                            newroot.MakeBody();

+                    }

+                }

+            }

+

+            else

+            {

+                lock (childrenPrim)

+                {

+                    childrenPrim.Remove(odePrim);

+                    odePrim.childPrim = false;

+                    odePrim._parent = null;

+                    //                    odePrim.UpdateDataFromGeom();

+                    if (remakebodies)

+                        odePrim.MakeBody();

+                }

+            }

+            if (remakebodies)

+                MakeBody();

+        }

+

+        protected void ChildRemove(OdePrim odePrim, bool reMakeBody)

+        {

+            // Okay, we have a delinked child.. destroy all body and remake

+            if (odePrim != this && !childrenPrim.Contains(odePrim))

+                return;

+

+            DestroyBody();

+

+            if (odePrim == this)

+            {

+                OdePrim newroot = null;

+                lock (childrenPrim)

+                {

+                    if (childrenPrim.Count > 0)

+                    {

+                        newroot = childrenPrim[0];

+                        childrenPrim.RemoveAt(0);

+                        foreach (OdePrim prm in childrenPrim)

+                        {

+                            newroot.childrenPrim.Add(prm);

+                        }

+                        childrenPrim.Clear();

+                    }

+                    if (newroot != null)

+                    {

+                        newroot.childPrim = false;

+                        newroot._parent = null;

+                        newroot.MakeBody();

+                    }

+                }

+                if (reMakeBody)

+                    MakeBody();

+                return;

+            }

+            else

+            {

+                lock (childrenPrim)

+                {

+                    childrenPrim.Remove(odePrim);

+                    odePrim.childPrim = false;

+                    odePrim._parent = null;

+                    if (reMakeBody)

+                        odePrim.MakeBody();

+                }

+            }

+            MakeBody();

+        }

+

+        #region changes

+

+        private void changeadd()

+        {

+        }

+

+        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;

+//                m_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();

+

+                _zeroFlag = false;

+                base.RequestPhysicsterseUpdate();

+            }

+

+            if (Body != IntPtr.Zero)

+            {

+                d.BodySetForce(Body, 0f, 0f, 0f);

+                d.BodySetTorque(Body, 0f, 0f, 0f);

+                d.BodySetLinearVel(Body, 0f, 0f, 0f);

+                d.BodySetAngularVel(Body, 0f, 0f, 0f);

+            }

+        }

+

+        private void changePhantomStatus(bool newval)

+        {

+            m_isphantom = newval;

+

+            UpdateCollisionCatFlags();

+            ApplyCollisionCatFlags();

+        }

+

+/* not in use

+        internal void ChildSelectedChange(bool childSelect)

+        {

+            if(childPrim)

+                return;

+

+            if (childSelect == m_isSelected)

+                return;

+

+            if (childSelect)

+            {

+                DoSelectedStatus(true);

+            }

+

+            else

+            {

+                foreach (OdePrim prm in childrenPrim)

+                {

+                    if (prm.m_isSelected)

+                        return;

+                }

+                DoSelectedStatus(false);

+            }

+        }

+*/

+        private void changeSelectedStatus(bool newval)

+        {

+            if (m_lastdoneSelected == newval)

+                return;

+

+            m_lastdoneSelected = newval;

+            DoSelectedStatus(newval);

+        }

+

+        private void CheckDelaySelect()

+        {

+            if (m_delaySelect)

+            {

+                DoSelectedStatus(m_isSelected);

+            }

+        }

+

+        private void DoSelectedStatus(bool newval)

+        {

+            m_isSelected = newval;

+            Stop();

+

+            if (newval)

+            {

+                if (!childPrim && Body != IntPtr.Zero)

+                    d.BodyDisable(Body);

+

+                if (m_delaySelect || m_isphysical)

+                {

+                    m_collisionCategories = CollisionCategories.Selected;

+                    m_collisionFlags = 0;

+

+                    if (!childPrim)

+                    {

+                        foreach (OdePrim prm in childrenPrim)

+                        {

+                            prm.m_collisionCategories = m_collisionCategories;

+                            prm.m_collisionFlags = m_collisionFlags;

+

+                            if (prm.prim_geom != IntPtr.Zero)

+                            {

+

+                                if (prm.m_NoColide)

+                                {

+                                    d.GeomSetCategoryBits(prm.prim_geom, 0);

+                                    d.GeomSetCollideBits(prm.prim_geom, 0);

+                                }

+                                else

+                                {

+                                    d.GeomSetCategoryBits(prm.prim_geom, (uint)m_collisionCategories);

+                                    d.GeomSetCollideBits(prm.prim_geom, (uint)m_collisionFlags);

+                                }

+                            }

+                            prm.m_delaySelect = false;

+                        }

+                    }

+//                    else if (_parent != null)

+//                        ((OdePrim)_parent).ChildSelectedChange(true);

+

+

+                    if (prim_geom != IntPtr.Zero)

+                    {

+                        if (m_NoColide)

+                        {

+                            d.GeomSetCategoryBits(prim_geom, 0);

+                            d.GeomSetCollideBits(prim_geom, 0);

+                            if (collide_geom != prim_geom && collide_geom != IntPtr.Zero)

+                            {

+                                d.GeomSetCategoryBits(collide_geom, 0);

+                                d.GeomSetCollideBits(collide_geom, 0);

+                            }

+

+                        }

+                        else

+                        {

+                            d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories);

+                            d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags);

+                            if (collide_geom != prim_geom && collide_geom != IntPtr.Zero)

+                            {

+                                d.GeomSetCategoryBits(collide_geom, (uint)m_collisionCategories);

+                                d.GeomSetCollideBits(collide_geom, (uint)m_collisionFlags);

+                            }

+                        }

+                    }

+

+                    m_delaySelect = false;

+                }

+                else if(!m_isphysical)

+                {

+                    m_delaySelect = true;

+                }

+            }

+            else

+            {

+                if (!childPrim)

+                {

+                    if (Body != IntPtr.Zero && !m_disabled)

+                        d.BodyEnable(Body);

+                }

+//                else if (_parent != null)

+//                    ((OdePrim)_parent).ChildSelectedChange(false);

+

+                UpdateCollisionCatFlags();

+                ApplyCollisionCatFlags();

+

+                m_delaySelect = false;

+            }

+

+            resetCollisionAccounting();

+        }

+

+        private void changePosition(Vector3 newPos)

+        {

+            CheckDelaySelect();

+            if (m_isphysical)

+            {

+                if (childPrim)  // inertia is messed, must rebuild

+                {

+                    if (m_building)

+                    {

+                        _position = newPos;

+                    }

+

+                    else if (m_forcePosOrRotation && _position != newPos && Body != IntPtr.Zero)

+                    {

+                        FixInertia(newPos);

+                        if (!d.BodyIsEnabled(Body))

+                            d.BodyEnable(Body);

+                    }

+                }

+                else

+                {

+                    if (_position != newPos)

+                    {

+                        d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z);

+                        _position = newPos;

+                    }

+                    if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))

+                        d.BodyEnable(Body);

+                }

+            }

+            else

+            {

+                if (prim_geom != IntPtr.Zero)

+                {

+                    if (newPos != _position)

+                    {

+                        d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z);

+                        _position = newPos;

+

+                        m_targetSpace = _parent_scene.MoveGeomToStaticSpace(prim_geom, _position, m_targetSpace);

+                    }

+                }

+            }

+            givefakepos--;

+            if (givefakepos < 0)

+                givefakepos = 0;

+//            changeSelectedStatus();

+            resetCollisionAccounting();

+        }

+

+        private void changeOrientation(Quaternion newOri)

+        {

+            CheckDelaySelect();

+            if (m_isphysical)

+            {

+                if (childPrim)  // inertia is messed, must rebuild

+                {

+                    if (m_building)

+                    {

+                        _orientation = newOri;

+                    }

+/*

+                    else if (m_forcePosOrRotation && _orientation != newOri && Body != IntPtr.Zero)

+                    {

+                        FixInertia(_position, newOri);

+                        if (!d.BodyIsEnabled(Body))

+                            d.BodyEnable(Body);

+                    }

+*/

+                }

+                else

+                {

+                    if (newOri != _orientation)

+                    {

+                        d.Quaternion myrot = new d.Quaternion();

+                        myrot.X = newOri.X;

+                        myrot.Y = newOri.Y;

+                        myrot.Z = newOri.Z;

+                        myrot.W = newOri.W;

+                        d.GeomSetQuaternion(prim_geom, ref myrot);

+                        _orientation = newOri;

+                        if (Body != IntPtr.Zero && !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)

+        {

+            CheckDelaySelect();

+            if (m_isphysical)

+            {

+                if (childPrim && m_building)  // inertia is messed, must rebuild

+                {

+                    _position = newPos;

+                    _orientation = newOri;

+                }

+                else

+                {

+                    if (newOri != _orientation)

+                    {

+                        d.Quaternion myrot = new d.Quaternion();

+                        myrot.X = newOri.X;

+                        myrot.Y = newOri.Y;

+                        myrot.Z = newOri.Z;

+                        myrot.W = newOri.W;

+                        d.GeomSetQuaternion(prim_geom, ref myrot);

+                        _orientation = newOri;

+                        if (Body != IntPtr.Zero && !m_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)

+        {

+            CheckDelaySelect();

+

+            m_isphysical = NewStatus;

+

+            if (!childPrim)

+            {

+                if (NewStatus)

+                {

+                    if (Body == IntPtr.Zero)

+                        MakeBody();

+                }

+                else

+                {

+                    if (Body != IntPtr.Zero)

+                    {

+                        DestroyBody();

+                    }

+                    Stop();

+                }

+            }

+

+            resetCollisionAccounting();

+        }

+

+        private void changeSize(Vector3 newSize)

+        {

+        }

+

+        private void changeShape(PrimitiveBaseShape newShape)

+        {

+        }

+

+        private void changeAddPhysRep(ODEPhysRepData repData)

+        {

+            _size = repData.size; //??

+            _pbs = repData.pbs;

+            m_shapetype = repData.shapetype;

+

+            m_mesh = repData.mesh;

+

+            m_assetID = repData.assetID;

+            m_meshState = repData.meshState;

+

+            m_hasOBB = repData.hasOBB;

+            m_OBBOffset = repData.OBBOffset;

+            m_OBB = repData.OBB;

+

+            primVolume = repData.volume;

+

+            CreateGeom();

+

+            if (prim_geom != IntPtr.Zero)

+            {

+                d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);

+                d.Quaternion myrot = new d.Quaternion();

+                myrot.X = _orientation.X;

+                myrot.Y = _orientation.Y;

+                myrot.Z = _orientation.Z;

+                myrot.W = _orientation.W;

+                d.GeomSetQuaternion(prim_geom, ref myrot);

+            }

+

+            if (!m_isphysical)

+            {

+                SetInStaticSpace(this);

+                UpdateCollisionCatFlags();

+                ApplyCollisionCatFlags();

+            }

+            else

+                MakeBody();

+

+            if ((m_meshState & MeshState.NeedMask) != 0)

+            {

+                repData.size = _size;

+                repData.pbs = _pbs;

+                repData.shapetype = m_shapetype;

+                _parent_scene.m_meshWorker.RequestMesh(repData);

+            }

+        }

+

+        private void changePhysRepData(ODEPhysRepData repData)

+        {

+            CheckDelaySelect();

+

+            OdePrim parent = (OdePrim)_parent;

+

+            bool chp = childPrim;

+

+            if (chp)

+            {

+                if (parent != null)

+                {

+                    parent.DestroyBody();

+                }

+            }

+            else

+            {

+                DestroyBody();

+            }

+

+            RemoveGeom();

+

+            _size = repData.size;

+            _pbs = repData.pbs;

+            m_shapetype = repData.shapetype;

+

+            m_mesh = repData.mesh;

+

+            m_assetID = repData.assetID;

+            m_meshState = repData.meshState;

+

+            m_hasOBB = repData.hasOBB;

+            m_OBBOffset = repData.OBBOffset;

+            m_OBB = repData.OBB;

+

+            primVolume = repData.volume;

+

+            CreateGeom();          

+

+            if (prim_geom != IntPtr.Zero)

+            {

+                d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);

+                d.Quaternion myrot = new d.Quaternion();

+                myrot.X = _orientation.X;

+                myrot.Y = _orientation.Y;

+                myrot.Z = _orientation.Z;

+                myrot.W = _orientation.W;

+                d.GeomSetQuaternion(prim_geom, ref myrot);

+            }

+

+            if (m_isphysical)

+            {

+                if (chp)

+                {

+                    if (parent != null)

+                    {

+                        parent.MakeBody();

+                    }

+                }

+                else

+                    MakeBody();

+            }

+            else

+            {

+                SetInStaticSpace(this);

+                UpdateCollisionCatFlags();

+                ApplyCollisionCatFlags();

+            }

+

+            resetCollisionAccounting();

+

+            if ((m_meshState & MeshState.NeedMask) != 0)

+            {

+                repData.size = _size;

+                repData.pbs = _pbs;

+                repData.shapetype = m_shapetype;

+                _parent_scene.m_meshWorker.RequestMesh(repData);

+            }

+        }

+

+        private void changeFloatOnWater(bool newval)

+        {

+            m_collidesWater = newval;

+

+            UpdateCollisionCatFlags();

+            ApplyCollisionCatFlags();

+        }

+

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

+

+                }

+                m_torque = newtorque;

+            }

+        }

+

+        private void changeForce(Vector3 force)

+        {

+            m_force = force;

+            if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))

+                d.BodyEnable(Body);

+        }

+

+        private void changeAddForce(Vector3 theforce)

+        {

+            m_forceacc += theforce;

+            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;

+            }

+        }

+

+        // actually angular impulse

+        private void changeAddAngularImpulse(Vector3 aimpulse)

+        {

+            m_angularForceacc += aimpulse * m_invTimeStep;

+            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;

+            }

+        }

+

+        private void changevelocity(Vector3 newVel)

+        {

+            float len = newVel.LengthSquared();

+            if (len > 100000.0f) // limit to 100m/s

+            {

+                len = 100.0f / (float)Math.Sqrt(len);

+                newVel *= len;

+            }

+

+            if (!m_isSelected)

+            {

+                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 changeangvelocity(Vector3 newAngVel)

+        {

+            float len = newAngVel.LengthSquared();

+            if (len > 144.0f) // limit to 12rad/s

+            {

+                len = 12.0f / (float)Math.Sqrt(len);

+                newAngVel *= len;

+            }

+

+            if (!m_isSelected)

+            {

+                if (Body != IntPtr.Zero)

+                {

+                    if (m_disabled)

+                        enableBodySoft();

+                    else if (!d.BodyIsEnabled(Body))

+                        d.BodyEnable(Body);

+

+

+                    d.BodySetAngularVel(Body, newAngVel.X, newAngVel.Y, newAngVel.Z);

+                }

+                //resetCollisionAccounting();           

+            }

+            m_rotationalVelocity = newAngVel;

+        }

+

+        private void changeVolumedetetion(bool newVolDtc)

+        {

+            m_isVolumeDetect = newVolDtc;

+            m_fakeisVolumeDetect = newVolDtc;

+            UpdateCollisionCatFlags();

+            ApplyCollisionCatFlags();

+        }

+

+        protected void changeBuilding(bool newbuilding)

+        {

+            // Check if we need to do anything

+            if (newbuilding == m_building)

+                return;

+

+            if ((bool)newbuilding)

+            {

+                m_building = true;

+                if (!childPrim)

+                    DestroyBody();

+            }

+            else

+            {

+                m_building = false;

+                CheckDelaySelect();

+                if (!childPrim)

+                    MakeBody();

+            }

+            if (!childPrim && childrenPrim.Count > 0)

+            {

+                foreach (OdePrim prm in childrenPrim)

+                    prm.changeBuilding(m_building); // call directly

+            }

+        }

+

+       public void changeSetVehicle(VehicleData vdata)

+        {

+            if (m_vehicle == null)

+                m_vehicle = new ODEDynamics(this);

+            m_vehicle.DoSetVehicle(vdata);

+        }

+

+        private void changeVehicleType(int value)

+        {

+            if (value == (int)Vehicle.TYPE_NONE)

+            {

+                if (m_vehicle != null)

+                    m_vehicle = null;

+            }

+            else

+            {

+                if (m_vehicle == null)

+                    m_vehicle = new ODEDynamics(this);

+

+                m_vehicle.ProcessTypeChange((Vehicle)value);

+            }

+        }

+

+        private void changeVehicleFloatParam(strVehicleFloatParam fp)

+        {

+            if (m_vehicle == null)

+                return;

+

+            m_vehicle.ProcessFloatVehicleParam((Vehicle)fp.param, fp.value);

+        }

+

+        private void changeVehicleVectorParam(strVehicleVectorParam vp)

+        {

+            if (m_vehicle == null)

+                return;

+            m_vehicle.ProcessVectorVehicleParam((Vehicle)vp.param, vp.value);

+        }

+

+        private void changeVehicleRotationParam(strVehicleQuatParam qp)

+        {

+            if (m_vehicle == null)

+                return;

+            m_vehicle.ProcessRotationVehicleParam((Vehicle)qp.param, qp.value);

+        }

+

+        private void changeVehicleFlags(strVehicleBoolParam bp)

+        {

+            if (m_vehicle == null)

+                return;

+            m_vehicle.ProcessVehicleFlags(bp.param, bp.value);

+        }

+

+        private void changeBuoyancy(float b)

+        {

+            m_buoyancy = b;

+        }

+

+        private void changePIDTarget(Vector3 trg)

+        {

+            m_PIDTarget = trg;

+        }

+

+        private void changePIDTau(float tau)

+        {

+            m_PIDTau = tau;

+        }

+

+        private void changePIDActive(bool val)

+        {

+            m_usePID = val;

+        }

+

+        private void changePIDHoverHeight(float val)

+        {

+          m_PIDHoverHeight = val;

+          if (val == 0)

+            m_useHoverPID = false;

+        }

+

+        private void changePIDHoverType(PIDHoverType type)

+        {

+            m_PIDHoverType = type;

+        }

+

+        private void changePIDHoverTau(float tau)

+        {

+            m_PIDHoverTau = tau;

+        }

+

+        private void changePIDHoverActive(bool active)

+        {

+            m_useHoverPID = active;

+        }

+

+        #endregion

+

+        public void Move()

+        {

+            if (!childPrim && m_isphysical && Body != IntPtr.Zero &&

+                !m_disabled && !m_isSelected && !m_building && !m_outbounds)

+            {

+                if (!d.BodyIsEnabled(Body))

+                {

+                    // let vehicles sleep

+                    if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE)

+                        return;

+

+                    if (++bodydisablecontrol < 20)

+                        return;

+

+                    

+                    d.BodyEnable(Body);

+                }

+

+                bodydisablecontrol = 0;

+

+                d.Vector3 lpos = d.GeomGetPosition(prim_geom); // root position that is seem by rest of simulator

+

+                if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE)

+                {

+                    // 'VEHICLES' are dealt with in ODEDynamics.cs

+                    m_vehicle.Step();

+                    return;

+                }

+

+                float fx = 0;

+                float fy = 0;

+                float fz = 0;

+

+                float m_mass = _mass;

+

+                if (m_usePID && m_PIDTau > 0)

+                {

+                    // for now position error

+                    _target_velocity =

+                        new Vector3(

+                            (m_PIDTarget.X - lpos.X),

+                            (m_PIDTarget.Y - lpos.Y),

+                            (m_PIDTarget.Z - lpos.Z)

+                            );

+

+                    if (_target_velocity.ApproxEquals(Vector3.Zero, 0.02f))

+                    {

+                        d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);

+                        d.BodySetLinearVel(Body, 0, 0, 0);

+                        return;

+                    }

+                    else

+                    {

+                        _zeroFlag = false;

+

+                        float tmp = 1 / m_PIDTau;

+                        _target_velocity *= tmp;

+

+                        // apply limits

+                        tmp = _target_velocity.Length();

+                        if (tmp > 50.0f)

+                        {

+                            tmp = 50 / tmp;

+                            _target_velocity *= tmp;

+                        }

+                        else if (tmp < 0.05f)

+                        {

+                            tmp = 0.05f / tmp;

+                            _target_velocity *= tmp;

+                        }

+

+                        d.Vector3 vel = d.BodyGetLinearVel(Body);

+                        fx = (_target_velocity.X - vel.X) * m_invTimeStep;

+                        fy = (_target_velocity.Y - vel.Y) * m_invTimeStep;

+                        fz = (_target_velocity.Z - vel.Z) * m_invTimeStep;

+//                        d.BodySetLinearVel(Body, _target_velocity.X, _target_velocity.Y, _target_velocity.Z);

+                    }

+                }        // end if (m_usePID)

+

+                // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller

+                else if (m_useHoverPID && m_PIDHoverTau != 0 && m_PIDHoverHeight != 0)

+                {

+

+                    //    Non-Vehicles have a limited set of Hover options.

+                    // determine what our target height really is based on HoverType

+

+                    m_groundHeight = _parent_scene.GetTerrainHeightAtXY(lpos.X, lpos.Y);

+

+                    switch (m_PIDHoverType)

+                    {

+                        case PIDHoverType.Ground:

+                            m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;

+                            break;

+

+                        case PIDHoverType.GroundAndWater:

+                            m_waterHeight = _parent_scene.GetWaterLevel();

+                            if (m_groundHeight > m_waterHeight)

+                                m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;

+                            else

+                                m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;

+                            break;

+                    }     // end switch (m_PIDHoverType)

+

+                    // don't go underground unless volumedetector 

+

+                    if (m_targetHoverHeight > m_groundHeight || m_isVolumeDetect)

+                    {

+                        d.Vector3 vel = d.BodyGetLinearVel(Body);

+

+                        fz = (m_targetHoverHeight - lpos.Z);

+

+                        //  if error is zero, use position control; otherwise, velocity control

+                        if (Math.Abs(fz) < 0.01f)

+                        {

+                            d.BodySetPosition(Body, lpos.X, lpos.Y, m_targetHoverHeight);

+                            d.BodySetLinearVel(Body, vel.X, vel.Y, 0);

+                        }

+                        else

+                        {

+                            _zeroFlag = false;

+                            fz /= m_PIDHoverTau;

+

+                            float tmp = Math.Abs(fz);

+                            if (tmp > 50)

+                                fz = 50 * Math.Sign(fz);

+                            else if (tmp < 0.1)

+                                fz = 0.1f * Math.Sign(fz);

+

+                            fz = ((fz - vel.Z) * m_invTimeStep);

+                        }

+                    }

+                }

+                else

+                {

+                    float b = (1.0f - m_buoyancy);

+                    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 = m_torque;

+                trq += m_angularForceacc;

+                m_angularForceacc = Vector3.Zero;

+                if (trq.X != 0 || trq.Y != 0 || trq.Z != 0)

+                {

+                    d.BodyAddTorque(Body, trq.X, trq.Y, trq.Z);

+                }

+            }

+            else

+            {    // is not physical, or is not a body or is selected

+                //  _zeroPosition = d.BodyGetPosition(Body);

+                return;

+                //Console.WriteLine("Nothing " +  Name);

+

+            }

+        }

+

+        public void UpdatePositionAndVelocity()

+        {

+            if (_parent == null && !m_disabled && !m_building && !m_outbounds && Body != IntPtr.Zero)

+            {

+                if (d.BodyIsEnabled(Body) || !_zeroFlag)

+                {

+                    bool lastZeroFlag = _zeroFlag;

+

+                    d.Vector3 lpos = d.GeomGetPosition(prim_geom);

+

+                    // check outside region

+                    if (lpos.Z < -100 || lpos.Z > 100000f)

+                    {

+                        m_outbounds = true;

+

+                        lpos.Z = Util.Clip(lpos.Z, -100f, 100000f);

+                        _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();

+

+//                        throttleCounter = 0;

+                        _zeroFlag = true;

+

+                        disableBodySoft(); // disable it and colisions

+                        base.RaiseOutOfBounds(_position);

+                        return;

+                    }

+

+                    if (lpos.X < 0f)

+                    {

+                        _position.X = Util.Clip(lpos.X, -2f, -0.1f);

+                        m_outbounds = true;

+                    }

+                    else if (lpos.X > _parent_scene.WorldExtents.X)

+                    {

+                        _position.X = Util.Clip(lpos.X, _parent_scene.WorldExtents.X + 0.1f, _parent_scene.WorldExtents.X + 2f);

+                        m_outbounds = true;

+                    }

+                    if (lpos.Y < 0f)

+                    {

+                        _position.Y = Util.Clip(lpos.Y, -2f, -0.1f);

+                        m_outbounds = true;

+                    }

+                    else if (lpos.Y > _parent_scene.WorldExtents.Y)

+                    {

+                        _position.Y = Util.Clip(lpos.Y, _parent_scene.WorldExtents.Y + 0.1f, _parent_scene.WorldExtents.Y + 2f);

+                        m_outbounds = true;

+                    }

+

+                    if (m_outbounds)

+                    {

+                        m_lastposition = _position;

+                        m_lastorientation = _orientation;

+

+                        d.Vector3 dtmp = d.BodyGetAngularVel(Body);

+                        m_rotationalVelocity.X = dtmp.X;

+                        m_rotationalVelocity.Y = dtmp.Y;

+                        m_rotationalVelocity.Z = dtmp.Z;

+

+                        dtmp = d.BodyGetLinearVel(Body);

+                        _velocity.X = dtmp.X;

+                        _velocity.Y = dtmp.Y;

+                        _velocity.Z = dtmp.Z;

+

+                        d.BodySetLinearVel(Body, 0, 0, 0); // stop it

+                        d.BodySetAngularVel(Body, 0, 0, 0);

+                        d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);

+                        disableBodySoft(); // stop collisions

+                        UnSubscribeEvents();

+

+                        base.RequestPhysicsterseUpdate();

+                        return;

+                    }

+

+                    d.Quaternion ori;

+                    d.GeomCopyQuaternion(prim_geom, out ori);

+

+                    // decide if moving

+                    // use positions since this are integrated quantities

+                    // tolerance values depende a lot on simulation noise...

+                    // use simple math.abs since we dont need to be exact

+

+                    if (

+                        (Math.Abs(_position.X - lpos.X) < 0.001f)

+                        && (Math.Abs(_position.Y - lpos.Y) < 0.001f)

+                        && (Math.Abs(_position.Z - lpos.Z) < 0.001f)

+                        && (Math.Abs(_orientation.X - ori.X) < 0.0001f)

+                        && (Math.Abs(_orientation.Y - ori.Y) < 0.0001f)

+                        && (Math.Abs(_orientation.Z - ori.Z) < 0.0001f)  // ignore W

+                        )

+                    {

+                        _zeroFlag = true;

+                    }

+                    else

+                        _zeroFlag = false;

+

+                    // update velocities and aceleration

+                    if (!(_zeroFlag && lastZeroFlag))

+                    {

+                        d.Vector3 vel = d.BodyGetLinearVel(Body);

+

+                        _acceleration = _velocity;

+

+                        if ((Math.Abs(vel.X) < 0.001f) &&

+                            (Math.Abs(vel.Y) < 0.001f) &&

+                            (Math.Abs(vel.Z) < 0.001f))

+                        {

+                            _velocity = Vector3.Zero;

+                            float t = -m_invTimeStep;

+                            _acceleration = _acceleration * t;

+                        }

+                        else

+                        {

+                            _velocity.X = vel.X;

+                            _velocity.Y = vel.Y;

+                            _velocity.Z = vel.Z;

+                            _acceleration = (_velocity - _acceleration) * m_invTimeStep;

+                        }

+

+                        if ((Math.Abs(_acceleration.X) < 0.01f) &&

+                            (Math.Abs(_acceleration.Y) < 0.01f) &&

+                            (Math.Abs(_acceleration.Z) < 0.01f))

+                        {

+                            _acceleration = Vector3.Zero;

+                        }

+

+                        if ((Math.Abs(_orientation.X - ori.X) < 0.0001) &&

+                           (Math.Abs(_orientation.Y - ori.Y) < 0.0001) &&

+                           (Math.Abs(_orientation.Z - ori.Z) < 0.0001)

+                           )

+                        {

+                            m_rotationalVelocity = Vector3.Zero;

+                        }

+                        else

+                        {

+                            vel = d.BodyGetAngularVel(Body);

+                            m_rotationalVelocity.X = vel.X;

+                            m_rotationalVelocity.Y = vel.Y;

+                            m_rotationalVelocity.Z = vel.Z;

+                        }

+                    }

+

+                    if (_zeroFlag)

+                    {

+                        if (lastZeroFlag)

+                        {

+                            _velocity = Vector3.Zero;

+                            _acceleration = Vector3.Zero;

+                            m_rotationalVelocity = Vector3.Zero;

+                        }

+

+                        if (!m_lastUpdateSent)

+                        {

+                            base.RequestPhysicsterseUpdate();

+                            if (lastZeroFlag)

+                                m_lastUpdateSent = true;

+                        }

+                        return;

+                    }

+

+                    _position.X = lpos.X;

+                    _position.Y = lpos.Y;

+                    _position.Z = lpos.Z;

+

+                    _orientation.X = ori.X;

+                    _orientation.Y = ori.Y;

+                    _orientation.Z = ori.Z;

+                    _orientation.W = ori.W;

+                    base.RequestPhysicsterseUpdate();

+                    m_lastUpdateSent = false;

+                }

+            }

+        }

+

+        internal static bool QuaternionIsFinite(Quaternion q)

+        {

+            if (Single.IsNaN(q.X) || Single.IsInfinity(q.X))

+                return false;

+            if (Single.IsNaN(q.Y) || Single.IsInfinity(q.Y))

+                return false;

+            if (Single.IsNaN(q.Z) || Single.IsInfinity(q.Z))

+                return false;

+            if (Single.IsNaN(q.W) || Single.IsInfinity(q.W))

+                return false;

+            return true;

+        }

+

+        internal static void DMassSubPartFromObj(ref d.Mass part, ref d.Mass theobj)

+        {

+            // assumes object center of mass is zero

+            float smass = part.mass;

+            theobj.mass -= smass;

+

+            smass *= 1.0f / (theobj.mass); ;

+

+            theobj.c.X -= part.c.X * smass;

+            theobj.c.Y -= part.c.Y * smass;

+            theobj.c.Z -= part.c.Z * smass;

+

+            theobj.I.M00 -= part.I.M00;

+            theobj.I.M01 -= part.I.M01;

+            theobj.I.M02 -= part.I.M02;

+            theobj.I.M10 -= part.I.M10;

+            theobj.I.M11 -= part.I.M11;

+            theobj.I.M12 -= part.I.M12;

+            theobj.I.M20 -= part.I.M20;

+            theobj.I.M21 -= part.I.M21;

+            theobj.I.M22 -= part.I.M22;

+        }

+

+        private void donullchange()

+        {

+        }

+

+        public bool DoAChange(changes what, object arg)

+        {

+            if (prim_geom == IntPtr.Zero && what != changes.Add && what != changes.AddPhysRep && what != changes.Remove)

+            {

+                return false;

+            }

+

+            // nasty switch

+            switch (what)

+            {

+                case changes.Add:

+                    changeadd();

+                    break;

+

+                case changes.AddPhysRep:

+                    changeAddPhysRep((ODEPhysRepData)arg);

+                    break;

+

+                case changes.Remove:

+                    //If its being removed, we don't want to rebuild the physical rep at all, so ignore this stuff...

+                    //When we return true, it destroys all of the prims in the linkset anyway

+                    if (_parent != null)

+                    {

+                        OdePrim parent = (OdePrim)_parent;

+                        parent.ChildRemove(this, false);

+                    }

+                    else

+                        ChildRemove(this, false);

+

+                    m_vehicle = null;

+                    RemoveGeom();

+                    m_targetSpace = IntPtr.Zero;

+                    UnSubscribeEvents();

+                    return true;

+

+                case changes.Link:

+                    OdePrim tmp = (OdePrim)arg;

+                    changeLink(tmp);

+                    break;

+

+                case changes.DeLink:

+                    changeLink(null);

+                    break;

+

+                case changes.Position:

+                    changePosition((Vector3)arg);

+                    break;

+

+                case changes.Orientation:

+                    changeOrientation((Quaternion)arg);

+                    break;

+

+                case changes.PosOffset:

+                    donullchange();

+                    break;

+

+                case changes.OriOffset:

+                    donullchange();

+                    break;

+

+                case changes.Velocity:

+                    changevelocity((Vector3)arg);

+                    break;

+

+//                case changes.Acceleration:

+//                    changeacceleration((Vector3)arg);

+//                    break;

+

+                case changes.AngVelocity:

+                    changeangvelocity((Vector3)arg);

+                    break;

+

+                case changes.Force:

+                    changeForce((Vector3)arg);

+                    break;

+

+                case changes.Torque:

+                    changeSetTorque((Vector3)arg);

+                    break;

+

+                case changes.AddForce:

+                    changeAddForce((Vector3)arg);

+                    break;

+

+                case changes.AddAngForce:

+                    changeAddAngularImpulse((Vector3)arg);

+                    break;

+

+                case changes.AngLock:

+                    changeAngularLock((Vector3)arg);

+                    break;

+

+                case changes.Size:

+                    changeSize((Vector3)arg);

+                    break;

+

+                case changes.Shape:

+                    changeShape((PrimitiveBaseShape)arg);

+                    break;

+

+                case changes.PhysRepData:

+                    changePhysRepData((ODEPhysRepData) arg);

+                    break;

+

+                case changes.CollidesWater:

+                    changeFloatOnWater((bool)arg);

+                    break;

+

+                case changes.VolumeDtc:

+                    changeVolumedetetion((bool)arg);

+                    break;

+

+                case changes.Phantom:

+                    changePhantomStatus((bool)arg);

+                    break;

+

+                case changes.Physical:

+                    changePhysicsStatus((bool)arg);

+                    break;

+

+                case changes.Selected:

+                    changeSelectedStatus((bool)arg);

+                    break;

+

+                case changes.disabled:

+                    changeDisable((bool)arg);

+                    break;

+

+                case changes.building:

+                    changeBuilding((bool)arg);

+                    break;

+

+                case changes.VehicleType:

+                    changeVehicleType((int)arg);

+                    break;

+

+                case changes.VehicleFlags:

+                    changeVehicleFlags((strVehicleBoolParam) arg);

+                    break;

+

+                case changes.VehicleFloatParam:

+                    changeVehicleFloatParam((strVehicleFloatParam) arg);

+                    break;

+

+                case changes.VehicleVectorParam:

+                    changeVehicleVectorParam((strVehicleVectorParam) arg);

+                    break;

+

+                case changes.VehicleRotationParam:

+                    changeVehicleRotationParam((strVehicleQuatParam) arg);

+                    break;

+

+                case changes.SetVehicle:

+                    changeSetVehicle((VehicleData) arg);

+                    break;

+

+                case changes.Buoyancy:

+                    changeBuoyancy((float)arg);

+                    break;

+

+                case changes.PIDTarget:

+                    changePIDTarget((Vector3)arg);

+                    break;

+

+                case changes.PIDTau:

+                    changePIDTau((float)arg);

+                    break;

+

+                case changes.PIDActive:

+                    changePIDActive((bool)arg);

+                    break;

+

+                case changes.PIDHoverHeight:

+                    changePIDHoverHeight((float)arg);

+                    break;

+

+                case changes.PIDHoverType:

+                    changePIDHoverType((PIDHoverType)arg);

+                    break;

+

+                case changes.PIDHoverTau:

+                    changePIDHoverTau((float)arg);

+                    break;

+

+                case changes.PIDHoverActive:

+                    changePIDHoverActive((bool)arg);

+                    break;

+

+                case changes.Null:

+                    donullchange();

+                    break;

+

+

+

+                default:

+                    donullchange();

+                    break;

+            }

+            return false;

+        }

+

+        public void AddChange(changes what, object arg)

+        {

+            _parent_scene.AddChange((PhysicsActor) this, what, arg);

+        }

+

+

+        private struct strVehicleBoolParam

+        {

+            public int param;

+            public bool value;

+        }

+

+        private struct strVehicleFloatParam

+        {

+            public int param;

+            public float value;

+        }

+

+        private struct strVehicleQuatParam

+        {

+            public int param;

+            public Quaternion value;

+        }

+

+        private struct strVehicleVectorParam

+        {

+            public int param;

+            public Vector3 value;

+        }

+    }

+}

diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs
new file mode 100644
index 0000000..7fe3109
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs
@@ -0,0 +1,800 @@
+/*
+ * Copyright (c) Contributors, http://opensimulator.org/
+ * See CONTRIBUTORS.TXT for a full list of copyright holders.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the OpenSimulator Project nor the
+ *       names of its contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+using System;
+using System.Collections.Generic;
+using System.Reflection;
+using System.Runtime.InteropServices;
+using System.Text;
+using OpenSim.Framework;
+using OpenSim.Region.Physics.Manager;
+using OdeAPI;
+using log4net;
+using OpenMetaverse;
+
+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; // the ray. we only need one for our lifetime
+        IntPtr Sphere;
+        IntPtr Box;
+        IntPtr Plane;
+
+        private int CollisionContactGeomsPerTest = 25;
+        private const int DefaultMaxCount = 25;
+        private const int MaxTimePerCallMS = 30;
+
+        /// <summary>
+        /// ODE near callback delegate
+        /// </summary>
+        private d.NearCallback nearCallback;
+        private d.NearCallback nearProbeCallback;
+        private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
+        private List<ContactResult> m_contactResults = new List<ContactResult>();
+        private RayFilterFlags CurrentRayFilter;
+        private int CurrentMaxCount;
+
+        public ODERayCastRequestManager(OdeScene pScene)
+        {
+            m_scene = pScene;
+            nearCallback = near;
+            nearProbeCallback = nearProbe;
+            ray = d.CreateRay(IntPtr.Zero, 1.0f);
+            d.GeomSetCategoryBits(ray, 0);
+            Box = d.CreateBox(IntPtr.Zero, 1.0f, 1.0f, 1.0f);
+            d.GeomSetCategoryBits(Box, 0);
+            Sphere = d.CreateSphere(IntPtr.Zero,1.0f);
+            d.GeomSetCategoryBits(Sphere, 0);
+            Plane = d.CreatePlane(IntPtr.Zero, 0f,0f,1f,1f);
+            d.GeomSetCategoryBits(Sphere, 0);
+        }
+
+        public void QueueRequest(ODERayRequest req)
+        {
+            if (req.Count == 0)
+                req.Count = DefaultMaxCount;
+
+            m_PendingRequests.Enqueue(req);
+        }
+
+        /// <summary>
+        /// Process all queued raycast requests
+        /// </summary>
+        /// <returns>Time in MS the raycasts took to process.</returns>
+        public int ProcessQueuedRequests()
+        {
+
+            if (m_PendingRequests.Count <= 0)
+                return 0;
+
+            if (m_scene.ContactgeomsArray == IntPtr.Zero || ray == IntPtr.Zero)
+                // oops something got wrong or scene isn't ready still
+            {
+                m_PendingRequests.Clear();
+                return 0;
+            }
+
+            int time = Util.EnvironmentTickCount();
+
+            ODERayRequest req;
+            int closestHit;
+            int backfacecull;
+            CollisionCategories catflags;
+
+            while (m_PendingRequests.Dequeue(out req))
+            {
+                if (req.callbackMethod != null)
+                {
+                    CurrentRayFilter = req.filter;
+                    CurrentMaxCount = req.Count;
+
+                    CollisionContactGeomsPerTest = req.Count & 0xffff;
+
+                    closestHit = ((CurrentRayFilter & RayFilterFlags.ClosestHit) == 0 ? 0 : 1);
+                    backfacecull = ((CurrentRayFilter & RayFilterFlags.BackFaceCull) == 0 ? 0 : 1);
+
+                    if (req.callbackMethod is ProbeBoxCallback)
+                    {
+                        if (CollisionContactGeomsPerTest > 80)
+                            CollisionContactGeomsPerTest = 80;
+                        d.GeomBoxSetLengths(Box, req.Normal.X, req.Normal.Y, req.Normal.Z);
+                        d.GeomSetPosition(Box, req.Origin.X, req.Origin.Y, req.Origin.Z);
+                        d.Quaternion qtmp;
+                        qtmp.X = req.orientation.X;
+                        qtmp.Y = req.orientation.Y;
+                        qtmp.Z = req.orientation.Z;
+                        qtmp.W = req.orientation.W;
+                        d.GeomSetQuaternion(Box, ref qtmp);
+                    }
+                    else if (req.callbackMethod is ProbeSphereCallback)
+                    {
+                        if (CollisionContactGeomsPerTest > 80)
+                            CollisionContactGeomsPerTest = 80;
+
+                        d.GeomSphereSetRadius(Sphere, req.length);
+                        d.GeomSetPosition(Sphere, req.Origin.X, req.Origin.Y, req.Origin.Z);
+                    }
+                    else if (req.callbackMethod is ProbePlaneCallback)
+                    {
+                        if (CollisionContactGeomsPerTest > 80)
+                            CollisionContactGeomsPerTest = 80;
+
+                        d.GeomPlaneSetParams(Plane, req.Normal.X, req.Normal.Y, req.Normal.Z, req.length);
+                    }
+
+                    else
+                    {
+                        if (CollisionContactGeomsPerTest > 25)
+                            CollisionContactGeomsPerTest = 25;
+
+                        d.GeomRaySetLength(ray, req.length);
+                        d.GeomRaySet(ray, req.Origin.X, req.Origin.Y, req.Origin.Z, req.Normal.X, req.Normal.Y, req.Normal.Z);
+                        d.GeomRaySetParams(ray, 0, backfacecull);
+                        d.GeomRaySetClosestHit(ray, closestHit);
+
+                        if (req.callbackMethod is RaycastCallback)
+                        {
+                            // if we only want one get only one per Collision pair saving memory
+                            CurrentRayFilter |= RayFilterFlags.ClosestHit;
+                            d.GeomRaySetClosestHit(ray, 1);
+                        }
+                        else
+                            d.GeomRaySetClosestHit(ray, closestHit);
+                    }
+
+                    if ((CurrentRayFilter & RayFilterFlags.ContactsUnImportant) != 0)
+                        unchecked
+                        {
+                            CollisionContactGeomsPerTest |= (int)d.CONTACTS_UNIMPORTANT;
+                        }
+
+                    if (req.geom == IntPtr.Zero)
+                    {
+                        // translate ray filter to Collision flags
+                        catflags = 0;
+                        if ((CurrentRayFilter & RayFilterFlags.volumedtc) != 0)
+                            catflags |= CollisionCategories.VolumeDtc;
+                        if ((CurrentRayFilter & RayFilterFlags.phantom) != 0)
+                            catflags |= CollisionCategories.Phantom;
+                        if ((CurrentRayFilter & RayFilterFlags.agent) != 0)
+                            catflags |= CollisionCategories.Character;
+                        if ((CurrentRayFilter & RayFilterFlags.PrimsNonPhantom) != 0)
+                            catflags |= CollisionCategories.Geom;
+                        if ((CurrentRayFilter & RayFilterFlags.land) != 0)
+                            catflags |= CollisionCategories.Land;
+                        if ((CurrentRayFilter & RayFilterFlags.water) != 0)
+                            catflags |= CollisionCategories.Water;
+
+                        if (catflags != 0)
+                        {
+                            if (req.callbackMethod is ProbeBoxCallback)
+                            {
+                                catflags |= CollisionCategories.Space;
+                                d.GeomSetCollideBits(Box, (uint)catflags);
+                                d.GeomSetCategoryBits(Box, (uint)catflags);
+                                doProbe(req, Box);
+                            }
+                            else if (req.callbackMethod is ProbeSphereCallback)
+                            {
+                                catflags |= CollisionCategories.Space;
+                                d.GeomSetCollideBits(Sphere, (uint)catflags);
+                                d.GeomSetCategoryBits(Sphere, (uint)catflags);
+                                doProbe(req, Sphere);
+                            }
+                            else if (req.callbackMethod is ProbePlaneCallback)
+                            {
+                                catflags |= CollisionCategories.Space;
+                                d.GeomSetCollideBits(Plane, (uint)catflags);
+                                d.GeomSetCategoryBits(Plane, (uint)catflags);
+                                doPlane(req);
+                            }
+                            else
+                            {
+                                d.GeomSetCollideBits(ray, (uint)catflags);
+                                doSpaceRay(req);
+                            }
+                        }
+                    }
+                    else
+                    {
+                        // if we select a geom don't use filters
+
+                        if (req.callbackMethod is ProbePlaneCallback)
+                        {
+                            d.GeomSetCollideBits(Plane, (uint)CollisionCategories.All);
+                            doPlane(req);
+                        }
+                        else
+                        {
+                            d.GeomSetCollideBits(ray, (uint)CollisionCategories.All);
+                            doGeomRay(req);
+                        }
+                    }
+                }
+
+                if (Util.EnvironmentTickCountSubtract(time) > MaxTimePerCallMS)
+                    break;
+            }
+
+            lock (m_contactResults)
+                m_contactResults.Clear();
+
+            return Util.EnvironmentTickCountSubtract(time);
+        }
+        /// <summary>
+        /// Method that actually initiates the raycast with spaces
+        /// </summary>
+        /// <param name="req"></param>
+        /// 
+
+        private const RayFilterFlags FilterActiveSpace = RayFilterFlags.agent | RayFilterFlags.physical | RayFilterFlags.LSLPhanton;
+//        private const RayFilterFlags FilterStaticSpace = RayFilterFlags.water | RayFilterFlags.land | RayFilterFlags.nonphysical | RayFilterFlags.LSLPhanton;
+        private const RayFilterFlags FilterStaticSpace = RayFilterFlags.water | RayFilterFlags.nonphysical | RayFilterFlags.LSLPhanton;
+
+        private void doSpaceRay(ODERayRequest req)
+        {
+            // Collide tests
+            if ((CurrentRayFilter & FilterActiveSpace) != 0)
+            {
+                d.SpaceCollide2(ray, m_scene.ActiveSpace, IntPtr.Zero, nearCallback);
+                d.SpaceCollide2(ray, m_scene.CharsSpace, IntPtr.Zero, nearCallback);
+            }
+            if ((CurrentRayFilter & FilterStaticSpace) != 0 && (m_contactResults.Count < CurrentMaxCount))
+                d.SpaceCollide2(ray, m_scene.StaticSpace, IntPtr.Zero, nearCallback);
+            if ((CurrentRayFilter & RayFilterFlags.land) != 0 && (m_contactResults.Count < CurrentMaxCount))
+            {
+                // current ode land to ray collisions is very bad
+                // so for now limit its range badly
+
+                if (req.length > 30.0f)
+                    d.GeomRaySetLength(ray, 30.0f);
+
+                d.SpaceCollide2(ray, m_scene.GroundSpace, IntPtr.Zero, nearCallback);
+            }
+
+            if (req.callbackMethod is RaycastCallback)
+            {
+                // Define default results
+                bool hitYN = false;
+                uint hitConsumerID = 0;
+                float distance = float.MaxValue;
+                Vector3 closestcontact = Vector3.Zero;
+                Vector3 snormal = Vector3.Zero;
+
+                // Find closest contact and object.
+                lock (m_contactResults)
+                {
+                    foreach (ContactResult cResult in m_contactResults)
+                    {
+                        if(cResult.Depth < distance)
+                        {
+                            closestcontact = cResult.Pos;
+                            hitConsumerID = cResult.ConsumerID;
+                            distance = cResult.Depth;
+                            snormal = cResult.Normal;
+                        }
+                    }
+                    m_contactResults.Clear();
+                }
+
+                if (distance > 0 && distance < float.MaxValue)
+                    hitYN = true;
+                ((RaycastCallback)req.callbackMethod)(hitYN, closestcontact, hitConsumerID, distance, snormal);
+            }
+            else
+            {
+                List<ContactResult> cresult = new List<ContactResult>(m_contactResults.Count);
+                lock (m_PendingRequests)
+                {
+                    cresult.AddRange(m_contactResults);
+                    m_contactResults.Clear();
+                }
+                ((RayCallback)req.callbackMethod)(cresult);
+            }
+        }
+
+        private void doProbe(ODERayRequest req, IntPtr probe)
+        {
+            // Collide tests
+            if ((CurrentRayFilter & FilterActiveSpace) != 0)
+            {
+                d.SpaceCollide2(probe, m_scene.ActiveSpace, IntPtr.Zero, nearCallback);
+                d.SpaceCollide2(probe, m_scene.CharsSpace, IntPtr.Zero, nearCallback);
+            }
+            if ((CurrentRayFilter & FilterStaticSpace) != 0 && (m_contactResults.Count < CurrentMaxCount))
+                d.SpaceCollide2(probe, m_scene.StaticSpace, IntPtr.Zero, nearCallback);
+            if ((CurrentRayFilter & RayFilterFlags.land) != 0 && (m_contactResults.Count < CurrentMaxCount))
+                d.SpaceCollide2(probe, m_scene.GroundSpace, IntPtr.Zero, nearCallback);
+
+            List<ContactResult> cresult = new List<ContactResult>(m_contactResults.Count);
+            lock (m_PendingRequests)
+            {
+                cresult.AddRange(m_contactResults);
+                m_contactResults.Clear();
+            }
+            if (req.callbackMethod is ProbeBoxCallback)
+                ((ProbeBoxCallback)req.callbackMethod)(cresult);
+            else if (req.callbackMethod is ProbeSphereCallback)
+                ((ProbeSphereCallback)req.callbackMethod)(cresult);
+        }
+
+        private void doPlane(ODERayRequest req)
+        {
+            // Collide tests
+            if (req.geom == IntPtr.Zero)
+            {
+                if ((CurrentRayFilter & FilterActiveSpace) != 0)
+                {
+                    d.SpaceCollide2(Plane, m_scene.ActiveSpace, IntPtr.Zero, nearCallback);
+                    d.SpaceCollide2(Plane, m_scene.CharsSpace, IntPtr.Zero, nearCallback);
+                }
+                if ((CurrentRayFilter & FilterStaticSpace) != 0 && (m_contactResults.Count < CurrentMaxCount))
+                    d.SpaceCollide2(Plane, m_scene.StaticSpace, IntPtr.Zero, nearCallback);
+                if ((CurrentRayFilter & RayFilterFlags.land) != 0 && (m_contactResults.Count < CurrentMaxCount))
+                    d.SpaceCollide2(Plane, m_scene.GroundSpace, IntPtr.Zero, nearCallback);
+            }
+            else
+            {
+                d.SpaceCollide2(Plane, req.geom, IntPtr.Zero, nearCallback);
+            }
+
+            List<ContactResult> cresult = new List<ContactResult>(m_contactResults.Count);
+            lock (m_PendingRequests)
+            {
+                cresult.AddRange(m_contactResults);
+                m_contactResults.Clear();
+            }
+
+            ((ProbePlaneCallback)req.callbackMethod)(cresult);
+        }
+
+        /// <summary>
+        /// Method that actually initiates the raycast with a geom
+        /// </summary>
+        /// <param name="req"></param>
+        private void doGeomRay(ODERayRequest req)
+        {
+            // Collide test
+            d.SpaceCollide2(ray, req.geom, IntPtr.Zero, nearCallback); // still do this to have full AABB pre test
+
+            if (req.callbackMethod is RaycastCallback)
+            {
+                // Define default results
+                bool hitYN = false;
+                uint hitConsumerID = 0;
+                float distance = float.MaxValue;
+                Vector3 closestcontact = Vector3.Zero;
+                Vector3 snormal = Vector3.Zero;
+
+                // Find closest contact and object.
+                lock (m_contactResults)
+                {
+                    foreach (ContactResult cResult in m_contactResults)
+                    {
+                        if(cResult.Depth < distance )
+                        {
+                            closestcontact = cResult.Pos;
+                            hitConsumerID = cResult.ConsumerID;
+                            distance = cResult.Depth;
+                            snormal = cResult.Normal;
+                        }
+                    }
+                    m_contactResults.Clear();
+                }
+
+                if (distance > 0 && distance < float.MaxValue)
+                    hitYN = true;
+
+                ((RaycastCallback)req.callbackMethod)(hitYN, closestcontact, hitConsumerID, distance, snormal);
+            }
+            else
+            {
+                List<ContactResult> cresult = new List<ContactResult>(m_contactResults.Count);
+                lock (m_PendingRequests)
+                {
+                    cresult.AddRange(m_contactResults);
+                    m_contactResults.Clear();
+                }
+                ((RayCallback)req.callbackMethod)(cresult);
+            }
+        }
+
+        private bool GetCurContactGeom(int index, ref d.ContactGeom newcontactgeom)
+        {
+            IntPtr ContactgeomsArray = m_scene.ContactgeomsArray;
+            if (ContactgeomsArray == IntPtr.Zero || index >= CollisionContactGeomsPerTest)
+                return false;
+
+            IntPtr contactptr = new IntPtr(ContactgeomsArray.ToInt64() + (Int64)(index * d.ContactGeom.unmanagedSizeOf));
+            newcontactgeom = (d.ContactGeom)Marshal.PtrToStructure(contactptr, typeof(d.ContactGeom));
+            return true;
+        }
+        
+        // This is the standard Near. g1 is the ray
+        private void near(IntPtr space, IntPtr g1, IntPtr g2)
+        {
+             if (g2 == IntPtr.Zero || g1 == g2)
+                return;
+
+             if (m_contactResults.Count >= CurrentMaxCount)
+                return;
+
+            if (d.GeomIsSpace(g2))
+            {
+                try
+                {
+                    d.SpaceCollide2(g1, g2, IntPtr.Zero, nearCallback);
+                }
+                catch (Exception e)
+                {
+                    m_log.WarnFormat("[PHYSICS Ray]: Unable to Space collide test an object: {0}", e.Message);
+                }
+                return;
+            }
+
+            int count = 0;
+            try
+            {
+                count = d.CollidePtr(g1, g2, CollisionContactGeomsPerTest, m_scene.ContactgeomsArray, d.ContactGeom.unmanagedSizeOf);
+            }
+            catch (Exception e)
+            {
+                m_log.WarnFormat("[PHYSICS Ray]: Unable to collide test an object: {0}", e.Message);
+                return;
+            }
+
+            if (count == 0)
+                return;
+/*
+            uint cat1 = d.GeomGetCategoryBits(g1);
+            uint cat2 = d.GeomGetCategoryBits(g2);
+            uint col1 = d.GeomGetCollideBits(g1);
+            uint col2 = d.GeomGetCollideBits(g2);
+*/
+            
+            uint ID = 0;
+            PhysicsActor p2 = null;
+
+            m_scene.actor_name_map.TryGetValue(g2, out p2);
+
+            if (p2 == null)
+                return;
+
+            switch (p2.PhysicsActorType)
+            {
+                case (int)ActorTypes.Prim:
+
+                    RayFilterFlags thisFlags;
+
+                    if (p2.IsPhysical)
+                        thisFlags = RayFilterFlags.physical;
+                    else
+                        thisFlags = RayFilterFlags.nonphysical;
+
+                    if (p2.Phantom)
+                        thisFlags |= RayFilterFlags.phantom;
+
+                    if (p2.IsVolumeDtc)
+                        thisFlags |= RayFilterFlags.volumedtc;
+
+                    if ((thisFlags & CurrentRayFilter) == 0)
+                        return;
+
+                    ID = ((OdePrim)p2).LocalID;
+                    break;
+
+                case (int)ActorTypes.Agent:
+
+                    if ((CurrentRayFilter & RayFilterFlags.agent) == 0)
+                        return;
+                    else
+                        ID = ((OdeCharacter)p2).LocalID;
+                    break;
+
+                case (int)ActorTypes.Ground:
+
+                    if ((CurrentRayFilter & RayFilterFlags.land) == 0)
+                        return;
+                    break;
+
+                case (int)ActorTypes.Water:
+
+                    if ((CurrentRayFilter & RayFilterFlags.water) == 0)
+                        return;
+                    break;
+
+                default:
+                    break;
+            }
+
+            d.ContactGeom curcontact = new d.ContactGeom();
+
+            // closestHit for now only works for meshs, so must do it for others
+            if ((CurrentRayFilter & RayFilterFlags.ClosestHit) == 0)
+            {
+                // Loop all contacts, build results.
+                for (int i = 0; i < count; i++)
+                {
+                    if (!GetCurContactGeom(i, ref curcontact))
+                        break;
+
+                    ContactResult collisionresult = new ContactResult();
+                    collisionresult.ConsumerID = ID;
+                    collisionresult.Pos.X = curcontact.pos.X;
+                    collisionresult.Pos.Y = curcontact.pos.Y;
+                    collisionresult.Pos.Z = curcontact.pos.Z;
+                    collisionresult.Depth = curcontact.depth;
+                    collisionresult.Normal.X = curcontact.normal.X;
+                    collisionresult.Normal.Y = curcontact.normal.Y;
+                    collisionresult.Normal.Z = curcontact.normal.Z;
+                    lock (m_contactResults)
+                    {
+                        m_contactResults.Add(collisionresult);
+                        if (m_contactResults.Count >= CurrentMaxCount)
+                            return;
+                    }
+                }
+            }
+            else
+            {
+                // keep only closest contact
+                ContactResult collisionresult = new ContactResult();
+                collisionresult.ConsumerID = ID;
+                collisionresult.Depth = float.MaxValue;
+
+                for (int i = 0; i < count; i++)
+                {
+                    if (!GetCurContactGeom(i, ref curcontact))
+                        break;
+
+                    if (curcontact.depth < collisionresult.Depth)
+                    {
+                        collisionresult.Pos.X = curcontact.pos.X;
+                        collisionresult.Pos.Y = curcontact.pos.Y;
+                        collisionresult.Pos.Z = curcontact.pos.Z;
+                        collisionresult.Depth = curcontact.depth;
+                        collisionresult.Normal.X = curcontact.normal.X;
+                        collisionresult.Normal.Y = curcontact.normal.Y;
+                        collisionresult.Normal.Z = curcontact.normal.Z;
+                    }
+                }
+
+                if (collisionresult.Depth != float.MaxValue)
+                {
+                    lock (m_contactResults)
+                        m_contactResults.Add(collisionresult);
+                }
+            }
+        }
+
+        private void nearProbe(IntPtr space, IntPtr g1, IntPtr g2)
+        {
+            if (g1 == IntPtr.Zero || g1 == g2)
+                return;
+
+            if (m_contactResults.Count >= CurrentMaxCount)
+                return;
+
+            if (d.GeomIsSpace(g1))
+            {
+                try
+                {
+                    d.SpaceCollide2(g1, g2, IntPtr.Zero, nearProbeCallback);
+                }
+                catch (Exception e)
+                {
+                    m_log.WarnFormat("[PHYSICS Ray]: Unable to Space collide test an object: {0}", e.Message);
+                }
+                return;
+            }
+
+            int count = 0;
+            try
+            {
+                count = d.CollidePtr(g1, g2, CollisionContactGeomsPerTest, m_scene.ContactgeomsArray, d.ContactGeom.unmanagedSizeOf);
+            }
+            catch (Exception e)
+            {
+                m_log.WarnFormat("[PHYSICS Ray]: Unable to collide test an object: {0}", e.Message);
+                return;
+            }
+
+            if (count == 0)
+                return;
+
+            uint ID = 0;
+            PhysicsActor p1 = null;
+
+            m_scene.actor_name_map.TryGetValue(g1, out p1);
+
+            if (p1 == null)
+                return;
+
+            switch (p1.PhysicsActorType)
+            {
+                case (int)ActorTypes.Prim:
+
+                    RayFilterFlags thisFlags;
+
+                    if (p1.IsPhysical)
+                        thisFlags = RayFilterFlags.physical;
+                    else
+                        thisFlags = RayFilterFlags.nonphysical;
+
+                    if (p1.Phantom)
+                        thisFlags |= RayFilterFlags.phantom;
+
+                    if (p1.IsVolumeDtc)
+                        thisFlags |= RayFilterFlags.volumedtc;
+
+                    if ((thisFlags & CurrentRayFilter) == 0)
+                        return;
+
+                    ID = ((OdePrim)p1).LocalID;
+                    break;
+
+                case (int)ActorTypes.Agent:
+
+                    if ((CurrentRayFilter & RayFilterFlags.agent) == 0)
+                        return;
+                    else
+                        ID = ((OdeCharacter)p1).LocalID;
+                    break;
+
+                case (int)ActorTypes.Ground:
+
+                    if ((CurrentRayFilter & RayFilterFlags.land) == 0)
+                        return;
+                    break;
+
+                case (int)ActorTypes.Water:
+
+                    if ((CurrentRayFilter & RayFilterFlags.water) == 0)
+                        return;
+                    break;
+
+                default:
+                    break;
+            }
+
+            d.ContactGeom curcontact = new d.ContactGeom();
+
+            // closestHit for now only works for meshs, so must do it for others
+            if ((CurrentRayFilter & RayFilterFlags.ClosestHit) == 0)
+            {
+                // Loop all contacts, build results.
+                for (int i = 0; i < count; i++)
+                {
+                    if (!GetCurContactGeom(i, ref curcontact))
+                        break;
+
+                    ContactResult collisionresult = new ContactResult();
+                    collisionresult.ConsumerID = ID;
+                    collisionresult.Pos.X = curcontact.pos.X;
+                    collisionresult.Pos.Y = curcontact.pos.Y;
+                    collisionresult.Pos.Z = curcontact.pos.Z;
+                    collisionresult.Depth = curcontact.depth;
+                    collisionresult.Normal.X = curcontact.normal.X;
+                    collisionresult.Normal.Y = curcontact.normal.Y;
+                    collisionresult.Normal.Z = curcontact.normal.Z;
+                    lock (m_contactResults)
+                    {
+                        m_contactResults.Add(collisionresult);
+                        if (m_contactResults.Count >= CurrentMaxCount)
+                            return;
+                    }
+                }
+            }
+            else
+            {
+                // keep only closest contact
+                ContactResult collisionresult = new ContactResult();
+                collisionresult.ConsumerID = ID;
+                collisionresult.Depth = float.MaxValue;
+
+                for (int i = 0; i < count; i++)
+                {
+                    if (!GetCurContactGeom(i, ref curcontact))
+                        break;
+
+                    if (curcontact.depth < collisionresult.Depth)
+                    {
+                        collisionresult.Pos.X = curcontact.pos.X;
+                        collisionresult.Pos.Y = curcontact.pos.Y;
+                        collisionresult.Pos.Z = curcontact.pos.Z;
+                        collisionresult.Depth = curcontact.depth;
+                        collisionresult.Normal.X = curcontact.normal.X;
+                        collisionresult.Normal.Y = curcontact.normal.Y;
+                        collisionresult.Normal.Z = curcontact.normal.Z;
+                    }
+                }
+
+                if (collisionresult.Depth != float.MaxValue)
+                {
+                    lock (m_contactResults)
+                        m_contactResults.Add(collisionresult);
+                }
+            }
+        }
+
+        /// <summary>
+        /// Dereference the creator scene so that it can be garbage collected if needed.
+        /// </summary>
+        internal void Dispose()
+        {
+            m_scene = null;
+            if (ray != IntPtr.Zero)
+            {
+                d.GeomDestroy(ray);
+                ray = IntPtr.Zero;
+            }
+            if (Box != IntPtr.Zero)
+            {
+                d.GeomDestroy(Box);
+                Box = IntPtr.Zero;
+            }
+            if (Sphere != IntPtr.Zero)
+            {
+                d.GeomDestroy(Sphere);
+                Sphere = IntPtr.Zero;
+            }
+            if (Plane != IntPtr.Zero)
+            {
+                d.GeomDestroy(Plane);
+                Plane = IntPtr.Zero;
+            }           
+        }
+    }
+
+    public struct ODERayRequest
+    {
+        public IntPtr geom;
+        public Vector3 Origin;
+        public Vector3 Normal;
+        public int Count;
+        public float length;
+        public object callbackMethod;
+        public RayFilterFlags filter;
+        public Quaternion orientation;
+    }
+}
\ No newline at end of file
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODESitAvatar.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODESitAvatar.cs
new file mode 100644
index 0000000..e9023c3
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODESitAvatar.cs
@@ -0,0 +1,356 @@
+/*
+ * Copyright (c) Contributors, http://opensimulator.org/
+ * See CONTRIBUTORS.TXT for a full list of copyright holders.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the OpenSimulator Project nor the
+ *       names of its contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+// Ubit 2012
+using System;
+using System.Collections.Generic;
+using System.Reflection;
+using System.Runtime.InteropServices;
+using System.Text;
+using OpenSim.Framework;
+using OpenSim.Region.Physics.Manager;
+using OdeAPI;
+using log4net;
+using OpenMetaverse;
+
+namespace OpenSim.Region.Physics.OdePlugin
+{
+    /// <summary>
+    /// </summary>
+    public class ODESitAvatar
+    {
+        private OdeScene m_scene;
+        private ODERayCastRequestManager m_raymanager;
+
+        public ODESitAvatar(OdeScene pScene, ODERayCastRequestManager raymanager)
+        {
+            m_scene = pScene;
+            m_raymanager = raymanager;
+        }
+
+        private static Vector3 SitAjust = new Vector3(0, 0, 0.4f);
+        private const RayFilterFlags RaySitFlags = RayFilterFlags.AllPrims | RayFilterFlags.ClosestHit;
+
+        private void RotAroundZ(float x, float y, ref Quaternion ori)
+        {
+            double ang = Math.Atan2(y, x);
+            ang *= 0.5d;
+            float s = (float)Math.Sin(ang);
+            float c = (float)Math.Cos(ang);
+
+            ori.X = 0;
+            ori.Y = 0;
+            ori.Z = s;
+            ori.W = c;
+        }
+
+
+        public void Sit(PhysicsActor actor, Vector3 avPos, Vector3 avCameraPosition, Vector3 offset, Vector3 avOffset, SitAvatarCallback PhysicsSitResponse)
+        {
+            if (!m_scene.haveActor(actor) || !(actor is OdePrim) || ((OdePrim)actor).prim_geom == IntPtr.Zero)
+            {
+                PhysicsSitResponse(-1, actor.LocalID, offset, Quaternion.Identity);
+                return;
+            }
+
+            IntPtr geom = ((OdePrim)actor).prim_geom;
+
+//            Vector3 geopos = d.GeomGetPositionOMV(geom);
+//            Quaternion geomOri = d.GeomGetQuaternionOMV(geom);
+
+            Vector3 geopos = actor.Position;
+            Quaternion geomOri = actor.Orientation;
+
+            Quaternion geomInvOri = Quaternion.Conjugate(geomOri);
+
+            Quaternion ori = Quaternion.Identity;
+
+            Vector3 rayDir = geopos + offset - avCameraPosition;
+
+            float raylen = rayDir.Length();
+            if (raylen < 0.001f)
+            {
+                PhysicsSitResponse(-1, actor.LocalID, offset, Quaternion.Identity);
+                return;
+            }
+            float t = 1 / raylen;
+            rayDir.X *= t;
+            rayDir.Y *= t;
+            rayDir.Z *= t;
+
+            raylen += 30f; // focal point may be far
+            List<ContactResult> rayResults;
+
+            rayResults = m_scene.RaycastActor(actor, avCameraPosition, rayDir, raylen, 1, RaySitFlags);
+            if (rayResults.Count == 0)
+            {
+/* if this fundamental ray failed, then just fail so user can try another spot and not be sitted far on a big prim
+                d.AABB aabb;
+                d.GeomGetAABB(geom, out aabb);
+                offset = new Vector3(avOffset.X, 0, aabb.MaxZ + avOffset.Z - geopos.Z);
+                ori = geomInvOri;
+                offset *= geomInvOri;
+                PhysicsSitResponse(1, actor.LocalID, offset, ori);
+*/
+                PhysicsSitResponse(0, actor.LocalID, offset, ori);
+                return;
+            }
+
+            int status = 1;
+
+            offset = rayResults[0].Pos - geopos;
+
+            d.GeomClassID geoclass = d.GeomGetClass(geom);
+
+            if (geoclass == d.GeomClassID.SphereClass)
+            {
+                float r = d.GeomSphereGetRadius(geom);
+
+                offset.Normalize();
+                offset *= r;
+
+                RotAroundZ(offset.X, offset.Y, ref ori);
+
+                if (r < 0.4f)
+                {
+                    offset = new Vector3(0, 0, r);
+                }
+                else
+                {
+                    if (offset.Z < 0.4f)
+                    {
+                        t = offset.Z;
+                        float rsq = r * r;
+
+                        t = 1.0f / (rsq - t * t);
+                        offset.X *= t;
+                        offset.Y *= t;
+                        offset.Z = 0.4f;
+                        t = rsq - 0.16f;
+                        offset.X *= t;
+                        offset.Y *= t;
+                    }
+                    else if (r > 0.8f && offset.Z > 0.8f * r)
+                    {
+                        status = 3;
+                        avOffset.X = -avOffset.X;
+                        avOffset.Z *= 1.6f;
+                    }
+                }
+
+                offset += avOffset * ori;
+
+                ori = geomInvOri * ori;
+                offset *= geomInvOri;
+
+                PhysicsSitResponse(status, actor.LocalID, offset, ori);
+                return;
+            }
+
+            Vector3 norm = rayResults[0].Normal;
+
+            if (norm.Z < -0.4f)
+            {
+                PhysicsSitResponse(0, actor.LocalID, offset, Quaternion.Identity);
+                return;
+            }
+
+
+            float SitNormX = -rayDir.X;
+            float SitNormY = -rayDir.Y;
+
+            Vector3 pivot = geopos + offset;
+
+            float edgeNormalX = norm.X;
+            float edgeNormalY = norm.Y;
+            float edgeDirX = -rayDir.X;
+            float edgeDirY = -rayDir.Y;
+            Vector3 edgePos = rayResults[0].Pos;
+            float edgeDist = float.MaxValue;
+
+            bool foundEdge = false;
+
+            if (norm.Z < 0.5f)
+            {
+                float rayDist = 4.0f;
+
+                for (int i = 0; i < 6; i++)
+                {
+                    pivot.X -= 0.01f * norm.X;
+                    pivot.Y -= 0.01f * norm.Y;
+                    pivot.Z -= 0.01f * norm.Z;
+
+                    rayDir.X = -norm.X * norm.Z;
+                    rayDir.Y = -norm.Y * norm.Z;
+                    rayDir.Z = 1.0f - norm.Z * norm.Z;
+                    rayDir.Normalize();
+
+                    rayResults = m_scene.RaycastActor(actor, pivot, rayDir, rayDist, 1, RayFilterFlags.AllPrims);
+                    if (rayResults.Count == 0)
+                        break;
+
+                    if (Math.Abs(rayResults[0].Normal.Z) < 0.7f)
+                    {
+                        rayDist -= rayResults[0].Depth;
+                        if (rayDist < 0f)
+                            break;
+
+                        pivot = rayResults[0].Pos;
+                        norm = rayResults[0].Normal;
+                        edgeNormalX = norm.X;
+                        edgeNormalY = norm.Y;
+                        edgeDirX = -rayDir.X;
+                        edgeDirY = -rayDir.Y;
+                    }
+                    else
+                    {
+                        foundEdge = true;
+                        edgePos = rayResults[0].Pos;
+                        break;
+                    }
+                }
+
+                if (!foundEdge)
+                {
+                    PhysicsSitResponse(0, actor.LocalID, offset, ori);
+                    return;
+                }
+                avOffset.X *= 0.5f;
+            }
+
+            else if (norm.Z > 0.866f)
+            {
+                float toCamBaseX = avCameraPosition.X - pivot.X;
+                float toCamBaseY = avCameraPosition.Y - pivot.Y;
+                float toCamX = toCamBaseX;
+                float toCamY = toCamBaseY;
+
+                for (int j = 0; j < 4; j++)
+                {
+                    float rayDist = 1.0f;
+                    float curEdgeDist = 0.0f;
+
+                    for (int i = 0; i < 3; i++)
+                    {
+                        pivot.Z -= 0.01f;
+                        rayDir.X = toCamX;
+                        rayDir.Y = toCamY;
+                        rayDir.Z = (-toCamX * norm.X - toCamY * norm.Y) / norm.Z;
+                        rayDir.Normalize();
+
+                        rayResults = m_scene.RaycastActor(actor, pivot, rayDir, rayDist, 1, RayFilterFlags.AllPrims);
+                        if (rayResults.Count == 0)
+                            break;
+
+                        curEdgeDist += rayResults[0].Depth;
+
+                        if (rayResults[0].Normal.Z > 0.5f)
+                        {
+                            rayDist -= rayResults[0].Depth;
+                            if (rayDist < 0f)
+                                break;
+
+                            pivot = rayResults[0].Pos;
+                            norm = rayResults[0].Normal;
+                        }
+                        else
+                        {
+                            foundEdge = true;
+                            if (curEdgeDist < edgeDist)
+                            {
+                                edgeDist = curEdgeDist;
+                                edgeNormalX = rayResults[0].Normal.X;
+                                edgeNormalY = rayResults[0].Normal.Y;
+                                edgeDirX = rayDir.X;
+                                edgeDirY = rayDir.Y;
+                                edgePos = rayResults[0].Pos;
+                            }
+                            break;
+                        }
+                    }
+                    if (foundEdge && edgeDist < 0.2f)
+                        break;
+
+                    pivot = geopos + offset;
+
+                    switch (j)
+                    {
+                        case 0:
+                            toCamX = -toCamBaseY;
+                            toCamY = toCamBaseX;
+                            break;
+                        case 1:
+                            toCamX = toCamBaseY;
+                            toCamY = -toCamBaseX;
+                            break;
+                        case 2:
+                            toCamX = -toCamBaseX;
+                            toCamY = -toCamBaseY;
+                            break;
+                        default:
+                            break;
+                    }
+                }
+
+                if (!foundEdge)
+                {
+                    avOffset.X = -avOffset.X;
+                    avOffset.Z *= 1.6f;
+
+                    RotAroundZ(SitNormX, SitNormY, ref ori);
+
+                    offset += avOffset * ori;
+
+                    ori = geomInvOri * ori;
+                    offset *= geomInvOri;
+
+                    PhysicsSitResponse(3, actor.LocalID, offset, ori);
+                    return;
+                }
+                avOffset.X *= 0.5f;
+            }
+
+            SitNormX = edgeNormalX;
+            SitNormY = edgeNormalY;
+            if (edgeDirX * SitNormX + edgeDirY * SitNormY < 0)
+            {
+                SitNormX = -SitNormX;
+                SitNormY = -SitNormY;
+            }
+
+            RotAroundZ(SitNormX, SitNormY, ref ori);
+
+            offset = edgePos + avOffset * ori;
+            offset -= geopos;
+
+            ori = geomInvOri * ori;
+            offset *= geomInvOri;
+
+            PhysicsSitResponse(1, actor.LocalID, offset, ori);
+            return;
+        }
+    }
+}
\ 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..10d7d50
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs
@@ -0,0 +1,2025 @@
+/*
+ * based on:
+ * Ode.NET - .NET bindings for ODE
+ * Jason Perkins (starkos@industriousone.com)
+  *  Licensed under the New BSD
+ *  Part of the OpenDynamicsEngine
+Open Dynamics Engine
+Copyright (c) 2001-2007, Russell L. Smith.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions
+are met:
+
+Redistributions of source code must retain the above copyright notice,
+this list of conditions and the following disclaimer.
+
+Redistributions in binary form must reproduce the above copyright notice,
+this list of conditions and the following disclaimer in the documentation
+and/or other materials provided with the distribution.
+
+Neither the names of ODE's copyright owner nor the names of its
+contributors may be used to endorse or promote products derived from
+this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ * 
+ * changes by opensim team; 
+ * changes by Aurora team http://www.aurora-sim.org/
+  
+ * Revision/fixs by Ubit Umarov
+ */
+
+using System;
+using System.Runtime.InteropServices;
+using System.Security;
+using OMV = OpenMetaverse;
+namespace OdeAPI
+{
+//#if dDOUBLE
+// don't see much use in double precision with time steps of 20ms and 10 iterations used on opensim
+// at least we save same memory and memory access time, FPU performance on intel usually is similar
+//      using dReal = System.Double;
+//#else
+        using dReal = System.Single;
+//#endif
+
+        public static class d
+        {
+                public static dReal Infinity = dReal.MaxValue;
+        public static int NTotalBodies = 0;
+        public static int NTotalGeoms = 0;
+
+        public const uint CONTACTS_UNIMPORTANT = 0x80000000;
+
+                #region Flags and Enumerations
+
+        [Flags]
+        public enum AllocateODEDataFlags : uint
+        {
+            BasicData = 0,
+            CollisionData = 0x00000001,
+            All = ~0u
+        }
+
+        [Flags]
+        public enum IniteODEFlags : uint
+        {
+            dInitFlagManualThreadCleanup = 0x00000001
+        }
+
+                [Flags]
+                public enum ContactFlags : int
+                {
+                        Mu2 = 0x001,
+                        FDir1 = 0x002,
+                        Bounce = 0x004,
+                        SoftERP = 0x008,
+                        SoftCFM = 0x010,
+                        Motion1 = 0x020,
+                        Motion2 = 0x040,
+            MotionN = 0x080,
+                        Slip1 = 0x100,
+                        Slip2 = 0x200,
+                        Approx0 = 0x0000,
+                        Approx1_1 = 0x1000,
+                        Approx1_2 = 0x2000,
+                        Approx1 = 0x3000
+                }
+
+                public enum GeomClassID : int
+                {
+                        SphereClass,
+                        BoxClass,
+                        CapsuleClass,
+                        CylinderClass,
+                        PlaneClass,
+                        RayClass,
+                        ConvexClass,
+                        GeomTransformClass,
+                        TriMeshClass,
+            HeightfieldClass,
+                        FirstSpaceClass,
+                        SimpleSpaceClass = FirstSpaceClass,
+                        HashSpaceClass,
+                        QuadTreeSpaceClass,
+                        LastSpaceClass = QuadTreeSpaceClass,
+            UbitTerrainClass,
+                        FirstUserClass,
+                        LastUserClass = FirstUserClass + MaxUserClasses - 1,
+                        NumClasses,
+                        MaxUserClasses = 5
+                }
+
+                public enum JointType : int
+                {
+                        None,
+                        Ball,
+                        Hinge,
+                        Slider,
+                        Contact,
+                        Universal,
+                        Hinge2,
+                        Fixed,
+                        Null,
+                        AMotor,
+                        LMotor,
+                        Plane2D
+                }
+
+                public enum JointParam : int
+                {
+                        LoStop,
+                        HiStop,
+                        Vel,
+                        FMax,
+                        FudgeFactor,
+                        Bounce,
+                        CFM,
+                        StopERP,
+                        StopCFM,
+                        SuspensionERP,
+                        SuspensionCFM,
+                        LoStop2 = 256,
+                        HiStop2,
+                        Vel2,
+                        FMax2,
+                        FudgeFactor2,
+                        Bounce2,
+                        CFM2,
+                        StopERP2,
+                        StopCFM2,
+                        SuspensionERP2,
+                        SuspensionCFM2,
+                        LoStop3 = 512,
+                        HiStop3,
+                        Vel3,
+                        FMax3,
+                        FudgeFactor3,
+                        Bounce3,
+                        CFM3,
+                        StopERP3,
+                        StopCFM3,
+                        SuspensionERP3,
+                        SuspensionCFM3
+                }
+
+        public enum dSweepAndPruneAxis : int
+        {
+            XYZ = ((0)|(1<<2)|(2<<4)),
+            XZY = ((0)|(2<<2)|(1<<4)),
+            YXZ = ((1)|(0<<2)|(2<<4)),
+            YZX = ((1)|(2<<2)|(0<<4)),
+            ZXY = ((2)|(0<<2)|(1<<4)),
+            ZYX = ((2)|(1<<2)|(0<<4))
+        }
+
+                #endregion
+
+                #region Callbacks
+
+                [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
+                public delegate int AABBTestFn(IntPtr o1, IntPtr o2, ref AABB aabb);
+
+                [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
+                public delegate int ColliderFn(IntPtr o1, IntPtr o2, int flags, out ContactGeom contact, int skip);
+
+                [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
+                public delegate void GetAABBFn(IntPtr geom, out AABB aabb);
+
+                [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
+                public delegate ColliderFn GetColliderFnFn(int num);
+
+                [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
+                public delegate void GeomDtorFn(IntPtr o);
+
+        [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
+        public delegate dReal HeightfieldGetHeight(IntPtr p_user_data, int x, int z);
+
+        [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
+        public delegate dReal UbitTerrainGetHeight(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 = "dCreateUbitTerrain"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr CreateiUbitTerrain(IntPtr space, IntPtr data, int bPlaceable);
+        public static IntPtr CreateUbitTerrain(IntPtr space, IntPtr data, int bPlaceable)
+        {
+            NTotalGeoms++;
+            return CreateiUbitTerrain(space, data, bPlaceable);
+        }
+
+
+
+        
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateGeom"), SuppressUnmanagedCodeSecurity]
+                public static extern IntPtr CreateiGeom(int classnum);
+        public static IntPtr CreateGeom(int classnum)
+        {
+            NTotalGeoms++;
+            return CreateiGeom(classnum);
+        }
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateGeomClass"), SuppressUnmanagedCodeSecurity]
+                public static extern int CreateGeomClass(ref GeomClass classptr);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateGeomTransform"), SuppressUnmanagedCodeSecurity]
+                public static extern IntPtr CreateGeomTransform(IntPtr space);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreatePlane"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr CreateiPlane(IntPtr space, dReal a, dReal b, dReal c, dReal d);
+        public static IntPtr CreatePlane(IntPtr space, dReal a, dReal b, dReal c, dReal d)
+        {
+            NTotalGeoms++;
+            return CreateiPlane(space, a, b, c, d);
+        }
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateRay"), SuppressUnmanagedCodeSecurity]
+                public static extern IntPtr CreateiRay(IntPtr space, dReal length);
+        public static IntPtr CreateRay(IntPtr space, dReal length)
+        {
+            NTotalGeoms++;
+            return CreateiRay(space, length);
+        }
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateSphere"), SuppressUnmanagedCodeSecurity]
+                public static extern IntPtr CreateiSphere(IntPtr space, dReal radius);
+        public static IntPtr CreateSphere(IntPtr space, dReal radius)
+        {
+            NTotalGeoms++;
+            return CreateiSphere(space, radius);
+        }
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateTriMesh"), SuppressUnmanagedCodeSecurity]
+                public static extern IntPtr CreateiTriMesh(IntPtr space, IntPtr data, 
+                        TriCallback callback, TriArrayCallback arrayCallback, TriRayCallback rayCallback);
+        public static IntPtr CreateTriMesh(IntPtr space, IntPtr data,
+            TriCallback callback, TriArrayCallback arrayCallback, TriRayCallback rayCallback)
+        {
+            NTotalGeoms++;
+            return CreateiTriMesh(space, data, callback, arrayCallback, rayCallback);
+        }
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dDot"), SuppressUnmanagedCodeSecurity]
+                public static extern dReal Dot(ref dReal X0, ref dReal X1, int n);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dDQfromW"), SuppressUnmanagedCodeSecurity]
+                public static extern void DQfromW(dReal[] dq, ref Vector3 w, ref Quaternion q);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dFactorCholesky"), SuppressUnmanagedCodeSecurity]
+                public static extern int FactorCholesky(ref dReal A00, int n);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dFactorLDLT"), SuppressUnmanagedCodeSecurity]
+                public static extern void FactorLDLT(ref dReal A, out dReal d, int n, int nskip);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomBoxGetLengths"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomBoxGetLengths(IntPtr geom, out Vector3 len);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomBoxGetLengths"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomBoxGetLengths(IntPtr geom, out dReal x);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomBoxPointDepth"), SuppressUnmanagedCodeSecurity]
+                public static extern dReal GeomBoxPointDepth(IntPtr geom, dReal x, dReal y, dReal z);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomBoxSetLengths"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomBoxSetLengths(IntPtr geom, dReal x, dReal y, dReal z);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCapsuleGetParams"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomCapsuleGetParams(IntPtr geom, out dReal radius, out dReal length);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCapsulePointDepth"), SuppressUnmanagedCodeSecurity]
+                public static extern dReal GeomCapsulePointDepth(IntPtr geom, dReal x, dReal y, dReal z);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCapsuleSetParams"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomCapsuleSetParams(IntPtr geom, dReal radius, dReal length);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomClearOffset"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomClearOffset(IntPtr geom);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyOffsetPosition"), SuppressUnmanagedCodeSecurity]
+                public static extern IntPtr GeomCopyOffsetPosition(IntPtr geom, ref Vector3 pos);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyOffsetPosition"), SuppressUnmanagedCodeSecurity]
+                public static extern IntPtr GeomCopyOffsetPosition(IntPtr geom, ref dReal X);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetOffsetQuaternion"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomCopyOffsetQuaternion(IntPtr geom, ref Quaternion Q);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetOffsetQuaternion"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomCopyOffsetQuaternion(IntPtr geom, ref dReal X);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyOffsetRotation"), SuppressUnmanagedCodeSecurity]
+                public static extern IntPtr GeomCopyOffsetRotation(IntPtr geom, ref Matrix3 R);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyOffsetRotation"), SuppressUnmanagedCodeSecurity]
+                public static extern IntPtr GeomCopyOffsetRotation(IntPtr geom, ref dReal M00);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyPosition"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomCopyPosition(IntPtr geom, out Vector3 pos);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyPosition"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomCopyPosition(IntPtr geom, out dReal X);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyRotation"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomCopyRotation(IntPtr geom, out Matrix3 R);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyRotation"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomCopyRotation(IntPtr geom, out dReal M00);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCylinderGetParams"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomCylinderGetParams(IntPtr geom, out dReal radius, out dReal length);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCylinderSetParams"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomCylinderSetParams(IntPtr geom, dReal radius, dReal length);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomDestroy"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomiDestroy(IntPtr geom);
+        public static void GeomDestroy(IntPtr geom)
+        {
+            NTotalGeoms--;
+            GeomiDestroy(geom);
+        }
+
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomDisable"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomDisable(IntPtr geom);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomEnable"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomEnable(IntPtr geom);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetAABB"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomGetAABB(IntPtr geom, out AABB aabb);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetAABB"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomGetAABB(IntPtr geom, out dReal minX);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetBody"), SuppressUnmanagedCodeSecurity]
+                public static extern IntPtr GeomGetBody(IntPtr geom);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetCategoryBits"), SuppressUnmanagedCodeSecurity]
+                public static extern uint GeomGetCategoryBits(IntPtr geom);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetClassData"), SuppressUnmanagedCodeSecurity]
+                public static extern IntPtr GeomGetClassData(IntPtr geom);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetCollideBits"), SuppressUnmanagedCodeSecurity]
+                public static extern uint GeomGetCollideBits(IntPtr geom);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetClass"), SuppressUnmanagedCodeSecurity]
+                public static extern GeomClassID GeomGetClass(IntPtr geom);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetData"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr GeomGetData(IntPtr geom);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetOffsetPosition"), SuppressUnmanagedCodeSecurity]
+                public extern unsafe static Vector3* GeomGetOffsetPositionUnsafe(IntPtr geom);
+                public static Vector3 GeomGetOffsetPosition(IntPtr geom)
+                {
+                        unsafe { return *(GeomGetOffsetPositionUnsafe(geom)); }
+                }
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetOffsetRotation"), SuppressUnmanagedCodeSecurity]
+                public extern unsafe static Matrix3* GeomGetOffsetRotationUnsafe(IntPtr geom);
+                public static Matrix3 GeomGetOffsetRotation(IntPtr geom)
+                {
+                        unsafe { return *(GeomGetOffsetRotationUnsafe(geom)); }
+                }
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetPosition"), SuppressUnmanagedCodeSecurity]
+                public extern unsafe static Vector3* GeomGetPositionUnsafe(IntPtr geom);
+                public static Vector3 GeomGetPosition(IntPtr geom)
+                {
+                        unsafe { return *(GeomGetPositionUnsafe(geom)); }
+                }
+                public static OMV.Vector3 GeomGetPositionOMV(IntPtr geom)
+                {
+            Vector3 vtmp = GeomGetPosition(geom);
+            return new OMV.Vector3(vtmp.X, vtmp.Y, vtmp.Z);
+                }
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetQuaternion"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomCopyQuaternion(IntPtr geom, out Quaternion q);
+        public static OMV.Quaternion GeomGetQuaternionOMV(IntPtr geom)
+        {
+            Quaternion qtmp;
+            GeomCopyQuaternion(geom, out qtmp);
+            return new OMV.Quaternion(qtmp.X, qtmp.Y, qtmp.Z, qtmp.W);
+        }
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetQuaternion"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomCopyQuaternion(IntPtr geom, out dReal X);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetRotation"), SuppressUnmanagedCodeSecurity]
+                public extern unsafe static Matrix3* GeomGetRotationUnsafe(IntPtr geom);
+                public static Matrix3 GeomGetRotation(IntPtr geom)
+                {
+                        unsafe { return *(GeomGetRotationUnsafe(geom)); }
+                }
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetSpace"), SuppressUnmanagedCodeSecurity]
+                public static extern IntPtr GeomGetSpace(IntPtr geom);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildByte"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomHeightfieldDataBuildByte(IntPtr d, byte[] pHeightData, int bCopyHeightData,
+                                dReal width, dReal depth, int widthSamples, int depthSamples,
+                                dReal scale, dReal offset, dReal thickness, int bWrap);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildByte"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomHeightfieldDataBuildByte(IntPtr d, IntPtr pHeightData, int bCopyHeightData,
+                                dReal width, dReal depth, int widthSamples, int depthSamples,
+                                dReal scale, dReal offset, dReal thickness,     int bWrap);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildCallback"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomHeightfieldDataBuildCallback(IntPtr d, IntPtr pUserData, HeightfieldGetHeight pCallback,
+                                dReal width, dReal depth, int widthSamples, int depthSamples,
+                                dReal scale, dReal offset, dReal thickness, int bWrap);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildShort"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomHeightfieldDataBuildShort(IntPtr d, ushort[] pHeightData, int bCopyHeightData,
+                                dReal width, dReal depth, int widthSamples, int depthSamples,
+                                dReal scale, dReal offset, dReal thickness, int bWrap);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildShort"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomHeightfieldDataBuildShort(IntPtr d, short[] pHeightData, int bCopyHeightData,
+                                dReal width, dReal depth, int widthSamples, int depthSamples,
+                                dReal scale, dReal offset, dReal thickness, int bWrap);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildShort"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomHeightfieldDataBuildShort(IntPtr d, IntPtr pHeightData, int bCopyHeightData,
+                                dReal width, dReal depth, int widthSamples, int depthSamples,
+                                dReal scale, dReal offset, dReal thickness, int bWrap);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildSingle"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomHeightfieldDataBuildSingle(IntPtr d, float[] pHeightData, int bCopyHeightData,
+                                dReal width, dReal depth, int widthSamples, int depthSamples,
+                                dReal scale, dReal offset, dReal thickness, int bWrap);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildSingle"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomHeightfieldDataBuildSingle(IntPtr d, IntPtr pHeightData, int bCopyHeightData,
+                                dReal width, dReal depth, int widthSamples, int depthSamples,
+                                dReal scale, dReal offset, dReal thickness, int bWrap);
+
+
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildDouble"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomHeightfieldDataBuildDouble(IntPtr d, double[] pHeightData, int bCopyHeightData,
+                                dReal width, dReal depth, int widthSamples, int depthSamples,
+                                dReal scale, dReal offset, dReal thickness, int bWrap);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildDouble"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomHeightfieldDataBuildDouble(IntPtr d, IntPtr pHeightData, int bCopyHeightData,
+                                dReal width, dReal depth, int widthSamples, int depthSamples,
+                                dReal scale, dReal offset, dReal thickness, int bWrap);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataCreate"), SuppressUnmanagedCodeSecurity]
+                public static extern IntPtr GeomHeightfieldDataCreate();
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataDestroy"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomHeightfieldDataDestroy(IntPtr d);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataSetBounds"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomHeightfieldDataSetBounds(IntPtr d, dReal minHeight, dReal maxHeight);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldGetHeightfieldData"), SuppressUnmanagedCodeSecurity]
+                public static extern IntPtr GeomHeightfieldGetHeightfieldData(IntPtr g);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldSetHeightfieldData"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomHeightfieldSetHeightfieldData(IntPtr g, IntPtr d);
+
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomUbitTerrainDataBuild"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomUbitTerrainDataBuild(IntPtr d, float[] pHeightData, int bCopyHeightData,
+                dReal sampleSize, int widthSamples, int depthSamples,
+                dReal offset, dReal thickness, int bWrap);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomUbitTerrainDataBuild"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomUbitTerrainDataBuild(IntPtr d, IntPtr pHeightData, int bCopyHeightData,
+                dReal sampleSize, int widthSamples, int depthSamples,
+                dReal thickness, int bWrap);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomUbitTerrainDataCreate"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr GeomUbitTerrainDataCreate();
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomUbitTerrainDataDestroy"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomUbitTerrainDataDestroy(IntPtr d);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomUbitTerrainDataSetBounds"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomUbitTerrainDataSetBounds(IntPtr d, dReal minHeight, dReal maxHeight);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomUbitTerrainGetHeightfieldData"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr GeomUbitTerrainGetHeightfieldData(IntPtr g);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomUbitTerrainSetHeightfieldData"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomUbitTerrainSetHeightfieldData(IntPtr g, IntPtr d);
+
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomIsEnabled"), SuppressUnmanagedCodeSecurity]
+                public static extern bool GeomIsEnabled(IntPtr geom);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomIsOffset"), SuppressUnmanagedCodeSecurity]
+                public static extern bool GeomIsOffset(IntPtr geom);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomIsSpace"), SuppressUnmanagedCodeSecurity]
+                public static extern bool GeomIsSpace(IntPtr geom);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomPlaneGetParams"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomPlaneGetParams(IntPtr geom, ref Vector4 result);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomPlaneGetParams"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomPlaneGetParams(IntPtr geom, ref dReal A);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomPlanePointDepth"), SuppressUnmanagedCodeSecurity]
+                public static extern dReal GeomPlanePointDepth(IntPtr geom, dReal x, dReal y, dReal z);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomPlaneSetParams"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomPlaneSetParams(IntPtr plane, dReal a, dReal b, dReal c, dReal d);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGet"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomRayGet(IntPtr ray, ref Vector3 start, ref Vector3 dir);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGet"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomRayGet(IntPtr ray, ref dReal startX, ref dReal dirX);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGetClosestHit"), SuppressUnmanagedCodeSecurity]
+                public static extern int GeomRayGetClosestHit(IntPtr ray);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGetLength"), SuppressUnmanagedCodeSecurity]
+                public static extern dReal GeomRayGetLength(IntPtr ray);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGetParams"), SuppressUnmanagedCodeSecurity]
+                public static extern dReal GeomRayGetParams(IntPtr g, out int firstContact, out int backfaceCull);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRaySet"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomRaySet(IntPtr ray, dReal px, dReal py, dReal pz, dReal dx, dReal dy, dReal dz);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRaySetClosestHit"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomRaySetClosestHit(IntPtr ray, int closestHit);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRaySetLength"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomRaySetLength(IntPtr ray, dReal length);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRaySetParams"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomRaySetParams(IntPtr ray, int firstContact, int backfaceCull);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetBody"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomSetBody(IntPtr geom, IntPtr body);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetCategoryBits"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomSetCategoryBits(IntPtr geom, uint bits);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetCollideBits"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomSetCollideBits(IntPtr geom, uint bits);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetConvex"), SuppressUnmanagedCodeSecurity]
+                public static extern IntPtr GeomSetConvex(IntPtr geom, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetData"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomSetData(IntPtr geom, IntPtr data);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetPosition"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomSetOffsetPosition(IntPtr geom, dReal x, dReal y, dReal z);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetQuaternion"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomSetOffsetQuaternion(IntPtr geom, ref Quaternion Q);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetQuaternion"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomSetOffsetQuaternion(IntPtr geom, ref dReal X);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetRotation"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomSetOffsetRotation(IntPtr geom, ref Matrix3 R);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetRotation"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomSetOffsetRotation(IntPtr geom, ref dReal M00);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldPosition"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomSetOffsetWorldPosition(IntPtr geom, dReal x, dReal y, dReal z);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldQuaternion"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomSetOffsetWorldQuaternion(IntPtr geom, ref Quaternion Q);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldQuaternion"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomSetOffsetWorldQuaternion(IntPtr geom, ref dReal X);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldRotation"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomSetOffsetWorldRotation(IntPtr geom, ref Matrix3 R);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldRotation"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomSetOffsetWorldRotation(IntPtr geom, ref dReal M00);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetPosition"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomSetPosition(IntPtr geom, dReal x, dReal y, dReal z);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetQuaternion"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomSetQuaternion(IntPtr geom, ref Quaternion quat);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetQuaternion"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomSetQuaternion(IntPtr geom, ref dReal w);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetRotation"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomSetRotation(IntPtr geom, ref Matrix3 R);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetRotation"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomSetRotation(IntPtr geom, ref dReal M00);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSphereGetRadius"), SuppressUnmanagedCodeSecurity]
+                public static extern dReal GeomSphereGetRadius(IntPtr geom);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSpherePointDepth"), SuppressUnmanagedCodeSecurity]
+                public static extern dReal GeomSpherePointDepth(IntPtr geom, dReal x, dReal y, dReal z);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSphereSetRadius"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomSphereSetRadius(IntPtr geom, dReal radius);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformGetCleanup"), SuppressUnmanagedCodeSecurity]
+                public static extern int GeomTransformGetCleanup(IntPtr geom);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformGetGeom"), SuppressUnmanagedCodeSecurity]
+                public static extern IntPtr GeomTransformGetGeom(IntPtr geom);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformGetInfo"), SuppressUnmanagedCodeSecurity]
+                public static extern int GeomTransformGetInfo(IntPtr geom);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformSetCleanup"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomTransformSetCleanup(IntPtr geom, int mode);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformSetGeom"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomTransformSetGeom(IntPtr geom, IntPtr obj);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformSetInfo"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomTransformSetInfo(IntPtr geom, int info);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildDouble"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomTriMeshDataBuildDouble(IntPtr d,
+                        double[] vertices, int vertexStride, int vertexCount,
+                        int[] indices, int indexCount, int triStride);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildDouble"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomTriMeshDataBuildDouble(IntPtr d,
+                        IntPtr vertices, int vertexStride, int vertexCount,
+                        IntPtr indices, int indexCount, int triStride);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildDouble1"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomTriMeshDataBuildDouble1(IntPtr d,
+                        double[] vertices, int vertexStride, int vertexCount,
+                        int[] indices, int indexCount, int triStride,
+                        double[] normals);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildDouble1"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomTriMeshDataBuildDouble(IntPtr d,
+                        IntPtr vertices, int vertexStride, int vertexCount,
+                        IntPtr indices, int indexCount, int triStride,
+                        IntPtr normals);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSimple"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomTriMeshDataBuildSingle(IntPtr d,
+                        dReal[] vertices, int vertexStride, int vertexCount,
+                        int[] indices, int indexCount, int triStride);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSimple"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomTriMeshDataBuildSingle(IntPtr d,
+                        IntPtr vertices, int vertexStride, int vertexCount,
+                        IntPtr indices, int indexCount, int triStride);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSimple1"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomTriMeshDataBuildSingle1(IntPtr d,
+                        dReal[] vertices, int vertexStride, int vertexCount,
+                        int[] indices, int indexCount, int triStride,
+                        dReal[] normals);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSimple1"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomTriMeshDataBuildSingle1(IntPtr d,
+                        IntPtr vertices, int vertexStride, int vertexCount,
+                        IntPtr indices, int indexCount, int triStride,
+                        IntPtr normals);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSingle"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomTriMeshDataBuildSimple(IntPtr d, 
+                        float[] vertices, int vertexStride, int vertexCount,
+                        int[] indices, int indexCount, int triStride);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSingle"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomTriMeshDataBuildSimple(IntPtr d,
+                        IntPtr vertices, int vertexStride, int vertexCount,
+                        IntPtr indices, int indexCount, int triStride);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSingle1"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomTriMeshDataBuildSimple1(IntPtr d, 
+                        float[] vertices, int vertexStride, int vertexCount,
+                        int[] indices, int indexCount, int triStride,
+                        float[] normals);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSingle1"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomTriMeshDataBuildSimple1(IntPtr d,
+                        IntPtr vertices, int vertexStride, int vertexCount,
+                        IntPtr indices, int indexCount, int triStride,
+                        IntPtr normals);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshClearTCCache"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomTriMeshClearTCCache(IntPtr g);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataCreate"), SuppressUnmanagedCodeSecurity]
+                public static extern IntPtr GeomTriMeshDataCreate();
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataDestroy"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomTriMeshDataDestroy(IntPtr d);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataGet"), SuppressUnmanagedCodeSecurity]
+                public static extern IntPtr GeomTriMeshDataGet(IntPtr d, int data_id);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataPreprocess"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomTriMeshDataPreprocess(IntPtr d);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataSet"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomTriMeshDataSet(IntPtr d, int data_id, IntPtr in_data);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataUpdate"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomTriMeshDataUpdate(IntPtr d);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshEnableTC"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomTriMeshEnableTC(IntPtr g, int geomClass, bool enable);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetArrayCallback"), SuppressUnmanagedCodeSecurity]
+                public static extern TriArrayCallback GeomTriMeshGetArrayCallback(IntPtr g);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetCallback"), SuppressUnmanagedCodeSecurity]
+                public static extern TriCallback GeomTriMeshGetCallback(IntPtr g);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetData"), SuppressUnmanagedCodeSecurity]
+                public static extern IntPtr GeomTriMeshGetData(IntPtr g);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetLastTransform"), SuppressUnmanagedCodeSecurity]
+                public extern unsafe static Matrix4* GeomTriMeshGetLastTransformUnsafe(IntPtr geom);
+                public static Matrix4 GeomTriMeshGetLastTransform(IntPtr geom)
+                {
+                        unsafe { return *(GeomTriMeshGetLastTransformUnsafe(geom)); }
+                }
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetPoint"), SuppressUnmanagedCodeSecurity]
+                public extern static void GeomTriMeshGetPoint(IntPtr g, int index, dReal u, dReal v, ref Vector3 outVec);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetRayCallback"), SuppressUnmanagedCodeSecurity]
+                public static extern TriRayCallback GeomTriMeshGetRayCallback(IntPtr g);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetTriangle"), SuppressUnmanagedCodeSecurity]
+                public extern static void GeomTriMeshGetTriangle(IntPtr g, int index, ref Vector3 v0, ref Vector3 v1, ref Vector3 v2);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetTriangleCount"), SuppressUnmanagedCodeSecurity]
+                public extern static int GeomTriMeshGetTriangleCount(IntPtr g);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetTriMeshDataID"), SuppressUnmanagedCodeSecurity]
+                public static extern IntPtr GeomTriMeshGetTriMeshDataID(IntPtr g);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshIsTCEnabled"), SuppressUnmanagedCodeSecurity]
+                public static extern bool GeomTriMeshIsTCEnabled(IntPtr g, int geomClass);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetArrayCallback"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomTriMeshSetArrayCallback(IntPtr g, TriArrayCallback arrayCallback);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetCallback"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomTriMeshSetCallback(IntPtr g, TriCallback callback);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetData"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomTriMeshSetData(IntPtr g, IntPtr data);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetLastTransform"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomTriMeshSetLastTransform(IntPtr g, ref Matrix4 last_trans);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetLastTransform"), SuppressUnmanagedCodeSecurity]
+                public static extern void GeomTriMeshSetLastTransform(IntPtr g, ref dReal M00);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetRayCallback"), SuppressUnmanagedCodeSecurity]
+        public static extern void GeomTriMeshSetRayCallback(IntPtr g, TriRayCallback callback);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGetConfiguration"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr iGetConfiguration();
+
+        public static string GetConfiguration()
+        {
+            IntPtr ptr = iGetConfiguration();
+            string s = Marshal.PtrToStringAnsi(ptr);
+            return s;
+        }
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dHashSpaceCreate"), SuppressUnmanagedCodeSecurity]
+                public static extern IntPtr HashSpaceCreate(IntPtr space);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dHashSpaceGetLevels"), SuppressUnmanagedCodeSecurity]
+                public static extern void HashSpaceGetLevels(IntPtr space, out int minlevel, out int maxlevel);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dHashSpaceSetLevels"), SuppressUnmanagedCodeSecurity]
+                public static extern void HashSpaceSetLevels(IntPtr space, int minlevel, int maxlevel);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dInfiniteAABB"), SuppressUnmanagedCodeSecurity]
+                public static extern void InfiniteAABB(IntPtr geom, out AABB aabb);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dInitODE"), SuppressUnmanagedCodeSecurity]
+                public static extern void InitODE();
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dInitODE2"), SuppressUnmanagedCodeSecurity]
+        public static extern int InitODE2(uint ODEInitFlags);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dIsPositiveDefinite"), SuppressUnmanagedCodeSecurity]
+                public static extern int IsPositiveDefinite(ref dReal A, int n);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dInvertPDMatrix"), SuppressUnmanagedCodeSecurity]
+                public static extern int InvertPDMatrix(ref dReal A, out dReal Ainv, int n);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddAMotorTorques"), SuppressUnmanagedCodeSecurity]
+                public static extern void JointAddAMotorTorques(IntPtr joint, dReal torque1, dReal torque2, dReal torque3);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddHingeTorque"), SuppressUnmanagedCodeSecurity]
+                public static extern void JointAddHingeTorque(IntPtr joint, dReal torque);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddHinge2Torque"), SuppressUnmanagedCodeSecurity]
+                public static extern void JointAddHinge2Torques(IntPtr joint, dReal torque1, dReal torque2);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddPRTorque"), SuppressUnmanagedCodeSecurity]
+                public static extern void JointAddPRTorque(IntPtr joint, dReal torque);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddUniversalTorque"), SuppressUnmanagedCodeSecurity]
+                public static extern void JointAddUniversalTorques(IntPtr joint, dReal torque1, dReal torque2);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddSliderForce"), SuppressUnmanagedCodeSecurity]
+                public static extern void JointAddSliderForce(IntPtr joint, dReal force);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAttach"), SuppressUnmanagedCodeSecurity]
+                public static extern void JointAttach(IntPtr joint, IntPtr body1, IntPtr body2);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateAMotor"), SuppressUnmanagedCodeSecurity]
+                public static extern IntPtr JointCreateAMotor(IntPtr world, IntPtr group);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateBall"), SuppressUnmanagedCodeSecurity]
+                public static extern IntPtr JointCreateBall(IntPtr world, IntPtr group);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateContact"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr JointCreateContact(IntPtr world, IntPtr group, ref Contact contact);
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateContact"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr JointCreateContactPtr(IntPtr world, IntPtr group, IntPtr contact);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateFixed"), SuppressUnmanagedCodeSecurity]
+                public static extern IntPtr JointCreateFixed(IntPtr world, IntPtr group);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateHinge"), SuppressUnmanagedCodeSecurity]
+                public static extern IntPtr JointCreateHinge(IntPtr world, IntPtr group);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateHinge2"), SuppressUnmanagedCodeSecurity]
+                public static extern IntPtr JointCreateHinge2(IntPtr world, IntPtr group);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateLMotor"), SuppressUnmanagedCodeSecurity]
+                public static extern IntPtr JointCreateLMotor(IntPtr world, IntPtr group);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateNull"), SuppressUnmanagedCodeSecurity]
+                public static extern IntPtr JointCreateNull(IntPtr world, IntPtr group);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreatePR"), SuppressUnmanagedCodeSecurity]
+                public static extern IntPtr JointCreatePR(IntPtr world, IntPtr group);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreatePlane2D"), SuppressUnmanagedCodeSecurity]
+                public static extern IntPtr JointCreatePlane2D(IntPtr world, IntPtr group);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateSlider"), SuppressUnmanagedCodeSecurity]
+                public static extern IntPtr JointCreateSlider(IntPtr world, IntPtr group);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateUniversal"), SuppressUnmanagedCodeSecurity]
+                public static extern IntPtr JointCreateUniversal(IntPtr world, IntPtr group);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointDestroy"), SuppressUnmanagedCodeSecurity]
+                public static extern void JointDestroy(IntPtr j);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorAngle"), SuppressUnmanagedCodeSecurity]
+                public static extern dReal JointGetAMotorAngle(IntPtr j, int anum);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorAngleRate"), SuppressUnmanagedCodeSecurity]
+                public static extern dReal JointGetAMotorAngleRate(IntPtr j, int anum);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorAxis"), SuppressUnmanagedCodeSecurity]
+                public static extern void JointGetAMotorAxis(IntPtr j, int anum, out Vector3 result);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorAxisRel"), SuppressUnmanagedCodeSecurity]
+                public static extern int JointGetAMotorAxisRel(IntPtr j, int anum);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorMode"), SuppressUnmanagedCodeSecurity]
+                public static extern int JointGetAMotorMode(IntPtr j);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorNumAxes"), SuppressUnmanagedCodeSecurity]
+                public static extern int JointGetAMotorNumAxes(IntPtr j);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorParam"), SuppressUnmanagedCodeSecurity]
+                public static extern dReal JointGetAMotorParam(IntPtr j, int parameter);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetBallAnchor"), SuppressUnmanagedCodeSecurity]
+                public static extern void JointGetBallAnchor(IntPtr j, out Vector3 result);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetBallAnchor2"), SuppressUnmanagedCodeSecurity]
+                public static extern void JointGetBallAnchor2(IntPtr j, out Vector3 result);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetBody"), SuppressUnmanagedCodeSecurity]
+                public static extern IntPtr JointGetBody(IntPtr j);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetData"), SuppressUnmanagedCodeSecurity]
+                public static extern IntPtr JointGetData(IntPtr j);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetFeedback"), SuppressUnmanagedCodeSecurity]
+                public extern unsafe static JointFeedback* JointGetFeedbackUnsafe(IntPtr j);
+                public static JointFeedback JointGetFeedback(IntPtr j)
+                {
+                        unsafe { return *(JointGetFeedbackUnsafe(j)); }
+                }
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAnchor"), SuppressUnmanagedCodeSecurity]
+                public static extern void JointGetHingeAnchor(IntPtr j, out Vector3 result);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAngle"), SuppressUnmanagedCodeSecurity]
+                public static extern dReal JointGetHingeAngle(IntPtr j);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAngleRate"), SuppressUnmanagedCodeSecurity]
+                public static extern dReal JointGetHingeAngleRate(IntPtr j);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAxis"), SuppressUnmanagedCodeSecurity]
+                public static extern void JointGetHingeAxis(IntPtr j, out Vector3 result);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeParam"), SuppressUnmanagedCodeSecurity]
+                public static extern dReal JointGetHingeParam(IntPtr j, int parameter);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Angle1"), SuppressUnmanagedCodeSecurity]
+                public static extern dReal JointGetHinge2Angle1(IntPtr j);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Angle1Rate"), SuppressUnmanagedCodeSecurity]
+                public static extern dReal JointGetHinge2Angle1Rate(IntPtr j);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Angle2Rate"), SuppressUnmanagedCodeSecurity]
+                public static extern dReal JointGetHinge2Angle2Rate(IntPtr j);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAnchor2"), SuppressUnmanagedCodeSecurity]
+                public static extern void JointGetHingeAnchor2(IntPtr j, out Vector3 result);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Anchor"), SuppressUnmanagedCodeSecurity]
+                public static extern void JointGetHinge2Anchor(IntPtr j, out Vector3 result);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Anchor2"), SuppressUnmanagedCodeSecurity]
+                public static extern void JointGetHinge2Anchor2(IntPtr j, out Vector3 result);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Axis1"), SuppressUnmanagedCodeSecurity]
+                public static extern void JointGetHinge2Axis1(IntPtr j, out Vector3 result);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Axis2"), SuppressUnmanagedCodeSecurity]
+                public static extern void JointGetHinge2Axis2(IntPtr j, out Vector3 result);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Param"), SuppressUnmanagedCodeSecurity]
+                public static extern dReal JointGetHinge2Param(IntPtr j, int parameter);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetLMotorAxis"), SuppressUnmanagedCodeSecurity]
+                public static extern void JointGetLMotorAxis(IntPtr j, int anum, out Vector3 result);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetLMotorNumAxes"), SuppressUnmanagedCodeSecurity]
+                public static extern int JointGetLMotorNumAxes(IntPtr j);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetLMotorParam"), SuppressUnmanagedCodeSecurity]
+                public static extern dReal JointGetLMotorParam(IntPtr j, int parameter);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRAnchor"), SuppressUnmanagedCodeSecurity]
+                public static extern void JointGetPRAnchor(IntPtr j, out Vector3 result);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRAxis1"), SuppressUnmanagedCodeSecurity]
+                public static extern void JointGetPRAxis1(IntPtr j, out Vector3 result);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRAxis2"), SuppressUnmanagedCodeSecurity]
+                public static extern void JointGetPRAxis2(IntPtr j, out Vector3 result);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRParam"), SuppressUnmanagedCodeSecurity]
+                public static extern dReal JointGetPRParam(IntPtr j, int parameter);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRPosition"), SuppressUnmanagedCodeSecurity]
+                public static extern dReal JointGetPRPosition(IntPtr j);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRPositionRate"), SuppressUnmanagedCodeSecurity]
+                public static extern dReal JointGetPRPositionRate(IntPtr j);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetSliderAxis"), SuppressUnmanagedCodeSecurity]
+                public static extern void JointGetSliderAxis(IntPtr j, out Vector3 result);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetSliderParam"), SuppressUnmanagedCodeSecurity]
+                public static extern dReal JointGetSliderParam(IntPtr j, int parameter);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetSliderPosition"), SuppressUnmanagedCodeSecurity]
+                public static extern dReal JointGetSliderPosition(IntPtr j);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetSliderPositionRate"), SuppressUnmanagedCodeSecurity]
+                public static extern dReal JointGetSliderPositionRate(IntPtr j);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetType"), SuppressUnmanagedCodeSecurity]
+                public static extern JointType JointGetType(IntPtr j);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAnchor"), SuppressUnmanagedCodeSecurity]
+                public static extern void JointGetUniversalAnchor(IntPtr j, out Vector3 result);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAnchor2"), SuppressUnmanagedCodeSecurity]
+                public static extern void JointGetUniversalAnchor2(IntPtr j, out Vector3 result);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngle1"), SuppressUnmanagedCodeSecurity]
+                public static extern dReal JointGetUniversalAngle1(IntPtr j);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngle1Rate"), SuppressUnmanagedCodeSecurity]
+                public static extern dReal JointGetUniversalAngle1Rate(IntPtr j);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngle2"), SuppressUnmanagedCodeSecurity]
+                public static extern dReal JointGetUniversalAngle2(IntPtr j);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngle2Rate"), SuppressUnmanagedCodeSecurity]
+                public static extern dReal JointGetUniversalAngle2Rate(IntPtr j);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngles"), SuppressUnmanagedCodeSecurity]
+                public static extern void JointGetUniversalAngles(IntPtr j, out dReal angle1, out dReal angle2);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAxis1"), SuppressUnmanagedCodeSecurity]
+                public static extern void JointGetUniversalAxis1(IntPtr j, out Vector3 result);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAxis2"), SuppressUnmanagedCodeSecurity]
+                public static extern void JointGetUniversalAxis2(IntPtr j, out Vector3 result);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalParam"), SuppressUnmanagedCodeSecurity]
+                public static extern dReal JointGetUniversalParam(IntPtr j, int parameter);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGroupCreate"), SuppressUnmanagedCodeSecurity]
+                public static extern IntPtr JointGroupCreate(int max_size);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGroupDestroy"), SuppressUnmanagedCodeSecurity]
+                public static extern void JointGroupDestroy(IntPtr group);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGroupEmpty"), SuppressUnmanagedCodeSecurity]
+                public static extern void JointGroupEmpty(IntPtr group);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorAngle"), SuppressUnmanagedCodeSecurity]
+                public static extern void JointSetAMotorAngle(IntPtr j, int anum, dReal angle);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorAxis"), SuppressUnmanagedCodeSecurity]
+                public static extern void JointSetAMotorAxis(IntPtr j, int anum, int rel, dReal x, dReal y, dReal z);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorMode"), SuppressUnmanagedCodeSecurity]
+                public static extern void JointSetAMotorMode(IntPtr j, int mode);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorNumAxes"), SuppressUnmanagedCodeSecurity]
+                public static extern void JointSetAMotorNumAxes(IntPtr group, int num);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorParam"), SuppressUnmanagedCodeSecurity]
+                public static extern void JointSetAMotorParam(IntPtr group, int parameter, dReal value);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetBallAnchor"), SuppressUnmanagedCodeSecurity]
+                public static extern void JointSetBallAnchor(IntPtr j, dReal x, dReal y, dReal z);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetBallAnchor2"), SuppressUnmanagedCodeSecurity]
+                public static extern void JointSetBallAnchor2(IntPtr j, dReal x, dReal y, dReal z);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetData"), SuppressUnmanagedCodeSecurity]
+                public static extern void JointSetData(IntPtr j, IntPtr data);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetFeedback"), SuppressUnmanagedCodeSecurity]
+                public static extern void JointSetFeedback(IntPtr j, out JointFeedback feedback);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetFixed"), SuppressUnmanagedCodeSecurity]
+                public static extern void JointSetFixed(IntPtr j);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHingeAnchor"), SuppressUnmanagedCodeSecurity]
+                public static extern void JointSetHingeAnchor(IntPtr j, dReal x, dReal y, dReal z);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHingeAnchorDelta"), SuppressUnmanagedCodeSecurity]
+                public static extern void JointSetHingeAnchorDelta(IntPtr j, dReal x, dReal y, dReal z, dReal ax, dReal ay, dReal az);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHingeAxis"), SuppressUnmanagedCodeSecurity]
+                public static extern void JointSetHingeAxis(IntPtr j, dReal x, dReal y, dReal z);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHingeParam"), SuppressUnmanagedCodeSecurity]
+                public static extern void JointSetHingeParam(IntPtr j, int parameter, dReal value);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHinge2Anchor"), SuppressUnmanagedCodeSecurity]
+                public static extern void JointSetHinge2Anchor(IntPtr j, dReal x, dReal y, dReal z);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHinge2Axis1"), SuppressUnmanagedCodeSecurity]
+                public static extern void JointSetHinge2Axis1(IntPtr j, dReal x, dReal y, dReal z);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHinge2Axis2"), SuppressUnmanagedCodeSecurity]
+                public static extern void JointSetHinge2Axis2(IntPtr j, dReal x, dReal y, dReal z);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHinge2Param"), SuppressUnmanagedCodeSecurity]
+                public static extern void JointSetHinge2Param(IntPtr j, int parameter, dReal value);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetLMotorAxis"), SuppressUnmanagedCodeSecurity]
+                public static extern void JointSetLMotorAxis(IntPtr j, int anum, int rel, dReal x, dReal y, dReal z);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetLMotorNumAxes"), SuppressUnmanagedCodeSecurity]
+                public static extern void JointSetLMotorNumAxes(IntPtr j, int num);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetLMotorParam"), SuppressUnmanagedCodeSecurity]
+                public static extern void JointSetLMotorParam(IntPtr j, int parameter, dReal value);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPlane2DAngleParam"), SuppressUnmanagedCodeSecurity]
+                public static extern void JointSetPlane2DAngleParam(IntPtr j, int parameter, dReal value);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPlane2DXParam"), SuppressUnmanagedCodeSecurity]
+                public static extern void JointSetPlane2DXParam(IntPtr j, int parameter, dReal value);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPlane2DYParam"), SuppressUnmanagedCodeSecurity]
+                public static extern void JointSetPlane2DYParam(IntPtr j, int parameter, dReal value);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPRAnchor"), SuppressUnmanagedCodeSecurity]
+                public static extern void JointSetPRAnchor(IntPtr j, dReal x, dReal y, dReal z);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPRAxis1"), SuppressUnmanagedCodeSecurity]
+                public static extern void JointSetPRAxis1(IntPtr j, dReal x, dReal y, dReal z);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPRAxis2"), SuppressUnmanagedCodeSecurity]
+                public static extern void JointSetPRAxis2(IntPtr j, dReal x, dReal y, dReal z);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPRParam"), SuppressUnmanagedCodeSecurity]
+                public static extern void JointSetPRParam(IntPtr j, int parameter, dReal value);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetSliderAxis"), SuppressUnmanagedCodeSecurity]
+                public static extern void JointSetSliderAxis(IntPtr j, dReal x, dReal y, dReal z);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetSliderAxisDelta"), SuppressUnmanagedCodeSecurity]
+                public static extern void JointSetSliderAxisDelta(IntPtr j, dReal x, dReal y, dReal z, dReal ax, dReal ay, dReal az);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetSliderParam"), SuppressUnmanagedCodeSecurity]
+                public static extern void JointSetSliderParam(IntPtr j, int parameter, dReal value);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetUniversalAnchor"), SuppressUnmanagedCodeSecurity]
+                public static extern void JointSetUniversalAnchor(IntPtr j, dReal x, dReal y, dReal z);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetUniversalAxis1"), SuppressUnmanagedCodeSecurity]
+                public static extern void JointSetUniversalAxis1(IntPtr j, dReal x, dReal y, dReal z);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetUniversalAxis2"), SuppressUnmanagedCodeSecurity]
+                public static extern void JointSetUniversalAxis2(IntPtr j, dReal x, dReal y, dReal z);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetUniversalParam"), SuppressUnmanagedCodeSecurity]
+                public static extern void JointSetUniversalParam(IntPtr j, int parameter, dReal value);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dLDLTAddTL"), SuppressUnmanagedCodeSecurity]
+                public static extern void LDLTAddTL(ref dReal L, ref dReal d, ref dReal a, int n, int nskip);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassAdd"), SuppressUnmanagedCodeSecurity]
+                public static extern void MassAdd(ref Mass a, ref Mass b);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassAdjust"), SuppressUnmanagedCodeSecurity]
+                public static extern void MassAdjust(ref Mass m, dReal newmass);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassCheck"), SuppressUnmanagedCodeSecurity]
+                public static extern bool MassCheck(ref Mass m);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassRotate"), SuppressUnmanagedCodeSecurity]
+                public static extern void MassRotate(ref Mass mass, ref Matrix3 R);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassRotate"), SuppressUnmanagedCodeSecurity]
+                public static extern void MassRotate(ref Mass mass, ref dReal M00);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetBox"), SuppressUnmanagedCodeSecurity]
+                public static extern void MassSetBox(out Mass mass, dReal density, dReal lx, dReal ly, dReal lz);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetBoxTotal"), SuppressUnmanagedCodeSecurity]
+                public static extern void MassSetBoxTotal(out Mass mass, dReal total_mass, dReal lx, dReal ly, dReal lz);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetCapsule"), SuppressUnmanagedCodeSecurity]
+                public static extern void MassSetCapsule(out Mass mass, dReal density, int direction, dReal radius, dReal length);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetCapsuleTotal"), SuppressUnmanagedCodeSecurity]
+                public static extern void MassSetCapsuleTotal(out Mass mass, dReal total_mass, int direction, dReal radius, dReal length);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetCylinder"), SuppressUnmanagedCodeSecurity]
+                public static extern void MassSetCylinder(out Mass mass, dReal density, int direction, dReal radius, dReal length);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetCylinderTotal"), SuppressUnmanagedCodeSecurity]
+                public static extern void MassSetCylinderTotal(out Mass mass, dReal total_mass, int direction, dReal radius, dReal length);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetParameters"), SuppressUnmanagedCodeSecurity]
+                public static extern void MassSetParameters(out Mass mass, dReal themass,
+                         dReal cgx, dReal cgy, dReal cgz,
+                         dReal i11, dReal i22, dReal i33,
+                         dReal i12, dReal i13, dReal i23);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetSphere"), SuppressUnmanagedCodeSecurity]
+                public static extern void MassSetSphere(out Mass mass, dReal density, dReal radius);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetSphereTotal"), SuppressUnmanagedCodeSecurity]
+                public static extern void dMassSetSphereTotal(out Mass mass, dReal total_mass, dReal radius);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetTrimesh"), SuppressUnmanagedCodeSecurity]
+                public static extern void MassSetTrimesh(out Mass mass, dReal density, IntPtr g);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetZero"), SuppressUnmanagedCodeSecurity]
+                public static extern void MassSetZero(out Mass mass);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassTranslate"), SuppressUnmanagedCodeSecurity]
+                public static extern void MassTranslate(ref Mass mass, dReal x, dReal y, dReal z);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMultiply0"), SuppressUnmanagedCodeSecurity]
+        public static extern void Multiply0(out dReal A00, ref dReal B00, ref dReal C00, int p, int q, int r);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMultiply0"), SuppressUnmanagedCodeSecurity]
+        private static extern void MultiplyiM3V3(out Vector3 vout, ref Matrix3 matrix, ref Vector3 vect,int p, int q, int r);
+        public static void MultiplyM3V3(out Vector3 outvector, ref Matrix3 matrix, ref Vector3 invector)
+        {
+            MultiplyiM3V3(out outvector, ref matrix, ref invector, 3, 3, 1);
+        }
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMultiply1"), SuppressUnmanagedCodeSecurity]
+                public static extern void Multiply1(out dReal A00, ref dReal B00, ref dReal C00, int p, int q, int r);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMultiply2"), SuppressUnmanagedCodeSecurity]
+                public static extern void Multiply2(out dReal A00, ref dReal B00, ref dReal C00, int p, int q, int r);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQFromAxisAndAngle"), SuppressUnmanagedCodeSecurity]
+                public static extern void QFromAxisAndAngle(out Quaternion q, dReal ax, dReal ay, dReal az, dReal angle);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQfromR"), SuppressUnmanagedCodeSecurity]
+                public static extern void QfromR(out Quaternion q, ref Matrix3 R);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQMultiply0"), SuppressUnmanagedCodeSecurity]
+                public static extern void QMultiply0(out Quaternion qa, ref Quaternion qb, ref Quaternion qc);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQMultiply1"), SuppressUnmanagedCodeSecurity]
+                public static extern void QMultiply1(out Quaternion qa, ref Quaternion qb, ref Quaternion qc);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQMultiply2"), SuppressUnmanagedCodeSecurity]
+                public static extern void QMultiply2(out Quaternion qa, ref Quaternion qb, ref Quaternion qc);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQMultiply3"), SuppressUnmanagedCodeSecurity]
+                public static extern void QMultiply3(out Quaternion qa, ref Quaternion qb, ref Quaternion qc);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQSetIdentity"), SuppressUnmanagedCodeSecurity]
+                public static extern void QSetIdentity(out Quaternion q);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQuadTreeSpaceCreate"), SuppressUnmanagedCodeSecurity]
+                public static extern IntPtr QuadTreeSpaceCreate(IntPtr space, ref Vector3 center, ref Vector3 extents, int depth);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQuadTreeSpaceCreate"), SuppressUnmanagedCodeSecurity]
+                public static extern IntPtr QuadTreeSpaceCreate(IntPtr space, ref dReal centerX, ref dReal extentsX, int depth);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRandReal"), SuppressUnmanagedCodeSecurity]
+                public static extern dReal RandReal();
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRFrom2Axes"), SuppressUnmanagedCodeSecurity]
+                public static extern void RFrom2Axes(out Matrix3 R, dReal ax, dReal ay, dReal az, dReal bx, dReal by, dReal bz);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRFromAxisAndAngle"), SuppressUnmanagedCodeSecurity]
+                public static extern void RFromAxisAndAngle(out Matrix3 R, dReal x, dReal y, dReal z, dReal angle);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRFromEulerAngles"), SuppressUnmanagedCodeSecurity]
+                public static extern void RFromEulerAngles(out Matrix3 R, dReal phi, dReal theta, dReal psi);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRfromQ"), SuppressUnmanagedCodeSecurity]
+                public static extern void RfromQ(out Matrix3 R, ref Quaternion q);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRFromZAxis"), SuppressUnmanagedCodeSecurity]
+                public static extern void RFromZAxis(out Matrix3 R, dReal ax, dReal ay, dReal az);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRSetIdentity"), SuppressUnmanagedCodeSecurity]
+                public static extern void RSetIdentity(out Matrix3 R);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSetValue"), SuppressUnmanagedCodeSecurity]
+                public static extern void SetValue(out dReal a, int n);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSetZero"), SuppressUnmanagedCodeSecurity]
+                public static extern void SetZero(out dReal a, int n);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSimpleSpaceCreate"), SuppressUnmanagedCodeSecurity]
+                public static extern IntPtr SimpleSpaceCreate(IntPtr space);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSolveCholesky"), SuppressUnmanagedCodeSecurity]
+                public static extern void SolveCholesky(ref dReal L, out dReal b, int n);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSolveL1"), SuppressUnmanagedCodeSecurity]
+                public static extern void SolveL1(ref dReal L, out dReal b, int n, int nskip);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSolveL1T"), SuppressUnmanagedCodeSecurity]
+                public static extern void SolveL1T(ref dReal L, out dReal b, int n, int nskip);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSolveLDLT"), SuppressUnmanagedCodeSecurity]
+                public static extern void SolveLDLT(ref dReal L, ref dReal d, out dReal b, int n, int nskip);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceAdd"), SuppressUnmanagedCodeSecurity]
+                public static extern void SpaceAdd(IntPtr space, IntPtr geom);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceLockQuery"), SuppressUnmanagedCodeSecurity]
+        public static extern bool SpaceLockQuery(IntPtr space);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceClean"), SuppressUnmanagedCodeSecurity]
+                public static extern void SpaceClean(IntPtr space);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceCollide"), SuppressUnmanagedCodeSecurity]
+                public static extern void SpaceCollide(IntPtr space, IntPtr data, NearCallback callback);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceCollide2"), SuppressUnmanagedCodeSecurity]
+                public static extern void SpaceCollide2(IntPtr space1, IntPtr space2, IntPtr data, NearCallback callback);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceDestroy"), SuppressUnmanagedCodeSecurity]
+                public static extern void SpaceDestroy(IntPtr space);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceGetCleanup"), SuppressUnmanagedCodeSecurity]
+                public static extern bool SpaceGetCleanup(IntPtr space);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceGetNumGeoms"), SuppressUnmanagedCodeSecurity]
+                public static extern int SpaceGetNumGeoms(IntPtr space);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceGetGeom"), SuppressUnmanagedCodeSecurity]
+                public static extern IntPtr SpaceGetGeom(IntPtr space, int i);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceGetSublevel"), SuppressUnmanagedCodeSecurity]
+        public static extern int SpaceGetSublevel(IntPtr space);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceQuery"), SuppressUnmanagedCodeSecurity]
+                public static extern bool SpaceQuery(IntPtr space, IntPtr geom);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceRemove"), SuppressUnmanagedCodeSecurity]
+                public static extern void SpaceRemove(IntPtr space, IntPtr geom);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceSetCleanup"), SuppressUnmanagedCodeSecurity]
+                public static extern void SpaceSetCleanup(IntPtr space, bool mode);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceSetSublevel"), SuppressUnmanagedCodeSecurity]
+        public static extern void SpaceSetSublevel(IntPtr space, int sublevel);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSweepAndPruneSpaceCreate"), SuppressUnmanagedCodeSecurity]
+        public static extern IntPtr SweepAndPruneSpaceCreate(IntPtr space, int AxisOrder);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dVectorScale"), SuppressUnmanagedCodeSecurity]
+                public static extern void VectorScale(out dReal a, ref dReal d, int n);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldCreate"), SuppressUnmanagedCodeSecurity]
+                public static extern IntPtr WorldCreate();
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldDestroy"), SuppressUnmanagedCodeSecurity]
+                public static extern void WorldDestroy(IntPtr world);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableAverageSamplesCount"), SuppressUnmanagedCodeSecurity]
+        public static extern int WorldGetAutoDisableAverageSamplesCount(IntPtr world);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableAngularThreshold"), SuppressUnmanagedCodeSecurity]
+                public static extern dReal WorldGetAutoDisableAngularThreshold(IntPtr world);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableFlag"), SuppressUnmanagedCodeSecurity]
+                public static extern bool WorldGetAutoDisableFlag(IntPtr world);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableLinearThreshold"), SuppressUnmanagedCodeSecurity]
+                public static extern dReal WorldGetAutoDisableLinearThreshold(IntPtr world);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableSteps"), SuppressUnmanagedCodeSecurity]
+                public static extern int WorldGetAutoDisableSteps(IntPtr world);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableTime"), SuppressUnmanagedCodeSecurity]
+                public static extern dReal WorldGetAutoDisableTime(IntPtr world);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoEnableDepthSF1"), SuppressUnmanagedCodeSecurity]
+                public static extern int WorldGetAutoEnableDepthSF1(IntPtr world);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetCFM"), SuppressUnmanagedCodeSecurity]
+                public static extern dReal WorldGetCFM(IntPtr world);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetERP"), SuppressUnmanagedCodeSecurity]
+                public static extern dReal WorldGetERP(IntPtr world);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetGravity"), SuppressUnmanagedCodeSecurity]
+                public static extern void WorldGetGravity(IntPtr world, out Vector3 gravity);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetGravity"), SuppressUnmanagedCodeSecurity]
+                public static extern void WorldGetGravity(IntPtr world, out dReal X);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetContactMaxCorrectingVel"), SuppressUnmanagedCodeSecurity]
+                public static extern dReal WorldGetContactMaxCorrectingVel(IntPtr world);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetContactSurfaceLayer"), SuppressUnmanagedCodeSecurity]
+                public static extern dReal WorldGetContactSurfaceLayer(IntPtr world);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAngularDamping"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal WorldGetAngularDamping(IntPtr world);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAngularDampingThreshold"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal WorldGetAngularDampingThreshold(IntPtr world);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetLinearDamping"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal WorldGetLinearDamping(IntPtr world);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetLinearDampingThreshold"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal WorldGetLinearDampingThreshold(IntPtr world);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetQuickStepNumIterations"), SuppressUnmanagedCodeSecurity]
+                public static extern int WorldGetQuickStepNumIterations(IntPtr world);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetQuickStepW"), SuppressUnmanagedCodeSecurity]
+                public static extern dReal WorldGetQuickStepW(IntPtr world);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetMaxAngularSpeed"), SuppressUnmanagedCodeSecurity]
+        public static extern dReal WorldGetMaxAngularSpeed(IntPtr world);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldImpulseToForce"), SuppressUnmanagedCodeSecurity]
+                public static extern void WorldImpulseToForce(IntPtr world, dReal stepsize, dReal ix, dReal iy, dReal iz, out Vector3 force);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldImpulseToForce"), SuppressUnmanagedCodeSecurity]
+                public static extern void WorldImpulseToForce(IntPtr world, dReal stepsize, dReal ix, dReal iy, dReal iz, out dReal forceX);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldQuickStep"), SuppressUnmanagedCodeSecurity]
+                public static extern void WorldQuickStep(IntPtr world, dReal stepsize);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAngularDamping"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldSetAngularDamping(IntPtr world, dReal scale);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAngularDampingThreshold"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldSetAngularDampingThreshold(IntPtr world, dReal threshold);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableAngularThreshold"), SuppressUnmanagedCodeSecurity]
+                public static extern void WorldSetAutoDisableAngularThreshold(IntPtr world, dReal angular_threshold);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableAverageSamplesCount"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldSetAutoDisableAverageSamplesCount(IntPtr world, int average_samples_count);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableFlag"), SuppressUnmanagedCodeSecurity]
+                public static extern void WorldSetAutoDisableFlag(IntPtr world, bool do_auto_disable);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableLinearThreshold"), SuppressUnmanagedCodeSecurity]
+                public static extern void WorldSetAutoDisableLinearThreshold(IntPtr world, dReal linear_threshold);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableSteps"), SuppressUnmanagedCodeSecurity]
+                public static extern void WorldSetAutoDisableSteps(IntPtr world, int steps);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableTime"), SuppressUnmanagedCodeSecurity]
+                public static extern void WorldSetAutoDisableTime(IntPtr world, dReal time);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoEnableDepthSF1"), SuppressUnmanagedCodeSecurity]
+                public static extern void WorldSetAutoEnableDepthSF1(IntPtr world, int autoEnableDepth);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetCFM"), SuppressUnmanagedCodeSecurity]
+                public static extern void WorldSetCFM(IntPtr world, dReal cfm);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetContactMaxCorrectingVel"), SuppressUnmanagedCodeSecurity]
+                public static extern void WorldSetContactMaxCorrectingVel(IntPtr world, dReal vel);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetContactSurfaceLayer"), SuppressUnmanagedCodeSecurity]
+                public static extern void WorldSetContactSurfaceLayer(IntPtr world, dReal depth);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetDamping"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldSetDamping(IntPtr world, dReal linear_scale, dReal angular_scale);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetERP"), SuppressUnmanagedCodeSecurity]
+                public static extern void WorldSetERP(IntPtr world, dReal erp);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetGravity"), SuppressUnmanagedCodeSecurity]
+                public static extern void WorldSetGravity(IntPtr world, dReal x, dReal y, dReal z);
+        
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetLinearDamping"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldSetLinearDamping(IntPtr world, dReal scale);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetLinearDampingThreshold"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldSetLinearDampingThreshold(IntPtr world, dReal threshold);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetQuickStepNumIterations"), SuppressUnmanagedCodeSecurity]
+                public static extern void WorldSetQuickStepNumIterations(IntPtr world, int num);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetQuickStepW"), SuppressUnmanagedCodeSecurity]
+                public static extern void WorldSetQuickStepW(IntPtr world, dReal over_relaxation);
+
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetMaxAngularSpeed"), SuppressUnmanagedCodeSecurity]
+        public static extern void WorldSetMaxAngularSpeed(IntPtr world, dReal max_speed);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldStep"), SuppressUnmanagedCodeSecurity]
+                public static extern void WorldStep(IntPtr world, dReal stepsize);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldStepFast1"), SuppressUnmanagedCodeSecurity]
+                public static extern void WorldStepFast1(IntPtr world, dReal stepsize, int maxiterations);
+
+                [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldExportDIF"), SuppressUnmanagedCodeSecurity]
+                public static extern void WorldExportDIF(IntPtr world, string filename, bool append, string prefix);
+        }
+}
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdePlugin.cs
new file mode 100644
index 0000000..d32188e
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitOdePlugin/OdePlugin.cs
@@ -0,0 +1,90 @@
+/*
+ * 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 (Util.IsWindows())
+                Util.LoadArchSpecificWindowsDll("ode.dll");
+
+            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..0d18adb
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs
@@ -0,0 +1,2855 @@
+/*
+ * 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;
+    }
+
+
+    // colision flags of things others can colide with
+    // rays, sensors, probes removed since can't  be colided with
+    // The top space where things are placed provided further selection
+    // ie physical are in active space nonphysical in static
+    // this should be exclusive as possible
+
+    [Flags]
+    public enum CollisionCategories : uint
+    {
+        Disabled = 0,
+        //by 'things' types
+        Space =         0x01,
+        Geom =          0x02, // aka prim/part
+        Character =     0x04,
+        Land =          0x08,
+        Water =         0x010,
+
+        // by state
+        Phantom =       0x01000,
+        VolumeDtc =     0x02000,
+        Selected =      0x04000,
+        NoShape =       0x08000,
+
+
+        All =           0xffffffff
+    }
+
+    /// <summary>
+    /// Material type for a primitive
+    /// </summary>
+    public enum Material : int
+    {
+        /// <summary></summary>
+        Stone = 0,
+        /// <summary></summary>
+        Metal = 1,
+        /// <summary></summary>
+        Glass = 2,
+        /// <summary></summary>
+        Wood = 3,
+        /// <summary></summary>
+        Flesh = 4,
+        /// <summary></summary>
+        Plastic = 5,
+        /// <summary></summary>
+        Rubber = 6,
+
+        light = 7 // compatibility with old viewers
+    }
+    
+    public enum changes : int
+    {
+        Add = 0,                // arg null. finishs the prim creation. should be used internally only ( to remove later ?)
+        Remove,
+        Link,               // arg AuroraODEPrim new parent prim or null to delink. Makes the prim part of a object with prim parent as root
+        //  or removes from a object if arg is null
+        DeLink,
+        Position,           // arg Vector3 new position in world coords. Changes prim position. Prim must know if it is root or child
+        Orientation,        // arg Quaternion new orientation in world coords. Changes prim position. Prim must know it it is root or child
+        PosOffset,          // not in use
+        // arg Vector3 new position in local coords. Changes prim position in object
+        OriOffset,          // not in use
+        // arg Vector3 new position in local coords. Changes prim position in object
+        Velocity,
+        AngVelocity,
+        Acceleration,
+        Force,
+        Torque,
+        Momentum,
+
+        AddForce,
+        AddAngForce,
+        AngLock,
+
+        Buoyancy,
+
+        PIDTarget,
+        PIDTau,
+        PIDActive,
+
+        PIDHoverHeight,
+        PIDHoverType,
+        PIDHoverTau,
+        PIDHoverActive,
+
+        Size,
+        AvatarSize,
+        Shape,
+        PhysRepData,
+        AddPhysRep,
+
+        CollidesWater,
+        VolumeDtc,
+
+        Physical,
+        Phantom,
+        Selected,
+        disabled,
+        building,
+
+        VehicleType,
+        VehicleFloatParam,
+        VehicleVectorParam,
+        VehicleRotationParam,
+        VehicleFlags,
+        SetVehicle,
+
+        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>();
+
+        public bool OdeUbitLib = false;
+//        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 MaxERP = 0.8f;
+        const float minERP = 0.1f;
+        const float comumContactCFM = 0.0001f;
+        
+        float frictionMovementMult = 0.8f;
+
+        float TerrainBounce = 0.1f;
+        float TerrainFriction = 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;
+        public float HalfOdeStep = 0.01f;
+        public int odetimestepMS = 20; // rounded
+        private float metersInSpace = 25.6f;
+        private float m_timeDilation = 1.0f;
+
+        private DateTime m_lastframe;
+        private DateTime m_lastMeshExpire;
+
+        public float gravityx = 0f;
+        public float gravityy = 0f;
+        public float gravityz = -9.8f;
+
+        private float waterlevel = 0f;
+        private int framecount = 0;
+
+        private int m_meshExpireCntr;
+
+//        private IntPtr WaterGeom = IntPtr.Zero;
+//        private IntPtr WaterHeightmapData = IntPtr.Zero;
+//        private GCHandle WaterMapHandler = new GCHandle();
+
+        private float avDensity = 3f;
+        private float avMovementDivisorWalk = 1.3f;
+        private float avMovementDivisorRun = 0.8f;
+        private float minimumGroundFlightOffset = 3f;
+        public float maximumMassObject = 10000.01f;
+
+
+        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 = 5;
+
+
+        private d.NearCallback nearCallback;
+
+        private HashSet<OdeCharacter> _characters = new HashSet<OdeCharacter>();
+        private HashSet<OdePrim> _prims = new HashSet<OdePrim>();
+        private HashSet<OdePrim> _activeprims = new HashSet<OdePrim>();
+        private HashSet<OdePrim> _activegroups = new HashSet<OdePrim>();
+
+        public OpenSim.Framework.LocklessQueue<ODEchangeitem> ChangesQueue = new OpenSim.Framework.LocklessQueue<ODEchangeitem>();
+
+        /// <summary>
+        /// A list of actors that should receive collision events.
+        /// </summary>
+        private List<PhysicsActor> _collisionEventPrim = new List<PhysicsActor>();
+        private List<PhysicsActor> _collisionEventPrimRemove = new List<PhysicsActor>();
+        
+        private HashSet<OdeCharacter> _badCharacter = new HashSet<OdeCharacter>();
+//        public Dictionary<IntPtr, 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 IntPtr contactgroup;
+
+        public ContactData[] m_materialContactsData = new ContactData[8];
+
+        private Dictionary<Vector3, IntPtr> RegionTerrain = new Dictionary<Vector3, IntPtr>();
+        private Dictionary<IntPtr, float[]> TerrainHeightFieldHeights = new Dictionary<IntPtr, float[]>();
+        private 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 PhysicsActor PANull = new NullPhysicsActor();
+        private float step_time = 0.0f;
+
+        public IntPtr world;
+
+
+        // split the spaces acording to contents type
+        // ActiveSpace contains characters and active prims
+        // StaticSpace contains land and other that is mostly static in enviroment
+        // this can contain subspaces, like the grid in staticspace
+        // as now space only contains this 2 top spaces
+
+        public IntPtr TopSpace; // the global space
+        public IntPtr ActiveSpace; // space for active prims
+        public IntPtr CharsSpace; // space for active prims
+        public IntPtr StaticSpace; // space for the static things around
+        public IntPtr GroundSpace; // space for ground
+
+        public IntPtr SharedRay;
+
+        // 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 IntPtr[] staticPrimspaceOffRegion;
+
+        public Object OdeLock;
+        public 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;
+        public ODEMeshWorker m_meshWorker;
+
+/* maybe needed if ode uses tls
+        private void checkThread()
+        {
+
+            int th = Thread.CurrentThread.ManagedThreadId;
+            if(th != threadid)
+            {
+                threadid = th;
+                d.AllocateODEDataForThread(~0U);
+            }
+        }
+ */
+        /// <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);
+                    CharsSpace = d.HashSpaceCreate(TopSpace);
+                    StaticSpace = d.HashSpaceCreate(TopSpace);
+                    GroundSpace = d.HashSpaceCreate(TopSpace);
+                    }
+                catch
+                    {
+                    // i must RtC#FM 
+                    // i did!
+                    }
+
+                d.HashSpaceSetLevels(TopSpace, -2, 8);
+                d.HashSpaceSetLevels(ActiveSpace, -2, 8);
+                d.HashSpaceSetLevels(CharsSpace, -4, 3);
+                d.HashSpaceSetLevels(StaticSpace, -2, 8);
+                d.HashSpaceSetLevels(GroundSpace, 0, 8);
+
+                // demote to second level
+                d.SpaceSetSublevel(ActiveSpace, 1);
+                d.SpaceSetSublevel(CharsSpace, 1);
+                d.SpaceSetSublevel(StaticSpace, 1);
+                d.SpaceSetSublevel(GroundSpace, 1);
+
+                d.GeomSetCategoryBits(ActiveSpace, (uint)(CollisionCategories.Space |
+                                                        CollisionCategories.Geom |
+                                                        CollisionCategories.Character |
+                                                        CollisionCategories.Phantom |
+                                                        CollisionCategories.VolumeDtc
+                                                        ));
+                d.GeomSetCollideBits(ActiveSpace, (uint)(CollisionCategories.Space |
+                                                        CollisionCategories.Geom |
+                                                        CollisionCategories.Character |
+                                                        CollisionCategories.Phantom |
+                                                        CollisionCategories.VolumeDtc
+                                                        ));
+                d.GeomSetCategoryBits(CharsSpace, (uint)(CollisionCategories.Space |
+                                        CollisionCategories.Geom |
+                                        CollisionCategories.Character |
+                                        CollisionCategories.Phantom |
+                                        CollisionCategories.VolumeDtc
+                                        ));
+                d.GeomSetCollideBits(CharsSpace, 0);
+
+                d.GeomSetCategoryBits(StaticSpace, (uint)(CollisionCategories.Space |
+                                                        CollisionCategories.Geom |
+//                                                        CollisionCategories.Land |
+//                                                        CollisionCategories.Water |
+                                                        CollisionCategories.Phantom |
+                                                        CollisionCategories.VolumeDtc
+                                                        ));
+                d.GeomSetCollideBits(StaticSpace, 0);
+
+                d.GeomSetCategoryBits(GroundSpace, (uint)(CollisionCategories.Land));
+                d.GeomSetCollideBits(GroundSpace, 0);
+
+                contactgroup = d.JointGroupCreate(0);
+                //contactgroup
+
+                SharedRay = d.CreateRay(TopSpace, 1.0f);
+
+                d.WorldSetAutoDisableFlag(world, false);
+            }
+        }
+
+        // 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;
+
+            string ode_config = d.GetConfiguration();
+            if (ode_config != null && ode_config != "")
+            {
+                m_log.WarnFormat("ODE configuration: {0}", ode_config);
+
+                if (ode_config.Contains("ODE_Ubit"))
+                {
+                    OdeUbitLib = true;
+                }
+            }
+
+            /*
+                        if (region != null)
+                        {
+                            WorldExtents.X = region.RegionSizeX;
+                            WorldExtents.Y = region.RegionSizeY;
+                        }
+             */
+
+            // Defaults
+
+            int contactsPerCollision = 80;
+
+            IConfig physicsconfig = null;
+
+            if (m_config != null)
+            {
+                physicsconfig = m_config.Configs["ODEPhysicsSettings"];
+                if (physicsconfig != null)
+                {
+                    gravityx = physicsconfig.GetFloat("world_gravityx", gravityx);
+                    gravityy = physicsconfig.GetFloat("world_gravityy", gravityy);
+                    gravityz = physicsconfig.GetFloat("world_gravityz", gravityz);
+
+                    metersInSpace = physicsconfig.GetFloat("meters_in_small_space", metersInSpace);
+
+                    contactsurfacelayer = physicsconfig.GetFloat("world_contact_surface_layer", contactsurfacelayer);
+
+                    ODE_STEPSIZE = physicsconfig.GetFloat("world_stepsize", ODE_STEPSIZE);
+                    m_physicsiterations = physicsconfig.GetInt("world_internal_steps_without_collisions", m_physicsiterations);
+
+                    avDensity = physicsconfig.GetFloat("av_density", avDensity);
+                    avMovementDivisorWalk = physicsconfig.GetFloat("av_movement_divisor_walk", avMovementDivisorWalk);
+                    avMovementDivisorRun = physicsconfig.GetFloat("av_movement_divisor_run", avMovementDivisorRun);
+
+                    contactsPerCollision = physicsconfig.GetInt("contacts_per_collision", contactsPerCollision);
+
+                    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", geomDefaultDensity);
+                    bodyFramesAutoDisable = physicsconfig.GetInt("body_frames_auto_disable", bodyFramesAutoDisable);
+
+                    physics_logging = physicsconfig.GetBoolean("physics_logging", false);
+                    physics_logging_interval = physicsconfig.GetInt("physics_logging_interval", 0);
+                    physics_logging_append_existing_logfile = physicsconfig.GetBoolean("physics_logging_append_existing_logfile", false);
+
+                    minimumGroundFlightOffset = physicsconfig.GetFloat("minimum_ground_flight_offset", minimumGroundFlightOffset);
+                    maximumMassObject = physicsconfig.GetFloat("maximum_mass_object", maximumMassObject);
+                }
+            }
+
+            m_meshWorker = new ODEMeshWorker(this, m_log, meshmerizer, physicsconfig);
+
+            HalfOdeStep = ODE_STEPSIZE * 0.5f;
+            odetimestepMS = (int)(1000.0f * ODE_STEPSIZE +0.5f);
+
+            ContactgeomsArray = Marshal.AllocHGlobal(contactsPerCollision * d.ContactGeom.unmanagedSizeOf);
+            GlobalContactsArray = Marshal.AllocHGlobal(maxContactsbeforedeath * d.Contact.unmanagedSizeOf);
+
+            m_materialContactsData[(int)Material.Stone].mu = 0.8f;
+            m_materialContactsData[(int)Material.Stone].bounce = 0.4f;
+
+            m_materialContactsData[(int)Material.Metal].mu = 0.3f;
+            m_materialContactsData[(int)Material.Metal].bounce = 0.4f;
+
+            m_materialContactsData[(int)Material.Glass].mu = 0.2f;
+            m_materialContactsData[(int)Material.Glass].bounce = 0.7f;
+
+            m_materialContactsData[(int)Material.Wood].mu = 0.6f;
+            m_materialContactsData[(int)Material.Wood].bounce = 0.5f;
+
+            m_materialContactsData[(int)Material.Flesh].mu = 0.9f;
+            m_materialContactsData[(int)Material.Flesh].bounce = 0.3f;
+
+            m_materialContactsData[(int)Material.Plastic].mu = 0.4f;
+            m_materialContactsData[(int)Material.Plastic].bounce = 0.7f;
+
+            m_materialContactsData[(int)Material.Rubber].mu = 0.9f;
+            m_materialContactsData[(int)Material.Rubber].bounce = 0.95f;
+
+            m_materialContactsData[(int)Material.light].mu = 0.0f;
+            m_materialContactsData[(int)Material.light].bounce = 0.0f;
+
+            // 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.002f);
+            d.WorldSetAngularDamping(world, 0.002f);
+            d.WorldSetAngularDampingThreshold(world, 0f);
+            d.WorldSetLinearDampingThreshold(world, 0f);
+            d.WorldSetMaxAngularSpeed(world, 100f);
+
+            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, 60.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);
+                    d.GeomSetCategoryBits(newspace, (uint)(CollisionCategories.Space |
+                                        CollisionCategories.Geom |
+                                        CollisionCategories.Land |
+                                        CollisionCategories.Water |
+                                        CollisionCategories.Phantom |
+                                        CollisionCategories.VolumeDtc
+                                        ));
+                    d.GeomSetCollideBits(newspace, 0);
+
+                    staticPrimspace[i, j] = newspace;
+                }
+            // let this now be real maximum values
+            spaceGridMaxX--;
+            spaceGridMaxY--;
+
+            // create 4 off world spaces (x<0,x>max,y<0,y>max)
+            staticPrimspaceOffRegion = new IntPtr[4];
+
+            for (i = 0; i < 4; i++)
+                {
+                    newspace = d.HashSpaceCreate(StaticSpace);
+                    d.GeomSetCategoryBits(newspace, (int)CollisionCategories.Space);
+                    waitForSpaceUnlock(newspace);
+                    d.SpaceSetSublevel(newspace, 2);
+                    d.HashSpaceSetLevels(newspace, -2, 8);
+                    d.GeomSetCategoryBits(newspace, (uint)(CollisionCategories.Space |
+                                        CollisionCategories.Geom |
+                                        CollisionCategories.Land |
+                                        CollisionCategories.Water |
+                                        CollisionCategories.Phantom |
+                                        CollisionCategories.VolumeDtc
+                                        ));
+                    d.GeomSetCollideBits(newspace, 0);
+
+                    staticPrimspaceOffRegion[i] = newspace;
+                }
+
+            m_lastframe = DateTime.UtcNow;
+            m_lastMeshExpire = m_lastframe;
+        }
+
+        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, float cfm, float erpscale, float dscale)
+        {
+            if (GlobalContactsArray == IntPtr.Zero || m_global_contactcount >= maxContactsbeforedeath)
+                return IntPtr.Zero;
+
+            float erp = contactGeom.depth;
+            erp *= erpscale;
+            if (erp < minERP)
+                erp = minERP;
+            else if (erp > MaxERP)
+                erp = MaxERP;
+
+            float depth = contactGeom.depth * dscale;
+            if (depth > 0.5f)
+                depth = 0.5f;
+
+            d.Contact newcontact = new d.Contact();
+            newcontact.geom.depth = 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 = cfm;
+            newcontact.surface.soft_erp = erp;
+
+            IntPtr contact = new IntPtr(GlobalContactsArray.ToInt64() + (Int64)(m_global_contactcount * d.Contact.unmanagedSizeOf));
+            Marshal.StructureToPtr(newcontact, contact, true);
+            return d.JointCreateContactPtr(world, contactgroup, contact);
+        }
+
+        private bool GetCurContactGeom(int index, ref d.ContactGeom newcontactgeom)
+        {
+            if (ContactgeomsArray == IntPtr.Zero || index >= contactsPerCollision)
+                return false;
+
+            IntPtr contactptr = new IntPtr(ContactgeomsArray.ToInt64() + (Int64)(index * d.ContactGeom.unmanagedSizeOf));
+            newcontactgeom = (d.ContactGeom)Marshal.PtrToStructure(contactptr, typeof(d.ContactGeom));
+            return true;
+        }
+
+        /// <summary>
+        /// This is our near callback.  A geometry is near a body
+        /// </summary>
+        /// <param name="space">The space that contains the geoms.  Remember, spaces are also geoms</param>
+        /// <param name="g1">a geometry or space</param>
+        /// <param name="g2">another geometry or space</param>
+        /// 
+
+        private void near(IntPtr space, IntPtr g1, IntPtr g2)
+        {
+            //  no lock here!  It's invoked from within Simulate(), which is thread-locked
+
+            if (m_global_contactcount >= maxContactsbeforedeath)
+                return;
+
+            // Test if we're colliding a geom with a space.
+            // If so we have to drill down into the space recursively
+
+            if (g1 == IntPtr.Zero || g2 == IntPtr.Zero)
+                return;
+
+            if (d.GeomIsSpace(g1) || d.GeomIsSpace(g2))
+            {
+                // We'll be calling near recursivly if one
+                // of them is a space to find all of the
+                // contact points in the space
+                try
+                {
+                    d.SpaceCollide2(g1, g2, IntPtr.Zero, nearCallback);
+                }
+                catch (AccessViolationException)
+                {
+                    m_log.Warn("[PHYSICS]: Unable to collide test a space");
+                    return;
+                }
+                //here one should check collisions of geoms inside a space
+                // but on each space we only should have geoms that not colide amoung each other
+                // so we don't dig inside spaces
+                return;
+            }
+
+            // get geom bodies to check if we already a joint contact
+            // guess this shouldn't happen now
+            IntPtr b1 = d.GeomGetBody(g1);
+            IntPtr b2 = d.GeomGetBody(g2);
+
+            // d.GeomClassID id = d.GeomGetClass(g1);
+
+            // Figure out how many contact points we have
+            int count = 0;
+            try
+            {
+                // Colliding Geom To Geom
+                // This portion of the function 'was' blatantly ripped off from BoxStack.cs
+
+                if (g1 == g2)
+                    return; // Can't collide with yourself
+
+                if (b1 != IntPtr.Zero && b2 != IntPtr.Zero && d.AreConnectedExcluding(b1, b2, d.JointType.Contact))
+                    return;
+                /*
+                // debug
+                                PhysicsActor dp2;
+                                if (d.GeomGetClass(g1) == d.GeomClassID.HeightfieldClass)
+                                {
+                                    d.AABB aabb;
+                                    d.GeomGetAABB(g2, out aabb);
+                                    float x = aabb.MaxX - aabb.MinX;
+                                    float y = aabb.MaxY - aabb.MinY;
+                                    float z = aabb.MaxZ - aabb.MinZ;
+                                    if (x > 60.0f || y > 60.0f || z > 60.0f)
+                                    {
+                                        if (!actor_name_map.TryGetValue(g2, out dp2))
+                                            m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 2");
+                                        else
+                                            m_log.WarnFormat("[PHYSICS]: land versus large prim geo {0},size {1}, AABBsize <{2},{3},{4}>, at {5} ori {6},({7})",
+                                                dp2.Name, dp2.Size, x, y, z,
+                                                dp2.Position.ToString(),
+                                                dp2.Orientation.ToString(),
+                                                dp2.Orientation.Length());
+                                        return;
+                                    }
+                                }
+                //
+                */
+
+
+                if (d.GeomGetCategoryBits(g1) == (uint)CollisionCategories.VolumeDtc ||
+                    d.GeomGetCategoryBits(g2) == (uint)CollisionCategories.VolumeDtc)
+                {
+                    int cflags;
+                    unchecked
+                    {
+                        cflags = (int)(1 | d.CONTACTS_UNIMPORTANT);
+                    }
+                    count = d.CollidePtr(g1, g2, cflags, ContactgeomsArray, d.ContactGeom.unmanagedSizeOf);
+                }
+                else
+                    count = d.CollidePtr(g1, g2, (contactsPerCollision & 0xffff), ContactgeomsArray, d.ContactGeom.unmanagedSizeOf);
+            }
+            catch (SEHException)
+            {
+                m_log.Error("[PHYSICS]: The Operating system shut down ODE because of corrupt memory.  This could be a result of really irregular terrain.  If this repeats continuously, restart using Basic Physics and terrain fill your terrain.  Restarting the sim.");
+                //                ode.drelease(world);
+                base.TriggerPhysicsBasedRestart();
+            }
+            catch (Exception e)
+            {
+                m_log.WarnFormat("[PHYSICS]: Unable to collide test an object: {0}", e.Message);
+                return;
+            }
+
+            // contacts done
+            if (count == 0)
+                return;
+
+            // try get physical actors 
+            PhysicsActor p1;
+            PhysicsActor p2;
+
+            if (!actor_name_map.TryGetValue(g1, out p1))
+            {
+                m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 1");
+                return;
+            }
+
+            if (!actor_name_map.TryGetValue(g2, out p2))
+            {
+                m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 2");
+                return;
+            }
+
+            // 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;
+
+            // do volume detection case
+            if ((p1.IsVolumeDtc || p2.IsVolumeDtc))
+            {
+                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, false
+                    );
+
+                collision_accounting_events(p1, p2, maxDepthContact);
+                return;
+            }
+
+            // big messy collision analises
+
+            float mu = 0;
+            float bounce = 0;
+            float cfm = 0.0001f;
+            float erpscale = 1.0f;
+            float dscale = 1.0f;
+            bool IgnoreNegSides = false;
+
+            ContactData contactdata1 = new ContactData(0, 0, false);
+            ContactData contactdata2 = new ContactData(0, 0, false);
+
+            bool dop1ava = false;
+            bool dop2ava = false;
+            bool ignore = false;
+
+            switch (p1.PhysicsActorType)
+            {
+                case (int)ActorTypes.Agent:
+                    {
+                        dop1ava = true;
+                        switch (p2.PhysicsActorType)
+                        {
+                            case (int)ActorTypes.Agent:
+                                p1.CollidingObj = true;
+                                p2.CollidingObj = true;
+                                break;
+
+                            case (int)ActorTypes.Prim:
+                                if (p2.Velocity.LengthSquared() > 0.0f)
+                                    p2.CollidingObj = true;
+                                break;
+
+                            default:
+                                ignore = true; // avatar to terrain and water ignored
+                                break;
+                        }
+                        break;
+                    }
+
+                case (int)ActorTypes.Prim:
+                    switch (p2.PhysicsActorType)
+                    {
+                        case (int)ActorTypes.Agent:
+
+                            dop2ava = true;
+
+                            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;
+                            }
+                            p1.getContactData(ref contactdata1);
+                            p2.getContactData(ref contactdata2);
+                            bounce = contactdata1.bounce * contactdata2.bounce;
+                            mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu);
+
+                            cfm = p1.Mass;
+                            if (cfm > p2.Mass)
+                                cfm = p2.Mass;
+                            dscale = 10 / cfm;
+                            dscale = (float)Math.Sqrt(dscale);
+                            if (dscale > 1.0f)
+                                dscale = 1.0f;
+                            erpscale = cfm * 0.01f;
+                            cfm = 0.0001f / cfm;
+                            if (cfm > 0.01f)
+                                cfm = 0.01f;
+                            else if (cfm < 0.00001f)
+                                cfm = 0.00001f;
+
+                            if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f))
+                                mu *= frictionMovementMult;
+
+                            break;
+
+                        case (int)ActorTypes.Ground:
+                            p1.getContactData(ref contactdata1);
+                            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;
+
+                            cfm = p1.Mass;
+                            dscale = 10 / cfm;
+                            dscale = (float)Math.Sqrt(dscale);
+                            if (dscale > 1.0f)
+                                dscale = 1.0f;
+                            erpscale = cfm * 0.01f;
+                            cfm = 0.0001f / cfm;
+                            if (cfm > 0.01f)
+                                cfm = 0.01f;
+                            else if (cfm < 0.00001f)
+                                cfm = 0.00001f;
+
+                            if (d.GeomGetClass(g1) == d.GeomClassID.TriMeshClass)
+                            {
+                                if (curContact.side1 > 0)
+                                    IgnoreNegSides = true;
+                            }
+                            break;
+
+                        case (int)ActorTypes.Water:
+                        default:
+                            ignore = true;
+                            break;
+                    }
+                    break;
+
+                case (int)ActorTypes.Ground:
+                    if (p2.PhysicsActorType == (int)ActorTypes.Prim)
+                    {
+                        p2.CollidingGround = true;
+                        p2.getContactData(ref contactdata2);
+                        bounce = contactdata2.bounce * TerrainBounce;
+                        mu = (float)Math.Sqrt(contactdata2.mu * TerrainFriction);
+
+                        cfm = p2.Mass;
+                        dscale = 10 / cfm;
+                        dscale = (float)Math.Sqrt(dscale);
+
+                        if (dscale > 1.0f)
+                            dscale = 1.0f;
+
+                        erpscale = cfm * 0.01f;
+                        cfm = 0.0001f / cfm;
+                        if (cfm > 0.01f)
+                            cfm = 0.01f;
+                        else if (cfm < 0.00001f)
+                            cfm = 0.00001f;
+
+                        if (curContact.side1 > 0) // should be 2 ?
+                            IgnoreNegSides = true;
+
+                        if (Math.Abs(p2.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y) > 0.1f)
+                            mu *= frictionMovementMult;
+                    }
+                    else
+                        ignore = true;
+                    break;
+
+                case (int)ActorTypes.Water:
+                default:
+                    break;
+            }
+
+            if (ignore)
+                return;
+
+
+            d.ContactGeom maxContact = curContact;
+//            if (IgnoreNegSides && curContact.side1 < 0)
+//                maxContact.depth = float.MinValue;
+
+            d.ContactGeom minContact = curContact;
+//            if (IgnoreNegSides && curContact.side1 < 0)
+//                minContact.depth = float.MaxValue;
+
+            IntPtr Joint;
+            bool FeetCollision = false;
+            int ncontacts = 0;
+
+
+                int i = 0;
+
+                while (true)
+                {
+                    if (m_global_contactcount >= maxContactsbeforedeath)
+                        break;
+
+//                    if (!(IgnoreNegSides && curContact.side1 < 0))
+                    {
+                        bool noskip = true;
+                        if (dop1ava)
+                        {
+                            if (!(((OdeCharacter)p1).Collide(g1,false, ref curContact, ref FeetCollision)))
+
+                                noskip = false;
+                        }
+                        else if (dop2ava)
+                        {
+                            if (!(((OdeCharacter)p2).Collide(g2,true, ref curContact, ref FeetCollision)))
+                                noskip = false;
+                        }
+
+                        if (noskip)
+                        {
+                            m_global_contactcount++;
+                            ncontacts++;
+
+                            Joint = CreateContacJoint(ref curContact, mu, bounce, cfm, erpscale, dscale);
+                            d.JointAttach(Joint, b1, b2);
+
+                            if (curContact.depth > maxContact.depth)
+                                maxContact = curContact;
+
+                            if (curContact.depth < minContact.depth)
+                                minContact = curContact;
+                        }
+                    }
+
+                    if (++i >= count)
+                        break;
+
+                    if (!GetCurContactGeom(i, ref curContact))
+                        break;
+                }
+
+            if (ncontacts > 0)
+            {
+                ContactPoint maxDepthContact = new ContactPoint(
+                            new Vector3(maxContact.pos.X, maxContact.pos.Y, maxContact.pos.Z),
+                            new Vector3(minContact.normal.X, minContact.normal.Y, minContact.normal.Z),
+                            maxContact.depth, FeetCollision
+                            );
+                collision_accounting_events(p1, p2, maxDepthContact);
+            }
+        }
+
+        private void collision_accounting_events(PhysicsActor p1, PhysicsActor p2, ContactPoint contact)
+        {
+            uint obj2LocalID = 0;
+
+            bool p1events = p1.SubscribedEvents();
+            bool p2events = p2.SubscribedEvents();
+
+            if (p1.IsVolumeDtc)
+                p2events = false;
+            if (p2.IsVolumeDtc)
+                p1events = false;
+
+            if (!p2events && !p1events)
+                return;
+
+            Vector3 vel = Vector3.Zero;
+            if (p2 != null && p2.IsPhysical)
+                vel = p2.Velocity;
+
+            if (p1 != null && p1.IsPhysical)
+                vel -= p1.Velocity;
+
+            contact.RelativeSpeed = Vector3.Dot(vel, contact.SurfaceNormal);
+
+            switch ((ActorTypes)p1.PhysicsActorType)
+            {
+                case ActorTypes.Agent:
+                case ActorTypes.Prim:
+                    {
+                        switch ((ActorTypes)p2.PhysicsActorType)
+                        {
+                            case ActorTypes.Agent:
+                            case ActorTypes.Prim:
+                                if (p2events)
+                                {
+                                    AddCollisionEventReporting(p2);
+                                    p2.AddCollisionEvent(p1.ParentActor.LocalID, contact);
+                                }
+                                obj2LocalID = p2.ParentActor.LocalID;
+                                break;
+
+                            case ActorTypes.Ground:
+                            case ActorTypes.Unknown:
+                            default:
+                                obj2LocalID = 0;
+                                break;
+                        }
+                        if (p1events)
+                        {
+                            contact.SurfaceNormal = -contact.SurfaceNormal;
+                            AddCollisionEventReporting(p1);
+                            p1.AddCollisionEvent(obj2LocalID, contact);
+                        }
+                        break;
+                    }
+                case ActorTypes.Ground:
+                case ActorTypes.Unknown:
+                default:
+                    {
+                        if (p2events && !p2.IsVolumeDtc)
+                        {
+                            AddCollisionEventReporting(p2);
+                            p2.AddCollisionEvent(0, contact);
+                        }
+                        break;
+                    }
+            }
+        }
+
+        /// <summary>
+        /// This is our collision testing routine in ODE
+        /// </summary>
+        /// <param name="timeStep"></param>
+        private void collision_optimized()
+        {
+            lock (_characters)
+                {
+                try
+                {
+                    foreach (OdeCharacter chr in _characters)
+                    {
+                        if (chr == null || chr.Body == IntPtr.Zero)
+                            continue;
+
+                        chr.IsColliding = false;
+                        //                    chr.CollidingGround = false; not done here
+                        chr.CollidingObj = false;
+                        // do colisions with static space
+                        d.SpaceCollide2(chr.collider, StaticSpace, IntPtr.Zero, nearCallback);
+
+                        // chars with chars
+                        d.SpaceCollide(CharsSpace, IntPtr.Zero, nearCallback);
+                        // no coll with gnd
+                    }
+                }
+                catch (AccessViolationException)
+                {
+                    m_log.Warn("[PHYSICS]: Unable to collide Character to static space");
+                }
+
+            }
+
+            lock (_activeprims)
+            {
+                foreach (OdePrim aprim in _activeprims)
+                {
+                    aprim.CollisionScore = 0;
+                    aprim.IsColliding = false;
+                }
+            }
+
+            // collide active prims with static enviroment
+            lock (_activegroups)
+            {
+                try
+                {
+                    foreach (OdePrim prm in _activegroups)
+                    {
+                        if (!prm.m_outbounds)
+                        {
+                            if (d.BodyIsEnabled(prm.Body))
+                            {
+                                d.SpaceCollide2(StaticSpace, prm.collide_geom, IntPtr.Zero, nearCallback);
+                                d.SpaceCollide2(GroundSpace, prm.collide_geom, IntPtr.Zero, nearCallback);
+                            }
+                        }
+                    }
+                }
+                catch (AccessViolationException)
+                {
+                    m_log.Warn("[PHYSICS]: Unable to collide Active prim to static space");
+                }
+            }
+            // colide active amoung them
+            try
+            {
+                d.SpaceCollide(ActiveSpace, IntPtr.Zero, nearCallback);
+            }
+            catch (AccessViolationException)
+            {
+                m_log.Warn("[PHYSICS]: Unable to collide Active with Characters space");
+            }
+            // and with chars
+            try
+            {
+                d.SpaceCollide2(CharsSpace,ActiveSpace, IntPtr.Zero, nearCallback);
+            }
+            catch (AccessViolationException)
+            {
+                m_log.Warn("[PHYSICS]: Unable to collide in Active space");
+            }
+            //            _perloopContact.Clear();
+        }
+
+        #endregion
+        /// <summary>
+        /// Add actor to the list that should receive collision events in the simulate loop.
+        /// </summary>
+        /// <param name="obj"></param>
+        public void AddCollisionEventReporting(PhysicsActor obj)
+        {
+            if (!_collisionEventPrim.Contains(obj))
+                _collisionEventPrim.Add(obj);
+        }
+
+        /// <summary>
+        /// Remove actor from the list that should receive collision events in the simulate loop.
+        /// </summary>
+        /// <param name="obj"></param>
+        public void RemoveCollisionEventReporting(PhysicsActor obj)
+        {
+            if (_collisionEventPrim.Contains(obj) && !_collisionEventPrimRemove.Contains(obj))
+                _collisionEventPrimRemove.Add(obj);
+        }
+
+        public override float TimeDilation
+        {
+            get { return m_timeDilation; }
+        }
+
+        public override bool SupportsNINJAJoints
+        {
+            get { return false; }
+        }
+
+        #region Add/Remove Entities
+
+        public override PhysicsActor AddAvatar(uint localID, string avName, Vector3 position, Vector3 size, float feetOffset, bool isFlying)
+        {
+            Vector3 pos;
+            pos.X = position.X;
+            pos.Y = position.Y;
+            pos.Z = position.Z;
+            OdeCharacter newAv = new OdeCharacter(localID,avName, this, pos, size, feetOffset, avDensity, avMovementDivisorWalk, avMovementDivisorRun);
+            newAv.Flying = isFlying;
+            newAv.MinimumGroundFlightOffset = minimumGroundFlightOffset;
+            
+            return newAv;
+        }
+
+        public void AddCharacter(OdeCharacter chr)
+        {
+            lock (_characters)
+            {
+                if (!_characters.Contains(chr))
+                {
+                    _characters.Add(chr);
+                    if (chr.bad)
+                        m_log.DebugFormat("[PHYSICS] Added BAD actor {0} to characters list", chr.m_uuid);
+                }
+            }
+        }
+
+        public void RemoveCharacter(OdeCharacter chr)
+        {
+            lock (_characters)
+            {
+                if (_characters.Contains(chr))
+                {
+                    _characters.Remove(chr);
+                }
+            }
+        }
+
+        public void BadCharacter(OdeCharacter chr)
+        {
+            lock (_badCharacter)
+            {
+                if (!_badCharacter.Contains(chr))
+                    _badCharacter.Add(chr);
+            }
+        }
+
+        public override void RemoveAvatar(PhysicsActor actor)
+        {
+            //m_log.Debug("[PHYSICS]:ODELOCK");
+            ((OdeCharacter) actor).Destroy();
+        }
+
+
+        public void addActivePrim(OdePrim activatePrim)
+        {
+            // adds active prim..   
+            lock (_activeprims)
+            {
+                if (!_activeprims.Contains(activatePrim))
+                    _activeprims.Add(activatePrim);
+            }
+        }
+
+        public void addActiveGroups(OdePrim activatePrim)
+        {
+            lock (_activegroups)
+            {
+                if (!_activegroups.Contains(activatePrim))
+                    _activegroups.Add(activatePrim);
+            }
+        }
+
+        private PhysicsActor AddPrim(String name, Vector3 position, Vector3 size, Quaternion rotation,
+                                     PrimitiveBaseShape pbs, bool isphysical, bool isPhantom, byte shapeType, uint localID)
+        {
+            OdePrim newPrim;
+            lock (OdeLock)
+            {
+                newPrim = new OdePrim(name, this, position, size, rotation, pbs, isphysical, isPhantom, shapeType, localID);
+                lock (_prims)
+                    _prims.Add(newPrim);
+            }
+            return newPrim;
+        }
+
+        public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
+                                                  Vector3 size, Quaternion rotation, bool isPhysical, bool isPhantom, uint localid)
+        {
+            return AddPrim(primName, position, size, rotation, pbs, isPhysical, isPhantom, 0 , localid);
+        }
+
+
+        public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
+                                                  Vector3 size, Quaternion rotation, bool isPhysical, uint localid)
+        {
+            return AddPrim(primName, position, size, rotation, pbs, isPhysical,false, 0, localid);
+        }
+
+        public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
+                                                  Vector3 size, Quaternion rotation, bool isPhysical, bool isPhantom, byte shapeType, uint localid)
+        {
+
+            return AddPrim(primName, position, size, rotation, pbs, isPhysical,isPhantom, shapeType, localid);
+        }
+
+        public void remActivePrim(OdePrim deactivatePrim)
+        {
+            lock (_activeprims)
+            {
+                _activeprims.Remove(deactivatePrim);
+            }
+        }
+        public void remActiveGroup(OdePrim deactivatePrim)
+        {
+            lock (_activegroups)
+            {
+                _activegroups.Remove(deactivatePrim);
+            }
+        }
+
+        public override void RemovePrim(PhysicsActor prim)
+        {
+            // As with all ODE physics operations, we don't remove the prim immediately but signal that it should be
+            // removed in the next physics simulate pass.
+            if (prim is OdePrim)
+            {
+//                lock (OdeLock)
+                {
+                    
+                    OdePrim p = (OdePrim)prim;
+                    p.setPrimForRemoval();
+                }
+            }
+        }
+        /// <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);
+            }
+
+        }
+
+        public bool havePrim(OdePrim prm)
+        {
+            lock (_prims)
+                return _prims.Contains(prm);
+        }
+
+        public bool haveActor(PhysicsActor actor)
+        {
+            if (actor is OdePrim)
+            {
+                lock (_prims)
+                    return _prims.Contains((OdePrim)actor);
+            }
+            else if (actor is OdeCharacter)
+            {
+                lock (_characters)
+                    return _characters.Contains((OdeCharacter)actor);
+            }
+            return false;
+        }
+
+        #endregion
+
+        #region Space Separation Calculation
+
+        /// <summary>
+        /// Called when a static prim moves or becomes static
+        /// Places the prim in a space one the static sub-spaces grid
+        /// </summary>
+        /// <param name="geom">the pointer to the geom that moved</param>
+        /// <param name="pos">the position that the geom moved to</param>
+        /// <param name="currentspace">a pointer to the space it was in before it was moved.</param>
+        /// <returns>a pointer to the new space it's in</returns>
+        public IntPtr MoveGeomToStaticSpace(IntPtr geom, Vector3 pos, IntPtr currentspace)
+        {
+            // moves a prim into another static sub-space or from another space into a static sub-space
+
+            // Called ODEPrim so
+            // it's already in locked space.
+
+            if (geom == IntPtr.Zero) // shouldn't happen
+                return IntPtr.Zero;
+
+            // get the static sub-space for current position
+            IntPtr newspace = calculateSpaceForGeom(pos);
+
+            if (newspace == currentspace) // if we are there all done
+                return newspace;
+
+            // else remove it from its current space
+            if (currentspace != IntPtr.Zero && d.SpaceQuery(currentspace, geom))
+            {
+                if (d.GeomIsSpace(currentspace))
+                {
+                    waitForSpaceUnlock(currentspace);
+                    d.SpaceRemove(currentspace, geom);
+
+                    if (d.SpaceGetSublevel(currentspace) > 2 && d.SpaceGetNumGeoms(currentspace) == 0)
+                    {
+                        d.SpaceDestroy(currentspace);
+                    }
+                }
+                else
+                {
+                    m_log.Info("[Physics]: Invalid or empty Space passed to 'MoveGeomToStaticSpace':" + currentspace +
+                                   " Geom:" + geom);
+                }
+            }
+            else // odd currentspace is null or doesn't contain the geom? lets try the geom ideia of current space
+            {
+                currentspace = d.GeomGetSpace(geom);
+                if (currentspace != IntPtr.Zero)
+                {
+                    if (d.GeomIsSpace(currentspace))
+                    {
+                        waitForSpaceUnlock(currentspace);
+                        d.SpaceRemove(currentspace, geom);
+
+                        if (d.SpaceGetSublevel(currentspace) > 2 && d.SpaceGetNumGeoms(currentspace) == 0)
+                        {
+                            d.SpaceDestroy(currentspace);
+                        }
+
+                    }
+                }
+            }
+
+            // put the geom in the newspace
+            waitForSpaceUnlock(newspace);
+            d.SpaceAdd(newspace, geom);
+
+            // let caller know this newspace
+            return newspace;
+        }
+
+        /// <summary>
+        /// Calculates the space the prim should be in by its position
+        /// </summary>
+        /// <param name="pos"></param>
+        /// <returns>a pointer to the space. This could be a new space or reused space.</returns>
+        public IntPtr calculateSpaceForGeom(Vector3 pos)
+        {
+            int x, y;
+
+            if (pos.X < 0)
+                return staticPrimspaceOffRegion[0];
+
+            if (pos.Y < 0)
+                return staticPrimspaceOffRegion[2];
+
+            x = (int)(pos.X * spacesPerMeter);
+            if (x > spaceGridMaxX)
+                return staticPrimspaceOffRegion[1];
+            
+            y = (int)(pos.Y * spacesPerMeter);
+            if (y > spaceGridMaxY)
+                return staticPrimspaceOffRegion[3];
+
+            return staticPrimspace[x, y];
+        }
+ 
+        #endregion
+
+
+        /// <summary>
+        /// Called to queue a change to a actor
+        /// to use in place of old taint mechanism so changes do have a time sequence
+        /// </summary>
+
+        public void AddChange(PhysicsActor actor, changes what, Object arg)
+        {
+            ODEchangeitem item = new ODEchangeitem();
+            item.actor = actor;
+            item.what = what;
+            item.arg = arg;
+            ChangesQueue.Enqueue(item);
+        }
+
+        /// <summary>
+        /// Called after our prim properties are set Scale, position etc.
+        /// We use this event queue like method to keep changes to the physical scene occuring in the threadlocked mutex
+        /// This assures us that we have no race conditions
+        /// </summary>
+        /// <param name="prim"></param>
+        public override void AddPhysicsActorTaint(PhysicsActor prim)
+        {
+        }
+
+        // does all pending changes generated during region load process
+        public override void PrepareSimulation()
+        {
+            lock (OdeLock)
+            {
+                if (world == IntPtr.Zero)
+                {
+                    ChangesQueue.Clear();
+                    return;
+                }
+
+                ODEchangeitem item;
+
+                int donechanges = 0;
+                if (ChangesQueue.Count > 0)
+                {
+                    m_log.InfoFormat("[ODE] start processing pending actor operations");
+                    int tstart = Util.EnvironmentTickCount();
+
+                    while (ChangesQueue.Dequeue(out item))
+                    {
+                        if (item.actor != null)
+                        {
+                            try
+                            {
+                                if (item.actor is OdeCharacter)
+                                    ((OdeCharacter)item.actor).DoAChange(item.what, item.arg);
+                                else if (((OdePrim)item.actor).DoAChange(item.what, item.arg))
+                                    RemovePrimThreadLocked((OdePrim)item.actor);
+                            }
+                            catch
+                            {
+                                m_log.WarnFormat("[PHYSICS]: Operation failed for a actor {0} {1}",
+                                    item.actor.Name, item.what.ToString());
+                            }
+                        }
+                        donechanges++;
+                    }
+                    int time = Util.EnvironmentTickCountSubtract(tstart);
+                    m_log.InfoFormat("[ODE] finished {0} operations in {1}ms", donechanges, time);
+                }
+            }
+        }
+
+        /// <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)
+        {
+            DateTime now = DateTime.UtcNow;
+            TimeSpan timedif = now - m_lastframe;
+            timeStep = (float)timedif.TotalSeconds;
+            m_lastframe = now;
+            
+            // acumulate time so we can reduce error
+            step_time += timeStep;
+
+            if (step_time < HalfOdeStep)
+                return 0;
+
+            if (framecount < 0)
+                framecount = 0;
+
+
+            framecount++;
+
+            int curphysiteractions;
+
+            // if in trouble reduce step resolution
+            if (step_time >= m_SkipFramesAtms)
+                curphysiteractions = m_physicsiterations / 2;
+            else
+                curphysiteractions = m_physicsiterations;
+
+//            checkThread();
+            int nodeframes = 0;
+
+            lock (SimulationLock)
+                lock(OdeLock)
+            {
+                if (world == IntPtr.Zero)
+                {
+                    ChangesQueue.Clear();
+                    return 0;
+                }
+
+                ODEchangeitem item;
+
+
+                
+                d.WorldSetQuickStepNumIterations(world, curphysiteractions);
+
+                int loopstartMS = Util.EnvironmentTickCount();
+                int looptimeMS = 0;
+                
+
+                while (step_time > HalfOdeStep)
+                {
+                    try
+                    {
+                        // clear pointer/counter to contacts to pass into joints
+                        m_global_contactcount = 0;
+
+                        if (ChangesQueue.Count > 0)
+                        {
+                            int changestartMS = Util.EnvironmentTickCount();
+                            int ttmp;
+                            while (ChangesQueue.Dequeue(out item))
+                            {
+                                if (item.actor != null)
+                                {
+                                    try
+                                    {
+                                        if (item.actor is OdeCharacter)
+                                            ((OdeCharacter)item.actor).DoAChange(item.what, item.arg);
+                                        else if (((OdePrim)item.actor).DoAChange(item.what, item.arg))
+                                            RemovePrimThreadLocked((OdePrim)item.actor);
+                                    }
+                                    catch
+                                    {
+                                        m_log.WarnFormat("[PHYSICS]: doChange failed for a actor {0} {1}",
+                                            item.actor.Name, item.what.ToString());
+                                    }
+                                }
+                                ttmp = Util.EnvironmentTickCountSubtract(changestartMS);
+                                if (ttmp > 20)
+                                    break;
+                            }
+                        }
+
+                        // Move characters
+                        lock (_characters)
+                        {
+                            List<OdeCharacter> defects = new List<OdeCharacter>();
+                            foreach (OdeCharacter actor in _characters)
+                            {
+                                if (actor != null)
+                                    actor.Move(defects);
+                            }
+                            if (defects.Count != 0)
+                            {
+                                foreach (OdeCharacter defect in defects)
+                                {
+                                    RemoveCharacter(defect);
+                                }
+                                defects.Clear();
+                            }
+                        }
+
+                        // Move other active objects
+                        lock (_activegroups)
+                        {
+                            foreach (OdePrim aprim in _activegroups)
+                            {
+                                aprim.Move();
+                            }
+                        }
+
+                        //if ((framecount % m_randomizeWater) == 0)
+                        // randomizeWater(waterlevel);
+
+                        m_rayCastManager.ProcessQueuedRequests();
+
+                        collision_optimized();
+
+                        foreach (PhysicsActor obj in _collisionEventPrim)
+                        {
+                            if (obj == null)
+                                continue;
+
+                            switch ((ActorTypes)obj.PhysicsActorType)
+                            {
+                                case ActorTypes.Agent:
+                                    OdeCharacter cobj = (OdeCharacter)obj;
+                                    cobj.AddCollisionFrameTime((int)(odetimestepMS));
+                                    cobj.SendCollisions();
+                                    break;
+
+                                case ActorTypes.Prim:
+                                    OdePrim pobj = (OdePrim)obj;
+                                    if (pobj.Body == IntPtr.Zero || (d.BodyIsEnabled(pobj.Body) && !pobj.m_outbounds))
+                                    if (!pobj.m_outbounds)
+                                    {
+                                        pobj.AddCollisionFrameTime((int)(odetimestepMS));
+                                        pobj.SendCollisions();
+                                    }
+                                    break;
+                            }
+                        }
+
+                        foreach (PhysicsActor obj in _collisionEventPrimRemove)
+                            _collisionEventPrim.Remove(obj);
+
+                        _collisionEventPrimRemove.Clear();
+
+                        // do a ode simulation step
+                        d.WorldQuickStep(world, ODE_STEPSIZE);
+                        d.JointGroupEmpty(contactgroup);
+
+                        // update managed ideia of physical data and do updates to core
+        /*
+                        lock (_characters)
+                        {
+                            foreach (OdeCharacter actor in _characters)
+                            {
+                                if (actor != null)
+                                {
+                                    if (actor.bad)
+                                        m_log.WarnFormat("[PHYSICS]: BAD Actor {0} in _characters list was not removed?", actor.m_uuid);
+
+                                    actor.UpdatePositionAndVelocity();
+                                }
+                            }
+                        }
+        */
+
+                        lock (_activegroups)
+                        {
+                            {
+                                foreach (OdePrim actor in _activegroups)
+                                {
+                                    if (actor.IsPhysical)
+                                    {
+                                        actor.UpdatePositionAndVelocity();
+                                    }
+                                }
+                            }
+                        }
+                    }
+                    catch (Exception e)
+                    {
+                        m_log.ErrorFormat("[PHYSICS]: {0}, {1}, {2}", e.Message, e.TargetSite, e);
+//                        ode.dunlock(world);
+                    }
+
+                    step_time -= ODE_STEPSIZE;
+                    nodeframes++;
+
+                    looptimeMS = Util.EnvironmentTickCountSubtract(loopstartMS);
+                    if (looptimeMS > 100)
+                        break;
+                }
+
+                lock (_badCharacter)
+                {
+                    if (_badCharacter.Count > 0)
+                    {
+                        foreach (OdeCharacter chr in _badCharacter)
+                        {
+                            RemoveCharacter(chr);
+                        }
+
+                        _badCharacter.Clear();
+                    }
+                }
+
+                timedif = now - m_lastMeshExpire;
+
+                if (timedif.Seconds > 10)
+                {
+                    mesher.ExpireReleaseMeshs();
+                    m_lastMeshExpire = now;
+                }
+
+// information block running in debug only
+/*
+                int ntopactivegeoms = d.SpaceGetNumGeoms(ActiveSpace);
+                int ntopstaticgeoms = d.SpaceGetNumGeoms(StaticSpace);
+                int ngroundgeoms = d.SpaceGetNumGeoms(GroundSpace);
+
+                int nactivegeoms = 0;
+                int nactivespaces = 0;
+
+                int nstaticgeoms = 0;
+                int nstaticspaces = 0;
+                IntPtr sp;
+
+                for (int i = 0; i < ntopactivegeoms; i++)
+                {
+                    sp = d.SpaceGetGeom(ActiveSpace, i);
+                    if (d.GeomIsSpace(sp))
+                    {
+                        nactivespaces++;
+                        nactivegeoms += d.SpaceGetNumGeoms(sp);
+                    }
+                    else
+                        nactivegeoms++;
+                }
+
+                for (int i = 0; i < ntopstaticgeoms; i++)
+                {
+                    sp = d.SpaceGetGeom(StaticSpace, i);
+                    if (d.GeomIsSpace(sp))
+                    {
+                        nstaticspaces++;
+                        nstaticgeoms += d.SpaceGetNumGeoms(sp);
+                    }
+                    else
+                        nstaticgeoms++;
+                }
+
+                int ntopgeoms = d.SpaceGetNumGeoms(TopSpace);
+
+                int totgeoms = nstaticgeoms + nactivegeoms + ngroundgeoms + 1; // one ray
+                int nbodies = d.NTotalBodies;
+                int ngeoms = d.NTotalGeoms;
+*/
+                // 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 as to do with dinamic step size that we dont' have
+                // even so tell something to world
+                if (looptimeMS < 100) // we did the requested loops
+                    m_timeDilation = 1.0f;
+                else if (step_time > 0)
+                {
+                    m_timeDilation = timeStep / step_time;
+                    if (m_timeDilation > 1)
+                        m_timeDilation = 1;
+                    if (step_time > m_SkipFramesAtms)
+                        step_time = 0;
+                    m_lastframe = DateTime.UtcNow; // skip also the time lost
+                }
+            }
+
+//            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); }
+        }
+
+        public float GetTerrainHeightAtXY(float x, float y)
+        {
+
+
+            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 + 3; // map size see setterrain number of samples
+
+            if (OdeUbitLib)
+            {
+                if (x < regsize - 1)
+                {
+                    ix = (int)x;
+                    dx = x - (float)ix;
+                }
+                else // out world use external height
+                {
+                    ix = regsize - 2;
+                    dx = 0;
+                }
+                if (y < regsize - 1)
+                {
+                    iy = (int)y;
+                    dy = y - (float)iy;
+                }
+                else
+                {
+                    iy = regsize - 2;
+                    dy = 0;
+                }
+            }
+
+            else
+            {
+                // we  still have square fixed size regions
+                // also flip x and y because of how map is done for ODE fliped axis
+                // so ix,iy,dx and dy are inter exchanged
+                if (x < regsize - 1)
+                {
+                    iy = (int)x;
+                    dy = x - (float)iy;
+                }
+                else // out world use external height
+                {
+                    iy = regsize - 2;
+                    dy = 0;
+                }
+                if (y < regsize - 1)
+                {
+                    ix = (int)y;
+                    dx = y - (float)ix;
+                }
+                else
+                {
+                    ix = regsize - 2;
+                    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
+                        }
+            */
+            h0 = ((float)heights[iy]); // 0,0 vertice
+
+            if ((dy > dx))
+            {
+                iy += regsize;
+                h2 = (float)heights[iy]; // 0,1 vertice
+                h1 = (h2 - h0) * dy; // 0,1 vertice minus 0,0
+                h2 = ((float)heights[iy + 1] - h2) * dx; // 1,1 vertice minus 0,1
+            }
+            else
+            {
+                iy++;
+                h2 = (float)heights[iy]; // vertice 1,0
+                h1 = (h2 - h0) * dx; // 1,0 vertice minus 0,0
+                h2 = (((float)heights[iy + regsize]) - h2) * dy; // 1,1 vertice minus 1,0
+            }
+
+            return h0 + h1 + h2;
+        }
+
+
+        public 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)
+        {
+            if (OdeUbitLib)
+                UbitSetTerrain(heightMap, pOffset);
+            else
+                OriSetTerrain(heightMap, pOffset);
+        }
+
+        public void OriSetTerrain(float[] heightMap, Vector3 pOffset)
+        {
+            // assumes 1m size grid and constante size square regions
+            // needs to know about sims around in future
+
+            float[] _heightmap;
+
+            uint heightmapWidth = Constants.RegionSize + 2;
+            uint heightmapHeight = Constants.RegionSize + 2;
+
+            uint heightmapWidthSamples = heightmapWidth + 1;
+            uint heightmapHeightSamples = heightmapHeight + 1;
+
+            _heightmap = new float[heightmapWidthSamples * heightmapHeightSamples];
+
+            const float scale = 1.0f;
+            const float offset = 0.0f;
+            const float thickness = 10f;
+            const int wrap = 0;
+
+            uint regionsize = Constants.RegionSize;
+ 
+            float hfmin = float.MaxValue;
+            float hfmax = float.MinValue;
+            float val;
+            uint xx;
+            uint yy;
+
+            uint maxXXYY = regionsize - 1;
+            // flipping map adding one margin all around so things don't fall in edges
+
+            uint xt = 0;
+            xx = 0;
+
+            for (uint x = 0; x < heightmapWidthSamples; x++)
+            {
+                if (x > 1 && xx < maxXXYY)
+                    xx++;
+                yy = 0;
+                for (uint y = 0; y < heightmapHeightSamples; y++)
+                {
+                    if (y > 1 && y < maxXXYY)
+                        yy += regionsize;
+
+                    val = heightMap[yy + xx];
+                    if (val < 0.0f)
+                        val = 0.0f; // no neg terrain as in chode
+                    _heightmap[xt + y] = val;
+
+                    if (hfmin > val)
+                        hfmin = val;
+                    if (hfmax < val)
+                        hfmax = val;
+                }
+                xt += heightmapHeightSamples;
+            }
+            lock (OdeLock)
+            {
+                IntPtr GroundGeom = IntPtr.Zero;
+                if (RegionTerrain.TryGetValue(pOffset, out GroundGeom))
+                {
+                    RegionTerrain.Remove(pOffset);
+                    if (GroundGeom != IntPtr.Zero)
+                    {
+                        actor_name_map.Remove(GroundGeom);
+                        d.GeomDestroy(GroundGeom);
+
+                        if (TerrainHeightFieldHeights.ContainsKey(GroundGeom))
+                            {
+                            TerrainHeightFieldHeightsHandlers[GroundGeom].Free();
+                            TerrainHeightFieldHeightsHandlers.Remove(GroundGeom);
+                            TerrainHeightFieldHeights.Remove(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(GroundSpace, HeightmapData, 1);
+
+                if (GroundGeom != IntPtr.Zero)
+                {
+                    d.GeomSetCategoryBits(GroundGeom, (uint)(CollisionCategories.Land));
+                    d.GeomSetCollideBits(GroundGeom, 0);
+
+                    PhysicsActor pa = new NullPhysicsActor();
+                    pa.Name = "Terrain";
+                    pa.PhysicsActorType = (int)ActorTypes.Ground;
+                    actor_name_map[GroundGeom] = pa;
+
+//                    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, pOffset.Y + (float)Constants.RegionSize * 0.5f, 0);
+                    RegionTerrain.Add(pOffset, GroundGeom);
+                    TerrainHeightFieldHeights.Add(GroundGeom, _heightmap);
+                    TerrainHeightFieldHeightsHandlers.Add(GroundGeom, _heightmaphandler);
+                }
+            }
+        }
+
+        public void UbitSetTerrain(float[] heightMap, Vector3 pOffset)
+        {
+            // assumes 1m size grid and constante size square regions
+            // needs to know about sims around in future
+
+            float[] _heightmap;
+
+            uint heightmapWidth = Constants.RegionSize + 2;
+            uint heightmapHeight = Constants.RegionSize + 2;
+
+            uint heightmapWidthSamples = heightmapWidth + 1;
+            uint heightmapHeightSamples = heightmapHeight + 1;
+
+            _heightmap = new float[heightmapWidthSamples * heightmapHeightSamples];
+
+
+            uint regionsize = Constants.RegionSize;
+
+            float hfmin = float.MaxValue;
+//            float hfmax = float.MinValue;
+            float val;
+
+
+            uint maxXXYY = regionsize - 1;
+            // adding one margin all around so things don't fall in edges
+
+            uint xx;
+            uint yy = 0;
+            uint yt = 0;
+
+            for (uint y = 0; y < heightmapHeightSamples; y++)
+            {
+                if (y > 1 && y < maxXXYY)
+                    yy += regionsize;
+                xx = 0;
+                for (uint x = 0; x < heightmapWidthSamples; x++)
+                {
+                    if (x > 1 && x < maxXXYY)
+                        xx++;
+
+                    val = heightMap[yy + xx];
+                    if (val < 0.0f)
+                        val = 0.0f; // no neg terrain as in chode
+                    _heightmap[yt + x] = val;
+
+                    if (hfmin > val)
+                        hfmin = val;
+//                    if (hfmax < val)
+//                        hfmax = val;
+                }
+                yt += heightmapWidthSamples;
+            }
+            lock (OdeLock)
+            {
+                IntPtr GroundGeom = IntPtr.Zero;
+                if (RegionTerrain.TryGetValue(pOffset, out GroundGeom))
+                {
+                    RegionTerrain.Remove(pOffset);
+                    if (GroundGeom != IntPtr.Zero)
+                    {
+                        actor_name_map.Remove(GroundGeom);
+                        d.GeomDestroy(GroundGeom);
+
+                        if (TerrainHeightFieldHeights.ContainsKey(GroundGeom))
+                        {
+                            if (TerrainHeightFieldHeightsHandlers[GroundGeom].IsAllocated)
+                                TerrainHeightFieldHeightsHandlers[GroundGeom].Free();
+                            TerrainHeightFieldHeightsHandlers.Remove(GroundGeom);
+                            TerrainHeightFieldHeights.Remove(GroundGeom);
+                        }
+                    }
+                }
+                IntPtr HeightmapData = d.GeomUbitTerrainDataCreate();
+
+                const int wrap = 0;
+                float thickness = hfmin;
+                if (thickness < 0)
+                    thickness = 1;
+
+                GCHandle _heightmaphandler = GCHandle.Alloc(_heightmap, GCHandleType.Pinned);
+
+                d.GeomUbitTerrainDataBuild(HeightmapData, _heightmaphandler.AddrOfPinnedObject(), 0, 1.0f,
+                                                 (int)heightmapWidthSamples, (int)heightmapHeightSamples,
+                                                 thickness, wrap);
+
+//                d.GeomUbitTerrainDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1);
+                GroundGeom = d.CreateUbitTerrain(GroundSpace, HeightmapData, 1);
+                if (GroundGeom != IntPtr.Zero)
+                {
+                    d.GeomSetCategoryBits(GroundGeom, (uint)(CollisionCategories.Land));
+                    d.GeomSetCollideBits(GroundGeom, 0);
+
+
+                    PhysicsActor pa = new NullPhysicsActor();
+                    pa.Name = "Terrain";
+                    pa.PhysicsActorType = (int)ActorTypes.Ground;
+                    actor_name_map[GroundGeom] = pa;
+
+//                    geom_name_map[GroundGeom] = "Terrain";
+
+                    d.GeomSetPosition(GroundGeom, pOffset.X + (float)Constants.RegionSize * 0.5f, pOffset.Y + (float)Constants.RegionSize * 0.5f, 0);
+                    RegionTerrain.Add(pOffset, GroundGeom);
+                    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 = Constants.RegionSize + 2;
+            const uint heightmapHeight = Constants.RegionSize + 2;
+            const uint heightmapWidthSamples = heightmapWidth + 1;
+            const uint heightmapHeightSamples = heightmapHeight + 1;
+
+            const float scale = 1.0f;
+            const float offset = 0.0f;
+            const int wrap = 0;
+
+            float[] _watermap = new float[heightmapWidthSamples * heightmapWidthSamples];
+
+            float maxheigh = float.MinValue;
+            float minheigh = float.MaxValue;
+            float val;
+            for (int i = 0; i < (heightmapWidthSamples * heightmapHeightSamples); i++)
+            {
+
+                val = (baseheight - 0.1f) + ((float)fluidRandomizer.Next(1, 9) / 10f);
+                _watermap[i] = val;
+                if (maxheigh < val)
+                    maxheigh = val;
+                if (minheigh > val)
+                    minheigh = val;
+            }
+
+            float thickness = minheigh;
+
+            lock (OdeLock)
+            {
+                if (WaterGeom != IntPtr.Zero)
+                {
+                    actor_name_map.Remove(WaterGeom);
+                    d.GeomDestroy(WaterGeom);
+                    d.GeomHeightfieldDataDestroy(WaterHeightmapData);
+                    WaterGeom = IntPtr.Zero;
+                    WaterHeightmapData = IntPtr.Zero;
+                    if(WaterMapHandler.IsAllocated)
+                        WaterMapHandler.Free();
+                }
+
+                WaterHeightmapData = d.GeomHeightfieldDataCreate();
+
+                WaterMapHandler = GCHandle.Alloc(_watermap, GCHandleType.Pinned);
+
+                d.GeomHeightfieldDataBuildSingle(WaterHeightmapData, WaterMapHandler.AddrOfPinnedObject(), 0, heightmapWidth, heightmapHeight,
+                                                 (int)heightmapWidthSamples, (int)heightmapHeightSamples, scale,
+                                                 offset, thickness, wrap);
+                d.GeomHeightfieldDataSetBounds(WaterHeightmapData, minheigh, maxheigh);
+                WaterGeom = d.CreateHeightfield(StaticSpace, WaterHeightmapData, 1);
+                if (WaterGeom != IntPtr.Zero)
+                {
+                    d.GeomSetCategoryBits(WaterGeom, (uint)(CollisionCategories.Water));
+                    d.GeomSetCollideBits(WaterGeom, 0);
+
+
+                    PhysicsActor pa = new NullPhysicsActor();
+                    pa.Name = "Water";
+                    pa.PhysicsActorType = (int)ActorTypes.Water;
+
+                    actor_name_map[WaterGeom] = pa;
+//                    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, (float)Constants.RegionSize * 0.5f, (float)Constants.RegionSize * 0.5f, 0);
+                }
+            }
+        }
+*/
+        public override void Dispose()
+        {
+            if (m_meshWorker != null)
+                m_meshWorker.Stop();
+
+            lock (OdeLock)
+            {
+                m_rayCastManager.Dispose();
+                m_rayCastManager = null;
+
+                lock (_prims)
+                {
+                    ChangesQueue.Clear();
+                    foreach (OdePrim prm in _prims)
+                    {
+                        prm.DoAChange(changes.Remove, null);
+                        _collisionEventPrim.Remove(prm);
+                    }
+                    _prims.Clear();
+                }
+
+                OdeCharacter[] chtorem;
+                lock (_characters)
+                {
+                    chtorem = new OdeCharacter[_characters.Count];
+                    _characters.CopyTo(chtorem);
+                }
+
+                ChangesQueue.Clear();
+                foreach (OdeCharacter ch in chtorem)
+                    ch.DoAChange(changes.Remove, null);
+
+                               
+                foreach (IntPtr GroundGeom in RegionTerrain.Values)
+                {
+                    if (GroundGeom != IntPtr.Zero)
+                        d.GeomDestroy(GroundGeom);
+                }
+
+
+                RegionTerrain.Clear();
+
+                if (TerrainHeightFieldHeightsHandlers.Count > 0)
+                {
+                    foreach (GCHandle gch in TerrainHeightFieldHeightsHandlers.Values)
+                    {
+                        if (gch.IsAllocated)
+                            gch.Free();
+                    }
+                }
+
+                TerrainHeightFieldHeightsHandlers.Clear();
+                TerrainHeightFieldHeights.Clear();
+/*
+                if (WaterGeom != IntPtr.Zero)
+                {
+                    d.GeomDestroy(WaterGeom);
+                        WaterGeom = IntPtr.Zero;
+                    if (WaterHeightmapData != IntPtr.Zero)
+                        d.GeomHeightfieldDataDestroy(WaterHeightmapData);
+                    WaterHeightmapData = IntPtr.Zero;
+
+                    if (WaterMapHandler.IsAllocated)
+                        WaterMapHandler.Free();
+                }
+*/
+                if (ContactgeomsArray != IntPtr.Zero)
+                    Marshal.FreeHGlobal(ContactgeomsArray);
+                if (GlobalContactsArray != IntPtr.Zero)
+                    Marshal.FreeHGlobal(GlobalContactsArray);
+
+
+                d.WorldDestroy(world);
+                world = IntPtr.Zero;
+                //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.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)
+            {
+                ODERayRequest req = new ODERayRequest();
+                req.geom = IntPtr.Zero;
+                req.callbackMethod = retMethod;
+                req.length = length;
+                req.Normal = direction;
+                req.Origin = position;
+                req.Count = 0;
+                req.filter = RayFilterFlags.AllPrims;
+
+                m_rayCastManager.QueueRequest(req);
+            }
+        }
+
+        public override void RaycastWorld(Vector3 position, Vector3 direction, float length, int Count, RayCallback retMethod)
+        {
+            if (retMethod != null)
+            {
+                ODERayRequest req = new ODERayRequest();
+                req.geom = IntPtr.Zero;
+                req.callbackMethod = retMethod;
+                req.length = length;
+                req.Normal = direction;
+                req.Origin = position;
+                req.Count = Count;
+                req.filter = RayFilterFlags.AllPrims;
+
+                m_rayCastManager.QueueRequest(req);
+            }
+        }
+
+       
+        public override List<ContactResult> RaycastWorld(Vector3 position, Vector3 direction, float length, int Count)
+        {
+            List<ContactResult> ourresults = new List<ContactResult>();
+            object SyncObject = new object();
+
+            RayCallback retMethod = delegate(List<ContactResult> results)
+            {
+                lock (SyncObject)
+                {
+                    ourresults = results;
+                    Monitor.PulseAll(SyncObject);
+                }
+            };
+
+            ODERayRequest req = new ODERayRequest();
+            req.geom = IntPtr.Zero;
+            req.callbackMethod = retMethod;
+            req.length = length;
+            req.Normal = direction;
+            req.Origin = position;
+            req.Count = Count;
+            req.filter = RayFilterFlags.AllPrims;
+
+            lock (SyncObject)
+            {
+                m_rayCastManager.QueueRequest(req);
+                if (!Monitor.Wait(SyncObject, 500))
+                    return null;
+                else
+                    return ourresults;
+            }
+        }
+
+        public override bool SuportsRaycastWorldFiltered()
+        {
+            return true;
+        }
+
+        public override object RaycastWorld(Vector3 position, Vector3 direction, float length, int Count, RayFilterFlags filter)
+        {
+            object SyncObject = new object();
+            List<ContactResult> ourresults = new List<ContactResult>();
+
+            RayCallback retMethod = delegate(List<ContactResult> results)
+            {
+                lock (SyncObject)
+                {
+                    ourresults = results;
+                    Monitor.PulseAll(SyncObject);
+                }
+            };
+
+            ODERayRequest req = new ODERayRequest();
+            req.geom = IntPtr.Zero;
+            req.callbackMethod = retMethod;
+            req.length = length;
+            req.Normal = direction;
+            req.Origin = position;
+            req.Count = Count;
+            req.filter = filter;
+
+            lock (SyncObject)
+            {
+                m_rayCastManager.QueueRequest(req);
+                if (!Monitor.Wait(SyncObject, 500))
+                    return null;
+                else
+                    return ourresults;
+            }
+        }
+
+        public override List<ContactResult> RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count, RayFilterFlags flags)
+        {
+            if (actor == null)
+                return new List<ContactResult>();
+
+            IntPtr geom;
+            if (actor is OdePrim)
+                geom = ((OdePrim)actor).prim_geom;
+            else if (actor is OdeCharacter)
+                geom = ((OdePrim)actor).prim_geom;
+            else
+                return new List<ContactResult>();
+
+            if (geom == IntPtr.Zero)
+                return new List<ContactResult>();
+
+            List<ContactResult> ourResults = null;
+            object SyncObject = new object();
+
+            RayCallback retMethod = delegate(List<ContactResult> results)
+            {
+                lock (SyncObject)
+                {
+                    ourResults = results;
+                    Monitor.PulseAll(SyncObject);
+                }
+            };
+
+            ODERayRequest req = new ODERayRequest();
+            req.geom = geom;
+            req.callbackMethod = retMethod;
+            req.length = length;
+            req.Normal = direction;
+            req.Origin = position;
+            req.Count = Count;
+            req.filter = flags;
+
+            lock (SyncObject)
+            {
+                m_rayCastManager.QueueRequest(req);
+                if (!Monitor.Wait(SyncObject, 500))
+                    return new List<ContactResult>();
+            }
+
+            if (ourResults == null)
+                return new List<ContactResult>();
+            return ourResults;
+        }
+
+        public override List<ContactResult> BoxProbe(Vector3 position, Vector3 size, Quaternion orientation, int Count, RayFilterFlags flags)
+        {
+            List<ContactResult> ourResults = null;
+            object SyncObject = new object();
+
+            ProbeBoxCallback retMethod = delegate(List<ContactResult> results)
+            {
+                lock (SyncObject)
+                {
+                    ourResults = results;
+                    Monitor.PulseAll(SyncObject);
+                }
+            };
+
+            ODERayRequest req = new ODERayRequest();
+            req.geom = IntPtr.Zero;
+            req.callbackMethod = retMethod;
+            req.Normal = size;
+            req.Origin = position;
+            req.orientation = orientation;
+            req.Count = Count;
+            req.filter = flags;
+
+            lock (SyncObject)
+            {
+                m_rayCastManager.QueueRequest(req);
+                if (!Monitor.Wait(SyncObject, 500))
+                    return new List<ContactResult>();
+            }
+
+            if (ourResults == null)
+                return new List<ContactResult>();
+            return ourResults;
+        }
+
+        public override List<ContactResult> SphereProbe(Vector3 position, float radius, int Count, RayFilterFlags flags)
+        {
+            List<ContactResult> ourResults = null;
+            object SyncObject = new object();
+
+            ProbeSphereCallback retMethod = delegate(List<ContactResult> results)
+            {
+                ourResults = results;
+                Monitor.PulseAll(SyncObject);
+            };
+
+            ODERayRequest req = new ODERayRequest();
+            req.geom = IntPtr.Zero;
+            req.callbackMethod = retMethod;
+            req.length = radius;
+            req.Origin = position;
+            req.Count = Count;
+            req.filter = flags;
+
+
+            lock (SyncObject)
+            {
+                m_rayCastManager.QueueRequest(req);
+                if (!Monitor.Wait(SyncObject, 500))
+                    return new List<ContactResult>();
+            }
+
+            if (ourResults == null)
+                return new List<ContactResult>();
+            return ourResults;
+        }
+
+        public override List<ContactResult> PlaneProbe(PhysicsActor actor, Vector4 plane, int Count, RayFilterFlags flags)
+        {
+            IntPtr geom = IntPtr.Zero;;
+
+            if (actor != null)
+            {
+                if (actor is OdePrim)
+                    geom = ((OdePrim)actor).prim_geom;
+                else if (actor is OdeCharacter)
+                    geom = ((OdePrim)actor).prim_geom;
+            }
+
+            List<ContactResult> ourResults = null;
+            object SyncObject = new object();
+
+            ProbePlaneCallback retMethod = delegate(List<ContactResult> results)
+            {
+                ourResults = results;
+                Monitor.PulseAll(SyncObject);
+            };
+
+            ODERayRequest req = new ODERayRequest();
+            req.geom = geom;
+            req.callbackMethod = retMethod;
+            req.length = plane.W;
+            req.Normal.X = plane.X;
+            req.Normal.Y = plane.Y;
+            req.Normal.Z = plane.Z;
+            req.Count = Count;
+            req.filter = flags;
+
+            lock (SyncObject)
+            {
+                m_rayCastManager.QueueRequest(req);
+                if (!Monitor.Wait(SyncObject, 500))
+                    return new List<ContactResult>();
+            }
+
+            if (ourResults == null)
+                return new List<ContactResult>();
+            return ourResults;
+        }
+
+        public override int SitAvatar(PhysicsActor actor, Vector3 AbsolutePosition, Vector3 CameraPosition, Vector3 offset, Vector3 AvatarSize, SitAvatarCallback PhysicsSitResponse)
+        {
+            Util.FireAndForget( delegate
+            {
+                ODESitAvatar sitAvatar = new ODESitAvatar(this, m_rayCastManager);
+                if(sitAvatar != null)
+                    sitAvatar.Sit(actor, AbsolutePosition, CameraPosition, offset, AvatarSize, PhysicsSitResponse);
+            });
+            return 1;
+        }
+
+    }
+}