11 files changed, 1084 insertions, 427 deletions

diff --git a/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsActor.cs b/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsActor.cs
index b1a3ff9..e43136a 100644
--- a/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsActor.cs
+++ b/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsActor.cs
@@ -36,13 +36,7 @@ namespace OpenSim.Region.Physics.BasicPhysicsPlugin
 {
     public class BasicActor : PhysicsActor
     {
-        private Vector3 _position;
-        private Vector3 _velocity;
-        private Vector3 _acceleration;
         private Vector3 _size;
-        private Vector3 m_rotationalVelocity;
-        private bool flying;
-        private bool iscolliding;
 
         public BasicActor(Vector3 size)
         {
@@ -55,11 +49,7 @@ namespace OpenSim.Region.Physics.BasicPhysicsPlugin
             set { return; }
         }
 
-        public override Vector3 RotationalVelocity
-        {
-            get { return m_rotationalVelocity; }
-            set { m_rotationalVelocity = value; }
-        }
+        public override Vector3 RotationalVelocity { get; set; }
 
         public override bool SetAlwaysRun
         {
@@ -105,17 +95,9 @@ namespace OpenSim.Region.Physics.BasicPhysicsPlugin
             set { return; }
         }
 
-        public override bool Flying
-        {
-            get { return flying; }
-            set { flying = value; }
-        }
+        public override bool Flying { get; set; }
 
-        public override bool IsColliding
-        {
-            get { return iscolliding; }
-            set { iscolliding = value; }
-        }
+        public override bool IsColliding { get; set; }
 
         public override bool CollidingGround
         {
@@ -134,11 +116,7 @@ namespace OpenSim.Region.Physics.BasicPhysicsPlugin
             get { return false; }
         }
 
-        public override Vector3 Position
-        {
-            get { return _position; }
-            set { _position = value; }
-        }
+        public override Vector3 Position { get; set; }
 
         public override Vector3 Size
         {
@@ -206,11 +184,7 @@ namespace OpenSim.Region.Physics.BasicPhysicsPlugin
             get { return Vector3.Zero; }
         }
 
-        public override Vector3 Velocity
-        {
-            get { return _velocity; }
-            set { _velocity = value; }
-        }
+        public override Vector3 Velocity { get; set; }
 
         public override Vector3 Torque
         {
@@ -230,11 +204,7 @@ namespace OpenSim.Region.Physics.BasicPhysicsPlugin
             set { }
         }
 
-        public override Vector3 Acceleration
-        {
-            get { return _acceleration; }
-            set { _acceleration = value; }
-        }
+        public override Vector3 Acceleration { get; set; }
 
         public override bool Kinematic
         {
diff --git a/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsPrim.cs b/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsPrim.cs
new file mode 100644
index 0000000..47d7df3
--- /dev/null
+++ b/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsPrim.cs
@@ -0,0 +1,315 @@
+/*
+ * Copyright (c) Contributors, http://opensimulator.org/
+ * See CONTRIBUTORS.TXT for a full list of copyright holders.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the OpenSimulator Project nor the
+ *       names of its contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+using System;
+using System.Collections.Generic;
+using Nini.Config;
+using OpenMetaverse;
+using OpenSim.Framework;
+using OpenSim.Region.Physics.Manager;
+
+namespace OpenSim.Region.Physics.BasicPhysicsPlugin
+{
+    public class BasicPhysicsPrim : PhysicsActor
+    {
+        private Vector3 _size;
+//        private PrimitiveBaseShape _shape;
+
+        public BasicPhysicsPrim(
+            string name, uint localId, Vector3 position, Vector3 size, Quaternion orientation, PrimitiveBaseShape shape)
+        {
+            Name = name;
+            LocalID = localId;
+            Position = position;
+            Size = size;
+            Orientation = orientation;
+            Shape = shape;
+        }
+
+        public override int PhysicsActorType
+        {
+            get { return (int) ActorTypes.Agent; }
+            set { return; }
+        }
+
+        public override Vector3 RotationalVelocity { get; set; }
+
+        public override bool SetAlwaysRun
+        {
+            get { return false; }
+            set { return; }
+        }
+
+        public override uint LocalID
+        {
+            set { return; }
+        }
+
+        public override bool Grabbed
+        {
+            set { return; }
+        }
+
+        public override bool Selected
+        {
+            set { return; }
+        }
+
+        public override float Buoyancy
+        {
+            get { return 0f; }
+            set { return; }
+        }
+
+        public override bool FloatOnWater
+        {
+            set { return; }
+        }
+
+        public override bool IsPhysical
+        {
+            get { return false; }
+            set { return; }
+        }
+
+        public override bool ThrottleUpdates
+        {
+            get { return false; }
+            set { return; }
+        }
+
+        public override bool Flying { get; set; }
+
+        public override bool IsColliding { get; set; }
+
+        public override bool CollidingGround
+        {
+            get { return false; }
+            set { return; }
+        }
+
+        public override bool CollidingObj
+        {
+            get { return false; }
+            set { return; }
+        }
+
+        public override bool Stopped
+        {
+            get { return false; }
+        }
+
+        public override Vector3 Position { get; set; }
+
+        public override Vector3 Size
+        {
+            get { return _size; }
+            set {
+                  _size = value;
+                  _size.Z = _size.Z / 2.0f;
+                }
+        }
+
+        public override PrimitiveBaseShape Shape
+        {
+//            set { _shape = value; }
+            set {}
+        }
+
+        public override float Mass
+        {
+            get { return 0f; }
+        }
+
+        public override Vector3 Force
+        {
+            get { return Vector3.Zero; }
+            set { return; }
+        }
+
+        public override int VehicleType
+        {
+            get { return 0; }
+            set { return; }
+        }
+
+        public override void VehicleFloatParam(int param, float value)
+        {
+
+        }
+
+        public override void VehicleVectorParam(int param, Vector3 value)
+        {
+
+        }
+
+        public override void VehicleRotationParam(int param, Quaternion rotation)
+        {
+
+        }
+
+        public override void VehicleFlags(int param, bool remove)
+        {
+
+        }
+
+        public override void SetVolumeDetect(int param)
+        {
+
+        }
+
+        public override Vector3 CenterOfMass
+        {
+            get { return Vector3.Zero; }
+        }
+
+        public override Vector3 GeometricCenter
+        {
+            get { return Vector3.Zero; }
+        }
+
+        public override Vector3 Velocity { get; set; }
+
+        public override Vector3 Torque
+        {
+            get { return Vector3.Zero; }
+            set { return; }
+        }
+
+        public override float CollisionScore
+        {
+            get { return 0f; }
+            set { }
+        }
+
+        public override Quaternion Orientation { get; set; }
+
+        public override Vector3 Acceleration { get; set; }
+
+        public override bool Kinematic
+        {
+            get { return true; }
+            set { }
+        }
+
+        public override void link(PhysicsActor obj)
+        {
+        }
+
+        public override void delink()
+        {
+        }
+
+        public override void LockAngularMotion(Vector3 axis)
+        {
+        }
+
+        public override void AddForce(Vector3 force, bool pushforce)
+        {
+        }
+
+        public override void AddAngularForce(Vector3 force, bool pushforce)
+        {
+        }
+
+        public override void SetMomentum(Vector3 momentum)
+        {
+        }
+
+        public override void CrossingFailure()
+        {
+        }
+
+        public override Vector3 PIDTarget
+        {
+            set { return; }
+        }
+
+        public override bool PIDActive
+        {
+            set { return; }
+        }
+
+        public override float PIDTau
+        {
+            set { return; }
+        }
+
+        public override float PIDHoverHeight
+        {
+            set { return; }
+        }
+
+        public override bool PIDHoverActive
+        {
+            set { return; }
+        }
+
+        public override PIDHoverType PIDHoverType
+        {
+            set { return; }
+        }
+
+        public override float PIDHoverTau
+        {
+            set { return; }
+        }
+
+        public override Quaternion APIDTarget 
+        { 
+            set { return; }
+        }
+        
+        public override bool APIDActive
+        { 
+            set { return; }
+        }
+        
+        public override float APIDStrength
+        { 
+            set { return; }
+        }
+        
+        public override float APIDDamping
+        { 
+            set { return; }
+        }
+
+        public override void SubscribeEvents(int ms)
+        {
+        }
+
+        public override void UnSubscribeEvents()
+        {
+        }
+
+        public override bool SubscribedEvents()
+        {
+            return false;
+        }
+    }
+}
diff --git a/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsScene.cs b/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsScene.cs
index 2e14216..f5826ed 100644
--- a/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsScene.cs
+++ b/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsScene.cs
@@ -34,9 +34,17 @@ using OpenSim.Region.Physics.Manager;
 
 namespace OpenSim.Region.Physics.BasicPhysicsPlugin
 {
+    /// <summary>
+    /// This is an incomplete extremely basic physics implementation
+    /// </summary>
+    /// <remarks>
+    /// Not useful for anything at the moment apart from some regression testing in other components where some form
+    /// of physics plugin is needed.
+    /// </remarks>
     public class BasicScene : PhysicsScene
     {
         private List<BasicActor> _actors = new List<BasicActor>();
+        private List<BasicPhysicsPrim> _prims = new List<BasicPhysicsPrim>();
         private float[] _heightMap;
 
         //protected internal string sceneIdentifier;
@@ -50,10 +58,19 @@ namespace OpenSim.Region.Physics.BasicPhysicsPlugin
         {
         }
 
-        public override void Dispose()
+        public override void Dispose() {}
+
+        public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
+                                                  Vector3 size, Quaternion rotation, bool isPhysical, uint localid)
         {
+            BasicPhysicsPrim prim = new BasicPhysicsPrim(primName, localid, position, size, rotation, pbs);
+            prim.IsPhysical = isPhysical;
+
+            _prims.Add(prim);
 
+            return prim;
         }
+
         public override PhysicsActor AddAvatar(string avName, Vector3 position, Vector3 size, bool isFlying)
         {
             BasicActor act = new BasicActor(size);
@@ -63,30 +80,18 @@ namespace OpenSim.Region.Physics.BasicPhysicsPlugin
             return act;
         }
 
-        public override void RemovePrim(PhysicsActor prim)
+        public override void RemovePrim(PhysicsActor actor)
         {
+            BasicPhysicsPrim prim = (BasicPhysicsPrim)actor;
+            if (_prims.Contains(prim))
+                _prims.Remove(prim);
         }
 
         public override void RemoveAvatar(PhysicsActor actor)
         {
-            BasicActor act = (BasicActor) actor;
+            BasicActor act = (BasicActor)actor;
             if (_actors.Contains(act))
-            {
                 _actors.Remove(act);
-            }
-        }
-
-/*
-        public override PhysicsActor AddPrim(Vector3 position, Vector3 size, Quaternion rotation)
-        {
-            return null;
-        }
-*/
-
-        public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
-                                                  Vector3 size, Quaternion rotation, bool isPhysical, uint localid)
-        {
-            return null;
         }
 
         public override void AddPhysicsActorTaint(PhysicsActor prim)
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
index b08d5db..dc0c008 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
@@ -74,7 +74,7 @@ public class BSCharacter : PhysicsActor
     private float _buoyancy;
 
     private int _subscribedEventsMs = 0;
-    private int _lastCollisionTime = 0;
+    private int _nextCollisionOkTime = 0;
 
     private Vector3 _PIDTarget;
     private bool _usePID;
@@ -360,17 +360,22 @@ public class BSCharacter : PhysicsActor
         }
         //m_lastUpdateSent = false;
     }
