6 files changed, 496 insertions, 641 deletions

diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
index 80c1277..0a4ebe4 100644
--- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
+++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
@@ -704,6 +704,8 @@ namespace OpenSim.Region.Physics.OdePlugin
             if (m_isphysical)
                 m_targetSpace = _parent_scene.space;
 
+            _triMeshData = IntPtr.Zero;
+
             m_primName = primName;
             m_taintserial = null;
             m_taintadd = true;
@@ -773,6 +775,8 @@ namespace OpenSim.Region.Physics.OdePlugin
                     m_targetSpace = _parent_scene.space;
             }
 
+            _triMeshData = IntPtr.Zero;
+
             m_taintserial = null;
             m_primName = primName;
             m_taintadd = true;
@@ -1762,7 +1766,7 @@ namespace OpenSim.Region.Physics.OdePlugin
 
         private static Dictionary<IMesh, IntPtr> m_MeshToTriMeshMap = new Dictionary<IMesh, IntPtr>();
 
-        public void setMesh(OdeScene parent_scene, IMesh mesh)
+        public bool setMesh(OdeScene parent_scene, IMesh mesh)
         {
             // This sleeper is there to moderate how long it takes between
             // setting up the mesh and pre-processing it when we get rapid fire mesh requests on a single object
@@ -1785,24 +1789,48 @@ namespace OpenSim.Region.Physics.OdePlugin
                     disableBody();
                 }
             }
+
+// do it on caller instead
+/*
+            if (_triMeshData != IntPtr.Zero)
+            {
+                d.GeomTriMeshDataDestroy(_triMeshData);
+                _triMeshData = IntPtr.Zero;
+            }
+*/
             IntPtr vertices, indices;
             int vertexCount, indexCount;
             int vertexStride, triStride;
             mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); // Note, that vertices are fixed in unmanaged heap
             mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); // Also fixed, needs release after usage
 
+            // warning this destroys the mesh for eventual future use. Only pinned float arrays stay valid
             mesh.releaseSourceMeshData(); // free up the original mesh data to save memory
+
+            if (vertexCount == 0 || indexCount == 0)
+            {
+                m_log.WarnFormat("[PHYSICS]: Got invalid mesh on prim {0} at <{1},{2},{3}>. It can be a sculp with alpha channel in map. Replacing it by a small box.", Name, _position.X, _position.Y, _position.Z);
+                _size.X = 0.05f;
+                _size.Y = 0.05f;
+                _size.Z = 0.05f;
+                return false;
+            }
+
+/*
             if (m_MeshToTriMeshMap.ContainsKey(mesh))
             {
                 _triMeshData = m_MeshToTriMeshMap[mesh];
             }
             else
+*/
+
+
             {
                 _triMeshData = d.GeomTriMeshDataCreate();
 
                 d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride);
                 d.GeomTriMeshDataPreprocess(_triMeshData);
-                m_MeshToTriMeshMap[mesh] = _triMeshData;
+//                m_MeshToTriMeshMap[mesh] = _triMeshData;
             }
 
             _parent_scene.waitForSpaceUnlock(m_targetSpace);
@@ -1810,13 +1838,23 @@ namespace OpenSim.Region.Physics.OdePlugin
             {
                 //    if (prim_geom == IntPtr.Zero)  // setGeom takes care of phys engine recreate and prim_geom pointer 
                 //    {
-                SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, parent_scene.triCallback, null, null));
+                //                SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, parent_scene.triCallback, null, null));
+                SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, null, null, null));
                 //    }
             }
-            catch (AccessViolationException)
+            catch (Exception e)
             {
-                m_log.Error("[PHYSICS]: MESH LOCKED");
-                return;
+                m_log.ErrorFormat("[PHYSICS]: Create trimesh failed on prim {0} : {1}",Name,e.Message);
+
+                if (_triMeshData != IntPtr.Zero)
+                {
+                    d.GeomTriMeshDataDestroy(_triMeshData);
+                    _triMeshData = IntPtr.Zero;
+                }
+                _size.X = 0.05f;
+                _size.Y = 0.05f;
+                _size.Z = 0.05f;
+                return false;
             }
 
 
@@ -1828,6 +1866,7 @@ namespace OpenSim.Region.Physics.OdePlugin
 
             //     enableBody();
             // }
+            return true;
         }
 
         public void ProcessTaints(float timestep) //=============================================================================
@@ -1837,6 +1876,9 @@ namespace OpenSim.Region.Physics.OdePlugin
                 changeadd(timestep);
             }
 
+            if (m_taintremove)
+                return;
+
             if (prim_geom != IntPtr.Zero)
             {
                 if (!_position.ApproxEquals(m_taintposition, 0f))
@@ -2286,75 +2328,62 @@ namespace OpenSim.Region.Physics.OdePlugin
 
         public void CreateGeom(IntPtr m_targetSpace, IMesh _mesh)
         {
+            bool gottrimesh = false;
+
+            if (_triMeshData != IntPtr.Zero)
+            {
+                d.GeomTriMeshDataDestroy(_triMeshData);
+                _triMeshData = IntPtr.Zero;
+            }
+
             if (_mesh != null)                                  // Special - make mesh
             {
-                setMesh(_parent_scene, _mesh);
+                gottrimesh = setMesh(_parent_scene, _mesh);
             }
-            else                                                // not a mesh
+
+            if (!gottrimesh)    // not a mesh
             {
-                if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1)           // special profile??
+                IntPtr geo = IntPtr.Zero;
+
+                if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1
+                    && _size.X == _size.Y && _size.X == _size.Z)
                 {
-                    if (_size.X == _size.Y && _size.Y == _size.Z && _size.X == _size.Z)                                 // Equi-size
+                    // its a sphere
+                    _parent_scene.waitForSpaceUnlock(m_targetSpace);
+                    try
                     {
-                        if (((_size.X / 2f) > 0f))                                                                      // Has size
-                        {
-                            _parent_scene.waitForSpaceUnlock(m_targetSpace);
-                            try
-                            {
-                                SetGeom(d.CreateSphere(m_targetSpace, _size.X / 2));
-                            }
-                            catch (AccessViolationException)
-                            {
-                                m_log.Warn("[PHYSICS]: Unable to create physics proxy for object");
-                                ode.dunlock(_parent_scene.world);
-                                return;
-                            }
-                        }
-                        else
-                        {
-                            _parent_scene.waitForSpaceUnlock(m_targetSpace);
-                            try
-                            {
-                                SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
-                            }
-                            catch (AccessViolationException)
-                            {
-                                m_log.Warn("[PHYSICS]: Unable to create physics proxy for object");
-                                ode.dunlock(_parent_scene.world);
-                                return;
-                            }
-                        }
+                        geo = d.CreateSphere(m_targetSpace, _size.X * 0.5f);
                     }
-                    else                                                                                                // not equi-size
+                    catch (Exception e)
                     {
-                        _parent_scene.waitForSpaceUnlock(m_targetSpace);
-                        try
-                        {
-                            SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
-                        }
-                        catch (AccessViolationException)
-                        {
-                            m_log.Warn("[PHYSICS]: Unable to create physics proxy for object");
-                            ode.dunlock(_parent_scene.world);
-                            return;
-                        }
+                        m_log.WarnFormat("[PHYSICS]: Unable to create basic sphere for object {0}", e.Message);
+                        geo = IntPtr.Zero;
+                        ode.dunlock(_parent_scene.world);
                     }
                 }
-
-                else                                                                                    // not special profile
+                else // make it a box
                 {
                     _parent_scene.waitForSpaceUnlock(m_targetSpace);
                     try
                     {
-                        SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
+                        geo = d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z);
                     }
-                    catch (AccessViolationException)
+                    catch (Exception e)
                     {
-                        m_log.Warn("[PHYSICS]: Unable to create physics proxy for object");
+                        m_log.WarnFormat("[PHYSICS]: Unable to create basic sphere for object {0}", e.Message);
+                        geo = IntPtr.Zero;
                         ode.dunlock(_parent_scene.world);
-                        return;
                     }
                 }
+
+                if (geo == IntPtr.Zero)
+                {
+                    m_taintremove = true;
+                    _parent_scene.AddPhysicsActorTaint(this);
+                    return;
+                }
+
+                SetGeom(geo);
             }
         }
 
