* Added a better check to the SimianGrid connectors to test if they are enabled or not. This method should work equally well with standalone or robust mode

* Applying #4602 from Misterblu to add collision detection to BulletDotNET

3 files changed, 189 insertions, 45 deletions

diff --git a/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETCharacter.cs b/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETCharacter.cs
index 6a54705..ac4e2b9 100644
--- a/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETCharacter.cs
+++ b/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETCharacter.cs
@@ -108,12 +108,11 @@ namespace OpenSim.Region.Physics.BulletDotNETPlugin
         private bool[] m_colliderarr = new bool[11];
         private bool[] m_colliderGroundarr = new bool[11];
 
-
-
         private BulletDotNETScene m_parent_scene;
 
         public int m_eventsubscription = 0;
-        // private CollisionEventUpdate CollisionEventsThisFrame = new CollisionEventUpdate();
+        private CollisionEventUpdate CollisionEventsThisFrame = null;
+        private int m_requestedUpdateFrequency = 0;
 
         public BulletDotNETCharacter(string avName, BulletDotNETScene parent_scene, Vector3 pos, Vector3 size, float pid_d, float pid_p, float capsule_radius, float tensor, float density, float height_fudge_factor, float walk_divisor, float rundivisor)
         {
@@ -212,7 +211,8 @@ namespace OpenSim.Region.Physics.BulletDotNETPlugin
             m_mass = Mass;
 
             Body = new btRigidBody(m_mass, m_bodyMotionState, Shell);
-            Body.setUserPointer(new IntPtr((int)Body.Handle));
+            // this is used for self identification. User localID instead of body handle
+            Body.setUserPointer(new IntPtr((int)m_localID));
             
             if (ClosestCastResult != null)
                 ClosestCastResult.Dispose();
@@ -717,6 +717,7 @@ namespace OpenSim.Region.Physics.BulletDotNETPlugin
         public override void SubscribeEvents(int ms)
         {
             m_eventsubscription = ms;
+            m_requestedUpdateFrequency = ms;
             m_parent_scene.addCollisionEventReporting(this);
         }
 
@@ -724,6 +725,7 @@ namespace OpenSim.Region.Physics.BulletDotNETPlugin
         {
              m_parent_scene.remCollisionEventReporting(this);
             m_eventsubscription = 0;
+            m_requestedUpdateFrequency = 0;
         }
 
         public override bool SubscribedEvents()
@@ -733,6 +735,29 @@ namespace OpenSim.Region.Physics.BulletDotNETPlugin
             return false;
         }
 
