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

//#define SPAM

using System;
using System.Collections.Generic;
using System.Reflection;
using System.Runtime.InteropServices;
using System.Threading;
using System.IO;
using System.Diagnostics;
using log4net;
using Nini.Config;
using OdeAPI;
using OpenSim.Framework;
using OpenSim.Region.Physics.Manager;
using OpenMetaverse;

namespace OpenSim.Region.Physics.OdePlugin
{
    public enum StatusIndicators : int
    {
        Generic = 0,
        Start = 1,
        End = 2
    }

    public struct sCollisionData
    {
        public uint ColliderLocalId;
        public uint CollidedWithLocalId;
        public int NumberOfCollisions;
        public int CollisionType;
        public int StatusIndicator;
        public int lastframe;
    }

    // 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 position. 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 position in local coords. Changes prim position in object
        Velocity,
        AngVelocity,
        Acceleration,
        Force,
        Torque,
        Momentum,

        AddForce,
        AddAngForce,
        AngLock,

        Size,
        Shape,

        CollidesWater,
        VolumeDtc,

        Physical,
        Phantom,
        Selected,
        disabled,
        building,

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

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

    public struct ODEchangeitem
    {
        public PhysicsActor actor;
        public OdeCharacter character;
        public changes what;
        public Object arg;
    }
     
    public class OdeScene : PhysicsScene
    {
        private readonly ILog m_log;
        // private Dictionary<string, sCollisionData> m_storedCollisions = new Dictionary<string, sCollisionData>();

        public bool OdeUbitLib = false;
//        private int threadid = 0;
        private Random fluidRandomizer = new Random(Environment.TickCount);

        const d.ContactFlags comumContactFlags = d.ContactFlags.SoftERP | d.ContactFlags.SoftCFM |d.ContactFlags.Approx1 | d.ContactFlags.Bounce;
        const float MaxERP = 0.8f;
        const float minERP = 0.1f;
        const float comumContactCFM = 0.0001f;
        
        float frictionMovementMult = 0.8f;

        float TerrainBounce = 0.1f;
        float TerrainFriction = 0.3f;

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

        private const uint m_regionWidth = Constants.RegionSize;
        private const uint m_regionHeight = Constants.RegionSize;

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

        DateTime m_lastframe;

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

        private float waterlevel = 0f;
        private int framecount = 0;

        private IntPtr WaterGeom = IntPtr.Zero;
        private IntPtr WaterHeightmapData = IntPtr.Zero;
        private GCHandle WaterMapHandler = new GCHandle();

        public float avPIDD = 2200f; // make it visible
        public float avPIDP = 900f; // make it visible
        private float avCapRadius = 0.37f;
        private float avDensity = 3f;
        private float avMovementDivisorWalk = 1.3f;
        private float avMovementDivisorRun = 0.8f;
        private float minimumGroundFlightOffset = 3f;
        public float maximumMassObject = 10000.01f;

        public bool meshSculptedPrim = true;
        public bool forceSimplePrimMeshing = false;

        public float meshSculptLOD = 32;
        public float MeshSculptphysicalLOD = 32;

        public float geomDefaultDensity = 10.000006836f;

        public int geomContactPointsStartthrottle = 3;
        public int geomUpdatesPerThrottledUpdate = 15;

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

//        public int geomCrossingFailuresBeforeOutofbounds = 6;

        public int bodyFramesAutoDisable = 5;


        private d.NearCallback nearCallback;

        private HashSet<OdeCharacter> _characters = new HashSet<OdeCharacter>();
        private HashSet<OdePrim> _prims = new HashSet<OdePrim>();
        private HashSet<OdePrim> _activeprims = new HashSet<OdePrim>();
        private HashSet<OdePrim> _activegroups = new HashSet<OdePrim>();

        public OpenSim.Framework.LocklessQueue<ODEchangeitem> ChangesQueue = new OpenSim.Framework.LocklessQueue<ODEchangeitem>();

        /// <summary>
        /// A list of actors that should receive collision events.
        /// </summary>
        private List<PhysicsActor> _collisionEventPrim = new List<PhysicsActor>();
        private List<PhysicsActor> _collisionEventPrimRemove = new List<PhysicsActor>();
        
        private HashSet<OdeCharacter> _badCharacter = new HashSet<OdeCharacter>();
        public Dictionary<IntPtr, String> geom_name_map = new Dictionary<IntPtr, String>();
        public Dictionary<IntPtr, PhysicsActor> actor_name_map = new Dictionary<IntPtr, PhysicsActor>();

        private float contactsurfacelayer = 0.002f;

        private int contactsPerCollision = 80;
        internal IntPtr ContactgeomsArray = IntPtr.Zero;
        private IntPtr GlobalContactsArray = IntPtr.Zero;

        const int maxContactsbeforedeath = 4000;
        private volatile int m_global_contactcount = 0;


        private IntPtr contactgroup;

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

        private Dictionary<Vector3, IntPtr> RegionTerrain = new Dictionary<Vector3, IntPtr>();
        private Dictionary<IntPtr, float[]> TerrainHeightFieldHeights = new Dictionary<IntPtr, float[]>();
        private Dictionary<IntPtr, GCHandle> TerrainHeightFieldHeightsHandlers = new Dictionary<IntPtr, GCHandle>();
       
        private int m_physicsiterations = 10;
        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

        // some speedup variables
        private int spaceGridMaxX;
        private int spaceGridMaxY;
        private float spacesPerMeter;

        // split static geometry collision into a grid as before
        private IntPtr[,] staticPrimspace;
        private IntPtr[] staticPrimspaceOffRegion;

        public Object OdeLock;
        private static Object SimulationLock;

        public IMesher mesher;

        private IConfigSource m_config;

        public bool physics_logging = false;
        public int physics_logging_interval = 0;
        public bool physics_logging_append_existing_logfile = false;

        private Vector3 m_worldOffset = Vector3.Zero;
        public Vector2 WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize);
        private PhysicsScene m_parentScene = null;

        private ODERayCastRequestManager m_rayCastManager;


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

            int th = Thread.CurrentThread.ManagedThreadId;
            if(th != threadid)
            {
                threadid = th;
                d.AllocateODEDataForThread(~0U);
            }
        }
 */
        /// <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>
        public OdeScene(string sceneIdentifier)
            {
            m_log 
                = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType.ToString() + "." + sceneIdentifier);

//            checkThread();
            Name = sceneIdentifier;

            OdeLock = new Object();
            SimulationLock = new Object();

            nearCallback = near;

            m_rayCastManager = new ODERayCastRequestManager(this);
            lock (OdeLock)
                {
                // Create the world and the first space
                try
                    {
                    world = d.WorldCreate();
                    TopSpace = d.HashSpaceCreate(IntPtr.Zero);

                    // now the major subspaces
                    ActiveSpace = d.HashSpaceCreate(TopSpace);
                    StaticSpace = d.HashSpaceCreate(TopSpace);
                    }
                catch
                    {
                    // i must RtC#FM 
                    }

                d.HashSpaceSetLevels(TopSpace, -2, 8);
                d.HashSpaceSetLevels(ActiveSpace, -2, 8);
                d.HashSpaceSetLevels(StaticSpace, -2, 8);

                // demote to second level
                d.SpaceSetSublevel(ActiveSpace, 1);
                d.SpaceSetSublevel(StaticSpace, 1);

                d.GeomSetCategoryBits(ActiveSpace, (uint)(CollisionCategories.Space |
                                                        CollisionCategories.Geom |
                                                        CollisionCategories.Character |
                                                        CollisionCategories.Phantom |
                                                        CollisionCategories.VolumeDtc
                                                        ));
                d.GeomSetCollideBits(ActiveSpace, 0);
                d.GeomSetCategoryBits(StaticSpace, (uint)(CollisionCategories.Space |
                                                        CollisionCategories.Geom |
                                                        CollisionCategories.Land |
                                                        CollisionCategories.Water |
                                                        CollisionCategories.Phantom |
                                                        CollisionCategories.VolumeDtc
                                                        ));
                d.GeomSetCollideBits(StaticSpace, 0);

                contactgroup = d.JointGroupCreate(0);
                //contactgroup

                d.WorldSetAutoDisableFlag(world, false);
            }
        }

        // Initialize the mesh plugin