@@ -2372,18 +2401,17 @@ namespace OpenSim.Region.Physics.OdePlugin
             {
                 if (_parent_scene.needsMeshing(_pbs))
                 {
-                    // Don't need to re-enable body..   it's done in SetMesh
                     try
                     {
-                        _mesh = _parent_scene.mesher.CreateMesh(m_primName, _pbs, _size, _parent_scene.meshSculptLOD, IsPhysical);
+                        _mesh = _parent_scene.mesher.CreateMesh(m_primName, _pbs, _size, (int)LevelOfDetail.High, true);
                     }
                     catch
                     {
                         //Don't continuously try to mesh prims when meshing has failed
                         m_meshfailed = true;
+                        _mesh = null;
+                        m_log.WarnFormat("[PHYSICS]: changeAdd CreateMesh fail on prim {0} at <{1},{2},{3}>", Name, _position.X, _position.Y, _position.Z);
                     }
-                    // createmesh returns null when it's a shape that isn't a cube.
-                    // m_log.Debug(m_localID);
                 }
             }
 
@@ -2630,17 +2658,17 @@ namespace OpenSim.Region.Physics.OdePlugin
                 try
                 {
                     if (_parent_scene.needsMeshing(_pbs))
-                        mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical);
+                        mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, (int)LevelOfDetail.High, true);
                 }
                 catch
                 {
                     m_meshfailed = true;
+                    mesh = null;
+                    m_log.WarnFormat("[PHYSICS]: changeSize CreateMesh fail on prim {0} at <{1},{2},{3}>", Name, _position.X, _position.Y, _position.Z);
                 }
 
                 //IMesh mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical);
                 CreateGeom(m_targetSpace, mesh);
-
-
             }
             else
             {
@@ -2732,18 +2760,23 @@ namespace OpenSim.Region.Physics.OdePlugin
             {
                 // Don't need to re-enable body..   it's done in SetMesh
                 float meshlod = _parent_scene.meshSculptLOD;
+                IMesh mesh;
 
                 if (IsPhysical)
                     meshlod = _parent_scene.MeshSculptphysicalLOD;
                 try
                 {
-                    IMesh mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical);
-                    CreateGeom(m_targetSpace, mesh);
+                    mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, (int)LevelOfDetail.High, true);
                 }
                 catch
                 {
+                    mesh = null;
                     m_meshfailed = true;
+                    m_log.WarnFormat("[PHYSICS]: changeAdd CreateMesh fail on prim {0} at <{1},{2},{3}>", Name, _position.X, _position.Y, _position.Z);
                 }
+
+                CreateGeom(m_targetSpace, mesh);
+
                 // createmesh returns null when it doesn't mesh.
             }
             else
diff --git a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs
index 61fb2d0..7a1e671 100644
--- a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs
+++ b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs
@@ -1772,7 +1772,7 @@ namespace OpenSim.Region.Physics.OdePlugin
             IMesh mesh = null;
 
             if (needsMeshing(pbs))
-                mesh = mesher.CreateMesh(primName, pbs, size, 32f, isPhysical);
+                mesh = mesher.CreateMesh(primName, pbs, size, (int)LevelOfDetail.High, true);
 
             result = AddPrim(primName, position, size, rotation, mesh, pbs, isPhysical, localid);
 
@@ -2174,6 +2174,16 @@ namespace OpenSim.Region.Physics.OdePlugin
                     {
                         prim.ResetTaints();
 
+                        try
+                        {
+                            if (prim._triMeshData != IntPtr.Zero)
+                            {
+                                d.GeomTriMeshDataDestroy(prim._triMeshData);
+                                prim._triMeshData = IntPtr.Zero;
+                            }
+                        }
+                        catch { };
+
                         if (prim.IsPhysical)
                         {
                             prim.disableBody();
@@ -2185,7 +2195,6 @@ namespace OpenSim.Region.Physics.OdePlugin
                                 prim.IsPhysical = false;
                             }
 
-
                         }
                         // we don't want to remove the main space
 
@@ -2505,7 +2514,7 @@ namespace OpenSim.Region.Physics.OdePlugin
             }
 
             // if it's a standard box or sphere with no cuts, hollows, twist or top shear, return false since ODE can use an internal representation for the prim
-            if (!forceSimplePrimMeshing)
+            if (!forceSimplePrimMeshing && !pbs.SculptEntry)
             {
                 if ((pbs.ProfileShape == ProfileShape.Square && pbs.PathCurve == (byte)Extrusion.Straight)
                     || (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1
@@ -2528,6 +2537,9 @@ namespace OpenSim.Region.Physics.OdePlugin
                 }
             }
 
+            if (forceSimplePrimMeshing)
+                return true;
+
             if (pbs.ProfileHollow != 0)
                 iPropertiesNotSupportedDefault++;
 
@@ -2592,6 +2604,8 @@ namespace OpenSim.Region.Physics.OdePlugin
                 }
             }
 
+            if (pbs.SculptEntry && meshSculptedPrim)
+                iPropertiesNotSupportedDefault++;
 
             if (iPropertiesNotSupportedDefault == 0)
             {
@@ -3443,8 +3457,8 @@ namespace OpenSim.Region.Physics.OdePlugin
             int heightmapWidth = regionsize + 2;                                // ODE map size 257 x 257 (Meters) (1 extra 
             int heightmapHeight = regionsize + 2;
             
-            int heightmapWidthSamples = (int)regionsize + 3;            // Sample file size, 258 x 258 samples
-            int heightmapHeightSamples = (int)regionsize + 3;
+            int heightmapWidthSamples = (int)regionsize + 2;            // Sample file size, 258 x 258 samples
+            int heightmapHeightSamples = (int)regionsize + 2;
             
             // Array of height samples for ODE 
             float[] _heightmap;
@@ -3481,7 +3495,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                                 // Output x = 0  1  2  3  ..... 255  256  257      258 total out
                     float val= heightMap[(yy * regionsize) + xx];  // input from heightMap,  <0-255 * 256> <0-255>
                     if (val < minele) val = minele;
-                    _heightmap[x * (heightmapHeightSamples) + y] = val; // samples output to _heightmap,  <0-257 * 258> <0-257>
+                    _heightmap[x * (regionsize + 2) + y] = val; // samples output to _heightmap,  <0-257 * 258> <0-257>
                     hfmin = (val < hfmin) ? val : hfmin;
                     hfmax = (val > hfmax) ? val : hfmax;
                 }
@@ -3531,8 +3545,6 @@ namespace OpenSim.Region.Physics.OdePlugin
                 d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle);
                 d.GeomSetRotation(GroundGeom, ref R);
                 d.GeomSetPosition(GroundGeom, (pOffset.X + (regionsize * 0.5f)) - 0.5f, (pOffset.Y + (regionsize * 0.5f)) - 0.5f, 0);
-                // having nsamples = size + 1  center is actually at size/2
-                d.GeomSetPosition(GroundGeom, (pOffset.X + (regionsize * 0.5f)), (pOffset.Y + (regionsize * 0.5f)), 0);
                 IntPtr testGround = IntPtr.Zero;
                 if (RegionTerrain.TryGetValue(pOffset, out testGround))
                 {
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs
index 80218e7..c9d0909 100644
--- a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs
@@ -83,7 +83,6 @@ namespace OpenSim.Region.Physics.OdePlugin
         private Vector3 m_linearFrictionTimescale = new Vector3(1000, 1000, 1000);
         private float m_linearMotorDecayTimescale = 120;
         private float m_linearMotorTimescale = 1000;
-        private Vector3 m_lastLinearVelocityVector = Vector3.Zero;
         private Vector3 m_linearMotorOffset = Vector3.Zero;
 
         //Angular properties
@@ -91,7 +90,6 @@ namespace OpenSim.Region.Physics.OdePlugin
         private float m_angularMotorTimescale = 1000;                      // motor angular velocity ramp up rate
         private float m_angularMotorDecayTimescale = 120;                 // motor angular velocity decay rate
         private Vector3 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);      // body angular velocity  decay rate
-        private Vector3 m_lastAngularVelocity = Vector3.Zero;           // what was last applied to body
 
         //Deflection properties
         private float m_angularDeflectionEfficiency = 0;
@@ -102,7 +100,7 @@ namespace OpenSim.Region.Physics.OdePlugin
         //Banking properties
         private float m_bankingEfficiency = 0;
         private float m_bankingMix = 0;
-        private float m_bankingTimescale = 0;
+        private float m_bankingTimescale = 1000;
 
         //Hover and Buoyancy properties
         private float m_VhoverHeight = 0f;
@@ -117,9 +115,8 @@ namespace OpenSim.Region.Physics.OdePlugin
         private float m_verticalAttractionEfficiency = 1.0f;        // damped
         private float m_verticalAttractionTimescale = 1000f;        // Timescale > 300  means no vert attractor.
 
