8 files changed, 159 insertions, 15 deletions

diff --git a/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsPlugin.cs b/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsPlugin.cs
index 13b3f1a..b7ed417 100644
--- a/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsPlugin.cs
+++ b/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsPlugin.cs
@@ -317,6 +317,11 @@ namespace OpenSim.Region.Physics.BasicPhysicsPlugin
             set { _velocity = value; }
         }
 
+        public override float CollisionScore
+        {
+            get { return 0f; }
+        }
+
         public override Quaternion Orientation
         {
             get { return Quaternion.Identity; }
diff --git a/OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs b/OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs
index fb32f93..1080aa4 100644
--- a/OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs
+++ b/OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs
@@ -769,7 +769,10 @@ namespace OpenSim.Region.Physics.BulletXPlugin
                 }
             }
         }
-
+        public override float CollisionScore
+        {
+            get { return 0f; }
+        }
         public override PhysicsVector Size
         {
             get { return _size; }
diff --git a/OpenSim/Region/Physics/Manager/PhysicsActor.cs b/OpenSim/Region/Physics/Manager/PhysicsActor.cs
index f97b279..7741687 100644
--- a/OpenSim/Region/Physics/Manager/PhysicsActor.cs
+++ b/OpenSim/Region/Physics/Manager/PhysicsActor.cs
@@ -172,6 +172,8 @@ namespace OpenSim.Region.Physics.Manager
 
         public abstract PhysicsVector Velocity { get; set; }
 
+        public abstract float CollisionScore { get;}
+
         public abstract PhysicsVector Acceleration { get; }
 
         public abstract Quaternion Orientation { get; set; }
@@ -272,6 +274,12 @@ namespace OpenSim.Region.Physics.Manager
             set { return; }
         }
 
+        public override float CollisionScore 
+        {
+            get { return 0f; }
+        }
+
+
         public override Quaternion Orientation
         {
             get { return Quaternion.Identity; }
diff --git a/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs
index 98069a0..9b75fb8 100644
--- a/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs
+++ b/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs
@@ -514,6 +514,11 @@ namespace OpenSim.Region.Physics.OdePlugin
             }
         }
 
+        public override float CollisionScore
+        {
+            get { return 0f; }
+        }
+
         public override bool Kinematic
         {
             get { return false; }
diff --git a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
index 5cdbb77..a063962 100644
--- a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
+++ b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
@@ -55,6 +55,7 @@ namespace OpenSim.Region.Physics.OdePlugin
         private bool m_taintPhysics = false;
         public bool m_taintremove = false;
         public bool m_taintdisable = false;
+        public bool m_disabled = false;
 
         private bool m_taintforce = false;
         private List<PhysicsVector> m_forcelist = new List<PhysicsVector>();
@@ -65,10 +66,15 @@ namespace OpenSim.Region.Physics.OdePlugin
         public IntPtr m_targetSpace = (IntPtr) 0;
         public IntPtr prim_geom;
         public IntPtr _triMeshData;
+
         private bool iscolliding = false;
         private bool m_isphysical = false;
         private bool m_throttleUpdates = false;
         private int throttleCounter = 0;
+        public int m_interpenetrationcount = 0;
+        public int m_collisionscore = 0;
+        public int m_roundsUnderMotionThreshold = 0;
+
         public bool outofBounds = false;
         private float m_density = 10.000006836f; // Aluminum g/cm3;
 
@@ -257,6 +263,10 @@ namespace OpenSim.Region.Physics.OdePlugin
             d.GeomSetBody(prim_geom, Body);
             d.BodySetAutoDisableFlag(Body, true);
             d.BodySetAutoDisableSteps(Body, 20);
+            
+            m_interpenetrationcount = 0;
+            m_collisionscore = 0;
+            m_disabled = false;
 
             _parent_scene.addActivePrim(this);
         }
@@ -383,6 +393,8 @@ namespace OpenSim.Region.Physics.OdePlugin
                 d.BodyDestroy(Body);
                 Body = (IntPtr) 0;
             }