//        public override void Initialise(IMesher meshmerizer, IConfigSource config, RegionInfo region )
        public override void Initialise(IMesher meshmerizer, IConfigSource config)
        {
//            checkThread();
            mesher = meshmerizer;
            m_config = config;

            string ode_config = d.GetConfiguration();
            if (ode_config != null && ode_config != "")
            {
                m_log.WarnFormat("ODE configuration: {0}", ode_config);

                if (ode_config.Contains("ODE_Ubit"))
                {
                    OdeUbitLib = true;
                }
            }

            /*
                        if (region != null)
                        {
                            WorldExtents.X = region.RegionSizeX;
                            WorldExtents.Y = region.RegionSizeY;
                        }
             */

            // Defaults

            int contactsPerCollision = 80;

            if (m_config != null)
            {
                IConfig 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);

                    metersInSpace = physicsconfig.GetFloat("meters_in_small_space", metersInSpace);

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

                    ODE_STEPSIZE = physicsconfig.GetFloat("world_stepsize", ODE_STEPSIZE);
                    m_physicsiterations = physicsconfig.GetInt("world_internal_steps_without_collisions", m_physicsiterations);

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

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

                    geomContactPointsStartthrottle = physicsconfig.GetInt("geom_contactpoints_start_throttling", 3);
                    geomUpdatesPerThrottledUpdate = physicsconfig.GetInt("geom_updates_before_throttled_update", 15);
//                    geomCrossingFailuresBeforeOutofbounds = physicsconfig.GetInt("geom_crossing_failures_before_outofbounds", 5);

                    geomDefaultDensity = physicsconfig.GetFloat("geometry_default_density", geomDefaultDensity);
                    bodyFramesAutoDisable = physicsconfig.GetInt("body_frames_auto_disable", bodyFramesAutoDisable);
/*
                    bodyPIDD = physicsconfig.GetFloat("body_pid_derivative", bodyPIDD);
                    bodyPIDG = physicsconfig.GetFloat("body_pid_gain", bodyPIDG);
*/
                    forceSimplePrimMeshing = physicsconfig.GetBoolean("force_simple_prim_meshing", forceSimplePrimMeshing);
                    meshSculptedPrim = physicsconfig.GetBoolean("mesh_sculpted_prim", meshSculptedPrim);
                    meshSculptLOD = physicsconfig.GetFloat("mesh_lod", meshSculptLOD);
                    MeshSculptphysicalLOD = physicsconfig.GetFloat("mesh_physical_lod", MeshSculptphysicalLOD);
/*
                    if (Environment.OSVersion.Platform == PlatformID.Unix)
                    {
                        avPIDD = physicsconfig.GetFloat("av_pid_derivative_linux", avPIDD);
                        avPIDP = physicsconfig.GetFloat("av_pid_proportional_linux", avPIDP);
                    }
                    else
                    {
 
                        avPIDD = physicsconfig.GetFloat("av_pid_derivative_win", avPIDD);
                        avPIDP = physicsconfig.GetFloat("av_pid_proportional_win", avPIDP);
                    }
*/
                    physics_logging = physicsconfig.GetBoolean("physics_logging", false);
                    physics_logging_interval = physicsconfig.GetInt("physics_logging_interval", 0);
                    physics_logging_append_existing_logfile = physicsconfig.GetBoolean("physics_logging_append_existing_logfile", false);

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

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

            ContactgeomsArray = Marshal.AllocHGlobal(contactsPerCollision * d.ContactGeom.unmanagedSizeOf);
            GlobalContactsArray = GlobalContactsArray = Marshal.AllocHGlobal(maxContactsbeforedeath * d.Contact.unmanagedSizeOf);

            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;

            // Set the gravity,, don't disable things automatically (we set it explicitly on some things)

            d.WorldSetGravity(world, gravityx, gravityy, gravityz);
            d.WorldSetContactSurfaceLayer(world, contactsurfacelayer);

            d.WorldSetLinearDamping(world, 0.002f);
            d.WorldSetAngularDamping(world, 0.002f);
            d.WorldSetAngularDampingThreshold(world, 0f);
            d.WorldSetLinearDampingThreshold(world, 0f);
            d.WorldSetMaxAngularSpeed(world, 100f);

            d.WorldSetCFM(world,1e-6f); // a bit harder than default
            //d.WorldSetCFM(world, 1e-4f); // a bit harder than default
            d.WorldSetERP(world, 0.6f); // higher than original

            // Set how many steps we go without running collision testing
            // This is in addition to the step size.
            // Essentially Steps * m_physicsiterations
            d.WorldSetQuickStepNumIterations(world, m_physicsiterations);

            d.WorldSetContactMaxCorrectingVel(world, 60.0f);

            spacesPerMeter = 1 / metersInSpace;
            spaceGridMaxX = (int)(WorldExtents.X * spacesPerMeter);
            spaceGridMaxY = (int)(WorldExtents.Y * spacesPerMeter);

            staticPrimspace = new IntPtr[spaceGridMaxX, spaceGridMaxY];

            // create all spaces now
            int i, j;
            IntPtr newspace;

            for (i = 0; i < spaceGridMaxX; i++)
                for (j = 0; j < spaceGridMaxY; j++)
                {
                    newspace = d.HashSpaceCreate(StaticSpace);
                    d.GeomSetCategoryBits(newspace, (int)CollisionCategories.Space);
                    waitForSpaceUnlock(newspace);
                    d.SpaceSetSublevel(newspace, 2);
                    d.HashSpaceSetLevels(newspace, -2, 8);
                    d.GeomSetCategoryBits(newspace, (uint)(CollisionCategories.Space |
                                        CollisionCategories.Geom |
                                        CollisionCategories.Land |
                                        CollisionCategories.Water |
                                        CollisionCategories.Phantom |
                                        CollisionCategories.VolumeDtc
                                        ));
                    d.GeomSetCollideBits(newspace, 0);

                    staticPrimspace[i, j] = newspace;
                }
            // let this now be real maximum values
            spaceGridMaxX--;
            spaceGridMaxY--;

            // create 4 off world spaces (x<0,x>max,y<0,y>max)
            staticPrimspaceOffRegion = new IntPtr[4];

            for (i = 0; i < 4; i++)
                {
                    newspace = d.HashSpaceCreate(StaticSpace);
                    d.GeomSetCategoryBits(newspace, (int)CollisionCategories.Space);
                    waitForSpaceUnlock(newspace);
                    d.SpaceSetSublevel(newspace, 2);
                    d.HashSpaceSetLevels(newspace, -2, 8);
                    d.GeomSetCategoryBits(newspace, (uint)(CollisionCategories.Space |
                                        CollisionCategories.Geom |
                                        CollisionCategories.Land |
                                        CollisionCategories.Water |
                                        CollisionCategories.Phantom |
                                        CollisionCategories.VolumeDtc
                                        ));
                    d.GeomSetCollideBits(newspace, 0);

                    staticPrimspaceOffRegion[i] = newspace;
                }

            m_lastframe = DateTime.UtcNow;
        }

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

        #region Collision Detection

        // sets a global contact for a joint for contactgeom , and base contact description)

        private IntPtr CreateContacJoint(ref d.ContactGeom contactGeom, float mu, float bounce, float cfm, float erpscale, float dscale)
        {
            if (GlobalContactsArray == IntPtr.Zero || m_global_contactcount >= maxContactsbeforedeath)
                return IntPtr.Zero;

            float erp = contactGeom.depth;
            erp *= erpscale;
            if (erp < minERP)
                erp = minERP;
            else if (erp > MaxERP)
                erp = MaxERP;

            float depth = contactGeom.depth * dscale;
            if (depth > 0.5f)
                depth = 0.5f;

            d.Contact newcontact = new d.Contact();
            newcontact.geom.depth = depth;
            newcontact.geom.g1 = contactGeom.g1;
            newcontact.geom.g2 = contactGeom.g2;
            newcontact.geom.pos = contactGeom.pos;
            newcontact.geom.normal = contactGeom.normal;
            newcontact.geom.side1 = contactGeom.side1;
            newcontact.geom.side2 = contactGeom.side2;

            // this needs bounce also
            newcontact.surface.mode = comumContactFlags;
            newcontact.surface.mu = mu;
            newcontact.surface.bounce = bounce;
            newcontact.surface.soft_cfm = cfm;
            newcontact.surface.soft_erp = erp;

            IntPtr contact = new IntPtr(GlobalContactsArray.ToInt64() + (Int64)(m_global_contactcount * d.Contact.unmanagedSizeOf));
            Marshal.StructureToPtr(newcontact, contact, true);
            return d.JointCreateContactPtr(world, contactgroup, contact);
        }

        private bool GetCurContactGeom(int index, ref d.ContactGeom newcontactgeom)
        {
            if (ContactgeomsArray == IntPtr.Zero || index >= contactsPerCollision)
                return false;

            IntPtr contactptr = new IntPtr(ContactgeomsArray.ToInt64() + (Int64)(index * d.ContactGeom.unmanagedSizeOf));
            newcontactgeom = (d.ContactGeom)Marshal.PtrToStructure(contactptr, typeof(d.ContactGeom));
            return true;
        }

        /// <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 near(IntPtr space, IntPtr g1, IntPtr g2)
        {
            //  no lock here!  It's invoked from within Simulate(), which is thread-locked

            if (m_global_contactcount >= maxContactsbeforedeath)
                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 (d.GeomIsSpace(g1) || d.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
                {
                    d.SpaceCollide2(g1, g2, IntPtr.Zero, nearCallback);
                }
                catch (AccessViolationException)
                {
                    m_log.Warn("[PHYSICS]: Unable to collide test a space");
                    return;
                }
                //here one should check collisions of geoms inside a space
                // but on each space we only should have geoms that not colide amoung each other
                // so we don't dig inside spaces
                return;
            }

            // get geom bodies to check if we already a joint contact
            // guess this shouldn't happen now
            IntPtr b1 = d.GeomGetBody(g1);
            IntPtr b2 = d.GeomGetBody(g2);

            // d.GeomClassID id = d.GeomGetClass(g1);

            // Figure out how many contact points we have
            int count = 0;
            try
            {
                // Colliding Geom To Geom
                // This portion of the function 'was' blatantly ripped off from BoxStack.cs

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

                if (b1 != IntPtr.Zero && b2 != IntPtr.Zero && d.AreConnectedExcluding(b1, b2, d.JointType.Contact))
                    return;
                if(d.GeomGetCategoryBits(g1) == (uint)CollisionCategories.VolumeDtc ||
                    d.GeomGetCategoryBits(g1) == (uint)CollisionCategories.VolumeDtc)
                {
                    int cflags;
                    unchecked
                    {
                        cflags = (int)(1 | d.CONTACTS_UNIMPORTANT);
                    }
                    count = d.CollidePtr(g1, g2, cflags, ContactgeomsArray, d.ContactGeom.unmanagedSizeOf);
                }
                else
                    count = d.CollidePtr(g1, g2, (contactsPerCollision & 0xffff), ContactgeomsArray, d.ContactGeom.unmanagedSizeOf);
            }
            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;
            }

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

            // try get physical actors 
            PhysicsActor p1;
            PhysicsActor p2;

            if (!actor_name_map.TryGetValue(g1, out p1))
            {
                p1 = PANull;
            }

            if (!actor_name_map.TryGetValue(g2, out p2))
            {
                p2 = PANull;
            }

            // update actors collision score
            if (p1.CollisionScore >= float.MaxValue - count)
                p1.CollisionScore = 0;
            p1.CollisionScore += count;

            if (p2.CollisionScore >= float.MaxValue - count)
                p2.CollisionScore = 0;
            p2.CollisionScore += count;


            // get first contact
            d.ContactGeom curContact = new d.ContactGeom();
            if (!GetCurContactGeom(0, ref curContact))
                return;
            // for now it's the one with max depth
            ContactPoint maxDepthContact = new ContactPoint(
                        new Vector3(curContact.pos.X, curContact.pos.Y, curContact.pos.Z),
                        new Vector3(curContact.normal.X, curContact.normal.Y, curContact.normal.Z),
                        curContact.depth
                        );
            // do volume detection case
            if (
                (p1.IsVolumeDtc || p2.IsVolumeDtc))
            {
                collision_accounting_events(p1, p2, maxDepthContact);
                return;
            }

            // big messy collision analises

            Vector3 normoverride = Vector3.Zero; //damm c#

            float mu = 0;
            float bounce = 0;
            float cfm = 0.0001f;
            float erpscale = 1.0f;
            float dscale = 1.0f;
            bool IgnoreNegSides = false;

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

            String name = null;
            bool dop1foot = false;
            bool dop2foot = false;
            bool ignore = false;
            bool AvanormOverride = false;

            switch (p1.PhysicsActorType)
            {
                case (int)ActorTypes.Agent:
                    {
                        AvanormOverride = true;
                        Vector3 tmp = p2.Position - p1.Position;
                        normoverride = p2.Velocity - p1.Velocity;
                        mu = normoverride.LengthSquared();

                        if (mu > 1e-6)
                        {
                            mu = 1.0f / (float)Math.Sqrt(mu);
                            normoverride *= mu;
                            mu = Vector3.Dot(tmp, normoverride);
                            if (mu > 0)
                                normoverride *= -1;
                        }
                        else
                        {
                            tmp.Normalize();
                            normoverride = -tmp;
                        }

                        switch (p2.PhysicsActorType)
                        {
                            case (int)ActorTypes.Agent:
/*
                                p1.getContactData(ref contactdata1);
                                p2.getContactData(ref contactdata2);

                                mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu);

                                if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f))
                                    mu *= frictionMovementMult;
*/
                                p1.CollidingObj = true;
                                p2.CollidingObj = true;
                                break;
                            case (int)ActorTypes.Prim:
/*
                                p1.getContactData(ref contactdata1);
                                p2.getContactData(ref contactdata2);


                                mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu);

                                if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f))
                                    mu *= frictionMovementMult;
 */
                                if (p2.Velocity.LengthSquared() > 0.0f)
                                    p2.CollidingObj = true;

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

                case (int)ActorTypes.Prim:
                    switch (p2.PhysicsActorType)
                    {
                        case (int)ActorTypes.Agent:
                            //                            p1.getContactData(ref contactdata1);
                            //                            p2.getContactData(ref contactdata2);

                            AvanormOverride = true;

                            Vector3 tmp = p2.Position - p1.Position;
                            normoverride = p2.Velocity - p1.Velocity;
                            mu = normoverride.LengthSquared();
                            if (mu > 1e-6)
                            {
                                mu = 1.0f / (float)Math.Sqrt(mu);
                                normoverride *= mu;
                                mu = Vector3.Dot(tmp, normoverride);
                                if (mu > 0)
                                    normoverride *= -1;
                            }
                            else
                            {
                                tmp.Normalize();
                                normoverride = -tmp;
                            }

                            bounce = 0;
                            mu = 0;
                            cfm = 0.0001f;
                            /*
                                                        mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu);

                                                        if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f))
                                                            mu *= frictionMovementMult;
                            */
                            dop2foot = true;
                            if (p1.Velocity.LengthSquared() > 0.0f)
                                p1.CollidingObj = true;
                            break;
                        case (int)ActorTypes.Prim:
                            if ((p1.Velocity - p2.Velocity).LengthSquared() > 0.0f)
                            {
                                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);

                            cfm = p1.Mass;
                            if (cfm > p2.Mass)
                                cfm = p2.Mass;
                            dscale = 10 / cfm;
                            dscale = (float)Math.Sqrt(dscale);
                            if (dscale > 1.0f)
                                dscale = 1.0f;
                            erpscale = cfm * 0.01f;
                            cfm = 0.0001f / cfm;
                            if (cfm > 0.01f)
                                cfm = 0.01f;
                            else if (cfm < 0.00001f)
                                cfm = 0.00001f;

                            if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f))
                                mu *= frictionMovementMult;

                            break;
                        default:
                            if (geom_name_map.TryGetValue(g2, out name))
                            {
                                if (name == "Terrain")
                                {
                                    p1.getContactData(ref contactdata1);
                                    bounce = contactdata1.bounce * TerrainBounce;
                                    mu = (float)Math.Sqrt(contactdata1.mu * TerrainFriction);
                                    if (Math.Abs(p1.Velocity.X) > 0.1f || Math.Abs(p1.Velocity.Y) > 0.1f)
                                        mu *= frictionMovementMult;
                                    p1.CollidingGround = true;

                                    cfm = p1.Mass;
                                    dscale = 10 / cfm;
                                    dscale = (float)Math.Sqrt(dscale);
                                    if (dscale > 1.0f)
                                        dscale = 1.0f;
                                    erpscale = cfm * 0.01f;
                                    cfm = 0.0001f / cfm;
                                    if (cfm > 0.01f)
                                        cfm = 0.01f;
                                    else if (cfm < 0.00001f)
                                        cfm = 0.00001f;

                                    if (d.GeomGetClass(g1) == d.GeomClassID.TriMeshClass)
                                    {
                                        if (curContact.side1 > 0)
                                            IgnoreNegSides = true;
                                    }

                                }
                                else if (name == "Water")
                                {
                                    ignore = true;
                                }
                            }
                            else
                                ignore = true;
                            break;
                    }
                    break;

                default:
                    if (geom_name_map.TryGetValue(g1, out name))
                    {
                        if (name == "Terrain")
                        {
                            if (p2.PhysicsActorType == (int)ActorTypes.Prim)
                            {
                                p2.CollidingGround = true;
                                p2.getContactData(ref contactdata2);
                                bounce = contactdata2.bounce * TerrainBounce;
                                mu = (float)Math.Sqrt(contactdata2.mu * TerrainFriction);

                                cfm = p2.Mass;
                                dscale = 10 / cfm;
                                dscale = (float)Math.Sqrt(dscale);

                                if (dscale > 1.0f)
                                    dscale = 1.0f;

                                erpscale = cfm * 0.01f;
                                cfm = 0.0001f / cfm;
                                if (cfm > 0.01f)
                                    cfm = 0.01f;
                                else if (cfm < 0.00001f)
                                    cfm = 0.00001f;

                                if (curContact.side1 > 0) // should be 2 ?
                                    IgnoreNegSides = true;

                                if (Math.Abs(p2.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y) > 0.1f)
                                    mu *= frictionMovementMult;
                            }
                            else
                                ignore = true;

                        }
                        else if (name == "Water" &&
                            (p2.PhysicsActorType == (int)ActorTypes.Prim || p2.PhysicsActorType == (int)ActorTypes.Agent))
                        {
                            ignore = true;
                        }
                    }
                    else
                        ignore = true;
                    break;
            }

            if (ignore)
                return;

            IntPtr Joint;

            int i = 0;
            while(true)
            {

                if (IgnoreNegSides && curContact.side1 < 0)
                {
                    if (++i >= count)
                        break;

                    if (!GetCurContactGeom(i, ref curContact))
                        break;
                }
                else

                {

                    if (AvanormOverride)
                    {
                        if (curContact.depth > 0.3f)
                        {
                            if (dop1foot && (p1.Position.Z - curContact.pos.Z) > (p1.Size.Z - avCapRadius) * 0.5f)
                                p1.IsColliding = true;
                            if (dop2foot && (p2.Position.Z - curContact.pos.Z) > (p2.Size.Z - avCapRadius) * 0.5f)
                                p2.IsColliding = true;
                            curContact.normal.X = normoverride.X;
                            curContact.normal.Y = normoverride.Y;
                            curContact.normal.Z = normoverride.Z;
                        }

                        else
                        {
                            if (dop1foot)
                            {
                                float sz = p1.Size.Z;
                                Vector3 vtmp = p1.Position;
                                float ppos = curContact.pos.Z - vtmp.Z + (sz - avCapRadius) * 0.5f;
                                if (ppos > 0f)
                                {
                                    if (!p1.Flying)
                                    {
                                        d.AABB aabb;
                                        d.GeomGetAABB(g2, out aabb);
                                        float tmp = vtmp.Z - sz * .25f;

                                        if (aabb.MaxZ < tmp)
                                        {
                                            vtmp.X = curContact.pos.X - vtmp.X;
                                            vtmp.Y = curContact.pos.Y - vtmp.Y;
                                            vtmp.Z = -0.2f;
                                            vtmp.Normalize();
                                            curContact.normal.X = vtmp.X;
                                            curContact.normal.Y = vtmp.Y;
                                            curContact.normal.Z = vtmp.Z;
                                        }
                                    }
                                }
                                else
                                    p1.IsColliding = true;

                            }

                            if (dop2foot)
                            {
                                float sz = p2.Size.Z;
                                Vector3 vtmp = p2.Position;
                                float ppos = curContact.pos.Z - vtmp.Z + (sz - avCapRadius) * 0.5f;
                                if (ppos > 0f)
                                {
                                    if (!p2.Flying)
                                    {
                                        d.AABB aabb;
                                        d.GeomGetAABB(g1, out aabb);
                                        float tmp = vtmp.Z - sz * .25f;

                                        if (aabb.MaxZ < tmp)
                                        {
                                            vtmp.X = curContact.pos.X - vtmp.X;
                                            vtmp.Y = curContact.pos.Y - vtmp.Y;
                                            vtmp.Z = -0.2f;
                                            vtmp.Normalize();
                                            curContact.normal.X = vtmp.X;
                                            curContact.normal.Y = vtmp.Y;
                                            curContact.normal.Z = vtmp.Z;
                                        }
                                    }
                                }
                                else
                                    p2.IsColliding = true;

                            }
                        }
                    }

                    Joint = CreateContacJoint(ref curContact, mu, bounce, cfm, erpscale, dscale);
                    d.JointAttach(Joint, b1, b2);

                    if (++m_global_contactcount >= maxContactsbeforedeath)
                        break;

                    if (++i >= count)
                        break;

                    if (!GetCurContactGeom(i, ref curContact))
                        break;

                    if (curContact.depth > maxDepthContact.PenetrationDepth)
                    {
                        maxDepthContact.Position.X = curContact.pos.X;
                        maxDepthContact.Position.Y = curContact.pos.Y;
                        maxDepthContact.Position.Z = curContact.pos.Z;
                        maxDepthContact.SurfaceNormal.X = curContact.normal.X;
                        maxDepthContact.SurfaceNormal.Y = curContact.normal.Y;
                        maxDepthContact.SurfaceNormal.Z = curContact.normal.Z;
                        maxDepthContact.PenetrationDepth = curContact.depth;
                    }
                }
            }

            collision_accounting_events(p1, p2, maxDepthContact);