+        public void AddCollision(uint collideWith, ContactPoint contact)
+        {
+            if (CollisionEventsThisFrame == null)
+            {
+                CollisionEventsThisFrame = new CollisionEventUpdate();
+            }
+            CollisionEventsThisFrame.addCollider(collideWith, contact);
+        }
+
+        public void SendCollisions()
+        {
+            if (m_eventsubscription >= m_requestedUpdateFrequency)
+            {
+                if (CollisionEventsThisFrame != null)
+                {
+                    base.SendCollisionUpdate(CollisionEventsThisFrame);
+                }
+                CollisionEventsThisFrame = new CollisionEventUpdate();
+                m_eventsubscription = 0;
+            }
+            return;
+        }
+
         internal void Dispose()
         {
             if (Body.isInWorld())
diff --git a/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETPrim.cs b/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETPrim.cs
index 920ed96..dc3229a 100644
--- a/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETPrim.cs
+++ b/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETPrim.cs
@@ -154,7 +154,8 @@ namespace OpenSim.Region.Physics.BulletDotNETPlugin
         private Vector3 _target_velocity;
 
         public int m_eventsubscription;
-        // private CollisionEventUpdate CollisionEventsThisFrame = null;
+        private int m_requestedUpdateFrequency = 0;
+        private CollisionEventUpdate CollisionEventsThisFrame = null;
 
         public volatile bool childPrim;
 
@@ -595,6 +596,7 @@ namespace OpenSim.Region.Physics.BulletDotNETPlugin
         public override void SubscribeEvents(int ms)
         {
             m_eventsubscription = ms;
+            m_requestedUpdateFrequency = ms;
             _parent_scene.addCollisionEventReporting(this);
         }
 
@@ -602,6 +604,7 @@ namespace OpenSim.Region.Physics.BulletDotNETPlugin
         {
             _parent_scene.remCollisionEventReporting(this);
             m_eventsubscription = 0;
+            m_requestedUpdateFrequency = 0;
         }
 
         public override bool SubscribedEvents()
@@ -611,7 +614,28 @@ namespace OpenSim.Region.Physics.BulletDotNETPlugin
 
         #endregion
 
+        public void AddCollision(uint collideWith, ContactPoint contact)
+        {
+            if (CollisionEventsThisFrame == null)
+            {
+                CollisionEventsThisFrame = new CollisionEventUpdate();
+            }
+            CollisionEventsThisFrame.addCollider(collideWith, contact);
+        }
 
+        public void SendCollisions()
+        {
+            if (m_eventsubscription >= m_requestedUpdateFrequency)
+            {
+                if (CollisionEventsThisFrame != null)
+                {
+                    base.SendCollisionUpdate(CollisionEventsThisFrame);
+                }
+                CollisionEventsThisFrame = null;
+                // m_eventsubscription = 0;
+            }
+            return;
+        }
 
         internal void Dispose()
         {
@@ -759,7 +783,7 @@ namespace OpenSim.Region.Physics.BulletDotNETPlugin
         {
             if (m_taintadd)
             {
-                m_log.Debug("[PHYSICS]: TaintAdd");
+                // m_log.Debug("[PHYSICS]: TaintAdd");
                 changeadd(timestep);
             }
 
@@ -771,7 +795,7 @@ namespace OpenSim.Region.Physics.BulletDotNETPlugin
                     SetBody(Mass);
                 else
                     SetBody(0);
-                m_log.Debug("[PHYSICS]: GEOM_DOESNT_EXSIT");
+                // m_log.Debug("[PHYSICS]: GEOM_DOESNT_EXSIT");
             }
 
             if (prim_geom.Handle == IntPtr.Zero)
@@ -782,31 +806,31 @@ namespace OpenSim.Region.Physics.BulletDotNETPlugin
                     SetBody(Mass);
                 else
                     SetBody(0);
-                m_log.Debug("[PHYSICS]: GEOM_DOESNT_EXSIT");
+                // m_log.Debug("[PHYSICS]: GEOM_DOESNT_EXSIT");
 
             }
 
             if (!_position.ApproxEquals(m_taintposition, 0f))
             {
-                m_log.Debug("[PHYSICS]: TaintMove");
+                // m_log.Debug("[PHYSICS]: TaintMove");
                 changemove(timestep);
             }
             if (m_taintrot != _orientation)
             {
-                m_log.Debug("[PHYSICS]: TaintRotate");
+                // m_log.Debug("[PHYSICS]: TaintRotate");
                 rotate(timestep);
             } //
 
             if (m_taintPhysics != m_isphysical && !(m_taintparent != _parent))
             {
-                m_log.Debug("[PHYSICS]: TaintPhysics");
+                // m_log.Debug("[PHYSICS]: TaintPhysics");
                 changePhysicsStatus(timestep);
             }
             //
 
             if (!_size.ApproxEquals(m_taintsize, 0f))
             {
-                m_log.Debug("[PHYSICS]: TaintSize");
+                // m_log.Debug("[PHYSICS]: TaintSize");
                 changesize(timestep);
             }
 
@@ -814,43 +838,43 @@ namespace OpenSim.Region.Physics.BulletDotNETPlugin
 
             if (m_taintshape)
             {
-                m_log.Debug("[PHYSICS]: TaintShape");
+                // m_log.Debug("[PHYSICS]: TaintShape");
                 changeshape(timestep);
             } //
 
             if (m_taintforce)
             {
-                m_log.Debug("[PHYSICS]: TaintForce");
+                // m_log.Debug("[PHYSICS]: TaintForce");
                 changeAddForce(timestep);
             }
             if (m_taintaddangularforce)
             {
-                m_log.Debug("[PHYSICS]: TaintAngularForce");
+                // m_log.Debug("[PHYSICS]: TaintAngularForce");
                 changeAddAngularForce(timestep);
             }
             if (!m_taintTorque.ApproxEquals(Vector3.Zero, 0.001f))
             {
-                m_log.Debug("[PHYSICS]: TaintTorque");
+                // m_log.Debug("[PHYSICS]: TaintTorque");
                 changeSetTorque(timestep);
             }
             if (m_taintdisable)
             {
-                m_log.Debug("[PHYSICS]: TaintDisable");
+                // m_log.Debug("[PHYSICS]: TaintDisable");
                 changedisable(timestep);
             }
             if (m_taintselected != m_isSelected)
             {
-                m_log.Debug("[PHYSICS]: TaintSelected");
+                // m_log.Debug("[PHYSICS]: TaintSelected");
                 changeSelectedStatus(timestep);
             }
             if (!m_taintVelocity.ApproxEquals(Vector3.Zero, 0.001f))
             {
-                m_log.Debug("[PHYSICS]: TaintVelocity");
+                // m_log.Debug("[PHYSICS]: TaintVelocity");
                 changevelocity(timestep);
             }
             if (m_taintparent != _parent)
             {
-                m_log.Debug("[PHYSICS]: TaintLink");
+                // m_log.Debug("[PHYSICS]: TaintLink");
                 changelink(timestep);
             }
             if (m_taintCollidesWater != m_collidesWater)
@@ -859,7 +883,7 @@ namespace OpenSim.Region.Physics.BulletDotNETPlugin
             }
             if (!m_angularlock.ApproxEquals(m_taintAngularLock, 0))
             {
-                m_log.Debug("[PHYSICS]: TaintAngularLock");
+                // m_log.Debug("[PHYSICS]: TaintAngularLock");
                 changeAngularLock(timestep);
             }
             if (m_taintremove)