+            m_disabled = true;
+            m_collisionscore = 0;
         }
 
         public void setMesh(OdeScene parent_scene, IMesh mesh)
@@ -425,7 +437,11 @@ namespace OpenSim.Region.Physics.OdePlugin
             if (IsPhysical && Body == (IntPtr) 0)
             {
                 // Recreate the body
+                m_interpenetrationcount = 0;
+                m_collisionscore = 0;
+                
                 enableBody();
+
             }
         }
 
@@ -485,7 +501,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                 _parent_scene.waitForSpaceUnlock(m_targetSpace);
                 d.SpaceAdd(m_targetSpace, prim_geom);
             }
-
+            resetCollisionAccounting();
             m_taintposition = _position;
         }
 
@@ -501,14 +517,23 @@ namespace OpenSim.Region.Physics.OdePlugin
             {
                 d.BodySetQuaternion(Body, ref myrot);
             }
-
+            resetCollisionAccounting();
             m_taintrot = _orientation;
         }
-        public void changedisable(float timestep)
+
+        private void resetCollisionAccounting()
         {
+            m_collisionscore = 0;
+            m_interpenetrationcount = 0;
+            m_disabled = false;
+        }
+
+        public void changedisable(float timestep)
+        {   
+            m_disabled = true;
             if (Body != (IntPtr) 0)
                 d.BodyDisable(Body);
-
+            
             m_taintdisable = false;
         }
 
@@ -528,8 +553,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                     disableBody();
                 }
             }
-
-
+            resetCollisionAccounting();
             m_taintPhysics = m_isphysical;
         }
 
@@ -670,7 +694,7 @@ namespace OpenSim.Region.Physics.OdePlugin
             }
 
             _parent_scene.geom_name_map[prim_geom] = oldname;
-
+            resetCollisionAccounting();
             m_taintsize = _size;
         }
 
@@ -724,7 +748,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                 d.GeomSetQuaternion(prim_geom, ref myrot);
             }
             _parent_scene.geom_name_map[prim_geom] = oldname;
-
+            resetCollisionAccounting();
             m_taintshape = false;
         }
 
@@ -746,6 +770,8 @@ namespace OpenSim.Region.Physics.OdePlugin
                 }
                 m_forcelist.Clear();
             }
+            m_collisionscore = 0;
+            m_interpenetrationcount = 0;
             m_taintforce = false;
 
         }
@@ -759,6 +785,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                     d.BodySetLinearVel(Body, m_taintVelocity.X, m_taintVelocity.Y, m_taintVelocity.Z);
                 }
             }
+            resetCollisionAccounting();
             m_taintVelocity = PhysicsVector.Zero;
         }
         public override bool IsPhysical
@@ -865,6 +892,11 @@ namespace OpenSim.Region.Physics.OdePlugin
             }
         }
 
+        public override float CollisionScore
+        {
+            get { return m_collisionscore; }
+        }
+
         public override bool Kinematic
         {
             get { return false; }
diff --git a/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs
index 1aa141b..a2f354a 100644
--- a/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs
+++ b/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs
@@ -89,6 +89,9 @@ namespace OpenSim.Region.Physics.OdePlugin
         private static float ODE_STEPSIZE = 0.020f;
         private static bool RENDER_FLAG = false;
         private static float metersInSpace = 29.9f;
+
+        private int interpenetrations_before_disable = 35;
+
         private IntPtr contactgroup;
         private IntPtr LandGeom = (IntPtr) 0;
         private float[] _heightmap;
@@ -109,13 +112,18 @@ namespace OpenSim.Region.Physics.OdePlugin
         private d.Contact AvatarMovementprimContact;
         private d.Contact AvatarMovementTerrainContact;
 
+        
+
         private int m_physicsiterations = 10;
         private float m_SkipFramesAtms = 0.40f; // Drop frames gracefully at a 400 ms lag
         private PhysicsActor PANull = new NullPhysicsActor();
         private float step_time = 0.0f;
+        private int ms = 0;
         public IntPtr world;
 
         public IntPtr space;
+
+        private IntPtr tmpSpace;
         // split static geometry collision handling into spaces of 30 meters
         public IntPtr[,] staticPrimspace = new IntPtr[(int) (300/metersInSpace),(int) (300/metersInSpace)];
 
@@ -206,6 +214,14 @@ namespace OpenSim.Region.Physics.OdePlugin
             }
         }
 