/*
            if (notskipedcount > geomContactPointsStartthrottle)
            {
                // If there are more then 3 contact points, it's likely
                // that we've got a pile of objects, so ...
                // We don't want to send out hundreds of terse updates over and over again
                // so lets throttle them and send them again after it's somewhat sorted out.
                 this needs checking so out for now
                                if (b1 != IntPtr.Zero)
                                    p1.ThrottleUpdates = true;
                                if (b2 != IntPtr.Zero)
                                    p2.ThrottleUpdates = true;
                
            }
 */
        }            

        private void collision_accounting_events(PhysicsActor p1, PhysicsActor p2, ContactPoint contact)
            {
            
            OdeCharacter cc1;
            OdePrim cp1;
            OdeCharacter cc2;
            OdePrim cp2;

            uint obj2LocalID = 0;
            bool p1events = p1.SubscribedEvents();
            bool p2events = p2.SubscribedEvents();
           
            if (p1.IsVolumeDtc)
                p2events = false;
            if (p2.IsVolumeDtc)
                p1events = false;

            if (!(p2events || p1events))
                return;

            if (p1events)
                AddCollisionEventReporting(p1);

            if (p2events)
                AddCollisionEventReporting(p2);

            Vector3 vel = Vector3.Zero;
            if (p2 != null && p2.IsPhysical)
                vel = p2.Velocity;

            if (p1 != null && p1.IsPhysical)
                vel -= p1.Velocity;

            contact.RelativeSpeed = Vector3.Dot(vel, contact.SurfaceNormal);
             
            switch ((ActorTypes)p1.PhysicsActorType)
                {
                case ActorTypes.Agent:
                    cc1 = (OdeCharacter)p1;
                    switch ((ActorTypes)p2.PhysicsActorType)
                        {
                        case ActorTypes.Agent:
                            cc2 = (OdeCharacter)p2;
                            obj2LocalID = cc2.m_localID;
                            if (p2events)
                                cc2.AddCollisionEvent(cc1.m_localID, contact);
                            break;

                        case ActorTypes.Prim:
                            if (p2 is OdePrim)
                                {
                                cp2 = (OdePrim)p2;
                                obj2LocalID = cp2.m_localID;
                                if (p2events)
                                    cp2.AddCollisionEvent(cc1.m_localID, contact);
                                }
                            break;

                        case ActorTypes.Ground:
                        case ActorTypes.Unknown:
                        default:
                            obj2LocalID = 0;
                            break;
                        }
                    if (p1events)
                        {
                        contact.SurfaceNormal = -contact.SurfaceNormal;
                        cc1.AddCollisionEvent(obj2LocalID, contact);
                        }
                    break;

                case ActorTypes.Prim:

                    if (p1 is OdePrim)
                        {
                        cp1 = (OdePrim)p1;

                        // obj1LocalID = cp2.m_localID;
                        switch ((ActorTypes)p2.PhysicsActorType)
                            {
                            case ActorTypes.Agent:
                                if (p2 is OdeCharacter)
                                    {
                                    cc2 = (OdeCharacter)p2;
                                    obj2LocalID = cc2.m_localID;
                                    if (p2events)
                                        cc2.AddCollisionEvent(cp1.m_localID, contact);
                                    }
                                break;
                            case ActorTypes.Prim:

                                if (p2 is OdePrim)
                                    {
                                    cp2 = (OdePrim)p2;
                                    obj2LocalID = cp2.m_localID;
                                    if (p2events)
                                        cp2.AddCollisionEvent(cp1.m_localID, contact);
                                    }
                                break;

                            case ActorTypes.Ground:
                            case ActorTypes.Unknown:
                            default:
                                obj2LocalID = 0;
                                break;
                            }
                        if (p1events)
                            {
                            contact.SurfaceNormal = -contact.SurfaceNormal;
                            cp1.AddCollisionEvent(obj2LocalID, contact);
                            }
                        }
                    break;
                case ActorTypes.Ground:
                case ActorTypes.Unknown:
                default:
                        switch ((ActorTypes)p2.PhysicsActorType)
                        {
                            case ActorTypes.Agent:
                                if (p2 is OdeCharacter)
                                {
                                    cc2 = (OdeCharacter)p2;
                                    obj2LocalID = cc2.m_localID;
                                    if (p2events)
                                        cc2.AddCollisionEvent(0, contact);
                                }
                                break;
                            case ActorTypes.Prim:
                                if (p2 is OdePrim)
                                {
                                    cp2 = (OdePrim)p2;
                                    obj2LocalID = cp2.m_localID;
                                    if (p2events)
                                        cp2.AddCollisionEvent(0, contact);
                                }
                                break;
                        }
                        break;
                }
            }

        /// <summary>
        /// This is our collision testing routine in ODE
        /// </summary>
        /// <param name="timeStep"></param>
        private void collision_optimized()
        {
            lock (_characters)
                {
                try
                {
                    foreach (OdeCharacter chr in _characters)
                    {
                        if (chr == null || chr.Shell == IntPtr.Zero || chr.Body == IntPtr.Zero)
                            continue;

                        chr.IsColliding = false;
                        //                    chr.CollidingGround = false; not done here
                        chr.CollidingObj = false;
                        // do colisions with static space
                        d.SpaceCollide2(StaticSpace, chr.Shell, IntPtr.Zero, nearCallback);
                    }
                }
                catch (AccessViolationException)
                {
                    m_log.Warn("[PHYSICS]: Unable to collide Character to static space");
                }

            }

            lock (_activeprims)
            {
                foreach (OdePrim aprim in _activeprims)
                {
                    aprim.CollisionScore = 0;
                    aprim.IsColliding = false;
                }
            }

            // collide active prims with static enviroment
            lock (_activegroups)
            {
                try
                {
                    foreach (OdePrim prm in _activegroups)
                    {
                        if (d.BodyIsEnabled(prm.Body) && !prm.m_outbounds)
                            d.SpaceCollide2(StaticSpace, prm.collide_geom, IntPtr.Zero, nearCallback);
                    }
                }
                catch (AccessViolationException)
                {
                    m_log.Warn("[PHYSICS]: Unable to collide Active prim to static space");
                }
            }
            // finally colide active things amoung them
            try
            {
                d.SpaceCollide(ActiveSpace, IntPtr.Zero, nearCallback);
            }
            catch (AccessViolationException)
            {
                m_log.Warn("[PHYSICS]: Unable to collide in Active space");
            }
//            _perloopContact.Clear();
        }

        #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)
        {
            if (!_collisionEventPrim.Contains(obj))
                _collisionEventPrim.Add(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)
        {
            if (_collisionEventPrim.Contains(obj) && !_collisionEventPrimRemove.Contains(obj))
                _collisionEventPrimRemove.Add(obj);
        }


        #region Add/Remove Entities

        public override PhysicsActor AddAvatar(string avName, Vector3 position, Vector3 size, bool isFlying)
        {
            Vector3 pos;
            pos.X = position.X;
            pos.Y = position.Y;
            pos.Z = position.Z;
            OdeCharacter newAv = new OdeCharacter(avName, this, pos, size, avPIDD, avPIDP, avCapRadius, avDensity, avMovementDivisorWalk, avMovementDivisorRun);
            newAv.Flying = isFlying;
            newAv.MinimumGroundFlightOffset = minimumGroundFlightOffset;
            
            return newAv;
        }

        public void AddCharacter(OdeCharacter chr)
        {
            lock (_characters)
            {
                if (!_characters.Contains(chr))
                {
                    _characters.Add(chr);
                    if (chr.bad)
                        m_log.DebugFormat("[PHYSICS] Added BAD actor {0} to characters list", chr.m_uuid);
                }
            }
        }

        public void RemoveCharacter(OdeCharacter chr)
        {
            lock (_characters)
            {
                if (_characters.Contains(chr))
                {
                    _characters.Remove(chr);
                }
            }
        }

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

        public override void RemoveAvatar(PhysicsActor actor)
        {
            //m_log.Debug("[PHYSICS]:ODELOCK");
            ((OdeCharacter) actor).Destroy();
        }

        private PhysicsActor AddPrim(String name, Vector3 position, Vector3 size, Quaternion rotation,
                                     PrimitiveBaseShape pbs, bool isphysical, uint localID)
        {
            Vector3 pos = position;
            Vector3 siz = size;
            Quaternion rot = rotation;

            OdePrim newPrim;
            lock (OdeLock)
            {
                newPrim = new OdePrim(name, this, pos, siz, rot, pbs, isphysical,false,0,localID);

                lock (_prims)
                    _prims.Add(newPrim);
            }
            return newPrim;
        }

        private PhysicsActor AddPrim(String name, Vector3 position, Vector3 size, Quaternion rotation,
                                     PrimitiveBaseShape pbs, bool isphysical, bool isPhantom, uint localID)
        {
            Vector3 pos = position;
            Vector3 siz = size;
            Quaternion rot = rotation;

            OdePrim newPrim;
            lock (OdeLock)
            {
                newPrim = new OdePrim(name, this, pos, siz, rot, pbs, isphysical, isPhantom, 0, localID);

                lock (_prims)
                    _prims.Add(newPrim);
            }
            return newPrim;
        }

        private PhysicsActor AddPrim(String name, Vector3 position, Vector3 size, Quaternion rotation,
                                     PrimitiveBaseShape pbs, bool isphysical, bool isPhantom, byte shapeType, uint localID)
        {
            Vector3 pos = position;
            Vector3 siz = size;
            Quaternion rot = rotation;

            OdePrim newPrim;
            lock (OdeLock)
            {
                newPrim = new OdePrim(name, this, pos, siz, rot, pbs, isphysical, isPhantom, shapeType, localID);

                lock (_prims)
                    _prims.Add(newPrim);
            }
            return newPrim;
        }

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

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

        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, localid);
        }


        public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
                                                  Vector3 size, Quaternion rotation, bool isPhysical, uint localid)
        {
#if SPAM
            m_log.DebugFormat("[PHYSICS]: Adding physics actor to {0}", primName);
#endif

            return AddPrim(primName, position, size, rotation, pbs, isPhysical, localid);
        }

        public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
                                                  Vector3 size, Quaternion rotation, bool isPhysical, bool isPhantom, byte shapeType, uint localid)
        {
#if SPAM
            m_log.DebugFormat("[PHYSICS]: Adding physics actor to {0}", primName);
#endif

            return AddPrim(primName, position, size, rotation, pbs, isPhysical,isPhantom, shapeType, localid);
        }

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

        public override bool SupportsNINJAJoints
        {
            get { return false; }
        }


        public void remActivePrim(OdePrim deactivatePrim)
        {
            lock (_activeprims)
            {
                _activeprims.Remove(deactivatePrim);
            }
        }
        public void remActiveGroup(OdePrim deactivatePrim)
        {
            lock (_activegroups)
            {
                _activegroups.Remove(deactivatePrim);
            }
        }

        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)
            {
//                lock (OdeLock)
                {
                    
                    OdePrim p = (OdePrim)prim;
                    p.setPrimForRemoval();
                }
            }
        }
        /// <summary>
        /// This is called from within simulate but outside the locked portion
        /// We need to do our own locking here
        /// (Note: As of 20110801 this no longer appears to be true - this is being called within lock (odeLock) in
        /// Simulate() -- justincc).
        ///
        /// Essentially, we need to remove the prim from our space segment, whatever segment it's in.
        ///
        /// If there are no more prim in the segment, we need to empty (spacedestroy)the segment and reclaim memory
        /// that the space was using.
        /// </summary>
        /// <param name="prim"></param>
        public void RemovePrimThreadLocked(OdePrim prim)
        {
            //Console.WriteLine("RemovePrimThreadLocked " +  prim.m_primName);
            lock (prim)
            {
                RemoveCollisionEventReporting(prim);
                lock (_prims)
                    _prims.Remove(prim);
            }

        }
        #endregion

        #region Space Separation Calculation

        /// <summary>
        /// Called when a static prim moves or becomes static
        /// Places the prim in a space one the static sub-spaces grid
        /// </summary>
        /// <param name="geom">the pointer to the geom that moved</param>
        /// <param name="pos">the position that the geom moved to</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, Vector3 pos, IntPtr currentspace)
        {
            // moves a prim into another static sub-space or from another space into a static sub-space

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

            if (geom == IntPtr.Zero) // shouldn't happen
                return IntPtr.Zero;

            // get the static sub-space for current position
            IntPtr newspace = calculateSpaceForGeom(pos);

            if (newspace == currentspace) // if we are there all done
                return newspace;

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

                    if (d.SpaceGetSublevel(currentspace) > 2 && d.SpaceGetNumGeoms(currentspace) == 0)
                    {
                        d.SpaceDestroy(currentspace);
                    }
                }
                else
                {
                    m_log.Info("[Physics]: Invalid or empty Space passed to 'MoveGeomToStaticSpace':" + currentspace +
                                   " Geom:" + geom);
                }
            }
            else // odd currentspace is null or doesn't contain the geom? lets try the geom ideia of current space
            {
                currentspace = d.GeomGetSpace(geom);
                if (currentspace != IntPtr.Zero)
                {
                    if (d.GeomIsSpace(currentspace))
                    {
                        waitForSpaceUnlock(currentspace);
                        d.SpaceRemove(currentspace, geom);

                        if (d.SpaceGetSublevel(currentspace) > 2 && d.SpaceGetNumGeoms(currentspace) == 0)
                        {
                            d.SpaceDestroy(currentspace);
                        }

                    }
                }
            }

            // put the geom in the newspace
            waitForSpaceUnlock(newspace);
            d.SpaceAdd(newspace, geom);

            // let caller know this newspace
            return newspace;
        }

        /// <summary>
        /// Calculates the space the prim should be in by its position
        /// </summary>
        /// <param name="pos"></param>
        /// <returns>a pointer to the space. This could be a new space or reused space.</returns>
        public IntPtr calculateSpaceForGeom(Vector3 pos)
        {
            int x, y;

            if (pos.X < 0)
                return staticPrimspaceOffRegion[0];

            if (pos.Y < 0)
                return staticPrimspaceOffRegion[2];

            x = (int)(pos.X * spacesPerMeter);
            if (x > spaceGridMaxX)
                return staticPrimspaceOffRegion[1];
            
            y = (int)(pos.Y * spacesPerMeter);
            if (y > spaceGridMaxY)
                return staticPrimspaceOffRegion[3];

            return staticPrimspace[x, y];
        }
 
        #endregion

        /// <summary>
        /// Routine to figure out if we need to mesh this prim with our mesher
        /// </summary>
        /// <param name="pbs"></param>
        /// <returns></returns>
        public bool needsMeshing(PrimitiveBaseShape pbs)
        {
            // check sculpts or meshs 
            if (pbs.SculptEntry)
            {
                if (meshSculptedPrim)
                    return true;

                if (pbs.SculptType == (byte)SculptType.Mesh) // always do meshs
                    return true;

                return false;
            }

            if (forceSimplePrimMeshing)
                return true;

            // if it's a standard box or sphere with no cuts, hollows, twist or top shear, return false since ODE can use an internal representation for the prim

            if ((pbs.ProfileShape == ProfileShape.Square && pbs.PathCurve == (byte)Extrusion.Straight)
                    || (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1
                    && pbs.Scale.X == pbs.Scale.Y && pbs.Scale.Y == pbs.Scale.Z))
            {

                if (pbs.ProfileBegin == 0 && pbs.ProfileEnd == 0
                    && pbs.ProfileHollow == 0
                    && pbs.PathTwist == 0 && pbs.PathTwistBegin == 0
                    && pbs.PathBegin == 0 && pbs.PathEnd == 0
                    && pbs.PathTaperX == 0 && pbs.PathTaperY == 0
                    && pbs.PathScaleX == 100 && pbs.PathScaleY == 100
                    && pbs.PathShearX == 0 && pbs.PathShearY == 0)
                {
#if SPAM
                m_log.Warn("NonMesh");
#endif
                    return false;
                }
            }

            //  following code doesn't give meshs to boxes and spheres ever
            // and it's odd..  so for now just return true if asked to force meshs
            // hopefully mesher will fail if doesn't suport so things still get basic boxes

            int iPropertiesNotSupportedDefault = 0;

            if (pbs.ProfileHollow != 0)
                iPropertiesNotSupportedDefault++;

            if ((pbs.PathBegin != 0) || pbs.PathEnd != 0)
                iPropertiesNotSupportedDefault++;

            if ((pbs.PathTwistBegin != 0) || (pbs.PathTwist != 0))
                iPropertiesNotSupportedDefault++; 

            if ((pbs.ProfileBegin != 0) || pbs.ProfileEnd != 0)
                iPropertiesNotSupportedDefault++;

            if ((pbs.PathScaleX != 100) || (pbs.PathScaleY != 100))
                iPropertiesNotSupportedDefault++;

            if ((pbs.PathShearX != 0) || (pbs.PathShearY != 0))
                iPropertiesNotSupportedDefault++;

            if (pbs.ProfileShape == ProfileShape.Circle && pbs.PathCurve == (byte)Extrusion.Straight)
                iPropertiesNotSupportedDefault++;

            if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1 && (pbs.Scale.X != pbs.Scale.Y || pbs.Scale.Y != pbs.Scale.Z || pbs.Scale.Z != pbs.Scale.X))
                iPropertiesNotSupportedDefault++;

            if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte) Extrusion.Curve1)
                iPropertiesNotSupportedDefault++;

            // test for torus
            if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Square)
            {
                if (pbs.PathCurve == (byte)Extrusion.Curve1)
                {
                    iPropertiesNotSupportedDefault++;
                }
            }
            else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Circle)
            {
                if (pbs.PathCurve == (byte)Extrusion.Straight)
                {
                    iPropertiesNotSupportedDefault++;
                }

                // ProfileCurve seems to combine hole shape and profile curve so we need to only compare against the lower 3 bits
                else if (pbs.PathCurve == (byte)Extrusion.Curve1)
                {
                    iPropertiesNotSupportedDefault++;
                }
            }
            else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.HalfCircle)
            {
                if (pbs.PathCurve == (byte)Extrusion.Curve1 || pbs.PathCurve == (byte)Extrusion.Curve2)
                {
                    iPropertiesNotSupportedDefault++;
                }
            }
            else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.EquilateralTriangle)
            {
                if (pbs.PathCurve == (byte)Extrusion.Straight)
                {
                    iPropertiesNotSupportedDefault++;
                }
                else if (pbs.PathCurve == (byte)Extrusion.Curve1)
                {
                    iPropertiesNotSupportedDefault++;
                }
            }

            if (iPropertiesNotSupportedDefault == 0)
            {
#if SPAM
                m_log.Warn("NonMesh");
#endif
                return false;
            }
