17 files changed, 475 insertions, 2 deletions

diff --git a/OpenSim/Client/MXP/ClientStack/MXPClientView.cs b/OpenSim/Client/MXP/ClientStack/MXPClientView.cs
index d8980c1..a7bcc07 100644
--- a/OpenSim/Client/MXP/ClientStack/MXPClientView.cs
+++ b/OpenSim/Client/MXP/ClientStack/MXPClientView.cs
@@ -1240,6 +1240,11 @@ namespace OpenSim.Client.MXP.ClientStack
             // Need to translate to MXP somehow
         }
 
+        public void SendCameraConstraint(Vector4 ConstraintPlane)
+        {
+
+        }
+
         public void SendLandParcelOverlay(byte[] data, int sequence_id)
         {
             // Need to translate to MXP somehow
diff --git a/OpenSim/Client/VWoHTTP/ClientStack/VWHClientView.cs b/OpenSim/Client/VWoHTTP/ClientStack/VWHClientView.cs
index 4570395..324b5af 100644
--- a/OpenSim/Client/VWoHTTP/ClientStack/VWHClientView.cs
+++ b/OpenSim/Client/VWoHTTP/ClientStack/VWHClientView.cs
@@ -780,6 +780,11 @@ namespace OpenSim.Client.VWoHTTP.ClientStack
             throw new System.NotImplementedException();
         }
 
+        public void SendCameraConstraint(Vector4 ConstraintPlane)
+        {
+
+        }
+
         public void SendLandObjectOwners(LandData land, List<UUID> groups, Dictionary<UUID, int> ownersAndCount)
         {
             throw new System.NotImplementedException();
diff --git a/OpenSim/Framework/IClientAPI.cs b/OpenSim/Framework/IClientAPI.cs
index fea0cea..52d9716 100644
--- a/OpenSim/Framework/IClientAPI.cs
+++ b/OpenSim/Framework/IClientAPI.cs
@@ -1007,6 +1007,7 @@ namespace OpenSim.Framework
 
         void SendLandAccessListData(List<UUID> avatars, uint accessFlag, int localLandID);
         void SendForceClientSelectObjects(List<uint> objectIDs);
+        void SendCameraConstraint(Vector4 ConstraintPlane);
         void SendLandObjectOwners(LandData land, List<UUID> groups, Dictionary<UUID, int> ownersAndCount);
         void SendLandParcelOverlay(byte[] data, int sequence_id);
 
diff --git a/OpenSim/Region/ClientStack/LindenUDP/LLClientView.cs b/OpenSim/Region/ClientStack/LindenUDP/LLClientView.cs
index a13f6d4..3a93007 100644
--- a/OpenSim/Region/ClientStack/LindenUDP/LLClientView.cs
+++ b/OpenSim/Region/ClientStack/LindenUDP/LLClientView.cs
@@ -3779,8 +3779,19 @@ namespace OpenSim.Region.ClientStack.LindenUDP
             }
         }
 
