/*
 * 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.
 */

// Revision 2011/12/13 by Ubit Umarov


using System;
using System.Collections.Concurrent;
using System.Collections.Generic;
using System.Linq;
using System.Reflection;
using System.Runtime.CompilerServices;
using System.Runtime.InteropServices;
using System.Threading;
using log4net;
using Nini.Config;
using OpenSim.Framework;
using OpenSim.Region.Framework.Scenes;
using OpenSim.Region.PhysicsModules.SharedBase;
using OpenMetaverse;

namespace OpenSim.Region.PhysicsModule.ubOde
{
     // colision flags of things others can colide with
    // rays, sensors, probes removed since can't  be colided with
    // The top space where things are placed provided further selection
    // ie physical are in active space nonphysical in static
    // this should be exclusive as possible

    [Flags]
    public enum CollisionCategories : uint
    {
        Disabled = 0,
        //by 'things' types
        Space =         0x01,
        Geom =          0x02, // aka prim/part
        Character =     0x04,
        Land =          0x08,
        Water =         0x010,

        // by state
        Phantom =       0x01000,
        VolumeDtc =     0x02000,
        Selected =      0x04000,
        NoShape =       0x08000,


        All =           0xffffffff
    }

    /// <summary>
    /// Material type for a primitive
    /// </summary>
    public enum Material : int
    {
        /// <summary></summary>
        Stone = 0,
        /// <summary></summary>
        Metal = 1,
        /// <summary></summary>
        Glass = 2,
        /// <summary></summary>
        Wood = 3,
        /// <summary></summary>
        Flesh = 4,
        /// <summary></summary>
        Plastic = 5,
        /// <summary></summary>
        Rubber = 6,

        light = 7 // compatibility with old viewers
    }

    public enum changes : int
    {
        Add = 0,                // arg null. finishs the prim creation. should be used internally only ( to remove later ?)
        Remove,
        Link,               // arg AuroraODEPrim new parent prim or null to delink. Makes the prim part of a object with prim parent as root
        //  or removes from a object if arg is null
        DeLink,
        Position,           // arg Vector3 new position in world coords. Changes prim position. Prim must know if it is root or child
        Orientation,        // arg Quaternion new orientation in world coords. Changes prim orientation. Prim must know it it is root or child
        PosOffset,          // not in use
        // arg Vector3 new position in local coords. Changes prim position in object
        OriOffset,          // not in use
        // arg Vector3 new orientation in local coords. Changes prim orientation in object
        Velocity,
        TargetVelocity,
        AngVelocity,
        Acceleration,
        Force,
        Torque,
        Momentum,

        AddForce,
        AddAngForce,
        AngLock,

        Buoyancy,

        PIDTarget,
        PIDTau,
        PIDActive,

        PIDHoverHeight,
        PIDHoverType,
        PIDHoverTau,
        PIDHoverActive,

        Size,
        AvatarSize,
        Shape,
        PhysRepData,
        AddPhysRep,

        VolumeDtc,

        Physical,
        Phantom,
        Selected,
        disabled,
        building,

        VehicleType,
        VehicleFloatParam,
        VehicleVectorParam,
        VehicleRotationParam,
        VehicleFlags,
        SetVehicle,
        SetInertia,

        Null             //keep this last used do dim the methods array. does nothing but pulsing the prim
    }

    public readonly struct ODEchangeitem
    {
        public readonly PhysicsActor actor;
        public readonly changes what;
        public readonly Object arg;

        public ODEchangeitem(PhysicsActor _actor, changes _what, Object _arg)
        {
            actor = _actor;
            what = _what;
            arg = _arg;
        }
    }

    public partial class ODEScene : PhysicsScene
    {
        private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);

        public Scene m_frameWorkScene = null;

        //private int threadid = 0;

        const UBOdeNative.ContactFlags commomContactFlags = UBOdeNative.ContactFlags.Bounce | UBOdeNative.ContactFlags.Approx1;
        const float commomContactERP = 0.75f;
        const float commonContactCFM = 0.0001f;
        const float commomContactSLIP = 0f;

        readonly float TerrainBounce = 0.001f;
        readonly float TerrainFriction = 0.3f;

        public float AvatarFriction = 0;// 0.9f * 0.5f;

        // this netx dimensions are only relevant for terrain partition (mega regions)
        // WorldExtents below has the simulation dimensions
        // they should be identical except on mega regions
        private int m_regionWidth = (int)Constants.RegionSize;
        private int m_regionHeight = (int)Constants.RegionSize;

        private int m_heightmapWidthSamples = (int)Constants.RegionSize + 3;
        private int m_heightmapHeightSamples = (int)Constants.RegionSize + 3;

        public float ODE_STEPSIZE = 0.020f;
        public float HalfOdeStep = 0.01f;
        public int odetimestepMS = 20; // rounded
        private float m_timeDilation = 1.0f;

        private double m_lastframe;
        private double m_lastMeshExpire;

        public float gravityx = 0f;
        public float gravityy = 0f;
        public float gravityz = -9.8f;

        public float WaterLevel = 0f;
        private int framecount = 0;

        private float avDensity = 80f;
        private float avMovementDivisorWalk = 1.3f;
        private float avMovementDivisorRun = 0.8f;
        private float minimumGroundFlightOffset = 3f;
        public float maximumMassObject = 10000.01f;
        public float geomDefaultDensity = 10.0f;

        public float maximumAngularVelocity = 12.0f; // default 12rad/s
        public float maxAngVelocitySQ = 144f;   // squared value

        public float bodyPIDD = 35f;
        public float bodyPIDG = 25;

        public int bodyFramesAutoDisable = 10;

        private UBOdeNative.NearCallback NearCallback;

        private readonly Dictionary<uint, OdePrim> _prims = new();
        private readonly HashSet<OdeCharacter> _characters = new();
        private readonly HashSet<OdePrim> _activeprims = new();
        private readonly HashSet<OdePrim> _activegroups = new();
        public List<OdeCharacter> _charactersList;

        public readonly ConcurrentQueue<ODEchangeitem> ChangesQueue = new();

        /// <summary>
        /// A list of actors that should receive collision events.
        /// </summary>
        private readonly Dictionary<uint, PhysicsActor> _collisionEventPrim = new();
        private readonly List<PhysicsActor> _collisionEventPrimRemove = new();

        private readonly List<OdeCharacter> _badCharacter = new();
        public readonly Dictionary<IntPtr, PhysicsActor> actor_name_map = new();

        private readonly float contactsurfacelayer = 0.002f;

        private readonly int contactsPerCollision = 80;
        internal IntPtr ContactgeomsArray = IntPtr.Zero;
        internal UBOdeNative.ContactGeom[] m_contacts;
        internal GCHandle m_contactsHandler;

        private IntPtr GlobalContactsArray;

        private UBOdeNative.Contact contactSharedForJoints = new();

        const int maxContactJoints = 6000;
        private volatile int ContactJointCount = 0;

        private IntPtr JointContactGroup;

        public readonly ContactData[] m_materialContactsData = new ContactData[8];

        public IntPtr TerrainGeom;
        private float[] m_terrainHeights;
        private GCHandle m_terrainHeightsHandler = new();
        private IntPtr HeightmapData;
        private int m_lastRegionWidth;
        private int m_lastRegionHeight;

        private readonly int m_physicsiterations = 15;
        private const float m_SkipFramesAtms = 0.40f; // Drop frames gracefully at a 400 ms lag
        //private PhysicsActor PANull = new NullPhysicsActor();
        private float step_time = 0.0f;

        public IntPtr world;

        // split the spaces acording to contents type
        // ActiveSpace contains characters and active prims
        // StaticSpace contains land and other that is mostly static in enviroment
        // this can contain subspaces, like the grid in staticspace
        // as now space only contains this 2 top spaces

        public IntPtr TopSpace; // the global space
        public IntPtr ActiveSpace; // space for active prims
        public IntPtr StaticSpace; // space for the static things around

        public readonly object OdeLock = new();
        public static readonly object SimulationLock = new();
        public IMesher mesher;

        public IConfigSource m_config;

        public Vector2 WorldExtents = new((int)Constants.RegionSize, (int)Constants.RegionSize);

        private ODERayCastRequestManager m_rayCastManager;
        public ODEMeshWorker m_meshWorker;

        /* maybe needed if ode uses tls
        private void checkThread()
        {

            int th = Thread.CurrentThread.ManagedThreadId;
            if(th != threadid)
            {
                threadid = th;
                d.AllocateODEDataForThread(~0U);
            }
        }
        */

        IConfig physicsconfig = null;

        public ODEScene(Scene pscene, IConfigSource psourceconfig, string pname, string pversion)
        {
            EngineType = pname;
            PhysicsSceneName = EngineType + "/" + pscene.RegionInfo.RegionName;
            EngineName = pname + " " + pversion;
            m_config = psourceconfig;

            m_frameWorkScene = pscene;

            m_frameWorkScene.RegisterModuleInterface<PhysicsScene>(this);

            Initialization();
        }