#if SPAM
            m_log.Debug("Mesh");
#endif
            return true; 
        }

        /// <summary>
        /// Called to queue a change to a actor
        /// to use in place of old taint mechanism so changes do have a time sequence
        /// </summary>

        public void AddChange(PhysicsActor actor, changes what, Object arg)
        {
            ODEchangeitem item = new ODEchangeitem();
            item.actor = actor;
            item.what = what;
            item.arg = arg;
            ChangesQueue.Enqueue(item);
        }

        /// <summary>
        /// Called after our prim properties are set Scale, position etc.
        /// We use this event queue like method to keep changes to the physical scene occuring in the threadlocked mutex
        /// This assures us that we have no race conditions
        /// </summary>
        /// <param name="prim"></param>
        public override void AddPhysicsActorTaint(PhysicsActor prim)
            {
            }

        /// <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 timeStep)
        {

            DateTime now = DateTime.UtcNow;
            TimeSpan SinceLastFrame = now - m_lastframe;
            m_lastframe = now;
            timeStep = (float)SinceLastFrame.TotalSeconds;
            
            // acumulate time so we can reduce error
            step_time += timeStep;

            if (step_time < HalfOdeStep)
                return 0;

            if (framecount < 0)
                framecount = 0;

            framecount++;

            int curphysiteractions;

            // if in trouble reduce step resolution
            if (step_time >= m_SkipFramesAtms)
                curphysiteractions = m_physicsiterations / 2;
            else
                curphysiteractions = m_physicsiterations;

            int nodeframes = 0;

//            checkThread();

            lock (SimulationLock)
                lock(OdeLock)
            {
                if (world == IntPtr.Zero)
                    return 0;

                // adjust number of iterations per step

//                try
//                {
                    d.WorldSetQuickStepNumIterations(world, curphysiteractions);
/*                }
                catch (StackOverflowException)
                {
                    m_log.Error("[PHYSICS]: The operating system wasn't able to allocate enough memory for the simulation.  Restarting the sim.");
//                    ode.drelease(world);
                    base.TriggerPhysicsBasedRestart();
                }
*/
                while (step_time > HalfOdeStep && nodeframes < 10) //limit number of steps so we don't say here for ever
                {
                    try
                    {
                        // clear pointer/counter to contacts to pass into joints
                        m_global_contactcount = 0;

                        ODEchangeitem item;
                        
                        if(ChangesQueue.Count >0)
                        {
                            int ttmpstart = Util.EnvironmentTickCount();
                            int ttmp;

                            while(ChangesQueue.Dequeue(out item))
                            {
                                if (item.actor != null)
                                {
                                    try
                                    {
                                        if (item.actor is OdeCharacter)
                                            ((OdeCharacter)item.actor).DoAChange(item.what, item.arg);
                                        else if (((OdePrim)item.actor).DoAChange(item.what, item.arg))
                                                RemovePrimThreadLocked((OdePrim)item.actor);
                                    }
                                    catch
                                    {
                                        m_log.Warn("[PHYSICS]: doChange failed for a actor");
                                    };
                                }
                                ttmp = Util.EnvironmentTickCountSubtract(ttmpstart);
                                if (ttmp > 20)
                                    break;
                            }
                        }

                        // Move characters
                        lock (_characters)
                        {
                            List<OdeCharacter> defects = new List<OdeCharacter>();
                            foreach (OdeCharacter actor in _characters)
                            {
                                if (actor != null)
                                    actor.Move(ODE_STEPSIZE, defects);
                            }
                            if (defects.Count != 0)
                            {
                                foreach (OdeCharacter defect in defects)
                                {
                                    RemoveCharacter(defect);
                                }
                                defects.Clear();
                            }
                        }

                        // Move other active objects
                        lock (_activegroups)
                        {
                            foreach (OdePrim aprim in _activegroups)
                            {
                                aprim.Move();
                            }
                        }

                        //if ((framecount % m_randomizeWater) == 0)
                        // randomizeWater(waterlevel);

                        m_rayCastManager.ProcessQueuedRequests();

                        collision_optimized();

                        foreach (PhysicsActor obj in _collisionEventPrim)
                        {
                            if (obj == null)
                                continue;

                            switch ((ActorTypes)obj.PhysicsActorType)
                            {
                                case ActorTypes.Agent:
                                    OdeCharacter cobj = (OdeCharacter)obj;
                                    cobj.AddCollisionFrameTime((int)(odetimestepMS));
                                    cobj.SendCollisions();
                                    break;

                                case ActorTypes.Prim:
                                    OdePrim pobj = (OdePrim)obj;
                                    if (pobj.Body == IntPtr.Zero || (d.BodyIsEnabled(pobj.Body) && !pobj.m_outbounds))
                                    {
                                        pobj.AddCollisionFrameTime((int)(odetimestepMS));
                                        pobj.SendCollisions();
                                    }
                                    break;
                            }
                        }

                        foreach (PhysicsActor obj in _collisionEventPrimRemove)
                            _collisionEventPrim.Remove(obj);

                        _collisionEventPrimRemove.Clear();

                        // do a ode simulation step
                        d.WorldQuickStep(world, ODE_STEPSIZE);
                        d.JointGroupEmpty(contactgroup);

                        // 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();
                                }
                            }
                        }
        */

                        lock (_activegroups)
                        {
                            {
                                foreach (OdePrim actor in _activegroups)
                                {
                                    if (actor.IsPhysical)
                                    {
                                        actor.UpdatePositionAndVelocity();
                                    }
                                }
                            }
                        }
                    }
                    catch (Exception e)
                    {
                        m_log.ErrorFormat("[PHYSICS]: {0}, {1}, {2}", e.Message, e.TargetSite, e);
//                        ode.dunlock(world);
                    }


                    step_time -= ODE_STEPSIZE;
                    nodeframes++;
                }

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

                        _badCharacter.Clear();
                    }
                }
