BulletSim: Merge branch 'BulletSim2017'

Adds raycast using Bullet physics engine.

16 files changed, 414 insertions, 86 deletions

diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSAPIUnman.cs b/OpenSim/Region/PhysicsModules/BulletS/BSAPIUnman.cs
index 42db7fe..840e453 100755
--- a/OpenSim/Region/PhysicsModules/BulletS/BSAPIUnman.cs
+++ b/OpenSim/Region/PhysicsModules/BulletS/BSAPIUnman.cs
@@ -1405,6 +1405,19 @@ public override float GetMargin(BulletShape shape)
 }
 
 // =====================================================================================
+// Raycast
+public override SweepHit ConvexSweepTest2(BulletWorld world, BulletBody sweepObject, Vector3 from, Vector3 to, float margin) {
+    BulletWorldUnman worldu = world as BulletWorldUnman;
+    BulletBodyUnman bodyu = sweepObject as BulletBodyUnman;
+    return BSAPICPP.ConvexSweepTest2(worldu.ptr, bodyu.ptr, from, to, margin);
+}
+
+public override RaycastHit RayTest2(BulletWorld world, Vector3 from, Vector3 to, uint filterGroup, uint filterMask) {
+    BulletWorldUnman worldu = world as BulletWorldUnman;
+    return BSAPICPP.RayTest2(worldu.ptr, from, to, filterGroup, filterMask);
+}
+
+// =====================================================================================
 // Debugging
 public override void DumpRigidBody(BulletWorld world, BulletBody collisionObject)
 {
@@ -2084,6 +2097,15 @@ public static extern void SetMargin2(IntPtr shape, float val);
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern float GetMargin2(IntPtr shape);
 
+
+// =====================================================================================
+// Raycast
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern SweepHit ConvexSweepTest2(IntPtr sim, IntPtr obj, Vector3 from, Vector3 to, float margin);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern RaycastHit RayTest2(IntPtr sim, Vector3 from, Vector3 to, uint filterGroup, uint filterMask);
+
 // =====================================================================================
 // Debugging
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSAPIXNA.cs b/OpenSim/Region/PhysicsModules/BulletS/BSAPIXNA.cs
index 37017b0..7d58728 100755
--- a/OpenSim/Region/PhysicsModules/BulletS/BSAPIXNA.cs
+++ b/OpenSim/Region/PhysicsModules/BulletS/BSAPIXNA.cs
@@ -2459,6 +2459,14 @@ private sealed class BulletConstraintXNA : BulletConstraint
         }
         return false;
     }
+
+    public override SweepHit ConvexSweepTest2(BulletWorld world, BulletBody obj, Vector3 from, Vector3 to, float margin) {
+        return new SweepHit();
+    }
+
+    public override RaycastHit RayTest2(BulletWorld world, Vector3 from, Vector3 to, uint filterGroup, uint filterMask) {
+        return new RaycastHit();
+    }
 }
 
 
diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSApiTemplate.cs b/OpenSim/Region/PhysicsModules/BulletS/BSApiTemplate.cs
index 816189f..a288048 100644
--- a/OpenSim/Region/PhysicsModules/BulletS/BSApiTemplate.cs
+++ b/OpenSim/Region/PhysicsModules/BulletS/BSApiTemplate.cs
@@ -121,6 +121,14 @@ public struct SweepHit
     public float Fraction;
     public Vector3 Normal;
     public Vector3 Point;
+
+    public bool hasHit()
+    {
+        float sum = Fraction
+                + Normal.X + Normal.Y + Normal.Z
+                + Point.X + Point.Y + Point.Z;
+        return (sum != 0) || (ID != 0);
+    }
 }
 [StructLayout(LayoutKind.Sequential)]
 public struct RaycastHit
@@ -128,6 +136,13 @@ public struct RaycastHit
     public UInt32 ID;
     public float Fraction;
     public Vector3 Normal;
+    public Vector3 Point;
+
+    public bool hasHit()
+    {
+        float sum = Normal.X + Normal.Y + Normal.Z + Point.X + Point.Y + Point.Z;
+        return (sum != 0);
+    }
 }
 [StructLayout(LayoutKind.Sequential)]
 public struct CollisionDesc
@@ -742,6 +757,12 @@ public abstract void SetMargin(BulletShape shape, float val);
 public abstract float GetMargin(BulletShape shape);
 
 // =====================================================================================
+// Raycast
+public abstract SweepHit ConvexSweepTest2(BulletWorld world, BulletBody obj, Vector3 from, Vector3 to, float margin);
+
+public abstract RaycastHit RayTest2(BulletWorld world, Vector3 from, Vector3 to, uint filterGroup, uint filterMask);
+
+// =====================================================================================
 // Debugging
 public virtual void DumpRigidBody(BulletWorld sim, BulletBody collisionObject) { }
 
diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSCharacter.cs b/OpenSim/Region/PhysicsModules/BulletS/BSCharacter.cs
index 57f03fb..7faee70 100644
--- a/OpenSim/Region/PhysicsModules/BulletS/BSCharacter.cs
+++ b/OpenSim/Region/PhysicsModules/BulletS/BSCharacter.cs
@@ -496,8 +496,7 @@ public sealed class BSCharacter : BSPhysObject
     public override OMV.Vector3 ForceVelocity {
         get { return RawVelocity; }
         set {
-            PhysScene.AssertInTaintTime("BSCharacter.ForceVelocity");
-            DetailLog("{0}: BSCharacter.ForceVelocity.set = {1}", LocalID, value);
+            DetailLog("{0},BSCharacter.ForceVelocity.set={1}", LocalID, value);
 
             RawVelocity = Util.ClampV(value, BSParam.MaxLinearVelocity);
             PhysScene.PE.SetLinearVelocity(PhysBody, RawVelocity);
@@ -638,8 +637,6 @@ public sealed class BSCharacter : BSPhysObject
     public override float ForceBuoyancy {
         get { return _buoyancy; }
         set {
-            PhysScene.AssertInTaintTime("BSCharacter.ForceBuoyancy");
-
             _buoyancy = value;
             DetailLog("{0},BSCharacter.setForceBuoyancy,taint,buoy={1}", LocalID, _buoyancy);
             // Buoyancy is faked by changing the gravity applied to the object
diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSLinksetCompound.cs b/OpenSim/Region/PhysicsModules/BulletS/BSLinksetCompound.cs
index 953ddee..dc390b2 100755
--- a/OpenSim/Region/PhysicsModules/BulletS/BSLinksetCompound.cs
+++ b/OpenSim/Region/PhysicsModules/BulletS/BSLinksetCompound.cs
@@ -450,6 +450,7 @@ public sealed class BSLinksetCompound : BSLinkset
             m_physicsScene.PE.AddObjectToWorld(m_physicsScene.World, LinksetRoot.PhysBody);
             DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,addBody,body={1},shape={2}",
                                         LinksetRoot.LocalID, LinksetRoot.PhysBody, linksetShape);
+            m_physicsScene.PE.ResetBroadphasePool(m_physicsScene.World);    // DEBUG DEBUG
 
             // With all of the linkset packed into the root prim, it has the mass of everyone.
             LinksetMass = ComputeLinksetMass();
diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSParam.cs b/OpenSim/Region/PhysicsModules/BulletS/BSParam.cs
index 352c03e..fcda92c 100755
--- a/OpenSim/Region/PhysicsModules/BulletS/BSParam.cs
+++ b/OpenSim/Region/PhysicsModules/BulletS/BSParam.cs
@@ -230,6 +230,8 @@ public static class BSParam
     public static float LinkConstraintCFM { get; private set; }
     public static float LinkConstraintSolverIterations { get; private set; }
 
+    public static bool UseBulletRaycast { get; private set; }
+
     public static float PID_D { get; private set; }    // derivative
     public static float PID_P { get; private set; }    // proportional
 
@@ -823,6 +825,9 @@ public static class BSParam
         new ParameterDefn<float>("LinkConstraintSolverIterations", "Number of solver iterations when computing constraint. (0 = Bullet default)",
             40 ),
 
+        new ParameterDefn<bool>("UseBulletRaycast", "If 'true', use the raycast function of the Bullet physics engine",
+            true ),
+
         new ParameterDefn<float>("DebugNumber", "A console setable number sometimes used for debugging",
             1.0f ),
 
@@ -833,7 +838,7 @@ public static class BSParam
         new ParameterDefn<float>("ResetBroadphasePool", "Setting this is any value resets the broadphase collision pool",
             0f,
             (s) => { return 0f; },
-            (s,v) => { BSParam.ResetBroadphasePoolTainted(s, v, false /* inTaintTime */); } ),
+            (s,v) => { BSParam.ResetBroadphasePoolTainted(s, v); } ),
         new ParameterDefn<float>("ResetConstraintSolver", "Setting this is any value resets the constraint solver",
             0f,
             (s) => { return 0f; },
@@ -919,10 +924,10 @@ public static class BSParam
     // =====================================================================
     // There are parameters that, when set, cause things to happen in the physics engine.
     // This causes the broadphase collision cache to be cleared.
-    private static void ResetBroadphasePoolTainted(BSScene pPhysScene, float v, bool inTaintTime)
+    private static void ResetBroadphasePoolTainted(BSScene pPhysScene, float v)
     {
         BSScene physScene = pPhysScene;
-        physScene.TaintedObject(inTaintTime, "BSParam.ResetBroadphasePoolTainted", delegate()
+        physScene.TaintedObject(BSScene.DetailLogZero, "BSParam.ResetBroadphasePoolTainted", delegate()
         {
             physScene.PE.ResetBroadphasePool(physScene.World);
         });
diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSPrim.cs b/OpenSim/Region/PhysicsModules/BulletS/BSPrim.cs
index b153761..f085d70 100644
--- a/OpenSim/Region/PhysicsModules/BulletS/BSPrim.cs
+++ b/OpenSim/Region/PhysicsModules/BulletS/BSPrim.cs
@@ -790,8 +790,6 @@ public class BSPrim : BSPhysObject
     public override OMV.Vector3 ForceVelocity {
         get { return RawVelocity; }
         set {
-            PhysScene.AssertInTaintTime("BSPrim.ForceVelocity");
-
             RawVelocity = Util.ClampV(value, BSParam.MaxLinearVelocity);
             if (PhysBody.HasPhysicalBody)
             {
diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSPrimDisplaced.cs b/OpenSim/Region/PhysicsModules/BulletS/BSPrimDisplaced.cs
index d8ed56b..3f90fc5 100755
--- a/OpenSim/Region/PhysicsModules/BulletS/BSPrimDisplaced.cs
+++ b/OpenSim/Region/PhysicsModules/BulletS/BSPrimDisplaced.cs
@@ -81,7 +81,6 @@ public class BSPrimDisplaced : BSPrim
     // Called at taint time.
     public virtual Vector3 SetEffectiveCenterOfMassDisplacement(Vector3 centerOfMassDisplacement)
     {
-        PhysScene.AssertInTaintTime("BSPrimDisplaced.SetEffectiveCenterOfMassDisplacement");
         Vector3 comDisp;
         if (UserSetCenterOfMassDisplacement.HasValue)
             comDisp = (OMV.Vector3)UserSetCenterOfMassDisplacement;
diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSScene.cs b/OpenSim/Region/PhysicsModules/BulletS/BSScene.cs
index 7ff0a07..163efaa 100644
--- a/OpenSim/Region/PhysicsModules/BulletS/BSScene.cs
+++ b/OpenSim/Region/PhysicsModules/BulletS/BSScene.cs
@@ -124,9 +124,10 @@ namespace OpenSim.Region.PhysicsModule.BulletS
         // True if initialized and ready to do simulation steps
         private bool m_initialized = false;
 
-        // Flag which is true when processing taints.
-        // Not guaranteed to be correct all the time (don't depend on this) but good for debugging.
-        public bool InTaintTime { get; private set; }
+        // Object locked whenever execution is inside the physics engine
+        public Object PhysicsEngineLock = new object();
+        // Flag that is true when the simulator is active and shouldn't be touched
+        public bool InSimulationTime { get; private set; }
 
         // Pinned memory used to pass step information between managed and unmanaged
         internal int m_maxCollisionsPerFrame;
@@ -344,7 +345,7 @@ namespace OpenSim.Region.PhysicsModule.BulletS
             // Put some informational messages into the log file.
             m_log.InfoFormat("{0} Linksets implemented with {1}", LogHeader, (BSLinkset.LinksetImplementation)BSParam.LinksetImplementation);
 
-            InTaintTime = false;
+            InSimulationTime = false;
             m_initialized = true;
 
             // If the physics engine runs on its own thread, start same.
@@ -657,48 +658,57 @@ namespace OpenSim.Region.PhysicsModule.BulletS
 
             int beforeTime = Util.EnvironmentTickCount();
             int simTime = 0;
+            int numTaints = 0;
+            int numSubSteps = 0;
 
-            int numTaints = _taintOperations.Count;
-            InTaintTime = true; // Only used for debugging so locking is not necessary.
+            lock (PhysicsEngineLock)
+            {
+                InSimulationTime = true;
+                // update the prim states while we know the physics engine is not busy
+                numTaints += ProcessTaints();
 
-            // update the prim states while we know the physics engine is not busy
-            ProcessTaints();
+                // Some of the physical objects requre individual, pre-step calls
+                //      (vehicles and avatar movement, in particular)
+                TriggerPreStepEvent(timeStep);
 
-            // Some of the physical objects requre individual, pre-step calls
-            //      (vehicles and avatar movement, in particular)
-            TriggerPreStepEvent(timeStep);
+                // the prestep actions might have added taints
+                numTaints += ProcessTaints();
 
-            // the prestep actions might have added taints
-            numTaints += _taintOperations.Count;
-            ProcessTaints();
+                // The following causes the unmanaged code to output ALL the values found in ALL the objects in the world.
+                // Only enable this in a limited test world with few objects.
+                if (m_physicsPhysicalDumpEnabled)
+                    PE.DumpAllInfo(World);
 
-            InTaintTime = false; // Only used for debugging so locking is not necessary.
+                // step the physical world one interval
+                m_simulationStep++;
+                try
+                {
+                    numSubSteps = PE.PhysicsStep(World, timeStep, m_maxSubSteps, m_fixedTimeStep, out updatedEntityCount, out collidersCount);
+                }
+                catch (Exception e)
+                {
+                    m_log.WarnFormat("{0},PhysicsStep Exception: nTaints={1}, substeps={2}, updates={3}, colliders={4}, e={5}",
+                                LogHeader, numTaints, numSubSteps, updatedEntityCount, collidersCount, e);
+                    DetailLog("{0},PhysicsStepException,call, nTaints={1}, substeps={2}, updates={3}, colliders={4}",
+                                DetailLogZero, numTaints, numSubSteps, updatedEntityCount, collidersCount);
+                    updatedEntityCount = 0;
+                    collidersCount = 0;
+                }
 
-            // The following causes the unmanaged code to output ALL the values found in ALL the objects in the world.
-            // Only enable this in a limited test world with few objects.
-            if (m_physicsPhysicalDumpEnabled)
-                PE.DumpAllInfo(World);
+                // Make the physics engine dump useful statistics periodically
+                if (PhysicsMetricDumpFrames != 0 && ((m_simulationStep % PhysicsMetricDumpFrames) == 0))
+                    PE.DumpPhysicsStatistics(World);
 
-            // step the physical world one interval
-            m_simulationStep++;
-            int numSubSteps = 0;
-            try
-            {
-                numSubSteps = PE.PhysicsStep(World, timeStep, m_maxSubSteps, m_fixedTimeStep, out updatedEntityCount, out collidersCount);
-            }
-            catch (Exception e)
-            {
-                m_log.WarnFormat("{0},PhysicsStep Exception: nTaints={1}, substeps={2}, updates={3}, colliders={4}, e={5}",
-                            LogHeader, numTaints, numSubSteps, updatedEntityCount, collidersCount, e);
-                DetailLog("{0},PhysicsStepException,call, nTaints={1}, substeps={2}, updates={3}, colliders={4}",
-                            DetailLogZero, numTaints, numSubSteps, updatedEntityCount, collidersCount);
-                updatedEntityCount = 0;
-                collidersCount = 0;
-            }
+                InSimulationTime = false;
+
+                // Some actors want to know when the simulation step is complete.
+                TriggerPostStepEvent(timeStep);
+
+                // In case there were any parameter updates that happened during the simulation step
+                numTaints += ProcessTaints();
 
-            // Make the physics engine dump useful statistics periodically
-            if (PhysicsMetricDumpFrames != 0 && ((m_simulationStep % PhysicsMetricDumpFrames) == 0))
-                PE.DumpPhysicsStatistics(World);
+                InSimulationTime = false;
+            }
 
             // Get a value for 'now' so all the collision and update routines don't have to get their own.
             SimulationNowTime = Util.EnvironmentTickCount();
@@ -748,9 +758,6 @@ namespace OpenSim.Region.PhysicsModule.BulletS
                 }
             }
 
-            // Some actors want to know when the simulation step is complete.
-            TriggerPostStepEvent(timeStep);
-
             simTime = Util.EnvironmentTickCountSubtract(beforeTime);
             if (PhysicsLogging.Enabled)
             {
@@ -956,6 +963,149 @@ namespace OpenSim.Region.PhysicsModule.BulletS
 
         #endregion // Terrain
 
+        #region Raycast
+
+        public override bool SupportsRayCast()
+        {
+            return BSParam.UseBulletRaycast;
+        }
+
+        public override bool SupportsRaycastWorldFiltered()
+        {
+            return BSParam.UseBulletRaycast;
+        }
+
+
+        /// <summary>
+        /// Queue a raycast against the physics scene.
+        /// The provided callback method will be called when the raycast is complete
+        ///
+        /// Many physics engines don't support collision testing at the same time as
+        /// manipulating the physics scene, so we queue the request up and callback
+        /// a custom method when the raycast is complete.
+        /// This allows physics engines that give an immediate result to callback immediately
+        /// and ones that don't, to callback when it gets a result back.
+        ///      public delegate void RayCallback(List<ContactResult> list);
+        ///
+        /// ODE for example will not allow you to change the scene while collision testing or
+        /// it asserts, 'opteration not valid for locked space'.  This includes adding a ray to the scene.
+        ///
+        /// This is named RayCastWorld to not conflict with modrex's Raycast method.
+        /// </summary>
+        /// <param name="position">Origin of the ray</param>
+        /// <param name="direction">Direction of the ray</param>
+        /// <param name="length">Length of ray in meters</param>
+        /// <param name="retMethod">Method to call when the raycast is complete</param>
+        public override void RaycastWorld(Vector3 position, Vector3 direction, float length, RaycastCallback retMethod)
+        {
+            if (retMethod != null)
+            {
+                if (BSParam.UseBulletRaycast)
+                {
+                    Vector3 posFrom = position;
+                    Vector3 posTo = Vector3.Normalize(direction) * length + position;
+
+                    TaintedObject(DetailLogZero, "BSScene.RaycastWorld1", delegate ()
+                    {
+                        RaycastHit hitInfo = PE.RayTest2(World, posFrom, posTo, 0xffff, 0xffff);
+                        retMethod(true, hitInfo.Point, hitInfo.ID, hitInfo.Fraction, hitInfo.Normal);
+                    });
+                }
+                else
+                {
+                    retMethod(false, Vector3.Zero, 0, 999999999999f, Vector3.Zero);
+                }
+            }
+        }
+
+        public override void RaycastWorld(Vector3 position, Vector3 direction, float length, int count, RayCallback retMethod)
+        {
+            if (retMethod != null)
+            {
+                if (BSParam.UseBulletRaycast)
+                {
+                    List<ContactResult> hitInfo = RaycastWorld(position, direction, length, count);
+                    retMethod(hitInfo);
+                }
+                else
+                {
+                    retMethod(new List<ContactResult>());
+                }
+            }
+        }
+
+        public override List<ContactResult> RaycastWorld(Vector3 position, Vector3 direction, float length, int count)
+        {
+            return (List<ContactResult>)RaycastWorld(position, direction, length, count, RayFilterFlags.All);
+        }
+
+        public override object RaycastWorld(Vector3 position, Vector3 direction, float length, int count, RayFilterFlags filter)
+        {
+            List<ContactResult> ret = new List<ContactResult>();
+            if (BSParam.UseBulletRaycast)
+            {
+                uint collisionFilter = 0;
+                uint collisionMask = 0;
+                if ((filter & RayFilterFlags.land) != 0)
+                {
+                    collisionFilter |= BulletSimData.CollisionTypeMasks[CollisionType.Terrain].group;
+                    collisionMask |= BulletSimData.CollisionTypeMasks[CollisionType.Terrain].mask;
+                }
+                if ((filter & RayFilterFlags.agent) != 0)
+                {
+                    collisionFilter |= BulletSimData.CollisionTypeMasks[CollisionType.Avatar].group;
+                    collisionMask |= BulletSimData.CollisionTypeMasks[CollisionType.Avatar].mask;
+                }
+                if ((filter & RayFilterFlags.nonphysical) != 0)
+                {
+                    collisionFilter |= BulletSimData.CollisionTypeMasks[CollisionType.Static].group;
+                    collisionMask |= BulletSimData.CollisionTypeMasks[CollisionType.Static].mask;
+                }
+                if ((filter & RayFilterFlags.physical) != 0)
+                {
+                    collisionFilter |= BulletSimData.CollisionTypeMasks[CollisionType.Dynamic].group;
+                    collisionMask |= BulletSimData.CollisionTypeMasks[CollisionType.Dynamic].mask;
+                }
+                // if ((filter & RayFilterFlags.phantom) != 0)
+                // {
+                //     collisionFilter |= BulletSimData.CollisionTypeMasks[CollisionType.VolumeDetect].group;
+                //     collisionMask |= BulletSimData.CollisionTypeMasks[CollisionType.VolumeDetect].mask;
+                // }
+                if ((filter & RayFilterFlags.volumedtc) != 0)
+                {
+                    collisionFilter |= BulletSimData.CollisionTypeMasks[CollisionType.VolumeDetect].group;
+                    collisionMask |= BulletSimData.CollisionTypeMasks[CollisionType.VolumeDetect].mask;
+                }
+                DetailLog("{0},RaycastWorld,pos={1},dir={2},len={3},count={4},filter={5},filter={6},mask={7}",
+                        DetailLogZero, position, direction, length, count, filter, collisionFilter, collisionMask);
+                // NOTE: locking ensures the physics engine is not executing.
+                //    The caller might have to wait for the physics engine to finish.
+                lock (PhysicsEngineLock)
+                {
+                    Vector3 posFrom = position;
+                    Vector3 posTo = Vector3.Normalize(direction) * length + position;
+                    DetailLog("{0},RaycastWorld,RayTest2,from={1},to={2}",
+                                DetailLogZero, posFrom, posTo);
+                    RaycastHit hitInfo = PE.RayTest2(World, posFrom, posTo, collisionFilter, collisionMask);
+                    if (hitInfo.hasHit())
+                    {
+                        ContactResult result = new ContactResult();
+                        result.Pos = hitInfo.Point;
+                        result.Normal = hitInfo.Normal;
+                        result.ConsumerID = hitInfo.ID;
+                        result.Depth = hitInfo.Fraction;
+                        ret.Add(result);
+                        DetailLog("{0},RaycastWorld,hit,pos={1},norm={2},depth={3},id={4}",
+                            DetailLogZero, result.Pos, result.Normal, result.Depth, result.ConsumerID);
+                    }
+                }
+            }
+            return ret;
+        }
+
+        #endregion Raycast
+
+
         public override Dictionary<uint, float> GetTopColliders()
         {
             Dictionary<uint, float> topColliders;
@@ -1068,32 +1218,35 @@ namespace OpenSim.Region.PhysicsModule.BulletS
         // Calls to the PhysicsActors can't directly call into the physics engine
         //       because it might be busy. We delay changes to a known time.
         // We rely on C#'s closure to save and restore the context for the delegate.
-        public void TaintedObject(string pOriginator, string pIdent, TaintCallback pCallback)
+        // NOTE: 'inTaintTime' is no longer used. This entry exists so all the calls don't have to be changed.
+        // public void TaintedObject(bool inTaintTime, String pIdent, TaintCallback pCallback)
+        // {
+        //     TaintedObject(BSScene.DetailLogZero, pIdent, pCallback);
+        // }
+        // NOTE: 'inTaintTime' is no longer used. This entry exists so all the calls don't have to be changed.
+        public void TaintedObject(bool inTaintTime, uint pOriginator, String pIdent, TaintCallback pCallback)
         {
-            TaintedObject(false /*inTaintTime*/, pOriginator, pIdent, pCallback);
+            TaintedObject(m_physicsLoggingEnabled ? pOriginator.ToString() : BSScene.DetailLogZero, pIdent, pCallback);
         }
         public void TaintedObject(uint pOriginator, String pIdent, TaintCallback pCallback)
         {
-            TaintedObject(false /*inTaintTime*/, m_physicsLoggingEnabled ? pOriginator.ToString() : BSScene.DetailLogZero, pIdent, pCallback);
-        }
-        public void TaintedObject(bool inTaintTime, String pIdent, TaintCallback pCallback)
-        {
-            TaintedObject(inTaintTime, BSScene.DetailLogZero, pIdent, pCallback);
-        }
-        public void TaintedObject(bool inTaintTime, uint pOriginator, String pIdent, TaintCallback pCallback)
-        {
-            TaintedObject(inTaintTime, m_physicsLoggingEnabled ? pOriginator.ToString() : BSScene.DetailLogZero, pIdent, pCallback);
+            TaintedObject(m_physicsLoggingEnabled ? pOriginator.ToString() : BSScene.DetailLogZero, pIdent, pCallback);
         }
         // Sometimes a potentially tainted operation can be used in and out of taint time.
         // This routine executes the command immediately if in taint-time otherwise it is queued.
-        public void TaintedObject(bool inTaintTime, string pOriginator, string pIdent, TaintCallback pCallback)
+        public void TaintedObject(string pOriginator, string pIdent, TaintCallback pCallback)
         {
             if (!m_initialized) return;
 
-            if (inTaintTime)
+            if (Monitor.TryEnter(PhysicsEngineLock))
+            {
+                // If we can get exclusive access to the physics engine, just do the operation
                 pCallback();
+                Monitor.Exit(PhysicsEngineLock);
+            }
             else
             {
+                // The physics engine is busy, queue the operation
                 lock (_taintLock)
                 {
                     _taintOperations.Add(new TaintCallbackEntry(pOriginator, pIdent, pCallback));
@@ -1120,14 +1273,21 @@ namespace OpenSim.Region.PhysicsModule.BulletS
         // When someone tries to change a property on a BSPrim or BSCharacter, the object queues
         // a callback into itself to do the actual property change. That callback is called
         // here just before the physics engine is called to step the simulation.
-        public void ProcessTaints()
+        // Returns the number of taints processed
+        // NOTE: Called while PhysicsEngineLock is locked
+        public int ProcessTaints()
         {
-            ProcessRegularTaints();
-            ProcessPostTaintTaints();
+            int ret = 0;
+            ret += ProcessRegularTaints();
+            ret += ProcessPostTaintTaints();
+            return ret;
         }
 
-        private void ProcessRegularTaints()
+        // Returns the number of taints processed
+        // NOTE: Called while PhysicsEngineLock is locked
+        private int ProcessRegularTaints()
         {
+            int ret = 0;
             if (m_initialized && _taintOperations.Count > 0)  // save allocating new list if there is nothing to process
             {
                 // swizzle a new list into the list location so we can process what's there
@@ -1144,6 +1304,7 @@ namespace OpenSim.Region.PhysicsModule.BulletS
                     {
                         DetailLog("{0},BSScene.ProcessTaints,doTaint,id={1}", tcbe.originator, tcbe.ident); // DEBUG DEBUG DEBUG
                         tcbe.callback();
+                        ret++;
                     }
                     catch (Exception e)
                     {
@@ -1152,6 +1313,7 @@ namespace OpenSim.Region.PhysicsModule.BulletS
                 }
                 oldList.Clear();
             }
+            return ret;
         }
 
         // Schedule an update to happen after all the regular taints are processed.
@@ -1170,8 +1332,11 @@ namespace OpenSim.Region.PhysicsModule.BulletS
         }
 
         // Taints that happen after the normal taint processing but before the simulation step.
-        private void ProcessPostTaintTaints()
+        // Returns the number of taints processed
+        // NOTE: Called while PhysicsEngineLock is locked
+        private int ProcessPostTaintTaints()
         {
+            int ret = 0;
             if (m_initialized && _postTaintOperations.Count > 0)
             {
                 Dictionary<string, TaintCallbackEntry> oldList;
@@ -1187,6 +1352,7 @@ namespace OpenSim.Region.PhysicsModule.BulletS
                     {
                         DetailLog("{0},BSScene.ProcessPostTaintTaints,doTaint,id={1}", DetailLogZero, kvp.Key); // DEBUG DEBUG DEBUG
                         kvp.Value.callback();
+                        ret++;
                     }
                     catch (Exception e)
                     {
@@ -1195,20 +1361,8 @@ namespace OpenSim.Region.PhysicsModule.BulletS
                 }
                 oldList.Clear();
             }
+            return ret;
         }
-
-        // Only used for debugging. Does not change state of anything so locking is not necessary.
-        public bool AssertInTaintTime(string whereFrom)
-        {
-            if (!InTaintTime)
-            {
-                DetailLog("{0},BSScene.AssertInTaintTime,NOT IN TAINT TIME,Region={1},Where={2}", DetailLogZero, RegionName, whereFrom);
-                m_log.ErrorFormat("{0} NOT IN TAINT TIME!! Region={1}, Where={2}", LogHeader, RegionName, whereFrom);
-                // Util.PrintCallStack(DetailLog);
-            }
-            return InTaintTime;
-        }
-
         #endregion // Taints
 
         #region IPhysicsParameters
diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSShapeCollection.cs b/OpenSim/Region/PhysicsModules/BulletS/BSShapeCollection.cs
index 4ec6f51..86bf23f 100755
--- a/OpenSim/Region/PhysicsModules/BulletS/BSShapeCollection.cs
+++ b/OpenSim/Region/PhysicsModules/BulletS/BSShapeCollection.cs
@@ -75,8 +75,6 @@ public sealed class BSShapeCollection : IDisposable
     // Called at taint-time.
     public bool GetBodyAndShape(bool forceRebuild, BulletWorld sim, BSPhysObject prim, PhysicalDestructionCallback bodyCallback)
     {
-        m_physicsScene.AssertInTaintTime("BSShapeCollection.GetBodyAndShape");
-
         bool ret = false;
 
         // This lock could probably be pushed down lower but building shouldn't take long
@@ -229,6 +227,8 @@ public sealed class BSShapeCollection : IDisposable
             ret = CreateGeomMeshOrHull(prim, shapeCallback);
         }
 
+        m_physicsScene.PE.ResetBroadphasePool(m_physicsScene.World);    // DEBUG DEBUG
+
         return ret;
     }
 
@@ -344,8 +344,6 @@ public sealed class BSShapeCollection : IDisposable
         if (!body.HasPhysicalBody)
             return;
 
-        m_physicsScene.AssertInTaintTime("BSShapeCollection.DereferenceBody");
-
         lock (m_collectionActivityLock)
         {
             if (DDetail) DetailLog("{0},BSShapeCollection.DereferenceBody,DestroyingBody,body={1}", body.ID, body);
diff --git a/OpenSim/Region/PhysicsModules/BulletS/BulletSimData.cs b/OpenSim/Region/PhysicsModules/BulletS/BulletSimData.cs
index 3329395..308769b 100755
--- a/OpenSim/Region/PhysicsModules/BulletS/BulletSimData.cs
+++ b/OpenSim/Region/PhysicsModules/BulletS/BulletSimData.cs
@@ -100,6 +100,7 @@ public class BulletBody
     }
 }
 
+// Handle to btCollisionObject - a shape that can be added to a btRidgidBody
 public class BulletShape
 {
     public BulletShape()
diff --git a/OpenSim/Region/PhysicsModules/BulletS/Tests/Raycast.cs b/OpenSim/Region/PhysicsModules/BulletS/Tests/Raycast.cs
new file mode 100755
index 0000000..bfa95c1
--- /dev/null
+++ b/OpenSim/Region/PhysicsModules/BulletS/Tests/Raycast.cs
@@ -0,0 +1,124 @@
+/*
+ * 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 System.Linq;
+using System.Text;
+
+using NUnit.Framework;
+using log4net;
+
+using OpenSim.Framework;
+using OpenSim.Region.PhysicsModule.BulletS;
+using OpenSim.Region.PhysicsModules.SharedBase;
+using OpenSim.Tests.Common;
+
+using OpenMetaverse;
+
+namespace OpenSim.Region.PhysicsModule.BulletS.Tests
+{
+    [TestFixture]
+    public class BulletSimRaycast : OpenSimTestCase
+    {
+        // Documentation on attributes: http://www.nunit.org/index.php?p=attributes&r=2.6.1
+        // Documentation on assertions: http://www.nunit.org/index.php?p=assertions&r=2.6.1
+
+        BSScene _physicsScene { get; set; }
+        BSPrim _targetSphere { get; set; }
+        Vector3 _targetSpherePosition { get; set; }
+        float _simulationTimeStep = 0.089f;
+
+        uint _targetLocalID = 123;
+
+        [TestFixtureSetUp]
+        public void Init()
+        {
+            Dictionary<string, string> engineParams = new Dictionary<string, string>();
+            engineParams.Add("UseBulletRaycast", "true");
+            _physicsScene = BulletSimTestsUtil.CreateBasicPhysicsEngine(engineParams);
+
+            PrimitiveBaseShape pbs = PrimitiveBaseShape.CreateSphere();
+            Vector3 pos = new Vector3(100.0f, 100.0f, 50f);
+            _targetSpherePosition = pos;
+            Vector3 size = new Vector3(10f, 10f, 10f);
+            pbs.Scale = size;
+            Quaternion rot = Quaternion.Identity;
+            bool isPhys = false;
+
+            _physicsScene.AddPrimShape("TargetSphere", pbs, pos, size, rot, isPhys, _targetLocalID);
+            _targetSphere = (BSPrim)_physicsScene.PhysObjects[_targetLocalID];
+            // The actual prim shape creation happens at taint time
+            _physicsScene.ProcessTaints();
+
+        }
+
+        [TestFixtureTearDown]
+        public void TearDown()
+        {
+            if (_physicsScene != null)
+            {
+                // The Dispose() will also free any physical objects in the scene
+                _physicsScene.Dispose();
+                _physicsScene = null;
+            }
+        }
+
+        // There is a 10x10x10 sphere at <100,100,50>
+        // Shoot rays around the sphere and verify it hits and doesn't hit
+        // TestCase parameters are <x,y,z> of start and <x,y,z> of end and expected result
+        [TestCase(100f, 50f, 50f, 100f, 150f, 50f, true, "Pass through sphere from front")]
+        [TestCase(50f, 100f, 50f, 150f, 100f, 50f, true, "Pass through sphere from side")]
+        [TestCase(50f, 50f, 50f, 150f, 150f, 50f, true, "Pass through sphere diaginally")]
+        [TestCase(100f, 100f, 100f, 100f, 100f, 20f, true, "Pass through sphere from above")]
+        [TestCase(20f, 20f, 50f, 80f, 80f, 50f, false, "Not reach sphere")]
+        [TestCase(50f, 50f, 65f, 150f, 150f, 65f, false, "Passed over sphere")]
+        public void RaycastAroundObject(float fromX, float fromY, float fromZ, float toX, float toY, float toZ, bool expected, string msg) {
+            Vector3 fromPos = new Vector3(fromX, fromY, fromZ);
+            Vector3 toPos = new Vector3(toX, toY, toZ);
+            Vector3 direction = toPos - fromPos;
+            float len = Vector3.Distance(fromPos, toPos);
+
+            List<ContactResult> results = _physicsScene.RaycastWorld(fromPos, direction, len, 1);
+
+            if (expected) {
+                // The  test coordinates should generate a hit
+                Assert.True(results.Count != 0, msg + ": Did not return a hit but expected to.");
+                Assert.True(results.Count == 1, msg + ": Raycast returned not just one hit result.");
+                Assert.True(results[0].ConsumerID == _targetLocalID, msg + ": Raycast returned a collision object other than the target");
+            }
+            else
+            {
+                // The test coordinates should not generate a hit
+                if (results.Count > 0)
+                {
+                    Assert.False(results.Count > 0, msg + ": Returned a hit at " + results[0].Pos.ToString());
+                }
+            }
+        }
+    }
+}
\ No newline at end of file
diff --git a/bin/lib32/BulletSim.dll b/bin/lib32/BulletSim.dll
index 5c3ccd0..c7138a4 100755
--- a/bin/lib32/BulletSim.dll
+++ b/bin/lib32/BulletSim.dll
diff --git a/bin/lib32/libBulletSim.so b/bin/lib32/libBulletSim.so
index 97dd73c..4fb7e3e 100755
--- a/bin/lib32/libBulletSim.so
+++ b/bin/lib32/libBulletSim.so
diff --git a/bin/lib64/BulletSim.dll b/bin/lib64/BulletSim.dll
index eea1020..58fe6b6 100755
--- a/bin/lib64/BulletSim.dll
+++ b/bin/lib64/BulletSim.dll
diff --git a/bin/lib64/libBulletSim.so b/bin/lib64/libBulletSim.so
index 3987835..2bf1159 100755
--- a/bin/lib64/libBulletSim.so
+++ b/bin/lib64/libBulletSim.so