-        // auxiliar 
-        private Vector3 m_dir = Vector3.Zero;                           // velocity applied to body
 
+        // auxiliar
         private float m_lmEfect = 0;                                            // current linear motor eficiency
         private float m_amEfect = 0;                                            // current angular motor eficiency
 
@@ -130,6 +127,82 @@ namespace OpenSim.Region.Physics.OdePlugin
             _pParentScene = rootPrim._parent_scene;
         }
 
+
+        public void DoSetVehicle(VehicleData vd)
+        {
+
+            float timestep = _pParentScene.ODE_STEPSIZE;
+            float invtimestep = 1.0f / timestep;
+
+            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 < timestep) m_linearFrictionTimescale.X = timestep;
+            if (m_linearFrictionTimescale.Y < timestep) m_linearFrictionTimescale.Y = timestep;
+            if (m_linearFrictionTimescale.Z < timestep) m_linearFrictionTimescale.Z = timestep;
+
+            m_linearMotorDecayTimescale = vd.m_linearMotorDecayTimescale;
+            if (m_linearMotorDecayTimescale < 0.5f) m_linearMotorDecayTimescale = 0.5f;
+            m_linearMotorDecayTimescale *= invtimestep;
+
+            m_linearMotorTimescale = vd.m_linearMotorTimescale;
+            if (m_linearMotorTimescale < timestep) m_linearMotorTimescale = timestep;
+
+            m_linearMotorOffset = vd.m_linearMotorOffset;
+
+            //Angular properties
+            m_angularMotorDirection = vd.m_angularMotorDirection;
+            m_angularMotorTimescale = vd.m_angularMotorTimescale;
+            if (m_angularMotorTimescale < timestep) m_angularMotorTimescale = timestep;
+
+            m_angularMotorDecayTimescale = vd.m_angularMotorDecayTimescale;
+            if (m_angularMotorDecayTimescale < 0.5f) m_angularMotorDecayTimescale = 0.5f;
+            m_angularMotorDecayTimescale *= invtimestep;
+
+            m_angularFrictionTimescale = vd.m_angularFrictionTimescale;
+            if (m_angularFrictionTimescale.X < timestep) m_angularFrictionTimescale.X = timestep;
+            if (m_angularFrictionTimescale.Y < timestep) m_angularFrictionTimescale.Y = timestep;
+            if (m_angularFrictionTimescale.Z < timestep) m_angularFrictionTimescale.Z = timestep;
+
+            //Deflection properties
+            m_angularDeflectionEfficiency = vd.m_angularDeflectionEfficiency;
+            m_angularDeflectionTimescale = vd.m_angularDeflectionTimescale;
+            if (m_angularDeflectionTimescale < timestep) m_angularDeflectionTimescale = timestep;
+
+            m_linearDeflectionEfficiency = vd.m_linearDeflectionEfficiency;
+            m_linearDeflectionTimescale = vd.m_linearDeflectionTimescale;
+            if (m_linearDeflectionTimescale < timestep) m_linearDeflectionTimescale = timestep;
+
+            //Banking properties
+            m_bankingEfficiency = vd.m_bankingEfficiency;
+            m_bankingMix = vd.m_bankingMix;
+            m_bankingTimescale = vd.m_bankingTimescale;
+            if (m_bankingTimescale < timestep) m_bankingTimescale = timestep;
+
+            //Hover and Buoyancy properties
+            m_VhoverHeight = vd.m_VhoverHeight;
+            m_VhoverEfficiency = vd.m_VhoverEfficiency;
+            m_VhoverTimescale = vd.m_VhoverTimescale;
+            if (m_VhoverTimescale < timestep) m_VhoverTimescale = timestep;
+
+            m_VehicleBuoyancy = vd.m_VehicleBuoyancy;
+
+            //Attractor properties
+            m_verticalAttractionEfficiency = vd.m_verticalAttractionEfficiency;
+            m_verticalAttractionTimescale = vd.m_verticalAttractionTimescale;
+            if (m_verticalAttractionTimescale < timestep) m_verticalAttractionTimescale = timestep;
+
+            // Axis
+            m_referenceFrame = vd.m_referenceFrame;
+
+            m_lmEfect = 0;
+            m_amEfect = 0;
+        }
+
         internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue)
         {
             float len;
@@ -231,6 +304,9 @@ namespace OpenSim.Region.Physics.OdePlugin
                     if (len > 12.566f)
                         m_angularMotorDirection *= (12.566f / len);
                     m_amEfect = 1.0f; // turn it on
+                    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 < timestep) pValue = timestep;
@@ -242,6 +318,9 @@ namespace OpenSim.Region.Physics.OdePlugin
                     if (len > 30.0f)
                         m_linearMotorDirection *= (30.0f / len);
                     m_lmEfect = 1.0f; // turn it on
+                    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);
@@ -273,6 +352,9 @@ namespace OpenSim.Region.Physics.OdePlugin
                     if (len > 12.566f)
                         m_angularMotorDirection *= (12.566f / len);
                     m_amEfect = 1.0f; // turn it on
+                    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 < timestep) pValue.X = timestep;
@@ -286,6 +368,9 @@ namespace OpenSim.Region.Physics.OdePlugin
                     if (len > 30.0f)
                         m_linearMotorDirection *= (30.0f / len);
                     m_lmEfect = 1.0f; // turn it on
+                    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);
@@ -347,12 +432,23 @@ namespace OpenSim.Region.Physics.OdePlugin
                     m_linearFrictionTimescale = new Vector3(1000, 1000, 1000);
                     m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
                     m_linearMotorTimescale = 1000;
-                    m_linearMotorDecayTimescale = 120 * invtimestep;
+                    m_linearMotorDecayTimescale = 120;
                     m_angularMotorTimescale = 1000;
-                    m_angularMotorDecayTimescale = 1000 * invtimestep;
+                    m_angularMotorDecayTimescale = 1000;
                     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;
 
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs
index 3b7f562..0ccdbc0 100644
--- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs
@@ -111,7 +111,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                                                         | CollisionCategories.Body
                                                         | CollisionCategories.Character
                                                         );
-        private bool m_collidesLand = true;
+//        private bool m_collidesLand = true;
         private bool m_collidesWater;
         public bool m_returnCollisions;
 
@@ -122,7 +122,7 @@ namespace OpenSim.Region.Physics.OdePlugin
         private CollisionCategories m_collisionFlags = m_default_collisionFlags;
 
         public bool m_disabled;
-        public bool m_taintselected;
+
 
         public uint m_localID;
 
@@ -142,20 +142,19 @@ namespace OpenSim.Region.Physics.OdePlugin
         private List<OdePrim> childrenPrim = new List<OdePrim>();
 
         private bool m_iscolliding;
-        private bool m_wascolliding;
-        private bool m_isSelected;
+
+        public bool m_isSelected;
+        private bool m_delaySelect;
+        private bool m_lastdoneSelected;
+        public bool m_outbounds;
 
         internal bool m_isVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively
 
         private bool m_throttleUpdates;
         private int throttleCounter;
-        public int m_interpenetrationcount;
         public float m_collisionscore;
         int m_colliderfilter = 0;
-        public int m_roundsUnderMotionThreshold;
-        private int m_crossingfailures;
 
-        public bool outofBounds;
         private float m_density = 10.000006836f; // Aluminum g/cm3;
 
         public bool _zeroFlag;
@@ -166,12 +165,11 @@ namespace OpenSim.Region.Physics.OdePlugin
         private Vector3 _target_velocity;
 
         public Vector3 primOOBsize; // prim real dimensions from mesh 
-        public Vector3 primOOBoffset; // is centroid out of mesh or rest aabb
+        public Vector3 primOOBoffset; // its centroid out of mesh or rest aabb
         public float primOOBradiusSQ;
         public d.Mass primdMass; // prim inertia information on it's own referencial
         float primMass; // prim own mass
         float _mass; // object mass acording to case
-        public d.Mass objectpMass; // object last computed inertia
         private bool hasOOBoffsetFromMesh = false; // if true we did compute it form mesh centroid, else from aabb
 
         public int givefakepos = 0;
@@ -182,9 +180,6 @@ namespace OpenSim.Region.Physics.OdePlugin
         public int m_eventsubscription;
         private CollisionEventUpdate CollisionEventsThisFrame = new CollisionEventUpdate();
 
-        private IntPtr m_linkJoint = IntPtr.Zero;
-        private IntPtr _linkJointGroup = IntPtr.Zero;
-
         public volatile bool childPrim;
 
         public ODEDynamics m_vehicle;
