3 files changed, 366 insertions, 258 deletions

diff --git a/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs
index 8397eb4..54b69a2 100644
--- a/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs
+++ b/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs
@@ -1261,14 +1261,20 @@ namespace OpenSim.Region.Physics.OdePlugin
         {
             m_requestedUpdateFrequency = ms;
             m_eventsubscription = ms;
-            CollisionEventsThisFrame.Clear();
+
+            // Don't clear collision event reporting here.  This is called directly from scene code and so can lead
+            // to a race condition with the simulate loop
+
             _parent_scene.AddCollisionEventReporting(this);
         }
 
         public override void UnSubscribeEvents()
         {
             CollisionEventsThisFrame.Clear();
-            _parent_scene.RemoveCollisionEventReporting(this);
+
+            // Don't clear collision event reporting here.  This is called directly from scene code and so can lead
+            // to a race condition with the simulate loop
+
             m_requestedUpdateFrequency = 0;
             m_eventsubscription = 0;
         }
diff --git a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
index 6f37347..a41c856 100644
--- a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
+++ b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
@@ -156,7 +156,15 @@ namespace OpenSim.Region.Physics.OdePlugin
         /// </summary>
         public IntPtr m_targetSpace = IntPtr.Zero;
 
+        /// <summary>
+        /// The prim geometry, used for collision detection.
+        /// </summary>
+        /// <remarks>
+        /// This is never null except for a brief period when the geometry needs to be replaced (due to resizing or
+        /// mesh change) or when the physical prim is being removed from the scene.
+        /// </remarks>
         public IntPtr prim_geom { get; private set; }
+
         public IntPtr _triMeshData { get; private set; }
 
         private IntPtr _linkJointGroup = IntPtr.Zero;
@@ -325,14 +333,12 @@ namespace OpenSim.Region.Physics.OdePlugin
         {
             prim_geom = geom;
 //Console.WriteLine("SetGeom to " + prim_geom + " for " + Name);
-            if (prim_geom != IntPtr.Zero)
-            {
-                d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
-                d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
 
-                _parent_scene.geom_name_map[prim_geom] = Name;
-                _parent_scene.actor_name_map[prim_geom] = this;
-            }
+            d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
+            d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
+
+            _parent_scene.geom_name_map[prim_geom] = Name;
+            _parent_scene.actor_name_map[prim_geom] = this;
 
             if (childPrim)
             {
@@ -765,11 +771,8 @@ namespace OpenSim.Region.Physics.OdePlugin
                         m_collisionCategories &= ~CollisionCategories.Body;
                         m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land);
 
-                        if (prim_geom != IntPtr.Zero)
-                        {
-                            d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
-                            d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
-                        }
+                        d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
+                        d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
 
                         d.BodyDestroy(Body);
                         lock (childrenPrim)
@@ -793,11 +796,8 @@ namespace OpenSim.Region.Physics.OdePlugin
                     m_collisionCategories &= ~CollisionCategories.Body;
                     m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land);
 
-                    if (prim_geom != IntPtr.Zero)
-                    {
-                        d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
-                        d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
-                    }
+                    d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
+                    d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
 
                     Body = IntPtr.Zero;
                 }