        public void RegionLoaded()
        {
            mesher = m_frameWorkScene.RequestModuleInterface<IMesher>();
            if (mesher == null)
            {
                m_log.ErrorFormat("[ubOde] No mesher. module disabled");
                return;
            }

            m_meshWorker = new ODEMeshWorker(this, m_log, mesher, physicsconfig);
            m_frameWorkScene.PhysicsEnabled = true;
        }
        /// <summary>
        /// Initiailizes the scene
        /// Sets many properties that ODE requires to be stable
        /// These settings need to be tweaked 'exactly' right or weird stuff happens.
        /// </summary>
        private void Initialization()
        {
            UBOdeNative.AllocateODEDataForThread(~0U);

            NearCallback = DefaultNearCallback;

            _charactersList = new List<OdeCharacter>(m_frameWorkScene.RegionInfo.AgentCapacity);

            WorldExtents.X = m_frameWorkScene.RegionInfo.RegionSizeX;
            m_regionWidth = (int)WorldExtents.X;
            WorldExtents.Y = m_frameWorkScene.RegionInfo.RegionSizeY;
            m_regionHeight = (int)WorldExtents.Y;

            lock (OdeLock)
            {
                // Create the world and the first space
                try
                {
                    world = UBOdeNative.WorldCreate();
                    TopSpace = UBOdeNative.SimpleSpaceCreate(IntPtr.Zero);
                    ActiveSpace = UBOdeNative.SimpleSpaceCreate(TopSpace);
                    float sx = m_regionWidth + 16;
                    float sy = m_regionHeight + 16;
                    UBOdeNative.Vector3 px = new(sx * 0.5f, sy  * 0.5f, 0);
                    if (sx < sy)
                        sx = sy;
                    int dp = Util.intLog2((uint)sx);
                    if(dp > 8)
                        dp = 8;
                    else if(dp < 4)
                        dp = 4;                  
                    StaticSpace = UBOdeNative.QuadTreeSpaceCreate(TopSpace, ref px, ref px, dp);
                }
                catch
                {
                }

                // move to high level
                UBOdeNative.SpaceSetSublevel(ActiveSpace, 1);
                UBOdeNative.SpaceSetSublevel(StaticSpace, 1);

                UBOdeNative.GeomSetCategoryBits(ActiveSpace, (uint)(CollisionCategories.Space |
                                                        CollisionCategories.Geom |
                                                        CollisionCategories.Character |
                                                        CollisionCategories.Phantom |
                                                        CollisionCategories.VolumeDtc
                                                        ));
                UBOdeNative.GeomSetCollideBits(ActiveSpace, (uint)(CollisionCategories.Space |
                                                        CollisionCategories.Geom |
                                                        CollisionCategories.Character |
                                                        CollisionCategories.Phantom |
                                                        CollisionCategories.VolumeDtc
                                                        ));

                UBOdeNative.GeomSetCategoryBits(StaticSpace, (uint)(CollisionCategories.Space |
                                                        CollisionCategories.Geom |
                                                        //CollisionCategories.Land |
                                                        //CollisionCategories.Water |
                                                        CollisionCategories.Phantom |
                                                        CollisionCategories.VolumeDtc
                                                        ));
                UBOdeNative.GeomSetCollideBits(StaticSpace, 0);

                JointContactGroup = UBOdeNative.JointGroupCreate(maxContactJoints + 1);
                //contactgroup

                UBOdeNative.WorldSetAutoDisableFlag(world, false);
            }


            //checkThread();


            // Defaults

            int contactsPerCollision = 80;

            physicsconfig = null;

            if (m_config != null)
            {
                physicsconfig = m_config.Configs["ODEPhysicsSettings"];
                if (physicsconfig != null)
                {
                    gravityx = physicsconfig.GetFloat("world_gravityx", gravityx);
                    gravityy = physicsconfig.GetFloat("world_gravityy", gravityy);
                    gravityz = physicsconfig.GetFloat("world_gravityz", gravityz);

                    //contactsurfacelayer = physicsconfig.GetFloat("world_contact_surface_layer", contactsurfacelayer);

                    ODE_STEPSIZE = physicsconfig.GetFloat("world_stepsize", ODE_STEPSIZE);

                    avDensity = physicsconfig.GetFloat("av_density", avDensity);
                    avMovementDivisorWalk = physicsconfig.GetFloat("av_movement_divisor_walk", avMovementDivisorWalk);
                    avMovementDivisorRun = physicsconfig.GetFloat("av_movement_divisor_run", avMovementDivisorRun);

                    contactsPerCollision = physicsconfig.GetInt("contacts_per_collision", contactsPerCollision);

                    geomDefaultDensity = physicsconfig.GetFloat("geometry_default_density", geomDefaultDensity);
//                    bodyFramesAutoDisable = physicsconfig.GetInt("body_frames_auto_disable", bodyFramesAutoDisable);

                    minimumGroundFlightOffset = physicsconfig.GetFloat("minimum_ground_flight_offset", minimumGroundFlightOffset);
                    maximumMassObject = physicsconfig.GetFloat("maximum_mass_object", maximumMassObject);

                    avDensity *= 3f / 80f;  // scale other engines density option to this
                }
            }

            float heartbeat = 1/m_frameWorkScene.FrameTime;
            maximumAngularVelocity = 0.49f * heartbeat *(float)Math.PI;
            maxAngVelocitySQ = maximumAngularVelocity * maximumAngularVelocity;

            UBOdeNative.WorldSetCFM(world, commonContactCFM);
            UBOdeNative.WorldSetERP(world, commomContactERP);

            UBOdeNative.WorldSetGravity(world, gravityx, gravityy, gravityz);

            UBOdeNative.WorldSetLinearDamping(world, 0.001f);
            UBOdeNative.WorldSetAngularDamping(world, 0.002f);
            UBOdeNative.WorldSetAngularDampingThreshold(world, 0f);
            UBOdeNative.WorldSetLinearDampingThreshold(world, 0f);
            UBOdeNative.WorldSetMaxAngularSpeed(world, maximumAngularVelocity);

            UBOdeNative.WorldSetQuickStepNumIterations(world, m_physicsiterations);

            UBOdeNative.WorldSetContactSurfaceLayer(world, contactsurfacelayer);
            UBOdeNative.WorldSetContactMaxCorrectingVel(world, 60.0f);

            HalfOdeStep = ODE_STEPSIZE * 0.5f;
            odetimestepMS = (int)(1000.0f * ODE_STEPSIZE + 0.5f);

            m_contacts = new UBOdeNative.ContactGeom[contactsPerCollision];
            m_contactsHandler = GCHandle.Alloc(m_contacts, GCHandleType.Pinned);
            ContactgeomsArray = m_contactsHandler.AddrOfPinnedObject();

            GlobalContactsArray = Marshal.AllocHGlobal((maxContactJoints + 100) * UBOdeNative.SizeOfContact);

            contactSharedForJoints.geom.g1 = IntPtr.Zero;
            contactSharedForJoints.geom.g2 = IntPtr.Zero;

            contactSharedForJoints.geom.side1 = -1;
            contactSharedForJoints.geom.side2 = -1;

            contactSharedForJoints.surface.mode = commomContactFlags;
            contactSharedForJoints.surface.mu = 0;
            contactSharedForJoints.surface.bounce = 0;
            contactSharedForJoints.surface.bounce_vel = 1.5f;
            contactSharedForJoints.surface.soft_cfm = commonContactCFM;
            contactSharedForJoints.surface.soft_erp = commomContactERP;
            contactSharedForJoints.surface.slip1 = commomContactSLIP;
            contactSharedForJoints.surface.slip2 = commomContactSLIP;

            m_materialContactsData[(int)Material.Stone].mu = 0.8f;
            m_materialContactsData[(int)Material.Stone].bounce = 0.4f;

            m_materialContactsData[(int)Material.Metal].mu = 0.3f;
            m_materialContactsData[(int)Material.Metal].bounce = 0.4f;

            m_materialContactsData[(int)Material.Glass].mu = 0.2f;
            m_materialContactsData[(int)Material.Glass].bounce = 0.7f;

            m_materialContactsData[(int)Material.Wood].mu = 0.6f;
            m_materialContactsData[(int)Material.Wood].bounce = 0.5f;

            m_materialContactsData[(int)Material.Flesh].mu = 0.9f;
            m_materialContactsData[(int)Material.Flesh].bounce = 0.3f;

            m_materialContactsData[(int)Material.Plastic].mu = 0.4f;
            m_materialContactsData[(int)Material.Plastic].bounce = 0.7f;

            m_materialContactsData[(int)Material.Rubber].mu = 0.9f;
            m_materialContactsData[(int)Material.Rubber].bounce = 0.95f;

            m_materialContactsData[(int)Material.light].mu = 0.0f;
            m_materialContactsData[(int)Material.light].bounce = 0.0f;

            m_lastframe = Util.GetTimeStamp();
            m_lastMeshExpire = m_lastframe;
            step_time = -1;

            m_rayCastManager = new ODERayCastRequestManager(this);

            base.Initialise(m_frameWorkScene.PhysicsRequestAsset,
                (m_frameWorkScene.Heightmap != null ? m_frameWorkScene.Heightmap.GetFloatsSerialised() : new float[m_frameWorkScene.RegionInfo.RegionSizeX * m_frameWorkScene.RegionInfo.RegionSizeY]),
                (float)m_frameWorkScene.RegionInfo.RegionSettings.WaterHeight);
        }

        /*
        internal void waitForSpaceUnlock(IntPtr space)
        {
            //if (space != IntPtr.Zero)
                //while (d.SpaceLockQuery(space)) { } // Wait and do nothing
        }
        */

