3 files changed, 94 insertions, 72 deletions

diff --git a/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs
index c86bc62..1bc4a25 100644
--- a/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs
+++ b/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs
@@ -1209,11 +1209,11 @@ namespace OpenSim.Region.Physics.OdePlugin
             m_requestedUpdateFrequency = 0;
             m_eventsubscription = 0;
         }
-        public void AddCollisionEvent(uint CollidedWith, float depth)
+        public void AddCollisionEvent(uint CollidedWith, ContactPoint contact)
         {
             if (m_eventsubscription > 0)
             {
-                CollisionEventsThisFrame.addCollider(CollidedWith, depth);
+                CollisionEventsThisFrame.addCollider(CollidedWith, contact);
             }
         }
 
diff --git a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
index 49bbab9..fa42023 100644
--- a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
+++ b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
@@ -2958,11 +2958,11 @@ Console.WriteLine(" JointCreateFixed");
             m_eventsubscription = 0;
         }
 
-        public void AddCollisionEvent(uint CollidedWith, float depth)
+        public void AddCollisionEvent(uint CollidedWith, ContactPoint contact)
         {
             if (CollisionEventsThisFrame == null)
                 CollisionEventsThisFrame = new CollisionEventUpdate();
-            CollisionEventsThisFrame.addCollider(CollidedWith,depth);
+            CollisionEventsThisFrame.addCollider(CollidedWith, contact);
         }
 
         public void SendCollisions()
diff --git a/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs
index 73ad15e..3c9a31d 100644
--- a/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs
+++ b/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs
@@ -159,6 +159,7 @@ namespace OpenSim.Region.Physics.OdePlugin
 
         private float ODE_STEPSIZE = 0.020f;
         private float metersInSpace = 29.9f;
+        private float m_timeDilation = 1.0f;
 
         public float gravityx = 0f;
         public float gravityy = 0f;
@@ -177,8 +178,8 @@ namespace OpenSim.Region.Physics.OdePlugin
         //private int m_returncollisions = 10;
 
         private readonly IntPtr contactgroup;
-        internal IntPtr LandGeom;
 
+        internal IntPtr LandGeom;
         internal IntPtr WaterGeom;
 
         private float nmTerrainContactFriction = 255.0f;
@@ -250,7 +251,7 @@ namespace OpenSim.Region.Physics.OdePlugin
         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>>();
-        private d.ContactGeom[] contacts = new d.ContactGeom[80];
+        private d.ContactGeom[] contacts;
         private readonly List<PhysicsJoint> requestedJointsToBeCreated = new List<PhysicsJoint>(); // lock only briefly. accessed by external code (to request new joints) and by OdeScene.Simulate() to move those joints into pending/active
         private readonly List<PhysicsJoint> pendingJoints = new List<PhysicsJoint>(); // can lock for longer. accessed only by OdeScene.
         private readonly List<PhysicsJoint> activeJoints = new List<PhysicsJoint>(); // can lock for longer. accessed only by OdeScene.
@@ -396,6 +397,8 @@ namespace OpenSim.Region.Physics.OdePlugin
                 avStandupTensor = 550000f;
             }
 
+            int contactsPerCollision = 80;
+
             if (m_config != null)
             {
                 IConfig physicsconfig = m_config.Configs["ODEPhysicsSettings"];
@@ -438,6 +441,8 @@ namespace OpenSim.Region.Physics.OdePlugin
                     avCapRadius = physicsconfig.GetFloat("av_capsule_radius", 0.37f);
                     avCapsuleTilted = physicsconfig.GetBoolean("av_capsule_tilted", true);
 
+                    contactsPerCollision = physicsconfig.GetInt("contacts_per_collision", 80);
+
                     geomContactPointsStartthrottle = physicsconfig.GetInt("geom_contactpoints_start_throttling", 3);
                     geomUpdatesPerThrottledUpdate = physicsconfig.GetInt("geom_updates_before_throttled_update", 15);
                     geomCrossingFailuresBeforeOutofbounds = physicsconfig.GetInt("geom_crossing_failures_before_outofbounds", 5);
@@ -475,10 +480,11 @@ namespace OpenSim.Region.Physics.OdePlugin
 
                     m_NINJA_physics_joints_enabled = physicsconfig.GetBoolean("use_NINJA_physics_joints", false);
                     minimumGroundFlightOffset = physicsconfig.GetFloat("minimum_ground_flight_offset", 3f);
-
                 }
             }
 