@@ -864,10 +864,7 @@ namespace OpenSim.Region.Physics.OdePlugin
 //            _parent_scene.waitForSpaceUnlock(m_targetSpace);
             try
             {
-                if (prim_geom == IntPtr.Zero)
-                {
-                    SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, parent_scene.triCallback, null, null));
-                }
+                SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, parent_scene.triCallback, null, null));
             }
             catch (AccessViolationException)
             {
@@ -890,73 +887,67 @@ namespace OpenSim.Region.Physics.OdePlugin
 #if SPAM
 Console.WriteLine("ZProcessTaints for " + Name);
 #endif
+
+            // This must be processed as the very first taint so that later operations have a prim_geom to work with
+            // if this is a new prim.
             if (m_taintadd)
-            {
                 changeadd();
-            }
-            
-            if (prim_geom != IntPtr.Zero)
-            {
-                 if (!_position.ApproxEquals(m_taintposition, 0f))
-                     changemove();
 
-                 if (m_taintrot != _orientation)
-                 {
-                    if (childPrim && IsPhysical)    // For physical child prim...
-                    {
-                        rotate();
-                        // KF: ODE will also rotate the parent prim!
-                        // so rotate the root back to where it was
-                        OdePrim parent = (OdePrim)_parent;
-                        parent.rotate();
-                    }
-                    else
-                    {
-                        //Just rotate the prim
-                        rotate();
-                    }
+            if (!_position.ApproxEquals(m_taintposition, 0f))
+                 changemove();
+
+            if (m_taintrot != _orientation)
+            {
+                if (childPrim && IsPhysical)    // For physical child prim...
+                {
+                    rotate();
+                    // KF: ODE will also rotate the parent prim!
+                    // so rotate the root back to where it was
+                    OdePrim parent = (OdePrim)_parent;
+                    parent.rotate();
                 }
-            
-                if (m_taintPhysics != IsPhysical && !(m_taintparent != _parent))
-                    changePhysicsStatus();
+                else
+                {
+                    //Just rotate the prim
+                    rotate();
+                }
+            }
+        
+            if (m_taintPhysics != IsPhysical && !(m_taintparent != _parent))
+                changePhysicsStatus();
 
-                if (!_size.ApproxEquals(m_taintsize, 0f))
-                    changesize();
+            if (!_size.ApproxEquals(m_taintsize, 0f))
+                changesize();
 
-                if (m_taintshape)
-                    changeshape();
+            if (m_taintshape)
+                changeshape();
 
-                if (m_taintforce)
-                    changeAddForce();
+            if (m_taintforce)
+                changeAddForce();
 
-                if (m_taintaddangularforce)
-                    changeAddAngularForce();
+            if (m_taintaddangularforce)
+                changeAddAngularForce();
 
-                if (!m_taintTorque.ApproxEquals(Vector3.Zero, 0.001f))
-                    changeSetTorque();
+            if (!m_taintTorque.ApproxEquals(Vector3.Zero, 0.001f))
+                changeSetTorque();
 
-                if (m_taintdisable)
-                    changedisable();
+            if (m_taintdisable)
+                changedisable();
 
-                if (m_taintselected != m_isSelected)
-                    changeSelectedStatus();
+            if (m_taintselected != m_isSelected)
+                changeSelectedStatus();
 
-                if (!m_taintVelocity.ApproxEquals(Vector3.Zero, 0.001f))
-                    changevelocity();
+            if (!m_taintVelocity.ApproxEquals(Vector3.Zero, 0.001f))
+                changevelocity();
 
-                if (m_taintparent != _parent)
-                    changelink();
+            if (m_taintparent != _parent)
+                changelink();
 
-                if (m_taintCollidesWater != m_collidesWater)
-                    changefloatonwater();
+            if (m_taintCollidesWater != m_collidesWater)
+                changefloatonwater();
 
-                if (!m_angularlock.ApproxEquals(m_taintAngularLock,0f))
-                    changeAngularLock();
-            }
-            else
-            {
-                m_log.ErrorFormat("[PHYSICS]: The scene reused a disposed PhysActor for {0}! *waves finger*, Don't be evil.  A couple of things can cause this.   An improper prim breakdown(be sure to set prim_geom to zero after d.GeomDestroy!   An improper buildup (creating the geom failed).   Or, the Scene Reused a physics actor after disposing it.)", Name);
-            }
+            if (!m_angularlock.ApproxEquals(m_taintAngularLock,0f))
+                changeAngularLock();
         }
 
         /// <summary>
@@ -1052,150 +1043,146 @@ Console.WriteLine("ZProcessTaints for " + Name);
         /// <param name="prim">Child prim</param>
         private void AddChildPrim(OdePrim prim)
         {
-//Console.WriteLine("AddChildPrim  " + Name);
-            if (LocalID != prim.LocalID)
+            if (LocalID == prim.LocalID)
+                return;
+
+            if (Body == IntPtr.Zero)
             {
-                if (Body == IntPtr.Zero)
+                Body = d.BodyCreate(_parent_scene.world);
+                setMass();
+            }
+
+            lock (childrenPrim)
+            {
+                if (childrenPrim.Contains(prim))
+                    return;
+
+//                m_log.DebugFormat(
+//                    "[ODE PRIM]: Linking prim {0} {1} to {2} {3}", prim.Name, prim.LocalID, Name, LocalID);
+
+                childrenPrim.Add(prim);
+
+                foreach (OdePrim prm in childrenPrim)
                 {
-                    Body = d.BodyCreate(_parent_scene.world);
-                    setMass();
+                    d.Mass m2;
+                    d.MassSetZero(out m2);
+                    d.MassSetBoxTotal(out m2, prim.CalculateMass(), prm._size.X, prm._size.Y, prm._size.Z);
+
+                    d.Quaternion quat = new d.Quaternion();
+                    quat.W = prm._orientation.W;
+                    quat.X = prm._orientation.X;
+                    quat.Y = prm._orientation.Y;
+                    quat.Z = prm._orientation.Z;
+
+                    d.Matrix3 mat = new d.Matrix3();
+                    d.RfromQ(out mat, ref quat);
+                    d.MassRotate(ref m2, ref mat);
+                    d.MassTranslate(ref m2, Position.X - prm.Position.X, Position.Y - prm.Position.Y, Position.Z - prm.Position.Z);
+                    d.MassAdd(ref pMass, ref m2);
                 }
-                if (Body != IntPtr.Zero)
+
+                foreach (OdePrim prm in childrenPrim)
                 {
-                    lock (childrenPrim)
-                    {
-                        if (!childrenPrim.Contains(prim))
-                        {
-//Console.WriteLine("childrenPrim.Add " + prim);
-                            childrenPrim.Add(prim);
-                            
-                            foreach (OdePrim prm in childrenPrim)
-                            {
-                                d.Mass m2;
-                                d.MassSetZero(out m2);
-                                d.MassSetBoxTotal(out m2, prim.CalculateMass(), prm._size.X, prm._size.Y, prm._size.Z);
-
-                                d.Quaternion quat = new d.Quaternion();
-                                quat.W = prm._orientation.W;
-                                quat.X = prm._orientation.X;
-                                quat.Y = prm._orientation.Y;
-                                quat.Z = prm._orientation.Z;
-
-                                d.Matrix3 mat = new d.Matrix3();
-                                d.RfromQ(out mat, ref quat);
-                                d.MassRotate(ref m2, ref mat);
-                                d.MassTranslate(ref m2, Position.X - prm.Position.X, Position.Y - prm.Position.Y, Position.Z - prm.Position.Z);
-                                d.MassAdd(ref pMass, ref m2);
-                            }
-                            
-                            foreach (OdePrim prm in childrenPrim)
-                            {
-                                prm.m_collisionCategories |= CollisionCategories.Body;
-                                prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
+                    prm.m_collisionCategories |= CollisionCategories.Body;
+                    prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
 
-                                if (prm.prim_geom == IntPtr.Zero)
-                                {
-                                    m_log.WarnFormat(
-                                        "[PHYSICS]: Unable to link one of the linkset elements {0} for parent {1}.  No geom yet", 
-                                        prm.Name, prim.Name);
-                                    continue;
-                                }
 //Console.WriteLine(" GeomSetCategoryBits 1: " + prm.prim_geom + " - " + (int)prm.m_collisionCategories + " for " + Name);
-                                d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories);
-                                d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags);
-
+                    d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories);
+                    d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags);
 
-                                d.Quaternion quat = new d.Quaternion();
-                                quat.W = prm._orientation.W;
-                                quat.X = prm._orientation.X;
-                                quat.Y = prm._orientation.Y;
-                                quat.Z = prm._orientation.Z;
+                    d.Quaternion quat = new d.Quaternion();
+                    quat.W = prm._orientation.W;
+                    quat.X = prm._orientation.X;
+                    quat.Y = prm._orientation.Y;
+                    quat.Z = prm._orientation.Z;
 
-                                d.Matrix3 mat = new d.Matrix3();
-                                d.RfromQ(out mat, ref quat);
-                                if (Body != IntPtr.Zero)
-                                {
-                                    d.GeomSetBody(prm.prim_geom, Body);
-                                    prm.childPrim = true;
-                                    d.GeomSetOffsetWorldPosition(prm.prim_geom, prm.Position.X , prm.Position.Y, prm.Position.Z);
-                                    //d.GeomSetOffsetPosition(prim.prim_geom,
-                                    //    (Position.X - prm.Position.X) - pMass.c.X,
-                                    //    (Position.Y - prm.Position.Y) - pMass.c.Y,
-                                    //    (Position.Z - prm.Position.Z) - pMass.c.Z);
-                                    d.GeomSetOffsetWorldRotation(prm.prim_geom, ref mat);
-                                    //d.GeomSetOffsetRotation(prm.prim_geom, ref mat);
-                                    d.MassTranslate(ref pMass, -pMass.c.X, -pMass.c.Y, -pMass.c.Z);
-                                    d.BodySetMass(Body, ref pMass);
-                                }
-                                else
-                                {
-                                    m_log.DebugFormat("[PHYSICS]: {0} ain't got no boooooooooddy, no body", Name);
-                                }
+                    d.Matrix3 mat = new d.Matrix3();
+                    d.RfromQ(out mat, ref quat);
+                    if (Body != IntPtr.Zero)
+                    {
+                        d.GeomSetBody(prm.prim_geom, Body);
+                        prm.childPrim = true;
+                        d.GeomSetOffsetWorldPosition(prm.prim_geom, prm.Position.X , prm.Position.Y, prm.Position.Z);
+                        //d.GeomSetOffsetPosition(prim.prim_geom,
+                        //    (Position.X - prm.Position.X) - pMass.c.X,
+                        //    (Position.Y - prm.Position.Y) - pMass.c.Y,
+                        //    (Position.Z - prm.Position.Z) - pMass.c.Z);
+                        d.GeomSetOffsetWorldRotation(prm.prim_geom, ref mat);
+                        //d.GeomSetOffsetRotation(prm.prim_geom, ref mat);
+                        d.MassTranslate(ref pMass, -pMass.c.X, -pMass.c.Y, -pMass.c.Z);
+                        d.BodySetMass(Body, ref pMass);
+                    }
+                    else
+                    {
+                        m_log.DebugFormat("[PHYSICS]: {0} ain't got no boooooooooddy, no body", Name);
+                    }
 
-                                prm.m_interpenetrationcount = 0;
-                                prm.m_collisionscore = 0;
-                                prm.m_disabled = false;
+                    prm.m_interpenetrationcount = 0;
+                    prm.m_collisionscore = 0;
+                    prm.m_disabled = false;
 
-                                // The body doesn't already have a finite rotation mode set here
-                                if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0f)) && _parent == null)
-                                {
-                                    prm.createAMotor(m_angularlock);
-                                }
-                                prm.Body = Body;
-                                _parent_scene.ActivatePrim(prm);
-                            }
+                    // The body doesn't already have a finite rotation mode set here
+                    if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0f)) && _parent == null)
+                    {
+                        prm.createAMotor(m_angularlock);
+                    }
+                    prm.Body = Body;
+                    _parent_scene.ActivatePrim(prm);
+                }
 