        #region Collision Detection
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private IntPtr CreateContacJoint(ref UBOdeNative.ContactGeom contactGeom, bool smooth)
        {
            if (ContactJointCount >= maxContactJoints)
                return IntPtr.Zero;

            ContactJointCount++;
            contactSharedForJoints.geom.depth = smooth ? contactGeom.depth * 0.05f : contactGeom.depth;
            contactSharedForJoints.geom.pos = contactGeom.pos;
            contactSharedForJoints.geom.normal = contactGeom.normal;

            IntPtr contact = new(GlobalContactsArray.ToInt64() + (Int64)(ContactJointCount * UBOdeNative.SizeOfContact));
            Marshal.StructureToPtr(contactSharedForJoints, contact, false);
            return UBOdeNative.JointCreateContactPtr(world, JointContactGroup, contact);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private IntPtr CreateCharContacJoint()
        {
            ContactJointCount++;

            IntPtr contact = new(GlobalContactsArray.ToInt64() + (Int64)(ContactJointCount * UBOdeNative.SizeOfContact));
            Marshal.StructureToPtr(contactSharedForJoints, contact, false);
            return UBOdeNative.JointCreateContactPtr(world, JointContactGroup, contact);
        }

        UBOdeNative.ContactGeom altWorkContact = new();

        /// <summary>
        /// This is our near callback.  A geometry is near a body
        /// </summary>
        /// <param name="space">The space that contains the geoms.  Remember, spaces are also geoms</param>
        /// <param name="g1">a geometry or space</param>
        /// <param name="g2">another geometry or space</param>
        ///
        private void DefaultNearCallback(IntPtr space, IntPtr g1, IntPtr g2)
        {
            //  no lock here!  It's invoked from within Simulate(), which is thread-locked
            if (ContactJointCount >= maxContactJoints)
                return;

            // Test if we're colliding a geom with a space.
            // If so we have to drill down into the space recursively
            if (g1 == IntPtr.Zero || g2 == IntPtr.Zero)
                return;

            if (UBOdeNative.GeomIsSpace(g1) || UBOdeNative.GeomIsSpace(g2))
            {
                // We'll be calling near recursivly if one
                // of them is a space to find all of the
                // contact points in the space
                try
                {
                    UBOdeNative.SpaceCollide2(g1, g2, IntPtr.Zero, DefaultNearCallback);
                }
                catch (AccessViolationException)
                {
                    m_log.Warn("[PHYSICS]: Unable to collide test a space");
                }
                return;
            }

            if (g1 == g2)
                return; // Can't collide with yourself

            // Figure out how many contact points we have
            int count = 0;
            try
            {
                if (UBOdeNative.GeomGetCategoryBits(g1) == (uint)CollisionCategories.VolumeDtc ||
                    UBOdeNative.GeomGetCategoryBits(g2) == (uint)CollisionCategories.VolumeDtc)
                {
                    int cflags = unchecked((int)(1 | UBOdeNative.CONTACTS_UNIMPORTANT));
                    count = UBOdeNative.CollidePtr(g1, g2, cflags, ContactgeomsArray, UBOdeNative.SizeOfContactGeom);
                }
                else
                    count = UBOdeNative.CollidePtr(g1, g2, contactsPerCollision, ContactgeomsArray, UBOdeNative.SizeOfContactGeom);
            }
            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.");
                //ode.drelease(world);
                base.TriggerPhysicsBasedRestart();
            }
            catch (Exception e)
            {
                m_log.WarnFormat("[PHYSICS]: Unable to collide test an object: {0}", e.Message);
                return;
            }

            // contacts done
            if (count == 0)
                return;

            // try get physical actors
            if (!actor_name_map.TryGetValue(g1, out PhysicsActor p1))
            {
                m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 1");
                return;
            }

            if (!actor_name_map.TryGetValue(g2, out PhysicsActor p2))
            {
                m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 2");
                return;
            }

            // do volume detection case
            if ((p1.IsVolumeDtc || p2.IsVolumeDtc))
            {
                ref UBOdeNative.ContactGeom curctc0 = ref m_contacts[0];
                ContactPoint volDepthContact = new(
                    new Vector3(curctc0.pos.X, curctc0.pos.Y, curctc0.pos.Z),
                    new Vector3(curctc0.normal.X, curctc0.normal.Y, curctc0.normal.Z),
                    curctc0.depth, false
                    );

                Collision_accounting_events(p1, p2, ref volDepthContact);
                return;
            }

            // big messy collision analises
            float mu = 0;
            float bounce = 0;
            //bool IgnoreNegSides = false;

            ContactData contactdata1 = new(0, 0, false);
            ContactData contactdata2 = new(0, 0, false);

            bool dop1ava = false;
            bool dop2ava = false;
            bool ignore = false;
            bool smoothMesh = false;

            switch (p1.PhysicsActorType)
            {
                case (int)ActorTypes.Agent:
                    {
                        dop1ava = true;
                        switch (p2.PhysicsActorType)
                        {
                            case (int)ActorTypes.Agent:
                            case (int)ActorTypes.Prim:
                                break;

                            default:
                                ignore = true; // avatar to terrain and water ignored
                                break;
                        }
                        break;
                    }

                case (int)ActorTypes.Prim:
                    {
                        switch (p2.PhysicsActorType)
                        {
                            case (int)ActorTypes.Agent:
                                dop2ava = true;
                                break;

                            case (int)ActorTypes.Prim:
                                //Vector3 relV = p1.rootVelocity - p2.rootVelocity;
                                //float relVlenSQ = relV.LengthSquared();
                                //if (relVlenSQ > 0.0001f)
                                {
                                    p1.CollidingObj = true;
                                    p2.CollidingObj = true;
                                }
                                p1.getContactData(ref contactdata1);
                                p2.getContactData(ref contactdata2);
                                bounce = contactdata1.bounce * contactdata2.bounce;
                                mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu);

                                //if (relVlenSQ > 0.01f)
                                //    mu *= frictionMovementMult;

                                if (UBOdeNative.GeomGetClass(g2) == UBOdeNative.GeomClassID.TriMeshClass &&
                                    UBOdeNative.GeomGetClass(g1) == UBOdeNative.GeomClassID.TriMeshClass)
                                    smoothMesh = true;
                                break;

                            case (int)ActorTypes.Ground:
                                p1.getContactData(ref contactdata1);
                                bounce = contactdata1.bounce * TerrainBounce;
                                mu = (float)Math.Sqrt(contactdata1.mu * TerrainFriction);

                                //Vector3 v1 = p1.rootVelocity;
                                //if (Math.Abs(v1.X) > 0.1f || Math.Abs(v1.Y) > 0.1f)
                                //    mu *= frictionMovementMult;
                                p1.CollidingGround = true;

                                if (UBOdeNative.GeomGetClass(g1) == UBOdeNative.GeomClassID.TriMeshClass)
                                    smoothMesh = true;
                                break;

                            case (int)ActorTypes.Water:
                            default:
                                ignore = true;
                                break;
                        }
                    }
                    break;

                case (int)ActorTypes.Ground:
                    if (p2.PhysicsActorType == (int)ActorTypes.Prim)
                    {
                        p2.CollidingGround = true;
                        p2.getContactData(ref contactdata2);
                        bounce = contactdata2.bounce * TerrainBounce;
                        mu = (float)Math.Sqrt(contactdata2.mu * TerrainFriction);

                        //if (curContact.side1 > 0) // should be 2 ?
                        //    IgnoreNegSides = true;
                        //Vector3 v2 = p2.rootVelocity;
                        //if (Math.Abs(v2.X) > 0.1f || Math.Abs(v2.Y) > 0.1f)
                        //    mu *= frictionMovementMult;

                        if (UBOdeNative.GeomGetClass(g2) == UBOdeNative.GeomClassID.TriMeshClass)
                            smoothMesh = true;
                    }
                    else
                        ignore = true;
                    break;

                case (int)ActorTypes.Water:
                default:
                    break;
            }

            if (ignore)
                return;

            IntPtr Joint;
            bool FeetCollision = false;
            int ncontacts = 0;

            ContactPoint maxDepthContact = new();

            float minDepth = float.MaxValue;
            float maxDepth = float.MinValue;

            contactSharedForJoints.surface.mu = mu;
            contactSharedForJoints.surface.bounce = bounce;

            bool useAltcontact;
            bool noskip;

            if (dop1ava || dop2ava)
                smoothMesh = false;

            IntPtr b1 = UBOdeNative.GeomGetBody(g1);
            IntPtr b2 = UBOdeNative.GeomGetBody(g2);

            for (int i = 0; i < count; ++i)
            {
                ref UBOdeNative.ContactGeom curctc = ref m_contacts[i];
                noskip = true;
                useAltcontact = false;

                if (dop1ava)
                {
                    if ((((OdeCharacter)p1).Collide(g2, false, ref curctc, ref altWorkContact, ref useAltcontact, ref FeetCollision)))
                    {
                        if (p2.PhysicsActorType == (int)ActorTypes.Agent)
                        {
                            p1.CollidingObj = true;
                            p2.CollidingObj = true;
                        }
                        else if (p2.rootVelocity.LengthSquared() > 0.0f)
                            p2.CollidingObj = true;
                    }
                    else
                        noskip = false;
                }
                else if (dop2ava)
                {
                    if ((((OdeCharacter)p2).Collide(g1, true, ref curctc, ref altWorkContact, ref useAltcontact, ref FeetCollision)))
                    {
                        if (p1.PhysicsActorType == (int)ActorTypes.Agent)
                        {
                            p1.CollidingObj = true;
                            p2.CollidingObj = true;
                        }
                        else if (p1.rootVelocity.LengthSquared() > 0.0f)
                            p1.CollidingObj = true;
                    }
                    else
                        noskip = false;
                }

                if (noskip)
                {
                    Joint = useAltcontact ? 
                                CreateContacJoint(ref altWorkContact, smoothMesh) : 
                                CreateContacJoint(ref curctc, smoothMesh);
                    if (Joint == IntPtr.Zero)
                        break;

                    UBOdeNative.JointAttach(Joint, b1, b2);

                    ncontacts++;

                    if (curctc.depth > maxDepth)
                    {
                        maxDepth = curctc.depth;
                        maxDepthContact.PenetrationDepth = maxDepth;
                        maxDepthContact.Position = Unsafe.As<UBOdeNative.Vector3, Vector3>(ref curctc.pos);
                        maxDepthContact.CharacterFeet = FeetCollision;
                    }

                    if (curctc.depth < minDepth)
                    {
                        minDepth = curctc.depth;
                        maxDepthContact.SurfaceNormal = Unsafe.As<UBOdeNative.Vector3, Vector3>(ref curctc.normal);
                    }
                }
            }

