2 files changed, 112 insertions, 92 deletions

diff --git a/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs
index 5a7626e..cfe64f2 100644
--- a/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs
+++ b/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs
@@ -862,11 +862,10 @@ namespace OpenSim.Region.Physics.OdePlugin
         /// Called from Simulate
         /// This is the avatar's movement control + PID Controller
         /// </summary>
-        /// <remarks>
-        /// This routine will remove the character from the physics scene if it detects something wrong (non-finite
+        /// <param name="defects">The character will be added to this list if there is something wrong (non-finite
         /// position or velocity).
-        /// </remarks>
-        internal void Move()
+        /// </param>
+        internal void Move(List<OdeCharacter> defects)
         {
             //  no lock; for now it's only called from within Simulate()
             
@@ -891,8 +890,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                     "[ODE CHARACTER]: Avatar position of {0} for {1} is non-finite!  Removing from physics scene.",
                     localPos, Name);
 
-                _parent_scene.RemoveCharacter(this);
-                DestroyOdeStructures();
+                defects.Add(this);
 
                 return;
             }
@@ -1031,15 +1029,19 @@ namespace OpenSim.Region.Physics.OdePlugin
                     "[ODE CHARACTER]: Got a NaN force vector {0} in Move() for {1}.  Removing character from physics scene.",
                     vec, Name);
 
-                _parent_scene.RemoveCharacter(this);
-                DestroyOdeStructures();
+                defects.Add(this);
+
+                return;
             }
         }
 
         /// <summary>
         /// Updates the reported position and velocity.  This essentially sends the data up to ScenePresence.
         /// </summary>