-                            m_collisionCategories |= CollisionCategories.Body;
-                            m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
+                m_collisionCategories |= CollisionCategories.Body;
+                m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
 
 //Console.WriteLine("GeomSetCategoryBits 2: " + prim_geom + " - " + (int)m_collisionCategories + " for " + Name);
-                            d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
+                d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
 //Console.WriteLine(" Post GeomSetCategoryBits 2");
-                            d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
-
-                            d.Quaternion quat2 = new d.Quaternion();
-                            quat2.W = _orientation.W;
-                            quat2.X = _orientation.X;
-                            quat2.Y = _orientation.Y;
-                            quat2.Z = _orientation.Z;
-
-                            d.Matrix3 mat2 = new d.Matrix3();
-                            d.RfromQ(out mat2, ref quat2);
-                            d.GeomSetBody(prim_geom, Body);
-                            d.GeomSetOffsetWorldPosition(prim_geom, Position.X - pMass.c.X, Position.Y - pMass.c.Y, Position.Z - pMass.c.Z);
-                            //d.GeomSetOffsetPosition(prim.prim_geom,
-                            //    (Position.X - prm.Position.X) - pMass.c.X,
-                            //    (Position.Y - prm.Position.Y) - pMass.c.Y,
-                            //    (Position.Z - prm.Position.Z) - pMass.c.Z);
-                            //d.GeomSetOffsetRotation(prim_geom, ref mat2);
-                            d.MassTranslate(ref pMass, -pMass.c.X, -pMass.c.Y, -pMass.c.Z);
-                            d.BodySetMass(Body, ref pMass);
-
-                            d.BodySetAutoDisableFlag(Body, true);
-                            d.BodySetAutoDisableSteps(Body, body_autodisable_frames);
+                d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
 
