8 files changed, 252 insertions, 0 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..afb0ba2 100644
--- a/OpenSim/Region/PhysicsModules/BulletS/BSApiTemplate.cs
+++ b/OpenSim/Region/PhysicsModules/BulletS/BSApiTemplate.cs
@@ -128,6 +128,7 @@ public struct RaycastHit
     public UInt32 ID;
     public float Fraction;
     public Vector3 Normal;
+    public Vector3 Point;
 }
 [StructLayout(LayoutKind.Sequential)]
 public struct CollisionDesc
@@ -742,6 +743,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/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..0792f5d 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 ),
 
diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSScene.cs b/OpenSim/Region/PhysicsModules/BulletS/BSScene.cs
index 7ff0a07..f1ff3a9 100644
--- a/OpenSim/Region/PhysicsModules/BulletS/BSScene.cs
+++ b/OpenSim/Region/PhysicsModules/BulletS/BSScene.cs
@@ -956,6 +956,98 @@ 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)
+        {
+            List<ContactResult> ret = new List<ContactResult>();
+            if (BSParam.UseBulletRaycast)
+            {
+            }
+            return ret;
+        }
+
+        public override object RaycastWorld(Vector3 position, Vector3 direction, float length, int Count, RayFilterFlags filter)
+        {
+            object ret = null;
+            if (BSParam.UseBulletRaycast)
+            {
+            }
+            return ret;
+        }
+
+        #endregion Raycast
+
+
         public override Dictionary<uint, float> GetTopColliders()
         {
             Dictionary<uint, float> topColliders;
diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSShapeCollection.cs b/OpenSim/Region/PhysicsModules/BulletS/BSShapeCollection.cs
index 4ec6f51..cd72c98 100755
--- a/OpenSim/Region/PhysicsModules/BulletS/BSShapeCollection.cs
+++ b/OpenSim/Region/PhysicsModules/BulletS/BSShapeCollection.cs
@@ -229,6 +229,8 @@ public sealed class BSShapeCollection : IDisposable
             ret = CreateGeomMeshOrHull(prim, shapeCallback);
         }
 
+        m_physicsScene.PE.ResetBroadphasePool(m_physicsScene.World);    // DEBUG DEBUG
+
         return ret;
     }
 
diff --git a/OpenSim/Region/PhysicsModules/BulletS/Tests/Raycast.cs b/OpenSim/Region/PhysicsModules/BulletS/Tests/Raycast.cs
new file mode 100755
index 0000000..046df56
--- /dev/null
+++ b/OpenSim/Region/PhysicsModules/BulletS/Tests/Raycast.cs
@@ -0,0 +1,115 @@
+/*
+ * 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;
+
+        [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);
+            pos.Z = PhysicsScene.TerrainManager.GetTerrainHeightAtXYZ(pos) + 2f;
+            TargetSpherePosition = pos;
+            Vector3 size = new Vector3(10f, 10f, 10f);
+            pbs.Scale = size;
+            Quaternion rot = Quaternion.Identity;
+            bool isPhys = false;
+            uint localID = 123;
+
+            PhysicsScene.AddPrimShape("TargetSphere", pbs, pos, size, rot, isPhys, localID);
+            TargetSphere = (BSPrim)PhysicsScene.PhysObjects[localID];
+            // 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(20f, 20f, 50f, 50f, 50f, 50f, true)]      // in front to sphere
+        [TestCase(20f, 20f, 100f, 50f, 50f, 50f, true)]     // from above to sphere
+        [TestCase(50f, 50f, 50f, 150f, 150f, 50f, true)]    // through sphere
+        [TestCase(50f, 50f, 65f, 150f, 150f, 65f, false)]   // pass over sphere
+        public void RaycastAroundObject(float fromX, float fromY, float fromZ, float toX, float toY, float toZ, bool expected) {
+            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) {
+                Assert.True(results.Count > 0);
+            }
+            else
+            {
+                Assert.False(results.Count > 0);
+            }
+        }
+    }
+}
\ No newline at end of file