/*
                int nactivegeoms = d.SpaceGetNumGeoms(ActiveSpace);
                int nstaticgeoms = d.SpaceGetNumGeoms(StaticSpace);
                int ntopgeoms = d.SpaceGetNumGeoms(TopSpace);
                int nbodies = d.NTotalBodies;
                int ngeoms = d.NTotalGeoms;
*/
                // Finished with all sim stepping. If requested, dump world state to file for debugging.
                // TODO: This call to the export function is already inside lock (OdeLock) - but is an extra lock needed?
                // TODO: This overwrites all dump files in-place. Should this be a growing logfile, or separate snapshots?
                if (physics_logging && (physics_logging_interval > 0) && (framecount % physics_logging_interval == 0))
                {
                    string fname = "state-" + world.ToString() + ".DIF"; // give each physics world a separate filename
                    string prefix = "world" + world.ToString(); // prefix for variable names in exported .DIF file

                    if (physics_logging_append_existing_logfile)
                    {
                        string header = "-------------- START OF PHYSICS FRAME " + framecount.ToString() + " --------------";
                        TextWriter fwriter = File.AppendText(fname);
                        fwriter.WriteLine(header);
                        fwriter.Close();
                    }

                    d.WorldExportDIF(world, fname, physics_logging_append_existing_logfile, prefix);
                }
                
                // think time dilation as to do with dinamic step size that we dont' have
                // even so tell something to world
                if (nodeframes < 10) // we did the requested loops
                    m_timeDilation = 1.0f;
                else if (step_time > 0)
                {
                    m_timeDilation = timeStep / step_time;
                    if (m_timeDilation > 1)
                        m_timeDilation = 1;
                    if (step_time > m_SkipFramesAtms)
                        step_time = 0;
                }
            }