@@ -264,7 +259,7 @@ namespace OpenSim.Region.Physics.OdePlugin
             set
             {
                 if (value)
-                    m_isSelected = value;
+                    m_isSelected = value; // if true set imediatly to stop moves etc
                 AddChange(changes.Selected, value);
             }
         }
@@ -298,13 +293,6 @@ namespace OpenSim.Region.Physics.OdePlugin
                     m_iscolliding = false;
                 else
                     m_iscolliding = true;
-
-                if (m_wascolliding != m_iscolliding)
-                {
-                    if (m_wascolliding && !m_isSelected && Body != IntPtr.Zero)
-                        d.BodyEnable(Body);
-                    m_wascolliding = m_iscolliding;
-                }
             }
         }
 
@@ -665,19 +653,21 @@ namespace OpenSim.Region.Physics.OdePlugin
             strVehicleQuatParam fp = new strVehicleQuatParam();
             fp.param = param;
             fp.value = value;
-            AddChange(changes.VehicleVectorParam, fp);
+            AddChange(changes.VehicleRotationParam, fp);
         }
 
         public override void VehicleFlags(int param, bool value)
         {
-            if (m_vehicle == null)
-                return;
             strVehicleBoolParam bp = new strVehicleBoolParam();
             bp.param = param;
             bp.value = value;
             AddChange(changes.VehicleFlags, bp);
         }
 
+        public override void SetVehicle(object vdata)
+        {
+            AddChange(changes.SetVehicle, vdata);
+        }
         public void SetAcceleration(Vector3 accel)
         {
             _acceleration = accel;
@@ -710,8 +700,30 @@ namespace OpenSim.Region.Physics.OdePlugin
 
         public override void CrossingFailure()
         {
-            m_crossingfailures++;
-            changeDisable(false);
+            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)
@@ -865,12 +877,14 @@ namespace OpenSim.Region.Physics.OdePlugin
             m_force = Vector3.Zero;
 
             m_iscolliding = false;
-            m_wascolliding = false;
             m_colliderfilter = 0;
 
             hasOOBoffsetFromMesh = false;
             _triMeshData = IntPtr.Zero;
 
+            m_lastdoneSelected = false;
+            m_isSelected = false;
+            m_delaySelect = false;
 
             primContactData.mu = parent_scene.m_materialContactsData[(int)Material.Wood].mu;
             primContactData.bounce = parent_scene.m_materialContactsData[(int)Material.Wood].bounce;
@@ -885,8 +899,6 @@ namespace OpenSim.Region.Physics.OdePlugin
         private void resetCollisionAccounting()
         {
             m_collisionscore = 0;
-            m_interpenetrationcount = 0;
-            m_disabled = false;
         }
 
         private void createAMotor(Vector3 axis)
@@ -926,9 +938,6 @@ namespace OpenSim.Region.Physics.OdePlugin
             curr.W = dcur.W;
             Vector3 ax;
 
-            const int StopERP = 7;
-            const int StopCFM = 8;
-
             int i = 0;
             int j = 0;
             if (axis.X == 0)
@@ -943,10 +952,10 @@ namespace OpenSim.Region.Physics.OdePlugin
                 d.JointSetAMotorParam(Amotor, (int)d.JointParam.FudgeFactor, 0.0001f);
                 d.JointSetAMotorParam(Amotor, (int)d.JointParam.Bounce, 0f);
                 d.JointSetAMotorParam(Amotor, (int)d.JointParam.FMax, 5e8f);
-                d.JointSetAMotorParam(Amotor, (int)StopCFM, 0f);
-                d.JointSetAMotorParam(Amotor, (int)StopERP, 0.8f);
+                d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopCFM, 0f);
+                d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopERP, 0.8f);
                 i++;
-                j = 256; // odeplugin.cs doesn't have all parameters so this moves to next axis set
+                j = 256; // move to next axis set
             }
 
             if (axis.Y == 0)
@@ -960,8 +969,8 @@ namespace OpenSim.Region.Physics.OdePlugin
                 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f);
                 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f);
                 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f);
-                d.JointSetAMotorParam(Amotor, j + (int)StopCFM, 0f);
-                d.JointSetAMotorParam(Amotor, j + (int)StopERP, 0.8f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopCFM, 0f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f);
                 i++;
                 j += 256;
             }
@@ -977,8 +986,8 @@ namespace OpenSim.Region.Physics.OdePlugin
                 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f);
                 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f);
                 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f);
-                d.JointSetAMotorParam(Amotor, j + (int)StopCFM, 0f);
-                d.JointSetAMotorParam(Amotor, j + (int)StopERP, 0.8f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopCFM, 0f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f);
             }
         }
 
@@ -1186,24 +1195,10 @@ namespace OpenSim.Region.Physics.OdePlugin
 
         public void enableBodySoft()
         {
-            if (!childPrim)
+            if (!childPrim && !m_isSelected)
             {
                 if (m_isphysical && Body != IntPtr.Zero && prim_geom != IntPtr.Zero)
                 {
-                    if (m_targetSpace != _parent_scene.ActiveSpace)
-                    {
-                        m_targetSpace = _parent_scene.ActiveSpace;
-
-                        foreach (OdePrim prm in childrenPrim)
-                        {
-                            if (prm.prim_geom != IntPtr.Zero)
-                            {
-                                d.SpaceAdd(m_targetSpace, prm.prim_geom);
-                                prm.m_targetSpace = m_targetSpace;
-                            }
-                        }
-                        d.SpaceAdd(m_targetSpace, prim_geom);
-                    }
                     d.GeomEnable(prim_geom);
                     foreach (OdePrim prm in childrenPrim)
                         d.GeomEnable(prm.prim_geom);
@@ -1211,6 +1206,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                     d.BodyEnable(Body);
                 }
             }
+            m_disabled = false;
             resetCollisionAccounting(); // this sets m_disable to false
         }
 
@@ -1221,19 +1217,6 @@ namespace OpenSim.Region.Physics.OdePlugin
             {
                 if (m_isphysical && Body != IntPtr.Zero && prim_geom != IntPtr.Zero)
                 {
-                    if (m_targetSpace == _parent_scene.ActiveSpace)
-                    {
-                        foreach (OdePrim prm in childrenPrim)
-                        {
-                            if (prm.m_targetSpace != IntPtr.Zero && prm.prim_geom != IntPtr.Zero)
-                            {
-                                d.SpaceRemove(prm.m_targetSpace, prm.prim_geom);
-                                prm.m_targetSpace = IntPtr.Zero;
-                            }
-                        }
-                        d.SpaceRemove(m_targetSpace, prim_geom);
-                        m_targetSpace = IntPtr.Zero;
-                    }
                     d.GeomDisable(prim_geom);
                     foreach (OdePrim prm in childrenPrim)
                         d.GeomDisable(prm.prim_geom);
@@ -1369,9 +1352,6 @@ namespace OpenSim.Region.Physics.OdePlugin
             d.BodySetMass(Body, ref objdmass);
             _mass = objdmass.mass;
 
-            m_collisionCategories |= CollisionCategories.Body;
-            m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
-
             // disconnect from world gravity so we can apply buoyancy
             d.BodySetGravityMode(Body, false);
 
@@ -1379,16 +1359,14 @@ namespace OpenSim.Region.Physics.OdePlugin
             d.BodySetAutoDisableSteps(Body, body_autodisable_frames);
             //            d.BodySetLinearDampingThreshold(Body, 0.01f);
             //            d.BodySetAngularDampingThreshold(Body, 0.001f);
-            d.BodySetDamping(Body, .001f, .0002f);
+            d.BodySetDamping(Body, .002f, .002f);
 
+            m_collisionCategories |= CollisionCategories.Body;
+            m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
             d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
             d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
-
-            m_interpenetrationcount = 0;
             m_collisionscore = 0;
 
-            m_disabled = false;
-
             if (m_targetSpace != _parent_scene.ActiveSpace)
             {
                 if (m_targetSpace != IntPtr.Zero)
@@ -1416,6 +1394,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                     prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
                     d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories);
                     d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags);
+                    prm.m_collisionscore = 0;
 
                     if (prm.m_targetSpace != _parent_scene.ActiveSpace)
                     {
@@ -1428,10 +1407,11 @@ namespace OpenSim.Region.Physics.OdePlugin
                         prm.m_targetSpace = _parent_scene.ActiveSpace;
                         d.SpaceAdd(m_targetSpace, prm.prim_geom);
                     }
-                    d.GeomEnable(prm.prim_geom);
+
+                    if (m_isSelected || m_disabled)
+                        d.GeomDisable(prm.prim_geom);
+
                     prm.m_disabled = false;
-                    prm.m_interpenetrationcount = 0;
-                    prm.m_collisionscore = 0;
                     _parent_scene.addActivePrim(prm);
                 }
             }