+
     public override void AddAngularForce(Vector3 force, bool pushforce) { 
     }
     public override void SetMomentum(Vector3 momentum) { 
     }
+
+    // Turn on collision events at a rate no faster than one every the given milliseconds
     public override void SubscribeEvents(int ms) {
         _subscribedEventsMs = ms;
-        _lastCollisionTime = Util.EnvironmentTickCount() - _subscribedEventsMs; // make first collision happen
+        _nextCollisionOkTime = Util.EnvironmentTickCount() - _subscribedEventsMs; // make first collision happen
     }
+    // Stop collision events
     public override void UnSubscribeEvents() { 
         _subscribedEventsMs = 0;
     }
+    // Return 'true' if someone has subscribed to events
     public override bool SubscribedEvents() {
         return (_subscribedEventsMs > 0);
     }
@@ -386,47 +391,57 @@ public class BSCharacter : PhysicsActor
         _mass = _density * _avatarVolume;
     }
 
+    // Set to 'true' if the individual changed items should be checked
+    //   (someday RequestPhysicsTerseUpdate() will take a bitmap of changed properties)
+    const bool SHOULD_CHECK_FOR_INDIVIDUAL_CHANGES = false;
+
     // The physics engine says that properties have updated. Update same and inform
     // the world that things have changed.
     public void UpdateProperties(EntityProperties entprop)
     {
         bool changed = false;
-        // we assign to the local variables so the normal set action does not happen
-        if (_position != entprop.Position)
-        {
-            _position = entprop.Position;
-            changed = true;
+        if (SHOULD_CHECK_FOR_INDIVIDUAL_CHANGES) {
+            // we assign to the local variables so the normal set action does not happen
+            if (_position != entprop.Position) {
+                _position = entprop.Position;
+                changed = true;
+            }
+            if (_orientation != entprop.Rotation) {
+                _orientation = entprop.Rotation;
+                changed = true;
+            }
+            if (_velocity != entprop.Velocity) {
+                _velocity = entprop.Velocity;
+                changed = true;
+            }
+            if (_acceleration != entprop.Acceleration) {
+                _acceleration = entprop.Acceleration;
+                changed = true;
+            }
+            if (_rotationalVelocity != entprop.RotationalVelocity) {
+                _rotationalVelocity = entprop.RotationalVelocity;
+                changed = true;
+            }
+            if (changed) {
+                // m_log.DebugFormat("{0}: UpdateProperties: id={1}, c={2}, pos={3}, rot={4}", LogHeader, LocalID, changed, _position, _orientation);
+                // Avatar movement is not done by generating this event. There is code in the heartbeat
+                //   loop that updates avatars.
+                // base.RequestPhysicsterseUpdate();
+            }
         }
-        if (_orientation != entprop.Rotation)
-        {
+        else {
+            _position = entprop.Position;
             _orientation = entprop.Rotation;
-            changed = true;
-        }
-        if (_velocity != entprop.Velocity)
-        {
             _velocity = entprop.Velocity;
-            changed = true;
-        }
-        if (_acceleration != entprop.Acceleration)
-        {
             _acceleration = entprop.Acceleration;
-            changed = true;
-        }
-        if (_rotationalVelocity != entprop.RotationalVelocity)
-        {
             _rotationalVelocity = entprop.RotationalVelocity;
-            changed = true;
-        }
-        if (changed)
-        {
-            // m_log.DebugFormat("{0}: UpdateProperties: id={1}, c={2}, pos={3}, rot={4}", LogHeader, LocalID, changed, _position, _orientation);
-            // Avatar movement is not done by generating this event. There is a system that
-            //  checks for avatar updates each heartbeat loop.
             // base.RequestPhysicsterseUpdate();
         }
     }
 
     // Called by the scene when a collision with this object is reported
+    // The collision, if it should be reported to the character, is placed in a collection
+    //   that will later be sent to the simulator when SendCollisions() is called.
     CollisionEventUpdate collisionCollection = null;
     public void Collide(uint collidingWith, ActorTypes type, Vector3 contactPoint, Vector3 contactNormal, float pentrationDepth)
     {
@@ -440,29 +455,34 @@ public class BSCharacter : PhysicsActor
         }
 
         // throttle collisions to the rate specified in the subscription
-        if (_subscribedEventsMs == 0) return;   // don't want collisions
-        int nowTime = _scene.SimulationNowTime;
-        if (nowTime < (_lastCollisionTime + _subscribedEventsMs)) return;
-        _lastCollisionTime = nowTime;
+        if (_subscribedEventsMs != 0) {
+            int nowTime = _scene.SimulationNowTime;
+            if (nowTime >= _nextCollisionOkTime) {
+                _nextCollisionOkTime = nowTime + _subscribedEventsMs;
 
-        if (collisionCollection == null)
-            collisionCollection = new CollisionEventUpdate();
-        collisionCollection.AddCollider(collidingWith, new ContactPoint(contactPoint, contactNormal, pentrationDepth));
+                if (collisionCollection == null)
+                    collisionCollection = new CollisionEventUpdate();
+                collisionCollection.AddCollider(collidingWith, new ContactPoint(contactPoint, contactNormal, pentrationDepth));
+            }
+        }
     }
 
     public void SendCollisions()
     {
-        // if (collisionCollection != null)
-        // {
-        //     base.SendCollisionUpdate(collisionCollection);
-        //     collisionCollection = null;
-        // }
+        /*
+        if (collisionCollection != null && collisionCollection.Count > 0)
+        {
+            base.SendCollisionUpdate(collisionCollection);
+            collisionCollection = null;
+        }
+         */
         // Kludge to make a collision call even if there are no collisions.
         // This causes the avatar animation to get updated.
         if (collisionCollection == null)
             collisionCollection = new CollisionEventUpdate();
         base.SendCollisionUpdate(collisionCollection);
-        collisionCollection = null;
+        collisionCollection.Clear();
+        // End kludge
     }
 
 }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPlugin.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPlugin.cs