//            return nodeframes * ODE_STEPSIZE; // return real simulated time
            return 1000 * nodeframes; // return steps for now * 1000 to keep core happy
        }

        /// <summary>
        public override void GetResults()
        {
        }

        public override bool IsThreaded
        {
            // for now we won't be multithreaded
            get { return (false); }
        }

        public float GetTerrainHeightAtXY(float x, float y)
        {


            int offsetX = ((int)(x / (int)Constants.RegionSize)) * (int)Constants.RegionSize;
            int offsetY = ((int)(y / (int)Constants.RegionSize)) * (int)Constants.RegionSize;


            IntPtr heightFieldGeom = IntPtr.Zero;

            // get region map
            if (!RegionTerrain.TryGetValue(new Vector3(offsetX, offsetY, 0), out heightFieldGeom))
                return 0f;

            if (heightFieldGeom == IntPtr.Zero)
                return 0f;

            if (!TerrainHeightFieldHeights.ContainsKey(heightFieldGeom))
                return 0f;

            // TerrainHeightField for ODE as offset 1m
            x += 1f - offsetX;
            y += 1f - offsetY;

            // make position fit into array
            if (x < 0)
                x = 0;
            if (y < 0)
                y = 0;

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

            int regsize = (int)Constants.RegionSize + 3; // map size see setterrain number of samples

            if (OdeUbitLib)
            {
                if (x < regsize - 1)
                {
                    ix = (int)x;
                    dx = x - (float)ix;
                }
                else // out world use external height
                {
                    ix = regsize - 2;
                    dx = 0;
                }
                if (y < regsize - 1)
                {
                    iy = (int)y;
                    dy = y - (float)iy;
                }
                else
                {
                    iy = regsize - 2;
                    dy = 0;
                }
            }

            else
            {
                // we  still have square fixed size regions
                // also flip x and y because of how map is done for ODE fliped axis
                // so ix,iy,dx and dy are inter exchanged
                if (x < regsize - 1)
                {
                    iy = (int)x;
                    dy = x - (float)iy;
                }
                else // out world use external height
                {
                    iy = regsize - 2;
                    dy = 0;
                }
                if (y < regsize - 1)
                {
                    ix = (int)y;
                    dx = y - (float)ix;
                }
                else
                {
                    ix = regsize - 2;
                    dx = 0;
                }
            }

            float h0;
            float h1;
            float h2;

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

            float[] heights = TerrainHeightFieldHeights[heightFieldGeom];
            /*
                        if ((dx + dy) <= 1.0f)
                        {
                            h0 = ((float)heights[iy]); // 0,0 vertice
                            h1 = (((float)heights[iy + 1]) - h0) * dx; // 1,0 vertice minus 0,0
                            h2 = (((float)heights[iy + regsize]) - h0) * dy; // 0,1 vertice minus 0,0
                        }
                        else
                        {
                            h0 = ((float)heights[iy + regsize + 1]); // 1,1 vertice
                            h1 = (((float)heights[iy + 1]) - h0) * (1 - dy); // 1,1 vertice minus 1,0
                            h2 = (((float)heights[iy + regsize]) - h0) * (1 - dx); // 1,1 vertice minus 0,1
                        }
            */
            h0 = ((float)heights[iy]); // 0,0 vertice

            if ((dy > dx))
            {
                iy += regsize;
                h2 = (float)heights[iy]; // 0,1 vertice
                h1 = (h2 - h0) * dy; // 0,1 vertice minus 0,0
                h2 = ((float)heights[iy + 1] - h2) * dx; // 1,1 vertice minus 0,1
            }
            else
            {
                iy++;
                h2 = (float)heights[iy]; // vertice 1,0
                h1 = (h2 - h0) * dx; // 1,0 vertice minus 0,0
                h2 = (((float)heights[iy + regsize]) - h2) * dy; // 1,1 vertice minus 1,0
            }

            return h0 + h1 + h2;
        }


        public override void SetTerrain(float[] heightMap)
        {
            if (m_worldOffset != Vector3.Zero && m_parentScene != null)
            {
                if (m_parentScene is OdeScene)
                {
                    ((OdeScene)m_parentScene).SetTerrain(heightMap, m_worldOffset);
                }
            }
            else
            {
                SetTerrain(heightMap, m_worldOffset);
            }
        }

        public override void CombineTerrain(float[] heightMap, Vector3 pOffset)
        {
            SetTerrain(heightMap, pOffset);
        }

        public void SetTerrain(float[] heightMap, Vector3 pOffset)
        {
            if (OdeUbitLib)
                UbitSetTerrain(heightMap, pOffset);
            else
                OriSetTerrain(heightMap, pOffset);
        }

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

            float[] _heightmap;

            uint heightmapWidth = Constants.RegionSize + 2;
            uint heightmapHeight = Constants.RegionSize + 2;

            uint heightmapWidthSamples = heightmapWidth + 1;
            uint heightmapHeightSamples = heightmapHeight + 1;

            _heightmap = new float[heightmapWidthSamples * heightmapHeightSamples];

            const float scale = 1.0f;
            const float offset = 0.0f;
            const float thickness = 10f;
            const int wrap = 0;

            uint regionsize = Constants.RegionSize;
 
            float hfmin = float.MaxValue;
            float hfmax = float.MinValue;
            float val;
            uint xx;
            uint yy;

            uint maxXXYY = regionsize - 1;
            // flipping map adding one margin all around so things don't fall in edges

            uint xt = 0;
            xx = 0;

            for (uint x = 0; x < heightmapWidthSamples; x++)
            {
                if (x > 1 && xx < maxXXYY)
                    xx++;
                yy = 0;
                for (uint y = 0; y < heightmapHeightSamples; y++)
                {
                    if (y > 1 && y < maxXXYY)
                        yy += regionsize;

                    val = heightMap[yy + xx];
                    if (val < 0.0f)
                        val = 0.0f; // no neg terrain as in chode
                    _heightmap[xt + y] = val;

                    if (hfmin > val)
                        hfmin = val;
                    if (hfmax < val)
                        hfmax = val;
                }
                xt += heightmapHeightSamples;
            }
            lock (OdeLock)
            {
                IntPtr GroundGeom = IntPtr.Zero;
                if (RegionTerrain.TryGetValue(pOffset, out GroundGeom))
                {
                    RegionTerrain.Remove(pOffset);
                    if (GroundGeom != IntPtr.Zero)
                    {
                        d.GeomDestroy(GroundGeom);

                        if (TerrainHeightFieldHeights.ContainsKey(GroundGeom))
                            {
                            TerrainHeightFieldHeightsHandlers[GroundGeom].Free();
                            TerrainHeightFieldHeightsHandlers.Remove(GroundGeom);
                            TerrainHeightFieldHeights.Remove(GroundGeom);
                            }
                    }
                }
                IntPtr HeightmapData = d.GeomHeightfieldDataCreate();

                GCHandle _heightmaphandler = GCHandle.Alloc(_heightmap, GCHandleType.Pinned);

                d.GeomHeightfieldDataBuildSingle(HeightmapData, _heightmaphandler.AddrOfPinnedObject(), 0, heightmapWidth , heightmapHeight,
                                                 (int)heightmapWidthSamples, (int)heightmapHeightSamples, scale,
                                                 offset, thickness, wrap);

                d.GeomHeightfieldDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1);
                GroundGeom = d.CreateHeightfield(StaticSpace, HeightmapData, 1);
                if (GroundGeom != IntPtr.Zero)
                {
                    d.GeomSetCategoryBits(GroundGeom, (uint)(CollisionCategories.Land));
                    d.GeomSetCollideBits(GroundGeom, 0);

                }
                geom_name_map[GroundGeom] = "Terrain";

                d.Matrix3 R = new d.Matrix3();

                Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f);
                Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f);
                

                q1 = q1 * q2;
                
                Vector3 v3;
                float angle;
                q1.GetAxisAngle(out v3, out angle);

                d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle);
                d.GeomSetRotation(GroundGeom, ref R);
                d.GeomSetPosition(GroundGeom, pOffset.X + (float)Constants.RegionSize * 0.5f, pOffset.Y + (float)Constants.RegionSize * 0.5f, 0);
                RegionTerrain.Add(pOffset, GroundGeom);
                TerrainHeightFieldHeights.Add(GroundGeom, _heightmap);
                TerrainHeightFieldHeightsHandlers.Add(GroundGeom, _heightmaphandler);             
            }
        }

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

            float[] _heightmap;

            uint heightmapWidth = Constants.RegionSize + 2;
            uint heightmapHeight = Constants.RegionSize + 2;

            uint heightmapWidthSamples = heightmapWidth + 1;
            uint heightmapHeightSamples = heightmapHeight + 1;

            _heightmap = new float[heightmapWidthSamples * heightmapHeightSamples];


            uint regionsize = Constants.RegionSize;

            float hfmin = float.MaxValue;
