7 files changed, 78 insertions, 44 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
index 4f48cfb..f164afe 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
@@ -488,7 +488,7 @@ public class BSCharacter : PhysicsActor
         // Avatars don't report their changes the usual way. Changes are checked for in the heartbeat loop.
         // base.RequestPhysicsterseUpdate();
 
-        DetailLog("{0},BSCharacter.UpdateProperties,child,pos={1},orient={2},vel={3},accel={4},rotVel={5}",
+        DetailLog("{0},BSCharacter.UpdateProperties,call,pos={1},orient={2},vel={3},accel={4},rotVel={5}",
                 LocalID, entprop.Position, entprop.Rotation, entprop.Velocity, 
                 entprop.Acceleration, entprop.RotationalVelocity);
     }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSConstraint.cs b/OpenSim/Region/Physics/BulletSPlugin/BSConstraint.cs
index a17efea..da26b72 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSConstraint.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSConstraint.cs
@@ -49,7 +49,9 @@ public abstract class BSConstraint : IDisposable
         if (m_enabled)
         {
             // BulletSimAPI.RemoveConstraint(m_world.ID, m_body1.ID, m_body2.ID);
-            BulletSimAPI.DestroyConstraint2(m_world.Ptr, m_constraint.Ptr);
+            bool success = BulletSimAPI.DestroyConstraint2(m_world.Ptr, m_constraint.Ptr);
+            m_world.scene.DetailLog("{0},BSConstraint.Dispose,taint,body1={1},body2={2},success={3}", BSScene.DetailLogZero, m_body1.ID, m_body2.ID, success);
+            m_constraint.Ptr = System.IntPtr.Zero;
             m_enabled = false;
         }
     }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSConstraintCollection.cs b/OpenSim/Region/Physics/BulletSPlugin/BSConstraintCollection.cs
index 397045a..3df2ddc 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSConstraintCollection.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSConstraintCollection.cs
@@ -68,6 +68,8 @@ public class BSConstraintCollection : IDisposable
         // There is only one constraint between any bodies. Remove any old just to make sure.
         RemoveAndDestroyConstraint(cons.Body1, cons.Body2);
 
+        m_world.scene.DetailLog("{0},BSConstraintCollection.AddConstraint,call,body1={1},body2={2}", BSScene.DetailLogZero, cons.Body1.ID, cons.Body2.ID);
+
         m_constraints.Add(cons);
 
         return true;
@@ -108,6 +110,7 @@ public class BSConstraintCollection : IDisposable
 
         if (this.TryGetConstraint(body1, body2, out constrain))
         {
+            m_world.scene.DetailLog("{0},BSConstraintCollection.RemoveAndDestroyConstraint,taint,body1={1},body2={2}", BSScene.DetailLogZero, body1.ID, body2.ID);
             // remove the constraint from our collection
             m_constraints.Remove(constrain);
             // tell the engine that all its structures need to be freed
@@ -148,10 +151,11 @@ public class BSConstraintCollection : IDisposable
 
     public bool RecalculateAllConstraints()
     {
-        foreach (BSConstraint constrain in m_constraints)
+        ForEachConstraint(delegate(BSConstraint constrain)
         {
             constrain.CalculateTransforms();
-        }
+            return false;
+        });
         return true;
     }
 
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
index f68e06e..4a71612 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
@@ -44,8 +44,6 @@ public class BSLinkset
 
     private List<BSPrim> m_children;
 
-    public int NumberOfChildren { get { return m_children.Count; } }
-
     // We lock the diddling of linkset classes to prevent any badness.
     // This locks the modification of the instances of this class. Changes
     //    to the physical representation is done via the tainting mechenism.
@@ -83,14 +81,14 @@ public class BSLinkset
 
     // Link to a linkset where the child knows the parent.
     // Parent changing should not happen so do some sanity checking.
-    // We return the parent's linkset so the child can track it's membership.
-    public BSLinkset AddMeToLinkset(BSPrim child, BSPrim parent)
+    // We return the parent's linkset so the child can track its membership.
+    public BSLinkset AddMeToLinkset(BSPrim child)
     {
         lock (m_linksetActivityLock)
         {
-            parent.Linkset.AddChildToLinkset(child);
+            AddChildToLinkset(child);
         }
-        return parent.Linkset;
+        return this;
     }
 
     public BSLinkset RemoveMeFromLinkset(BSPrim child)
