From 5e4d6cab00cb29cd088ab7b62ab13aff103b64cb Mon Sep 17 00:00:00 2001
From: onefang
Date: Sun, 19 May 2019 21:24:15 +1000
Subject: Dump OpenSim 0.9.0.1 into it's own branch.
---
OpenSim/Region/PhysicsModules/ubOde/ODEScene.cs | 2824 +++++++++++++++++++++++
1 file changed, 2824 insertions(+)
create mode 100644 OpenSim/Region/PhysicsModules/ubOde/ODEScene.cs
(limited to 'OpenSim/Region/PhysicsModules/ubOde/ODEScene.cs')
diff --git a/OpenSim/Region/PhysicsModules/ubOde/ODEScene.cs b/OpenSim/Region/PhysicsModules/ubOde/ODEScene.cs
new file mode 100644
index 0000000..0b51820
--- /dev/null
+++ b/OpenSim/Region/PhysicsModules/ubOde/ODEScene.cs
@@ -0,0 +1,2824 @@
+/*
+ * Copyright (c) Contributors, http://opensimulator.org/
+ * See CONTRIBUTORS.TXT for a full list of copyright holders.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ * * Neither the name of the OpenSimulator Project nor the
+ * names of its contributors may be used to endorse or promote products
+ * derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+// Revision 2011/12/13 by Ubit Umarov
+//#define SPAM
+
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Reflection;
+using System.Runtime.InteropServices;
+using System.Threading;
+using System.IO;
+using System.Diagnostics;
+using log4net;
+using Nini.Config;
+using Mono.Addins;
+using OdeAPI;
+using OpenSim.Framework;
+using OpenSim.Region.Framework.Scenes;
+using OpenSim.Region.Framework.Interfaces;
+using OpenSim.Region.PhysicsModules.SharedBase;
+using OpenMetaverse;
+
+namespace OpenSim.Region.PhysicsModule.ubOde
+{
+ // colision flags of things others can colide with
+ // rays, sensors, probes removed since can't be colided with
+ // The top space where things are placed provided further selection
+ // ie physical are in active space nonphysical in static
+ // this should be exclusive as possible
+
+ [Flags]
+ public enum CollisionCategories : uint
+ {
+ Disabled = 0,
+ //by 'things' types
+ Space = 0x01,
+ Geom = 0x02, // aka prim/part
+ Character = 0x04,
+ Land = 0x08,
+ Water = 0x010,
+
+ // by state
+ Phantom = 0x01000,
+ VolumeDtc = 0x02000,
+ Selected = 0x04000,
+ NoShape = 0x08000,
+
+
+ All = 0xffffffff
+ }
+
+ ///
+ /// Material type for a primitive
+ ///
+ public enum Material : int
+ {
+ ///
+ Stone = 0,
+ ///
+ Metal = 1,
+ ///
+ Glass = 2,
+ ///
+ Wood = 3,
+ ///
+ Flesh = 4,
+ ///
+ Plastic = 5,
+ ///
+ 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,
+ TargetVelocity,
+ AngVelocity,
+ Acceleration,
+ Force,
+ Torque,
+ Momentum,
+
+ AddForce,
+ AddAngForce,
+ AngLock,
+
+ Buoyancy,
+
+ PIDTarget,
+ PIDTau,
+ PIDActive,
+
+ PIDHoverHeight,
+ PIDHoverType,
+ PIDHoverTau,
+ PIDHoverActive,
+
+ Size,
+ AvatarSize,
+ Shape,
+ PhysRepData,
+ AddPhysRep,
+
+ CollidesWater,
+ VolumeDtc,
+
+ Physical,
+ Phantom,
+ Selected,
+ disabled,
+ building,
+
+ VehicleType,
+ VehicleFloatParam,
+ VehicleVectorParam,
+ VehicleRotationParam,
+ VehicleFlags,
+ SetVehicle,
+ SetInertia,
+
+ Null //keep this last used do dim the methods array. does nothing but pulsing the prim
+ }
+
+ public struct ODEchangeitem
+ {
+ public PhysicsActor actor;
+ public OdeCharacter character;
+ public changes what;
+ public Object arg;
+ }
+
+ public class ODEScene : PhysicsScene
+ {
+ private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
+
+ public bool m_OSOdeLib = false;
+ public Scene m_frameWorkScene = null;
+
+// private int threadid = 0;
+
+// const d.ContactFlags comumContactFlags = d.ContactFlags.SoftERP | d.ContactFlags.SoftCFM |d.ContactFlags.Approx1 | d.ContactFlags.Bounce;
+
+ const d.ContactFlags comumContactFlags = d.ContactFlags.Bounce | d.ContactFlags.Approx1 | d.ContactFlags.Slip1 | d.ContactFlags.Slip2;
+ const float comumContactERP = 0.75f;
+ const float comumContactCFM = 0.0001f;
+ const float comumContactSLIP = 0f;
+
+ float frictionMovementMult = 0.8f;
+
+ float TerrainBounce = 0.001f;
+ float TerrainFriction = 0.3f;
+
+ public float AvatarFriction = 0;// 0.9f * 0.5f;
+
+ // this netx dimensions are only relevant for terrain partition (mega regions)
+ // WorldExtents below has the simulation dimensions
+ // they should be identical except on mega regions
+ private uint m_regionWidth = Constants.RegionSize;
+ private uint m_regionHeight = Constants.RegionSize;
+
+ public float ODE_STEPSIZE = 0.020f;
+ public float HalfOdeStep = 0.01f;
+ public int odetimestepMS = 20; // rounded
+ private float metersInSpace = 25.6f;
+ private float m_timeDilation = 1.0f;
+
+ private double m_lastframe;
+ private double m_lastMeshExpire;
+
+ public float gravityx = 0f;
+ public float gravityy = 0f;
+ public float gravityz = -9.8f;
+
+ private float waterlevel = 0f;
+ private int framecount = 0;
+
+ private float avDensity = 80f;
+ private float avMovementDivisorWalk = 1.3f;
+ private float avMovementDivisorRun = 0.8f;
+ private float minimumGroundFlightOffset = 3f;
+ public float maximumMassObject = 10000.01f;
+ public float geomDefaultDensity = 10.0f;
+
+ public float maximumAngularVelocity = 12.0f; // default 12rad/s
+ public float maxAngVelocitySQ = 144f; // squared value
+
+ public float bodyPIDD = 35f;
+ public float bodyPIDG = 25;
+
+ public int bodyFramesAutoDisable = 5;
+
+ private d.NearCallback nearCallback;
+
+ private Dictionary _prims = new Dictionary();
+ private HashSet _characters = new HashSet();
+ private HashSet _activeprims = new HashSet();
+ private HashSet _activegroups = new HashSet();
+
+ public OpenSim.Framework.LocklessQueue ChangesQueue = new OpenSim.Framework.LocklessQueue();
+
+ ///
+ /// A list of actors that should receive collision events.
+ ///
+ private List _collisionEventPrim = new List();
+ private List _collisionEventPrimRemove = new List();
+
+ private HashSet _badCharacter = new HashSet();
+ public Dictionary actor_name_map = new Dictionary();
+
+ private float contactsurfacelayer = 0.002f;
+
+ private int contactsPerCollision = 80;
+ internal IntPtr ContactgeomsArray = IntPtr.Zero;
+ private IntPtr GlobalContactsArray = IntPtr.Zero;
+ private d.Contact SharedTmpcontact = new d.Contact();
+
+ const int maxContactsbeforedeath = 6000;
+ private volatile int m_global_contactcount = 0;
+
+ private IntPtr contactgroup;
+
+ public ContactData[] m_materialContactsData = new ContactData[8];
+
+ private IntPtr TerrainGeom;
+ private float[] TerrainHeightFieldHeight;
+ private GCHandle TerrainHeightFieldHeightsHandler = new GCHandle();
+
+ private int m_physicsiterations = 15;
+ private const float m_SkipFramesAtms = 0.40f; // Drop frames gracefully at a 400 ms lag
+// private PhysicsActor PANull = new NullPhysicsActor();
+ private float step_time = 0.0f;
+
+ public IntPtr world;
+
+ // split the spaces acording to contents type
+ // ActiveSpace contains characters and active prims
+ // StaticSpace contains land and other that is mostly static in enviroment
+ // this can contain subspaces, like the grid in staticspace
+ // as now space only contains this 2 top spaces
+
+ public IntPtr TopSpace; // the global space
+ public IntPtr ActiveSpace; // space for active prims
+ public IntPtr CharsSpace; // space for active prims
+ public IntPtr StaticSpace; // space for the static things around
+ public IntPtr GroundSpace; // space for ground
+
+ // some speedup variables
+ private int spaceGridMaxX;
+ private int spaceGridMaxY;
+ private float spacesPerMeterX;
+ private float spacesPerMeterY;
+
+ // split static geometry collision into a grid as before
+ private IntPtr[,] staticPrimspace;
+ private IntPtr[] staticPrimspaceOffRegion;
+
+ public Object OdeLock;
+ public static Object SimulationLock;
+
+ public IMesher mesher;
+
+ public IConfigSource m_config;
+
+ public bool physics_logging = false;
+ public int physics_logging_interval = 0;
+ public bool physics_logging_append_existing_logfile = false;
+
+ public Vector2 WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize);
+
+ private ODERayCastRequestManager m_rayCastManager;
+ public ODEMeshWorker m_meshWorker;
+
+ /* maybe needed if ode uses tls
+ private void checkThread()
+ {
+
+ int th = Thread.CurrentThread.ManagedThreadId;
+ if(th != threadid)
+ {
+ threadid = th;
+ d.AllocateODEDataForThread(~0U);
+ }
+ }
+ */
+
+ IConfig physicsconfig = null;
+
+ public ODEScene(Scene pscene, IConfigSource psourceconfig, string pname, string pversion, bool pOSOdeLib)
+ {
+ OdeLock = new Object();
+
+ EngineType = pname;
+ PhysicsSceneName = EngineType + "/" + pscene.RegionInfo.RegionName;
+ EngineName = pname + " " + pversion;
+ m_config = psourceconfig;
+ m_OSOdeLib = pOSOdeLib;
+
+// m_OSOdeLib = false; //debug
+
+ m_frameWorkScene = pscene;
+
+ m_frameWorkScene.RegisterModuleInterface(this);
+
+ Initialization();
+
+ base.Initialise(m_frameWorkScene.PhysicsRequestAsset,
+ (m_frameWorkScene.Heightmap != null ? m_frameWorkScene.Heightmap.GetFloatsSerialised() : new float[m_frameWorkScene.RegionInfo.RegionSizeX * m_frameWorkScene.RegionInfo.RegionSizeY]),
+ (float)m_frameWorkScene.RegionInfo.RegionSettings.WaterHeight);
+ }
+
+ public void RegionLoaded()
+ {
+ mesher = m_frameWorkScene.RequestModuleInterface();
+ if (mesher == null)
+ {
+ m_log.ErrorFormat("[ubOde] No mesher. module disabled");
+ return;
+ }
+
+ m_meshWorker = new ODEMeshWorker(this, m_log, mesher, physicsconfig);
+ m_frameWorkScene.PhysicsEnabled = true;
+ }
+ ///
+ /// Initiailizes the scene
+ /// Sets many properties that ODE requires to be stable
+ /// These settings need to be tweaked 'exactly' right or weird stuff happens.
+ ///
+ private void Initialization()
+ {
+ d.AllocateODEDataForThread(~0U);
+
+ SimulationLock = new Object();
+
+ nearCallback = near;
+
+ m_rayCastManager = new ODERayCastRequestManager(this);
+
+ WorldExtents.X = m_frameWorkScene.RegionInfo.RegionSizeX;
+ m_regionWidth = (uint)WorldExtents.X;
+ WorldExtents.Y = m_frameWorkScene.RegionInfo.RegionSizeY;
+ m_regionHeight = (uint)WorldExtents.Y;
+
+ 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);
+ CharsSpace = d.HashSpaceCreate(TopSpace);
+ StaticSpace = d.HashSpaceCreate(TopSpace);
+ GroundSpace = d.HashSpaceCreate(TopSpace);
+ }
+ catch
+ {
+ // i must RtC#FM
+ // i did!
+ }
+
+ d.HashSpaceSetLevels(TopSpace, -5, 12);
+ d.HashSpaceSetLevels(ActiveSpace, -5, 10);
+ d.HashSpaceSetLevels(CharsSpace, -4, 3);
+ d.HashSpaceSetLevels(StaticSpace, -5, 12);
+ d.HashSpaceSetLevels(GroundSpace, 0, 8);
+
+ // demote to second level
+ d.SpaceSetSublevel(ActiveSpace, 1);
+ d.SpaceSetSublevel(CharsSpace, 1);
+ d.SpaceSetSublevel(StaticSpace, 1);
+ d.SpaceSetSublevel(GroundSpace, 1);
+
+ d.GeomSetCategoryBits(ActiveSpace, (uint)(CollisionCategories.Space |
+ CollisionCategories.Geom |
+ CollisionCategories.Character |
+ CollisionCategories.Phantom |
+ CollisionCategories.VolumeDtc
+ ));
+ d.GeomSetCollideBits(ActiveSpace, (uint)(CollisionCategories.Space |
+ CollisionCategories.Geom |
+ CollisionCategories.Character |
+ CollisionCategories.Phantom |
+ CollisionCategories.VolumeDtc
+ ));
+ d.GeomSetCategoryBits(CharsSpace, (uint)(CollisionCategories.Space |
+ CollisionCategories.Geom |
+ CollisionCategories.Character |
+ CollisionCategories.Phantom |
+ CollisionCategories.VolumeDtc
+ ));
+ d.GeomSetCollideBits(CharsSpace, 0);
+
+ d.GeomSetCategoryBits(StaticSpace, (uint)(CollisionCategories.Space |
+ CollisionCategories.Geom |
+ // CollisionCategories.Land |
+ // CollisionCategories.Water |
+ CollisionCategories.Phantom |
+ CollisionCategories.VolumeDtc
+ ));
+ d.GeomSetCollideBits(StaticSpace, 0);
+
+ d.GeomSetCategoryBits(GroundSpace, (uint)(CollisionCategories.Land));
+ d.GeomSetCollideBits(GroundSpace, 0);
+
+ contactgroup = d.JointGroupCreate(maxContactsbeforedeath + 1);
+ //contactgroup
+
+ d.WorldSetAutoDisableFlag(world, false);
+ }
+
+
+ // checkThread();
+
+
+ // Defaults
+
+ int contactsPerCollision = 80;
+
+ physicsconfig = null;
+
+ if (m_config != null)
+ {
+ physicsconfig = m_config.Configs["ODEPhysicsSettings"];
+ if (physicsconfig != null)
+ {
+ gravityx = physicsconfig.GetFloat("world_gravityx", gravityx);
+ gravityy = physicsconfig.GetFloat("world_gravityy", gravityy);
+ gravityz = physicsconfig.GetFloat("world_gravityz", gravityz);
+
+ metersInSpace = physicsconfig.GetFloat("meters_in_small_space", metersInSpace);
+
+ // contactsurfacelayer = physicsconfig.GetFloat("world_contact_surface_layer", contactsurfacelayer);
+
+ ODE_STEPSIZE = physicsconfig.GetFloat("world_stepsize", ODE_STEPSIZE);
+
+ avDensity = physicsconfig.GetFloat("av_density", avDensity);
+ avMovementDivisorWalk = physicsconfig.GetFloat("av_movement_divisor_walk", avMovementDivisorWalk);
+ avMovementDivisorRun = physicsconfig.GetFloat("av_movement_divisor_run", avMovementDivisorRun);
+
+ contactsPerCollision = physicsconfig.GetInt("contacts_per_collision", contactsPerCollision);
+
+ geomDefaultDensity = physicsconfig.GetFloat("geometry_default_density", geomDefaultDensity);
+// bodyFramesAutoDisable = physicsconfig.GetInt("body_frames_auto_disable", bodyFramesAutoDisable);
+
+ physics_logging = physicsconfig.GetBoolean("physics_logging", false);
+ physics_logging_interval = physicsconfig.GetInt("physics_logging_interval", 0);
+ physics_logging_append_existing_logfile = physicsconfig.GetBoolean("physics_logging_append_existing_logfile", false);
+
+ minimumGroundFlightOffset = physicsconfig.GetFloat("minimum_ground_flight_offset", minimumGroundFlightOffset);
+ maximumMassObject = physicsconfig.GetFloat("maximum_mass_object", maximumMassObject);
+
+ avDensity *= 3f / 80f; // scale other engines density option to this
+ }
+ }
+
+ float heartbeat = 1/m_frameWorkScene.FrameTime;
+ maximumAngularVelocity = 0.49f * heartbeat *(float)Math.PI;
+ maxAngVelocitySQ = maximumAngularVelocity * maximumAngularVelocity;
+
+ d.WorldSetCFM(world, comumContactCFM);
+ d.WorldSetERP(world, comumContactERP);
+
+ d.WorldSetGravity(world, gravityx, gravityy, gravityz);
+
+ d.WorldSetLinearDamping(world, 0.001f);
+ d.WorldSetAngularDamping(world, 0.002f);
+ d.WorldSetAngularDampingThreshold(world, 0f);
+ d.WorldSetLinearDampingThreshold(world, 0f);
+ d.WorldSetMaxAngularSpeed(world, maximumAngularVelocity);
+
+ d.WorldSetQuickStepNumIterations(world, m_physicsiterations);
+
+ d.WorldSetContactSurfaceLayer(world, contactsurfacelayer);
+ d.WorldSetContactMaxCorrectingVel(world, 60.0f);
+
+ HalfOdeStep = ODE_STEPSIZE * 0.5f;
+ odetimestepMS = (int)(1000.0f * ODE_STEPSIZE + 0.5f);
+
+ ContactgeomsArray = Marshal.AllocHGlobal(contactsPerCollision * d.ContactGeom.unmanagedSizeOf);
+ GlobalContactsArray = Marshal.AllocHGlobal((maxContactsbeforedeath + 100) * d.Contact.unmanagedSizeOf);
+
+ SharedTmpcontact.geom.g1 = IntPtr.Zero;
+ SharedTmpcontact.geom.g2 = IntPtr.Zero;
+
+ SharedTmpcontact.geom.side1 = -1;
+ SharedTmpcontact.geom.side2 = -1;
+
+ SharedTmpcontact.surface.mode = comumContactFlags;
+ SharedTmpcontact.surface.mu = 0;
+ SharedTmpcontact.surface.bounce = 0;
+ SharedTmpcontact.surface.bounce_vel = 1.5f;
+ SharedTmpcontact.surface.soft_cfm = comumContactCFM;
+ SharedTmpcontact.surface.soft_erp = comumContactERP;
+ SharedTmpcontact.surface.slip1 = comumContactSLIP;
+ SharedTmpcontact.surface.slip2 = comumContactSLIP;
+
+ m_materialContactsData[(int)Material.Stone].mu = 0.8f;
+ m_materialContactsData[(int)Material.Stone].bounce = 0.4f;
+
+ m_materialContactsData[(int)Material.Metal].mu = 0.3f;
+ m_materialContactsData[(int)Material.Metal].bounce = 0.4f;
+
+ m_materialContactsData[(int)Material.Glass].mu = 0.2f;
+ m_materialContactsData[(int)Material.Glass].bounce = 0.7f;
+
+ m_materialContactsData[(int)Material.Wood].mu = 0.6f;
+ m_materialContactsData[(int)Material.Wood].bounce = 0.5f;
+
+ m_materialContactsData[(int)Material.Flesh].mu = 0.9f;
+ m_materialContactsData[(int)Material.Flesh].bounce = 0.3f;
+
+ m_materialContactsData[(int)Material.Plastic].mu = 0.4f;
+ m_materialContactsData[(int)Material.Plastic].bounce = 0.7f;
+
+ m_materialContactsData[(int)Material.Rubber].mu = 0.9f;
+ m_materialContactsData[(int)Material.Rubber].bounce = 0.95f;
+
+ m_materialContactsData[(int)Material.light].mu = 0.0f;
+ m_materialContactsData[(int)Material.light].bounce = 0.0f;
+
+
+ spacesPerMeterX = 1.0f / metersInSpace;
+ spacesPerMeterY = spacesPerMeterX;
+ spaceGridMaxX = (int)(WorldExtents.X * spacesPerMeterX);
+ spaceGridMaxY = (int)(WorldExtents.Y * spacesPerMeterY);
+
+ if (spaceGridMaxX > 24)
+ {
+ spaceGridMaxX = 24;
+ spacesPerMeterX = spaceGridMaxX / WorldExtents.X;
+ }
+
+ if (spaceGridMaxY > 24)
+ {
+ spaceGridMaxY = 24;
+ spacesPerMeterY = spaceGridMaxY / WorldExtents.Y;
+ }
+
+ staticPrimspace = new IntPtr[spaceGridMaxX, spaceGridMaxY];
+
+ // create all spaces now
+ int i, j;
+ IntPtr newspace;
+
+ for (i = 0; i < spaceGridMaxX; i++)
+ for (j = 0; j < spaceGridMaxY; j++)
+ {
+ newspace = d.HashSpaceCreate(StaticSpace);
+ d.GeomSetCategoryBits(newspace, (int)CollisionCategories.Space);
+ waitForSpaceUnlock(newspace);
+ d.SpaceSetSublevel(newspace, 2);
+ d.HashSpaceSetLevels(newspace, -2, 8);
+ d.GeomSetCategoryBits(newspace, (uint)(CollisionCategories.Space |
+ CollisionCategories.Geom |
+ CollisionCategories.Land |
+ CollisionCategories.Water |
+ CollisionCategories.Phantom |
+ CollisionCategories.VolumeDtc
+ ));
+ d.GeomSetCollideBits(newspace, 0);
+
+ staticPrimspace[i, j] = newspace;
+ }
+
+ // let this now be index limit
+ spaceGridMaxX--;
+ spaceGridMaxY--;
+
+ // create 4 off world spaces (x<0,x>max,y<0,y>max)
+ staticPrimspaceOffRegion = new IntPtr[4];
+
+ for (i = 0; i < 4; i++)
+ {
+ newspace = d.HashSpaceCreate(StaticSpace);
+ d.GeomSetCategoryBits(newspace, (int)CollisionCategories.Space);
+ waitForSpaceUnlock(newspace);
+ d.SpaceSetSublevel(newspace, 2);
+ d.HashSpaceSetLevels(newspace, -2, 8);
+ d.GeomSetCategoryBits(newspace, (uint)(CollisionCategories.Space |
+ CollisionCategories.Geom |
+ CollisionCategories.Land |
+ CollisionCategories.Water |
+ CollisionCategories.Phantom |
+ CollisionCategories.VolumeDtc
+ ));
+ d.GeomSetCollideBits(newspace, 0);
+
+ staticPrimspaceOffRegion[i] = newspace;
+ }
+
+ m_lastframe = Util.GetTimeStamp();
+ m_lastMeshExpire = m_lastframe;
+ step_time = -1;
+ }
+
+ 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,bool smooth)
+ {
+ if (m_global_contactcount >= maxContactsbeforedeath)
+ return IntPtr.Zero;
+
+ m_global_contactcount++;
+ if(smooth)
+ SharedTmpcontact.geom.depth = contactGeom.depth * 0.05f;
+ else
+ SharedTmpcontact.geom.depth = contactGeom.depth;
+ SharedTmpcontact.geom.pos = contactGeom.pos;
+ SharedTmpcontact.geom.normal = contactGeom.normal;
+
+ IntPtr contact = new IntPtr(GlobalContactsArray.ToInt64() + (Int64)(m_global_contactcount * d.Contact.unmanagedSizeOf));
+ Marshal.StructureToPtr(SharedTmpcontact, contact, true);
+ return d.JointCreateContactPtr(world, contactgroup, contact);
+ }
+
+ private bool GetCurContactGeom(int index, ref d.ContactGeom newcontactgeom)
+ {
+ if (ContactgeomsArray == IntPtr.Zero || index >= contactsPerCollision)
+ return false;
+
+ IntPtr contactptr = new IntPtr(ContactgeomsArray.ToInt64() + (Int64)(index * d.ContactGeom.unmanagedSizeOf));
+ newcontactgeom = (d.ContactGeom)Marshal.PtrToStructure(contactptr, typeof(d.ContactGeom));
+ return true;
+ }
+
+ ///
+ /// This is our near callback. A geometry is near a body
+ ///
+ /// The space that contains the geoms. Remember, spaces are also geoms
+ /// a geometry or space
+ /// another geometry or space
+ ///
+
+ 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;
+ /*
+ // debug
+ PhysicsActor dp2;
+ if (d.GeomGetClass(g1) == d.GeomClassID.HeightfieldClass)
+ {
+ d.AABB aabb;
+ d.GeomGetAABB(g2, out aabb);
+ float x = aabb.MaxX - aabb.MinX;
+ float y = aabb.MaxY - aabb.MinY;
+ float z = aabb.MaxZ - aabb.MinZ;
+ if (x > 60.0f || y > 60.0f || z > 60.0f)
+ {
+ if (!actor_name_map.TryGetValue(g2, out dp2))
+ m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 2");
+ else
+ m_log.WarnFormat("[PHYSICS]: land versus large prim geo {0},size {1}, AABBsize <{2},{3},{4}>, at {5} ori {6},({7})",
+ dp2.Name, dp2.Size, x, y, z,
+ dp2.Position.ToString(),
+ dp2.Orientation.ToString(),
+ dp2.Orientation.Length());
+ return;
+ }
+ }
+ //
+ */
+
+
+ if (d.GeomGetCategoryBits(g1) == (uint)CollisionCategories.VolumeDtc ||
+ d.GeomGetCategoryBits(g2) == (uint)CollisionCategories.VolumeDtc)
+ {
+ int cflags;
+ unchecked
+ {
+ cflags = (int)(1 | d.CONTACTS_UNIMPORTANT);
+ }
+ count = d.CollidePtr(g1, g2, cflags, ContactgeomsArray, d.ContactGeom.unmanagedSizeOf);
+ }
+ else
+ count = d.CollidePtr(g1, g2, (contactsPerCollision & 0xffff), ContactgeomsArray, d.ContactGeom.unmanagedSizeOf);
+ }
+ catch (SEHException)
+ {
+ m_log.Error("[PHYSICS]: The Operating system shut down ODE because of corrupt memory. This could be a result of really irregular terrain. If this repeats continuously, restart using Basic Physics and terrain fill your terrain. Restarting the sim.");
+ // ode.drelease(world);
+ base.TriggerPhysicsBasedRestart();
+ }
+ catch (Exception e)
+ {
+ m_log.WarnFormat("[PHYSICS]: Unable to collide test an object: {0}", e.Message);
+ return;
+ }
+
+ // contacts done
+ if (count == 0)
+ return;
+
+ // try get physical actors
+ PhysicsActor p1;
+ PhysicsActor p2;
+
+ if (!actor_name_map.TryGetValue(g1, out p1))
+ {
+ m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 1");
+ return;
+ }
+
+ if (!actor_name_map.TryGetValue(g2, out p2))
+ {
+ m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 2");
+ return;
+ }
+
+
+ // get first contact
+ d.ContactGeom curContact = new d.ContactGeom();
+
+ if (!GetCurContactGeom(0, ref curContact))
+ return;
+
+ ContactPoint maxDepthContact = new ContactPoint();
+
+ // do volume detection case
+ if ((p1.IsVolumeDtc || p2.IsVolumeDtc))
+ {
+ 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, false
+ );
+
+ collision_accounting_events(p1, p2, maxDepthContact);
+ return;
+ }
+
+ // big messy collision analises
+
+ float mu = 0;
+ float bounce = 0;
+// bool IgnoreNegSides = false;
+
+ ContactData contactdata1 = new ContactData(0, 0, false);
+ ContactData contactdata2 = new ContactData(0, 0, false);
+
+ bool dop1ava = false;
+ bool dop2ava = false;
+ bool ignore = false;
+ bool smoothMesh = false;
+
+ switch (p1.PhysicsActorType)
+ {
+ case (int)ActorTypes.Agent:
+ {
+ dop1ava = true;
+ switch (p2.PhysicsActorType)
+ {
+ case (int)ActorTypes.Agent:
+ case (int)ActorTypes.Prim:
+ break;
+
+ default:
+ ignore = true; // avatar to terrain and water ignored
+ break;
+ }
+ break;
+ }
+
+ case (int)ActorTypes.Prim:
+ {
+ switch (p2.PhysicsActorType)
+ {
+ case (int)ActorTypes.Agent:
+ dop2ava = true;
+ break;
+
+ case (int)ActorTypes.Prim:
+ Vector3 relV = p1.rootVelocity - p2.rootVelocity;
+ float relVlenSQ = relV.LengthSquared();
+ if (relVlenSQ > 0.0001f)
+ {
+ p1.CollidingObj = true;
+ p2.CollidingObj = true;
+ }
+ p1.getContactData(ref contactdata1);
+ p2.getContactData(ref contactdata2);
+ bounce = contactdata1.bounce * contactdata2.bounce;
+ mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu);
+
+ if (relVlenSQ > 0.01f)
+ mu *= frictionMovementMult;
+
+ if(d.GeomGetClass(g2) == d.GeomClassID.TriMeshClass &&
+ d.GeomGetClass(g1) == d.GeomClassID.TriMeshClass)
+ smoothMesh = true;
+ break;
+
+ case (int)ActorTypes.Ground:
+ p1.getContactData(ref contactdata1);
+ bounce = contactdata1.bounce * TerrainBounce;
+ mu = (float)Math.Sqrt(contactdata1.mu * TerrainFriction);
+
+ Vector3 v1 = p1.rootVelocity;
+ if (Math.Abs(v1.X) > 0.1f || Math.Abs(v1.Y) > 0.1f)
+ mu *= frictionMovementMult;
+ p1.CollidingGround = true;
+
+ if(d.GeomGetClass(g1) == d.GeomClassID.TriMeshClass)
+ smoothMesh = true;
+ break;
+
+ case (int)ActorTypes.Water:
+ default:
+ ignore = true;
+ break;
+ }
+ }
+ break;
+
+ case (int)ActorTypes.Ground:
+ if (p2.PhysicsActorType == (int)ActorTypes.Prim)
+ {
+ p2.CollidingGround = true;
+ p2.getContactData(ref contactdata2);
+ bounce = contactdata2.bounce * TerrainBounce;
+ mu = (float)Math.Sqrt(contactdata2.mu * TerrainFriction);
+
+// if (curContact.side1 > 0) // should be 2 ?
+// IgnoreNegSides = true;
+ Vector3 v2 = p2.rootVelocity;
+ if (Math.Abs(v2.X) > 0.1f || Math.Abs(v2.Y) > 0.1f)
+ mu *= frictionMovementMult;
+
+ if(d.GeomGetClass(g2) == d.GeomClassID.TriMeshClass)
+ smoothMesh = true;
+ }
+ else
+ ignore = true;
+ break;
+
+ case (int)ActorTypes.Water:
+ default:
+ break;
+ }
+
+ if (ignore)
+ return;
+
+ IntPtr Joint;
+ bool FeetCollision = false;
+ int ncontacts = 0;
+
+ int i = 0;
+
+ maxDepthContact = new ContactPoint();
+ maxDepthContact.PenetrationDepth = float.MinValue;
+ ContactPoint minDepthContact = new ContactPoint();
+ minDepthContact.PenetrationDepth = float.MaxValue;
+
+ SharedTmpcontact.geom.depth = 0;
+ SharedTmpcontact.surface.mu = mu;
+ SharedTmpcontact.surface.bounce = bounce;
+
+ d.ContactGeom altContact = new d.ContactGeom();
+ bool useAltcontact;
+ bool noskip;
+
+ if(dop1ava || dop2ava)
+ smoothMesh = false;
+
+ while (true)
+ {
+ noskip = true;
+ useAltcontact = false;
+
+ if (dop1ava)
+ {
+ if ((((OdeCharacter)p1).Collide(g1, g2, false, ref curContact, ref altContact , ref useAltcontact, ref FeetCollision)))
+ {
+ if (p2.PhysicsActorType == (int)ActorTypes.Agent)
+ {
+ p1.CollidingObj = true;
+ p2.CollidingObj = true;
+ }
+ else if (p2.rootVelocity.LengthSquared() > 0.0f)
+ p2.CollidingObj = true;
+ }
+ else
+ noskip = false;
+ }
+ else if (dop2ava)
+ {
+ if ((((OdeCharacter)p2).Collide(g2, g1, true, ref curContact, ref altContact , ref useAltcontact, ref FeetCollision)))
+ {
+ if (p1.PhysicsActorType == (int)ActorTypes.Agent)
+ {
+ p1.CollidingObj = true;
+ p2.CollidingObj = true;
+ }
+ else if (p1.rootVelocity.LengthSquared() > 0.0f)
+ p1.CollidingObj = true;
+ }
+ else
+ noskip = false;
+ }
+
+ if (noskip)
+ {
+ if(useAltcontact)
+ Joint = CreateContacJoint(ref altContact,smoothMesh);
+ else
+ Joint = CreateContacJoint(ref curContact,smoothMesh);
+ if (Joint == IntPtr.Zero)
+ break;
+
+ d.JointAttach(Joint, b1, b2);
+
+ ncontacts++;
+
+ if (curContact.depth > maxDepthContact.PenetrationDepth)
+ {
+ maxDepthContact.Position.X = curContact.pos.X;
+ maxDepthContact.Position.Y = curContact.pos.Y;
+ maxDepthContact.Position.Z = curContact.pos.Z;
+ maxDepthContact.PenetrationDepth = curContact.depth;
+ maxDepthContact.CharacterFeet = FeetCollision;
+ }
+
+ if (curContact.depth < minDepthContact.PenetrationDepth)
+ {
+ minDepthContact.PenetrationDepth = curContact.depth;
+ minDepthContact.SurfaceNormal.X = curContact.normal.X;
+ minDepthContact.SurfaceNormal.Y = curContact.normal.Y;
+ minDepthContact.SurfaceNormal.Z = curContact.normal.Z;
+ }
+ }
+
+ if (++i >= count)
+ break;
+
+ if (!GetCurContactGeom(i, ref curContact))
+ break;
+ }
+
+ if (ncontacts > 0)
+ {
+ maxDepthContact.SurfaceNormal.X = minDepthContact.SurfaceNormal.X;
+ maxDepthContact.SurfaceNormal.Y = minDepthContact.SurfaceNormal.Y;
+ maxDepthContact.SurfaceNormal.Z = minDepthContact.SurfaceNormal.Z;
+
+ collision_accounting_events(p1, p2, maxDepthContact);
+ }
+ }
+
+ private void collision_accounting_events(PhysicsActor p1, PhysicsActor p2, ContactPoint contact)
+ {
+ uint obj2LocalID = 0;
+
+ // update actors collision score
+ if (p1.CollisionScore < float.MaxValue)
+ p1.CollisionScore += 1.0f;
+ if (p2.CollisionScore < float.MaxValue)
+ p2.CollisionScore += 1.0f;
+
+ bool p1events = p1.SubscribedEvents();
+ bool p2events = p2.SubscribedEvents();
+
+ if (p1.IsVolumeDtc)
+ p2events = false;
+ if (p2.IsVolumeDtc)
+ p1events = false;
+
+ if (!p2events && !p1events)
+ return;
+
+ Vector3 vel = Vector3.Zero;
+ if (p2 != null && p2.IsPhysical)
+ vel = p2.rootVelocity;
+
+ if (p1 != null && p1.IsPhysical)
+ vel -= p1.rootVelocity;
+
+ contact.RelativeSpeed = Vector3.Dot(vel, contact.SurfaceNormal);
+
+ switch ((ActorTypes)p1.PhysicsActorType)
+ {
+ case ActorTypes.Agent:
+ case ActorTypes.Prim:
+ {
+ switch ((ActorTypes)p2.PhysicsActorType)
+ {
+ case ActorTypes.Agent:
+ case ActorTypes.Prim:
+ if (p2events)
+ {
+ //AddCollisionEventReporting(p2);
+ p2.AddCollisionEvent(p1.ParentActor.LocalID, contact);
+ }
+ else if(p1.IsVolumeDtc)
+ p2.AddVDTCCollisionEvent(p1.ParentActor.LocalID, contact);
+
+ obj2LocalID = p2.ParentActor.LocalID;
+ break;
+
+ case ActorTypes.Ground:
+ case ActorTypes.Unknown:
+ default:
+ obj2LocalID = 0;
+ break;
+ }
+ if (p1events)
+ {
+ contact.SurfaceNormal = -contact.SurfaceNormal;
+ contact.RelativeSpeed = -contact.RelativeSpeed;
+ //AddCollisionEventReporting(p1);
+ p1.AddCollisionEvent(obj2LocalID, contact);
+ }
+ else if(p2.IsVolumeDtc)
+ {
+ contact.SurfaceNormal = -contact.SurfaceNormal;
+ contact.RelativeSpeed = -contact.RelativeSpeed;
+ //AddCollisionEventReporting(p1);
+ p1.AddVDTCCollisionEvent(obj2LocalID, contact);
+ }
+ break;
+ }
+ case ActorTypes.Ground:
+ case ActorTypes.Unknown:
+ default:
+ {
+ if (p2events && !p2.IsVolumeDtc)
+ {
+ //AddCollisionEventReporting(p2);
+ p2.AddCollisionEvent(0, contact);
+ }
+ break;
+ }
+ }
+ }
+
+ ///
+ /// This is our collision testing routine in ODE
+ ///
+ ///
+ private void collision_optimized()
+ {
+ lock (_characters)
+ {
+ try
+ {
+ foreach (OdeCharacter chr in _characters)
+ {
+ if (chr == null)
+ continue;
+
+ chr.IsColliding = false;
+ // chr.CollidingGround = false; not done here
+ chr.CollidingObj = false;
+
+ if(chr.Body == IntPtr.Zero || chr.collider == IntPtr.Zero )
+ continue;
+
+ // do colisions with static space
+ d.SpaceCollide2(chr.collider, StaticSpace, IntPtr.Zero, nearCallback);
+
+ // no coll with gnd
+ }
+ // chars with chars
+ d.SpaceCollide(CharsSpace, IntPtr.Zero, nearCallback);
+
+ }
+ catch (AccessViolationException)
+ {
+ m_log.Warn("[PHYSICS]: Unable to collide Character to static space");
+ }
+
+ }
+
+ lock (_activeprims)
+ {
+ foreach (OdePrim aprim in _activeprims)
+ {
+ aprim.CollisionScore = 0;
+ aprim.IsColliding = false;
+ if(!aprim.m_outbounds && d.BodyIsEnabled(aprim.Body))
+ aprim.clearSleeperCollisions();
+ }
+ }
+
+ lock (_activegroups)
+ {
+ try
+ {
+ foreach (OdePrim aprim in _activegroups)
+ {
+ if(!aprim.m_outbounds && d.BodyIsEnabled(aprim.Body) &&
+ aprim.collide_geom != IntPtr.Zero)
+ {
+ d.SpaceCollide2(StaticSpace, aprim.collide_geom, IntPtr.Zero, nearCallback);
+ d.SpaceCollide2(GroundSpace, aprim.collide_geom, IntPtr.Zero, nearCallback);
+ }
+ }
+ }
+ catch (Exception e)
+ {
+ m_log.Warn("[PHYSICS]: Unable to collide Active to Static: " + e.Message);
+ }
+ }
+
+ // colide active amoung them
+ try
+ {
+ d.SpaceCollide(ActiveSpace, IntPtr.Zero, nearCallback);
+ }
+ catch (Exception e)
+ {
+ m_log.Warn("[PHYSICS]: Unable to collide in Active: " + e.Message);
+ }
+
+ // and with chars
+ try
+ {
+ d.SpaceCollide2(CharsSpace,ActiveSpace, IntPtr.Zero, nearCallback);
+ }
+ catch (Exception e)
+ {
+ m_log.Warn("[PHYSICS]: Unable to collide Active to Character: " + e.Message);
+ }
+ }
+
+ #endregion
+ ///
+ /// Add actor to the list that should receive collision events in the simulate loop.
+ ///
+ ///
+ public void AddCollisionEventReporting(PhysicsActor obj)
+ {
+ if (!_collisionEventPrim.Contains(obj))
+ _collisionEventPrim.Add(obj);
+ }
+
+ ///
+ /// Remove actor from the list that should receive collision events in the simulate loop.
+ ///
+ ///
+ public void RemoveCollisionEventReporting(PhysicsActor obj)
+ {
+ lock(_collisionEventPrimRemove)
+ {
+ if (_collisionEventPrim.Contains(obj) && !_collisionEventPrimRemove.Contains(obj))
+ _collisionEventPrimRemove.Add(obj);
+ }
+ }
+
+ public override float TimeDilation
+ {
+ get { return m_timeDilation; }
+ }
+
+ public override bool SupportsNINJAJoints
+ {
+ get { return false; }
+ }
+
+ #region Add/Remove Entities
+
+ public override PhysicsActor AddAvatar(string avName, Vector3 position, Vector3 velocity, Vector3 size, bool isFlying)
+ {
+ return null;
+ }
+
+ public override PhysicsActor AddAvatar(uint localID, string avName, Vector3 position, Vector3 size, float feetOffset, bool isFlying)
+ {
+ OdeCharacter newAv = new OdeCharacter(localID, avName, this, position,
+ size, feetOffset, 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");
+ lock (OdeLock)
+ {
+ d.AllocateODEDataForThread(0);
+ ((OdeCharacter) actor).Destroy();
+ }
+ }
+
+
+ public void addActivePrim(OdePrim activatePrim)
+ {
+ // adds active prim..
+ lock (_activeprims)
+ {
+ if (!_activeprims.Contains(activatePrim))
+ _activeprims.Add(activatePrim);
+ }
+ }
+
+ public void addActiveGroups(OdePrim activatePrim)
+ {
+ lock (_activegroups)
+ {
+ if (!_activegroups.Contains(activatePrim))
+ _activegroups.Add(activatePrim);
+ }
+ }
+
+ private PhysicsActor AddPrim(String name, Vector3 position, Vector3 size, Quaternion rotation,
+ PrimitiveBaseShape pbs, bool isphysical, bool isPhantom, byte shapeType, uint localID)
+ {
+ OdePrim newPrim;
+ lock (OdeLock)
+ {
+
+ newPrim = new OdePrim(name, this, position, size, rotation, pbs, isphysical, isPhantom, shapeType, localID);
+ }
+ return newPrim;
+ }
+
+ public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
+ Vector3 size, Quaternion rotation, bool isPhysical, bool isPhantom, uint localid)
+ {
+ return AddPrim(primName, position, size, rotation, pbs, isPhysical, isPhantom, 0 , localid);
+ }
+
+
+ public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
+ Vector3 size, Quaternion rotation, bool isPhysical, uint localid)
+ {
+ return AddPrim(primName, position, size, rotation, pbs, isPhysical,false, 0, localid);
+ }
+
+ public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
+ Vector3 size, Quaternion rotation, bool isPhysical, bool isPhantom, byte shapeType, uint localid)
+ {
+ return AddPrim(primName, position, size, rotation, pbs, isPhysical,isPhantom, shapeType, localid);
+ }
+
+ public void remActivePrim(OdePrim deactivatePrim)
+ {
+ lock (_activeprims)
+ {
+ _activeprims.Remove(deactivatePrim);
+ }
+ }
+ public void remActiveGroup(OdePrim deactivatePrim)
+ {
+ lock (_activegroups)
+ {
+ _activegroups.Remove(deactivatePrim);
+ }
+ }
+
+ public override void RemovePrim(PhysicsActor prim)
+ {
+ // As with all ODE physics operations, we don't remove the prim immediately but signal that it should be
+ // removed in the next physics simulate pass.
+ if (prim is OdePrim)
+ {
+// lock (OdeLock)
+ {
+
+ OdePrim p = (OdePrim)prim;
+ p.setPrimForRemoval();
+ }
+ }
+ }
+
+ public void RemovePrimThreadLocked(OdePrim prim)
+ {
+ //Console.WriteLine("RemovePrimThreadLocked " + prim.m_primName);
+ lock (prim)
+ {
+// RemoveCollisionEventReporting(prim);
+ lock (_prims)
+ _prims.Remove(prim.LocalID);
+ }
+
+ }
+
+ public void addToPrims(OdePrim prim)
+ {
+ lock (_prims)
+ _prims[prim.LocalID] = prim;
+ }
+
+ public OdePrim getPrim(uint id)
+ {
+ lock (_prims)
+ {
+ if(_prims.ContainsKey(id))
+ return _prims[id];
+ else
+ return null;
+ }
+ }
+
+ public bool havePrim(OdePrim prm)
+ {
+ lock (_prims)
+ return _prims.ContainsKey(prm.LocalID);
+ }
+
+ public void changePrimID(OdePrim prim,uint oldID)
+ {
+ lock (_prims)
+ {
+ if(_prims.ContainsKey(oldID))
+ _prims.Remove(oldID);
+ _prims[prim.LocalID] = prim;
+ }
+ }
+
+ public bool haveActor(PhysicsActor actor)
+ {
+ if (actor is OdePrim)
+ {
+ lock (_prims)
+ return _prims.ContainsKey(((OdePrim)actor).LocalID);
+ }
+ else if (actor is OdeCharacter)
+ {
+ lock (_characters)
+ return _characters.Contains((OdeCharacter)actor);
+ }
+ return false;
+ }
+
+ #endregion
+
+ #region Space Separation Calculation
+
+ ///
+ /// Called when a static prim moves or becomes static
+ /// Places the prim in a space one the static sub-spaces grid
+ ///
+ /// the pointer to the geom that moved
+ /// the position that the geom moved to
+ /// a pointer to the space it was in before it was moved.
+ /// a pointer to the new space it's in
+ public IntPtr MoveGeomToStaticSpace(IntPtr geom, Vector3 pos, IntPtr currentspace)
+ {
+ // moves a prim into another static sub-space or from another space into a static sub-space
+
+ // Called ODEPrim so
+ // it's already in locked space.
+
+ if (geom == IntPtr.Zero) // shouldn't happen
+ return IntPtr.Zero;
+
+ // get the static sub-space for current position
+ IntPtr newspace = calculateSpaceForGeom(pos);
+
+ if (newspace == currentspace) // if we are there all done
+ return newspace;
+
+ // else remove it from its current space
+ if (currentspace != IntPtr.Zero && d.SpaceQuery(currentspace, geom))
+ {
+ if (d.GeomIsSpace(currentspace))
+ {
+ waitForSpaceUnlock(currentspace);
+ d.SpaceRemove(currentspace, geom);
+
+ if (d.SpaceGetSublevel(currentspace) > 2 && d.SpaceGetNumGeoms(currentspace) == 0)
+ {
+ d.SpaceDestroy(currentspace);
+ }
+ }
+ else
+ {
+ m_log.Info("[Physics]: Invalid or empty Space passed to 'MoveGeomToStaticSpace':" + currentspace +
+ " Geom:" + geom);
+ }
+ }
+ else // odd currentspace is null or doesn't contain the geom? lets try the geom ideia of current space
+ {
+ currentspace = d.GeomGetSpace(geom);
+ if (currentspace != IntPtr.Zero)
+ {
+ if (d.GeomIsSpace(currentspace))
+ {
+ waitForSpaceUnlock(currentspace);
+ d.SpaceRemove(currentspace, geom);
+
+ if (d.SpaceGetSublevel(currentspace) > 2 && d.SpaceGetNumGeoms(currentspace) == 0)
+ {
+ d.SpaceDestroy(currentspace);
+ }
+
+ }
+ }
+ }
+
+ // put the geom in the newspace
+ waitForSpaceUnlock(newspace);
+ d.SpaceAdd(newspace, geom);
+
+ // let caller know this newspace
+ return newspace;
+ }
+
+ ///
+ /// Calculates the space the prim should be in by its position
+ ///
+ ///
+ /// a pointer to the space. This could be a new space or reused space.
+ public IntPtr calculateSpaceForGeom(Vector3 pos)
+ {
+ int x, y;
+
+ if (pos.X < 0)
+ return staticPrimspaceOffRegion[0];
+
+ if (pos.Y < 0)
+ return staticPrimspaceOffRegion[2];
+
+ x = (int)(pos.X * spacesPerMeterX);
+ if (x > spaceGridMaxX)
+ return staticPrimspaceOffRegion[1];
+
+ y = (int)(pos.Y * spacesPerMeterY);
+ if (y > spaceGridMaxY)
+ return staticPrimspaceOffRegion[3];
+
+ return staticPrimspace[x, y];
+ }
+
+ #endregion
+
+
+ ///
+ /// Called to queue a change to a actor
+ /// to use in place of old taint mechanism so changes do have a time sequence
+ ///
+
+ public void AddChange(PhysicsActor actor, changes what, Object arg)
+ {
+ ODEchangeitem item = new ODEchangeitem();
+ item.actor = actor;
+ item.what = what;
+ item.arg = arg;
+ ChangesQueue.Enqueue(item);
+ }
+
+ ///
+ /// 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
+ ///
+ ///
+ public override void AddPhysicsActorTaint(PhysicsActor prim)
+ {
+ }
+
+ // does all pending changes generated during region load process
+ public override void ProcessPreSimulation()
+ {
+ lock (OdeLock)
+ {
+ if (world == IntPtr.Zero)
+ {
+ ChangesQueue.Clear();
+ return;
+ }
+
+ d.AllocateODEDataForThread(~0U);
+
+ ODEchangeitem item;
+
+ int donechanges = 0;
+ if (ChangesQueue.Count > 0)
+ {
+ m_log.InfoFormat("[ubOde] start processing pending actor operations");
+ int tstart = Util.EnvironmentTickCount();
+
+ while (ChangesQueue.Dequeue(out item))
+ {
+ if (item.actor != null)
+ {
+ try
+ {
+ if (item.actor is OdeCharacter)
+ ((OdeCharacter)item.actor).DoAChange(item.what, item.arg);
+ else if (((OdePrim)item.actor).DoAChange(item.what, item.arg))
+ RemovePrimThreadLocked((OdePrim)item.actor);
+ }
+ catch
+ {
+ m_log.WarnFormat("[PHYSICS]: Operation failed for a actor {0} {1}",
+ item.actor.Name, item.what.ToString());
+ }
+ }
+ donechanges++;
+ }
+ int time = Util.EnvironmentTickCountSubtract(tstart);
+ m_log.InfoFormat("[ubOde] finished {0} operations in {1}ms", donechanges, time);
+ }
+ m_log.InfoFormat("[ubOde] {0} prim actors loaded",_prims.Count);
+ }
+ m_lastframe = Util.GetTimeStamp() + 0.5;
+ step_time = -0.5f;
+ }
+
+ ///
+ /// 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)
+ ///
+ ///
+ ///
+ public override float Simulate(float reqTimeStep)
+ {
+ double now = Util.GetTimeStamp();
+ double timeStep = now - m_lastframe;
+ m_lastframe = now;
+
+ // acumulate time so we can reduce error
+ step_time += (float)timeStep;
+
+ if (step_time < HalfOdeStep)
+ return 0;
+
+ if (framecount < 0)
+ framecount = 0;
+
+ framecount++;
+
+// checkThread();
+ int nodeframes = 0;
+ float fps = 0;
+
+ lock (SimulationLock)
+ lock(OdeLock)
+ {
+ if (world == IntPtr.Zero)
+ {
+ ChangesQueue.Clear();
+ return 0;
+ }
+
+ ODEchangeitem item;
+
+// d.WorldSetQuickStepNumIterations(world, curphysiteractions);
+
+ double loopstartMS = Util.GetTimeStampMS();
+ double looptimeMS = 0;
+ double changestimeMS = 0;
+ double maxChangestime = (int)(reqTimeStep * 500f); // half the time
+ double maxLoopTime = (int)(reqTimeStep * 1200f); // 1.2 the time
+
+// double collisionTime = 0;
+// double qstepTIme = 0;
+// double tmpTime = 0;
+
+ d.AllocateODEDataForThread(~0U);
+
+ if (ChangesQueue.Count > 0)
+ {
+ while (ChangesQueue.Dequeue(out item))
+ {
+ if (item.actor != null)
+ {
+ try
+ {
+ if (item.actor is OdeCharacter)
+ ((OdeCharacter)item.actor).DoAChange(item.what, item.arg);
+ else if (((OdePrim)item.actor).DoAChange(item.what, item.arg))
+ RemovePrimThreadLocked((OdePrim)item.actor);
+ }
+ catch
+ {
+ m_log.WarnFormat("[PHYSICS]: doChange failed for a actor {0} {1}",
+ item.actor.Name, item.what.ToString());
+ }
+ }
+ changestimeMS = Util.GetTimeStampMS() - loopstartMS;
+ if (changestimeMS > maxChangestime)
+ break;
+ }
+ }
+
+ // do simulation taking at most 150ms total time including changes
+ while (step_time > HalfOdeStep)
+ {
+ try
+ {
+ // clear pointer/counter to contacts to pass into joints
+ m_global_contactcount = 0;
+
+
+ // Move characters
+ lock (_characters)
+ {
+ List defects = new List();
+ foreach (OdeCharacter actor in _characters)
+ {
+ if (actor != null)
+ actor.Move(defects);
+ }
+ if (defects.Count != 0)
+ {
+ foreach (OdeCharacter defect in defects)
+ {
+ RemoveCharacter(defect);
+ }
+ defects.Clear();
+ }
+ }
+
+ // Move other active objects
+ lock (_activegroups)
+ {
+ foreach (OdePrim aprim in _activegroups)
+ {
+ aprim.Move();
+ }
+ }
+
+ m_rayCastManager.ProcessQueuedRequests();
+
+// tmpTime = Util.GetTimeStampMS();
+ collision_optimized();
+// collisionTime += Util.GetTimeStampMS() - tmpTime;
+
+ lock(_collisionEventPrimRemove)
+ {
+ foreach (PhysicsActor obj in _collisionEventPrimRemove)
+ _collisionEventPrim.Remove(obj);
+
+ _collisionEventPrimRemove.Clear();
+ }
+
+ List sleepers = new List();
+ foreach (PhysicsActor obj in _collisionEventPrim)
+ {
+ if (obj == null)
+ continue;
+
+ switch ((ActorTypes)obj.PhysicsActorType)
+ {
+ case ActorTypes.Agent:
+ OdeCharacter cobj = (OdeCharacter)obj;
+ cobj.SendCollisions((int)(odetimestepMS));
+ break;
+
+ case ActorTypes.Prim:
+ OdePrim pobj = (OdePrim)obj;
+ if (!pobj.m_outbounds)
+ {
+ pobj.SendCollisions((int)(odetimestepMS));
+ if(pobj.Body != IntPtr.Zero && !pobj.m_isSelected &&
+ !pobj.m_disabled && !pobj.m_building &&
+ !d.BodyIsEnabled(pobj.Body))
+ sleepers.Add(pobj);
+ }
+ break;
+ }
+ }
+
+ foreach(OdePrim prm in sleepers)
+ prm.SleeperAddCollisionEvents();
+ sleepers.Clear();
+
+ // do a ode simulation step
+// tmpTime = Util.GetTimeStampMS();
+ d.WorldQuickStep(world, ODE_STEPSIZE);
+ d.JointGroupEmpty(contactgroup);
+// qstepTIme += Util.GetTimeStampMS() - tmpTime;
+
+ // update managed ideia of physical data and do updates to core
+ /*
+ lock (_characters)
+ {
+ foreach (OdeCharacter actor in _characters)
+ {
+ if (actor != null)
+ {
+ if (actor.bad)
+ m_log.WarnFormat("[PHYSICS]: BAD Actor {0} in _characters list was not removed?", actor.m_uuid);
+
+ actor.UpdatePositionAndVelocity();
+ }
+ }
+ }
+ */
+
+ lock (_activegroups)
+ {
+ {
+ foreach (OdePrim actor in _activegroups)
+ {
+ if (actor.IsPhysical)
+ {
+ actor.UpdatePositionAndVelocity(framecount);
+ }
+ }
+ }
+ }
+ }
+ catch (Exception e)
+ {
+ m_log.ErrorFormat("[PHYSICS]: {0}, {1}, {2}", e.Message, e.TargetSite, e);
+// ode.dunlock(world);
+ }
+
+ step_time -= ODE_STEPSIZE;
+ nodeframes++;
+
+ looptimeMS = Util.GetTimeStampMS() - loopstartMS;
+ if (looptimeMS > maxLoopTime)
+ break;
+ }
+
+ lock (_badCharacter)
+ {
+ if (_badCharacter.Count > 0)
+ {
+ foreach (OdeCharacter chr in _badCharacter)
+ {
+ RemoveCharacter(chr);
+ }
+
+ _badCharacter.Clear();
+ }
+ }
+
+// information block for in debug breakpoint only
+/*
+ int ntopactivegeoms = d.SpaceGetNumGeoms(ActiveSpace);
+ int ntopstaticgeoms = d.SpaceGetNumGeoms(StaticSpace);
+ int ngroundgeoms = d.SpaceGetNumGeoms(GroundSpace);
+
+ int nactivegeoms = 0;
+ int nactivespaces = 0;
+
+ int nstaticgeoms = 0;
+ int nstaticspaces = 0;
+ IntPtr sp;
+
+ for (int i = 0; i < ntopactivegeoms; i++)
+ {
+ sp = d.SpaceGetGeom(ActiveSpace, i);
+ if (d.GeomIsSpace(sp))
+ {
+ nactivespaces++;
+ nactivegeoms += d.SpaceGetNumGeoms(sp);
+ }
+ else
+ nactivegeoms++;
+ }
+
+ for (int i = 0; i < ntopstaticgeoms; i++)
+ {
+ sp = d.SpaceGetGeom(StaticSpace, i);
+ if (d.GeomIsSpace(sp))
+ {
+ nstaticspaces++;
+ nstaticgeoms += d.SpaceGetNumGeoms(sp);
+ }
+ else
+ nstaticgeoms++;
+ }
+
+ int ntopgeoms = d.SpaceGetNumGeoms(TopSpace);
+
+ int totgeoms = nstaticgeoms + nactivegeoms + ngroundgeoms + 1; // one ray
+ int nbodies = d.NTotalBodies;
+ int ngeoms = d.NTotalGeoms;
+*/
+/*
+ looptimeMS /= nodeframes;
+ if(looptimeMS > 0.080)
+ {
+ collisionTime /= nodeframes;
+ qstepTIme /= nodeframes;
+ }
+*/
+ // 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);
+ }
+
+ fps = (float)nodeframes * ODE_STEPSIZE / reqTimeStep;
+
+ if(step_time < HalfOdeStep)
+ m_timeDilation = 1.0f;
+ else if (step_time > m_SkipFramesAtms)
+ {
+ // if we lag too much skip frames
+ m_timeDilation = 0.0f;
+ step_time = 0;
+ m_lastframe = Util.GetTimeStamp(); // skip also the time lost
+ }
+ else
+ {
+ m_timeDilation = ODE_STEPSIZE / step_time;
+ if (m_timeDilation > 1)
+ m_timeDilation = 1;
+ }
+
+ if (m_timeDilation == 1 && now - m_lastMeshExpire > 30)
+ {
+ mesher.ExpireReleaseMeshs();
+ m_lastMeshExpire = now;
+ }
+
+
+ }
+
+ return fps;
+ }
+
+ ///
+ public override void GetResults()
+ {
+ }
+
+ public override bool IsThreaded
+ {
+ // for now we won't be multithreaded
+ get { return (false); }
+ }
+
+ public float GetTerrainHeightAtXY(float x, float y)
+ {
+ if (TerrainGeom == IntPtr.Zero)
+ return 0f;
+
+ if (TerrainHeightFieldHeight == null || TerrainHeightFieldHeight.Length == 0)
+ return 0f;
+
+ // TerrainHeightField for ODE as offset 1m
+ x += 1f;
+ y += 1f;
+
+ // 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 regsizeX = (int)m_regionWidth + 3; // map size see setterrain number of samples
+ int regsizeY = (int)m_regionHeight + 3; // map size see setterrain number of samples
+ int regsize = regsizeX;
+
+ if (m_OSOdeLib)
+ {
+ if (x < regsizeX - 1)
+ {
+ ix = (int)x;
+ dx = x - (float)ix;
+ }
+ else // out world use external height
+ {
+ ix = regsizeX - 2;
+ dx = 0;
+ }
+ if (y < regsizeY - 1)
+ {
+ iy = (int)y;
+ dy = y - (float)iy;
+ }
+ else
+ {
+ iy = regsizeY - 2;
+ dy = 0;
+ }
+ }
+ else
+ {
+ // we still have square fixed size regions
+ // also flip x and y because of how map is done for ODE fliped axis
+ // so ix,iy,dx and dy are inter exchanged
+
+ regsize = regsizeY;
+
+ if (x < regsizeX - 1)
+ {
+ iy = (int)x;
+ dy = x - (float)iy;
+ }
+ else // out world use external height
+ {
+ iy = regsizeX - 2;
+ dy = 0;
+ }
+ if (y < regsizeY - 1)
+ {
+ ix = (int)y;
+ dx = y - (float)ix;
+ }
+ else
+ {
+ ix = regsizeY - 2;
+ dx = 0;
+ }
+ }
+
+ float h0;
+ float h1;
+ float h2;
+
+ iy *= regsize;
+ iy += ix; // all indexes have iy + ix
+
+ float[] heights = TerrainHeightFieldHeight;
+ /*
+ if ((dx + dy) <= 1.0f)
+ {
+ h0 = ((float)heights[iy]); // 0,0 vertice
+ h1 = (((float)heights[iy + 1]) - h0) * dx; // 1,0 vertice minus 0,0
+ h2 = (((float)heights[iy + regsize]) - h0) * dy; // 0,1 vertice minus 0,0
+ }
+ else
+ {
+ h0 = ((float)heights[iy + regsize + 1]); // 1,1 vertice
+ h1 = (((float)heights[iy + 1]) - h0) * (1 - dy); // 1,1 vertice minus 1,0
+ h2 = (((float)heights[iy + regsize]) - h0) * (1 - dx); // 1,1 vertice minus 0,1
+ }
+ */
+ h0 = ((float)heights[iy]); // 0,0 vertice
+
+ if (dy>dx)
+ {
+ iy += regsize;
+ h2 = (float)heights[iy]; // 0,1 vertice
+ h1 = (h2 - h0) * dy; // 0,1 vertice minus 0,0
+ h2 = ((float)heights[iy + 1] - h2) * dx; // 1,1 vertice minus 0,1
+ }
+ else
+ {
+ iy++;
+ h2 = (float)heights[iy]; // vertice 1,0
+ h1 = (h2 - h0) * dx; // 1,0 vertice minus 0,0
+ h2 = (((float)heights[iy + regsize]) - h2) * dy; // 1,1 vertice minus 1,0
+ }
+
+ return h0 + h1 + h2;
+ }
+
+ public Vector3 GetTerrainNormalAtXY(float x, float y)
+ {
+ Vector3 norm = new Vector3(0, 0, 1);
+
+ if (TerrainGeom == IntPtr.Zero)
+ return norm;
+
+ if (TerrainHeightFieldHeight == null || TerrainHeightFieldHeight.Length == 0)
+ return norm;
+
+ // TerrainHeightField for ODE as offset 1m
+ x += 1f;
+ y += 1f;
+
+ // 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 regsizeX = (int)m_regionWidth + 3; // map size see setterrain number of samples
+ int regsizeY = (int)m_regionHeight + 3; // map size see setterrain number of samples
+ int regsize = regsizeX;
+
+ int xstep = 1;
+ int ystep = regsizeX;
+ bool firstTri = false;
+
+ if (m_OSOdeLib)
+ {
+ if (x < regsizeX - 1)
+ {
+ ix = (int)x;
+ dx = x - (float)ix;
+ }
+ else // out world use external height
+ {
+ ix = regsizeX - 2;
+ dx = 0;
+ }
+ if (y < regsizeY - 1)
+ {
+ iy = (int)y;
+ dy = y - (float)iy;
+ }
+ else
+ {
+ iy = regsizeY - 2;
+ dy = 0;
+ }
+ firstTri = dy > dx;
+ }
+
+ else
+ {
+ xstep = regsizeY;
+ ystep = 1;
+ regsize = regsizeY;
+
+ // 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 < regsizeX - 1)
+ {
+ iy = (int)x;
+ dy = x - (float)iy;
+ }
+ else // out world use external height
+ {
+ iy = regsizeX - 2;
+ dy = 0;
+ }
+ if (y < regsizeY - 1)
+ {
+ ix = (int)y;
+ dx = y - (float)ix;
+ }
+ else
+ {
+ ix = regsizeY - 2;
+ dx = 0;
+ }
+ firstTri = dx > dy;
+ }
+
+ float h0;
+ float h1;
+ float h2;
+
+ iy *= regsize;
+ iy += ix; // all indexes have iy + ix
+
+ float[] heights = TerrainHeightFieldHeight;
+
+ if (firstTri)
+ {
+ h1 = ((float)heights[iy]); // 0,0 vertice
+ iy += ystep;
+ h0 = (float)heights[iy]; // 0,1
+ h2 = (float)heights[iy+xstep]; // 1,1 vertice
+ norm.X = h0 - h2;
+ norm.Y = h1 - h0;
+ }
+ else
+ {
+ h2 = ((float)heights[iy]); // 0,0 vertice
+ iy += xstep;
+ h0 = ((float)heights[iy]); // 1,0 vertice
+ h1 = (float)heights[iy+ystep]; // vertice 1,1
+ norm.X = h2 - h0;
+ norm.Y = h0 - h1;
+ }
+ norm.Z = 1;
+ norm.Normalize();
+ return norm;
+ }
+
+ public override void SetTerrain(float[] heightMap)
+ {
+ if (m_OSOdeLib)
+ OSSetTerrain(heightMap);
+ else
+ OriSetTerrain(heightMap);
+ }
+
+ public void OriSetTerrain(float[] heightMap)
+ {
+ // assumes 1m size grid and constante size square regions
+ // needs to know about sims around in future
+
+ float[] _heightmap;
+
+ uint regionsizeX = m_regionWidth;
+ uint regionsizeY = m_regionHeight;
+
+ // map is rotated
+ uint heightmapWidth = regionsizeY + 2;
+ uint heightmapHeight = regionsizeX + 2;
+
+ uint heightmapWidthSamples = heightmapWidth + 1;
+ uint heightmapHeightSamples = heightmapHeight + 1;
+
+ _heightmap = new float[heightmapWidthSamples * heightmapHeightSamples];
+
+ const float scale = 1.0f;
+ const float offset = 0.0f;
+ const float thickness = 10f;
+ const int wrap = 0;
+
+
+ float hfmin = float.MaxValue;
+ float hfmax = float.MinValue;
+ float val;
+ uint xx;
+ uint yy;
+
+ uint maxXX = regionsizeX - 1;
+ uint maxYY = regionsizeY - 1;
+ // flipping map adding one margin all around so things don't fall in edges
+
+ uint xt = 0;
+ xx = 0;
+
+ for (uint x = 0; x < heightmapWidthSamples; x++)
+ {
+ if (x > 1 && xx < maxXX)
+ xx++;
+ yy = 0;
+ for (uint y = 0; y < heightmapHeightSamples; y++)
+ {
+ if (y > 1 && y < maxYY)
+ yy += regionsizeX;
+
+ val = heightMap[yy + xx];
+ if (val < 0.0f)
+ val = 0.0f; // no neg terrain as in chode
+ _heightmap[xt + y] = val;
+
+ if (hfmin > val)
+ hfmin = val;
+ if (hfmax < val)
+ hfmax = val;
+ }
+ xt += heightmapHeightSamples;
+ }
+
+ lock (OdeLock)
+ {
+ d.AllocateODEDataForThread(~0U);
+
+ if (TerrainGeom != IntPtr.Zero)
+ {
+ actor_name_map.Remove(TerrainGeom);
+ d.GeomDestroy(TerrainGeom);
+
+ }
+
+ if (TerrainHeightFieldHeightsHandler.IsAllocated)
+ TerrainHeightFieldHeightsHandler.Free();
+
+ IntPtr HeightmapData = d.GeomHeightfieldDataCreate();
+
+ TerrainHeightFieldHeightsHandler = GCHandle.Alloc(_heightmap, GCHandleType.Pinned);
+
+ d.GeomHeightfieldDataBuildSingle(HeightmapData, TerrainHeightFieldHeightsHandler.AddrOfPinnedObject(), 0,
+ heightmapHeight, heightmapWidth ,
+ (int)heightmapHeightSamples, (int)heightmapWidthSamples, scale,
+ offset, thickness, wrap);
+
+ d.GeomHeightfieldDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1);
+
+ TerrainGeom = d.CreateHeightfield(GroundSpace, HeightmapData, 1);
+
+ if (TerrainGeom != IntPtr.Zero)
+ {
+ d.GeomSetCategoryBits(TerrainGeom, (uint)(CollisionCategories.Land));
+ d.GeomSetCollideBits(TerrainGeom, 0);
+
+ PhysicsActor pa = new NullPhysicsActor();
+ pa.Name = "Terrain";
+ pa.PhysicsActorType = (int)ActorTypes.Ground;
+ actor_name_map[TerrainGeom] = pa;
+
+// geom_name_map[GroundGeom] = "Terrain";
+
+ d.Quaternion q = new d.Quaternion();
+ q.X = 0.5f;
+ q.Y = 0.5f;
+ q.Z = 0.5f;
+ q.W = 0.5f;
+
+ d.GeomSetQuaternion(TerrainGeom, ref q);
+ d.GeomSetPosition(TerrainGeom, m_regionWidth * 0.5f, m_regionHeight * 0.5f, 0.0f);
+ TerrainHeightFieldHeight = _heightmap;
+ }
+ else
+ TerrainHeightFieldHeightsHandler.Free();
+ }
+ }
+
+ public void OSSetTerrain(float[] heightMap)
+ {
+ // assumes 1m size grid and constante size square regions
+ // needs to know about sims around in future
+
+ float[] _heightmap;
+
+ uint regionsizeX = m_regionWidth;
+ uint regionsizeY = m_regionHeight;
+
+ uint heightmapWidth = regionsizeX + 2;
+ uint heightmapHeight = regionsizeY + 2;
+
+ uint heightmapWidthSamples = heightmapWidth + 1;
+ uint heightmapHeightSamples = heightmapHeight + 1;
+
+ _heightmap = new float[heightmapWidthSamples * heightmapHeightSamples];
+
+
+ float hfmin = float.MaxValue;
+// float hfmax = float.MinValue;
+ float val;
+
+
+ uint maxXX = regionsizeX + 1;
+ uint maxYY = regionsizeY + 1;
+ // adding one margin all around so things don't fall in edges
+
+ uint xx;
+ uint yy = 0;
+ uint yt = 0;
+
+ for (uint y = 0; y < heightmapHeightSamples; y++)
+ {
+ if (y > 1 && y < maxYY)
+ yy += regionsizeX;
+ xx = 0;
+ for (uint x = 0; x < heightmapWidthSamples; x++)
+ {
+ if (x > 1 && x < maxXX)
+ xx++;
+
+ val = heightMap[yy + xx];
+ if (val < 0.0f)
+ val = 0.0f; // no neg terrain as in chode
+ _heightmap[yt + x] = val;
+
+ if (hfmin > val)
+ hfmin = val;
+// if (hfmax < val)
+// hfmax = val;
+ }
+ yt += heightmapWidthSamples;
+ }
+
+ lock (OdeLock)
+ {
+ if (TerrainGeom != IntPtr.Zero)
+ {
+ actor_name_map.Remove(TerrainGeom);
+ d.GeomDestroy(TerrainGeom);
+ }
+
+ if (TerrainHeightFieldHeightsHandler.IsAllocated)
+ TerrainHeightFieldHeightsHandler.Free();
+
+ TerrainHeightFieldHeight = null;
+
+ IntPtr HeightmapData = d.GeomOSTerrainDataCreate();
+
+ const int wrap = 0;
+ float thickness = hfmin;
+ if (thickness < 0)
+ thickness = 1;
+
+ TerrainHeightFieldHeightsHandler = GCHandle.Alloc(_heightmap, GCHandleType.Pinned);
+
+ d.GeomOSTerrainDataBuild(HeightmapData, TerrainHeightFieldHeightsHandler.AddrOfPinnedObject(), 0, 1.0f,
+ (int)heightmapWidthSamples, (int)heightmapHeightSamples,
+ thickness, wrap);
+
+// d.GeomOSTerrainDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1);
+ TerrainGeom = d.CreateOSTerrain(GroundSpace, HeightmapData, 1);
+ if (TerrainGeom != IntPtr.Zero)
+ {
+ d.GeomSetCategoryBits(TerrainGeom, (uint)(CollisionCategories.Land));
+ d.GeomSetCollideBits(TerrainGeom, 0);
+
+ PhysicsActor pa = new NullPhysicsActor();
+ pa.Name = "Terrain";
+ pa.PhysicsActorType = (int)ActorTypes.Ground;
+ actor_name_map[TerrainGeom] = pa;
+
+// geom_name_map[GroundGeom] = "Terrain";
+
+ d.GeomSetPosition(TerrainGeom, m_regionWidth * 0.5f, m_regionHeight * 0.5f, 0.0f);
+ TerrainHeightFieldHeight = _heightmap;
+ }
+ else
+ TerrainHeightFieldHeightsHandler.Free();
+ }
+ }
+
+ public override void DeleteTerrain()
+ {
+ }
+
+ public float GetWaterLevel()
+ {
+ return waterlevel;
+ }
+
+ public override void SetWaterLevel(float baseheight)
+ {
+ waterlevel = baseheight;
+ }
+
+ public override void Dispose()
+ {
+ lock (OdeLock)
+ {
+
+ if (world == IntPtr.Zero)
+ return;
+
+ d.AllocateODEDataForThread(~0U);
+
+ if (m_meshWorker != null)
+ m_meshWorker.Stop();
+
+ if (m_rayCastManager != null)
+ {
+ m_rayCastManager.Dispose();
+ m_rayCastManager = null;
+ }
+
+ lock (_prims)
+ {
+ ChangesQueue.Clear();
+ foreach (OdePrim prm in _prims.Values)
+ {
+ prm.DoAChange(changes.Remove, null);
+ _collisionEventPrim.Remove(prm);
+ }
+ _prims.Clear();
+ }
+
+ OdeCharacter[] chtorem;
+ lock (_characters)
+ {
+ chtorem = new OdeCharacter[_characters.Count];
+ _characters.CopyTo(chtorem);
+ }
+
+ ChangesQueue.Clear();
+ foreach (OdeCharacter ch in chtorem)
+ ch.DoAChange(changes.Remove, null);
+
+ if (TerrainGeom != IntPtr.Zero)
+ d.GeomDestroy(TerrainGeom);
+ TerrainGeom = IntPtr.Zero;
+
+ if (TerrainHeightFieldHeightsHandler.IsAllocated)
+ TerrainHeightFieldHeightsHandler.Free();
+
+ TerrainHeightFieldHeight = null;
+
+ if (ContactgeomsArray != IntPtr.Zero)
+ {
+ Marshal.FreeHGlobal(ContactgeomsArray);
+ ContactgeomsArray = IntPtr.Zero;
+ }
+ if (GlobalContactsArray != IntPtr.Zero)
+ {
+ Marshal.FreeHGlobal(GlobalContactsArray);
+ GlobalContactsArray = IntPtr.Zero;
+ }
+
+ d.WorldDestroy(world);
+ world = IntPtr.Zero;
+ //d.CloseODE();
+ }
+ }
+
+ private int compareByCollisionsDesc(OdePrim A, OdePrim B)
+ {
+ return -A.CollisionScore.CompareTo(B.CollisionScore);
+ }
+
+ public override Dictionary GetTopColliders()
+ {
+ Dictionary topColliders;
+ List orderedPrims;
+ lock (_activeprims)
+ orderedPrims = new List(_activeprims);
+
+ orderedPrims.Sort(compareByCollisionsDesc);
+ topColliders = orderedPrims.Take(25).ToDictionary(p => p.LocalID, p => p.CollisionScore);
+
+ return topColliders;
+ }
+
+ public override bool SupportsRayCast()
+ {
+ return true;
+ }
+
+ public override void RaycastWorld(Vector3 position, Vector3 direction, float length, RaycastCallback retMethod)
+ {
+ if (retMethod != null)
+ {
+ ODERayRequest req = new ODERayRequest();
+ req.actor = null;
+ req.callbackMethod = retMethod;
+ req.length = length;
+ req.Normal = direction;
+ req.Origin = position;
+ req.Count = 0;
+ req.filter = RayFilterFlags.AllPrims;
+
+ m_rayCastManager.QueueRequest(req);
+ }
+ }
+
+ public override void RaycastWorld(Vector3 position, Vector3 direction, float length, int Count, RayCallback retMethod)
+ {
+ if (retMethod != null)
+ {
+ ODERayRequest req = new ODERayRequest();
+ req.actor = null;
+ req.callbackMethod = retMethod;
+ req.length = length;
+ req.Normal = direction;
+ req.Origin = position;
+ req.Count = Count;
+ req.filter = RayFilterFlags.AllPrims;
+
+ m_rayCastManager.QueueRequest(req);
+ }
+ }
+
+
+ public override List RaycastWorld(Vector3 position, Vector3 direction, float length, int Count)
+ {
+ List ourresults = new List();
+ object SyncObject = new object();
+
+ RayCallback retMethod = delegate(List results)
+ {
+ lock (SyncObject)
+ {
+ ourresults = results;
+ Monitor.PulseAll(SyncObject);
+ }
+ };
+
+ ODERayRequest req = new ODERayRequest();
+ req.actor = null;
+ req.callbackMethod = retMethod;
+ req.length = length;
+ req.Normal = direction;
+ req.Origin = position;
+ req.Count = Count;
+ req.filter = RayFilterFlags.AllPrims;
+
+ lock (SyncObject)
+ {
+ m_rayCastManager.QueueRequest(req);
+ if (!Monitor.Wait(SyncObject, 500))
+ return null;
+ else
+ return ourresults;
+ }
+ }
+
+ public override bool SupportsRaycastWorldFiltered()
+ {
+ return true;
+ }
+
+ public override object RaycastWorld(Vector3 position, Vector3 direction, float length, int Count, RayFilterFlags filter)
+ {
+ object SyncObject = new object();
+ List ourresults = new List();
+
+ RayCallback retMethod = delegate(List results)
+ {
+ lock (SyncObject)
+ {
+ ourresults = results;
+ Monitor.PulseAll(SyncObject);
+ }
+ };
+
+ ODERayRequest req = new ODERayRequest();
+ req.actor = null;
+ req.callbackMethod = retMethod;
+ req.length = length;
+ req.Normal = direction;
+ req.Origin = position;
+ req.Count = Count;
+ req.filter = filter;
+
+ lock (SyncObject)
+ {
+ m_rayCastManager.QueueRequest(req);
+ if (!Monitor.Wait(SyncObject, 500))
+ return null;
+ else
+ return ourresults;
+ }
+ }
+
+ public override List RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count, RayFilterFlags flags)
+ {
+ if (actor == null)
+ return new List();
+
+ 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();
+
+ if (geom == IntPtr.Zero)
+ return new List();
+
+ List ourResults = null;
+ object SyncObject = new object();
+
+ RayCallback retMethod = delegate(List results)
+ {
+ lock (SyncObject)
+ {
+ ourResults = results;
+ Monitor.PulseAll(SyncObject);
+ }
+ };
+
+ ODERayRequest req = new ODERayRequest();
+ req.actor = actor;
+ req.callbackMethod = retMethod;
+ req.length = length;
+ req.Normal = direction;
+ req.Origin = position;
+ req.Count = Count;
+ req.filter = flags;
+
+ lock (SyncObject)
+ {
+ m_rayCastManager.QueueRequest(req);
+ if (!Monitor.Wait(SyncObject, 500))
+ return new List();
+ }
+
+ if (ourResults == null)
+ return new List();
+ return ourResults;
+ }
+
+ public override List BoxProbe(Vector3 position, Vector3 size, Quaternion orientation, int Count, RayFilterFlags flags)
+ {
+ List ourResults = null;
+ object SyncObject = new object();
+
+ ProbeBoxCallback retMethod = delegate(List results)
+ {
+ lock (SyncObject)
+ {
+ ourResults = results;
+ Monitor.PulseAll(SyncObject);
+ }
+ };
+
+ ODERayRequest req = new ODERayRequest();
+ req.actor = null;
+ req.callbackMethod = retMethod;
+ req.Normal = size;
+ req.Origin = position;
+ req.orientation = orientation;
+ req.Count = Count;
+ req.filter = flags;
+
+ lock (SyncObject)
+ {
+ m_rayCastManager.QueueRequest(req);
+ if (!Monitor.Wait(SyncObject, 500))
+ return new List();
+ }
+
+ if (ourResults == null)
+ return new List();
+ return ourResults;
+ }
+
+ public override List SphereProbe(Vector3 position, float radius, int Count, RayFilterFlags flags)
+ {
+ List ourResults = null;
+ object SyncObject = new object();
+
+ ProbeSphereCallback retMethod = delegate(List results)
+ {
+ ourResults = results;
+ Monitor.PulseAll(SyncObject);
+ };
+
+ ODERayRequest req = new ODERayRequest();
+ req.actor = null;
+ req.callbackMethod = retMethod;
+ req.length = radius;
+ req.Origin = position;
+ req.Count = Count;
+ req.filter = flags;
+
+
+ lock (SyncObject)
+ {
+ m_rayCastManager.QueueRequest(req);
+ if (!Monitor.Wait(SyncObject, 500))
+ return new List();
+ }
+
+ if (ourResults == null)
+ return new List();
+ return ourResults;
+ }
+
+ public override List PlaneProbe(PhysicsActor actor, Vector4 plane, int Count, RayFilterFlags flags)
+ {
+ IntPtr geom = IntPtr.Zero;;
+
+ if (actor != null)
+ {
+ if (actor is OdePrim)
+ geom = ((OdePrim)actor).prim_geom;
+ else if (actor is OdeCharacter)
+ geom = ((OdePrim)actor).prim_geom;
+ }
+
+ List ourResults = null;
+ object SyncObject = new object();
+
+ ProbePlaneCallback retMethod = delegate(List results)
+ {
+ ourResults = results;
+ Monitor.PulseAll(SyncObject);
+ };
+
+ ODERayRequest req = new ODERayRequest();
+ req.actor = null;
+ req.callbackMethod = retMethod;
+ req.length = plane.W;
+ req.Normal.X = plane.X;
+ req.Normal.Y = plane.Y;
+ req.Normal.Z = plane.Z;
+ req.Count = Count;
+ req.filter = flags;
+
+ lock (SyncObject)
+ {
+ m_rayCastManager.QueueRequest(req);
+ if (!Monitor.Wait(SyncObject, 500))
+ return new List();
+ }
+
+ if (ourResults == null)
+ return new List();
+ return ourResults;
+ }
+
+ public override int SitAvatar(PhysicsActor actor, Vector3 AbsolutePosition, Vector3 CameraPosition, Vector3 offset, Vector3 AvatarSize, SitAvatarCallback PhysicsSitResponse)
+ {
+ Util.FireAndForget( delegate
+ {
+ ODESitAvatar sitAvatar = new ODESitAvatar(this, m_rayCastManager);
+ if(sitAvatar != null)
+ sitAvatar.Sit(actor, AbsolutePosition, CameraPosition, offset, AvatarSize, PhysicsSitResponse);
+ });
+ return 1;
+ }
+
+ }
+}
--
cgit v1.1