-                            m_interpenetrationcount = 0;
-                            m_collisionscore = 0;
-                            m_disabled = false;
+                d.Quaternion quat2 = new d.Quaternion();
+                quat2.W = _orientation.W;
+                quat2.X = _orientation.X;
+                quat2.Y = _orientation.Y;
+                quat2.Z = _orientation.Z;
 
-                            // The body doesn't already have a finite rotation mode set here
-                            if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0f)) && _parent == null)
-                            {
-                                createAMotor(m_angularlock);
-                            }
-                            d.BodySetPosition(Body, Position.X, Position.Y, Position.Z);
-                            if (m_vehicle.Type != Vehicle.TYPE_NONE)
-                                m_vehicle.Enable(Body, _parent_scene);
+                d.Matrix3 mat2 = new d.Matrix3();
+                d.RfromQ(out mat2, ref quat2);
+                d.GeomSetBody(prim_geom, Body);
+                d.GeomSetOffsetWorldPosition(prim_geom, Position.X - pMass.c.X, Position.Y - pMass.c.Y, Position.Z - pMass.c.Z);
+                //d.GeomSetOffsetPosition(prim.prim_geom,
+                //    (Position.X - prm.Position.X) - pMass.c.X,
+                //    (Position.Y - prm.Position.Y) - pMass.c.Y,
+                //    (Position.Z - prm.Position.Z) - pMass.c.Z);
+                //d.GeomSetOffsetRotation(prim_geom, ref mat2);
+                d.MassTranslate(ref pMass, -pMass.c.X, -pMass.c.Y, -pMass.c.Z);
+                d.BodySetMass(Body, ref pMass);
 
