2 files changed, 65 insertions, 13 deletions

diff --git a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
index 0716214..e7b3b2b 100644
--- a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
+++ b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
@@ -63,6 +63,9 @@ namespace OpenSim.Region.Physics.OdePlugin
 
         private bool m_isphysical;
 
+        public int ExpectedCollisionContacts { get { return m_expectedCollisionContacts; } }
+        private int m_expectedCollisionContacts = 0;
+
         /// <summary>
         /// Is this prim subject to physics?  Even if not, it's still solid for collision purposes.
         /// </summary>
@@ -150,7 +153,7 @@ namespace OpenSim.Region.Physics.OdePlugin
 
         private PrimitiveBaseShape _pbs;
         private OdeScene _parent_scene;
-
+        
         /// <summary>
         /// The physics space which contains prim geometries
         /// </summary>
@@ -840,7 +843,7 @@ namespace OpenSim.Region.Physics.OdePlugin
             int vertexStride, triStride;
             mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); // Note, that vertices are fixed in unmanaged heap
             mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); // Also fixed, needs release after usage
-
+            m_expectedCollisionContacts = indexCount;
             mesh.releaseSourceMeshData(); // free up the original mesh data to save memory
 
             // We must lock here since m_MeshToTriMeshMap is static and multiple scene threads may call this method at