@@ -917,7 +941,7 @@ namespace OpenSim.Region.Physics.BulletDotNETPlugin
         private void changemove(float timestep)
         {
 
-            m_log.Debug("[PHYSICS]: _________ChangeMove");
+            // m_log.Debug("[PHYSICS]: _________ChangeMove");
             if (!m_isphysical)
             {
                 tempTransform2 = Body.getWorldTransform();
@@ -977,7 +1001,7 @@ namespace OpenSim.Region.Physics.BulletDotNETPlugin
 
         private void rotate(float timestep)
         {
-            m_log.Debug("[PHYSICS]: _________ChangeRotate");
+            // m_log.Debug("[PHYSICS]: _________ChangeRotate");
             tempTransform2 = Body.getWorldTransform();
             tempOrientation2 = new btQuaternion(_orientation.X, _orientation.Y, _orientation.Z, _orientation.W);
             tempTransform2.setRotation(tempOrientation2);
@@ -1000,7 +1024,7 @@ namespace OpenSim.Region.Physics.BulletDotNETPlugin
                 //Body = null;
                 // TODO: dispose parts that make up body
             }
-            m_log.Debug("[PHYSICS]: _________ChangePhysics");
+            // m_log.Debug("[PHYSICS]: _________ChangePhysics");
 
             ProcessGeomCreation();
 
@@ -1092,7 +1116,7 @@ namespace OpenSim.Region.Physics.BulletDotNETPlugin
                 // TODO: dispose parts that make up body
             }
 
-            m_log.Debug("[PHYSICS]: _________ChangeSize");
+            // m_log.Debug("[PHYSICS]: _________ChangeSize");
             SetCollisionShape(null);
             // Construction of new prim
             ProcessGeomCreation();
@@ -1297,13 +1321,13 @@ namespace OpenSim.Region.Physics.BulletDotNETPlugin
             // TODO: throw new NotImplementedException();
             if (m_taintselected)
             {
-                Body.setCollisionFlags((int)ContactFlags.CF_NO_CONTACT_RESPONSE);
+                // Body.setCollisionFlags((int)ContactFlags.CF_NO_CONTACT_RESPONSE);
                 disableBodySoft();
 
             }
             else
             {
-                Body.setCollisionFlags(0 | (int)ContactFlags.CF_CUSTOM_MATERIAL_CALLBACK);
+                // Body.setCollisionFlags(0 | (int)ContactFlags.CF_CUSTOM_MATERIAL_CALLBACK);
                 enableBodySoft();
             }
             m_isSelected = m_taintselected;
@@ -1605,6 +1629,11 @@ namespace OpenSim.Region.Physics.BulletDotNETPlugin
                         enableBodySoft();
                     }
                     */