//            float hfmax = float.MinValue;
            float val;


            uint maxXXYY = regionsize - 1;
            // adding one margin all around so things don't fall in edges

            uint xx;
            uint yy = 0;
            uint yt = 0;

            for (uint y = 0; y < heightmapHeightSamples; y++)
            {
                if (y > 1 && y < maxXXYY)
                    yy += regionsize;
                xx = 0;
                for (uint x = 0; x < heightmapWidthSamples; x++)
                {
                    if (x > 1 && x < maxXXYY)
                        xx++;

                    val = heightMap[yy + xx];
                    if (val < 0.0f)
                        val = 0.0f; // no neg terrain as in chode
                    _heightmap[yt + x] = val;

                    if (hfmin > val)
                        hfmin = val;
//                    if (hfmax < val)
//                        hfmax = val;
                }
                yt += heightmapWidthSamples;
            }
            lock (OdeLock)
            {
                IntPtr GroundGeom = IntPtr.Zero;
                if (RegionTerrain.TryGetValue(pOffset, out GroundGeom))
                {
                    RegionTerrain.Remove(pOffset);
                    if (GroundGeom != IntPtr.Zero)
                    {
                        d.GeomDestroy(GroundGeom);

                        if (TerrainHeightFieldHeights.ContainsKey(GroundGeom))
                        {
                            if (TerrainHeightFieldHeightsHandlers[GroundGeom].IsAllocated)
                                TerrainHeightFieldHeightsHandlers[GroundGeom].Free();
                            TerrainHeightFieldHeightsHandlers.Remove(GroundGeom);
                            TerrainHeightFieldHeights.Remove(GroundGeom);
                        }
                    }
                }
                IntPtr HeightmapData = d.GeomHeightfieldDataCreate();

                const int wrap = 0;
                float thickness = hfmin;
                if (thickness < 0)
                    thickness = 1;

                GCHandle _heightmaphandler = GCHandle.Alloc(_heightmap, GCHandleType.Pinned);

                d.GeomUbitTerrainDataBuild(HeightmapData, _heightmaphandler.AddrOfPinnedObject(), 0, 1.0f,
                                                 (int)heightmapWidthSamples, (int)heightmapHeightSamples,
                                                 thickness, wrap);

//                d.GeomUbitTerrainDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1);
                GroundGeom = d.CreateUbitTerrain(StaticSpace, HeightmapData, 1);
                if (GroundGeom != IntPtr.Zero)
                {
                    d.GeomSetCategoryBits(GroundGeom, (uint)(CollisionCategories.Land));
                    d.GeomSetCollideBits(GroundGeom, 0);

                }
                geom_name_map[GroundGeom] = "Terrain";

                d.GeomSetPosition(GroundGeom, pOffset.X + (float)Constants.RegionSize * 0.5f, pOffset.Y + (float)Constants.RegionSize * 0.5f, 0);
                RegionTerrain.Add(pOffset, GroundGeom);
                TerrainHeightFieldHeights.Add(GroundGeom, _heightmap);
                TerrainHeightFieldHeightsHandlers.Add(GroundGeom, _heightmaphandler);
            }
        }


        public override void DeleteTerrain()
        {
        }

        public float GetWaterLevel()
        {
            return waterlevel;
        }

        public override bool SupportsCombining()
        {
            return true;
        }