-                            _parent_scene.ActivatePrim(this);
-                        }
-                    }
+                d.BodySetAutoDisableFlag(Body, true);
+                d.BodySetAutoDisableSteps(Body, body_autodisable_frames);
+
+                m_interpenetrationcount = 0;
+                m_collisionscore = 0;
+                m_disabled = false;
+
+                // The body doesn't already have a finite rotation mode set here
+                if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0f)) && _parent == null)
+                {
+                    createAMotor(m_angularlock);
                 }
+
+                d.BodySetPosition(Body, Position.X, Position.Y, Position.Z);
+
+                if (m_vehicle.Type != Vehicle.TYPE_NONE)
+                    m_vehicle.Enable(Body, _parent_scene);
+
+                _parent_scene.ActivatePrim(this);
             }
         }
 
         private void ChildSetGeom(OdePrim odePrim)
         {
+//            m_log.DebugFormat(
+//                "[ODE PRIM]: ChildSetGeom {0} {1} for {2} {3}", odePrim.Name, odePrim.LocalID, Name, LocalID);
+
             //if (IsPhysical && Body != IntPtr.Zero)
             lock (childrenPrim)
             {
@@ -1210,12 +1197,14 @@ Console.WriteLine("ZProcessTaints for " + Name);
                     //prm.childPrim = false;
                 }
             }
+
             disableBody();
 
-            if (Body != IntPtr.Zero)
-            {
-                _parent_scene.DeactivatePrim(this);
-            }
+            // Spurious - Body == IntPtr.Zero after disableBody()
+//            if (Body != IntPtr.Zero)
+//            {
+//                _parent_scene.DeactivatePrim(this);
+//            }
 
             lock (childrenPrim)
             {
@@ -1229,6 +1218,9 @@ Console.WriteLine("ZProcessTaints for " + Name);
 
         private void ChildDelink(OdePrim odePrim)
         {
+//            m_log.DebugFormat(
+//                "[ODE PRIM]: Delinking prim {0} {1} from {2} {3}", odePrim.Name, odePrim.LocalID, Name, LocalID);
+
             // Okay, we have a delinked child..   need to rebuild the body.
             lock (childrenPrim)
             {
@@ -1243,6 +1235,7 @@ Console.WriteLine("ZProcessTaints for " + Name);
                     //prm.childPrim = false;
                 }
             }
+
             disableBody();
 
             lock (childrenPrim)
@@ -1251,10 +1244,11 @@ Console.WriteLine("ZProcessTaints for " + Name);
                 childrenPrim.Remove(odePrim);
             }
 
-            if (Body != IntPtr.Zero)
-            {
-                _parent_scene.DeactivatePrim(this);
-            }
+            // Spurious - Body == IntPtr.Zero after disableBody()
+//            if (Body != IntPtr.Zero)
+//            {
+//                _parent_scene.DeactivatePrim(this);
+//            }
 
             lock (childrenPrim)
             {
@@ -1303,11 +1297,8 @@ Console.WriteLine("ZProcessTaints for " + Name);
                     disableBodySoft();
                 }
 
-                if (prim_geom != IntPtr.Zero)
-                {
-                    d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
-                    d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
-                }
+                d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
+                d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
 
                 if (IsPhysical)
                 {
@@ -1328,11 +1319,8 @@ Console.WriteLine("ZProcessTaints for " + Name);
                 if (m_collidesWater)
                     m_collisionFlags |= CollisionCategories.Water;
 
-                if (prim_geom != IntPtr.Zero)
-                {
-                    d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
-                    d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
-                }
+                d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
+                d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
 
                 if (IsPhysical)
                 {
@@ -1472,6 +1460,9 @@ Console.WriteLine("CreateGeom:");
             }
             else
             {
+                m_log.WarnFormat(
+                    "[ODE PRIM]: Called RemoveGeom() on {0} {1} where geometry was already null.", Name, LocalID);
+
                 return false;
             }
         }