index 0730824..0f027b8 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPlugin.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPlugin.cs
@@ -32,6 +32,14 @@ using OpenMetaverse;
 
 namespace OpenSim.Region.Physics.BulletSPlugin
 {
+    /// <summary>
+    /// Entry for a port of Bullet (http://bulletphysics.org/) to OpenSim. 
+    /// This module interfaces to an unmanaged C++ library which makes the
+    /// actual calls into the Bullet physics engine.
+    /// The unmanaged library is found in opensim-libs::trunk/unmanaged/BulletSim/.
+    /// The unmanaged library is compiled and linked statically with Bullet
+    /// to create BulletSim.dll and libBulletSim.so (for both 32 and 64 bit).
+    /// </summary>
 public class BSPlugin : IPhysicsPlugin
 {
     //private static readonly log4net.ILog m_log = log4net.LogManager.GetLogger(System.Reflection.MethodBase.GetCurrentMethod().DeclaringType);
@@ -53,6 +61,9 @@ public class BSPlugin : IPhysicsPlugin
         {
             if (Util.IsWindows())
                 Util.LoadArchSpecificWindowsDll("BulletSim.dll");
+            // If not Windows, loading is performed by the
+            // Mono loader as specified in 
+            // "bin/Physics/OpenSim.Region.Physics.BulletSPlugin.dll.config".
 
             _mScene = new BSScene(sceneIdentifier);
         }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
index 248d1f2..130f1ca 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
@@ -90,7 +90,7 @@ public sealed class BSPrim : PhysicsActor
     private BSPrim _parentPrim;
 
     private int _subscribedEventsMs = 0;
-    private int _lastCollisionTime = 0;
+    private int _nextCollisionOkTime = 0;
     long _collidingStep;
     long _collidingGroundStep;
 
@@ -597,7 +597,8 @@ public sealed class BSPrim : PhysicsActor
     }
     public override void SubscribeEvents(int ms) { 
         _subscribedEventsMs = ms;
-        _lastCollisionTime = Util.EnvironmentTickCount() - _subscribedEventsMs; // make first collision happen
+        // make sure first collision happens
+        _nextCollisionOkTime = Util.EnvironmentTickCount() - _subscribedEventsMs;
     }
     public override void UnSubscribeEvents() { 
         _subscribedEventsMs = 0;
@@ -1338,23 +1339,27 @@ public sealed class BSPrim : PhysicsActor
             _collidingGroundStep = _scene.SimulationStep;
         }
 
-        if (_subscribedEventsMs == 0) return;   // nothing in the object is waiting for collision events
-        // throttle the collisions to the number of milliseconds specified in the subscription
-        int nowTime = _scene.SimulationNowTime;
-        if (nowTime < (_lastCollisionTime + _subscribedEventsMs)) return;
-        _lastCollisionTime = nowTime;
+        // if someone is subscribed to collision events....
+        if (_subscribedEventsMs != 0) {
+            // throttle the collisions to the number of milliseconds specified in the subscription
+            int nowTime = _scene.SimulationNowTime;
+            if (nowTime >= _nextCollisionOkTime) {
+                _nextCollisionOkTime = nowTime + _subscribedEventsMs;
 
-        if (collisionCollection == null)
-            collisionCollection = new CollisionEventUpdate();
-        collisionCollection.AddCollider(collidingWith, new ContactPoint(contactPoint, contactNormal, pentrationDepth));
+                if (collisionCollection == null)
+                    collisionCollection = new CollisionEventUpdate();
+                collisionCollection.AddCollider(collidingWith, new ContactPoint(contactPoint, contactNormal, pentrationDepth));
+            }
+        }
     }
 