@@ -176,6 +174,8 @@ public class BSLinkset
         return (requestor.LocalID == m_linksetRoot.LocalID);
     }
 
+    public int NumberOfChildren { get { return m_children.Count; } }
+
     // Return 'true' if this linkset has any children (more than the root member)
     public bool HasAnyChildren { get { return (m_children.Count > 0); } }
 
@@ -303,7 +303,7 @@ public class BSLinkset
         // create a constraint that allows no freedom of movement between the two objects
         // http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?t=4818
         // DebugLog("{0}: CreateLinkset: Adding a constraint between root prim {1} and child prim {2}", LogHeader, LocalID, childPrim.LocalID);
-        DetailLog("{0},LinkAChildToMe,taint,root={1},child={2}", rootPrim.LocalID, rootPrim.LocalID, childPrim.LocalID);
+        DetailLog("{0},PhysicallyLinkAChildToRoot,taint,root={1},child={2}", rootPrim.LocalID, rootPrim.LocalID, childPrim.LocalID);
         BS6DofConstraint constrain = new BS6DofConstraint(
                         m_scene.World, rootPrim.Body, childPrim.Body,
                         childRelativePosition,
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
index 4193d22..05cc822 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
@@ -138,13 +138,14 @@ public sealed class BSPrim : PhysicsActor
         _isPhysical = pisPhysical;
         _isVolumeDetect = false;
         _subscribedEventsMs = 0;
-        _friction = _scene.Params.defaultFriction; // TODO: compute based on object material
-        _density = _scene.Params.defaultDensity; // TODO: compute based on object material
+        _friction = _scene.Params.defaultFriction;  // TODO: compute based on object material
+        _density = _scene.Params.defaultDensity;    // TODO: compute based on object material
         _restitution = _scene.Params.defaultRestitution;
         _linkset = new BSLinkset(_scene, this);     // a linkset of one
-        _vehicle = new BSDynamics(this);    // add vehicleness
+        _vehicle = new BSDynamics(this);            // add vehicleness
         _mass = CalculateMass();
         // do the actual object creation at taint time
+        DetailLog("{0},BSPrim.constructor,call", LocalID);
         _scene.TaintedObject("BSPrim.create", delegate()
         {
             RecreateGeomAndObject();
@@ -160,19 +161,22 @@ public sealed class BSPrim : PhysicsActor
     public void Destroy()
     {
         // m_log.DebugFormat("{0}: Destroy, id={1}", LogHeader, LocalID);
-        // DetailLog("{0},BSPrim.Destroy", LocalID);
-
-        // Undo any vehicle properties
-        _vehicle.ProcessTypeChange(Vehicle.TYPE_NONE);
-        _scene.RemoveVehiclePrim(this);     // just to make sure
 
         // Undo any links between me and any other object
+        BSPrim parentBefore = _linkset.Root;
+        int childrenBefore = _linkset.NumberOfChildren;
+
         _linkset = _linkset.RemoveMeFromLinkset(this);
 
+        DetailLog("{0},BSPrim.Destroy,call,parentBefore={1},childrenBefore={2},parentAfter={3},childrenAfter={4}",
+            LocalID, parentBefore.LocalID, childrenBefore, _linkset.Root.LocalID, _linkset.NumberOfChildren);
+
+        // Undo any vehicle properties
+        this.VehicleType = (int)Vehicle.TYPE_NONE;
+
         _scene.TaintedObject("BSPrim.destroy", delegate()
         {
             DetailLog("{0},BSPrim.Destroy,taint,", LocalID);
-
             // everything in the C# world will get garbage collected. Tell the C++ world to free stuff.
             BulletSimAPI.DestroyObject(_scene.WorldID, LocalID);
         });
@@ -229,8 +233,13 @@ public sealed class BSPrim : PhysicsActor
         if (parent != null)
         {
             DebugLog("{0}: link {1}/{2} to {3}", LogHeader, _avName, _localID, parent.LocalID);
-            DetailLog("{0},BSPrim.link,parent={1}", LocalID, parent.LocalID);
-            _linkset = _linkset.AddMeToLinkset(this, parent);
+            BSPrim parentBefore = _linkset.Root;
+            int childrenBefore = _linkset.NumberOfChildren;
+
+            _linkset = parent.Linkset.AddMeToLinkset(this);
+
+            DetailLog("{0},BSPrim.link,call,parentBefore={1}, childrenBefore=={2}, parentAfter={3}, childrenAfter={4}", 
+                LocalID, parentBefore.LocalID, childrenBefore, _linkset.Root.LocalID, _linkset.NumberOfChildren);
         }
         return; 
     }
@@ -340,21 +349,14 @@ public sealed class BSPrim : PhysicsActor
         } 
         set {
             Vehicle type = (Vehicle)value;
-            _scene.TaintedObject("BSPrim.setVehicleType", delegate()
+            BSPrim vehiclePrim = this;
+            _scene.TaintedObject("setVehicleType", delegate()
             {
-                DetailLog("{0},BSPrim.SetVehicleType,taint,type={1}", LocalID, type);
+                // Done at taint time so we're sure the physics engine is not using the variables
+                // Vehicle code changes the parameters for this vehicle type.
                 _vehicle.ProcessTypeChange(type);
-                if (type == Vehicle.TYPE_NONE)
-                {
-                    _scene.RemoveVehiclePrim(this);
-                }
-                else
-                {
-                    BulletSimAPI.ClearForces2(this.Body.Ptr);
-                    // make it so the scene will call us each tick to do vehicle things
-                    _scene.AddVehiclePrim(this);
-                }
-                return;
+                // Tell the scene about the vehicle so it will get processing each frame.
+                _scene.VehicleInSceneTypeChanged(this, type);
             });
         } 
     }
@@ -493,8 +495,10 @@ public sealed class BSPrim : PhysicsActor
         // Bullet wants static objects to have a mass of zero
         float mass = IsStatic ? 0f : _mass;
 
-        DetailLog("{0},BSPrim.SetObjectDynamic,taint,static={1},solid={2},mass={3}", LocalID, IsStatic, IsSolid, mass);
         BulletSimAPI.SetObjectProperties(_scene.WorldID, LocalID, IsStatic, IsSolid, SubscribedEvents(), mass);
+
+        CollisionFlags cf = BulletSimAPI.GetCollisionFlags2(Body.Ptr);
+        DetailLog("{0},BSPrim.SetObjectDynamic,taint,static={1},solid={2},mass={3}, cf={4}", LocalID, IsStatic, IsSolid, mass, cf);
     }
 
     // prims don't fly
@@ -1224,7 +1228,7 @@ public sealed class BSPrim : PhysicsActor
         bool ret = BulletSimAPI.CreateObject(_scene.WorldID, shape);
 
         // the CreateObject() may have recreated the rigid body. Make sure we have the latest.
-        m_body.Ptr = BulletSimAPI.GetBodyHandle2(_scene.World.Ptr, LocalID);
+        Body = new BulletBody(LocalID, BulletSimAPI.GetBodyHandle2(_scene.World.Ptr, LocalID));
 
         return ret;
     }
