update ubitODE to my current working state

4 files changed, 378 insertions, 568 deletions

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