+        public void starttiming()
+        {
+            ms = Environment.TickCount;
+        }
+        public int stoptiming()
+        {
+            return Environment.TickCount - ms;
+        }
         // Initialize the mesh plugin
         public override void Initialise(IMesher meshmerizer)
         {
@@ -311,6 +327,7 @@ namespace OpenSim.Region.Physics.OdePlugin
 
                 for (int i = 0; i < count; i++)
                 {
+                    //m_log.Warn("[CCOUNT]: " + count);
                     IntPtr joint;
                     // If we're colliding with terrain, use 'TerrainContact' instead of contact.
                     // allows us to have different settings
@@ -405,13 +422,46 @@ namespace OpenSim.Region.Physics.OdePlugin
                         // If you interpenetrate a prim with another prim
                         if (p1.PhysicsActorType == (int) ActorTypes.Prim && p2.PhysicsActorType == (int) ActorTypes.Prim)
                         {
-                            if (contacts[i].depth >= 0.25f)
+                            OdePrim op1 = (OdePrim)p1;
+                            OdePrim op2 = (OdePrim)p2;
+                            op1.m_collisionscore++;
+                            op2.m_collisionscore++;
+                            
+
+                            if (op1.m_collisionscore > 80 || op2.m_collisionscore > 80)
                             {
-                                // Don't collide, one or both prim will explode.
-                                ((OdePrim)p1).m_taintdisable = true;
+                                op1.m_taintdisable = true;
                                 AddPhysicsActorTaint(p1);
-                                ((OdePrim)p2).m_taintdisable = true;
+                                op2.m_taintdisable = true;
                                 AddPhysicsActorTaint(p2);
+                            }
+                           
+                            if (contacts[i].depth >= 0.25f)
+                            {
+                                // Don't collide, one or both prim will explode.
+                                
+
+                                op1.m_interpenetrationcount++;
+                                op2.m_interpenetrationcount++;
+                                interpenetrations_before_disable = 20;
+                                if (op1.m_interpenetrationcount >= interpenetrations_before_disable)
+                                {
+                                    op1.m_taintdisable = true;
+                                    AddPhysicsActorTaint(p1);
+                                }
+                                if (op2.m_interpenetrationcount >= interpenetrations_before_disable)
+                                {
+                                    op2.m_taintdisable = true;
+                                    AddPhysicsActorTaint(p2);
+                                }
+
+                               
+                                //contacts[i].depth = contacts[i].depth / 8f;
+                                //contacts[i].normal = new d.Vector3(0, 0, 1);
+                            }
+                            if (op1.m_disabled || op2.m_disabled)
+                            {
+                                //Manually disabled objects stay disabled 
                                 contacts[i].depth = 0f;
                             }
                         }
@@ -531,6 +581,7 @@ namespace OpenSim.Region.Physics.OdePlugin
         /// <param name="timeStep"></param>
         private void collision_optimized(float timeStep)
         {
+            starttiming();
             foreach (OdeCharacter chr in _characters)
             {
                 // Reset the collision values to false
@@ -554,6 +605,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                     //forcedZ = true;
                 //}
             }
+            int avms = stoptiming();
 
             // If the sim is running slow this frame, 
             // don't process collision for prim!
@@ -562,9 +614,11 @@ namespace OpenSim.Region.Physics.OdePlugin
                 foreach (OdePrim chr in _activeprims)
                 {
                     // This if may not need to be there..    it might be skipped anyway.
-                    if (d.BodyIsEnabled(chr.Body))
+                    if (d.BodyIsEnabled(chr.Body) && (!chr.m_disabled))
                     {
+                        
                         d.SpaceCollide2(space, chr.prim_geom, IntPtr.Zero, nearCallback);
+                        //calculateSpaceForGeom(chr.Position)
                         //foreach (OdePrim ch2 in _prims)
                         /// should be a separate space -- lots of avatars will be N**2 slow
                         //{
@@ -580,6 +634,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                         //}
                         //}
                     }
+                    d.SpaceCollide2(LandGeom, chr.prim_geom, IntPtr.Zero, nearCallback);
                 }
             }
             else
@@ -593,6 +648,9 @@ namespace OpenSim.Region.Physics.OdePlugin
                     {
                         // Collide test the prims with the terrain..   since if you don't do this, 
                         // next frame, all of the physical prim in the scene will awaken and explode upwards
+                        tmpSpace = calculateSpaceForGeom(chr.Position);
+                        if (tmpSpace != (IntPtr) 0 && d.GeomIsSpace(tmpSpace))
+                        d.SpaceCollide2(calculateSpaceForGeom(chr.Position), chr.prim_geom, IntPtr.Zero, nearCallback);
                         d.SpaceCollide2(LandGeom, chr.prim_geom, IntPtr.Zero, nearCallback);
                     }
                 }
@@ -1140,7 +1198,14 @@ namespace OpenSim.Region.Physics.OdePlugin
                     }
 
                     collision_optimized(timeStep);