+                    if (!Body.isActive())
+                    {
+                        Body.clearForces();
+                        enableBodySoft();
+                    }
                     // 35x10 = 350n times the mass per second applied maximum.
 
                     float nmax = 35f * m_mass;
@@ -1632,6 +1661,12 @@ namespace OpenSim.Region.Physics.BulletDotNETPlugin
                         Body.applyCentralImpulse(tempAddForce);
                     }
                 }
+                else
+                {
+                    // if no forces on the prim, make sure everything is zero
+                    Body.clearForces();
+                    enableBodySoft();
+                }
             }
             else
             {
@@ -1985,7 +2020,7 @@ namespace OpenSim.Region.Physics.BulletDotNETPlugin
 
         public void CreateGeom(IntPtr m_targetSpace, IMesh p_mesh)
         {
-            m_log.Debug("[PHYSICS]: _________CreateGeom");
+            // m_log.Debug("[PHYSICS]: _________CreateGeom");
             if (p_mesh != null)
             {
                 //_mesh = _parent_scene.mesher.CreateMesh(m_primName, _pbs, _size, _parent_scene.meshSculptLOD, IsPhysical);
@@ -2042,7 +2077,7 @@ namespace OpenSim.Region.Physics.BulletDotNETPlugin
             // TODO: Set Collision Body Mesh
             // This sleeper is there to moderate how long it takes between
             // setting up the mesh and pre-processing it when we get rapid fire mesh requests on a single object
-            m_log.Debug("_________SetMesh");
+            // m_log.Debug("_________SetMesh");
             Thread.Sleep(10);
 
             //Kill Body so that mesh can re-make the geom
@@ -2159,7 +2194,14 @@ namespace OpenSim.Region.Physics.BulletDotNETPlugin
 
                 //     Body = new btRigidBody(mass, tempMotionState1, prim_geom);
                 //else
-                Body = new btRigidBody(mass, tempMotionState1, prim_geom, tempInertia1);
+                // Body = new btRigidBody(mass, tempMotionState1, prim_geom, tempInertia1);
+                if (Body == null)
+                {
+                    Body = new btRigidBody(mass, tempMotionState1, prim_geom, tempInertia1);
+                    // add localID so we can later map bullet object back to OpenSim object
+                    Body.setUserPointer(new IntPtr((int)m_localID));
+                }
+                
 
                 if (prim_geom is btGImpactMeshShape)
                 {
@@ -2250,7 +2292,13 @@ namespace OpenSim.Region.Physics.BulletDotNETPlugin
 
                 //     Body = new btRigidBody(mass, tempMotionState1, prim_geom);
                 //else
-                Body = new btRigidBody(mass, tempMotionState1, prim_geom, tempInertia1);
+                // Body = new btRigidBody(mass, tempMotionState1, prim_geom, tempInertia1);
+                if (Body == null)
+                {
+                    Body = new btRigidBody(mass, tempMotionState1, prim_geom, tempInertia1);
+                    // each body has the localID stored into it so we can identify collision objects
+                    Body.setUserPointer(new IntPtr((int)m_localID));
+                }
 
                 if (prim_geom is btGImpactMeshShape)
                 {
diff --git a/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETScene.cs b/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETScene.cs
index 9e048ab..85e34c1 100644
--- a/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETScene.cs
+++ b/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETScene.cs
@@ -47,7 +47,9 @@ namespace OpenSim.Region.Physics.BulletDotNETPlugin
         // private string m_sceneIdentifier = string.Empty;
         
         private List<BulletDotNETCharacter> m_characters = new List<BulletDotNETCharacter>();
+        private Dictionary<uint, BulletDotNETCharacter> m_charactersLocalID = new Dictionary<uint, BulletDotNETCharacter>();
         private List<BulletDotNETPrim> m_prims = new List<BulletDotNETPrim>();
+        private Dictionary<uint, BulletDotNETPrim> m_primsLocalID = new Dictionary<uint, BulletDotNETPrim>();
         private List<BulletDotNETPrim> m_activePrims = new List<BulletDotNETPrim>();
         private List<PhysicsActor> m_taintedActors = new List<PhysicsActor>();
         private btDiscreteDynamicsWorld m_world;
@@ -134,7 +136,7 @@ namespace OpenSim.Region.Physics.BulletDotNETPlugin
             m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
             m_world = new btDiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_solver, m_collisionConfiguration);
             m_world.setGravity(m_gravity);
-            //EnableCollisionInterface();
+            EnableCollisionInterface();
             
 
         }
@@ -145,7 +147,16 @@ namespace OpenSim.Region.Physics.BulletDotNETPlugin
                                                                   avCapRadius, avStandupTensor, avDensity,
                                                                   avHeightFudgeFactor, avMovementDivisorWalk,
                                                                   avMovementDivisorRun);
-            m_characters.Add(chr);
+            try
+            {
+                m_characters.Add(chr);
+                m_charactersLocalID.Add(chr.m_localID, chr);
+            }
+            catch
+            {
+                // noop if it's already there
+                m_log.Debug("[PHYSICS] BulletDotNet: adding duplicate avatar localID");
+            }
             AddPhysicsActorTaint(chr);
             return chr;
         }
@@ -154,6 +165,7 @@ namespace OpenSim.Region.Physics.BulletDotNETPlugin
         {
             BulletDotNETCharacter chr = (BulletDotNETCharacter) actor;
 
+            m_charactersLocalID.Remove(chr.m_localID);
             m_characters.Remove(chr);
             m_world.removeRigidBody(chr.Body);
             m_world.removeCollisionObject(chr.Body);
@@ -279,7 +291,7 @@ namespace OpenSim.Region.Physics.BulletDotNETPlugin
                     prim.Move(timeStep);
                 }
             }