+        public void SendCameraConstraint(Vector4 ConstraintPlane)
+        {
+            CameraConstraintPacket cpack = (CameraConstraintPacket)PacketPool.Instance.GetPacket(PacketType.CameraConstraint);
+            cpack.CameraCollidePlane = new CameraConstraintPacket.CameraCollidePlaneBlock();
+            cpack.CameraCollidePlane.Plane = ConstraintPlane;
+            m_log.DebugFormat("[CLIENTVIEW]: Constraint {0}", ConstraintPlane);
+            OutPacket(cpack, ThrottleOutPacketType.Task);
+        }
+
         public void SendLandObjectOwners(LandData land, List<UUID> groups, Dictionary<UUID, int> ownersAndCount)
         {
+            
+
             int notifyCount = ownersAndCount.Count;
             ParcelObjectOwnersReplyPacket pack = (ParcelObjectOwnersReplyPacket)PacketPool.Instance.GetPacket(PacketType.ParcelObjectOwnersReply);
 
diff --git a/OpenSim/Region/Examples/SimpleModule/MyNpcCharacter.cs b/OpenSim/Region/Examples/SimpleModule/MyNpcCharacter.cs
index ed38046..62779e7 100644
--- a/OpenSim/Region/Examples/SimpleModule/MyNpcCharacter.cs
+++ b/OpenSim/Region/Examples/SimpleModule/MyNpcCharacter.cs
@@ -900,6 +900,11 @@ namespace OpenSim.Region.Examples.SimpleModule
         public void SendLandObjectOwners(LandData land, List<UUID> groups, Dictionary<UUID, int> ownersAndCount)
         {
         }
+
+        public void SendCameraConstraint(Vector4 ConstraintPlane)
+        {
+
+        }
         
         public void SendLandParcelOverlay(byte[] data, int sequence_id)
         {
diff --git a/OpenSim/Region/Framework/Scenes/ScenePresence.cs b/OpenSim/Region/Framework/Scenes/ScenePresence.cs
index 87207a0..2a8436b 100644
--- a/OpenSim/Region/Framework/Scenes/ScenePresence.cs
+++ b/OpenSim/Region/Framework/Scenes/ScenePresence.cs
@@ -186,6 +186,14 @@ namespace OpenSim.Region.Framework.Scenes
         //PauPaw:Proper PID Controler for autopilot************
         private bool m_moveToPositionInProgress;
         private Vector3 m_moveToPositionTarget = Vector3.Zero;
+
+        private bool m_followCamAuto = false;
+
+        private int m_movementUpdateCount = 0;
+
+        private const int NumMovementsBetweenRayCast = 5;
+
+        private bool CameraConstraintActive = false;
         //private int m_moveToPositionStateStatus = 0;
         //*****************************************************
 
@@ -1073,6 +1081,44 @@ namespace OpenSim.Region.Framework.Scenes
             }
         }
 
+
+        /// <summary>
+        /// Callback for the Camera view block check.  Gets called with the results of the camera view block test
+        /// hitYN is true when there's something in the way.
+        /// </summary>
+        /// <param name="hitYN"></param>
+        /// <param name="collisionPoint"></param>
+        /// <param name="localid"></param>
+        /// <param name="distance"></param>
+        public void RayCastCameraCallback(bool hitYN, Vector3 collisionPoint, uint localid, float distance)
+        {
+            if (m_followCamAuto)
+            {
+
+                if (hitYN)
+                {
+                    CameraConstraintActive = true;
+                    //m_log.DebugFormat("[RAYCASTRESULT]: {0}, {1}, {2}, {3}", hitYN, collisionPoint, localid, distance);
+                    
+                    Vector3 normal = Vector3.Normalize(new Vector3(0,0,collisionPoint.Z) - collisionPoint);
+                    ControllingClient.SendCameraConstraint(new Vector4(normal.X, normal.Y, normal.Z, -1 * Vector3.Distance(new Vector3(0,0,collisionPoint.Z),collisionPoint)));
+                }
+                else 
+                {
+                    if (((Util.GetDistanceTo(lastPhysPos, AbsolutePosition) > 0.02)
+                         || (Util.GetDistanceTo(m_lastVelocity, m_velocity) > 0.02)
+                         || lastPhysRot != m_bodyRot))
+                    {
+                        if (CameraConstraintActive)
+                        {
+                            ControllingClient.SendCameraConstraint(new Vector4(0, 0.5f, 0.9f, -3000f));
+                            CameraConstraintActive = false;
+                        }
+                    }
+                }
+            } 
+        }
+
         /// <summary>
         /// This is the event handler for client movement.   If a client is moving, this event is triggering.
         /// </summary>
@@ -1098,11 +1144,18 @@ namespace OpenSim.Region.Framework.Scenes
             //    return;
             //}
 
