/*
 * 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 USE_DRAWSTUFF
//#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;
#if USE_DRAWSTUFF
using ODEDrawstuff;
#endif
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;
    }

    [Flags]
    public enum CollisionCategories : int
    {
        Disabled = 0,
        Geom = 0x00000001,
        Body = 0x00000002,
        Space = 0x00000004,
        Character = 0x00000008,
        Land = 0x00000010,
        Water = 0x00000020,
        Wind = 0x00000040,
        Sensor = 0x00000080,
        Selected = 0x00000100
    }

    /// <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,

        AddForce,
        AddAngForce,
        AngLock,

        Size,
        Shape,

        CollidesWater,
        VolumeDtc,

        Physical,
        Selected,
        disabled,
        building,

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

    public struct ODEchangeitem
    {
        public OdePrim prim;
        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>();

        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 comumContactERP = 0.6f;
        const float comumSoftContactERP = 0.1f;
        const float comumContactCFM = 0.0001f;
        
        float frictionScale = 1.0f;
        
        float frictionMovementMult = 0.3f;

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

        public float AvatarBounce = 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;
        private float metersInSpace = 25.6f;
        private float m_timeDilation = 1.0f;

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


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

        internal IntPtr WaterGeom;

        public float avPIDD = 3200f; // make it visible
        public float avPIDP = 1400f; // 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 = 16;

        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 = 20;

        private float[] _watermap;
        private bool m_filterCollisions = true;

        private d.NearCallback nearCallback;

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

        private readonly Object _taintedCharacterLock = new Object();
        private readonly HashSet<OdeCharacter> _taintedCharacterH = new HashSet<OdeCharacter>(); // faster verification of repeated character taints
        private readonly Queue<OdeCharacter> _taintedCharacterQ = new Queue<OdeCharacter>(); // character taints

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

        /// <summary>
        /// A list of actors that should receive collision events.
        /// </summary>
        private readonly List<PhysicsActor> _collisionEventPrim = new List<PhysicsActor>();
        
        private readonly 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 readonly IntPtr contactgroup;

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

        private readonly DoubleDictionary<Vector3, IntPtr, IntPtr> RegionTerrain = new DoubleDictionary<Vector3, IntPtr, IntPtr>();
        private readonly Dictionary<IntPtr, float[]> TerrainHeightFieldHeights = new Dictionary<IntPtr, float[]>();
        private readonly 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 readonly PhysicsActor PANull = new NullPhysicsActor();
        private float step_time = 0.0f;

        public IntPtr world;

        private uint obj2LocalID = 0;
        private OdeCharacter cc1;
        private OdePrim cp1;
        private OdeCharacter cc2;
        private OdePrim cp2;

        // 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 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); // cell sizes from .25 to 256 ?? need check what this really does
                d.HashSpaceSetLevels(ActiveSpace, -2, 8);
                d.HashSpaceSetLevels(StaticSpace, -2, 8);

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

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

                d.WorldSetAutoDisableFlag(world, false);
                #if USE_DRAWSTUFF
                
                Thread viewthread = new Thread(new ParameterizedThreadStart(startvisualization));
                viewthread.Start();
                #endif
            }

            _watermap = new float[258 * 258];
        }

#if USE_DRAWSTUFF
        public void startvisualization(object o)
        {
            ds.Functions fn;
            fn.version = ds.VERSION;
            fn.start = new ds.CallbackFunction(start);
            fn.step = new ds.CallbackFunction(step);
            fn.command = new ds.CallbackFunction(command);
            fn.stop = null;
            fn.path_to_textures = "./textures";
            string[] args = new string[0];
            ds.SimulationLoop(args.Length, args, 352, 288, ref fn);
        }
#endif

        // 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;

//            m_log.WarnFormat("ODE configuration: {0}", d.GetConfiguration("ODE"));
            /*
                        if (region != null)
                        {
                            WorldExtents.X = region.RegionSizeX;
                            WorldExtents.Y = region.RegionSizeY;
                        }
             */

            // Defaults

            avPIDD = 2200.0f;
            avPIDP = 900.0f;

            int contactsPerCollision = 80;

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

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

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

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

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

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

                    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", 10.000006836f);
                    bodyFramesAutoDisable = physicsconfig.GetInt("body_frames_auto_disable", 20);

                    bodyPIDD = physicsconfig.GetFloat("body_pid_derivative", 35f);
                    bodyPIDG = physicsconfig.GetFloat("body_pid_gain", 25f);

                    forceSimplePrimMeshing = physicsconfig.GetBoolean("force_simple_prim_meshing", forceSimplePrimMeshing);
                    meshSculptedPrim = physicsconfig.GetBoolean("mesh_sculpted_prim", true);
                    meshSculptLOD = physicsconfig.GetFloat("mesh_lod", 32f);
                    MeshSculptphysicalLOD = physicsconfig.GetFloat("mesh_physical_lod", 16f);
                    m_filterCollisions = physicsconfig.GetBoolean("filter_collisions", false);

                    if (Environment.OSVersion.Platform == PlatformID.Unix)
                    {
                        avPIDD = physicsconfig.GetFloat("av_pid_derivative_linux", 2200.0f);
                        avPIDP = physicsconfig.GetFloat("av_pid_proportional_linux", 900.0f);
                    }
                    else
                    {
                        avPIDD = physicsconfig.GetFloat("av_pid_derivative_win", 2200.0f);
                        avPIDP = physicsconfig.GetFloat("av_pid_proportional_win", 900.0f);
                    }

                    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", 3f);
                    maximumMassObject = physicsconfig.GetFloat("maximum_mass_object", 10000.01f);
                }
            }

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

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

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

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

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

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

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

            m_materialContactsData[(int)Material.Rubber].mu = frictionScale * 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;

            TerrainFriction *= frictionScale;