@@ -1442,8 +1422,12 @@ namespace OpenSim.Region.Physics.OdePlugin
                 createAMotor(m_angularlock);
             }
 
-            d.GeomEnable(prim_geom);
-            m_disabled = false;
+            if (m_isSelected || m_disabled)
+            {
+                d.GeomDisable(prim_geom);
+                d.BodyDisable(Body);
+            }
+
             _parent_scene.addActivePrim(this);
         }
 
@@ -1484,12 +1468,16 @@ namespace OpenSim.Region.Physics.OdePlugin
                             prm.m_collisionscore = 0;
                         }
                     }
+                    if (Amotor != IntPtr.Zero)
+                    {
+                        d.JointDestroy(Amotor);
+                        Amotor = IntPtr.Zero;
+                    }
                     d.BodyDestroy(Body);
                 }
                 Body = IntPtr.Zero;
             }
             _mass = primMass;
-            m_disabled = true;
             m_collisionscore = 0;
         }
 
@@ -2115,49 +2103,72 @@ namespace OpenSim.Region.Physics.OdePlugin
                 d.BodySetTorque(Body, 0f, 0f, 0f);
                 d.BodySetLinearVel(Body, 0f, 0f, 0f);
                 d.BodySetAngularVel(Body, 0f, 0f, 0f);
-
             }
         }
 
         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)
             {
-                m_collisionCategories = CollisionCategories.Selected;
-                m_collisionFlags = (CollisionCategories.Sensor | CollisionCategories.Space);
+                if (!childPrim && Body != IntPtr.Zero)
+                    d.BodyDisable(Body);
 
-                if (prim_geom != IntPtr.Zero)
+                if (m_delaySelect)
                 {
-                    d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
-                    d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
+                    if (!childPrim)
+                    {
+                        foreach (OdePrim prm in childrenPrim)
+                        {
+                            d.GeomDisable(prm.prim_geom);
+                            prm.m_delaySelect = false;
+                        }
+                    }
+                    d.GeomDisable(prim_geom);
+                    m_delaySelect = false;
+                }
+                else
+                {
+                    m_delaySelect = true;
                 }
-
-                disableBodySoft();
             }
             else
             {
-                m_collisionCategories = CollisionCategories.Geom;
-
-                if (m_isphysical)
-                    m_collisionCategories |= CollisionCategories.Body;
-
-                m_collisionFlags = m_default_collisionFlags;
-
-                if (m_collidesLand)
-                    m_collisionFlags |= CollisionCategories.Land;
-                if (m_collidesWater)
-                    m_collisionFlags |= CollisionCategories.Water;
+                if (!childPrim && Body != IntPtr.Zero && !m_disabled)
+                    d.BodyEnable(Body);
 
-                if (prim_geom != IntPtr.Zero)
+                if (!childPrim)
                 {
-                    d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
-                    d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
+                    foreach (OdePrim prm in childrenPrim)
+                    {
+                        if(!prm.m_disabled)
+                            d.GeomEnable(prm.prim_geom);
+                        prm.m_delaySelect = false;
+                    }
                 }
+                if(!m_disabled)
+                    d.GeomEnable(prim_geom);
 
-                enableBodySoft();
+                m_delaySelect = false;
             }
 
             resetCollisionAccounting();
@@ -2165,6 +2176,7 @@ namespace OpenSim.Region.Physics.OdePlugin
 
         private void changePosition(Vector3 newPos)
         {
+            CheckDelaySelect();
             if (m_isphysical)
             {
                 if (childPrim)  // inertia is messed, must rebuild
@@ -2207,6 +2219,7 @@ namespace OpenSim.Region.Physics.OdePlugin
 
         private void changeOrientation(Quaternion newOri)
         {
+            CheckDelaySelect();
             if (m_isphysical)
             {
                 if (childPrim)  // inertia is messed, must rebuild
@@ -2258,6 +2271,7 @@ namespace OpenSim.Region.Physics.OdePlugin
 
         private void changePositionAndOrientation(Vector3 newPos, Quaternion newOri)
         {
+            CheckDelaySelect();
             if (m_isphysical)
             {
                 if (childPrim && m_building)  // inertia is messed, must rebuild
@@ -2342,6 +2356,8 @@ namespace OpenSim.Region.Physics.OdePlugin
 
         private void changePhysicsStatus(bool NewStatus)
         {
+            CheckDelaySelect();
+
             m_isphysical = NewStatus;
 
             if (!childPrim)
@@ -2384,6 +2400,8 @@ namespace OpenSim.Region.Physics.OdePlugin
 
         private void changeprimsizeshape()
         {
+            CheckDelaySelect();
+
             OdePrim parent = (OdePrim)_parent;
 
             bool chp = childPrim;
@@ -2508,7 +2526,6 @@ namespace OpenSim.Region.Physics.OdePlugin
                 }
 
                 m_collisionscore = 0;
-                m_interpenetrationcount = 0;
             }
         }
 
@@ -2528,7 +2545,6 @@ namespace OpenSim.Region.Physics.OdePlugin
                     }
                 }
                 m_collisionscore = 0;
-                m_interpenetrationcount = 0;
             }
         }
 
@@ -2565,6 +2581,7 @@ namespace OpenSim.Region.Physics.OdePlugin
             else
             {
                 m_building = false;
+                CheckDelaySelect();
                 if (!childPrim)
                     MakeBody();
             }
@@ -2575,18 +2592,26 @@ namespace OpenSim.Region.Physics.OdePlugin
             }
         }
 
-        private void changeVehicleType(int value)
+       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 (value != (int)Vehicle.TYPE_NONE)
-                {
-                    m_vehicle = new ODEDynamics(this);
-                    m_vehicle.ProcessTypeChange((Vehicle)value);
-                }
+                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)
@@ -2595,8 +2620,6 @@ namespace OpenSim.Region.Physics.OdePlugin
                 return;
 
             m_vehicle.ProcessFloatVehicleParam((Vehicle)fp.param, fp.value);
-            if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
-                d.BodyEnable(Body);
         }
 
         private void changeVehicleVectorParam(strVehicleVectorParam vp)
@@ -2604,8 +2627,6 @@ namespace OpenSim.Region.Physics.OdePlugin
             if (m_vehicle == null)
                 return;
             m_vehicle.ProcessVectorVehicleParam((Vehicle)vp.param, vp.value);
-            if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
-                d.BodyEnable(Body);
         }
 
         private void changeVehicleRotationParam(strVehicleQuatParam qp)
@@ -2613,8 +2634,6 @@ namespace OpenSim.Region.Physics.OdePlugin
             if (m_vehicle == null)
                 return;
             m_vehicle.ProcessRotationVehicleParam((Vehicle)qp.param, qp.value);
-            if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
-                d.BodyEnable(Body);
         }
 
         private void changeVehicleFlags(strVehicleBoolParam bp)
@@ -2622,8 +2641,6 @@ namespace OpenSim.Region.Physics.OdePlugin
             if (m_vehicle == null)
                 return;
             m_vehicle.ProcessVehicleFlags(bp.param, bp.value);
-            if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
-                d.BodyEnable(Body);
         }
 
         #endregion