+    // The scene is telling us it's time to pass our collected collisions into the simulator
     public void SendCollisions()
     {
-        if (collisionCollection != null)
+        if (collisionCollection != null && collisionCollection.Count > 0)
         {
             base.SendCollisionUpdate(collisionCollection);
-            collisionCollection = null;
+            collisionCollection.Clear();
         }
     }
 }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs b/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
index 94a0ccf..417cb5f 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
@@ -52,6 +52,7 @@ using OpenSim.Region.Framework;
 // Should prim.link() and prim.delink() membership checking happen at taint time?
 // Mesh sharing. Use meshHash to tell if we already have a hull of that shape and only create once
 // Do attachments need to be handled separately? Need collision events. Do not collide with VolumeDetect
+// Use collision masks for collision with terrain and phantom objects 
 // Implement the genCollisions feature in BulletSim::SetObjectProperties (don't pass up unneeded collisions)
 // Implement LockAngularMotion
 // Decide if clearing forces is the right thing to do when setting position (BulletSim::SetObjectTranslation)
@@ -62,9 +63,6 @@ using OpenSim.Region.Framework;
 // Multiple contact points on collision?
 //    See code in ode::near... calls to collision_accounting_events()
 //    (This might not be a problem. ODE collects all the collisions with one object in one tick.)
-// Use collision masks for collision with terrain and phantom objects 
-// Figure out how to not allocate a new Dictionary and List for every collision
-//    in BSPrim.Collide() and BSCharacter.Collide(). Can the same ones be reused?
 // Raycast
 // 
 namespace OpenSim.Region.Physics.BulletSPlugin
@@ -405,6 +403,8 @@ public class BSScene : PhysicsScene, IPhysicsParameters
         // prevent simulation until we've been initialized
         if (!m_initialized) return 10.0f;
 
+        long simulateStartTime = Util.EnvironmentTickCount();
+
         // update the prim states while we know the physics engine is not busy
         ProcessTaints();
 
@@ -437,13 +437,18 @@ public class BSScene : PhysicsScene, IPhysicsParameters
             }
         }
 
-        // The SendCollision's batch up the collisions on the objects. Now push the collisions into the simulator.
+        // The above SendCollision's batch up the collisions on the objects.
+        //      Now push the collisions into the simulator.
         foreach (BSPrim bsp in m_primsWithCollisions)
             bsp.SendCollisions();
         m_primsWithCollisions.Clear();
+
+        // This is a kludge to get avatar movement updated. 
+        //   Don't send collisions only if there were collisions -- send everytime.
+        //   ODE sends collisions even if there are none and this is used to update
+        //   avatar animations and stuff.
         // foreach (BSCharacter bsc in m_avatarsWithCollisions)
         //     bsc.SendCollisions();
-        // This is a kludge to get avatar movement updated. ODE sends collisions even if there isn't any
         foreach (KeyValuePair<uint, BSCharacter> kvp in m_avatars)
             kvp.Value.SendCollisions();
         m_avatarsWithCollisions.Clear();
@@ -465,10 +470,12 @@ public class BSScene : PhysicsScene, IPhysicsParameters
                 if (m_avatars.TryGetValue(entprop.ID, out actor))
                 {
                     actor.UpdateProperties(entprop);
+                    continue;
                 }
             }
         }
 
+        // If enabled, call into the physics engine to dump statistics
         if (m_detailedStatsStep > 0)
         {
             if ((m_simulationStep % m_detailedStatsStep) == 0)
@@ -477,6 +484,11 @@ public class BSScene : PhysicsScene, IPhysicsParameters
             }
         }
 
+        // this is a waste since the outside routine also calcuates the physics simulation
+        //   period. TODO: There should be a way of computing physics frames from simulator computation.
+        // long simulateTotalTime = Util.EnvironmentTickCountSubtract(simulateStartTime);
+        // return (timeStep * (float)simulateTotalTime);
+
         // TODO: FIX THIS: fps calculation wrong. This calculation always returns about 1 in normal operation.
         return timeStep / (numSubSteps * m_fixedTimeStep) * 1000f;
     }