-        internal void UpdatePositionAndVelocity()
+        /// <param name="defects">The character will be added to this list if there is something wrong (non-finite
+        /// position or velocity).
+        /// </param>
+        internal void UpdatePositionAndVelocity(List<OdeCharacter> defects)
         {
             //  no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit!
             d.Vector3 newPos;
@@ -1050,11 +1052,12 @@ namespace OpenSim.Region.Physics.OdePlugin
             catch (NullReferenceException)
             {
                 bad = true;
-                _parent_scene.RemoveCharacter(this);
-                DestroyOdeStructures();
+                defects.Add(this);
                 newPos = new d.Vector3(_position.X, _position.Y, _position.Z);
                 base.RaiseOutOfBounds(_position); // Tells ScenePresence that there's a problem!
                 m_log.WarnFormat("[ODE CHARACTER]: Avatar Null reference for Avatar {0}, physical actor {1}", Name, m_uuid);
+
+                return;
             }
 
             //  kluge to keep things in bounds.  ODE lets dead avatars drift away (they should be removed!)
@@ -1134,7 +1137,7 @@ namespace OpenSim.Region.Physics.OdePlugin
         /// <summary>
         /// Used internally to destroy the ODE structures associated with this character.
         /// </summary>
-        private void DestroyOdeStructures()
+        internal void DestroyOdeStructures()
         {
             // destroy avatar capsule and related ODE data
             if (Amotor != IntPtr.Zero)
diff --git a/OpenSim/Region/Physics/OdePlugin/OdeScene.cs b/OpenSim/Region/Physics/OdePlugin/OdeScene.cs
index 7db188f..89568b6 100644
--- a/OpenSim/Region/Physics/OdePlugin/OdeScene.cs
+++ b/OpenSim/Region/Physics/OdePlugin/OdeScene.cs
@@ -189,7 +189,11 @@ namespace OpenSim.Region.Physics.OdePlugin
         public d.TriCallback triCallback;
         public d.TriArrayCallback triArrayCallback;
 
+        /// <summary>
+        /// Avatars in the physics scene.
+        /// </summary>
         private readonly HashSet<OdeCharacter> _characters = new HashSet<OdeCharacter>();
+        
         private readonly HashSet<OdePrim> _prims = new HashSet<OdePrim>();
         private readonly HashSet<OdePrim> _activeprims = new HashSet<OdePrim>();
 
@@ -254,6 +258,14 @@ namespace OpenSim.Region.Physics.OdePlugin
         /// </remarks>
         public Dictionary<IntPtr, PhysicsActor> actor_name_map = new Dictionary<IntPtr, PhysicsActor>();
 
+        /// <summary>
+        /// Defects list to remove characters that no longer have finite positions due to some other bug.
+        /// </summary>
+        /// <remarks>
+        /// Used repeatedly in Simulate() but initialized once here.
+        /// </remarks>
+        private readonly List<OdeCharacter> defects = new List<OdeCharacter>();
+
         private bool m_NINJA_physics_joints_enabled = false;
         //private Dictionary<String, IntPtr> jointpart_name_map = new Dictionary<String,IntPtr>();
         private readonly Dictionary<String, List<PhysicsJoint>> joints_connecting_actor = new Dictionary<String, List<PhysicsJoint>>();
@@ -1515,45 +1527,42 @@ namespace OpenSim.Region.Physics.OdePlugin
         {
             _perloopContact.Clear();
 
-            lock (_characters)
+            foreach (OdeCharacter chr in _characters)
             {
-                foreach (OdeCharacter chr in _characters)
+                // Reset the collision values to false
+                // since we don't know if we're colliding yet
+                
+                // For some reason this can happen. Don't ask...
+                //
+                if (chr == null)
+                    continue;
+                
+                if (chr.Shell == IntPtr.Zero || chr.Body == IntPtr.Zero)
+                    continue;
+                
+                chr.IsColliding = false;
+                chr.CollidingGround = false;
+                chr.CollidingObj = false;
+                
+                // test the avatar's geometry for collision with the space
+                // This will return near and the space that they are the closest to
+                // And we'll run this again against the avatar and the space segment
+                // This will return with a bunch of possible objects in the space segment
+                // and we'll run it again on all of them.
+                try
                 {
-                    // Reset the collision values to false
-                    // since we don't know if we're colliding yet
-                    
-                    // For some reason this can happen. Don't ask...
-                    //
-                    if (chr == null)
-                        continue;
-                    
-                    if (chr.Shell == IntPtr.Zero || chr.Body == IntPtr.Zero)
-                        continue;
-                    
-                    chr.IsColliding = false;
-                    chr.CollidingGround = false;
-                    chr.CollidingObj = false;
-                    
-                    // test the avatar's geometry for collision with the space
-                    // This will return near and the space that they are the closest to
-                    // And we'll run this again against the avatar and the space segment
-                    // This will return with a bunch of possible objects in the space segment
-                    // and we'll run it again on all of them.
-                    try
-                    {
-                        d.SpaceCollide2(space, chr.Shell, IntPtr.Zero, nearCallback);
-                    }
-                    catch (AccessViolationException)
-                    {
-                        m_log.Warn("[PHYSICS]: Unable to space collide");
-                    }
-                    //float terrainheight = GetTerrainHeightAtXY(chr.Position.X, chr.Position.Y);
-                    //if (chr.Position.Z + (chr.Velocity.Z * timeStep) < terrainheight + 10)
-                    //{
-                    //chr.Position.Z = terrainheight + 10.0f;
-                    //forcedZ = true;
-                    //}
+                    d.SpaceCollide2(space, chr.Shell, IntPtr.Zero, nearCallback);
+                }
+                catch (AccessViolationException)
+                {
+                    m_log.Warn("[PHYSICS]: Unable to space collide");
                 }
+                //float terrainheight = GetTerrainHeightAtXY(chr.Position.X, chr.Position.Y);
+                //if (chr.Position.Z + (chr.Velocity.Z * timeStep) < terrainheight + 10)
+                //{
+                //chr.Position.Z = terrainheight + 10.0f;
+                //forcedZ = true;
+                //}
             }
 
             lock (_activeprims)
@@ -1716,28 +1725,22 @@ namespace OpenSim.Region.Physics.OdePlugin
 
         internal void AddCharacter(OdeCharacter chr)
         {
-            lock (_characters)
+            if (!_characters.Contains(chr))
             {
-                if (!_characters.Contains(chr))
-                {
-                    _characters.Add(chr);
+                _characters.Add(chr);
 
-                    if (chr.bad)
-                        m_log.ErrorFormat("[PHYSICS] Added BAD actor {0} to characters list", chr.m_uuid);
-                }
+                if (chr.bad)
+                    m_log.ErrorFormat("[PHYSICS] Added BAD actor {0} to characters list", chr.m_uuid);
             }
         }
 
         internal void RemoveCharacter(OdeCharacter chr)
         {
-            lock (_characters)
+            if (_characters.Contains(chr))
             {
-                if (_characters.Contains(chr))
-                {
-                    _characters.Remove(chr);
-                    geom_name_map.Remove(chr.Shell);
-                    actor_name_map.Remove(chr.Shell);
-                }
+                _characters.Remove(chr);
+                geom_name_map.Remove(chr.Shell);
+                actor_name_map.Remove(chr.Shell);
             }
         }
 
@@ -2797,13 +2800,21 @@ Console.WriteLine("AddPhysicsActorTaint to " + taintedprim.Name);
                         }
 
                         // Move characters
-                        lock (_characters)
+                        foreach (OdeCharacter actor in _characters)
                         {
-                            foreach (OdeCharacter actor in _characters)
+                            if (actor != null)
+                                actor.Move(defects);
+                        }
+
+                        if (defects.Count != 0)
+                        {
+                            foreach (OdeCharacter actor in defects)
                             {
-                                if (actor != null)
-                                    actor.Move();
+                                RemoveCharacter(actor);
+                                actor.DestroyOdeStructures();
                             }
+
+                            defects.Clear();
                         }
 
                         // Move other active objects
@@ -2860,18 +2871,26 @@ Console.WriteLine("AddPhysicsActorTaint to " + taintedprim.Name);
                     timeLeft -= ODE_STEPSIZE;
                 }
 
-                lock (_characters)
+                foreach (OdeCharacter actor in _characters)
                 {
-                    foreach (OdeCharacter actor in _characters)
+                    if (actor != null)
                     {
-                        if (actor != null)
-                        {
-                            if (actor.bad)
-                                m_log.WarnFormat("[PHYSICS]: BAD Actor {0} in _characters list was not removed?", actor.m_uuid);
+                        if (actor.bad)
+                            m_log.WarnFormat("[PHYSICS]: BAD Actor {0} in _characters list was not removed?", actor.m_uuid);
 
-                            actor.UpdatePositionAndVelocity();
-                        }
+                        actor.UpdatePositionAndVelocity(defects);
+                    }
+                }
+
+                if (defects.Count != 0)
+                {
+                    foreach (OdeCharacter actor in defects)
+                    {
+                        RemoveCharacter(actor);
+                        actor.DestroyOdeStructures();
                     }
+
+                    defects.Clear();
                 }
 
                 lock (_activeprims)
@@ -3899,30 +3918,28 @@ Console.WriteLine("AddPhysicsActorTaint to " + taintedprim.Name);
                 }
             }
             ds.SetColor(1.0f, 0.0f, 0.0f);