@@ -1505,16 +1496,13 @@ Console.WriteLine("changeadd 1");
 #endif
             CreateGeom(m_targetSpace, mesh);
 
-            if (prim_geom != IntPtr.Zero)
-            {
-                d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
-                d.Quaternion myrot = new d.Quaternion();
-                myrot.X = _orientation.X;
-                myrot.Y = _orientation.Y;
-                myrot.Z = _orientation.Z;
-                myrot.W = _orientation.W;
-                d.GeomSetQuaternion(prim_geom, ref myrot);
-            }
+            d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
+            d.Quaternion myrot = new d.Quaternion();
+            myrot.X = _orientation.X;
+            myrot.Y = _orientation.Y;
+            myrot.Z = _orientation.Z;
+            myrot.W = _orientation.W;
+            d.GeomSetQuaternion(prim_geom, ref myrot);
 
             if (IsPhysical && Body == IntPtr.Zero)
                 enableBody();
@@ -1579,24 +1567,20 @@ Console.WriteLine(" JointCreateFixed");
                     //m_log.Debug("[BUG]: race!");
                 //}
             }
-            else
-            {
-                // string primScenAvatarIn = _parent_scene.whichspaceamIin(_position);
-                // int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position);
-//                _parent_scene.waitForSpaceUnlock(m_targetSpace);
 
-                IntPtr tempspace = _parent_scene.recalculateSpaceForGeom(prim_geom, _position, m_targetSpace);
-                m_targetSpace = tempspace;
+            // string primScenAvatarIn = _parent_scene.whichspaceamIin(_position);
+            // int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position);
+//          _parent_scene.waitForSpaceUnlock(m_targetSpace);
+
+            IntPtr tempspace = _parent_scene.recalculateSpaceForGeom(prim_geom, _position, m_targetSpace);
+            m_targetSpace = tempspace;
 
 //                _parent_scene.waitForSpaceUnlock(m_targetSpace);
-                if (prim_geom != IntPtr.Zero)
-                {
-                    d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
+
+            d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
 
 //                    _parent_scene.waitForSpaceUnlock(m_targetSpace);
-                    d.SpaceAdd(m_targetSpace, prim_geom);
-                }
-            }
+            d.SpaceAdd(m_targetSpace, prim_geom);
 
             changeSelectedStatus();
 
@@ -2047,18 +2031,16 @@ Console.WriteLine(" JointCreateFixed");
         {
             m_collidesWater = m_taintCollidesWater;
 
-            if (prim_geom != IntPtr.Zero)
+            if (m_collidesWater)
             {
-                if (m_collidesWater)
-                {
-                    m_collisionFlags |= CollisionCategories.Water;
-                }
-                else
-                {
-                    m_collisionFlags &= ~CollisionCategories.Water;
-                }
-                d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
+                m_collisionFlags |= CollisionCategories.Water;
             }
+            else
+            {
+                m_collisionFlags &= ~CollisionCategories.Water;
+            }
+
+            d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
         }
 
         /// <summary>
diff --git a/OpenSim/Region/Physics/OdePlugin/OdeScene.cs b/OpenSim/Region/Physics/OdePlugin/OdeScene.cs
index 842ff91..fa65945 100644
--- a/OpenSim/Region/Physics/OdePlugin/OdeScene.cs
+++ b/OpenSim/Region/Physics/OdePlugin/OdeScene.cs
@@ -131,6 +131,41 @@ namespace OpenSim.Region.Physics.OdePlugin
         /// </remarks>
         internal static Object UniversalColliderSyncObject = new Object();
 
+        /// <summary>
+        /// Is stats collecting enabled for this ODE scene?
+        /// </summary>
+        public bool CollectStats { get; set; }
+
+        /// <summary>
+        /// Statistics for this scene.
+        /// </summary>
+        private Dictionary<string, float> m_stats = new Dictionary<string, float>();
+
+        /// <summary>
+        /// Stat name for recording the number of milliseconds that ODE spends in native collision code.
+        /// </summary>
+        public const string ODENativeCollisionFrameMsStatName = "ODENativeCollisionFrameMS";
+
+        /// <summary>
+        /// Used to hold tick numbers for stat collection purposes.
+        /// </summary>
+        private int m_nativeCollisionTickRecorder;
+
+        /// <summary>
+        /// A messy way to tell if we need to avoid adding a collision time because this was already done in the callback.
+        /// </summary>
+        private bool m_inCollisionTiming;
+
+        /// <summary>
+        /// Used in calculating physics frame time dilation
+        /// </summary>
+        private int tickCountFrameRun;
+
+        /// <summary>
+        /// Used in calculating physics frame time dilation
+        /// </summary>
+        private int latertickcount;
+
         private Random fluidRandomizer = new Random(Environment.TickCount);
 
         private const uint m_regionWidth = Constants.RegionSize;