-            float steps = m_world.stepSimulation(timeStep * 1000, 10, WorldTimeComp);
+            float steps = m_world.stepSimulation(timeStep, 10, WorldTimeComp);
 
             foreach (BulletDotNETCharacter chr in m_characters)
             {
@@ -296,20 +308,67 @@ namespace OpenSim.Region.Physics.BulletDotNETPlugin
             }
             if (m_CollisionInterface != null)
             {
-                List<int> collisions = m_CollisionInterface.GetContactList();
-                lock (collisions)
+                List<BulletDotNETPrim> primsWithCollisions = new List<BulletDotNETPrim>();
+                List<BulletDotNETCharacter> charactersWithCollisions = new List<BulletDotNETCharacter>();
+
+                // get the collisions that happened this tick
+                List<BulletDotNET.ContactAddedCallbackHandler.ContactInfo> collisions = m_CollisionInterface.GetContactList();
+                // passed back the localID of the prim so we can associate the prim
+                foreach (BulletDotNET.ContactAddedCallbackHandler.ContactInfo ci in collisions)
                 {
-                    foreach (int pvalue in collisions)
-                    {
-                        System.Console.Write(string.Format("{0} ", pvalue));
-                    }
+                    // ContactPoint = { contactPoint, contactNormal, penetrationDepth }
+                    ContactPoint contact = new ContactPoint(new Vector3(ci.pX, ci.pY, ci.pZ),
+                                    new Vector3(ci.nX, ci.nY, ci.nZ), ci.depth);
+                    
+                    ProcessContact(ci.contact, ci.contactWith, contact, ref primsWithCollisions, ref charactersWithCollisions);
+                    ProcessContact(ci.contactWith, ci.contact, contact, ref primsWithCollisions, ref charactersWithCollisions);
+
                 }
                 m_CollisionInterface.Clear();
-
+                // for those prims and characters that had collisions cause collision events
+                foreach (BulletDotNETPrim bdnp in primsWithCollisions)
+                {
+                    bdnp.SendCollisions();
+                }
+                foreach (BulletDotNETCharacter bdnc in charactersWithCollisions)
+                {
+                    bdnc.SendCollisions();
+                }
             }
             return steps;
         }
 
