OpenSim-Test/OpenSim/Region/Physics/OdePlugin/ODERayCastRequestManager.cs

/*
 * 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>
        /// Pending ray requests
        /// </summary>
        protected List<ODERayRequest> m_PendingRayRequests = new List<ODERayRequest>();

        /// <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>
        /// 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="count"></param>
        /// <param name="retMethod">Return method to send the results</param>
        public void QueueRequest(Vector3 position, Vector3 direction, float length, int count, RayCallback retMethod)
        {
            lock (m_PendingRequests)
            {
                ODERayRequest req = new ODERayRequest();
                req.callbackMethod = retMethod;
                req.length = length;
                req.Normal = direction;
                req.Origin = position;
                req.Count = count;

                m_PendingRayRequests.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)
                {
                    ODERayCastRequest[] reqs = m_PendingRequests.ToArray();
                    for (int i = 0; i < reqs.Length; i++)
                    {
                        if (reqs[i].callbackMethod != null) // quick optimization here, don't raycast 
                            RayCast(reqs[i]);               // if there isn't anyone to send results
                    }

                    m_PendingRequests.Clear();
                }
            }

            lock (m_PendingRayRequests)
            {
                if (m_PendingRayRequests.Count > 0)
                {
                    ODERayRequest[] reqs = m_PendingRayRequests.ToArray();
                    for (int i = 0; i < reqs.Length; i++)
                    {
                        if (reqs[i].callbackMethod != null) // quick optimization here, don't raycast 
                            RayCast(reqs[i]);               // if there isn't anyone to send results
                    }

                    m_PendingRayRequests.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);
            Vector3 snormal = Vector3.Zero;

            // 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;
                        snormal = cResult.Normal;
                    }
                }

                m_contactResults.Clear();
            }

            // Return results
            if (req.callbackMethod != null)
                req.callbackMethod(hitYN, closestcontact, hitConsumerID, distance, snormal);
        }

        /// <summary>
        /// Method that actually initiates the raycast
        /// </summary>
        /// <param name="req"></param>
        private void RayCast(ODERayRequest 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);

            // Find closest contact and object.
            lock (m_contactResults)
            {
                // Return results
                if (req.callbackMethod != null)
                    req.callbackMethod(m_contactResults);
            }
        }
        
        // This is the standard Near.   Uses space AABBs to speed up detection.
        private void near(IntPtr space, IntPtr g1, IntPtr g2)
        {

            //Don't test against heightfield Geom, or you'll be sorry!
            
            /*
             terminate called after throwing an instance of 'std::bad_alloc'
                  what():  std::bad_alloc
                Stacktrace:
                 
                  at (wrapper managed-to-native) Ode.NET.d.Collide (intptr,intptr,int,Ode.NET.d/ContactGeom[],int) <0x00004>
                  at (wrapper managed-to-native) Ode.NET.d.Collide (intptr,intptr,int,Ode.NET.d/ContactGeom[],int) <0xffffffff>
                  at OpenSim.Region.Physics.OdePlugin.ODERayCastRequestManager.near (intptr,intptr,intptr) <0x00280>
                  at (wrapper native-to-managed) OpenSim.Region.Physics.OdePlugin.ODERayCastRequestManager.near (intptr,intptr,intptr) <0xfff
                fffff>
                  at (wrapper managed-to-native) Ode.NET.d.SpaceCollide2 (intptr,intptr,intptr,Ode.NET.d/NearCallback) <0x00004>
                  at (wrapper managed-to-native) Ode.NET.d.SpaceCollide2 (intptr,intptr,intptr,Ode.NET.d/NearCallback) <0xffffffff>
                  at OpenSim.Region.Physics.OdePlugin.ODERayCastRequestManager.RayCast (OpenSim.Region.Physics.OdePlugin.ODERayCastRequest) <
                0x00114>
                  at OpenSim.Region.Physics.OdePlugin.ODERayCastRequestManager.ProcessQueuedRequests () <0x000eb>
                  at OpenSim.Region.Physics.OdePlugin.OdeScene.Simulate (single) <0x017e6>
                  at OpenSim.Region.Framework.Scenes.SceneGraph.UpdatePhysics (double) <0x00042>
                  at OpenSim.Region.Framework.Scenes.Scene.Update () <0x0039e>
                  at OpenSim.Region.Framework.Scenes.Scene.Heartbeat (object) <0x00019>
                  at (wrapper runtime-invoke) object.runtime_invoke_void__this___object (object,intptr,intptr,intptr) <0xffffffff>
                 
                Native stacktrace:
                 
                        mono [0x80d2a42]
                        [0xb7f5840c]
                        /lib/i686/cmov/libc.so.6(abort+0x188) [0xb7d1a018]
                        /usr/lib/libstdc++.so.6(_ZN9__gnu_cxx27__verbose_terminate_handlerEv+0x158) [0xb45fc988]
                        /usr/lib/libstdc++.so.6 [0xb45fa865]
                        /usr/lib/libstdc++.so.6 [0xb45fa8a2]
                        /usr/lib/libstdc++.so.6 [0xb45fa9da]
                        /usr/lib/libstdc++.so.6(_Znwj+0x83) [0xb45fb033]
                        /usr/lib/libstdc++.so.6(_Znaj+0x1d) [0xb45fb11d]
                        libode.so(_ZN13dxHeightfield23dCollideHeightfieldZoneEiiiiP6dxGeomiiP12dContactGeomi+0xd04) [0xb46678e4]
                        libode.so(_Z19dCollideHeightfieldP6dxGeomS0_iP12dContactGeomi+0x54b) [0xb466832b]
                        libode.so(dCollide+0x102) [0xb46571b2]
                        [0x95cfdec9]
                        [0x8ea07fe1]
                        [0xab260146]
                        libode.so [0xb465a5c4]
                        libode.so(_ZN11dxHashSpace8collide2EPvP6dxGeomPFvS0_S2_S2_E+0x75) [0xb465bcf5]
                        libode.so(dSpaceCollide2+0x177) [0xb465ac67]
                        [0x95cf978e]
                        [0x8ea07945]
                        [0x95cf2bbc]
                        [0xab2787e7]
                        [0xab419fb3]
                        [0xab416657]
                        [0xab415bda]
                        [0xb609b08e]
                        mono(mono_runtime_delegate_invoke+0x34) [0x8192534]
                        mono [0x81a2f0f]
                        mono [0x81d28b6]
                        mono [0x81ea2c6]
                        /lib/i686/cmov/libpthread.so.0 [0xb7e744c0]
                        /lib/i686/cmov/libc.so.6(clone+0x5e) [0xb7dcd6de]
             */

            // Exclude heightfield geom

            if (g1 == IntPtr.Zero || g2 == IntPtr.Zero)
                return;
            if (d.GeomGetClass(g1) == d.GeomClassID.HeightfieldClass || d.GeomGetClass(g2) == d.GeomClassID.HeightfieldClass)
                return;

            // 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 = p1.LocalID;
                        collisionresult.Pos = new Vector3(contacts[i].pos.X, contacts[i].pos.Y, contacts[i].pos.Z);
                        collisionresult.Depth = contacts[i].depth;
                        collisionresult.Normal = new Vector3(contacts[i].normal.X, contacts[i].normal.Y,
                                                             contacts[i].normal.Z);
                        lock (m_contactResults)
                            m_contactResults.Add(collisionresult);
                    }
                }

                if (p2 != null)
                {
                    if (p2 is OdePrim)
                    {
                        ContactResult collisionresult = new ContactResult();

                        collisionresult.ConsumerID = p2.LocalID;
                        collisionresult.Pos = new Vector3(contacts[i].pos.X, contacts[i].pos.Y, contacts[i].pos.Z);
                        collisionresult.Depth = contacts[i].depth;
                        collisionresult.Normal = new Vector3(contacts[i].normal.X, contacts[i].normal.Y,
                                      contacts[i].normal.Z);

                        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 ODERayRequest
    {
        public Vector3 Origin;
        public Vector3 Normal;
        public int Count;
        public float length;
        public RayCallback callbackMethod;
    }
}