2 files changed, 65 insertions, 23 deletions

diff --git a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
index 527a5cc..ececa6c 100644
--- a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
+++ b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
@@ -142,8 +142,7 @@ namespace OpenSim.Region.Physics.OdePlugin
             m_taintadd = true;
             _parent_scene.AddPhysicsActorTaint(this);
             //  don't do .add() here; old geoms get recycled with the same hash
-            parent_scene.geom_name_map[prim_geom] = primName;
-            parent_scene.actor_name_map[prim_geom] = (PhysicsActor) this;
+            
         }
 
         /// <summary>
@@ -378,7 +377,7 @@ namespace OpenSim.Region.Physics.OdePlugin
 
         public void ProcessTaints(float timestep)
         {
-            System.Threading.Thread.Sleep(5);
+            
 
             if (m_taintadd)
                 changeadd(timestep);
@@ -412,7 +411,26 @@ namespace OpenSim.Region.Physics.OdePlugin
                 changevelocity(timestep);
         }
 
+        public void ResetTaints()
+        {
+
+            m_taintposition = _position;
+
+            m_taintrot = _orientation;
 
+            m_taintPhysics = m_isphysical;
+
+            m_taintsize = _size;
+            
+
+            m_taintshape = false;
+
+            m_taintforce = false;
+              
+            m_taintdisable = false;
+
+            m_taintVelocity = PhysicsVector.Zero;
+        }
         public void changeadd(float timestep)
         {
             if (_mesh != null)
@@ -525,7 +543,9 @@ namespace OpenSim.Region.Physics.OdePlugin
                 }
 
 
-            }
+            } 
+            _parent_scene.geom_name_map[prim_geom] = this.m_primName;
+            _parent_scene.actor_name_map[prim_geom] = (PhysicsActor)this;
             m_taintadd = false;
 
 
@@ -610,8 +630,14 @@ namespace OpenSim.Region.Physics.OdePlugin
 
         public void changesize(float timestamp)
         {
+            if (!_parent_scene.geom_name_map.ContainsKey(prim_geom))
+            {
+                m_taintsize = _size;
+                return;
+            }
             string oldname = _parent_scene.geom_name_map[prim_geom];
 
+            
             // Cleanup of old prim geometry
             if (_mesh != null)
             {
@@ -995,11 +1021,16 @@ namespace OpenSim.Region.Physics.OdePlugin
         public override void CrossingFailure()
         {
             m_crossingfailures++;
-            if (m_crossingfailures >= 5)
+            if (m_crossingfailures > 5)
             {
-                m_log.Warn("[PHYSICS]: Too many crossing failures for: " + m_primName);
+                base.RaiseOutOfBounds(_position);
+                return;
 
             }
+            else if (m_crossingfailures == 5)
+            {
+                m_log.Warn("[PHYSICS]: Too many crossing failures for: " + m_primName);
+            }
         }
         public void UpdatePositionAndVelocity()
         {
diff --git a/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs
index 66af095..5ff74dc 100644
--- a/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs
+++ b/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs
@@ -736,6 +736,7 @@ namespace OpenSim.Region.Physics.OdePlugin
         /// <param name="prim"></param>
         public void RemovePrimThreadLocked(OdePrim prim)
         {
+            prim.ResetTaints();
             lock (OdeLock)
             {
                 System.Threading.Thread.Sleep(20);
@@ -1237,31 +1238,39 @@ namespace OpenSim.Region.Physics.OdePlugin
                         actor.Move(timeStep);
                         actor.collidelock = true;
                     }
-                    
-                    ode.dlock(world);
+                    if (!ode.lockquery())
+                    {
+                        ode.dlock(world);
 
-                    collision_optimized(timeStep);
+                        collision_optimized(timeStep);
 
-                    
 
-                    ode.dunlock(world);
-                    
-                    try
-                    {
-                        d.WorldQuickStep(world, ODE_STEPSIZE);
+
+                       
+
+                        try
+                        {
+                            d.WorldQuickStep(world, ODE_STEPSIZE);
+                        }
+                        catch (StackOverflowException)
+                        {
+                            d.WorldQuickStep(world, 0.001f);
+                        }
+                        d.JointGroupEmpty(contactgroup);
+                        ode.dunlock(world);
+
+                        step_time -= ODE_STEPSIZE;
+                        i++;
                     }
-                    catch (StackOverflowException)
+                    else
                     {
-                        d.WorldQuickStep(world, 0.001f);
+                        fps = 0;
                     }
-                    d.JointGroupEmpty(contactgroup);
+
                     foreach (OdeCharacter actor in _characters)
                     {
                         actor.collidelock = false;
                     }
-
-                    step_time -= ODE_STEPSIZE;
-                    i++;
                 }
 
                 foreach (OdeCharacter actor in _characters)
@@ -1273,12 +1282,14 @@ namespace OpenSim.Region.Physics.OdePlugin
                 {
                     bool processedtaints = false;
                     foreach (OdePrim prim in _taintedPrim)
-                    {
-                        prim.ProcessTaints(timeStep);
+                    {   
                         if (prim.m_taintremove)
                         {
                             RemovePrimThreadLocked(prim);
                         }
+                        
+                        prim.ProcessTaints(timeStep);
+
                         processedtaints = true;
                         prim.m_collisionscore = 0;
                     }