@@ -2849,41 +2866,6 @@ namespace OpenSim.Region.Physics.OdePlugin
             {
                 if (Body != IntPtr.Zero)
                 {
-                    if (m_crossingfailures != 0 && m_crossingfailures < 5)
-                    {
-                        _position.X = Util.Clip(_position.X, 0.4f, _parent_scene.WorldExtents.X - 0.4f);
-                        _position.Y = Util.Clip(_position.Y, 0.4f, _parent_scene.WorldExtents.Y - 0.4f);
-                        _position.Z = Util.Clip(_position.Z + 0.2f, -100f, 50000f);
-
-                        float tmp = _parent_scene.GetTerrainHeightAtXY(_position.X, _position.Y);
-                        if (_position.Z < tmp)
-                            _position.Z = tmp + 0.2f;
-
-                        m_lastposition = _position;
-                        m_lastorientation = _orientation;
-                        _velocity.X = 0;
-                        _velocity.Y = 0;
-                        _velocity.Z = 0;
-
-                        m_lastVelocity = _velocity;
-                        m_rotationalVelocity = _velocity;
-                        if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE)
-                            m_vehicle.Stop();
-
-                        m_crossingfailures = 0; // do this only once
-                        d.BodySetLinearVel(Body, 0, 0, 0); // stop it
-                        d.BodySetAngularVel(Body, 0, 0, 0);
-                        d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
-                        enableBodySoft();
-                        base.RequestPhysicsterseUpdate();
-                        return;
-                    }
-
-                    else if (m_crossingfailures != 0)
-                    {
-                        return;
-                    }
-
                     Vector3 pv = Vector3.Zero;
                     bool lastZeroFlag = _zeroFlag;
 
@@ -2899,24 +2881,21 @@ namespace OpenSim.Region.Physics.OdePlugin
                         // we can't let it keeping moving and having colisions
                         // since it can be stucked between something like terrain and edge
                         // so lets stop and disable it until something else kicks it
-                        if (m_crossingfailures == 0)
-                        {
 
-                            _position.X = Util.Clip(lpos.X, -0.5f, _parent_scene.WorldExtents.X + 0.5f);
-                            _position.Y = Util.Clip(lpos.Y, -0.5f, _parent_scene.WorldExtents.Y + 0.5f);
-                            _position.Z = Util.Clip(lpos.Z, -100f, 50000f);
+                        _position.X = Util.Clip(lpos.X, -0.2f, _parent_scene.WorldExtents.X + 0.2f);
+                        _position.Y = Util.Clip(lpos.Y, -0.2f, _parent_scene.WorldExtents.Y + 0.2f);
+                        _position.Z = Util.Clip(lpos.Z, -100f, 50000f);
 
-                            m_lastposition = _position;
-                            m_lastorientation = _orientation;
+                        m_lastposition = _position;
+//                        m_lastorientation = _orientation;
 
-                            d.BodySetLinearVel(Body, 0, 0, 0); // stop it
-                            d.BodySetAngularVel(Body, 0, 0, 0);
-                            d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
-                            disableBodySoft(); // stop collisions
-                            m_crossingfailures++; // do this only once
-                            base.RequestPhysicsterseUpdate();
+                        d.BodySetLinearVel(Body, 0, 0, 0); // stop it
+//                        d.BodySetAngularVel(Body, 0, 0, 0);
+                        d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
+                        disableBodySoft(); // stop collisions
+                        m_outbounds = true;
+                        base.RequestPhysicsterseUpdate();
                             return;
-                        }
                     }
 
                     if (lpos.Z < -100 || lpos.Z > 100000f)
@@ -3159,6 +3138,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                     else
                         ChildRemove(this, false);
 
+                    m_vehicle = null;
                     RemoveGeom();
                     m_targetSpace = IntPtr.Zero;
                     if (m_eventsubscription > 0)
@@ -3273,6 +3253,9 @@ namespace OpenSim.Region.Physics.OdePlugin
                     changeVehicleRotationParam((strVehicleQuatParam) arg);
                     break;
 