+            contacts = new d.ContactGeom[contactsPerCollision];
+
             staticPrimspace = new IntPtr[(int)(300 / metersInSpace), (int)(300 / metersInSpace)];
 
             // Centeral contact friction and bounce
@@ -771,7 +777,9 @@ namespace OpenSim.Region.Physics.OdePlugin
 
                 lock (contacts)
                 {
-                    count = d.Collide(g1, g2, contacts.GetLength(0), contacts, d.ContactGeom.SizeOf);
+                    count = d.Collide(g1, g2, contacts.Length, contacts, d.ContactGeom.SizeOf);
+                    if (count > contacts.Length)
+                        m_log.Error("[PHYSICS]: Got " + count + " contacts when we asked for a maximum of " + contacts.Length);
                 }
             }
             catch (SEHException)
@@ -799,7 +807,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                 p2 = PANull;
             }
 
-            float max_collision_depth = 0f;
+            ContactPoint maxDepthContact = new ContactPoint();
             if (p1.CollisionScore + count >= float.MaxValue)
                 p1.CollisionScore = 0;
             p1.CollisionScore += count;
@@ -810,9 +818,17 @@ namespace OpenSim.Region.Physics.OdePlugin
 
             for (int i = 0; i < count; i++)
             {
+                d.ContactGeom curContact = contacts[i];
 
+                if (curContact.depth > maxDepthContact.PenetrationDepth)
+                {
+                    maxDepthContact = new ContactPoint(
+                        new Vector3(curContact.pos.X, curContact.pos.Y, curContact.pos.Z),
+                        new Vector3(curContact.normal.X, curContact.normal.Y, curContact.normal.Z),
+                        curContact.depth
+                    );
+                }
 
-                max_collision_depth = (contacts[i].depth > max_collision_depth) ? contacts[i].depth : max_collision_depth;
                 //m_log.Warn("[CCOUNT]: " + count);
                 IntPtr joint;
                 // If we're colliding with terrain, use 'TerrainContact' instead of contact.
@@ -829,7 +845,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                         p2.CollidingObj = true;
                         break;
                     case (int)ActorTypes.Prim:
-                        if (p2.Velocity.X > 0 || p2.Velocity.Y > 0 || p2.Velocity.Z > 0)
+                        if (p2.Velocity.LengthSquared() > 0.0f)
                             p2.CollidingObj = true;
                         break;
                     case (int)ActorTypes.Unknown:
@@ -845,14 +861,14 @@ namespace OpenSim.Region.Physics.OdePlugin
                 #region InterPenetration Handling - Unintended physics explosions
 # region disabled code1
 
-                if (contacts[i].depth >= 0.08f)
+                if (curContact.depth >= 0.08f)
                 {
                     //This is disabled at the moment only because it needs more tweaking
                     //It will eventually be uncommented
                     /*
-                    if (contacts[i].depth >= 1.00f)
+                    if (contact.depth >= 1.00f)
                     {
-                        //m_log.Debug("[PHYSICS]: " + contacts[i].depth.ToString());
+                        //m_log.Debug("[PHYSICS]: " + contact.depth.ToString());
                     }
 
                     //If you interpenetrate a prim with an agent
@@ -862,37 +878,37 @@ namespace OpenSim.Region.Physics.OdePlugin
                          p2.PhysicsActorType == (int) ActorTypes.Prim))
                     {
                         
-                        //contacts[i].depth = contacts[i].depth * 4.15f;
+                        //contact.depth = contact.depth * 4.15f;
                         /*
                         if (p2.PhysicsActorType == (int) ActorTypes.Agent)
                         {
                             p2.CollidingObj = true;
-                            contacts[i].depth = 0.003f;
+                            contact.depth = 0.003f;
                             p2.Velocity = p2.Velocity + new PhysicsVector(0, 0, 2.5f);
                             OdeCharacter character = (OdeCharacter) p2;
                             character.SetPidStatus(true);
-                            contacts[i].pos = new d.Vector3(contacts[i].pos.X + (p1.Size.X / 2), contacts[i].pos.Y + (p1.Size.Y / 2), contacts[i].pos.Z + (p1.Size.Z / 2));
+                            contact.pos = new d.Vector3(contact.pos.X + (p1.Size.X / 2), contact.pos.Y + (p1.Size.Y / 2), contact.pos.Z + (p1.Size.Z / 2));
 
                         }
                         else
                         {
 
-                            //contacts[i].depth = 0.0000000f;
+                            //contact.depth = 0.0000000f;
                         }
                         if (p1.PhysicsActorType == (int) ActorTypes.Agent)
                         {
 
                             p1.CollidingObj = true;
-                            contacts[i].depth = 0.003f;
+                            contact.depth = 0.003f;
                             p1.Velocity = p1.Velocity + new PhysicsVector(0, 0, 2.5f);
-                            contacts[i].pos = new d.Vector3(contacts[i].pos.X + (p2.Size.X / 2), contacts[i].pos.Y + (p2.Size.Y / 2), contacts[i].pos.Z + (p2.Size.Z / 2));
+                            contact.pos = new d.Vector3(contact.pos.X + (p2.Size.X / 2), contact.pos.Y + (p2.Size.Y / 2), contact.pos.Z + (p2.Size.Z / 2));
                             OdeCharacter character = (OdeCharacter)p1;
                             character.SetPidStatus(true);
                         }
                         else
                         {
 
-                            //contacts[i].depth = 0.0000000f;
+                            //contact.depth = 0.0000000f;
                         }
                           
                         
@@ -917,7 +933,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                             //AddPhysicsActorTaint(p2);
                         //}
 
-                        //if (contacts[i].depth >= 0.25f)
+                        //if (contact.depth >= 0.25f)
                         //{
                             // Don't collide, one or both prim will expld.
 
@@ -935,21 +951,21 @@ namespace OpenSim.Region.Physics.OdePlugin
                                 //AddPhysicsActorTaint(p2);
                             //}
 
-                            //contacts[i].depth = contacts[i].depth / 8f;
-                            //contacts[i].normal = new d.Vector3(0, 0, 1);
+                            //contact.depth = contact.depth / 8f;
+                            //contact.normal = new d.Vector3(0, 0, 1);
                         //}
                         //if (op1.m_disabled || op2.m_disabled)
                         //{
                             //Manually disabled objects stay disabled
-                            //contacts[i].depth = 0f;
+                            //contact.depth = 0f;
                         //}
                         #endregion
                     }
                     */
 #endregion
-                    if (contacts[i].depth >= 1.00f)
+                    if (curContact.depth >= 1.00f)
                     {
-                        //m_log.Info("[P]: " + contacts[i].depth.ToString());
+                        //m_log.Info("[P]: " + contact.depth.ToString());
                         if ((p2.PhysicsActorType == (int) ActorTypes.Agent &&
                              p1.PhysicsActorType == (int) ActorTypes.Unknown) ||
                             (p1.PhysicsActorType == (int) ActorTypes.Agent &&
@@ -962,12 +978,12 @@ namespace OpenSim.Region.Physics.OdePlugin
                                     OdeCharacter character = (OdeCharacter) p2;
 
                                     //p2.CollidingObj = true;
-                                    contacts[i].depth = 0.00000003f;
+                                    curContact.depth = 0.00000003f;
                                     p2.Velocity = p2.Velocity + new Vector3(0f, 0f, 0.5f);
-                                    contacts[i].pos =
-                                        new d.Vector3(contacts[i].pos.X + (p1.Size.X/2),
-                                                      contacts[i].pos.Y + (p1.Size.Y/2),
-                                                      contacts[i].pos.Z + (p1.Size.Z/2));
+                                    curContact.pos =
+                                        new d.Vector3(curContact.pos.X + (p1.Size.X/2),
+                                                      curContact.pos.Y + (p1.Size.Y/2),
+                                                      curContact.pos.Z + (p1.Size.Z/2));
                                     character.SetPidStatus(true);
                                 }
                             }
@@ -980,12 +996,12 @@ namespace OpenSim.Region.Physics.OdePlugin
                                     OdeCharacter character = (OdeCharacter) p1;
 
                                     //p2.CollidingObj = true;
-                                    contacts[i].depth = 0.00000003f;
+                                    curContact.depth = 0.00000003f;
                                     p1.Velocity = p1.Velocity + new Vector3(0f, 0f, 0.5f);
-                                    contacts[i].pos =
-                                        new d.Vector3(contacts[i].pos.X + (p1.Size.X/2),
-                                                      contacts[i].pos.Y + (p1.Size.Y/2),
-                                                      contacts[i].pos.Z + (p1.Size.Z/2));
+                                    curContact.pos =
+                                        new d.Vector3(curContact.pos.X + (p1.Size.X/2),
+                                                      curContact.pos.Y + (p1.Size.Y/2),
+                                                      curContact.pos.Z + (p1.Size.Z/2));
                                     character.SetPidStatus(true);
                                 }
                             }
@@ -1007,16 +1023,15 @@ namespace OpenSim.Region.Physics.OdePlugin
                 if (!skipThisContact && (p2 is OdePrim) && (((OdePrim)p2).m_isVolumeDetect))
                     skipThisContact = true;   // No collision on volume detect prims
 
-                if (!skipThisContact && contacts[i].depth < 0f)
+                if (!skipThisContact && curContact.depth < 0f)
                     skipThisContact = true;
 
-                if (!skipThisContact && checkDupe(contacts[i], p2.PhysicsActorType))
+                if (!skipThisContact && checkDupe(curContact, p2.PhysicsActorType))
                     skipThisContact = true;
 
-                int maxContactsbeforedeath = 4000;
+                const int maxContactsbeforedeath = 4000;
                 joint = IntPtr.Zero;
 
-
                 if (!skipThisContact)
                 {
                     // If we're colliding against terrain
@@ -1027,8 +1042,8 @@ namespace OpenSim.Region.Physics.OdePlugin
                             (Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f))
                         {
                             // Use the movement terrain contact
-                            AvatarMovementTerrainContact.geom = contacts[i];
-                            _perloopContact.Add(contacts[i]);
+                            AvatarMovementTerrainContact.geom = curContact;
+                            _perloopContact.Add(curContact);
                             if (m_global_contactcount < maxContactsbeforedeath)
                             {
                                 joint = d.JointCreateContact(world, contactgroup, ref AvatarMovementTerrainContact);
@@ -1040,8 +1055,8 @@ namespace OpenSim.Region.Physics.OdePlugin
                             if (p2.PhysicsActorType == (int)ActorTypes.Agent)
                             {
                                 // Use the non moving terrain contact
-                                TerrainContact.geom = contacts[i];
-                                _perloopContact.Add(contacts[i]);
+                                TerrainContact.geom = curContact;
+                                _perloopContact.Add(curContact);
                                 if (m_global_contactcount < maxContactsbeforedeath)
                                 {
                                     joint = d.JointCreateContact(world, contactgroup, ref TerrainContact);
@@ -1066,8 +1081,8 @@ namespace OpenSim.Region.Physics.OdePlugin
                                         material = ((OdePrim)p2).m_material;
 
                                     //m_log.DebugFormat("Material: {0}", material);
-                                    m_materialContacts[material, movintYN].geom = contacts[i];
-                                    _perloopContact.Add(contacts[i]);
+                                    m_materialContacts[material, movintYN].geom = curContact;
+                                    _perloopContact.Add(curContact);
 
                                     if (m_global_contactcount < maxContactsbeforedeath)
                                     {
@@ -1092,8 +1107,8 @@ namespace OpenSim.Region.Physics.OdePlugin
                                     if (p2 is OdePrim)
                                         material = ((OdePrim)p2).m_material;
                                     //m_log.DebugFormat("Material: {0}", material);
-                                    m_materialContacts[material, movintYN].geom = contacts[i];
-                                    _perloopContact.Add(contacts[i]);
+                                    m_materialContacts[material, movintYN].geom = curContact;
+                                    _perloopContact.Add(curContact);
 
                                     if (m_global_contactcount < maxContactsbeforedeath)
                                     {
@@ -1121,20 +1136,20 @@ namespace OpenSim.Region.Physics.OdePlugin
                         */
                         //WaterContact.surface.soft_cfm = 0.0000f;
                         //WaterContact.surface.soft_erp = 0.00000f;
-                        if (contacts[i].depth > 0.1f)
+                        if (curContact.depth > 0.1f)
                         {
-                            contacts[i].depth *= 52;
-                            //contacts[i].normal = new d.Vector3(0, 0, 1);
-                            //contacts[i].pos = new d.Vector3(0, 0, contacts[i].pos.Z - 5f);
+                            curContact.depth *= 52;
+                            //contact.normal = new d.Vector3(0, 0, 1);
+                            //contact.pos = new d.Vector3(0, 0, contact.pos.Z - 5f);
                         }
-                        WaterContact.geom = contacts[i];
-                        _perloopContact.Add(contacts[i]);
+                        WaterContact.geom = curContact;
+                        _perloopContact.Add(curContact);
                         if (m_global_contactcount < maxContactsbeforedeath)
                         {
                             joint = d.JointCreateContact(world, contactgroup, ref WaterContact);
                             m_global_contactcount++;
                         }
-                        //m_log.Info("[PHYSICS]: Prim Water Contact" + contacts[i].depth);
+                        //m_log.Info("[PHYSICS]: Prim Water Contact" + contact.depth);
                     }
                     else
                     {
@@ -1145,8 +1160,8 @@ namespace OpenSim.Region.Physics.OdePlugin
                             if ((Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f))
                             {
                                 // Use the Movement prim contact
-                                AvatarMovementprimContact.geom = contacts[i];
-                                _perloopContact.Add(contacts[i]);
+                                AvatarMovementprimContact.geom = curContact;
+                                _perloopContact.Add(curContact);
                                 if (m_global_contactcount < maxContactsbeforedeath)
                                 {
                                     joint = d.JointCreateContact(world, contactgroup, ref AvatarMovementprimContact);
@@ -1156,8 +1171,8 @@ namespace OpenSim.Region.Physics.OdePlugin
                             else
                             {
                                 // Use the non movement contact
-                                contact.geom = contacts[i];
-                                _perloopContact.Add(contacts[i]);
+                                contact.geom = curContact;
+                                _perloopContact.Add(curContact);
 
                                 if (m_global_contactcount < maxContactsbeforedeath)
                                 {
@@ -1175,8 +1190,8 @@ namespace OpenSim.Region.Physics.OdePlugin
                                 material = ((OdePrim)p2).m_material;
                             
                             //m_log.DebugFormat("Material: {0}", material);
-                            m_materialContacts[material, 0].geom = contacts[i];
-                            _perloopContact.Add(contacts[i]);
+                            m_materialContacts[material, 0].geom = curContact;
+                            _perloopContact.Add(curContact);
 
                             if (m_global_contactcount < maxContactsbeforedeath)
                             {
@@ -1194,7 +1209,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                     }
 
                 }
-                collision_accounting_events(p1, p2, max_collision_depth);
+                collision_accounting_events(p1, p2, maxDepthContact);
                 if (count > geomContactPointsStartthrottle)
                 {
                     // If there are more then 3 contact points, it's likely
@@ -1278,7 +1293,7 @@ namespace OpenSim.Region.Physics.OdePlugin
             return result;
         }
 
-        private void collision_accounting_events(PhysicsActor p1, PhysicsActor p2, float collisiondepth)
+        private void collision_accounting_events(PhysicsActor p1, PhysicsActor p2, ContactPoint contact)
         {
             // obj1LocalID = 0;
             //returncollisions = false;
@@ -1299,7 +1314,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                         case ActorTypes.Agent:
                             cc1 = (OdeCharacter)p1;
                             obj2LocalID = cc1.m_localID;
-                            cc1.AddCollisionEvent(cc2.m_localID, collisiondepth);
+                            cc1.AddCollisionEvent(cc2.m_localID, contact);
                             //ctype = (int)CollisionCategories.Character;
 
                             //if (cc1.CollidingObj)
@@ -1314,7 +1329,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                             {
                                 cp1 = (OdePrim) p1;
                                 obj2LocalID = cp1.m_localID;
-                                cp1.AddCollisionEvent(cc2.m_localID, collisiondepth);
+                                cp1.AddCollisionEvent(cc2.m_localID, contact);
                             }
                             //ctype = (int)CollisionCategories.Geom;
 
@@ -1334,7 +1349,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                             break;
                     }
 
-                    cc2.AddCollisionEvent(obj2LocalID, collisiondepth);
+                    cc2.AddCollisionEvent(obj2LocalID, contact);
                     break;
                 case ActorTypes.Prim:
 
@@ -1350,7 +1365,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                                 {
                                     cc1 = (OdeCharacter) p1;
                                     obj2LocalID = cc1.m_localID;
-                                    cc1.AddCollisionEvent(cp2.m_localID, collisiondepth);
+                                    cc1.AddCollisionEvent(cp2.m_localID, contact);
                                     //ctype = (int)CollisionCategories.Character;
 
                                     //if (cc1.CollidingObj)
@@ -1366,7 +1381,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                                 {
                                     cp1 = (OdePrim) p1;
                                     obj2LocalID = cp1.m_localID;
-                                    cp1.AddCollisionEvent(cp2.m_localID, collisiondepth);
+                                    cp1.AddCollisionEvent(cp2.m_localID, contact);
                                     //ctype = (int)CollisionCategories.Geom;
 
                                     //if (cp1.CollidingObj)
@@ -1387,7 +1402,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                                 break;
                         }
 
-                        cp2.AddCollisionEvent(obj2LocalID, collisiondepth);
+                        cp2.AddCollisionEvent(obj2LocalID, contact);
                     }
                     break;
             }
@@ -1750,6 +1765,11 @@ namespace OpenSim.Region.Physics.OdePlugin
             return result;
         }
 
+        public override float TimeDilation
+        {
+            get { return m_timeDilation; }
+        }
+
         public override bool SupportsNINJAJoints
         {
             get { return m_NINJA_physics_joints_enabled; }
@@ -2657,8 +2677,10 @@ namespace OpenSim.Region.Physics.OdePlugin
 
                 // Figure out the Frames Per Second we're going at.
                 //(step_time == 0.004f, there's 250 of those per second.   Times the step time/step size
-                
-                fps = (step_time/ODE_STEPSIZE) * 1000;
+
+                fps = (step_time / ODE_STEPSIZE) * 1000;
+                // HACK: Using a time dilation of 1.0 to debug rubberbanding issues
+                //m_timeDilation = Math.Min((step_time / ODE_STEPSIZE) / (0.09375f / ODE_STEPSIZE), 1.0f);
 
                 step_time = 0.09375f;