-            lock (_characters)
+
+            foreach (OdeCharacter chr in _characters)
             {
-                foreach (OdeCharacter chr in _characters)
+                if (chr.Shell != IntPtr.Zero)
                 {
-                    if (chr.Shell != IntPtr.Zero)
-                    {
-                        IntPtr body = d.GeomGetBody(chr.Shell);
+                    IntPtr body = d.GeomGetBody(chr.Shell);
 
-                        d.Vector3 pos;
-                        d.GeomCopyPosition(chr.Shell, out pos);
-                        //d.BodyCopyPosition(body, out pos);
+                    d.Vector3 pos;
+                    d.GeomCopyPosition(chr.Shell, out pos);
+                    //d.BodyCopyPosition(body, out pos);
 
-                        d.Matrix3 R;
-                        d.GeomCopyRotation(chr.Shell, out R);
-                        //d.BodyCopyRotation(body, out R);
+                    d.Matrix3 R;
+                    d.GeomCopyRotation(chr.Shell, out R);
+                    //d.BodyCopyRotation(body, out R);
 
-                        ds.DrawCapsule(ref pos, ref R, chr.Size.Z, 0.35f);
-                        d.Vector3 sides = new d.Vector3();
-                        sides.X = 0.5f;
-                        sides.Y = 0.5f;
-                        sides.Z = 0.5f;
+                    ds.DrawCapsule(ref pos, ref R, chr.Size.Z, 0.35f);
+                    d.Vector3 sides = new d.Vector3();
+                    sides.X = 0.5f;
+                    sides.Y = 0.5f;
+                    sides.Z = 0.5f;
 
-                        ds.DrawBox(ref pos, ref R, ref sides);
-                    }
+                    ds.DrawBox(ref pos, ref R, ref sides);
                 }
             }
         }