@@ -345,9 +380,6 @@ namespace OpenSim.Region.Physics.OdePlugin
         private OdePrim cp1;
         private OdeCharacter cc2;
         private OdePrim cp2;
-        private int tickCountFrameRun;
-        
-        private int latertickcount=0;
         //private int cStartStop = 0;
         //private string cDictKey = "";
 
@@ -440,6 +472,8 @@ namespace OpenSim.Region.Physics.OdePlugin
         // Initialize the mesh plugin
         public override void Initialise(IMesher meshmerizer, IConfigSource config)
         {
+            m_stats[ODENativeCollisionFrameMsStatName] = 0;
+
             mesher = meshmerizer;
             m_config = config;
             // Defaults
@@ -464,6 +498,8 @@ namespace OpenSim.Region.Physics.OdePlugin
                 IConfig physicsconfig = m_config.Configs["ODEPhysicsSettings"];
                 if (physicsconfig != null)
                 {
+                    CollectStats = physicsconfig.GetBoolean("collect_stats", false);
+
                     gravityx = physicsconfig.GetFloat("world_gravityx", 0f);
                     gravityy = physicsconfig.GetFloat("world_gravityy", 0f);
                     gravityz = physicsconfig.GetFloat("world_gravityz", -9.8f);
@@ -765,6 +801,62 @@ namespace OpenSim.Region.Physics.OdePlugin
         #region Collision Detection
 
         /// <summary>
+        /// Collides two geometries.
+        /// </summary>
+        /// <returns></returns>
+        /// <param name='geom1'></param>
+        /// <param name='geom2'>/param>
+        /// <param name='maxContacts'></param>
+        /// <param name='contactsArray'></param>
+        /// <param name='contactGeomSize'></param>
+        private int CollideGeoms(
+            IntPtr geom1, IntPtr geom2, int maxContacts, Ode.NET.d.ContactGeom[] contactsArray, int contactGeomSize)
+        {
+            int count;
+
+            lock (OdeScene.UniversalColliderSyncObject)
+            {
+                // We do this inside the lock so that we don't count any delay in acquiring it
+                if (CollectStats)
+                    m_nativeCollisionTickRecorder = Util.EnvironmentTickCount();
+
+                count = d.Collide(geom1, geom2, maxContacts, contactsArray, contactGeomSize);
+            }
+
+            // We do this outside the lock so that any waiting threads aren't held up, though the effect is probably
+            // negligable
+            if (CollectStats)
+                m_stats[ODENativeCollisionFrameMsStatName]
+                    += Util.EnvironmentTickCountSubtract(m_nativeCollisionTickRecorder);
+
+            return count;
+        }
+
+        /// <summary>
+        /// Collide two spaces or a space and a geometry.
+        /// </summary>
+        /// <param name='space1'></param>
+        /// <param name='space2'>/param>
+        /// <param name='data'></param>
+        private void CollideSpaces(IntPtr space1, IntPtr space2, IntPtr data)
+        {
+            if (CollectStats)
+            {
+                m_inCollisionTiming = true;
+                m_nativeCollisionTickRecorder = Util.EnvironmentTickCount();
+            }
+
+            d.SpaceCollide2(space1, space2, data, nearCallback);
+
+            if (CollectStats && m_inCollisionTiming)
+            {
+                m_stats[ODENativeCollisionFrameMsStatName]
+                    += Util.EnvironmentTickCountSubtract(m_nativeCollisionTickRecorder);
+                m_inCollisionTiming = false;
+            }
+        }
+
+        /// <summary>
         /// This is our near callback.  A geometry is near a body
         /// </summary>
         /// <param name="space">The space that contains the geoms.  Remember, spaces are also geoms</param>
@@ -772,6 +864,13 @@ namespace OpenSim.Region.Physics.OdePlugin
         /// <param name="g2">another geometry or space</param>
         private void near(IntPtr space, IntPtr g1, IntPtr g2)
         {
+            if (CollectStats && m_inCollisionTiming)
+            {
+                m_stats[ODENativeCollisionFrameMsStatName]
+                    += Util.EnvironmentTickCountSubtract(m_nativeCollisionTickRecorder);
+                m_inCollisionTiming = false;
+            }
+
 //            m_log.DebugFormat("[PHYSICS]: Colliding {0} and {1} in {2}", g1, g2, space);
             //  no lock here!  It's invoked from within Simulate(), which is thread-locked
 
@@ -789,7 +888,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                 // contact points in the space
                 try
                 {
-                    d.SpaceCollide2(g1, g2, IntPtr.Zero, nearCallback);
+                    CollideSpaces(g1, g2, IntPtr.Zero);
                 }
                 catch (AccessViolationException)
                 {
@@ -832,6 +931,7 @@ namespace OpenSim.Region.Physics.OdePlugin
 
             // Figure out how many contact points we have
             int count = 0;
+
             try
             {
                 // Colliding Geom To Geom
@@ -843,8 +943,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                 if (b1 != IntPtr.Zero && b2 != IntPtr.Zero && d.AreConnectedExcluding(b1, b2, d.JointType.Contact))
                     return;
 
-                lock (OdeScene.UniversalColliderSyncObject)
-                    count = d.Collide(g1, g2, contacts.Length, contacts, d.ContactGeom.SizeOf);
+                count = CollideGeoms(g1, g2, contacts.Length, contacts, d.ContactGeom.SizeOf);
 
                 if (count > contacts.Length)
                     m_log.Error("[ODE SCENE]: Got " + count + " contacts when we asked for a maximum of " + contacts.Length);
@@ -1578,7 +1677,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                 // and we'll run it again on all of them.
                 try
                 {
-                    d.SpaceCollide2(space, chr.Shell, IntPtr.Zero, nearCallback);
+                    CollideSpaces(space, chr.Shell, IntPtr.Zero);
                 }
                 catch (AccessViolationException)
                 {
@@ -1593,6 +1692,9 @@ namespace OpenSim.Region.Physics.OdePlugin
                 //}
             }
 
+//            if (framecount % 55 == 0)
+//                m_log.DebugFormat("Processed {0} collisions", _perloopContact.Count);
+
             List<OdePrim> removeprims = null;
             foreach (OdePrim chr in _activeprims)
             {
@@ -1604,7 +1706,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                         {
                             if (space != IntPtr.Zero && chr.prim_geom != IntPtr.Zero && chr.m_taintremove == false)
                             {
-                                d.SpaceCollide2(space, chr.prim_geom, IntPtr.Zero, nearCallback);
+                                CollideSpaces(space, chr.prim_geom, IntPtr.Zero);
                             }
                             else
                             {
@@ -2226,7 +2328,8 @@ namespace OpenSim.Region.Physics.OdePlugin
         /// <param name="prim"></param>
         internal void RemovePrimThreadLocked(OdePrim prim)
         {
-//Console.WriteLine("RemovePrimThreadLocked " +  prim.m_primName);
+//            m_log.DebugFormat("[ODE SCENE]: Removing physical prim {0} {1}", prim.Name, prim.LocalID);
+
             lock (prim)
             {
                 RemoveCollisionEventReporting(prim);
@@ -2688,7 +2791,7 @@ namespace OpenSim.Region.Physics.OdePlugin
         /// It calls the methods that report back to the object owners.. (scenepresence, SceneObjectGroup)
         /// </summary>
         /// <param name="timeStep"></param>
-        /// <returns></returns>
+        /// <returns>The number of frames simulated over that period.</returns>
         public override float Simulate(float timeStep)
         {
             if (framecount >= int.MaxValue)
@@ -3189,7 +3292,7 @@ namespace OpenSim.Region.Physics.OdePlugin
         public override bool IsThreaded
         {
             // for now we won't be multithreaded
-            get { return (false); }
+            get { return false; }
         }
 
         #region ODE Specific Terrain Fixes
@@ -3954,5 +4057,22 @@ namespace OpenSim.Region.Physics.OdePlugin
             ds.SetViewpoint(ref xyz, ref hpr);
         }
 #endif
+
+        public override Dictionary<string, float> GetStats()
+        {
+            if (!CollectStats)
+                return null;
+
+            Dictionary<string, float> returnStats;
+
+            lock (OdeLock)
+            {
+                returnStats = new Dictionary<string, float>(m_stats);
+
+                m_stats[ODENativeCollisionFrameMsStatName] = 0;
+            }
+
+            return returnStats;
+        }
     }
 }
\ No newline at end of file