+                case changes.SetVehicle:
+                    changeSetVehicle((VehicleData) arg);
+                    break;
                 case changes.Null:
                     donullchange();
                     break;
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs
index e62746e..2b6bc59 100644
--- a/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs
+++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs
@@ -1,4 +1,3 @@
-
 /*
  * based on:
  * Ode.NET - .NET bindings for ODE
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs
index 56f3786..6e4c373 100644
--- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs
+++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs
@@ -137,6 +137,7 @@ namespace OpenSim.Region.Physics.OdePlugin
         VehicleVectorParam,
         VehicleRotationParam,
         VehicleFlags,
+        SetVehicle,
 
         Null             //keep this last used do dim the methods array. does nothing but pulsing the prim
     }
@@ -166,8 +167,8 @@ namespace OpenSim.Region.Physics.OdePlugin
         
         float frictionMovementMult = 0.3f;
 
-        float TerrainBounce = 0.3f;
-        float TerrainFriction = 0.3f;
+        float TerrainBounce = 0.1f;
+        float TerrainFriction = 0.1f;
 
         public float AvatarBounce = 0.3f;
         public float AvatarFriction = 0;// 0.9f * 0.5f;
@@ -989,145 +990,62 @@ namespace OpenSim.Region.Physics.OdePlugin
         /// <param name="timeStep"></param>
         private void collision_optimized()
         {
-//        _perloopContact.Clear();
-// clear characts IsColliding until we do it some other way
-
             lock (_characters)
                 {
-                foreach (OdeCharacter chr in _characters)
+                try
+                {
+                    foreach (OdeCharacter chr in _characters)
                     {
-                    // this are odd checks  if they are needed something is wrong elsewhere
-                    // keep for now
-                    if (chr == null)
-                        continue;
-
-                    if (chr.Shell == IntPtr.Zero || chr.Body == IntPtr.Zero)
-                        continue;
-
-                    chr.IsColliding = false;
-                    //                    chr.CollidingGround = false; not done here
-                    chr.CollidingObj = false;
+                        if (chr == null || chr.Shell == IntPtr.Zero || chr.Body == IntPtr.Zero)
+                            continue;
+
+                        chr.IsColliding = false;
+                        //                    chr.CollidingGround = false; not done here
+                        chr.CollidingObj = false;
+                        // do colisions with static space
+                        d.SpaceCollide2(StaticSpace, chr.Shell, IntPtr.Zero, nearCallback);
                     }
                 }
-
-            // now let ode do its job
-            // colide active things amoung them
-
-            int st = Util.EnvironmentTickCount();
-            int ta;
-            int ts;
-            try
-                {
-                d.SpaceCollide(ActiveSpace, IntPtr.Zero, nearCallback);
-                }
-            catch (AccessViolationException)
+                catch (AccessViolationException)
                 {
-                m_log.Warn("[PHYSICS]: Unable to Active space collide");
+                    m_log.Warn("[PHYSICS]: Unable to collide Character to static space");
                 }
-            ta = Util.EnvironmentTickCountSubtract(st);
-            // then active things with static enviroment
-            try
+
+            }
+
+            // collide active prims with static enviroment
+            lock (_activeprims)
+            {
+                try
                 {
-                d.SpaceCollide2(ActiveSpace,StaticSpace, IntPtr.Zero, nearCallback);
+                    foreach (OdePrim prm in _activeprims)
+                    {
+                        if (d.BodyIsEnabled(prm.Body))
+                            d.SpaceCollide2(StaticSpace, prm.prim_geom, IntPtr.Zero, nearCallback);
+                    }
                 }
-            catch (AccessViolationException)
+                catch (AccessViolationException)
                 {
-                m_log.Warn("[PHYSICS]: Unable to Active to static space collide");
+                    m_log.Warn("[PHYSICS]: Unable to collide Active prim to static space");
                 }
-            ts = Util.EnvironmentTickCountSubtract(st);
-//            _perloopContact.Clear();
-        }
-
-        #endregion
-
-
-        public float GetTerrainHeightAtXY(float x, float y)
-        {
-            // assumes 1m size grid and constante size square regions
-            // region offset in mega position
-
-            int offsetX = ((int)(x / (int)Constants.RegionSize)) * (int)Constants.RegionSize;
-            int offsetY = ((int)(y / (int)Constants.RegionSize)) * (int)Constants.RegionSize;
-
-            IntPtr heightFieldGeom = IntPtr.Zero;
-
-            // get region map
-            if (!RegionTerrain.TryGetValue(new Vector3(offsetX, offsetY, 0), out heightFieldGeom))
-                return 0f;
-
-            if (heightFieldGeom == IntPtr.Zero)
-                return 0f;
-
-            if (!TerrainHeightFieldHeights.ContainsKey(heightFieldGeom))
-                return 0f;
-
-            // TerrainHeightField for ODE as offset 1m
-            x += 1f - offsetX;
-            y += 1f - offsetY;
-
-            // make position fit into array
-            if (x < 0)
-                x = 0;
-            if (y < 0)
-                y = 0;
-
-            // integer indexs
-            int ix;
-            int iy;
-            //  interpolators offset
-            float dx;
-            float dy;
-
-            int regsize = (int)Constants.RegionSize + 2; // map size see setterrain
-
-            // we  still have square fixed size regions
-            // also flip x and y because of how map is done for ODE fliped axis
-            // so ix,iy,dx and dy are inter exchanged
-            if (x < regsize - 1)
-            {
-                iy = (int)x;
-                dy = x - (float)iy;
-            }
-            else // out world use external height
-            {
-                iy = regsize - 1;
-                dy = 0;
             }
-            if (y < regsize - 1)
+
+            // finally colide active things amoung them
+            try
             {
-                ix = (int)y;
-                dx = y - (float)ix;
+                d.SpaceCollide(ActiveSpace, IntPtr.Zero, nearCallback);
             }
-            else
+            catch (AccessViolationException)
             {
-                ix = regsize - 1;
-                dx = 0;
+                m_log.Warn("[PHYSICS]: Unable to collide in Active space");
             }
 
-            float h0;
-            float h1;
-            float h2;
-
-            iy *= regsize;
-            iy += ix; // all indexes have iy + ix
+//            _perloopContact.Clear();
+        }
 
-            float[] heights = TerrainHeightFieldHeights[heightFieldGeom];
+        #endregion
 
-            if ((dx + dy) <= 1.0f)
-            {
-                h0 = ((float)heights[iy]); // 0,0 vertice
-                h1 = (((float)heights[iy + 1]) - h0) * dx; // 1,0 vertice minus 0,0
-                h2 = (((float)heights[iy + regsize]) - h0) * dy; // 0,1 vertice minus 0,0
-            }
-            else
-            {
-                h0 = ((float)heights[iy + regsize + 1]); // 1,1 vertice
-                h1 = (((float)heights[iy + 1]) - h0) * (1 - dy); // 1,1 vertice minus 1,0
-                h2 = (((float)heights[iy + regsize]) - h0) * (1 - dx); // 1,1 vertice minus 0,1
-            }
 
-            return h0 + h1 + h2;
-        }
 
         /// <summary>
         /// Add actor to the list that should receive collision events in the simulate loop.
@@ -1835,273 +1753,94 @@ namespace OpenSim.Region.Physics.OdePlugin
             get { return (false); }
         }
 
-        #region ODE Specific Terrain Fixes
-        public float[] ResizeTerrain512NearestNeighbour(float[] heightMap)
+        public float GetTerrainHeightAtXY(float x, float y)
         {
-            float[] returnarr = new float[262144];
-            float[,] resultarr = new float[(int)WorldExtents.X, (int)WorldExtents.Y];
+            // assumes 1m size grid and constante size square regions
+            // needs to know about sims around in future
+            // region offset in mega position
 
-            // Filling out the array into its multi-dimensional components
-            for (int y = 0; y < WorldExtents.Y; y++)
-            {
-                for (int x = 0; x < WorldExtents.X; x++)
-                {
-                    resultarr[y, x] = heightMap[y * (int)WorldExtents.Y + x];
-                }
-            }
+            int offsetX = ((int)(x / (int)Constants.RegionSize)) * (int)Constants.RegionSize;
+            int offsetY = ((int)(y / (int)Constants.RegionSize)) * (int)Constants.RegionSize;
 
-            // Resize using Nearest Neighbour
-
-            // This particular way is quick but it only works on a multiple of the original
-
-            // The idea behind this method can be described with the following diagrams
-            // second pass and third pass happen in the same loop really..  just separated
-            // them to show what this does.
-
-            // First Pass
-            // ResultArr:
-            // 1,1,1,1,1,1
-            // 1,1,1,1,1,1
-            // 1,1,1,1,1,1
-            // 1,1,1,1,1,1
-            // 1,1,1,1,1,1
-            // 1,1,1,1,1,1
-
-            // Second Pass
-            // ResultArr2:
-            // 1,,1,,1,,1,,1,,1,
-            // ,,,,,,,,,,
-            // 1,,1,,1,,1,,1,,1,
-            // ,,,,,,,,,,
-            // 1,,1,,1,,1,,1,,1,
-            // ,,,,,,,,,,
-            // 1,,1,,1,,1,,1,,1,
-            // ,,,,,,,,,,
-            // 1,,1,,1,,1,,1,,1,
-            // ,,,,,,,,,,
-            // 1,,1,,1,,1,,1,,1,
-
-            // Third pass fills in the blanks
-            // ResultArr2:
-            // 1,1,1,1,1,1,1,1,1,1,1,1
-            // 1,1,1,1,1,1,1,1,1,1,1,1
-            // 1,1,1,1,1,1,1,1,1,1,1,1
-            // 1,1,1,1,1,1,1,1,1,1,1,1
-            // 1,1,1,1,1,1,1,1,1,1,1,1
-            // 1,1,1,1,1,1,1,1,1,1,1,1
-            // 1,1,1,1,1,1,1,1,1,1,1,1
-            // 1,1,1,1,1,1,1,1,1,1,1,1
-            // 1,1,1,1,1,1,1,1,1,1,1,1
-            // 1,1,1,1,1,1,1,1,1,1,1,1
-            // 1,1,1,1,1,1,1,1,1,1,1,1
-
-            // X,Y = .
-            // X+1,y = ^
-            // X,Y+1 = *
-            // X+1,Y+1 = #
-
-            // Filling in like this;
-            // .*
-            // ^#
-            // 1st .
-            // 2nd *
-            // 3rd ^
-            // 4th #
-            // on single loop.
-
-            float[,] resultarr2 = new float[512, 512];
-            for (int y = 0; y < WorldExtents.Y; y++)
-            {
-                for (int x = 0; x < WorldExtents.X; x++)
-                {
-                    resultarr2[y * 2, x * 2] = resultarr[y, x];
+            IntPtr heightFieldGeom = IntPtr.Zero;
 
-                    if (y < WorldExtents.Y)
-                    {
-                        resultarr2[(y * 2) + 1, x * 2] = resultarr[y, x];
-                    }
-                    if (x < WorldExtents.X)
-                    {
-                        resultarr2[y * 2, (x * 2) + 1] = resultarr[y, x];
-                    }
-                    if (x < WorldExtents.X && y < WorldExtents.Y)
-                    {
-                        resultarr2[(y * 2) + 1, (x * 2) + 1] = resultarr[y, x];
-                    }
-                }
-            }
+            // get region map
+            if (!RegionTerrain.TryGetValue(new Vector3(offsetX, offsetY, 0), out heightFieldGeom))
+                return 0f;
 
-            //Flatten out the array
-            int i = 0;
-            for (int y = 0; y < 512; y++)
-            {
-                for (int x = 0; x < 512; x++)
-                {
-                    if (resultarr2[y, x] <= 0)
-                        returnarr[i] = 0.0000001f;
-                    else
-                        returnarr[i] = resultarr2[y, x];
+            if (heightFieldGeom == IntPtr.Zero)
+                return 0f;
 
-                    i++;
-                }
-            }
+            if (!TerrainHeightFieldHeights.ContainsKey(heightFieldGeom))
+                return 0f;
 
-            return returnarr;
-        }
+            // TerrainHeightField for ODE as offset 1m
+            x += 1f - offsetX;
+            y += 1f - offsetY;
 
-        public float[] ResizeTerrain512Interpolation(float[] heightMap)
-        {
-            float[] returnarr = new float[262144];
-            float[,] resultarr = new float[512,512];
+            // 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
 
-            // Filling out the array into its multi-dimensional components
-            for (int y = 0; y < 256; y++)
+            // 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)
             {
-                for (int x = 0; x < 256; x++)
-                {
-                    resultarr[y, x] = heightMap[y * 256 + x];
-                }
+                iy = (int)x;
+                dy = x - (float)iy;
             }
-
-            // Resize using interpolation
-
-            // This particular way is quick but it only works on a multiple of the original
-
-            // The idea behind this method can be described with the following diagrams
-            // second pass and third pass happen in the same loop really..  just separated
-            // them to show what this does.
-
-            // First Pass
-            // ResultArr:
-            // 1,1,1,1,1,1
-            // 1,1,1,1,1,1
-            // 1,1,1,1,1,1
-            // 1,1,1,1,1,1
-            // 1,1,1,1,1,1
-            // 1,1,1,1,1,1
-
-            // Second Pass
-            // ResultArr2:
-            // 1,,1,,1,,1,,1,,1,
-            // ,,,,,,,,,,
-            // 1,,1,,1,,1,,1,,1,
-            // ,,,,,,,,,,
-            // 1,,1,,1,,1,,1,,1,
-            // ,,,,,,,,,,
-            // 1,,1,,1,,1,,1,,1,
-            // ,,,,,,,,,,
-            // 1,,1,,1,,1,,1,,1,
-            // ,,,,,,,,,,
-            // 1,,1,,1,,1,,1,,1,
-
-            // Third pass fills in the blanks
-            // ResultArr2:
-            // 1,1,1,1,1,1,1,1,1,1,1,1
-            // 1,1,1,1,1,1,1,1,1,1,1,1
-            // 1,1,1,1,1,1,1,1,1,1,1,1
-            // 1,1,1,1,1,1,1,1,1,1,1,1
-            // 1,1,1,1,1,1,1,1,1,1,1,1
-            // 1,1,1,1,1,1,1,1,1,1,1,1
-            // 1,1,1,1,1,1,1,1,1,1,1,1
-            // 1,1,1,1,1,1,1,1,1,1,1,1
-            // 1,1,1,1,1,1,1,1,1,1,1,1
-            // 1,1,1,1,1,1,1,1,1,1,1,1
-            // 1,1,1,1,1,1,1,1,1,1,1,1
-
-            // X,Y = .
-            // X+1,y = ^
-            // X,Y+1 = *
-            // X+1,Y+1 = #
-
-            // Filling in like this;
-            // .*
-            // ^#
-            // 1st .
-            // 2nd *
-            // 3rd ^
-            // 4th #
-            // on single loop.
-
-            float[,] resultarr2 = new float[512,512];
-            for (int y = 0; y < (int)Constants.RegionSize; y++)
+            else // out world use external height
             {
-                for (int x = 0; x < (int)Constants.RegionSize; x++)
-                {
-                    resultarr2[y*2, x*2] = resultarr[y, x];
-
-                    if (y < (int)Constants.RegionSize)
-                    {
-                        if (y + 1 < (int)Constants.RegionSize)
-                        {
-                            if (x + 1 < (int)Constants.RegionSize)
-                            {
-                                resultarr2[(y*2) + 1, x*2] = ((resultarr[y, x] + resultarr[y + 1, x] +
-                                                               resultarr[y, x + 1] + resultarr[y + 1, x + 1])/4);
-                            }
-                            else
-                            {
-                                resultarr2[(y*2) + 1, x*2] = ((resultarr[y, x] + resultarr[y + 1, x])/2);
-                            }
-                        }
-                        else
-                        {
-                            resultarr2[(y*2) + 1, x*2] = resultarr[y, x];
-                        }
-                    }
-                    if (x < (int)Constants.RegionSize)
-                    {
-                        if (x + 1 < (int)Constants.RegionSize)
-                        {
-                            if (y + 1 < (int)Constants.RegionSize)
-                            {
-                                resultarr2[y*2, (x*2) + 1] = ((resultarr[y, x] + resultarr[y + 1, x] +
-                                                               resultarr[y, x + 1] + resultarr[y + 1, x + 1])/4);
-                            }
-                            else
-                            {
-                                resultarr2[y*2, (x*2) + 1] = ((resultarr[y, x] + resultarr[y, x + 1])/2);
-                            }
-                        }
-                        else
-                        {
-                            resultarr2[y*2, (x*2) + 1] = resultarr[y, x];
-                        }
-                    }
-                    if (x < (int)Constants.RegionSize && y < (int)Constants.RegionSize)
-                    {
-                        if ((x + 1 < (int)Constants.RegionSize) && (y + 1 < (int)Constants.RegionSize))
-                        {
-                            resultarr2[(y*2) + 1, (x*2) + 1] = ((resultarr[y, x] + resultarr[y + 1, x] +
-                                                                 resultarr[y, x + 1] + resultarr[y + 1, x + 1])/4);
-                        }
-                        else
-                        {
-                            resultarr2[(y*2) + 1, (x*2) + 1] = resultarr[y, x];
-                        }
-                    }
-                }
+                iy = regsize - 1;
+                dy = 0;
             }
-            //Flatten out the array
-            int i = 0;
-            for (int y = 0; y < 512; y++)
+            if (y < regsize - 1)
             {
-                for (int x = 0; x < 512; x++)
-                {
-                    if (Single.IsNaN(resultarr2[y, x]) || Single.IsInfinity(resultarr2[y, x]))
-                    {
-                        m_log.Warn("[PHYSICS]: Non finite heightfield element detected.  Setting it to 0");
-                        resultarr2[y, x] = 0;
-                    }
-                    returnarr[i] = resultarr2[y, x];
-                    i++;
-                }
+                ix = (int)y;
+                dx = y - (float)ix;
+            }
+            else
+            {
+                ix = regsize - 1;
+                dx = 0;
             }
 
-            return returnarr;
-        }
+            float h0;
+            float h1;
+            float h2;
 
-        #endregion
+            iy *= regsize;
+            iy += ix; // all indexes have iy + ix
 
+            float[] heights = TerrainHeightFieldHeights[heightFieldGeom];
+
+            if ((dx + dy) <= 1.0f)
+            {
+                h0 = ((float)heights[iy]); // 0,0 vertice
+                h1 = (((float)heights[iy + 1]) - h0) * dx; // 1,0 vertice minus 0,0
+                h2 = (((float)heights[iy + regsize]) - h0) * dy; // 0,1 vertice minus 0,0
+            }
+            else
+            {
+                h0 = ((float)heights[iy + regsize + 1]); // 1,1 vertice
+                h1 = (((float)heights[iy + 1]) - h0) * (1 - dy); // 1,1 vertice minus 1,0
+                h2 = (((float)heights[iy + regsize]) - h0) * (1 - dx); // 1,1 vertice minus 0,1
+            }
+
+            return h0 + h1 + h2;
+        }
         public override void SetTerrain(float[] heightMap)
         {
             if (m_worldOffset != Vector3.Zero && m_parentScene != null)
@@ -2124,48 +1863,47 @@ namespace OpenSim.Region.Physics.OdePlugin
 
         public void SetTerrain(float[] heightMap, Vector3 pOffset)
             {
+            // assumes 1m size grid and constante size square regions
+            // needs to know about sims around in future
 
             float[] _heightmap;
-            _heightmap = new float[(((int)Constants.RegionSize + 2) * ((int)Constants.RegionSize + 2))];
 
             uint heightmapWidth = Constants.RegionSize + 2;
             uint heightmapHeight = Constants.RegionSize + 2;
 
-            uint heightmapWidthSamples;
+            uint heightmapWidthSamples = heightmapWidth + 1;
+            uint heightmapHeightSamples = heightmapHeight + 1;
 
-            uint heightmapHeightSamples;
- 
-            heightmapWidthSamples = (uint)Constants.RegionSize + 2;
-            heightmapHeightSamples = (uint)Constants.RegionSize + 2;
+            _heightmap = new float[heightmapWidthSamples * heightmapHeightSamples];
 
             const float scale = 1.0f;
             const float offset = 0.0f;
             const float thickness = 10f;
             const int wrap = 0;
 
-            int regionsize = (int) Constants.RegionSize + 2;
+            uint regionsize = Constants.RegionSize;
  
             float hfmin = float.MaxValue;
             float hfmax = float.MinValue;
             float val;
-            int xx;
-            int yy;
+            uint xx;
+            uint yy;
 
-            int maxXXYY = regionsize - 3;
+            uint maxXXYY = regionsize - 1;
             // flipping map adding one margin all around so things don't fall in edges
 
-            int xt = 0;
+            uint xt = 0;
             xx = 0;
 
-            for (int x = 0; x < heightmapWidthSamples; x++)
+            for (uint x = 0; x < heightmapWidthSamples; x++)
             {
                 if (x > 1 && xx < maxXXYY)
                     xx++;
                 yy = 0;
-                for (int y = 0; y < heightmapHeightSamples; y++)
+                for (uint y = 0; y < heightmapHeightSamples; y++)
                 {
                     if (y > 1 && y < maxXXYY)
-                        yy += (int)Constants.RegionSize;
+                        yy += regionsize;
 
                     val = heightMap[yy + xx];
                     _heightmap[xt + y] = val;
@@ -2176,8 +1914,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                         hfmax = val;
 
                 }
-
-                xt += regionsize;
+                xt += heightmapHeightSamples;
             }
             lock (OdeLock)
             {
@@ -2230,11 +1967,6 @@ namespace OpenSim.Region.Physics.OdePlugin
                 d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle);
                 d.GeomSetRotation(GroundGeom, ref R);
                 d.GeomSetPosition(GroundGeom, pOffset.X + (float)Constants.RegionSize * 0.5f - 0.5f, pOffset.Y + (float)Constants.RegionSize * 0.5f - 0.5f, 0);
-                IntPtr testGround = IntPtr.Zero;
-                if (RegionTerrain.TryGetValue(pOffset, out testGround))
-                {
-                    RegionTerrain.Remove(pOffset);
-                }
                 RegionTerrain.Add(pOffset, GroundGeom, GroundGeom);
 //                TerrainHeightFieldHeights.Add(GroundGeom, ODElandMap);
                 TerrainHeightFieldHeights.Add(GroundGeom, _heightmap);