-                    d.WorldQuickStep(world, ODE_STEPSIZE);
+                    try
+                    {
+                        d.WorldQuickStep(world, ODE_STEPSIZE);
+                    }
+                    catch (StackOverflowException)
+                    {
+                        d.WorldQuickStep(world, 0.001f);
+                    }
                     d.JointGroupEmpty(contactgroup);
                     foreach (OdeCharacter actor in _characters)
                     {
@@ -1165,6 +1230,12 @@ namespace OpenSim.Region.Physics.OdePlugin
                         RemovePrimThreadLocked(prim);
                     }
                     processedtaints = true;
+                    prim.m_collisionscore = 0;
+                }
+
+                foreach (OdePrim prim in _activeprims)
+                {
+                    prim.m_collisionscore = 0;
                 }
 
                 if (processedtaints)
diff --git a/OpenSim/Region/Physics/POSPlugin/POSPlugin.cs b/OpenSim/Region/Physics/POSPlugin/POSPlugin.cs
index 079e66a..5b132cf 100644
--- a/OpenSim/Region/Physics/POSPlugin/POSPlugin.cs
+++ b/OpenSim/Region/Physics/POSPlugin/POSPlugin.cs
@@ -446,6 +446,11 @@ namespace OpenSim.Region.Physics.POSPlugin
             set { _target_velocity = value; }
         }
 
+        public override float CollisionScore
+        {
+            get { return 0f; }
+        }
+
         public override Quaternion Orientation
         {
             get { return Quaternion.Identity; }
@@ -579,6 +584,11 @@ namespace OpenSim.Region.Physics.POSPlugin
             set { _velocity = value; }
         }
 
+        public override float CollisionScore
+        {
+            get { return 0f; }
+        }
+
         public override Quaternion Orientation
         {
             get { return _orientation; }
diff --git a/OpenSim/Region/Physics/PhysXPlugin/PhysXPlugin.cs b/OpenSim/Region/Physics/PhysXPlugin/PhysXPlugin.cs
index 509bdfa..ad18507 100644
--- a/OpenSim/Region/Physics/PhysXPlugin/PhysXPlugin.cs
+++ b/OpenSim/Region/Physics/PhysXPlugin/PhysXPlugin.cs
@@ -325,6 +325,11 @@ namespace OpenSim.Region.Physics.PhysXPlugin
             set { _velocity = value; }
         }
 
+        public override float CollisionScore
+        {
+            get { return 0f; }
+        }
+
         public override bool Kinematic
         {
             get { return false; }
@@ -503,6 +508,11 @@ namespace OpenSim.Region.Physics.PhysXPlugin
             set { _velocity = value; }
         }
 
+        public override float CollisionScore
+        {
+            get { return 0f; }
+        }
+
         public override bool Kinematic
         {
             get { return _prim.Kinematic; }