10 files changed, 305 insertions, 50 deletions

diff --git a/OpenSim/Region/ClientStack/Linden/UDP/LLClientView.cs b/OpenSim/Region/ClientStack/Linden/UDP/LLClientView.cs
index 6dd3885..c44c741 100644
--- a/OpenSim/Region/ClientStack/Linden/UDP/LLClientView.cs
+++ b/OpenSim/Region/ClientStack/Linden/UDP/LLClientView.cs
@@ -5589,6 +5589,13 @@ namespace OpenSim.Region.ClientStack.LindenUDP
                 velocity = presence.Velocity;
                 acceleration = Vector3.Zero;
                 rotation = presence.Rotation;
+                // tpvs can only see rotations around Z in some cases
+                if(!presence.Flying && !presence.IsSatOnObject)
+                {
+                    rotation.X = 0f;
+                    rotation.Y = 0f;
+                    rotation.Normalize();
+                }
                 angularVelocity = presence.AngularVelocity;
 
 //                m_log.DebugFormat(
@@ -5718,6 +5725,12 @@ namespace OpenSim.Region.ClientStack.LindenUDP
 
             Vector3 velocity = new Vector3(0, 0, 0);
             Vector3 acceleration = new Vector3(0, 0, 0);
+            // tpvs can only see rotations around Z in some cases
+            if(!data.Flying && !data.IsSatOnObject)
+            {
+                rotation.X = 0f;
+                rotation.Y = 0f;
+            }
             rotation.Normalize();
 
             data.CollisionPlane.ToBytes(objectData, 0);
diff --git a/OpenSim/Region/Framework/Scenes/ScenePresence.cs b/OpenSim/Region/Framework/Scenes/ScenePresence.cs
index f9c7031..9cceb06 100644
--- a/OpenSim/Region/Framework/Scenes/ScenePresence.cs
+++ b/OpenSim/Region/Framework/Scenes/ScenePresence.cs
@@ -290,8 +290,6 @@ namespace OpenSim.Region.Framework.Scenes
         private Vector3 m_lastSize = new Vector3(0.45f,0.6f,1.9f);
         private bool SentInitialData = false;
 
-        private bool m_followCamAuto = false;
-
         private int m_userFlags;
         public int UserFlags
         {
@@ -344,6 +342,11 @@ namespace OpenSim.Region.Framework.Scenes
         private Vector3 m_lastChildAgentUpdatePosition;
 //        private Vector3 m_lastChildAgentUpdateCamPosition;
 
+        private Vector3 m_lastCameraRayCastCam;
+        private Vector3 m_lastCameraRayCastPos;
+
+        private float m_FOV = 1.04f;
+
         private const int LAND_VELOCITYMAG_MAX = 12;
 
         private const float FLY_ROLL_MAX_RADIANS = 1.1f;
@@ -374,9 +377,6 @@ namespace OpenSim.Region.Framework.Scenes
         /// </summary>
         public bool LandAtTarget { get; private set; }
 
-        private int m_movementUpdateCount;
-        private const int NumMovementsBetweenRayCast = 5;
-
         private bool CameraConstraintActive;
 
         private object m_collisionEventLock = new Object();
@@ -1159,6 +1159,7 @@ namespace OpenSim.Region.Framework.Scenes
             ControllingClient.OnForceReleaseControls += HandleForceReleaseControls;
             ControllingClient.OnAutoPilotGo += MoveToTarget;
             ControllingClient.OnUpdateThrottles += RaiseUpdateThrottles;
+//            ControllingClient.OnAgentFOV += HandleAgentFOV;
 
             // ControllingClient.OnChildAgentStatus += new StatusChange(this.ChildStatusChange);
             // ControllingClient.OnStopMovement += new GenericCall2(this.StopMovement);
@@ -1178,6 +1179,7 @@ namespace OpenSim.Region.Framework.Scenes
             ControllingClient.OnForceReleaseControls -= HandleForceReleaseControls;
             ControllingClient.OnAutoPilotGo -= MoveToTarget;
             ControllingClient.OnUpdateThrottles -= RaiseUpdateThrottles;
+//            ControllingClient.OnAgentFOV += HandleAgentFOV;
         }
 
         private void SetDirectionVectors()