            if (ncontacts > 0)
            {
                Collision_accounting_events(p1, p2, ref maxDepthContact);
            }
        }

        private void CharPrimNearCallback(IntPtr space, IntPtr g1, IntPtr g2)
        {
            //  no lock here!  It's invoked from within Simulate(), which is thread-locked
            if (ContactJointCount >= maxContactJoints)
                return;

            // Test if we're colliding a geom with a space.
            // If so we have to drill down into the space recursively
            if (g1 == IntPtr.Zero || g2 == IntPtr.Zero)
                return;

            //if (SafeNativeMethods.GeomIsSpace(g1) || SafeNativeMethods.GeomIsSpace(g2))
            if (UBOdeNative.GeomIsSpace(g2))
            {
                // We'll be calling near recursivly if one
                // of them is a space to find all of the
                // contact points in the space
                try
                {
                    UBOdeNative.SpaceCollide2(g1, g2, IntPtr.Zero, CharPrimNearCallback);
                }
                catch (AccessViolationException)
                {
                    m_log.Warn("[PHYSICS]: Unable to collide test a space");
                }
                return;
            }

            // Figure out how many contact points we have
            int count = 0;
            try
            {
                if (UBOdeNative.GeomGetCategoryBits(g2) == (uint)CollisionCategories.VolumeDtc)
                {
                    int cflags = unchecked((int)(1 | UBOdeNative.CONTACTS_UNIMPORTANT));
                    count = UBOdeNative.CollidePtr(g1, g2, cflags, ContactgeomsArray, UBOdeNative.SizeOfContactGeom);
                }
                else
                    count = UBOdeNative.CollidePtr(g1, g2, contactsPerCollision, ContactgeomsArray, UBOdeNative.SizeOfContactGeom);
            }
            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.");
                //ode.drelease(world);
                base.TriggerPhysicsBasedRestart();
            }
            catch (Exception e)
            {
                m_log.WarnFormat("[PHYSICS]: Unable to collide test an object: {0}", e.Message);
                return;
            }

            // contacts done
            if (count == 0)
                return;

            // try get physical actors
            if (!actor_name_map.TryGetValue(g1, out PhysicsActor p1))
            {
                m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 1");
                return;
            }

            if (!actor_name_map.TryGetValue(g2, out PhysicsActor p2))
            {
                m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 2");
                return;
            }

            // do volume detection case
            if (p2.IsVolumeDtc)
            {
                ref UBOdeNative.ContactGeom curctc0 = ref m_contacts[0];
                ContactPoint volDepthContact = new(
                    new Vector3(curctc0.pos.X, curctc0.pos.Y, curctc0.pos.Z),
                    new Vector3(curctc0.normal.X, curctc0.normal.Y, curctc0.normal.Z),
                    curctc0.depth, false
                    );

                Collision_accounting_events(p1, p2, ref volDepthContact);
                return;
            }

            if(p2.PhysicsActorType != (int)ActorTypes.Prim)
                    return;

            IntPtr Joint;
            bool FeetCollision = false;
            int ncontacts = 0;

            ContactPoint accountContact = new();

            float minDepth = float.MaxValue;
            float maxDepth = float.MinValue;

            contactSharedForJoints.surface.mu = 0;
            contactSharedForJoints.surface.bounce = 0;

            bool useAltcontact;

            IntPtr b1 = UBOdeNative.GeomGetBody(g1);
            IntPtr b2 = UBOdeNative.GeomGetBody(g2);

            for (int i = 0; i < count; ++i)
            {
                ref UBOdeNative.ContactGeom curctc = ref m_contacts[i];
                useAltcontact = false;
                 
                if ((((OdeCharacter)p1).Collide(g2, false, ref curctc, ref altWorkContact, ref useAltcontact, ref FeetCollision)))
                {
                    if(p2.rootVelocity.LengthSquared() > 0.0f)
                        p2.CollidingObj = true;
 
                    Joint = useAltcontact ?
                                CreateContacJoint(ref altWorkContact, false) :
                                CreateContacJoint(ref curctc, false);
                    if (Joint == IntPtr.Zero)
                        break;

                    UBOdeNative.JointAttach(Joint, b1, b2);

                    ncontacts++;

                    if (curctc.depth > maxDepth)
                    {
                        maxDepth = curctc.depth;
                        accountContact.PenetrationDepth = maxDepth;
                        accountContact.Position = Unsafe.As<UBOdeNative.Vector3, Vector3>(ref curctc.pos);
                        accountContact.CharacterFeet = FeetCollision;
                    }

                    if (curctc.depth < minDepth)
                    {
                        minDepth = curctc.depth;
                        accountContact.SurfaceNormal = Unsafe.As<UBOdeNative.Vector3, Vector3>(ref curctc.normal);
                    }
                }
            }

            if (ncontacts > 0)
            {
                Collision_accounting_events(p1, p2, ref accountContact);
            }
        }

        private static void Collision_accounting_events(PhysicsActor p1, PhysicsActor p2, ref ContactPoint contact)
        {

            // update actors collision score
            if (p1.CollisionScore < float.MaxValue)
                p1.CollisionScore += 1.0f;
            if (p2.CollisionScore < float.MaxValue)
                p2.CollisionScore += 1.0f;

            bool p1events = p1.SubscribedEvents();
            bool p2events = p2.SubscribedEvents();

            if (p1.IsVolumeDtc)
                p2events = false;
            if (p2.IsVolumeDtc)
                p1events = false;

            if (!p2events && !p1events)
                return;

            Vector3 vel = Vector3.Zero;
            if (p2.IsPhysical)
                vel = p2.rootVelocity;

            if (p1.IsPhysical)
                vel -= p1.rootVelocity;

            contact.RelativeSpeed = Vector3.Dot(vel, contact.SurfaceNormal);

            uint obj2LocalID;

            switch ((ActorTypes)p1.PhysicsActorType)
            {
                case ActorTypes.Agent:
                case ActorTypes.Prim:
                {
                    switch ((ActorTypes)p2.PhysicsActorType)
                    {
                        case ActorTypes.Agent:
                        case ActorTypes.Prim:
                            if (p2events)
                            {
                                //AddCollisionEventReporting(p2);
                                p2.AddCollisionEvent(p1.ParentActor.m_baseLocalID, contact);
                            }
                            else if(p1.IsVolumeDtc)
                                p2.AddVDTCCollisionEvent(p1.ParentActor.m_baseLocalID, contact);

                            obj2LocalID = p2.ParentActor.m_baseLocalID;
                            break;

                        case ActorTypes.Ground:
                        case ActorTypes.Unknown:
                        default:
                            obj2LocalID = 0;
                            break;
                    }
                    if (p1events)
                    {
                        contact.SurfaceNormal = -contact.SurfaceNormal;
                        contact.RelativeSpeed = -contact.RelativeSpeed;
                        //AddCollisionEventReporting(p1);
                        p1.AddCollisionEvent(obj2LocalID, contact);
                    }
                    else if(p2.IsVolumeDtc)
                    {
                        contact.SurfaceNormal = -contact.SurfaceNormal;
                        contact.RelativeSpeed = -contact.RelativeSpeed;
                        //AddCollisionEventReporting(p1);
                        p1.AddVDTCCollisionEvent(obj2LocalID, contact);
                    }
                    break;
                }
                case ActorTypes.Ground:
                case ActorTypes.Unknown:
                default:
                {
                    if (p2events && !p2.IsVolumeDtc)
                    {
                        //AddCollisionEventReporting(p2);
                        p2.AddCollisionEvent(0, contact);
                    }
                    break;
                }
            }
        }

        /// <summary>
        /// This is our collision testing routine in ODE
        /// </summary>
        /// <param name="timeStep"></param>
        private void collision_optimized()
        {
            lock (_characters)
            {
                if (_charactersList.Count > 0)
                {
                    try
                    {
                        Span<OdeCharacter> charsSpan = CollectionsMarshal.AsSpan(_charactersList);
                        if (charsSpan.Length == 1)
                        {
                            OdeCharacter chr = charsSpan[0];
                            if (chr.Colliderfilter < -1)
                                chr.Colliderfilter = -1;
                            else
                            {
                                chr.IsColliding = false;
                                chr.CollidingObj = false;

                                UBOdeNative.SpaceCollide2(chr.collider, StaticSpace, IntPtr.Zero, CharPrimNearCallback);
                                UBOdeNative.SpaceCollide2(chr.collider, ActiveSpace, IntPtr.Zero, CharPrimNearCallback);
                            }
                        }
                        else
                        {
                            for (int i = 0; i < charsSpan.Length; ++i)
                            {
                                OdeCharacter chr = charsSpan[i];

                                if (chr.Colliderfilter < -1)
                                    chr.Colliderfilter = -1;
                                else
                                {
                                    chr.IsColliding = false;
                                    chr.CollidingObj = false;

                                    // do colisions with static space
                                    UBOdeNative.SpaceCollide2(chr.collider, StaticSpace, IntPtr.Zero, CharPrimNearCallback);
                                    UBOdeNative.SpaceCollide2(chr.collider, ActiveSpace, IntPtr.Zero, CharPrimNearCallback);

                                    float mx = chr._AABB2D.minx;
                                    float Mx = chr._AABB2D.maxx;
                                    float my = chr._AABB2D.miny;
                                    float My = chr._AABB2D.maxy;

                                    for (int j = i + 1 ; j < charsSpan.Length; ++j)
                                    {
                                        OdeCharacter chr2 = charsSpan[j];
                                        if (chr2.Colliderfilter < -1)
                                            continue;

                                        if(Mx < chr2._AABB2D.minx ||
                                           mx > chr2._AABB2D.maxx ||
                                           My < chr2._AABB2D.miny ||
                                           my > chr2._AABB2D.maxy)
                                            continue;
 
                                        CollideCharChar(chr, chr2);
                                    }
                                }
                            }
                        }

                        // chars with chars
                        //SafeNativeMethods.SpaceCollide(CharsSpace, IntPtr.Zero, nearCallback);
                    }
                    catch (AccessViolationException)
                    {
                        m_log.Warn("[PHYSICS]: Unable to collide Character to static space");
                    }
                }
            }

            if(_activeprims.Count > 0)
            {
                lock (_activeprims)
                {
                    foreach (OdePrim aprim in _activeprims)
                    {
                        aprim.CollisionScore = 0;
                        aprim.IsColliding = false;
                        if(!aprim.m_outbounds && UBOdeNative.BodyIsEnabled(aprim.Body))
                            aprim.clearSleeperCollisions();
                    }
                }

                lock (_activegroups)
                {
                    try
                    {
                        foreach (OdePrim aprim in _activegroups)
                        {
                            if(!aprim.m_outbounds && UBOdeNative.BodyIsEnabled(aprim.Body) &&
                                    aprim.m_collide_geom != IntPtr.Zero)
                            {
                                UBOdeNative.SpaceCollide2(StaticSpace, aprim.m_collide_geom, IntPtr.Zero, NearCallback);
                                UBOdeNative.SpaceCollide2(TerrainGeom, aprim.m_collide_geom, IntPtr.Zero, NearCallback);
                            }
                        }
                    }
                    catch (Exception e)
                    {
                        m_log.Warn("[PHYSICS]: Unable to collide Active to Static: " + e.Message);
                    }
                }
                // colide active amoung them
                try
                {
                    UBOdeNative.SpaceCollide(ActiveSpace, IntPtr.Zero, NearCallback);
                }
                catch (Exception e)
                {
                        m_log.Warn("[PHYSICS]: Unable to collide in Active: " + e.Message);
                }
            }
            /*
            // and with chars
            try
            {
                SafeNativeMethods.SpaceCollide2(CharsSpace,ActiveSpace, IntPtr.Zero, nearCallback);
            }
            catch (Exception e)
            {
                    m_log.Warn("[PHYSICS]: Unable to collide Active to Character: " + e.Message);
            }
            */
        }

        #endregion
        /// <summary>
        /// Add actor to the list that should receive collision events in the simulate loop.
        /// </summary>
        /// <param name="obj"></param>
        public void AddCollisionEventReporting(PhysicsActor obj)
        {
            _collisionEventPrim[obj.LocalID] = obj;
        }

        /// <summary>
        /// Remove actor from the list that should receive collision events in the simulate loop.
        /// </summary>
        /// <param name="obj"></param>
        public void RemoveCollisionEventReporting(PhysicsActor obj)
        {
            lock(_collisionEventPrimRemove)
            {
                _collisionEventPrimRemove.Add(obj);
            }
        }

        public override float TimeDilation
        {
            get { return m_timeDilation; }
        }

        #region Add/Remove Entities

        public override PhysicsActor AddAvatar(string avName, Vector3 position, Vector3 velocity, Vector3 size, bool isFlying)
        {
            return null;
        }

        public override PhysicsActor AddAvatar(uint localID, string avName, Vector3 position, Vector3 size, float feetOffset, bool isFlying)
        {
            OdeCharacter newAv = new(localID, avName, this, position,
                size, feetOffset, avDensity, avMovementDivisorWalk, avMovementDivisorRun)
            {
                Flying = isFlying,
                MinimumGroundFlightOffset = minimumGroundFlightOffset
            };

            return newAv;
        }

        public void AddCharacter(OdeCharacter chr)
        {
            lock (_characters)
            {
                if (_characters.Add(chr))
                {
                    chr._charsListIndex = _charactersList.Count;
                    _charactersList.Add(chr);
                }
                else
                    chr._charsListIndex = -1;
                if (chr.bad)
                    m_log.DebugFormat("[PHYSICS] Added BAD actor {0} to characters list", chr.m_baseLocalID);
            }
        }

        public void RemoveCharacter(OdeCharacter chr)
        {
            lock (_characters)
            {
                if (_characters.Remove(chr))
                {
                    if (chr._charsListIndex >= 0)
                    {
                        int last = _charactersList.Count - 1;
                        if(chr._charsListIndex != last)
                        {
                            _charactersList[chr._charsListIndex] = _charactersList[last];
                            _charactersList[chr._charsListIndex]._charsListIndex = chr._charsListIndex;
                        }
                        _charactersList.RemoveAt(last);
                        chr._charsListIndex = -1;
                    }
                }
            }
        }

        public void BadCharacter(OdeCharacter chr)
        {
            lock (_badCharacter)
            {
                _badCharacter.Add(chr);
            }
        }

        public override void RemoveAvatar(PhysicsActor actor)
        {
            //m_log.Debug("[PHYSICS]:ODELOCK");
            if (world == IntPtr.Zero)
                return;
            ((OdeCharacter) actor).Destroy();
        }

        public void addActivePrim(OdePrim activatePrim)
        {
            // adds active prim..
            lock (_activeprims)
            {
                _activeprims.Add(activatePrim);
            }
        }

        public void addActiveGroups(OdePrim activatePrim)
        {
            lock (_activegroups)
            {
                _activegroups.Add(activatePrim);
            }
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private PhysicsActor AddPrim(String name, Vector3 position, Vector3 size, Quaternion rotation,
                                     PrimitiveBaseShape pbs, bool isphysical, bool isPhantom, byte shapeType, uint localID)
        {
            OdePrim newPrim;
            lock (OdeLock)
            {
                newPrim = new OdePrim(name, this, position, size, rotation, pbs, isphysical, isPhantom, shapeType, localID);
            }
            return newPrim;
        }

        public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
                                                  Vector3 size, Quaternion rotation, bool isPhysical, bool isPhantom, uint localid)
        {
            return AddPrim(primName, position, size, rotation, pbs, isPhysical, isPhantom, 0 , localid);
        }

        public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
                                                  Vector3 size, Quaternion rotation, bool isPhysical, uint localid)
        {
            return AddPrim(primName, position, size, rotation, pbs, isPhysical,false, 0, localid);
        }

        public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
                                                  Vector3 size, Quaternion rotation, bool isPhysical, bool isPhantom, byte shapeType, uint localid)
        {
            return AddPrim(primName, position, size, rotation, pbs, isPhysical,isPhantom, shapeType, localid);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void remActivePrim(OdePrim deactivatePrim)
        {
            lock (_activeprims)
            {
                _activeprims.Remove(deactivatePrim);
            }
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void remActiveGroup(OdePrim deactivatePrim)
        {
            lock (_activegroups)
            {
                _activegroups.Remove(deactivatePrim);
            }
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public override void RemovePrim(PhysicsActor prim)
        {
            // As with all ODE physics operations, we don't remove the prim immediately but signal that it should be
            // removed in the next physics simulate pass.
            if (prim is OdePrim p)
            {
                p.setPrimForRemoval();
            }
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void RemovePrimThreadLocked(OdePrim prim)
        {
            //Console.WriteLine("RemovePrimThreadLocked " +  prim.m_primName);
            lock (_prims)
                _prims.Remove(prim.m_baseLocalID);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void addToPrims(OdePrim prim)
        {
            lock (_prims)
            {
                _prims[prim.m_baseLocalID] = prim;
            }
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public OdePrim getPrim(uint id)
        {
            lock (_prims)
            {
                if(_prims.TryGetValue(id, out OdePrim p))
                    return p;
                else
                    return null;
            }
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public bool havePrim(OdePrim prim)
        {
            lock (_prims)
                return _prims.ContainsKey(prim.m_baseLocalID);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void changePrimID(OdePrim prim,uint oldID)
        {
            lock (_prims)
            {
                _prims.Remove(oldID);
                _prims[prim.m_baseLocalID] = prim;
            }
        }

        public bool haveActor(PhysicsActor actor)
        {
            if (actor is OdePrim prim)
            {
                lock (_prims)
                    return _prims.ContainsKey(prim.m_baseLocalID);
            }
            else if (actor is OdeCharacter ch)
            {
                lock (_characters)
                    return _characters.Contains(ch);
            }
            return false;
        }

        #endregion

        #region Space Separation Calculation

        /// <summary>
        /// Called when a static prim moves or becomes static
        /// Places the prim in a space one the static space
        /// </summary>
        /// <param name="geom">the pointer to the geom that moved</param>
        /// <param name="currentspace">a pointer to the space it was in before it was moved.</param>
        /// <returns>a pointer to the new space it's in</returns>
        public IntPtr MoveGeomToStaticSpace(IntPtr geom, IntPtr currentspace)
        {
            // moves a prim into static sub-space

            // Called ODEPrim so
            // it's already in locked space.

            if (geom == IntPtr.Zero) // shouldn't happen
                return IntPtr.Zero;
            
            if (StaticSpace == currentspace) // if we are there all done
                return StaticSpace;

            // else remove it from its current space
            if (currentspace != IntPtr.Zero && UBOdeNative.SpaceQuery(currentspace, geom))
            {
                if (UBOdeNative.GeomIsSpace(currentspace))
                {
                    //waitForSpaceUnlock(currentspace);
                    UBOdeNative.SpaceRemove(currentspace, geom);

                    if (UBOdeNative.SpaceGetSublevel(currentspace) == 0 && UBOdeNative.SpaceGetNumGeoms(currentspace) == 0)
                    {
                        UBOdeNative.SpaceDestroy(currentspace);
                    }
                }
                else
                {
                    m_log.Info("[Physics]: Invalid or empty Space passed to 'MoveGeomToStaticSpace':" + currentspace +
                                   " Geom:" + geom);
                }
            }
            else
            {
                currentspace = UBOdeNative.GeomGetSpace(geom);
                if (currentspace != IntPtr.Zero)
                {
                    if (UBOdeNative.GeomIsSpace(currentspace))
                    {
                        //waitForSpaceUnlock(currentspace);
                        UBOdeNative.SpaceRemove(currentspace, geom);

                        if (UBOdeNative.SpaceGetSublevel(currentspace) == 0 && UBOdeNative.SpaceGetNumGeoms(currentspace) == 0)
                        {
                            UBOdeNative.SpaceDestroy(currentspace);
                        }
                    }
                }
            }

            // put the geom in the newspace
            //waitForSpaceUnlock(StaticSpace);
            if(UBOdeNative.SpaceQuery(StaticSpace, geom))
                m_log.Info("[Physics]: 'MoveGeomToStaticSpace' geom already in static space:" + geom);
            else
                UBOdeNative.SpaceAdd(StaticSpace, geom);

            return StaticSpace;
        }
        #endregion


        /// <summary>
        /// Called to queue a change to a actor
        /// </summary>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        internal void AddChange(PhysicsActor _actor, changes _what, Object _arg)
        {
            if (world == IntPtr.Zero)
                return;
            ChangesQueue.Enqueue(new ODEchangeitem(_actor, _what, _arg));
        }

        // does all pending changes generated during region load process
        public override void ProcessPreSimulation()
        {
            lock (OdeLock)
            {
                if (world == IntPtr.Zero)
                    return;

                int donechanges = 0;
                if (!ChangesQueue.IsEmpty)
                {
                    m_log.InfoFormat("[ubOde] start processing pending actor operations");
                    int tstart = Util.EnvironmentTickCount();

                    UBOdeNative.AllocateODEDataForThread(~0U);

                    while (ChangesQueue.TryDequeue(out ODEchangeitem item))
                    {
                        try
                        {
                            lock (SimulationLock)
                            {
                                if (item.actor is OdePrim prim)
                                {
                                    if (prim.DoAChange(item.what, item.arg))
                                        RemovePrimThreadLocked((OdePrim)item.actor);
                                }
                                else
                                    ((OdeCharacter)item.actor).DoAChange(item.what, item.arg);
                            }
                        }
                        catch
                        {
                            try
                            {
                                m_log.WarnFormat($"[PHYSICS]: Operation failed for a actor {item.actor.Name} {item.what}");
                            }
                            catch
                            {
                                m_log.WarnFormat("[PHYSICS]: Operation failed for a unknown actor");
                            }
                        }
                        donechanges++;
                    }
                    int time = Util.EnvironmentTickCountSubtract(tstart);
                    m_log.InfoFormat("[ubOde] finished {0} operations in {1}ms", donechanges, time);
                }
                m_log.InfoFormat("[ubOde] {0} prim actors loaded",_prims.Count);
            }
            m_lastframe = Util.GetTimeStamp() + 0.5;
            step_time = -0.5f;
        }

        /// <summary>
        /// This is our main simulate loop
        /// It's thread locked by a Mutex in the scene.
        /// It holds Collisions, it instructs ODE to step through the physical reactions
        /// It moves the objects around in memory
        /// It calls the methods that report back to the object owners.. (scenepresence, SceneObjectGroup)
        /// </summary>
        /// <param name="timeStep"></param>
        /// <returns></returns>
        public override float Simulate(float reqTimeStep)
        {
            if (world == IntPtr.Zero)
                return 0;

            double now = Util.GetTimeStamp();
            double timeStep = now - m_lastframe;
            m_lastframe = now;

            // acumulate time so we can reduce error
            step_time += (float)timeStep;

            if (step_time < HalfOdeStep)
                return 0;

            if (framecount <= 0)
                framecount = 1;
            else
                framecount++;

            //checkThread();
            int nodeframes = 0;
            float fps = 0;

            lock (OdeLock)
            {

                //d.WorldSetQuickStepNumIterations(world, curphysiteractions);

                double loopstartMS = Util.GetTimeStampMS();
                double maxChangestime = (int)(reqTimeStep * 500f) + loopstartMS; // half the time
                double maxLoopTime = (int)(reqTimeStep * 1200f) + loopstartMS; // 1.2 the time


                //double collisionTime = 0;
                //double qstepTIme = 0;
                //double tmpTime = 0;
                //double changestot = 0;
                //double collisonRepo = 0;
                //double updatesTime = 0;
                //double moveTime = 0;
                //double rayTime = 0;


                UBOdeNative.AllocateODEDataForThread(~0U);

                while (ChangesQueue.TryDequeue(out ODEchangeitem item))
                {
                    try
                    {
                        lock (SimulationLock)
                        {
                            if (item.actor is OdePrim prim)
                            {
                                if (prim.DoAChange(item.what, item.arg))
                                    RemovePrimThreadLocked((OdePrim)item.actor);
                            }
                            else
                                ((OdeCharacter)item.actor).DoAChange(item.what, item.arg);
                        }
                    }
                    catch
                    {
                        try
                        {
                            m_log.WarnFormat($"[PHYSICS]: Operation failed for a actor {item.actor.Name} {item.what}");
                        }
                        catch
                        {
                            m_log.WarnFormat("[PHYSICS]: Operation failed for a unknown actor");
                        }
                    }
                    if (maxChangestime < Util.GetTimeStampMS())
                        break;
                }

                // do simulation taking at most 150ms total time including changes
                while (step_time > HalfOdeStep)
                {
                    try
                    {
                        // clear pointer/counter to contacts to pass into joints
                        ContactJointCount = 0;

                        //tmpTime =  Util.GetTimeStampMS();

                        // Move characters
                        lock (_characters)
                        {
                            foreach (OdeCharacter actor in _characters)
                            {
                                    actor.Move();
                            }
                        }

                        // Move other active objects
                        lock (_activegroups)
                        {
                            foreach (OdePrim aprim in _activegroups)
                            {
                                    aprim.Move();
                            }
                        }
                        //moveTime += Util.GetTimeStampMS() - tmpTime;
                        //tmpTime =  Util.GetTimeStampMS();
                        lock (SimulationLock)
                        {
                            m_rayCastManager.ProcessQueuedRequests();

                            //rayTime += Util.GetTimeStampMS() - tmpTime;
                            //tmpTime =  Util.GetTimeStampMS();

                            collision_optimized();
                        }

                        //collisionTime += Util.GetTimeStampMS() - tmpTime;
                        //tmpTime =  Util.GetTimeStampMS();

                        lock (_collisionEventPrimRemove)
                        {
                            foreach (PhysicsActor obj in CollectionsMarshal.AsSpan(_collisionEventPrimRemove))
                                _collisionEventPrim.Remove(obj.LocalID);

                            _collisionEventPrimRemove.Clear();
                        }

                        List<OdePrim> sleepers = new();
                        foreach (PhysicsActor obj in _collisionEventPrim.Values)
                        {
                            switch ((ActorTypes)obj.PhysicsActorType)
                            {
                                case ActorTypes.Agent:
                                    OdeCharacter cobj = (OdeCharacter)obj;
                                    cobj.SendCollisions(odetimestepMS);
                                    break;

                                case ActorTypes.Prim:
                                    OdePrim pobj = (OdePrim)obj;
                                    if (!pobj.m_outbounds)
                                    {
                                        pobj.SendCollisions(odetimestepMS);
                                        lock(SimulationLock)
                                        {
                                            if(pobj.Body != IntPtr.Zero && !pobj.m_isSelected &&
                                                !pobj.m_disabled && !pobj.m_building &&
                                                !UBOdeNative.BodyIsEnabled(pobj.Body))
                                            sleepers.Add(pobj);
                                        }
                                    }
                                    break;
                            }
                        }

                        foreach(OdePrim prm in sleepers)
                            prm.SleeperAddCollisionEvents();
                        sleepers.Clear();

                        //collisonRepo += Util.GetTimeStampMS() - tmpTime;
                        //tmpTime = Util.GetTimeStampMS();

                        // do a ode simulation step
                        lock (SimulationLock)
                        {
                            UBOdeNative.WorldQuickStep(world, ODE_STEPSIZE);
                            UBOdeNative.JointGroupEmpty(JointContactGroup);
                        }
                        //qstepTIme += Util.GetTimeStampMS() - tmpTime;

                        // update managed ideia of physical data and do updates to core
                        /*
                        lock (_characters)
                        {
                            foreach (OdeCharacter actor in _characters)
                            {
                                if (actor != null)
                                {
                                    if (actor.bad)
                                        m_log.WarnFormat("[PHYSICS]: BAD Actor {0} in _characters list was not removed?", actor.m_uuid);

                                    actor.UpdatePositionAndVelocity();
                                }
                            }
                        }
                        */
                        //tmpTime =  Util.GetTimeStampMS();
                        lock (_activegroups)
                        {
                            {
                                foreach (OdePrim actor in _activegroups)
                                {
                                    if (actor.IsPhysical)
                                    {
                                        actor.UpdatePositionAndVelocity(framecount);
                                    }
                                }
                            }
                        }

                        //updatesTime += Util.GetTimeStampMS() - tmpTime;
                    }
                    catch (Exception e)
                    {
                        m_log.ErrorFormat("[PHYSICS]: {0}, {1}, {2}", e.Message, e.TargetSite, e);
                        //ode.dunlock(world);
                    }

                    step_time -= ODE_STEPSIZE;
                    nodeframes++;

                     if (Util.GetTimeStampMS() > maxLoopTime)
                        break;
                }

                lock (_badCharacter)
                {
                    if (_badCharacter.Count > 0)
                    {
                        foreach (OdeCharacter chr in _badCharacter)
                        {
                            RemoveCharacter(chr);
                        }
                        _badCharacter.Clear();
                    }
                }

                // information block for in debug breakpoint only
                /*
                int ntopactivegeoms = SafeNativeMethods.SpaceGetNumGeoms(ActiveSpace);
                int ntopstaticgeoms = SafeNativeMethods.SpaceGetNumGeoms(StaticSpace);
                int ngroundgeoms = SafeNativeMethods.SpaceGetNumGeoms(GroundSpace);

                int nactivegeoms = 0;
                int nactivespaces = 0;

                int nstaticgeoms = 0;
                int nstaticspaces = 0;
                IntPtr sp;

                for (int i = 0; i < ntopactivegeoms; i++)
                {
                    sp = SafeNativeMethods.SpaceGetGeom(ActiveSpace, i);
                    if (SafeNativeMethods.GeomIsSpace(sp))
                    {
                        nactivespaces++;
                        nactivegeoms += SafeNativeMethods.SpaceGetNumGeoms(sp);
                    }
                    else
                        nactivegeoms++;
                }

                for (int i = 0; i < ntopstaticgeoms; i++)
                {
                    sp = SafeNativeMethods.SpaceGetGeom(StaticSpace, i);
                    if (SafeNativeMethods.GeomIsSpace(sp))
                    {
                        nstaticspaces++;
                        nstaticgeoms += SafeNativeMethods.SpaceGetNumGeoms(sp);
                    }
                    else
                        nstaticgeoms++;
                }
                
                int ntopgeoms = SafeNativeMethods.SpaceGetNumGeoms(TopSpace);

                int totgeoms = nstaticgeoms + nactivegeoms + ngroundgeoms + 1; // one ray
                int nbodies = SafeNativeMethods.NTotalBodies;
                int ngeoms = SafeNativeMethods.NTotalGeoms;
                */

                //looptimeMS /= nodeframes;
                //collisionTime /= nodeframes;
                //qstepTIme /= nodeframes;
                //changestot /= nodeframes; 
                //collisonRepo /= nodeframes;
                //updatesTime /= nodeframes;
                //moveTime /= nodeframes;
                //rayTime /= nodeframes;

                //if(looptimeMS > .05)
                {
                }
                
                fps = (float)nodeframes * ODE_STEPSIZE / reqTimeStep;

                if(step_time < HalfOdeStep)
                    m_timeDilation = 1.0f;
                else if (step_time > m_SkipFramesAtms)
                {
                    // if we lag too much skip frames
                    m_timeDilation = 0.0f;
                    step_time = 0;
                    m_lastframe = Util.GetTimeStamp(); // skip also the time lost
                }
                else
                {
                    m_timeDilation = ODE_STEPSIZE / step_time;
                    if (m_timeDilation > 1)
                        m_timeDilation = 1;
                }

                if (m_timeDilation == 1 && now - m_lastMeshExpire > 30)
                {
                    mesher.ExpireReleaseMeshs();
                    m_lastMeshExpire = now;
                }
            }

            return fps;
        }

        public unsafe float GetTerrainHeightAtXY(float x, float y)
        {
            // TerrainHeightField for ODE as offset 1m
            x += 1f;
            y += 1f;

            // integer indexs
            int ix;
            int iy;
            //  interpolators offset
            float dx;
            float dy;

            // make position fit into array
            if (x < 0)
            {
                ix = 0;
                dx = 0;
            }
            else if (x < m_heightmapWidthSamples - 1)
            {
                ix = (int)x;
                dx = x - ix;
            }
            else // out world use external height
            {
                ix = m_heightmapWidthSamples - 2;
                dx = 0;
            }
            if (y < 0)
            {
                iy = 0;
                dy = 0;
            }
            else if (y < m_heightmapHeightSamples - 1)
            {
                iy = (int)y;
                dy = y - iy;
            }
            else
            {
                iy = m_heightmapHeightSamples - 2;
                dy = 0;
            }

            float h0;
            float h1;
            float h2;

            iy *= m_heightmapWidthSamples;
            iy += ix; // all indexes have iy + ix

            fixed(float* heightsb = &m_terrainHeights[iy])
            {
                float* heights = heightsb;
                h0 = *heights; // 0,0 vertice

                if (dy>dx)
                {
                    heights += m_heightmapWidthSamples;
                    h2 = *heights; // 0,1 vertice
                    h1 = (h2 - h0) * dy; // 0,1 vertice minus 0,0
                    ++heights;
                    h2 = (*heights - h2) * dx; // 1,1 vertice minus 0,1
                }
                else
                {
                    ++heights;
                    h2 = *heights; // vertice 1,0
                    h1 = (h2 - h0) * dx; // 1,0 vertice minus 0,0
                    heights += m_heightmapWidthSamples;
                    h2 = (*heights - h2) * dy; // 1,1 vertice minus 1,0
                }
            }
            return h0 + h1 + h2;
        }

        public unsafe Vector3 GetTerrainNormalAtXY(float x, float y)
        {
            // TerrainHeightField for ODE as offset 1m
            x += 1f;
            y += 1f;

            int ix;
            int iy;
            float dx;
            float dy;

            // make position fit into array
            if (x < 0)
            {
                ix = 0;
                dx = 0;
            }
            else if (x < m_heightmapWidthSamples - 1)
            {
                ix = (int)x;
                dx = x - ix;
            }
            else // out world use external height
            {
                ix = m_heightmapWidthSamples - 2;
                dx = 0;
            }
            if (y < 0)
            {
                iy = 0;
                dy = 0;
            }
            else if (y < m_heightmapHeightSamples - 1)
            {
                iy = (int)y;
                dy = y - iy;
            }
            else
            {
                iy = m_heightmapHeightSamples - 2;
                dy = 0;
            }

            float h0;
            float h1;
            float h2;

            iy *= m_heightmapWidthSamples;
            iy += ix; // all indexes have iy + ix

            float rx;
            float ry;
            fixed (float* heightsB = &m_terrainHeights[iy])
            {
                float* heights = heightsB;
                if (dy > dx)
                {
                    h1 = *heights; // 0,0 vertice
                    heights += m_heightmapWidthSamples;
                    h0 = *heights; // 0,1
                    h2 = *(heights + 1); // 1,1 vertice
                    rx = h0 - h2;
                    ry = h1 - h0;
                }
                else
                {
                    h2 = *heights; // 0,0 vertice
                    heights++;
                    h0 = *heights; // 1,0 vertice
                    h1 = *(heights + m_heightmapWidthSamples); // vertice 1,1
                    rx = h2 - h0;
                    ry = h0 - h1;
                }
            }
            h0 = rx * rx + ry * ry + 1.0f;
            h0 = 1.0f / MathF.Sqrt(h0);
            return new Vector3(rx * h0, ry * h0, h0);
        }

        private void InitTerrain()
        {
            lock(SimulationLock)
            lock (OdeLock)
            {
                UBOdeNative.AllocateODEDataForThread(~0U);

                if (TerrainGeom != IntPtr.Zero)
                {
                    actor_name_map.Remove(TerrainGeom);
                    UBOdeNative.GeomDestroy(TerrainGeom);
                }

                if (m_terrainHeightsHandler.IsAllocated)
                    m_terrainHeightsHandler.Free();
                m_terrainHeights = null;

                m_heightmapWidthSamples = m_regionWidth + 3;
                m_heightmapHeightSamples = m_regionHeight + 3;

                m_terrainHeights = new float[m_heightmapWidthSamples * m_heightmapHeightSamples];
                m_terrainHeightsHandler = GCHandle.Alloc(m_terrainHeights, GCHandleType.Pinned);

                m_lastRegionWidth = m_regionWidth;
                m_lastRegionHeight = m_regionHeight;

                HeightmapData = UBOdeNative.GeomOSTerrainDataCreate();
                UBOdeNative.GeomOSTerrainDataBuild(HeightmapData, m_terrainHeightsHandler.AddrOfPinnedObject(), 0, 1.0f,
                                                 m_heightmapWidthSamples, m_heightmapHeightSamples,
                                                 1, 0);

                TerrainGeom = UBOdeNative.CreateOSTerrain(TopSpace, HeightmapData, 1);
                if (TerrainGeom != IntPtr.Zero)
                {
                    UBOdeNative.GeomSetCategoryBits(TerrainGeom, (uint)(CollisionCategories.Land));
                    UBOdeNative.GeomSetCollideBits(TerrainGeom, 0);

                        PhysicsActor pa = new NullPhysicsActor
                        {
                            Name = "Terrain",
                            PhysicsActorType = (int)ActorTypes.Ground
                        };
                        actor_name_map[TerrainGeom] = pa;

                    //geom_name_map[GroundGeom] = "Terrain";

                    UBOdeNative.GeomSetPosition(TerrainGeom, m_regionWidth * 0.5f, m_regionHeight * 0.5f, 0.0f);
                }
                else
                    m_terrainHeightsHandler.Free();
            }
        }

        public override void SetTerrain(float[] heightMap)
        {
            // assumes 1m size grid and constante size square regions
            // needs to know about sims around in future

            if(m_regionWidth != m_lastRegionWidth ||
                    m_regionHeight != m_lastRegionHeight ||
                    !m_terrainHeightsHandler.IsAllocated ||
                    TerrainGeom == IntPtr.Zero)
                InitTerrain();

            int regionsizeX = m_regionWidth;
            int regionsizeY = m_regionHeight;

            int heightmapWidth = regionsizeX + 2;
            int heightmapHeight = regionsizeY + 2;

            m_heightmapWidthSamples = heightmapWidth + 1;
            m_heightmapHeightSamples = heightmapHeight + 1;

            float val;

            int maxXX = regionsizeX + 1;
            int maxYY = regionsizeY + 1;
            // adding one margin all around so things don't fall in edges

            int xx;
            int yy = 0;
            int yt = 0;
            float minH = float.MaxValue;
            float maxH = float.MinValue;

            for (int y = 0; y < m_heightmapHeightSamples; y++)
            {
                if (y > 1 && y < maxYY)
                    yy += regionsizeX;
                xx = 0;

                lock(OdeLock)
                {
                    for (int x = 0; x < m_heightmapWidthSamples; x++)
                    {
                        if (x > 1 && x < maxXX)
                            xx++;

                        val = Utils.Clamp(heightMap[yy + xx], Constants.MinTerrainHeightmap, Constants.MaxTerrainHeightmap);
                        if(val > maxH)
                            maxH = val;
                        if(val < minH)
                            minH = val;
                        m_terrainHeights[yt + x] = val;
                    }
                }
                yt += m_heightmapWidthSamples;
            }

            lock(SimulationLock)
            lock (OdeLock)
            {
                UBOdeNative.GeomOSTerrainDataSetBounds(HeightmapData, minH, maxH);
                UBOdeNative.GeomSetPosition(TerrainGeom, m_regionWidth * 0.5f, m_regionHeight * 0.5f, 0.0f);
            }
        }

        public override void DeleteTerrain()
        {
        }

        public override void SetWaterLevel(float baseheight)
        {
            WaterLevel = baseheight;
        }

        public override void Dispose()
        {
            lock(SimulationLock)
            lock (OdeLock)
            {
                if (world == IntPtr.Zero)
                    return;

                UBOdeNative.AllocateODEDataForThread(~0U);

                m_meshWorker?.Stop();

                if (m_rayCastManager != null)
                {
                    m_rayCastManager.Dispose();
                    m_rayCastManager = null;
                }

                lock (_prims)
                {
                    foreach (OdePrim prm in _prims.Values)
                    {
                        prm.DoAChange(changes.Remove, null);
                        _collisionEventPrim.Remove(prm.LocalID);
                    }
                    _prims.Clear();
                }

                OdeCharacter[] chtorem;
                lock (_characters)
                {
                    chtorem = new OdeCharacter[_characters.Count];
                    _characters.CopyTo(chtorem);
                }

                foreach (OdeCharacter ch in chtorem)
                    ch.DoAChange(changes.Remove, null);

                if (TerrainGeom != IntPtr.Zero)
                {
                    UBOdeNative.GeomDestroy(TerrainGeom);
                    TerrainGeom = IntPtr.Zero;
                }

                if (m_terrainHeightsHandler.IsAllocated)
                    m_terrainHeightsHandler.Free();
                m_terrainHeights = null;

                if (m_contactsHandler.IsAllocated)
                {
                    m_contactsHandler.Free();
                    ContactgeomsArray = IntPtr.Zero;
                }

                if (GlobalContactsArray != IntPtr.Zero)
                {
                    Marshal.FreeHGlobal(GlobalContactsArray);
                    GlobalContactsArray = IntPtr.Zero;
                }

                UBOdeNative.WorldDestroy(world);
                world = IntPtr.Zero;
                //d.CloseODE();
            }
        }

        private int compareByCollisionsDesc(OdePrim A, OdePrim B)
        {
            return -A.CollisionScore.CompareTo(B.CollisionScore);
        }

        public override Dictionary<uint, float> GetTopColliders()
        {
            Dictionary<uint, float> topColliders;
            List<OdePrim> orderedPrims;
            lock (_activeprims)
                orderedPrims = new List<OdePrim>(_activeprims);

            orderedPrims.Sort(compareByCollisionsDesc);
            topColliders = orderedPrims.Take(25).ToDictionary(p => p.m_baseLocalID, p => p.CollisionScore);

            return topColliders;
        }

        public override bool SupportsRayCast()
        {
            return true;
        }

        public override void RaycastWorld(Vector3 position, Vector3 direction, float length, RaycastCallback retMethod)
        {
            if (retMethod != null)
            {
                ODERayRequest req = new()
                {
                    actor = null,
                    callbackMethod = retMethod,
                    length = length,
                    Normal = direction,
                    Origin = position,
                    Count = 0,
                    filter = RayFilterFlags.AllPrims
                };
                m_rayCastManager.QueueRequest(req);
            }
        }

        public override void RaycastWorld(Vector3 position, Vector3 direction, float length, int Count, RayCallback retMethod)
        {
            if (retMethod != null)
            {
                ODERayRequest req = new()
                {
                    actor = null,
                    callbackMethod = retMethod,
                    length = length,
                    Normal = direction,
                    Origin = position,
                    Count = Count,
                    filter = RayFilterFlags.AllPrims
                };

                m_rayCastManager.QueueRequest(req);
            }
        }

        public override List<ContactResult> RaycastWorld(Vector3 position, Vector3 direction, float length, int Count)
        {
            List<ContactResult> ourresults = new();
            object SyncObject = new();

            RayCallback retMethod = delegate(List<ContactResult> results)
            {
                lock (SyncObject)
                {
                    ourresults = results;
                    Monitor.PulseAll(SyncObject);
                }
            };

            ODERayRequest req = new()
            {
                actor = null,
                callbackMethod = retMethod,
                length = length,
                Normal = direction,
                Origin = position,
                Count = Count,
                filter = RayFilterFlags.AllPrims
            };

            lock (SyncObject)
            {
                m_rayCastManager.QueueRequest(req);
                if (!Monitor.Wait(SyncObject, 500))
                    return null;
                else
                    return ourresults;
            }
        }

        public override bool SupportsRaycastWorldFiltered()
        {
            return true;
        }

        public override object RaycastWorld(Vector3 position, Vector3 direction, float length, int Count, RayFilterFlags filter)
        {
            object SyncObject = new();
            List<ContactResult> ourresults = new();

            RayCallback retMethod = delegate(List<ContactResult> results)
            {
                lock (SyncObject)
                {
                    ourresults = results;
                    Monitor.PulseAll(SyncObject);
                }
            };

            ODERayRequest req = new()
            {
                actor = null,
                callbackMethod = retMethod,
                length = length,
                Normal = direction,
                Origin = position,
                Count = Count,
                filter = filter
            };

            lock (SyncObject)
            {
                m_rayCastManager.QueueRequest(req);
                if (!Monitor.Wait(SyncObject, 500))
                    return null;
                else
                    return ourresults;
            }
        }

        public override List<ContactResult> RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count, RayFilterFlags flags)
        {
            if (actor == null)
                return new List<ContactResult>();

            IntPtr geom;
            if (actor is OdePrim prim)
                geom = prim.m_prim_geom;
            else if (actor is OdeCharacter ch)
                geom = ch.collider;
            else
                return new List<ContactResult>();

            if (geom == IntPtr.Zero)
                return new List<ContactResult>();

            List<ContactResult> ourResults = null;
            object SyncObject = new();

            RayCallback retMethod = delegate(List<ContactResult> results)
            {
                lock (SyncObject)
                {
                    ourResults = results;
                    Monitor.PulseAll(SyncObject);
                }
            };

            ODERayRequest req = new()
            {
                actor = actor,
                callbackMethod = retMethod,
                length = length,
                Normal = direction,
                Origin = position,
                Count = Count,
                filter = flags
            };

            lock (SyncObject)
            {
                m_rayCastManager.QueueRequest(req);
                if (!Monitor.Wait(SyncObject, 500))
                    return new List<ContactResult>();
            }

            if (ourResults == null)
                return new List<ContactResult>();
            return ourResults;
        }

        public override int SitAvatar(PhysicsActor actor, Vector3 AbsolutePosition, Vector3 CameraPosition, Vector3 offset, Vector3 AvatarSize, SitAvatarCallback PhysicsSitResponse)
        {
            Util.FireAndForget( delegate
            {
                ODESitAvatar sitAvatar = new(this, m_rayCastManager);
                sitAvatar?.Sit(actor, AbsolutePosition, CameraPosition, offset, AvatarSize, PhysicsSitResponse);
            });
            return 1;
        }
    }
}