@@ -1344,7 +1348,7 @@ public sealed class BSPrim : PhysicsActor
         else
         {
             // For debugging, we also report the movement of children
-            DetailLog("{0},BSPrim.BSPrim.UpdateProperties,child,pos={1},orient={2},vel={3},accel={4},rotVel={5}",
+            DetailLog("{0},BSPrim.UpdateProperties,child,pos={1},orient={2},vel={3},accel={4},rotVel={5}",
                     LocalID, entprop.Position, entprop.Rotation, entprop.Velocity, 
                     entprop.Acceleration, entprop.RotationalVelocity);
         }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs b/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
index 65a8014..beaea1f 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
@@ -255,7 +255,7 @@ public class BSScene : PhysicsScene, IPhysicsParameters
 
         // Initialization to support the transition to a new API which puts most of the logic
         //   into the C# code so it is easier to modify and add to.
-        m_worldSim = new BulletSim(m_worldID, BulletSimAPI.GetSimHandle2(m_worldID));
+        m_worldSim = new BulletSim(m_worldID, this, BulletSimAPI.GetSimHandle2(m_worldID));
         m_constraintCollection = new BSConstraintCollection(World);
 
         m_initialized = true;
@@ -362,6 +362,7 @@ public class BSScene : PhysicsScene, IPhysicsParameters
         BSPrim bsprim = prim as BSPrim;
         if (bsprim != null)
         {
+            DetailLog("{0},RemovePrim,call", bsprim.LocalID);
             // m_log.DebugFormat("{0}: RemovePrim. id={1}/{2}", LogHeader, bsprim.Name, bsprim.LocalID);
             try
             {
@@ -387,6 +388,8 @@ public class BSScene : PhysicsScene, IPhysicsParameters
 
         if (!m_initialized) return null;
 
+        DetailLog("{0},AddPrimShape,call", localID);
+
         BSPrim prim = new BSPrim(localID, primName, this, position, size, rotation, pbs, isPhysical);
         lock (m_prims) m_prims.Add(localID, prim);
         return prim;
@@ -426,12 +429,12 @@ public class BSScene : PhysicsScene, IPhysicsParameters
         {
             numSubSteps = BulletSimAPI.PhysicsStep(m_worldID, timeStep, m_maxSubSteps, m_fixedTimeStep,
                         out updatedEntityCount, out updatedEntitiesPtr, out collidersCount, out collidersPtr);
-            DetailLog("{0},Simulate,call, substeps={1}, updates={2}, colliders={3}", "0000000000", numSubSteps, updatedEntityCount, collidersCount); 
+            DetailLog("{0},Simulate,call, substeps={1}, updates={2}, colliders={3}", DetailLogZero, numSubSteps, updatedEntityCount, collidersCount); 
         }
         catch (Exception e)
         {
             m_log.WarnFormat("{0},PhysicsStep Exception: substeps={1}, updates={2}, colliders={3}, e={4}", LogHeader, numSubSteps, updatedEntityCount, collidersCount, e);
-            DetailLog("{0},PhysicsStepException,call, substeps={1}, updates={2}, colliders={3}", "0000000000", numSubSteps, updatedEntityCount, collidersCount);
+            DetailLog("{0},PhysicsStepException,call, substeps={1}, updates={2}, colliders={3}", DetailLogZero, numSubSteps, updatedEntityCount, collidersCount);
             // updatedEntityCount = 0;
             collidersCount = 0;
         }
@@ -777,6 +780,20 @@ public class BSScene : PhysicsScene, IPhysicsParameters
     }
 
     #region Vehicles
+
+    public void VehicleInSceneTypeChanged(BSPrim vehic, Vehicle newType)
+    {
+        if (newType == Vehicle.TYPE_NONE)
+        {
+            RemoveVehiclePrim(vehic);
+        }
+        else
+        {
+            // make it so the scene will call us each tick to do vehicle things
+           AddVehiclePrim(vehic);
+        }
+    }
+
     // Make so the scene will call this prim for vehicle actions each tick.
     // Safe to call if prim is already in the vehicle list.
     public void AddVehiclePrim(BSPrim vehicle)
@@ -1304,10 +1321,12 @@ public class BSScene : PhysicsScene, IPhysicsParameters
     #endregion Runtime settable parameters
 
     // Invoke the detailed logger and output something if it's enabled.
-    private void DetailLog(string msg, params Object[] args)
+    public void DetailLog(string msg, params Object[] args)
     {
         PhysicsLogging.Write(msg, args);
     }
+    // used to fill in the LocalID when there isn't one
+    public const string DetailLogZero = "0000000000";
 
 }
 }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs b/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
index c016402..6800b96 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
@@ -35,9 +35,11 @@ namespace OpenSim.Region.Physics.BulletSPlugin {
 // Classes to allow some type checking for the API
 public struct BulletSim
 {
-    public BulletSim(uint id, IntPtr xx) { ID = id;  Ptr = xx; }
-    public IntPtr Ptr;
+    public BulletSim(uint id, BSScene bss, IntPtr xx) { ID = id; scene = bss;  Ptr = xx; }
     public uint ID;
+    // The scene is only in here so very low level routines have a handle to print debug/error messages
+    public BSScene scene;
+    public IntPtr Ptr;
 }
 
 public struct BulletBody
@@ -492,6 +494,9 @@ public static extern bool SetLinearVelocity2(IntPtr obj, Vector3 val);
 public static extern bool SetInterpolation2(IntPtr obj, Vector3 lin, Vector3 ang);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern CollisionFlags GetCollisionFlags2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern IntPtr SetCollisionFlags2(IntPtr obj, CollisionFlags flags);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]