@@ -528,6 +540,7 @@ public class BSScene : PhysicsScene, IPhysicsParameters
     public override void SetWaterLevel(float baseheight) 
     {
         m_waterLevel = baseheight;
+        // TODO: pass to physics engine so things will float?
     }
     public float GetWaterLevel()
     {
diff --git a/OpenSim/Region/Physics/Manager/PhysicsScene.cs b/OpenSim/Region/Physics/Manager/PhysicsScene.cs
index d10a2aa..cfede55 100644
--- a/OpenSim/Region/Physics/Manager/PhysicsScene.cs
+++ b/OpenSim/Region/Physics/Manager/PhysicsScene.cs
@@ -241,8 +241,22 @@ namespace OpenSim.Region.Physics.Manager
 
         public abstract void AddPhysicsActorTaint(PhysicsActor prim);
 
+        /// <summary>
+        /// Perform a simulation of the current physics scene over the given timestep.
+        /// </summary>
+        /// <param name="timeStep"></param>
+        /// <returns>The number of frames simulated over that period.</returns>
         public abstract float Simulate(float timeStep);
 
+        /// <summary>
+        /// Get statistics about this scene.
+        /// </summary>
+        /// <remarks>This facility is currently experimental and subject to change.</remarks>
+        /// <returns>
+        /// A dictionary where the key is the statistic name.  If no statistics are supplied then returns null.
+        /// </returns>
+        public virtual Dictionary<string, float> GetStats() { return null; }
+
         public abstract void GetResults();
 
         public abstract void SetTerrain(float[] heightMap);
diff --git a/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs
index 8397eb4..f3b0630 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..32e81e2 100644
--- a/OpenSim/Region/Physics/OdePlugin/OdeScene.cs
+++ b/OpenSim/Region/Physics/OdePlugin/OdeScene.cs
@@ -30,20 +30,21 @@
 
 using System;
 using System.Collections.Generic;
+using System.Diagnostics;
+using System.IO;
+using System.Linq;
 using System.Reflection;
 using System.Runtime.InteropServices;
 using System.Threading;
-using System.IO;
-using System.Diagnostics;
 using log4net;
 using Nini.Config;
 using Ode.NET;
+using OpenMetaverse;
 #if USE_DRAWSTUFF
 using Drawstuff.NET;
 #endif 
 using OpenSim.Framework;
 using OpenSim.Region.Physics.Manager;
-using OpenMetaverse;
 
 namespace OpenSim.Region.Physics.OdePlugin
 {
@@ -54,15 +55,15 @@ namespace OpenSim.Region.Physics.OdePlugin
         End = 2
     }
 
-    public struct sCollisionData
-    {
-        public uint ColliderLocalId;
-        public uint CollidedWithLocalId;
-        public int NumberOfCollisions;
-        public int CollisionType;
-        public int StatusIndicator;
-        public int lastframe;
-    }
+//    public struct sCollisionData
+//    {
+//        public uint ColliderLocalId;
+//        public uint CollidedWithLocalId;
+//        public int NumberOfCollisions;
+//        public int CollisionType;
+//        public int StatusIndicator;
+//        public int lastframe;
+//    }
 
     [Flags]
     public enum CollisionCategories : int
@@ -131,6 +132,135 @@ 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 total number of avatars in this ODE scene.
+        /// </summary>
+        public const string ODETotalAvatarsStatName = "ODETotalAvatars";
+
+        /// <summary>
+        /// Stat name for total number of prims in this ODE scene.
+        /// </summary>
+        public const string ODETotalPrimsStatName = "ODETotalPrims";
+
+        /// <summary>
+        /// Stat name for total number of prims with active physics in this ODE scene.
+        /// </summary>
+        public const string ODEActivePrimsStatName = "ODEActivePrims";
+
+        /// <summary>
+        /// Stat name for the total time spent in ODE frame processing.
+        /// </summary>
+        /// <remarks>
+        /// A sanity check for the main scene loop physics time.
+        /// </remarks>
+        public const string ODETotalFrameMsStatName = "ODETotalFrameMS";
+
+        /// <summary>
+        /// Stat name for time spent processing avatar taints per frame
+        /// </summary>
+        public const string ODEAvatarTaintMsStatName = "ODEAvatarTaintFrameMS";
+
+        /// <summary>
+        /// Stat name for time spent processing prim taints per frame
+        /// </summary>
+        public const string ODEPrimTaintMsStatName = "ODEPrimTaintFrameMS";
+
+        /// <summary>
+        /// Stat name for time spent calculating avatar forces per frame.
+        /// </summary>
+        public const string ODEAvatarForcesFrameMsStatName = "ODEAvatarForcesFrameMS";
+
+        /// <summary>
+        /// Stat name for time spent calculating prim forces per frame
+        /// </summary>
+        public const string ODEPrimForcesFrameMsStatName = "ODEPrimForcesFrameMS";
+
+        /// <summary>
+        /// Stat name for time spent fulfilling raycasting requests per frame
+        /// </summary>
+        public const string ODERaycastingFrameMsStatName = "ODERaycastingFrameMS";
+
+        /// <summary>
+        /// Stat name for time spent in native code that actually steps through the simulation.
+        /// </summary>
+        public const string ODENativeStepFrameMsStatName = "ODENativeStepFrameMS";
+
+        /// <summary>
+        /// Stat name for the number of milliseconds that ODE spends in native space collision code.
+        /// </summary>
+        public const string ODENativeSpaceCollisionFrameMsStatName = "ODENativeSpaceCollisionFrameMS";
+
+        /// <summary>
+        /// Stat name for milliseconds that ODE spends in native geom collision code.
+        /// </summary>
+        public const string ODENativeGeomCollisionFrameMsStatName = "ODENativeGeomCollisionFrameMS";
+
+        /// <summary>
+        /// Time spent in collision processing that is not spent in native space or geom collision code.
+        /// </summary>
+        public const string ODEOtherCollisionFrameMsStatName = "ODEOtherCollisionFrameMS";
+
+        /// <summary>
+        /// Stat name for time spent notifying listeners of collisions
+        /// </summary>
+        public const string ODECollisionNotificationFrameMsStatName = "ODECollisionNotificationFrameMS";
+
+        /// <summary>
+        /// Stat name for milliseconds spent updating avatar position and velocity
+        /// </summary>
+        public const string ODEAvatarUpdateFrameMsStatName = "ODEAvatarUpdateFrameMS";
+
+        /// <summary>
+        /// Stat name for the milliseconds spent updating prim position and velocity
+        /// </summary>
+        public const string ODEPrimUpdateFrameMsStatName = "ODEPrimUpdateFrameMS";
+
+        /// <summary>
+        /// Stat name for avatar collisions with another entity.
+        /// </summary>
+        public const string ODEAvatarContactsStatsName = "ODEAvatarContacts";
+
+        /// <summary>
+        /// Stat name for prim collisions with another entity.
+        /// </summary>
+        public const string ODEPrimContactsStatName = "ODEPrimContacts";
+
+        /// <summary>
+        /// Used to hold tick numbers for stat collection purposes.
+        /// </summary>
+        private int m_nativeCollisionStartTick;
+
+        /// <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>
+        /// A temporary holder for the number of avatar collisions in a frame, so we can work out how many object
+        /// collisions occured using the _perloopcontact if stats collection is enabled.
+        /// </summary>
+        private int m_tempAvatarCollisionsThisFrame;
+
+        /// <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;
@@ -257,12 +387,12 @@ namespace OpenSim.Region.Physics.OdePlugin
         /// <summary>
         /// A dictionary of actors that should receive collision events.
         /// </summary>
-        private readonly Dictionary<uint, PhysicsActor> _collisionEventPrim = new Dictionary<uint, PhysicsActor>();
+        private readonly Dictionary<uint, PhysicsActor> m_collisionEventActors = new Dictionary<uint, PhysicsActor>();
 
         /// <summary>
         /// A dictionary of collision event changes that are waiting to be processed.
         /// </summary>
-        private readonly Dictionary<uint, PhysicsActor> _collisionEventPrimChanges = new Dictionary<uint, PhysicsActor>();
+        private readonly Dictionary<uint, PhysicsActor> m_collisionEventActorsChanges = new Dictionary<uint, PhysicsActor>();
 
         /// <summary>
         /// Maps a unique geometry id (a memory location) to a physics actor name.
@@ -345,9 +475,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 +567,8 @@ namespace OpenSim.Region.Physics.OdePlugin
         // Initialize the mesh plugin
         public override void Initialise(IMesher meshmerizer, IConfigSource config)
         {
+            InitializeExtraStats();
+
             mesher = meshmerizer;
             m_config = config;
             // Defaults
@@ -464,6 +593,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 +896,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_nativeCollisionStartTick = 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[ODENativeGeomCollisionFrameMsStatName]
+                    += Util.EnvironmentTickCountSubtract(m_nativeCollisionStartTick);
+
+            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_nativeCollisionStartTick = Util.EnvironmentTickCount();
+            }
+
+            d.SpaceCollide2(space1, space2, data, nearCallback);
+
+            if (CollectStats && m_inCollisionTiming)
+            {
+                m_stats[ODENativeSpaceCollisionFrameMsStatName]
+                    += Util.EnvironmentTickCountSubtract(m_nativeCollisionStartTick);
+                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 +959,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[ODENativeSpaceCollisionFrameMsStatName]
+                    += Util.EnvironmentTickCountSubtract(m_nativeCollisionStartTick);
+                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 +983,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 +1026,7 @@ namespace OpenSim.Region.Physics.OdePlugin
 
             // Figure out how many contact points we have
             int count = 0;
+
             try
             {
                 // Colliding Geom To Geom
@@ -843,8 +1038,11 @@ 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);
+
+                // All code after this is only relevant if we have any collisions
+                if (count <= 0)
+                    return;
 
                 if (count > contacts.Length)
                     m_log.Error("[ODE SCENE]: Got " + count + " contacts when we asked for a maximum of " + contacts.Length);
@@ -1113,14 +1311,12 @@ namespace OpenSim.Region.Physics.OdePlugin
                 {
                     _perloopContact.Add(curContact);
 
-                    // If we're colliding against terrain
                     if (name1 == "Terrain" || name2 == "Terrain")
                     {
-                        // If we're moving
                         if ((p2.PhysicsActorType == (int) ActorTypes.Agent) &&
                             (Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f))
                         {
-                            // Use the movement terrain contact
+                            // Avatar is moving on terrain, use the movement terrain contact
                             AvatarMovementTerrainContact.geom = curContact;
 
                             if (m_global_contactcount < maxContactsbeforedeath)
@@ -1133,7 +1329,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                         {
                             if (p2.PhysicsActorType == (int)ActorTypes.Agent)
                             {
-                                // Use the non moving terrain contact
+                                // Avatar is standing on terrain, use the non moving terrain contact
                                 TerrainContact.geom = curContact;
 
                                 if (m_global_contactcount < maxContactsbeforedeath)
@@ -1228,13 +1424,11 @@ namespace OpenSim.Region.Physics.OdePlugin
                     }
                     else
                     {
-                        // we're colliding with prim or avatar
-                        // check if we're moving
                         if ((p2.PhysicsActorType == (int)ActorTypes.Agent))
                         {
                             if ((Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f))
                             {
-                                // Use the Movement prim contact
+                                // Avatar is moving on a prim, use the Movement prim contact
                                 AvatarMovementprimContact.geom = curContact;
 
                                 if (m_global_contactcount < maxContactsbeforedeath)
@@ -1245,9 +1439,8 @@ namespace OpenSim.Region.Physics.OdePlugin
                             }
                             else
                             {
-                                // Use the non movement contact
+                                // Avatar is standing still on a prim, use the non movement contact
                                 contact.geom = curContact;
-                                _perloopContact.Add(curContact);
 
                                 if (m_global_contactcount < maxContactsbeforedeath)
                                 {
@@ -1355,7 +1548,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                                 break;
                             }
                         }
-                        //m_log.DebugFormat("[Collsion]: Depth {0}", Math.Abs(contact.depth - contactGeom.depth));
+                        //m_log.DebugFormat("[Collision]: Depth {0}", Math.Abs(contact.depth - contactGeom.depth));
                         //m_log.DebugFormat("[Collision]: <{0},{1},{2}>", Math.Abs(contactGeom.normal.X - contact.normal.X), Math.Abs(contactGeom.normal.Y - contact.normal.Y), Math.Abs(contactGeom.normal.Z - contact.normal.Z));
                     }
                 }
@@ -1578,7 +1771,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 +1786,12 @@ namespace OpenSim.Region.Physics.OdePlugin
                 //}
             }
 
+            if (CollectStats)
+            {
+                m_tempAvatarCollisionsThisFrame = _perloopContact.Count;
+                m_stats[ODEAvatarContactsStatsName] += m_tempAvatarCollisionsThisFrame;
+            }
+
             List<OdePrim> removeprims = null;
             foreach (OdePrim chr in _activeprims)
             {
@@ -1604,7 +1803,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
                             {
@@ -1625,6 +1824,9 @@ namespace OpenSim.Region.Physics.OdePlugin
                 }
             }
 
+            if (CollectStats)
+                m_stats[ODEPrimContactsStatName] += _perloopContact.Count - m_tempAvatarCollisionsThisFrame;
+
             if (removeprims != null)
             {
                 foreach (OdePrim chr in removeprims)
@@ -1706,8 +1908,8 @@ namespace OpenSim.Region.Physics.OdePlugin
         {
 //            m_log.DebugFormat("[PHYSICS]: Adding {0} {1} to collision event reporting", obj.SOPName, obj.LocalID);
             
-            lock (_collisionEventPrimChanges)
-                _collisionEventPrimChanges[obj.LocalID] = obj;
+            lock (m_collisionEventActorsChanges)
+                m_collisionEventActorsChanges[obj.LocalID] = obj;
         }
 
         /// <summary>
@@ -1718,8 +1920,8 @@ namespace OpenSim.Region.Physics.OdePlugin
         {
 //            m_log.DebugFormat("[PHYSICS]: Removing {0} {1} from collision event reporting", obj.SOPName, obj.LocalID);
 
-            lock (_collisionEventPrimChanges)
-                _collisionEventPrimChanges[obj.LocalID] = null;
+            lock (m_collisionEventActorsChanges)
+                m_collisionEventActorsChanges[obj.LocalID] = null;
         }
 
         #region Add/Remove Entities
@@ -2226,7 +2428,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);
@@ -2682,21 +2885,23 @@ namespace OpenSim.Region.Physics.OdePlugin
 
         /// <summary>
         /// This is our main simulate loop
+        /// </summary>
+        /// <remarks>
         /// It's thread locked by a Mutex in the scene.
         /// It holds Collisions, it instructs ODE to step through the physical reactions
         /// It moves the objects around in memory
         /// It calls the methods that report back to the object owners.. (scenepresence, SceneObjectGroup)
-        /// </summary>
+        /// </remarks>
         /// <param name="timeStep"></param>
-        /// <returns></returns>
+        /// <returns>The number of frames simulated over that period.</returns>
         public override float Simulate(float timeStep)
         {
+            int startFrameTick = CollectStats ? Util.EnvironmentTickCount() : 0;
+            int tempTick = 0, tempTick2 = 0;
+
             if (framecount >= int.MaxValue)
                 framecount = 0;
 
-            //if (m_worldOffset != Vector3.Zero)
-            //    return 0;
-
             framecount++;
 
             float fps = 0;
@@ -2704,7 +2909,7 @@ namespace OpenSim.Region.Physics.OdePlugin
             float timeLeft = timeStep;
 
             //m_log.Info(timeStep.ToString());
-//            step_time += timeStep;
+//            step_time += timeSte
 //            
 //            // If We're loaded down by something else,
 //            // or debugging with the Visual Studio project on pause
@@ -2725,17 +2930,17 @@ namespace OpenSim.Region.Physics.OdePlugin
             // We change _collisionEventPrimChanges to avoid locking _collisionEventPrim itself and causing potential
             // deadlock if the collision event tries to lock something else later on which is already locked by a
             // caller that is adding or removing the collision event.
-            lock (_collisionEventPrimChanges)
+            lock (m_collisionEventActorsChanges)
             {
-                foreach (KeyValuePair<uint, PhysicsActor> kvp in _collisionEventPrimChanges)
+                foreach (KeyValuePair<uint, PhysicsActor> kvp in m_collisionEventActorsChanges)
                 {
                     if (kvp.Value == null)
-                        _collisionEventPrim.Remove(kvp.Key);
+                        m_collisionEventActors.Remove(kvp.Key);
                     else
-                        _collisionEventPrim[kvp.Key] = kvp.Value;
+                        m_collisionEventActors[kvp.Key] = kvp.Value;
                 }
 
-                _collisionEventPrimChanges.Clear();
+                m_collisionEventActorsChanges.Clear();
             }
 
             if (SupportsNINJAJoints)
@@ -2770,6 +2975,9 @@ namespace OpenSim.Region.Physics.OdePlugin
                 {
                     try
                     {
+                        if (CollectStats)
+                            tempTick = Util.EnvironmentTickCount();
+
                         lock (_taintedActors)
                         {
                             foreach (OdeCharacter character in _taintedActors)
@@ -2778,6 +2986,13 @@ namespace OpenSim.Region.Physics.OdePlugin
                             _taintedActors.Clear();
                         }
 
+                        if (CollectStats)
+                        {
+                            tempTick2 = Util.EnvironmentTickCount();
+                            m_stats[ODEAvatarTaintMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick);
+                            tempTick = tempTick2;
+                        }
+
                         lock (_taintedPrims)
                         {
                             foreach (OdePrim prim in _taintedPrims)
@@ -2808,6 +3023,13 @@ namespace OpenSim.Region.Physics.OdePlugin
                             _taintedPrims.Clear();
                         }
 
+                        if (CollectStats)
+                        {
+                            tempTick2 = Util.EnvironmentTickCount();
+                            m_stats[ODEPrimTaintMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick);
+                            tempTick = tempTick2;
+                        }
+
                         // Move characters
                         foreach (OdeCharacter actor in _characters)
                             actor.Move(defects);
@@ -2827,6 +3049,13 @@ namespace OpenSim.Region.Physics.OdePlugin
                             defects.Clear();
                         }
 
+                        if (CollectStats)
+                        {
+                            tempTick2 = Util.EnvironmentTickCount();
+                            m_stats[ODEAvatarForcesFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick);
+                            tempTick = tempTick2;
+                        }
+
                         // Move other active objects
                         foreach (OdePrim prim in _activeprims)
                         {
@@ -2834,15 +3063,36 @@ namespace OpenSim.Region.Physics.OdePlugin
                             prim.Move(timeStep);
                         }
 
+                        if (CollectStats)
+                        {
+                            tempTick2 = Util.EnvironmentTickCount();
+                            m_stats[ODEPrimForcesFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick);
+                            tempTick = tempTick2;
+                        }
+
                         //if ((framecount % m_randomizeWater) == 0)
                            // randomizeWater(waterlevel);
 
                         //int RayCastTimeMS = m_rayCastManager.ProcessQueuedRequests();
                         m_rayCastManager.ProcessQueuedRequests();
 
+                        if (CollectStats)
+                        {
+                            tempTick2 = Util.EnvironmentTickCount();
+                            m_stats[ODERaycastingFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick);
+                            tempTick = tempTick2;
+                        }
+
                         collision_optimized();
 
-                        foreach (PhysicsActor obj in _collisionEventPrim.Values)
+                        if (CollectStats)
+                        {
+                            tempTick2 = Util.EnvironmentTickCount();
+                            m_stats[ODEOtherCollisionFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick);
+                            tempTick = tempTick2;
+                        }
+
+                        foreach (PhysicsActor obj in m_collisionEventActors.Values)
                         {
 //                                m_log.DebugFormat("[PHYSICS]: Assessing {0} {1} for collision events", obj.SOPName, obj.LocalID);
 
@@ -2866,9 +3116,19 @@ namespace OpenSim.Region.Physics.OdePlugin
 //                                "[PHYSICS]: Collision contacts to process this frame = {0}", m_global_contactcount);
 
                         m_global_contactcount = 0;
-                        
+
+                        if (CollectStats)
+                        {
+                            tempTick2 = Util.EnvironmentTickCount();
+                            m_stats[ODECollisionNotificationFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick);
+                            tempTick = tempTick2;
+                        }
+
                         d.WorldQuickStep(world, ODE_STEPSIZE);
 
+                        if (CollectStats)
+                            m_stats[ODENativeStepFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick);
+
                         d.JointGroupEmpty(contactgroup);
                     }
                     catch (Exception e)
@@ -2879,6 +3139,9 @@ namespace OpenSim.Region.Physics.OdePlugin
                     timeLeft -= ODE_STEPSIZE;
                 }
 
+                if (CollectStats)
+                    tempTick = Util.EnvironmentTickCount();
+
                 foreach (OdeCharacter actor in _characters)
                 {
                     if (actor.bad)
@@ -2902,6 +3165,13 @@ namespace OpenSim.Region.Physics.OdePlugin
                     defects.Clear();
                 }
 
+                if (CollectStats)
+                {
+                    tempTick2 = Util.EnvironmentTickCount();
+                    m_stats[ODEAvatarUpdateFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick);
+                    tempTick = tempTick2;
+                }
+
                 //if (timeStep < 0.2f)
 
                 foreach (OdePrim prim in _activeprims)
@@ -2915,6 +3185,9 @@ namespace OpenSim.Region.Physics.OdePlugin
                     }
                 }
 
+                if (CollectStats)
+                    m_stats[ODEPrimUpdateFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick);
+
                 //DumpJointInfo();
 
                 // Finished with all sim stepping. If requested, dump world state to file for debugging.
@@ -2936,7 +3209,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                     d.WorldExportDIF(world, fname, physics_logging_append_existing_logfile, prefix);
                 }
 
-                latertickcount = Util.EnvironmentTickCount() - tickCountFrameRun;
+                latertickcount = Util.EnvironmentTickCountSubtract(tickCountFrameRun);
 
                 // OpenSimulator above does 10 fps.  10 fps = means that the main thread loop and physics
                 // has a max of 100 ms to run theoretically.
@@ -2954,6 +3227,9 @@ namespace OpenSim.Region.Physics.OdePlugin
                 }
 
                 tickCountFrameRun = Util.EnvironmentTickCount();
+
+                if (CollectStats)
+                    m_stats[ODETotalFrameMsStatName] += Util.EnvironmentTickCountSubtract(startFrameTick);
             }
 
             return fps;
@@ -3189,7 +3465,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
@@ -3765,26 +4041,19 @@ namespace OpenSim.Region.Physics.OdePlugin
 
         public override Dictionary<uint, float> GetTopColliders()
         {
-            Dictionary<uint, float> returncolliders = new Dictionary<uint, float>();
-            int cnt = 0;
+            Dictionary<uint, float> topColliders;
+
             lock (_prims)
             {
-                foreach (OdePrim prm in _prims)
-                {
-                    if (prm.CollisionScore > 0)
-                    {
-                        returncolliders.Add(prm.LocalID, prm.CollisionScore);
-                        cnt++;
-                        prm.CollisionScore = 0f;
-                        if (cnt > 25)
-                        {
-                            break;
-                        }
-                    }
-                }
+                List<OdePrim> orderedPrims = new List<OdePrim>(_prims);
+                orderedPrims.OrderByDescending(p => p.CollisionScore).Take(25);
+                topColliders = orderedPrims.ToDictionary(p => p.LocalID, p => p.CollisionScore);
+
+                foreach (OdePrim p in _prims)
+                    p.CollisionScore = 0;
             }
 
-            return returncolliders;
+            return topColliders;
         }
 
         public override bool SupportsRayCast()
@@ -3954,5 +4223,52 @@ 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);
+
+                // FIXME: This is a SUPER DUMB HACK until we can establish stats that aren't subject to a division by
+                // 3 from the SimStatsReporter.
+                returnStats[ODETotalAvatarsStatName] = _characters.Count * 3;
+                returnStats[ODETotalPrimsStatName] = _prims.Count * 3;
+                returnStats[ODEActivePrimsStatName] = _activeprims.Count * 3;
+
+                InitializeExtraStats();
+            }
+
+            returnStats[ODEOtherCollisionFrameMsStatName]
+                = returnStats[ODEOtherCollisionFrameMsStatName]
+                    - returnStats[ODENativeSpaceCollisionFrameMsStatName]
+                    - returnStats[ODENativeGeomCollisionFrameMsStatName];
+
+            return returnStats;
+        }
+
+        private void InitializeExtraStats()
+        {
+            m_stats[ODETotalFrameMsStatName] = 0;
+            m_stats[ODEAvatarTaintMsStatName] = 0;
+            m_stats[ODEPrimTaintMsStatName] = 0;
+            m_stats[ODEAvatarForcesFrameMsStatName] = 0;
+            m_stats[ODEPrimForcesFrameMsStatName] = 0;
+            m_stats[ODERaycastingFrameMsStatName] = 0;
+            m_stats[ODENativeStepFrameMsStatName] = 0;
+            m_stats[ODENativeSpaceCollisionFrameMsStatName] = 0;
+            m_stats[ODENativeGeomCollisionFrameMsStatName] = 0;
+            m_stats[ODEOtherCollisionFrameMsStatName] = 0;
+            m_stats[ODECollisionNotificationFrameMsStatName] = 0;
+            m_stats[ODEAvatarContactsStatsName] = 0;
+            m_stats[ODEPrimContactsStatName] = 0;
+            m_stats[ODEAvatarUpdateFrameMsStatName] = 0;
+            m_stats[ODEPrimUpdateFrameMsStatName] = 0;
+        }
     }
 }
\ No newline at end of file