From 6bc8e2413f2780fe4ce071a589a9d16efeaf2265 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Tue, 29 Jan 2019 13:49:29 +0000 Subject: [PATCH] ubode replace a locklessqueue by .net concurrentqueue --- .../PhysicsModules/ubOde/ODECharacter.cs | 60 ++++---- .../Region/PhysicsModules/ubOde/ODEScene.cs | 144 +++++++----------- 2 files changed, 84 insertions(+), 120 deletions(-) diff --git a/OpenSim/Region/PhysicsModules/ubOde/ODECharacter.cs b/OpenSim/Region/PhysicsModules/ubOde/ODECharacter.cs index 5242399dc8..5320912a1f 100644 --- a/OpenSim/Region/PhysicsModules/ubOde/ODECharacter.cs +++ b/OpenSim/Region/PhysicsModules/ubOde/ODECharacter.cs @@ -1143,7 +1143,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde /// This is the avatar's movement control + PID Controller /// /// - public void Move(List defects) + public void Move() { if(Body == IntPtr.Zero) return; @@ -1180,40 +1180,50 @@ namespace OpenSim.Region.PhysicsModule.ubOde _zeroPosition = localpos; } - if(!localpos.IsFinite()) - { - m_log.Warn("[PHYSICS]: Avatar Position is non-finite!"); - defects.Add(this); - // _parent_scene.RemoveCharacter(this); - - // destroy avatar capsule and related ODE data - AvatarGeomAndBodyDestroy(); - return; - } - // check outbounds forcing to be in world bool fixbody = false; - if(localpos.X < 0.0f) + float tmp = localpos.X; + if ((Single.IsNaN(tmp) || Single.IsInfinity(tmp))) + { + fixbody = true; + localpos.X = 128f; + } + else if (tmp < 0.0f) { fixbody = true; localpos.X = 0.1f; } - else if(localpos.X > m_parent_scene.WorldExtents.X - 0.1f) + else if (tmp > m_parent_scene.WorldExtents.X - 0.1f) { fixbody = true; localpos.X = m_parent_scene.WorldExtents.X - 0.1f; } - if(localpos.Y < 0.0f) + + tmp = localpos.Y; + if ((Single.IsNaN(tmp) || Single.IsInfinity(tmp))) + { + fixbody = true; + localpos.X = 128f; + } + else if (tmp < 0.0f) { fixbody = true; localpos.Y = 0.1f; } - else if(localpos.Y > m_parent_scene.WorldExtents.Y - 0.1) + else if(tmp > m_parent_scene.WorldExtents.Y - 0.1) { fixbody = true; localpos.Y = m_parent_scene.WorldExtents.Y - 0.1f; } - if(fixbody) + + tmp = localpos.Z; + if ((Single.IsNaN(tmp) || Single.IsInfinity(tmp))) + { + fixbody = true; + localpos.Z = 128f; + } + + if (fixbody) { m_freemove = false; SafeNativeMethods.BodySetPosition(Body,localpos.X,localpos.Y,localpos.Z); @@ -1387,7 +1397,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde _zeroFlag = false; fz /= m_PIDHoverTau; - float tmp = Math.Abs(fz); + tmp = Math.Abs(fz); if(tmp > 50) fz = 50 * Math.Sign(fz); else if(tmp < 0.1) @@ -1573,20 +1583,10 @@ namespace OpenSim.Region.PhysicsModule.ubOde if(vec.IsFinite()) { - if(vec.X != 0 || vec.Y !=0 || vec.Z !=0) + if((vec.X != 0 || vec.Y !=0 || vec.Z !=0)) SafeNativeMethods.BodyAddForce(Body,vec.X,vec.Y,vec.Z); } - else - { - m_log.Warn("[PHYSICS]: Got a NaN force vector in Move()"); - m_log.Warn("[PHYSICS]: Avatar Position is non-finite!"); - defects.Add(this); - // _parent_scene.RemoveCharacter(this); - // destroy avatar capsule and related ODE data - AvatarGeomAndBodyDestroy(); - return; - } - + // update our local ideia of position velocity and aceleration // _position = localpos; _position = localpos; diff --git a/OpenSim/Region/PhysicsModules/ubOde/ODEScene.cs b/OpenSim/Region/PhysicsModules/ubOde/ODEScene.cs index 9703eb57d3..97dcb69ab6 100644 --- a/OpenSim/Region/PhysicsModules/ubOde/ODEScene.cs +++ b/OpenSim/Region/PhysicsModules/ubOde/ODEScene.cs @@ -26,9 +26,10 @@ */ // Revision 2011/12/13 by Ubit Umarov -//#define SPAM + using System; +using System.Collections.Concurrent; using System.Collections.Generic; using System.Linq; using System.Reflection; @@ -158,7 +159,6 @@ namespace OpenSim.Region.PhysicsModule.ubOde public struct ODEchangeitem { public PhysicsActor actor; - public OdeCharacter character; public changes what; public Object arg; } @@ -230,7 +230,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde private HashSet _activeprims = new HashSet(); private HashSet _activegroups = new HashSet(); - public OpenSim.Framework.LocklessQueue ChangesQueue = new OpenSim.Framework.LocklessQueue(); + public ConcurrentQueue ChangesQueue = new ConcurrentQueue(); /// /// A list of actors that should receive collision events. @@ -621,8 +621,6 @@ namespace OpenSim.Region.PhysicsModule.ubOde return; } - - // Figure out how many contact points we have int count = 0; try @@ -1180,12 +1178,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde //m_log.Debug("[PHYSICS]:ODELOCK"); if (world == IntPtr.Zero) return; - lock (SimulationLock) - lock (OdeLock) - { - SafeNativeMethods.AllocateODEDataForThread(0); - ((OdeCharacter) actor).Destroy(); - } + ((OdeCharacter) actor).Destroy(); } public void addActivePrim(OdePrim activatePrim) @@ -1224,7 +1217,6 @@ namespace OpenSim.Region.PhysicsModule.ubOde 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) { @@ -1244,6 +1236,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde _activeprims.Remove(deactivatePrim); } } + public void remActiveGroup(OdePrim deactivatePrim) { lock (_activegroups) @@ -1258,25 +1251,16 @@ namespace OpenSim.Region.PhysicsModule.ubOde // removed in the next physics simulate pass. if (prim is OdePrim) { -// lock (OdeLock) - { - - OdePrim p = (OdePrim)prim; - p.setPrimForRemoval(); - } + 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); - } - + lock (_prims) + _prims.Remove(prim.LocalID); } public void addToPrims(OdePrim prim) @@ -1405,12 +1389,17 @@ namespace OpenSim.Region.PhysicsModule.ubOde /// to use in place of old taint mechanism so changes do have a time sequence /// - public void AddChange(PhysicsActor actor, changes what, Object arg) + public void AddChange(PhysicsActor _actor, changes _what, Object _arg) { - ODEchangeitem item = new ODEchangeitem(); - item.actor = actor; - item.what = what; - item.arg = arg; + if (world == IntPtr.Zero) + return; + + ODEchangeitem item = new ODEchangeitem + { + actor = _actor, + what = _what, + arg = _arg + }; ChangesQueue.Enqueue(item); } @@ -1430,22 +1419,19 @@ namespace OpenSim.Region.PhysicsModule.ubOde lock (OdeLock) { if (world == IntPtr.Zero) - { - ChangesQueue.Clear(); return; - } - - SafeNativeMethods.AllocateODEDataForThread(~0U); ODEchangeitem item; int donechanges = 0; - if (ChangesQueue.Count > 0) + if (!ChangesQueue.IsEmpty) { m_log.InfoFormat("[ubOde] start processing pending actor operations"); int tstart = Util.EnvironmentTickCount(); - while (ChangesQueue.Dequeue(out item)) + SafeNativeMethods.AllocateODEDataForThread(~0U); + + while (ChangesQueue.TryDequeue(out item)) { if (item.actor != null) { @@ -1511,13 +1497,6 @@ namespace OpenSim.Region.PhysicsModule.ubOde lock (OdeLock) { - if (world == IntPtr.Zero) - { - ChangesQueue.Clear(); - return 0; - } - - ODEchangeitem item; // d.WorldSetQuickStepNumIterations(world, curphysiteractions); @@ -1539,17 +1518,19 @@ namespace OpenSim.Region.PhysicsModule.ubOde */ SafeNativeMethods.AllocateODEDataForThread(~0U); - if (ChangesQueue.Count > 0) + if (!ChangesQueue.IsEmpty) { - while (ChangesQueue.Dequeue(out item)) + ODEchangeitem item; + + while (ChangesQueue.TryDequeue(out item)) { if (item.actor != null) { try { - lock (SimulationLock) - { - if (item.actor is OdeCharacter) + lock (SimulationLock) + { + 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); @@ -1575,31 +1556,20 @@ namespace OpenSim.Region.PhysicsModule.ubOde // clear pointer/counter to contacts to pass into joints m_global_contactcount = 0; + // tmpTime = Util.GetTimeStampMS(); -// tmpTime = Util.GetTimeStampMS(); - - // 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 (SimulationLock) { + // Move characters + lock (_characters) + { + foreach (OdeCharacter actor in _characters) + { + actor.Move(); + } + } + + // Move other active objects lock (_activegroups) { foreach (OdePrim aprim in _activegroups) @@ -1629,9 +1599,6 @@ namespace OpenSim.Region.PhysicsModule.ubOde List sleepers = new List(); foreach (PhysicsActor obj in _collisionEventPrim) { - if (obj == null) - continue; - switch ((ActorTypes)obj.PhysicsActorType) { case ActorTypes.Agent: @@ -1644,10 +1611,13 @@ namespace OpenSim.Region.PhysicsModule.ubOde if (!pobj.m_outbounds) { pobj.SendCollisions((int)(odetimestepMS)); - if(pobj.Body != IntPtr.Zero && !pobj.m_isSelected && - !pobj.m_disabled && !pobj.m_building && - !SafeNativeMethods.BodyIsEnabled(pobj.Body)) - sleepers.Add(pobj); + lock(SimulationLock) + { + if(pobj.Body != IntPtr.Zero && !pobj.m_isSelected && + !pobj.m_disabled && !pobj.m_building && + !SafeNativeMethods.BodyIsEnabled(pobj.Body)) + sleepers.Add(pobj); + } } break; } @@ -1656,17 +1626,17 @@ namespace OpenSim.Region.PhysicsModule.ubOde foreach(OdePrim prm in sleepers) prm.SleeperAddCollisionEvents(); sleepers.Clear(); - // collisonRepo += Util.GetTimeStampMS() - tmpTime; + // collisonRepo += Util.GetTimeStampMS() - tmpTime; // do a ode simulation step - // tmpTime = Util.GetTimeStampMS(); + // tmpTime = Util.GetTimeStampMS(); lock (SimulationLock) { SafeNativeMethods.WorldQuickStep(world, ODE_STEPSIZE); SafeNativeMethods.JointGroupEmpty(contactgroup); } - // qstepTIme += Util.GetTimeStampMS() - tmpTime; + // qstepTIme += Util.GetTimeStampMS() - tmpTime; // update managed ideia of physical data and do updates to core /* @@ -1709,7 +1679,6 @@ namespace OpenSim.Region.PhysicsModule.ubOde // ode.dunlock(world); } - step_time -= ODE_STEPSIZE; nodeframes++; @@ -1727,7 +1696,6 @@ namespace OpenSim.Region.PhysicsModule.ubOde { RemoveCharacter(chr); } - _badCharacter.Clear(); } } @@ -1815,8 +1783,6 @@ namespace OpenSim.Region.PhysicsModule.ubOde mesher.ExpireReleaseMeshs(); m_lastMeshExpire = now; } - - } return fps; @@ -2005,6 +1971,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde private void InitTerrain() { + lock(SimulationLock) lock (OdeLock) { SafeNativeMethods.AllocateODEDataForThread(~0U); @@ -2110,6 +2077,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde yt += heightmapWidthSamples; } + lock(SimulationLock) lock (OdeLock) { SafeNativeMethods.GeomOSTerrainDataSetBounds(HeightmapData, minH, maxH); @@ -2133,9 +2101,9 @@ namespace OpenSim.Region.PhysicsModule.ubOde public override void Dispose() { + lock(SimulationLock) lock (OdeLock) { - if (world == IntPtr.Zero) return; @@ -2152,7 +2120,6 @@ namespace OpenSim.Region.PhysicsModule.ubOde lock (_prims) { - ChangesQueue.Clear(); foreach (OdePrim prm in _prims.Values) { prm.DoAChange(changes.Remove, null); @@ -2168,7 +2135,6 @@ namespace OpenSim.Region.PhysicsModule.ubOde _characters.CopyTo(chtorem); } - ChangesQueue.Clear(); foreach (OdeCharacter ch in chtorem) ch.DoAChange(changes.Remove, null); @@ -2257,7 +2223,6 @@ namespace OpenSim.Region.PhysicsModule.ubOde } } - public override List RaycastWorld(Vector3 position, Vector3 direction, float length, int Count) { List ourresults = new List(); @@ -2498,6 +2463,5 @@ namespace OpenSim.Region.PhysicsModule.ubOde }); return 1; } - } }