/*
        public override void UnCombine(PhysicsScene pScene)
        {
            IntPtr localGround = IntPtr.Zero;
//            float[] localHeightfield;
            bool proceed = false;
            List<IntPtr> geomDestroyList = new List<IntPtr>();

            lock (OdeLock)
            {
                if (RegionTerrain.TryGetValue(Vector3.Zero, out localGround))
                {
                    foreach (IntPtr geom in TerrainHeightFieldHeights.Keys)
                    {
                        if (geom == localGround)
                        {
//                            localHeightfield = TerrainHeightFieldHeights[geom];
                            proceed = true;
                        }
                        else
                        {
                            geomDestroyList.Add(geom);
                        }
                    }

                    if (proceed)
                    {
                        m_worldOffset = Vector3.Zero;
                        WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize);
                        m_parentScene = null;

                        foreach (IntPtr g in geomDestroyList)
                        {
                            // removingHeightField needs to be done or the garbage collector will
                            // collect the terrain data before we tell ODE to destroy it causing 
                            // memory corruption
                            if (TerrainHeightFieldHeights.ContainsKey(g))
                            {
//                                float[] removingHeightField = TerrainHeightFieldHeights[g];
                                TerrainHeightFieldHeights.Remove(g);

                                if (RegionTerrain.ContainsKey(g))
                                {
                                    RegionTerrain.Remove(g);
                                }

                                d.GeomDestroy(g);
                                //removingHeightField = new float[0];
                            }
                        }

                    }
                    else
                    {
                        m_log.Warn("[PHYSICS]: Couldn't proceed with UnCombine.  Region has inconsistant data.");
                    }
                }
            }
        }
*/
        public override void SetWaterLevel(float baseheight)
        {
            waterlevel = baseheight;
            randomizeWater(waterlevel);
        }

        public void randomizeWater(float baseheight)
        {
            const uint heightmapWidth = Constants.RegionSize + 2;
            const uint heightmapHeight = Constants.RegionSize + 2;
            const uint heightmapWidthSamples = heightmapWidth + 1;
            const uint heightmapHeightSamples = heightmapHeight + 1;

            const float scale = 1.0f;
            const float offset = 0.0f;
            const int wrap = 0;

            float[] _watermap = new float[heightmapWidthSamples * heightmapWidthSamples];

            float maxheigh = float.MinValue;
            float minheigh = float.MaxValue;
            float val;
            for (int i = 0; i < (heightmapWidthSamples * heightmapHeightSamples); i++)
            {

                val = (baseheight - 0.1f) + ((float)fluidRandomizer.Next(1, 9) / 10f);
                _watermap[i] = val;
                if (maxheigh < val)
                    maxheigh = val;
                if (minheigh > val)
                    minheigh = val;
            }

            float thickness = minheigh;

            lock (OdeLock)
            {
                if (WaterGeom != IntPtr.Zero)
                {
                    d.GeomDestroy(WaterGeom);
                    d.GeomHeightfieldDataDestroy(WaterHeightmapData);
                    WaterGeom = IntPtr.Zero;
                    WaterHeightmapData = IntPtr.Zero;
                    if(WaterMapHandler.IsAllocated)
                        WaterMapHandler.Free();
                }

                WaterHeightmapData = d.GeomHeightfieldDataCreate();

                WaterMapHandler = GCHandle.Alloc(_watermap, GCHandleType.Pinned);

                d.GeomHeightfieldDataBuildSingle(WaterHeightmapData, WaterMapHandler.AddrOfPinnedObject(), 0, heightmapWidth, heightmapHeight,
                                                 (int)heightmapWidthSamples, (int)heightmapHeightSamples, scale,
                                                 offset, thickness, wrap);
                d.GeomHeightfieldDataSetBounds(WaterHeightmapData, minheigh, maxheigh);
                WaterGeom = d.CreateHeightfield(StaticSpace, WaterHeightmapData, 1);
                if (WaterGeom != IntPtr.Zero)
                {
                    d.GeomSetCategoryBits(WaterGeom, (uint)(CollisionCategories.Water));
                    d.GeomSetCollideBits(WaterGeom, 0);

                    geom_name_map[WaterGeom] = "Water";

                    d.Matrix3 R = new d.Matrix3();

                    Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f);
                    Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f);

                    q1 = q1 * q2;
                    Vector3 v3;
                    float angle;
                    q1.GetAxisAngle(out v3, out angle);

                    d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle);
                    d.GeomSetRotation(WaterGeom, ref R);
                    d.GeomSetPosition(WaterGeom, (float)Constants.RegionSize * 0.5f, (float)Constants.RegionSize * 0.5f, 0);
                }
            }
        }

        public override void Dispose()
        {
            lock (OdeLock)
            {
                m_rayCastManager.Dispose();
                m_rayCastManager = null;

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

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

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

                               
                foreach (IntPtr GroundGeom in RegionTerrain.Values)
                {
                    if (GroundGeom != IntPtr.Zero)
                        d.GeomDestroy(GroundGeom);
                }

                RegionTerrain.Clear();

                if (TerrainHeightFieldHeightsHandlers.Count > 0)
                {
                    foreach (GCHandle gch in TerrainHeightFieldHeightsHandlers.Values)
                    {
                        if (gch.IsAllocated)
                            gch.Free();
                    }
                }

                TerrainHeightFieldHeightsHandlers.Clear();
                TerrainHeightFieldHeights.Clear();

                if (WaterGeom != IntPtr.Zero)
                {
                    d.GeomDestroy(WaterGeom);
                        WaterGeom = IntPtr.Zero;
                    if (WaterHeightmapData != IntPtr.Zero)
                        d.GeomHeightfieldDataDestroy(WaterHeightmapData);
                    WaterHeightmapData = IntPtr.Zero;

                    if (WaterMapHandler.IsAllocated)
                        WaterMapHandler.Free();
                }


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


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

        public override Dictionary<uint, float> GetTopColliders()
        {
            Dictionary<uint, float> returncolliders = new Dictionary<uint, float>();
            int cnt = 0;
            lock (_prims)
            {
                foreach (OdePrim prm in _prims)
                {
                    if (prm.CollisionScore > 0)
                    {
                        returncolliders.Add(prm.m_localID, prm.CollisionScore);
                        cnt++;
                        prm.CollisionScore = 0f;
                        if (cnt > 25)
                        {
                            break;
                        }
                    }
                }
            }
            return returncolliders;
        }

        public override bool SupportsRayCast()
        {
            return true;
        }

        public override void RaycastWorld(Vector3 position, Vector3 direction, float length, RaycastCallback retMethod)
        {
            if (retMethod != null)
            {
                m_rayCastManager.QueueRequest(position, direction, length, retMethod);
            }
        }

        public override void RaycastWorld(Vector3 position, Vector3 direction, float length, int Count, RayCallback retMethod)
        {
            if (retMethod != null)
            {
                m_rayCastManager.QueueRequest(position, direction, length, Count, retMethod);
            }
        }

        // don't like this
        public override List<ContactResult> RaycastWorld(Vector3 position, Vector3 direction, float length, int Count)
        {
            ContactResult[] ourResults = null;
            RayCallback retMethod = delegate(List<ContactResult> results)
            {
                ourResults = new ContactResult[results.Count];
                results.CopyTo(ourResults, 0);
            };
            int waitTime = 0;
            m_rayCastManager.QueueRequest(position, direction, length, Count, retMethod);
            while (ourResults == null && waitTime < 1000)
            {
                Thread.Sleep(1);
                waitTime++;
            }
            if (ourResults == null)
                return new List<ContactResult>();
            return new List<ContactResult>(ourResults);
        }

        public override bool SuportsRaycastWorldFiltered()
        {
            return true;
        }

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

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

            lock (SyncObject)
            {
                m_rayCastManager.QueueRequest(position, direction, length, Count,filter, retMethod);
                if (!Monitor.Wait(SyncObject, 500))
                    return null;
                else
                    return ourresults;
            }
        }

        public override void RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, RaycastCallback retMethod)
        {
            if (retMethod != null && actor !=null)
            {
                IntPtr geom;
                if (actor is OdePrim)
                    geom = ((OdePrim)actor).prim_geom;
                else if (actor is OdeCharacter)
                    geom = ((OdePrim)actor).prim_geom;
                else
                    return;
                if (geom == IntPtr.Zero)
                    return;
                m_rayCastManager.QueueRequest(geom, position, direction, length, retMethod);
            }
        }

        public override void RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count, RayCallback retMethod)
        {
            if (retMethod != null && actor != null)
            {
                IntPtr geom;
                if (actor is OdePrim)
                    geom = ((OdePrim)actor).prim_geom;
                else if (actor is OdeCharacter)
                    geom = ((OdePrim)actor).prim_geom;
                else
                    return;
                if (geom == IntPtr.Zero)
                    return;

                m_rayCastManager.QueueRequest(geom,position, direction, length, Count, retMethod);
            }
        }

        // don't like this
        public override List<ContactResult> RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count)
        {
            if (actor != null)
            {
                IntPtr geom;
                if (actor is OdePrim)
                    geom = ((OdePrim)actor).prim_geom;
                else if (actor is OdeCharacter)
                    geom = ((OdePrim)actor).prim_geom;
                else
                    return new List<ContactResult>();
                if (geom == IntPtr.Zero)
                    return new List<ContactResult>();

                ContactResult[] ourResults = null;
                RayCallback retMethod = delegate(List<ContactResult> results)
                {
                    ourResults = new ContactResult[results.Count];
                    results.CopyTo(ourResults, 0);
                };
                int waitTime = 0;
                m_rayCastManager.QueueRequest(geom,position, direction, length, Count, retMethod);
                while (ourResults == null && waitTime < 1000)
                {
                    Thread.Sleep(1);
                    waitTime++;
                }
                if (ourResults == null)
                    return new List<ContactResult>();
                return new List<ContactResult>(ourResults);
            }
            return new List<ContactResult>();
        }
    }
}