+        private void ProcessContact(uint cont, uint contWith, ContactPoint contact, 
+                    ref List<BulletDotNETPrim> primsWithCollisions,
+                    ref List<BulletDotNETCharacter> charactersWithCollisions)
+        {
+            BulletDotNETPrim bdnp;
+            // collisions with a normal prim?
+            if (m_primsLocalID.TryGetValue(cont, out bdnp))
+            {
+                // Added collision event to the prim. This creates a pile of events
+                // that will be sent to any subscribed listeners.
+                bdnp.AddCollision(contWith, contact);
+                if (!primsWithCollisions.Contains(bdnp))
+                {
+                    primsWithCollisions.Add(bdnp);
+                }
+            }
+            else
+            {
+                BulletDotNETCharacter bdnc;
+                // if not a prim, maybe it's one of the characters
+                if (m_charactersLocalID.TryGetValue(cont, out bdnc))
+                {
+                    bdnc.AddCollision(contWith, contact);
+                    if (!charactersWithCollisions.Contains(bdnc))
+                    {
+                        charactersWithCollisions.Add(bdnc);
+                    }
+                }
+            }
+        }
+
         public override void GetResults()
         {
             
@@ -387,6 +446,7 @@ namespace OpenSim.Region.Physics.BulletDotNETPlugin
             m_terrainTransform = new btTransform(QuatIdentity, m_terrainPosition);
             m_terrainMotionState = new btDefaultMotionState(m_terrainTransform);
             TerrainBody = new btRigidBody(0, m_terrainMotionState, m_terrainShape);
+            TerrainBody.setUserPointer((IntPtr)0);
             m_world.addRigidBody(TerrainBody);
 
 
@@ -459,6 +519,7 @@ namespace OpenSim.Region.Physics.BulletDotNETPlugin
         {
             lock (m_prims)
             {
+                m_primsLocalID.Clear();
                 foreach (BulletDotNETPrim prim in m_prims)
                 {
                     if (prim.Body != null)
@@ -513,6 +574,7 @@ namespace OpenSim.Region.Physics.BulletDotNETPlugin
                     m_world.removeRigidBody(body);
                 }
                 remActivePrim(prm);
+                m_primsLocalID.Remove(prm.m_localID);
                 m_prims.Remove(prm);
             }
 
@@ -686,9 +748,18 @@ namespace OpenSim.Region.Physics.BulletDotNETPlugin
             {
                 if (!m_prims.Contains(pPrim))
                 {
-                    m_prims.Add(pPrim);
+                    try
+                    {
+                        m_prims.Add(pPrim);
+                        m_primsLocalID.Add(pPrim.m_localID, pPrim);
+                    }
+                    catch
+                    {
+                        // noop if it's already there
+                        m_log.Debug("[PHYSICS] BulletDotNet: adding duplicate prim localID");
+                    }
                     m_world.addRigidBody(pPrim.Body);
-                    m_log.Debug("ADDED");
+                    // m_log.Debug("[PHYSICS] added prim to scene");
                 }
             }
         }
@@ -696,8 +767,8 @@ namespace OpenSim.Region.Physics.BulletDotNETPlugin
         {
             if (m_CollisionInterface == null)
             {
-                m_CollisionInterface = new ContactAddedCallbackHandler();
-                m_world.SetCollisionAddedCallback(m_CollisionInterface);
+                m_CollisionInterface = new ContactAddedCallbackHandler(m_world);
+                // m_world.SetCollisionAddedCallback(m_CollisionInterface);
             }
         }