+            
+            m_movementUpdateCount++;
+            if (m_movementUpdateCount >= int.MaxValue)
+                m_movementUpdateCount = 1;
+
+
             // Must check for standing up even when PhysicsActor is null,
             // since sitting currently removes avatar from physical scene
             //m_log.Debug("agentPos:" + AbsolutePosition.ToString());
 
             // This is irritating.  Really.
+
             if (!AbsolutePosition.IsFinite())
             {
                 RemoveFromPhysicalScene();
@@ -1157,8 +1210,26 @@ namespace OpenSim.Region.Framework.Scenes
             {
                 StandUp();
             }
-
             
+           
+
+            // Check if Client has camera in 'follow cam' or 'build' mode.
+            Vector3 camdif = (Vector3.One * m_bodyRot - Vector3.One * CameraRotation);
+
+            m_followCamAuto = ((m_CameraUpAxis.Z > 0.959f && m_CameraUpAxis.Z < 0.98f) 
+               && (Math.Abs(camdif.X) < 0.4f && Math.Abs(camdif.Y) < 0.4f)) ? true : false;
+
+
+            // Raycast from the avatar's head to the camera to see if there's anything blocking the view
+            if ((m_movementUpdateCount % NumMovementsBetweenRayCast) == 0 && m_scene.PhysicsScene.SupportsRayCast())
+            {
+
+                if (m_followCamAuto)
+                {
+                    Vector3 headadjustment = new Vector3(0, 0, 0.3f);
+                    m_scene.PhysicsScene.RaycastWorld(m_pos, Vector3.Normalize(m_CameraCenter - (m_pos + headadjustment)), Vector3.Distance(m_CameraCenter, (m_pos + headadjustment)) + 0.3f, RayCastCameraCallback);
+                }
+            }
 
             m_mouseLook = (flags & (uint) AgentManager.ControlFlags.AGENT_CONTROL_MOUSELOOK) != 0;
 
diff --git a/OpenSim/Region/OptionalModules/Agent/InternetRelayClientView/Server/IRCClientView.cs b/OpenSim/Region/OptionalModules/Agent/InternetRelayClientView/Server/IRCClientView.cs
index 776e972..44bd716 100644
--- a/OpenSim/Region/OptionalModules/Agent/InternetRelayClientView/Server/IRCClientView.cs
+++ b/OpenSim/Region/OptionalModules/Agent/InternetRelayClientView/Server/IRCClientView.cs
@@ -1233,6 +1233,11 @@ namespace OpenSim.Region.OptionalModules.Agent.InternetRelayClientView.Server
             
         }
 
+        public void SendCameraConstraint(Vector4 ConstraintPlane)
+        {
+
+        }
+
         public void SendLandObjectOwners(LandData land, List<UUID> groups, Dictionary<UUID, int> ownersAndCount)
         {
             
diff --git a/OpenSim/Region/OptionalModules/World/NPC/NPCAvatar.cs b/OpenSim/Region/OptionalModules/World/NPC/NPCAvatar.cs
index a1ed6ee..2102db9 100644
--- a/OpenSim/Region/OptionalModules/World/NPC/NPCAvatar.cs
+++ b/OpenSim/Region/OptionalModules/World/NPC/NPCAvatar.cs
@@ -904,6 +904,9 @@ namespace OpenSim.Region.OptionalModules.World.NPC
         public void SendForceClientSelectObjects(List<uint> objectIDs)
         {
         }
+        public void SendCameraConstraint(Vector4 ConstraintPlane)
+        {
+        }
         public void SendLandObjectOwners(LandData land, List<UUID> groups, Dictionary<UUID, int> ownersAndCount)
         {
         }
diff --git a/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsPlugin.cs b/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsPlugin.cs
index 83d7a1d..92d5771 100644
--- a/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsPlugin.cs
+++ b/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsPlugin.cs
@@ -210,6 +210,7 @@ namespace OpenSim.Region.Physics.BasicPhysicsPlugin
             Dictionary<uint, float> returncolliders = new Dictionary<uint, float>();
             return returncolliders;
         }
+        
     }
 
     public class BasicActor : PhysicsActor