@@ -2352,34 +2354,41 @@ namespace OpenSim.Region.Framework.Scenes
 
         private void checkCameraCollision()
         {
-            if(!m_scene.PhysicsScene.SupportsRayCast())
+            if(m_doingCamRayCast || !m_scene.PhysicsScene.SupportsRayCast())
                 return;
 
-            ++m_movementUpdateCount;
-            if (m_movementUpdateCount < 1)
-                m_movementUpdateCount = 1;
+            if(m_mouseLook || ParentID != 0)
+            {
+                if (CameraConstraintActive)
+                {
+                    Vector4 plane = new Vector4(0.9f, 0.0f, 0.361f, -10000f); // not right...
+                    UpdateCameraCollisionPlane(plane);
+                    CameraConstraintActive = false;
+                }
+                return;
+            }
+           
+            Vector3 posAdjusted = AbsolutePosition;
+            posAdjusted.Z += 1.0f; // viewer current camera focus point
 
-            if (m_doingCamRayCast || m_movementUpdateCount % NumMovementsBetweenRayCast != 0)
+            if(posAdjusted.ApproxEquals(m_lastCameraRayCastPos, 0.2f) &&
+                CameraPosition.ApproxEquals(m_lastCameraRayCastCam, 0.2f))
                 return;
 
-            if (m_followCamAuto && !m_mouseLook)
-            {
-                Vector3 posAdjusted = AbsolutePosition;
-//                    posAdjusted.Z += 0.5f * Appearance.AvatarSize.Z - 0.5f;
-                // not good for tiny or huge avatars
-                posAdjusted.Z += 1.0f; // viewer current camera focus point
-                Vector3 tocam = CameraPosition - posAdjusted;
+            m_lastCameraRayCastCam = CameraPosition;
+            m_lastCameraRayCastPos = posAdjusted;
 
-                float distTocamlen = tocam.LengthSquared();
-                if (distTocamlen > 0.01f && distTocamlen < 400)
-                {
-                    distTocamlen = (float)Math.Sqrt(distTocamlen);
-                    tocam *= (1.0f / distTocamlen);
+            Vector3 tocam = CameraPosition - posAdjusted;
 
-                    m_doingCamRayCast = true;
-                    m_scene.PhysicsScene.RaycastWorld(posAdjusted, tocam, distTocamlen + 1.0f, RayCastCameraCallback);
-                    return;
-                }
+            float distTocamlen = tocam.LengthSquared();
+            if (distTocamlen > 0.01f && distTocamlen < 400)
+            {
+                distTocamlen = (float)Math.Sqrt(distTocamlen);
+                tocam *= (1.0f / distTocamlen);
+
+                m_doingCamRayCast = true;
+                m_scene.PhysicsScene.RaycastWorld(posAdjusted, tocam, distTocamlen + 1.0f, RayCastCameraCallback);
+                return;
             }
 
             if (CameraConstraintActive)
@@ -2401,9 +2410,6 @@ namespace OpenSim.Region.Framework.Scenes
 
         public void RayCastCameraCallback(bool hitYN, Vector3 collisionPoint, uint localid, float distance, Vector3 pNormal)
         {
-//            const float POSITION_TOLERANCE = 0.02f;
-//            const float ROTATION_TOLERANCE = 0.02f;
-
             if (hitYN && localid != LocalId)
             {
                 if (localid != 0)
@@ -2441,8 +2447,6 @@ namespace OpenSim.Region.Framework.Scenes
                     UpdateCameraCollisionPlane(plane);
                 }
             }
-//            else if (!m_pos.ApproxEquals(m_lastPosition, POSITION_TOLERANCE) ||
-//                     !Rotation.ApproxEquals(m_lastRotation, ROTATION_TOLERANCE))
             else if(CameraConstraintActive)
             {
                 Vector4 plane = new Vector4(0.9f, 0.0f, 0.361f, -9000f); // not right...
@@ -2538,7 +2542,7 @@ namespace OpenSim.Region.Framework.Scenes
 
             // Raycast from the avatar's head to the camera to see if there's anything blocking the view
             // this exclude checks may not be complete
-            if(agentData.NeedsCameraCollision  && ParentID == 0) // condition parentID may be wrong
+            if(agentData.NeedsCameraCollision) // condition parentID may be wrong
                 checkCameraCollision();
 
             uint flagsForScripts = (uint)flags;
@@ -2807,6 +2811,10 @@ namespace OpenSim.Region.Framework.Scenes
 //            m_scene.EventManager.TriggerOnClientMovement(this);
         }
 
+        private void HandleAgentFOV(IClientAPI remoteClient, float _fov)
+        {
+            m_FOV = _fov;
+        }
 
         /// <summary>
         /// This is the event handler for client cameras. If a client is moving, or moving the camera, this event is triggering.
@@ -2843,24 +2851,6 @@ namespace OpenSim.Region.Framework.Scenes
             Quaternion camRot = Util.Axes2Rot(CameraAtAxis, CameraLeftAxis, CameraUpAxis);
             CameraRotation = camRot;
 
-            // Check if Client has camera in 'follow cam' or 'build' mode.
-//            Vector3 camdif = (Vector3.One * Rotation - Vector3.One * CameraRotation);
-            m_followCamAuto = false;
-            if(!m_mouseLook)
-            {
-                if((CameraUpAxis.Z > 0.959f && CameraUpAxis.Z < 0.99f))
-                {
-                    Vector3 camdif = new Vector3(1f, 0f, 0f) * Rotation;
-                    float ftmp = camdif.X - CameraAtAxis.X;
-                    if(Math.Abs(ftmp) < 0.1f)
-                    {
-                        ftmp = camdif.Y - CameraAtAxis.Y;
-                        if(Math.Abs(ftmp) < 0.1f)
-                            m_followCamAuto = true;
-                    }
-                }
-            }
-
             if(agentData.NeedsCameraCollision)
                 checkCameraCollision();
 
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 bcc3b4a..52aea87 100644
--- a/OpenSim/Region/PhysicsModules/BulletS/BSScene.cs
+++ b/OpenSim/Region/PhysicsModules/BulletS/BSScene.cs
@@ -969,6 +969,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 8d8fc95..cda33e4 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