@@ -1377,6 +1380,7 @@ Console.WriteLine("CreateGeom:");
                             {
 //Console.WriteLine(" CreateGeom 1");
                                 SetGeom(d.CreateSphere(m_targetSpace, _size.X / 2));
+                                m_expectedCollisionContacts = 3;
                             }
                             catch (AccessViolationException)
                             {
@@ -1391,6 +1395,7 @@ Console.WriteLine("CreateGeom:");
                             {
 //Console.WriteLine(" CreateGeom 2");
                                 SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
+                                m_expectedCollisionContacts = 4;
                             }
                             catch (AccessViolationException)
                             {
@@ -1406,6 +1411,7 @@ Console.WriteLine("CreateGeom:");
                         {
 //Console.WriteLine("  CreateGeom 3");
                             SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
+                            m_expectedCollisionContacts = 4;
                         }
                         catch (AccessViolationException)
                         {
@@ -1421,6 +1427,7 @@ Console.WriteLine("CreateGeom:");
                     {
 //Console.WriteLine("  CreateGeom 4");
                         SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
+                        m_expectedCollisionContacts = 4;
                     }
                     catch (AccessViolationException)
                     {
@@ -1446,11 +1453,13 @@ Console.WriteLine("CreateGeom:");
                     _parent_scene.geom_name_map.Remove(prim_geom);
                     _parent_scene.actor_name_map.Remove(prim_geom);
                     d.GeomDestroy(prim_geom);
+                    m_expectedCollisionContacts = 0;
                     prim_geom = IntPtr.Zero;
                 }
                 catch (System.AccessViolationException)
                 {
                     prim_geom = IntPtr.Zero;
+                    m_expectedCollisionContacts = 0;
                     m_log.ErrorFormat("[PHYSICS]: PrimGeom dead for {0}", Name);
 
                     return false;
@@ -2395,15 +2404,15 @@ Console.WriteLine(" JointCreateFixed");
         {
             get
             {
-                // Averate previous velocity with the new one so
+                // Average previous velocity with the new one so
                 // client object interpolation works a 'little' better
                 if (_zeroFlag)
                     return Vector3.Zero;
 
                 Vector3 returnVelocity = Vector3.Zero;
-                returnVelocity.X = (m_lastVelocity.X + _velocity.X)/2;
-                returnVelocity.Y = (m_lastVelocity.Y + _velocity.Y)/2;
-                returnVelocity.Z = (m_lastVelocity.Z + _velocity.Z)/2;
+                returnVelocity.X = (m_lastVelocity.X + _velocity.X) * 0.5f; // 0.5f is mathematically equiv to '/ 2'
+                returnVelocity.Y = (m_lastVelocity.Y + _velocity.Y) * 0.5f;
+                returnVelocity.Z = (m_lastVelocity.Z + _velocity.Z) * 0.5f;
                 return returnVelocity;
             }
             set
@@ -2600,6 +2609,7 @@ Console.WriteLine(" JointCreateFixed");
             {
                 Vector3 pv = Vector3.Zero;
                 bool lastZeroFlag = _zeroFlag;
+                float m_minvelocity = 0;
                 if (Body != (IntPtr)0) // FIXME -> or if it is a joint
                 {
                     d.Vector3 vec = d.BodyGetPosition(Body);
@@ -2752,8 +2762,21 @@ Console.WriteLine(" JointCreateFixed");
                         _acceleration = ((_velocity - m_lastVelocity) / 0.1f);
                         _acceleration = new Vector3(_velocity.X - m_lastVelocity.X / 0.1f, _velocity.Y - m_lastVelocity.Y / 0.1f, _velocity.Z - m_lastVelocity.Z / 0.1f);
                         //m_log.Info("[PHYSICS]: V1: " + _velocity + " V2: " + m_lastVelocity + " Acceleration: " + _acceleration.ToString());
+                       
+                        // Note here that linearvelocity is affecting angular velocity...  so I'm guessing this is a vehicle specific thing... 
+                        // it does make sense to do this for tiny little instabilities with physical prim, however 0.5m/frame is fairly large. 
+                        // reducing this to 0.02m/frame seems to help the angular rubberbanding quite a bit, however, to make sure it doesn't affect elevators and vehicles
+                        // adding these logical exclusion situations to maintain this where I think it was intended to be.
+                        if (m_throttleUpdates || m_usePID || (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) || (Amotor != IntPtr.Zero)) 
+                        {
+                            m_minvelocity = 0.5f;
+                        }
+                        else
+                        {
+                            m_minvelocity = 0.02f;
+                        }
 
-                        if (_velocity.ApproxEquals(pv, 0.5f))
+                        if (_velocity.ApproxEquals(pv, m_minvelocity))
                         {
                             m_rotationalVelocity = pv;
                         }
diff --git a/OpenSim/Region/Physics/OdePlugin/OdeScene.cs b/OpenSim/Region/Physics/OdePlugin/OdeScene.cs
index 929b019..7a50c4c 100644
--- a/OpenSim/Region/Physics/OdePlugin/OdeScene.cs
+++ b/OpenSim/Region/Physics/OdePlugin/OdeScene.cs
@@ -336,6 +336,7 @@ namespace OpenSim.Region.Physics.OdePlugin
 
         public int geomContactPointsStartthrottle = 3;
         public int geomUpdatesPerThrottledUpdate = 15;
+        private const int avatarExpectedContacts = 3;
 
         public float bodyPIDD = 35f;
         public float bodyPIDG = 25;
@@ -474,6 +475,8 @@ namespace OpenSim.Region.Physics.OdePlugin
         private OdePrim cp1;
         private OdeCharacter cc2;
         private OdePrim cp2;
+        private int p1ExpectedPoints = 0;
+        private int p2ExpectedPoints = 0;
         //private int cStartStop = 0;
         //private string cDictKey = "";
 
@@ -498,6 +501,7 @@ namespace OpenSim.Region.Physics.OdePlugin
         public int physics_logging_interval = 0;
         public bool physics_logging_append_existing_logfile = false;
 
+
         public d.Vector3 xyz = new d.Vector3(128.1640f, 128.3079f, 25.7600f);
         public d.Vector3 hpr = new d.Vector3(125.5000f, -17.0000f, 0.0000f);
 
@@ -644,7 +648,7 @@ namespace OpenSim.Region.Physics.OdePlugin
 
                     contactsPerCollision = physicsconfig.GetInt("contacts_per_collision", 80);
 
-                    geomContactPointsStartthrottle = physicsconfig.GetInt("geom_contactpoints_start_throttling", 3);
+                    geomContactPointsStartthrottle = physicsconfig.GetInt("geom_contactpoints_start_throttling", 5);
                     geomUpdatesPerThrottledUpdate = physicsconfig.GetInt("geom_updates_before_throttled_update", 15);
                     geomCrossingFailuresBeforeOutofbounds = physicsconfig.GetInt("geom_crossing_failures_before_outofbounds", 5);
 
@@ -1064,7 +1068,10 @@ namespace OpenSim.Region.Physics.OdePlugin
 
             PhysicsActor p1;
             PhysicsActor p2;
-
+            
+            p1ExpectedPoints = 0;
+            p2ExpectedPoints = 0;
+            
             if (!actor_name_map.TryGetValue(g1, out p1))
             {
                 p1 = PANull;
@@ -1121,9 +1128,13 @@ namespace OpenSim.Region.Physics.OdePlugin
                 switch (p1.PhysicsActorType)
                 {
                     case (int)ActorTypes.Agent:
+                        p1ExpectedPoints = avatarExpectedContacts;
                         p2.CollidingObj = true;
                         break;
                     case (int)ActorTypes.Prim:
+                        if (p1 != null && p1 is OdePrim)
+                            p1ExpectedPoints = ((OdePrim) p1).ExpectedCollisionContacts;
+
                         if (p2.Velocity.LengthSquared() > 0.0f)
                             p2.CollidingObj = true;
                         break;
@@ -1319,6 +1330,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                         if ((p2.PhysicsActorType == (int) ActorTypes.Agent) &&
                             (Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f))
                         {
+                            p2ExpectedPoints = avatarExpectedContacts;
                             // Avatar is moving on terrain, use the movement terrain contact
                             AvatarMovementTerrainContact.geom = curContact;
 
@@ -1332,6 +1344,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                         {
                             if (p2.PhysicsActorType == (int)ActorTypes.Agent)
                             {
+                                p2ExpectedPoints = avatarExpectedContacts;
                                 // Avatar is standing on terrain, use the non moving terrain contact
                                 TerrainContact.geom = curContact;
 
@@ -1356,9 +1369,18 @@ namespace OpenSim.Region.Physics.OdePlugin
                                     }
 
                                     if (p2 is OdePrim)
-                                        material = ((OdePrim)p2).m_material;
-
+                                    {
+                                        material = ((OdePrim) p2).m_material;
+                                        p2ExpectedPoints = ((OdePrim)p2).ExpectedCollisionContacts;
+                                    }
+                                   
+                                    // Unnessesary because p1 is defined above
+                                    //if (p1 is OdePrim)
+                                    // { 
+                                    //     p1ExpectedPoints = ((OdePrim)p1).ExpectedCollisionContacts;
+                                    // }
                                     //m_log.DebugFormat("Material: {0}", material);
+
                                     m_materialContacts[material, movintYN].geom = curContact;
 
                                     if (m_global_contactcount < maxContactsbeforedeath)
@@ -1379,7 +1401,10 @@ namespace OpenSim.Region.Physics.OdePlugin
                                     int material = (int)Material.Wood;
 
                                     if (p2 is OdePrim)
+                                    {
                                         material = ((OdePrim)p2).m_material;
+                                        p2ExpectedPoints = ((OdePrim)p2).ExpectedCollisionContacts;
+                                    }
 
                                     //m_log.DebugFormat("Material: {0}", material);
                                     m_materialContacts[material, movintYN].geom = curContact;
@@ -1429,6 +1454,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                     {
                         if ((p2.PhysicsActorType == (int)ActorTypes.Agent))
                         {
+                            p2ExpectedPoints = avatarExpectedContacts;
                             if ((Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f))
                             {
                                 // Avatar is moving on a prim, use the Movement prim contact
@@ -1458,7 +1484,10 @@ namespace OpenSim.Region.Physics.OdePlugin
                             int material = (int)Material.Wood;
 
                             if (p2 is OdePrim)
+                            {
                                 material = ((OdePrim)p2).m_material;
+                                p2ExpectedPoints = ((OdePrim)p2).ExpectedCollisionContacts;
+                            }
                             
                             //m_log.DebugFormat("Material: {0}", material);
                             m_materialContacts[material, 0].geom = curContact;
@@ -1479,8 +1508,8 @@ namespace OpenSim.Region.Physics.OdePlugin
                 }
 
                 collision_accounting_events(p1, p2, maxDepthContact);
-
-                if (count > geomContactPointsStartthrottle)
+                
+                if (count > ((p1ExpectedPoints + p2ExpectedPoints) * 0.25) + (geomContactPointsStartthrottle))
                 {
                     // If there are more then 3 contact points, it's likely
                     // that we've got a pile of objects, so ...