diff --git a/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETScene.cs b/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETScene.cs
index 58c2a15..e0f856a 100644
--- a/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETScene.cs
+++ b/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETScene.cs
@@ -700,6 +700,8 @@ namespace OpenSim.Region.Physics.BulletDotNETPlugin
                 m_world.SetCollisionAddedCallback(m_CollisionInterface);
             }
         }
+        
+
 
     }
 }
diff --git a/OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs b/OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs
index 7e5e812..abed8df 100644
--- a/OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs
+++ b/OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs
@@ -817,9 +817,14 @@ namespace OpenSim.Region.Physics.BulletXPlugin
                 GC.Collect();
                 BulletXMessage("Terrain erased!", false);
             }
+
+            
+
             //this._heightmap = null;
         }
 
+        
+
         internal void AddForgottenRigidBody(RigidBody forgottenRigidBody)
         {
             _forgottenRigidBodies.Add(forgottenRigidBody);
diff --git a/OpenSim/Region/Physics/Manager/PhysicsScene.cs b/OpenSim/Region/Physics/Manager/PhysicsScene.cs
index 410707b..95699fb 100644
--- a/OpenSim/Region/Physics/Manager/PhysicsScene.cs
+++ b/OpenSim/Region/Physics/Manager/PhysicsScene.cs
@@ -36,6 +36,8 @@ namespace OpenSim.Region.Physics.Manager
 {
     public delegate void physicsCrash();
 
+    public delegate void RaycastCallback(bool hitYN, Vector3 collisionPoint, uint localid, float distance);
+
     public abstract class PhysicsScene
     {
         private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
@@ -152,6 +154,39 @@ namespace OpenSim.Region.Physics.Manager
 
         public abstract bool IsThreaded { get; }
 
+        /// <summary>
+        /// True if the physics plugin supports raycasting against the physics scene
+        /// </summary>
+        public virtual bool SupportsRayCast()
+        {
+            return false;
+        }
+
+        /// <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.
+        /// 
+        /// 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 virtual void RaycastWorld( Vector3 position, Vector3 direction, float length, RaycastCallback retMethod)
+        {
+            if (retMethod != null)
+                retMethod(false, Vector3.Zero, 0, 999999999999f);
+        }
+
         private class NullPhysicsScene : PhysicsScene
         {
             private static int m_workIndicator;
@@ -240,6 +275,7 @@ namespace OpenSim.Region.Physics.Manager
                 Dictionary<uint, float> returncolliders = new Dictionary<uint, float>();
                 return returncolliders;
             }
+            
         }
     }
     public delegate void JointMoved(PhysicsJoint joint);
diff --git a/OpenSim/Region/Physics/OdePlugin/ODERayCastRequestManager.cs b/OpenSim/Region/Physics/OdePlugin/ODERayCastRequestManager.cs
new file mode 100644
index 0000000..3a4bb02
--- /dev/null
+++ b/OpenSim/Region/Physics/OdePlugin/ODERayCastRequestManager.cs
@@ -0,0 +1,295 @@
+/*
+ * 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.Reflection;
+using System.Runtime.InteropServices;
+using System.Text;
+using OpenMetaverse;
+using OpenSim.Region.Physics.Manager;
+using Ode.NET;
+using log4net;
+
+namespace OpenSim.Region.Physics.OdePlugin
+{
+    /// <summary>
+    /// Processes raycast requests as ODE is in a state to be able to do them.
+    /// This ensures that it's thread safe and there will be no conflicts.
+    /// Requests get returned by a different thread then they were requested by.
+    /// </summary>
+    public class ODERayCastRequestManager
+    {
+        /// <summary>
+        /// Pending Raycast Requests
+        /// </summary>
+        protected List<ODERayCastRequest> m_PendingRequests = new List<ODERayCastRequest>();
+
+        /// <summary>
+        /// Scene that created this object.
+        /// </summary>
+        private OdeScene m_scene;
+
+        /// <summary>
+        /// ODE contact array to be filled by the collision testing
+        /// </summary>
+        d.ContactGeom[] contacts = new d.ContactGeom[5];
+
+        /// <summary>
+        /// ODE near callback delegate
+        /// </summary>
+        private d.NearCallback nearCallback;
+        private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
+        private List<ContactResult> m_contactResults = new List<ContactResult>();
+
+
+        public ODERayCastRequestManager( OdeScene pScene)
+        {
+            m_scene = pScene;
+            nearCallback = near;
+            
+        }
+
+        /// <summary>
+        /// Queues a raycast
+        /// </summary>
+        /// <param name="position">Origin of Ray</param>
+        /// <param name="direction">Ray normal</param>
+        /// <param name="length">Ray length</param>
+        /// <param name="retMethod">Return method to send the results</param>
+        public void QueueRequest(Vector3 position, Vector3 direction, float length, RaycastCallback retMethod)
+        {
+            lock (m_PendingRequests)
+            {
+                ODERayCastRequest req = new ODERayCastRequest();
+                req.callbackMethod = retMethod;
+                req.length = length;
+                req.Normal = direction;
+                req.Origin = position;
+
+                m_PendingRequests.Add(req);
+            }
+        }
+
+        /// <summary>
+        /// Process all queued raycast requests
+        /// </summary>
+        /// <returns>Time in MS the raycasts took to process.</returns>
+        public int ProcessQueuedRequests()
+        {
+            int time = System.Environment.TickCount;
+            lock (m_PendingRequests)
+            {
+                if (m_PendingRequests.Count > 0)
+                {
+                    foreach (ODERayCastRequest req in m_PendingRequests)
+                    {
+                        if (req.callbackMethod != null) // quick optimization here, don't raycast 
+                            RayCast(req);               // if there isn't anyone to send results to
+                            
+                    }
+
+                    m_PendingRequests.Clear();
+                }
+            }
+
+            lock (m_contactResults)
+                m_contactResults.Clear();
+
+            return System.Environment.TickCount - time;
+        }
+
+        /// <summary>
+        /// Method that actually initiates the raycast
+        /// </summary>
+        /// <param name="req"></param>
+        private void RayCast(ODERayCastRequest req)
+        {
+            // Create the ray
+            IntPtr ray = d.CreateRay(m_scene.space, req.length);
+            d.GeomRaySet(ray, req.Origin.X, req.Origin.Y, req.Origin.Z, req.Normal.X, req.Normal.Y, req.Normal.Z);
+
+            // Collide test
+            d.SpaceCollide2(m_scene.space, ray, IntPtr.Zero, nearCallback);
+
+            // Remove Ray
+            d.GeomDestroy(ray);
+
+
+            // Define default results
+            bool hitYN = false;
+            uint hitConsumerID = 0;
+            float distance = 999999999999f;
+            Vector3 closestcontact = new Vector3(99999f, 99999f, 99999f);
+
+            // Find closest contact and object.
+            lock (m_contactResults)
+            {
+                foreach(ContactResult cResult in m_contactResults)
+                {
+                    if (Vector3.Distance(req.Origin, cResult.Pos) < Vector3.Distance(req.Origin, closestcontact))
+                    {
+                        closestcontact = cResult.Pos;
+                        hitConsumerID = cResult.ConsumerID;
+                        distance = cResult.Depth;
+                        hitYN = true;
+                    }
+                }
+
+                m_contactResults.Clear();
+            }
+
+            // Return results
+            if (req.callbackMethod != null)
+                req.callbackMethod(hitYN, closestcontact, hitConsumerID, distance);
+        }
+
+        // This is the standard Near.   Uses space AABBs to speed up detection.
+        private void near(IntPtr space, IntPtr g1, IntPtr g2)
+        {
+            // Raytest against AABBs of spaces first, then dig into the spaces it hits for actual geoms.
+            if (d.GeomIsSpace(g1) || d.GeomIsSpace(g2))
+            {
+                if (g1 == IntPtr.Zero || g2 == IntPtr.Zero)
+                    return;
+                
+                // Separating static prim geometry spaces.
+                // We'll be calling near recursivly if one
+                // of them is a space to find all of the
+                // contact points in the space
+                try
+                {
+                    d.SpaceCollide2(g1, g2, IntPtr.Zero, nearCallback);
+                }
+                catch (AccessViolationException)
+                {
+                    m_log.Warn("[PHYSICS]: Unable to collide test a space");
+                    return;
+                }
+                //Colliding a space or a geom with a space or a geom. so drill down
+
+                //Collide all geoms in each space..
+                //if (d.GeomIsSpace(g1)) d.SpaceCollide(g1, IntPtr.Zero, nearCallback);
+                //if (d.GeomIsSpace(g2)) d.SpaceCollide(g2, IntPtr.Zero, nearCallback);
+                return;
+            }
+
+            if (g1 == IntPtr.Zero || g2 == IntPtr.Zero)
+                return;
+
+            int count = 0;
+            try
+            {
+                
+                if (g1 == g2)
+                    return; // Can't collide with yourself
+
+                lock (contacts)
+                {
+                    count = d.Collide(g1, g2, contacts.GetLength(0), contacts, d.ContactGeom.SizeOf);
+                }
+            }
+            catch (SEHException)
+            {
+                m_log.Error("[PHYSICS]: The Operating system shut down ODE because of corrupt memory.  This could be a result of really irregular terrain.  If this repeats continuously, restart using Basic Physics and terrain fill your terrain.  Restarting the sim.");
+            }
+            catch (Exception e)
+            {
+                m_log.WarnFormat("[PHYSICS]: Unable to collide test an object: {0}", e.Message);
+                return;
+            }
+
+            PhysicsActor p1 = null;
+            PhysicsActor p2 = null;
+
+            if (g1 != IntPtr.Zero)
+                m_scene.actor_name_map.TryGetValue(g1, out p1);
+
+            if (g2 != IntPtr.Zero)
+                m_scene.actor_name_map.TryGetValue(g1, out p2);
+
+            // Loop over contacts, build results.
+            for (int i = 0; i < count; i++)
+            {
+                if (p1 != null) { 
+                    if (p1 is OdePrim)
+                    {
+                        ContactResult collisionresult = new ContactResult();
+                    
+                        collisionresult.ConsumerID = ((OdePrim)p1).m_localID;
+                        collisionresult.Pos = new Vector3(contacts[i].pos.X, contacts[i].pos.Y, contacts[i].pos.Z);
+                        collisionresult.Depth = contacts[i].depth;
+                        
+                        lock (m_contactResults)
+                            m_contactResults.Add(collisionresult);
+                    }
+                }
+
+                if (p2 != null)
+                {
+                    if (p2 is OdePrim)
+                    {
+                        ContactResult collisionresult = new ContactResult();
+
+                        collisionresult.ConsumerID = ((OdePrim)p2).m_localID;
+                        collisionresult.Pos = new Vector3(contacts[i].pos.X, contacts[i].pos.Y, contacts[i].pos.Z);
+                        collisionresult.Depth = contacts[i].depth;
+
+                        lock (m_contactResults)
+                            m_contactResults.Add(collisionresult);
+                    }
+                }
+
+                
+            }
+
+        }
+
+        /// <summary>
+        /// Dereference the creator scene so that it can be garbage collected if needed.
+        /// </summary>
+        internal void Dispose()
+        {
+            m_scene = null;
+        }
+    }
+
+    public struct ODERayCastRequest
+    {
+        public Vector3 Origin;
+        public Vector3 Normal;
+        public float length;
+        public RaycastCallback callbackMethod;
+    }
+
+    public struct ContactResult
+    {
+        public Vector3 Pos;
+        public float Depth;
+        public uint ConsumerID;
+    }
+}
diff --git a/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs
index 889afb6..87357a3 100644
--- a/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs
+++ b/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs
@@ -306,6 +306,8 @@ namespace OpenSim.Region.Physics.OdePlugin
 
         private volatile int m_global_contactcount = 0;
 
+        private ODERayCastRequestManager m_rayCastManager;
+
         /// <summary>
         /// Initiailizes the scene
         /// Sets many properties that ODE requires to be stable
@@ -321,7 +323,7 @@ namespace OpenSim.Region.Physics.OdePlugin
             nearCallback = near;
             triCallback = TriCallback;
             triArrayCallback = TriArrayCallback;
-
+            m_rayCastManager = new ODERayCastRequestManager(this);
             lock (OdeLock)
             {
                 // Create the world and the first space
@@ -2833,6 +2835,8 @@ namespace OpenSim.Region.Physics.OdePlugin
                                 //if ((framecount % m_randomizeWater) == 0)
                                    // randomizeWater(waterlevel);
 
+                                int RayCastTimeMS = m_rayCastManager.ProcessQueuedRequests();
+
                                 collision_optimized(timeStep);
 
                                 lock (_collisionEventPrim)
@@ -3377,6 +3381,9 @@ namespace OpenSim.Region.Physics.OdePlugin
 
         public override void Dispose()
         {
+            m_rayCastManager.Dispose();
+            m_rayCastManager = null;
+
             lock (OdeLock)
             {
                 lock (_prims)
@@ -3417,6 +3424,20 @@ namespace OpenSim.Region.Physics.OdePlugin
             }
             return returncolliders;
         }
+
+        public override bool SupportsRayCast()
+        {
+            return true;
+        }
+
+        public override void RaycastWorld(Vector3 position, Vector3 direction, float length, RaycastCallback retMethod)
+        {
+            if (retMethod != null)
+            {
+                m_rayCastManager.QueueRequest(position, direction, length, retMethod);
+            }
+        }
+
 #if USE_DRAWSTUFF
         // Keyboard callback
         public void command(int cmd)
diff --git a/OpenSim/Region/Physics/POSPlugin/POSScene.cs b/OpenSim/Region/Physics/POSPlugin/POSScene.cs
index 3128477..029dcce 100644
--- a/OpenSim/Region/Physics/POSPlugin/POSScene.cs
+++ b/OpenSim/Region/Physics/POSPlugin/POSScene.cs
@@ -272,5 +272,7 @@ namespace OpenSim.Region.Physics.POSPlugin
             Dictionary<uint, float> returncolliders = new Dictionary<uint, float>();
             return returncolliders;
         }
+
+        
     }
 }
diff --git a/OpenSim/Region/Physics/PhysXPlugin/PhysXPlugin.cs b/OpenSim/Region/Physics/PhysXPlugin/PhysXPlugin.cs
index 3082455..e7d989c 100644
--- a/OpenSim/Region/Physics/PhysXPlugin/PhysXPlugin.cs
+++ b/OpenSim/Region/Physics/PhysXPlugin/PhysXPlugin.cs
@@ -214,6 +214,7 @@ namespace OpenSim.Region.Physics.PhysXPlugin
             Dictionary<uint, float> returncolliders = new Dictionary<uint, float>();
             return returncolliders;
         }
+        
     }
 
     public class PhysXCharacter : PhysicsActor
diff --git a/OpenSim/Tests/Common/Mock/TestClient.cs b/OpenSim/Tests/Common/Mock/TestClient.cs
index 21541e1..4b59b50 100644
--- a/OpenSim/Tests/Common/Mock/TestClient.cs
+++ b/OpenSim/Tests/Common/Mock/TestClient.cs
@@ -942,6 +942,10 @@ namespace OpenSim.Tests.Common.Mock
         {
         }
 
+        public void SendCameraConstraint(Vector4 ConstraintPlane)
+        {
+        }
+
         public void SendLandObjectOwners(LandData land, List<UUID> groups, Dictionary<UUID, int> ownersAndCount)
         {
         }