//            AvatarFriction *= frictionScale;

            // 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.001f);
            d.WorldSetAngularDamping(world, 0.001f);
            d.WorldSetAngularDampingThreshold(world, 0f);
            d.WorldSetLinearDampingThreshold(world, 0f);
            d.WorldSetMaxAngularSpeed(world, 256f);

            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, 100.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);
                    staticPrimspace[i, j] = newspace;
                }
            // let this now be real maximum values
            spaceGridMaxX--;
            spaceGridMaxY--;
        }

        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, bool softerp)
        {
            if (GlobalContactsArray == IntPtr.Zero || m_global_contactcount >= maxContactsbeforedeath)
                return IntPtr.Zero;

            d.Contact newcontact = new d.Contact();
            newcontact.geom.depth = contactGeom.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 = comumContactCFM;
            if (softerp)
                newcontact.surface.soft_erp = comumSoftContactERP;
            else
                newcontact.surface.soft_erp = comumContactERP;

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


        /// <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 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;
        }



        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;

                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 is OdePrim) && (((OdePrim)p1).m_isVolumeDetect) ||
                                (p2 is OdePrim) && (((OdePrim)p2).m_isVolumeDetect))
            {
                collision_accounting_events(p1, p2, maxDepthContact);
                return;
            }

            // big messy collision analises
            float mu = 0;
            float bounce = 0;
            ContactData contactdata1;
            ContactData contactdata2;
            bool erpSoft = false;

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

            switch (p1.PhysicsActorType)
            {
                case (int)ActorTypes.Agent:
                    switch (p2.PhysicsActorType)
                    {
                        case (int)ActorTypes.Agent:
                            contactdata1 = p1.ContactData;
                            contactdata2 = p2.ContactData;
                            bounce = contactdata1.bounce * contactdata2.bounce;
                            
                            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:
                            contactdata1 = p1.ContactData;
                            contactdata2 = p2.ContactData;
                            bounce = contactdata1.bounce * contactdata2.bounce;
                            
                            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:
                            contactdata1 = p1.ContactData;
                            contactdata2 = p2.ContactData;
                            bounce = contactdata1.bounce * contactdata2.bounce;
                            
                            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;
                            }
                            contactdata1 = p1.ContactData;
                            contactdata2 = p2.ContactData;
                            bounce = contactdata1.bounce * contactdata2.bounce;
                            erpSoft = true;
                            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;

                            break;
                        default:
                            if (geom_name_map.TryGetValue(g2, out name))
                            {
                                if (name == "Terrain")
                                {
                                    erpSoft = true;
                                    contactdata1 = p1.ContactData;
                                    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;
                                }
                                else if (name == "Water")
                                {
                                    erpSoft = true;
                                }
                            }
                            else
                                ignore=true;
                            break;
                    }
                    break;

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

                                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))
                        {
                            erpSoft = true;
                        }
                    }
                    else
                        ignore = true;
                    break;
            }

            if (ignore)
                return;

            IntPtr Joint;

            int i = 0;
            while(true)
            {
                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;

                Joint = CreateContacJoint(ref curContact, mu, bounce, erpSoft);
                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)
            {
            // obj1LocalID = 0;
            //returncollisions = false;
            obj2LocalID = 0;
            //ctype = 0;
            //cStartStop = 0;
            if (!(p2.SubscribedEvents() || p1.SubscribedEvents()))
                return;

            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 (p2.SubscribedEvents())
                                cc2.AddCollisionEvent(cc1.m_localID, contact);
                            break;

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

                        case ActorTypes.Ground:
                        case ActorTypes.Unknown:
                        default:
                            obj2LocalID = 0;
                            break;
                        }
                    if (p1.SubscribedEvents())
                        {
                        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 (p2.SubscribedEvents())
                                        cc2.AddCollisionEvent(cp1.m_localID, contact);
                                    }
                                break;
                            case ActorTypes.Prim:

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

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

        /// <summary>
        /// This is our collision testing routine in ODE
        /// </summary>
        /// <param name="timeStep"></param>
        private void collision_optimized()
        {
//        _perloopContact.Clear();
// clear characts IsColliding until we do it some other way

            lock (_characters)
                {
                foreach (OdeCharacter chr in _characters)
                    {
                    // this are odd checks  if they are needed something is wrong elsewhere
                    // keep for now
                    if (chr == null)
                        continue;

                    if (chr.Shell == IntPtr.Zero || chr.Body == IntPtr.Zero)
                        continue;

                    chr.IsColliding = false;
                    //                    chr.CollidingGround = false; not done here
                    chr.CollidingObj = false;
                    }
                }

            // now let ode do its job
            // colide active things amoung them

            int st = Util.EnvironmentTickCount();
            int ta;
            int ts;
            try
                {
                d.SpaceCollide(ActiveSpace, IntPtr.Zero, nearCallback);
                }
            catch (AccessViolationException)
                {
                m_log.Warn("[PHYSICS]: Unable to Active space collide");
                }
            ta = Util.EnvironmentTickCountSubtract(st);
            // then active things with static enviroment
            try
                {
                d.SpaceCollide2(ActiveSpace,StaticSpace, IntPtr.Zero, nearCallback);
                }
            catch (AccessViolationException)
                {
                m_log.Warn("[PHYSICS]: Unable to Active to static space collide");
                }
            ts = Util.EnvironmentTickCountSubtract(st);
//            _perloopContact.Clear();
        }

        #endregion


        public float GetTerrainHeightAtXY(float x, float y)
        {
            // assumes 1m size grid and constante size square regions
            // region offset in mega position

            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 + 2; // map size see setterrain

            // 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 - 1;
                dy = 0;
            }
            if (y < regsize - 1)
            {
                ix = (int)y;
                dx = y - (float)ix;
            }
            else
            {
                ix = regsize - 1;
                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
            }

            return h0 + h1 + h2;
        }

        /// <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)
        {
            lock (_collisionEventPrim)
            {
                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)
        {
            lock (_collisionEventPrim)
            {
                if (_collisionEventPrim.Contains(obj))
                    _collisionEventPrim.Remove(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);

                lock (_prims)
                    _prims.Add(newPrim);
            }
            newPrim.LocalID = localID;
            return newPrim;
        }

        public void addActivePrim(OdePrim activatePrim)
        {
            // adds active prim..   (ones that should be iterated over in collisions_optimized
            lock (_activeprims)
            {
                if (!_activeprims.Contains(activatePrim))
                    _activeprims.Add(activatePrim);
                //else
                  //  m_log.Warn("[PHYSICS]: Double Entry in _activeprims detected, potential crash immenent");
            }
        }

        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 float TimeDilation
        {
            get { return m_timeDilation; }
        }

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


        public void remActivePrim(OdePrim deactivatePrim)
        {
            lock (_activeprims)
            {
                _activeprims.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);
                }
                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);
                    }
                }
            }

            // 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;
            x = (int)(pos.X * spacesPerMeter);
            if (x < 0)
                x = 0;
            else if (x > spaceGridMaxX)
                x = spaceGridMaxX;

            y = (int)(pos.Y * spacesPerMeter);
            if (y < 0)
                y = 0;
            else if (y >spaceGridMaxY)
                y = spaceGridMaxY;

            IntPtr tmpSpace = staticPrimspace[x, y];
            return tmpSpace;
        }
 
        #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)
        {
            // most of this is redundant now as the mesher will return null if it cant mesh a prim
            // but we still need to check for sculptie meshing being enabled so this is the most
            // convenient place to do it for now...

        //    //if (pbs.PathCurve == (byte)Primitive.PathCurve.Circle && pbs.ProfileCurve == (byte)Primitive.ProfileCurve.Circle && pbs.PathScaleY <= 0.75f)
        //    //m_log.Debug("needsMeshing: " + " pathCurve: " + pbs.PathCurve.ToString() + " profileCurve: " + pbs.ProfileCurve.ToString() + " pathScaleY: " + Primitive.UnpackPathScale(pbs.PathScaleY).ToString());
            int iPropertiesNotSupportedDefault = 0;

            if (pbs.SculptEntry)
            {
                if(!meshSculptedPrim)
                    return false;
            }

            // 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 (!forceSimplePrimMeshing && !pbs.SculptEntry)
            {
                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

            if (forceSimplePrimMeshing)
                return true;

            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 (pbs.SculptEntry && meshSculptedPrim)
                iPropertiesNotSupportedDefault++;

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

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

        /// <summary>
        /// Called to queue a change to a prim
        /// to use in place of old taint mechanism so changes do have a time sequence
        /// </summary>
        public void AddChange(OdeCharacter character, changes what, Object arg)
        {
            ODEchangeitem item = new ODEchangeitem();
            item.character = character;
            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)
            {
            if (prim is OdePrim)
                {
/*                OdePrim taintedprim = ((OdePrim) prim);
                lock (_taintedPrimLock)
                    {
                    if (!(_taintedPrimH.Contains(taintedprim))) 
                        {
                        _taintedPrimH.Add(taintedprim);                    // HashSet for searching
                        _taintedPrimQ.Enqueue(taintedprim);                    // List for ordered readout
                        }
                    }
 */
                return;
                }
            else if (prim is OdeCharacter)
                {
                OdeCharacter taintedchar = ((OdeCharacter)prim);
                lock (_taintedCharacterLock)
                    {
                    if (!(_taintedCharacterH.Contains(taintedchar)))
                        {
                        _taintedCharacterH.Add(taintedchar);
                        _taintedCharacterQ.Enqueue(taintedchar);
                        if (taintedchar.bad)
                            m_log.DebugFormat("[PHYSICS]: Added BAD actor {0} to tainted actors", taintedchar.m_uuid);
                        }
                    }
                }
            }

        /// <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)
        {
            int statstart;
            int statchanges = 0;
            int statchmove = 0;
            int statactmove = 0;
            int statray = 0;
            int statcol = 0;
            int statstep = 0;
            int statmovchar = 0;
            int statmovprim;
            int totjcontact = 0;

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

            if (step_time < ODE_STEPSIZE)
                return 0;

            if (framecount >= int.MaxValue)
                framecount = 0;

            framecount++;

            int curphysiteractions = m_physicsiterations;

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

            int nodeframes = 0;

//            checkThread();

            lock (SimulationLock)
            {
                // 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 >= ODE_STEPSIZE && nodeframes < 10) //limit number of steps so we don't say here for ever
                {
                    try
                    {
                        statstart = Util.EnvironmentTickCount();

                        // clear pointer/counter to contacts to pass into joints
                        m_global_contactcount = 0;

                        // do characters requested changes

                        OdeCharacter character;
                        int numtaints;
                        lock (_taintedCharacterLock)
                        {
                            numtaints = _taintedCharacterQ.Count;
                            //                            if (numtaints > 50)
                            //                                numtaints = 50;
                            while (numtaints > 0)
                            {
                                character = _taintedCharacterQ.Dequeue();
                                character.ProcessTaints(ODE_STEPSIZE);
                                _taintedCharacterH.Remove(character);
                                numtaints--;
                            }
                        }
                        // do other objects requested changes

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

                            while(ChangesQueue.Dequeue(out item))
                            {
                                if (item.prim != null)
                                {
                                    try
                                    {
                                        if (item.prim.DoAChange(item.what, item.arg))
                                            RemovePrimThreadLocked(item.prim);
                                    }
                                    catch { };
                                }
                                ttmp = Util.EnvironmentTickCountSubtract(ttmpstart);
                                if (ttmp > 20)
                                    break;
                            }

                            ttmp2 = Util.EnvironmentTickCountSubtract(ttmpstart);
                            if (ttmp2 > 50)
                                ttmp2 = 0;

                        }

                        statchanges += Util.EnvironmentTickCountSubtract(statstart);

                        // 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);
                                }
                            }
                        }
                        statchmove += Util.EnvironmentTickCountSubtract(statstart);

                        // Move other active objects
                        lock (_activeprims)
                        {
                            foreach (OdePrim aprim in _activeprims)
                            {
                                aprim.CollisionScore = 0;
                                aprim.IsColliding = false;
                                aprim.Move();
                            }
                        }

                        statactmove += Util.EnvironmentTickCountSubtract(statstart);
                        //if ((framecount % m_randomizeWater) == 0)
                        // randomizeWater(waterlevel);

                        m_rayCastManager.ProcessQueuedRequests();

                        statray += Util.EnvironmentTickCountSubtract(statstart);
                        collision_optimized();
                        statcol += Util.EnvironmentTickCountSubtract(statstart);

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

                                switch ((ActorTypes)obj.PhysicsActorType)
                                {
                                    case ActorTypes.Agent:
                                        OdeCharacter cobj = (OdeCharacter)obj;
                                        cobj.AddCollisionFrameTime((int)(ODE_STEPSIZE*1000.0f));
                                        cobj.SendCollisions();
                                        break;

                                    case ActorTypes.Prim:
                                        OdePrim pobj = (OdePrim)obj;
                                        pobj.SendCollisions();
                                        break;
                                }
                            }
                        }

                        d.WorldQuickStep(world, ODE_STEPSIZE);
                        statstep += Util.EnvironmentTickCountSubtract(statstart);
                        d.JointGroupEmpty(contactgroup);
                        totjcontact += m_global_contactcount;
                        //ode.dunlock(world);
                    }
                    catch (Exception e)
                    {
                        m_log.ErrorFormat("[PHYSICS]: {0}, {1}, {2}", e.Message, e.TargetSite, e);
//                        ode.dunlock(world);
                    }

                    step_time -= ODE_STEPSIZE;
                    nodeframes++;
                }

                statstart = Util.EnvironmentTickCount();

                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 (_badCharacter)
                {
                    if (_badCharacter.Count > 0)
                    {
                        foreach (OdeCharacter chr in _badCharacter)
                        {
                            RemoveCharacter(chr);
                        }

                        _badCharacter.Clear();
                    }
                }
                statmovchar = Util.EnvironmentTickCountSubtract(statstart);

                lock (_activeprims)
                {
                    {
                        foreach (OdePrim actor in _activeprims)
                        {
                            if (actor.IsPhysical)
                            {
                                actor.UpdatePositionAndVelocity((float)nodeframes * ODE_STEPSIZE);
                            }
                        }
                    }
                }

                statmovprim = Util.EnvironmentTickCountSubtract(statstart);

                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 is not a physics issue alone..  but ok let's fake something
                if (step_time < ODE_STEPSIZE) // we did the required loops
                    m_timeDilation = 1.0f;
                else
                { // we didn't forget the lost ones and let user know something
                    m_timeDilation = 1 - step_time / timeStep;
                    if (m_timeDilation < 0)
                        m_timeDilation = 0;
                    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); }
        }

        #region ODE Specific Terrain Fixes
        public float[] ResizeTerrain512NearestNeighbour(float[] heightMap)
        {
            float[] returnarr = new float[262144];
            float[,] resultarr = new float[(int)WorldExtents.X, (int)WorldExtents.Y];

            // Filling out the array into its multi-dimensional components
            for (int y = 0; y < WorldExtents.Y; y++)
            {
                for (int x = 0; x < WorldExtents.X; x++)
                {
                    resultarr[y, x] = heightMap[y * (int)WorldExtents.Y + x];
                }
            }

            // Resize using Nearest Neighbour

            // This particular way is quick but it only works on a multiple of the original

            // The idea behind this method can be described with the following diagrams
            // second pass and third pass happen in the same loop really..  just separated
            // them to show what this does.

            // First Pass
            // ResultArr:
            // 1,1,1,1,1,1
            // 1,1,1,1,1,1
            // 1,1,1,1,1,1
            // 1,1,1,1,1,1
            // 1,1,1,1,1,1
            // 1,1,1,1,1,1

            // Second Pass
            // ResultArr2:
            // 1,,1,,1,,1,,1,,1,
            // ,,,,,,,,,,
            // 1,,1,,1,,1,,1,,1,
            // ,,,,,,,,,,
            // 1,,1,,1,,1,,1,,1,
            // ,,,,,,,,,,
            // 1,,1,,1,,1,,1,,1,
            // ,,,,,,,,,,
            // 1,,1,,1,,1,,1,,1,
            // ,,,,,,,,,,
            // 1,,1,,1,,1,,1,,1,

            // Third pass fills in the blanks
            // ResultArr2:
            // 1,1,1,1,1,1,1,1,1,1,1,1
            // 1,1,1,1,1,1,1,1,1,1,1,1
            // 1,1,1,1,1,1,1,1,1,1,1,1
            // 1,1,1,1,1,1,1,1,1,1,1,1
            // 1,1,1,1,1,1,1,1,1,1,1,1
            // 1,1,1,1,1,1,1,1,1,1,1,1
            // 1,1,1,1,1,1,1,1,1,1,1,1
            // 1,1,1,1,1,1,1,1,1,1,1,1
            // 1,1,1,1,1,1,1,1,1,1,1,1
            // 1,1,1,1,1,1,1,1,1,1,1,1
            // 1,1,1,1,1,1,1,1,1,1,1,1

            // X,Y = .
            // X+1,y = ^
            // X,Y+1 = *
            // X+1,Y+1 = #

            // Filling in like this;
            // .*
            // ^#
            // 1st .
            // 2nd *
            // 3rd ^
            // 4th #
            // on single loop.

            float[,] resultarr2 = new float[512, 512];
            for (int y = 0; y < WorldExtents.Y; y++)
            {
                for (int x = 0; x < WorldExtents.X; x++)
                {
                    resultarr2[y * 2, x * 2] = resultarr[y, x];

                    if (y < WorldExtents.Y)
                    {
                        resultarr2[(y * 2) + 1, x * 2] = resultarr[y, x];
                    }
                    if (x < WorldExtents.X)
                    {
                        resultarr2[y * 2, (x * 2) + 1] = resultarr[y, x];
                    }
                    if (x < WorldExtents.X && y < WorldExtents.Y)
                    {
                        resultarr2[(y * 2) + 1, (x * 2) + 1] = resultarr[y, x];
                    }
                }
            }

            //Flatten out the array
            int i = 0;
            for (int y = 0; y < 512; y++)
            {
                for (int x = 0; x < 512; x++)
                {
                    if (resultarr2[y, x] <= 0)
                        returnarr[i] = 0.0000001f;
                    else
                        returnarr[i] = resultarr2[y, x];

                    i++;
                }
            }

            return returnarr;
        }

        public float[] ResizeTerrain512Interpolation(float[] heightMap)
        {
            float[] returnarr = new float[262144];
            float[,] resultarr = new float[512,512];

            // Filling out the array into its multi-dimensional components
            for (int y = 0; y < 256; y++)
            {
                for (int x = 0; x < 256; x++)
                {
                    resultarr[y, x] = heightMap[y * 256 + x];
                }
            }

            // Resize using interpolation

            // This particular way is quick but it only works on a multiple of the original

            // The idea behind this method can be described with the following diagrams
            // second pass and third pass happen in the same loop really..  just separated
            // them to show what this does.

            // First Pass
            // ResultArr:
            // 1,1,1,1,1,1
            // 1,1,1,1,1,1
            // 1,1,1,1,1,1
            // 1,1,1,1,1,1
            // 1,1,1,1,1,1
            // 1,1,1,1,1,1

            // Second Pass
            // ResultArr2:
            // 1,,1,,1,,1,,1,,1,
            // ,,,,,,,,,,
            // 1,,1,,1,,1,,1,,1,
            // ,,,,,,,,,,
            // 1,,1,,1,,1,,1,,1,
            // ,,,,,,,,,,
            // 1,,1,,1,,1,,1,,1,
            // ,,,,,,,,,,
            // 1,,1,,1,,1,,1,,1,
            // ,,,,,,,,,,
            // 1,,1,,1,,1,,1,,1,

            // Third pass fills in the blanks
            // ResultArr2:
            // 1,1,1,1,1,1,1,1,1,1,1,1
            // 1,1,1,1,1,1,1,1,1,1,1,1
            // 1,1,1,1,1,1,1,1,1,1,1,1
            // 1,1,1,1,1,1,1,1,1,1,1,1
            // 1,1,1,1,1,1,1,1,1,1,1,1
            // 1,1,1,1,1,1,1,1,1,1,1,1
            // 1,1,1,1,1,1,1,1,1,1,1,1
            // 1,1,1,1,1,1,1,1,1,1,1,1
            // 1,1,1,1,1,1,1,1,1,1,1,1
            // 1,1,1,1,1,1,1,1,1,1,1,1
            // 1,1,1,1,1,1,1,1,1,1,1,1

            // X,Y = .
            // X+1,y = ^
            // X,Y+1 = *
            // X+1,Y+1 = #

            // Filling in like this;
            // .*
            // ^#
            // 1st .
            // 2nd *
            // 3rd ^
            // 4th #
            // on single loop.

            float[,] resultarr2 = new float[512,512];
            for (int y = 0; y < (int)Constants.RegionSize; y++)
            {
                for (int x = 0; x < (int)Constants.RegionSize; x++)
                {
                    resultarr2[y*2, x*2] = resultarr[y, x];

                    if (y < (int)Constants.RegionSize)
                    {
                        if (y + 1 < (int)Constants.RegionSize)
                        {
                            if (x + 1 < (int)Constants.RegionSize)
                            {
                                resultarr2[(y*2) + 1, x*2] = ((resultarr[y, x] + resultarr[y + 1, x] +
                                                               resultarr[y, x + 1] + resultarr[y + 1, x + 1])/4);
                            }
                            else
                            {
                                resultarr2[(y*2) + 1, x*2] = ((resultarr[y, x] + resultarr[y + 1, x])/2);
                            }
                        }
                        else
                        {
                            resultarr2[(y*2) + 1, x*2] = resultarr[y, x];
                        }
                    }
                    if (x < (int)Constants.RegionSize)
                    {
                        if (x + 1 < (int)Constants.RegionSize)
                        {
                            if (y + 1 < (int)Constants.RegionSize)
                            {
                                resultarr2[y*2, (x*2) + 1] = ((resultarr[y, x] + resultarr[y + 1, x] +
                                                               resultarr[y, x + 1] + resultarr[y + 1, x + 1])/4);
                            }
                            else
                            {
                                resultarr2[y*2, (x*2) + 1] = ((resultarr[y, x] + resultarr[y, x + 1])/2);
                            }
                        }
                        else
                        {
                            resultarr2[y*2, (x*2) + 1] = resultarr[y, x];
                        }
                    }
                    if (x < (int)Constants.RegionSize && y < (int)Constants.RegionSize)
                    {
                        if ((x + 1 < (int)Constants.RegionSize) && (y + 1 < (int)Constants.RegionSize))
                        {
                            resultarr2[(y*2) + 1, (x*2) + 1] = ((resultarr[y, x] + resultarr[y + 1, x] +
                                                                 resultarr[y, x + 1] + resultarr[y + 1, x + 1])/4);
                        }
                        else
                        {
                            resultarr2[(y*2) + 1, (x*2) + 1] = resultarr[y, x];
                        }
                    }
                }
            }
            //Flatten out the array
            int i = 0;
            for (int y = 0; y < 512; y++)
            {
                for (int x = 0; x < 512; x++)
                {
                    if (Single.IsNaN(resultarr2[y, x]) || Single.IsInfinity(resultarr2[y, x]))
                    {
                        m_log.Warn("[PHYSICS]: Non finite heightfield element detected.  Setting it to 0");
                        resultarr2[y, x] = 0;
                    }
                    returnarr[i] = resultarr2[y, x];
                    i++;
                }
            }

            return returnarr;
        }

        #endregion

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

            float[] _heightmap;
            _heightmap = new float[(((int)Constants.RegionSize + 2) * ((int)Constants.RegionSize + 2))];

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

            uint heightmapWidthSamples;

            uint heightmapHeightSamples;
 
            heightmapWidthSamples = (uint)Constants.RegionSize + 2;
            heightmapHeightSamples = (uint)Constants.RegionSize + 2;

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

            int regionsize = (int) Constants.RegionSize + 2;
 
            float hfmin = float.MaxValue;
            float hfmax = float.MinValue;
            float val;
            int xx;
            int yy;

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

            int xt = 0;
            xx = 0;

            for (int x = 0; x < heightmapWidthSamples; x++)
            {
                if (x > 1 && xx < maxXXYY)
                    xx++;
                yy = 0;
                for (int y = 0; y < heightmapHeightSamples; y++)
                {
                    if (y > 1 && y < maxXXYY)
                        yy += (int)Constants.RegionSize;

                    val = heightMap[yy + xx];
                    _heightmap[xt + y] = val;

                    if (hfmin > val)
                        hfmin = val;
                    if (hfmax < val)
                        hfmax = val;

                }

                xt += regionsize;
            }
            lock (OdeLock)
            {
                IntPtr GroundGeom = IntPtr.Zero;
                if (RegionTerrain.TryGetValue(pOffset, out GroundGeom))
                {
                    RegionTerrain.Remove(pOffset);
                    if (GroundGeom != IntPtr.Zero)
                    {
                        if (TerrainHeightFieldHeights.ContainsKey(GroundGeom))
                            {
                            TerrainHeightFieldHeightsHandlers[GroundGeom].Free();
                            TerrainHeightFieldHeightsHandlers.Remove(GroundGeom);
                            TerrainHeightFieldHeights.Remove(GroundGeom);
                            }
                        d.SpaceRemove(StaticSpace, GroundGeom);
                        d.GeomDestroy(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, (int)(CollisionCategories.Land));
                    d.GeomSetCollideBits(GroundGeom, (int)(CollisionCategories.Space));

                }
                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 - 0.5f, pOffset.Y + (float)Constants.RegionSize * 0.5f - 0.5f, 0);
                IntPtr testGround = IntPtr.Zero;
                if (RegionTerrain.TryGetValue(pOffset, out testGround))
                {
                    RegionTerrain.Remove(pOffset);
                }
                RegionTerrain.Add(pOffset, GroundGeom, GroundGeom);
//                TerrainHeightFieldHeights.Add(GroundGeom, ODElandMap);
                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 = m_regionWidth + 2;
            const uint heightmapHeight = m_regionHeight + 2;
            const uint heightmapWidthSamples = m_regionWidth + 2;
            const uint heightmapHeightSamples = m_regionHeight + 2;
            const float scale = 1.0f;
            const float offset = 0.0f;
            const float thickness = 2.9f;
            const int wrap = 0;

            for (int i = 0; i < (258 * 258); i++)
            {
                _watermap[i] = (baseheight-0.1f) + ((float)fluidRandomizer.Next(1,9) / 10f);
               // m_log.Info((baseheight - 0.1f) + ((float)fluidRandomizer.Next(1, 9) / 10f));
            }

            lock (OdeLock)
            {
                if (WaterGeom != IntPtr.Zero)
                {
                d.SpaceRemove(StaticSpace, WaterGeom);
                }
                IntPtr HeightmapData = d.GeomHeightfieldDataCreate();
                d.GeomHeightfieldDataBuildSingle(HeightmapData, _watermap, 0, heightmapWidth, heightmapHeight,
                                                 (int)heightmapWidthSamples, (int)heightmapHeightSamples, scale,
                                                 offset, thickness, wrap);
                d.GeomHeightfieldDataSetBounds(HeightmapData, m_regionWidth, m_regionHeight);
                WaterGeom = d.CreateHeightfield(StaticSpace, HeightmapData, 1);
                if (WaterGeom != IntPtr.Zero)
                {
                    d.GeomSetCategoryBits(WaterGeom, (int)(CollisionCategories.Water));
                    d.GeomSetCollideBits(WaterGeom, (int)(CollisionCategories.Space));

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

        }

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

            lock (OdeLock)
            {
                lock (_prims)
                {
                    foreach (OdePrim prm in _prims)
                    {
                        RemovePrim(prm);
                    }
                }

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

                d.WorldDestroy(world);
                //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 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>();
        }

#if USE_DRAWSTUFF
        // Keyboard callback
        public void command(int cmd)
        {
            IntPtr geom;
            d.Mass mass;
            d.Vector3 sides = new d.Vector3(d.RandReal() * 0.5f + 0.1f, d.RandReal() * 0.5f + 0.1f, d.RandReal() * 0.5f + 0.1f);

            

            Char ch = Char.ToLower((Char)cmd);
            switch ((Char)ch)
            {
                case 'w':
                    try
                    {
                        Vector3 rotate = (new Vector3(1, 0, 0) * Quaternion.CreateFromEulers(hpr.Z * Utils.DEG_TO_RAD, hpr.Y * Utils.DEG_TO_RAD, hpr.X * Utils.DEG_TO_RAD));

                        xyz.X += rotate.X; xyz.Y += rotate.Y; xyz.Z += rotate.Z;
                        ds.SetViewpoint(ref xyz, ref hpr);
                    }
                    catch (ArgumentException)
                    { hpr.X = 0; }
                    break;

                case 'a':
                    hpr.X++;
                    ds.SetViewpoint(ref xyz, ref hpr);
                    break;

                case 's':
                    try
                    {
                        Vector3 rotate2 = (new Vector3(-1, 0, 0) * Quaternion.CreateFromEulers(hpr.Z * Utils.DEG_TO_RAD, hpr.Y * Utils.DEG_TO_RAD, hpr.X * Utils.DEG_TO_RAD));

                        xyz.X += rotate2.X; xyz.Y += rotate2.Y; xyz.Z += rotate2.Z;
                        ds.SetViewpoint(ref xyz, ref hpr);
                    }
                    catch (ArgumentException)
                    { hpr.X = 0; }
                    break;
                case 'd':
                    hpr.X--;
                    ds.SetViewpoint(ref xyz, ref hpr);
                    break;
                case 'r':
                    xyz.Z++;
                    ds.SetViewpoint(ref xyz, ref hpr);
                    break;
                case 'f':
                    xyz.Z--;
                    ds.SetViewpoint(ref xyz, ref hpr);
                    break;
                case 'e':
                    xyz.Y++;
                    ds.SetViewpoint(ref xyz, ref hpr);
                    break;
                case 'q':
                    xyz.Y--;
                    ds.SetViewpoint(ref xyz, ref hpr);
                    break;
            }
        }

        public void step(int pause)
        {
            
            ds.SetColor(1.0f, 1.0f, 0.0f);
            ds.SetTexture(ds.Texture.Wood);
            lock (_prims)
            {
                foreach (OdePrim prm in _prims)
                {
                    //IntPtr body = d.GeomGetBody(prm.prim_geom);
                    if (prm.prim_geom != IntPtr.Zero)
                    {
                        d.Vector3 pos;
                        d.GeomCopyPosition(prm.prim_geom, out pos);
                        //d.BodyCopyPosition(body, out pos);

                        d.Matrix3 R;
                        d.GeomCopyRotation(prm.prim_geom, out R);
                        //d.BodyCopyRotation(body, out R);


                        d.Vector3 sides = new d.Vector3();
                        sides.X = prm.Size.X;
                        sides.Y = prm.Size.Y;
                        sides.Z = prm.Size.Z;

                        ds.DrawBox(ref pos, ref R, ref sides);
                    }
                }
            }
            ds.SetColor(1.0f, 0.0f, 0.0f);
            lock (_characters)
            {
                foreach (OdeCharacter chr in _characters)
                {
                    if (chr.Shell != IntPtr.Zero)
                    {
                        IntPtr body = d.GeomGetBody(chr.Shell);

                        d.Vector3 pos;
                        d.GeomCopyPosition(chr.Shell, out pos);
                        //d.BodyCopyPosition(body, out pos);

                        d.Matrix3 R;
                        d.GeomCopyRotation(chr.Shell, out R);
                        //d.BodyCopyRotation(body, out R);

                        ds.DrawCapsule(ref pos, ref R, chr.Size.Z, 0.35f);
                        d.Vector3 sides = new d.Vector3();
                        sides.X = 0.5f;
                        sides.Y = 0.5f;
                        sides.Z = 0.5f;

                        ds.DrawBox(ref pos, ref R, ref sides);
                    }
                }
            }
        }

        public void start(int unused)
        {
            ds.SetViewpoint(ref xyz, ref hpr);
        }
#endif
    }
}