From a8f1cd7e8643fc6f4f4f12b1af3e52b9830f5a5a Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Wed, 29 Feb 2012 19:32:23 +0000 Subject: [PATCH] update ubitODE to current working state --- .../Region/Physics/UbitOdePlugin/ODEPrim.cs | 189 +++++++++++------- .../Region/Physics/UbitOdePlugin/OdeScene.cs | 123 ++++++++---- 2 files changed, 199 insertions(+), 113 deletions(-) diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index 0ccdbc0d3a..17f38e881b 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -155,6 +155,8 @@ namespace OpenSim.Region.Physics.OdePlugin public float m_collisionscore; int m_colliderfilter = 0; + public IntPtr collide_geom; // for objects: geom if single prim space it linkset + private float m_density = 10.000006836f; // Aluminum g/cm3; public bool _zeroFlag; @@ -453,8 +455,6 @@ namespace OpenSim.Region.Physics.OdePlugin { get { - // Averate previous velocity with the new one so - // client object interpolation works a 'little' better if (_zeroFlag) return Vector3.Zero; return _velocity; @@ -833,6 +833,7 @@ namespace OpenSim.Region.Physics.OdePlugin body_autodisable_frames = parent_scene.bodyFramesAutoDisable; prim_geom = IntPtr.Zero; + collide_geom = IntPtr.Zero; Body = IntPtr.Zero; if (!size.IsFinite()) @@ -1367,7 +1368,7 @@ namespace OpenSim.Region.Physics.OdePlugin d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); m_collisionscore = 0; - if (m_targetSpace != _parent_scene.ActiveSpace) +// if (m_targetSpace != _parent_scene.ActiveSpace) { if (m_targetSpace != IntPtr.Zero) { @@ -1376,9 +1377,26 @@ namespace OpenSim.Region.Physics.OdePlugin d.SpaceRemove(m_targetSpace, prim_geom); } +// m_targetSpace = _parent_scene.ActiveSpace; +// d.SpaceAdd(m_targetSpace, prim_geom); + } + + + if (childrenPrim.Count == 0) + { + collide_geom = prim_geom; m_targetSpace = _parent_scene.ActiveSpace; d.SpaceAdd(m_targetSpace, prim_geom); } + else + { + m_targetSpace = d.HashSpaceCreate(_parent_scene.ActiveSpace); + d.HashSpaceSetLevels(m_targetSpace, -2, 8); + d.SpaceSetSublevel(m_targetSpace, 3); + d.SpaceSetCleanup(m_targetSpace, false); + d.SpaceAdd(m_targetSpace, prim_geom); + collide_geom = m_targetSpace; + } lock (childrenPrim) { @@ -1396,15 +1414,15 @@ namespace OpenSim.Region.Physics.OdePlugin d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags); prm.m_collisionscore = 0; - if (prm.m_targetSpace != _parent_scene.ActiveSpace) + if (prm.m_targetSpace != m_targetSpace) { if (prm.m_targetSpace != IntPtr.Zero) { - _parent_scene.waitForSpaceUnlock(m_targetSpace); + _parent_scene.waitForSpaceUnlock(prm.m_targetSpace); if (d.SpaceQuery(prm.m_targetSpace, prm.prim_geom)) d.SpaceRemove(prm.m_targetSpace, prm.prim_geom); } - prm.m_targetSpace = _parent_scene.ActiveSpace; + prm.m_targetSpace = m_targetSpace; d.SpaceAdd(m_targetSpace, prm.prim_geom); } @@ -1427,8 +1445,14 @@ namespace OpenSim.Region.Physics.OdePlugin d.GeomDisable(prim_geom); d.BodyDisable(Body); } + else + { + d.BodySetAngularVel(Body, m_rotationalVelocity.X, m_rotationalVelocity.Y, m_rotationalVelocity.Z); + d.BodySetLinearVel(Body, _velocity.X, _velocity.Y, _velocity.Z); + } _parent_scene.addActivePrim(this); + _parent_scene.addActiveGroups(this); } private void DestroyBody() @@ -1473,6 +1497,7 @@ namespace OpenSim.Region.Physics.OdePlugin d.JointDestroy(Amotor); Amotor = IntPtr.Zero; } + _parent_scene.remActiveGroup(this); d.BodyDestroy(Body); } Body = IntPtr.Zero; @@ -2390,8 +2415,8 @@ namespace OpenSim.Region.Physics.OdePlugin else */ DestroyBody(); - Stop(); } + Stop(); } } @@ -2576,7 +2601,8 @@ namespace OpenSim.Region.Physics.OdePlugin if ((bool)newbuilding) { m_building = true; - DestroyBody(); + if (!childPrim) + DestroyBody(); } else { @@ -2648,12 +2674,95 @@ namespace OpenSim.Region.Physics.OdePlugin public void Move() { if (!childPrim && m_isphysical && Body != IntPtr.Zero && - !m_disabled && !m_isSelected && d.BodyIsEnabled(Body) && !m_building) // KF: Only move root prims. + !m_disabled && !m_isSelected && d.BodyIsEnabled(Body) && !m_building && !m_outbounds) + // !m_disabled && !m_isSelected && !m_building && !m_outbounds) { - // if (!d.BodyIsEnabled(Body)) d.BodyEnable(Body); // KF add 161009 +// if (!d.BodyIsEnabled(Body)) d.BodyEnable(Body); // KF add 161009 float timestep = _parent_scene.ODE_STEPSIZE; + // check outside region + d.Vector3 lpos; + d.GeomCopyPosition(prim_geom, out lpos); // root position that is seem by rest of simulator + + if (lpos.Z < -100 || lpos.Z > 100000f) + { + m_outbounds = true; + + lpos.Z = Util.Clip(lpos.Z, -100f, 100000f); + _acceleration.X = 0; + _acceleration.Y = 0; + _acceleration.Z = 0; + + _velocity.X = 0; + _velocity.Y = 0; + _velocity.Z = 0; + m_rotationalVelocity.X = 0; + m_rotationalVelocity.Y = 0; + m_rotationalVelocity.Z = 0; + + d.BodySetLinearVel(Body, 0, 0, 0); // stop it + d.BodySetAngularVel(Body, 0, 0, 0); // stop it + d.BodySetPosition(Body, lpos.X, lpos.Y, lpos.Z); // put it somewhere + m_lastposition = _position; + m_lastorientation = _orientation; + + base.RequestPhysicsterseUpdate(); + + m_throttleUpdates = false; + throttleCounter = 0; + _zeroFlag = true; + + disableBodySoft(); // disable it and colisions + base.RaiseOutOfBounds(_position); + return; + } + + if (lpos.X < 0f) + { + _position.X = Util.Clip(lpos.X, -2f, -0.1f); + m_outbounds = true; + } + else if(lpos.X > _parent_scene.WorldExtents.X) + { + _position.X = Util.Clip(lpos.X, _parent_scene.WorldExtents.X + 0.1f, _parent_scene.WorldExtents.X + 2f); + m_outbounds = true; + } + if (lpos.Y < 0f) + { + _position.Y = Util.Clip(lpos.Y, -2f, -0.1f); + m_outbounds = true; + } + else if(lpos.Y > _parent_scene.WorldExtents.Y) + { + _position.Y = Util.Clip(lpos.Y, _parent_scene.WorldExtents.Y + 0.1f, _parent_scene.WorldExtents.Y + 2f); + m_outbounds = true; + } + + if(m_outbounds) + { + m_lastposition = _position; + m_lastorientation = _orientation; + + d.Vector3 dtmp = d.BodyGetAngularVel(Body); + m_rotationalVelocity.X = dtmp.X; + m_rotationalVelocity.Y = dtmp.Y; + m_rotationalVelocity.Z = dtmp.Z; + + dtmp = d.BodyGetLinearVel(Body); + _velocity.X = dtmp.X; + _velocity.Y = dtmp.Y; + _velocity.Z = dtmp.Z; + + d.BodySetLinearVel(Body, 0, 0, 0); // stop it + d.BodySetAngularVel(Body, 0, 0, 0); + d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); + disableBodySoft(); // stop collisions + base.RequestPhysicsterseUpdate(); + return; + } + + float fx = 0; float fy = 0; float fz = 0; @@ -2862,7 +2971,7 @@ namespace OpenSim.Region.Physics.OdePlugin public void UpdatePositionAndVelocity(float simulatedtime) { // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit! - if (_parent == null && !m_disabled && !m_building) + if (_parent == null && !m_disabled && !m_building && !m_outbounds) { if (Body != IntPtr.Zero) { @@ -2872,64 +2981,6 @@ namespace OpenSim.Region.Physics.OdePlugin d.Vector3 lpos; d.GeomCopyPosition(prim_geom, out lpos); // root position that is seem by rest of simulator - // we need to use root position since that's all the rest of scene uses - if (lpos.X < 0f || lpos.X > _parent_scene.WorldExtents.X - || lpos.Y < 0f || lpos.Y > _parent_scene.WorldExtents.Y - ) - { - // we are outside current region - // we can't let it keeping moving and having colisions - // since it can be stucked between something like terrain and edge - // so lets stop and disable it until something else kicks it - - _position.X = Util.Clip(lpos.X, -0.2f, _parent_scene.WorldExtents.X + 0.2f); - _position.Y = Util.Clip(lpos.Y, -0.2f, _parent_scene.WorldExtents.Y + 0.2f); - _position.Z = Util.Clip(lpos.Z, -100f, 50000f); - - m_lastposition = _position; -// m_lastorientation = _orientation; - - d.BodySetLinearVel(Body, 0, 0, 0); // stop it -// d.BodySetAngularVel(Body, 0, 0, 0); - d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); - disableBodySoft(); // stop collisions - m_outbounds = true; - base.RequestPhysicsterseUpdate(); - return; - } - - if (lpos.Z < -100 || lpos.Z > 100000f) - { - lpos.Z = Util.Clip(lpos.Z, -100f, 50000f); - - _acceleration.X = 0; - _acceleration.Y = 0; - _acceleration.Z = 0; - - _velocity.X = 0; - _velocity.Y = 0; - _velocity.Z = 0; - m_rotationalVelocity.X = 0; - m_rotationalVelocity.Y = 0; - m_rotationalVelocity.Z = 0; - - d.BodySetLinearVel(Body, 0, 0, 0); // stop it - d.BodySetAngularVel(Body, 0, 0, 0); // stop it - d.BodySetPosition(Body, lpos.X, lpos.Y, lpos.Z); // put it somewhere - m_lastposition = _position; - m_lastorientation = _orientation; - - base.RequestPhysicsterseUpdate(); - - m_throttleUpdates = false; - throttleCounter = 0; - _zeroFlag = true; - - disableBodySoft(); // disable it and colisions - base.RaiseOutOfBounds(_position); - - return; - } d.Quaternion ori; d.GeomCopyQuaternion(prim_geom, out ori); diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index 6e4c373a56..ccb4c23a9d 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -224,6 +224,7 @@ namespace OpenSim.Region.Physics.OdePlugin private readonly HashSet _characters = new HashSet(); private readonly HashSet _prims = new HashSet(); private readonly HashSet _activeprims = new HashSet(); + private readonly HashSet _activegroups = new HashSet(); public OpenSim.Framework.LocklessQueue ChangesQueue = new OpenSim.Framework.LocklessQueue(); @@ -1013,15 +1014,24 @@ namespace OpenSim.Region.Physics.OdePlugin } - // collide active prims with static enviroment lock (_activeprims) + { + foreach (OdePrim aprim in _activeprims) + { + aprim.CollisionScore = 0; + aprim.IsColliding = false; + } + } + + // collide active prims with static enviroment + lock (_activegroups) { try { - foreach (OdePrim prm in _activeprims) + foreach (OdePrim prm in _activegroups) { - if (d.BodyIsEnabled(prm.Body)) - d.SpaceCollide2(StaticSpace, prm.prim_geom, IntPtr.Zero, nearCallback); + if (d.BodyIsEnabled(prm.Body) && !prm.m_outbounds) + d.SpaceCollide2(StaticSpace, prm.collide_geom, IntPtr.Zero, nearCallback); } } catch (AccessViolationException) @@ -1029,7 +1039,6 @@ namespace OpenSim.Region.Physics.OdePlugin m_log.Warn("[PHYSICS]: Unable to collide Active prim to static space"); } } - // finally colide active things amoung them try { @@ -1039,7 +1048,6 @@ namespace OpenSim.Region.Physics.OdePlugin { m_log.Warn("[PHYSICS]: Unable to collide in Active space"); } - // _perloopContact.Clear(); } @@ -1148,13 +1156,20 @@ namespace OpenSim.Region.Physics.OdePlugin public void addActivePrim(OdePrim activatePrim) { - // adds active prim.. (ones that should be iterated over in collisions_optimized + // adds active prim.. lock (_activeprims) { if (!_activeprims.Contains(activatePrim)) _activeprims.Add(activatePrim); - //else - // m_log.Warn("[PHYSICS]: Double Entry in _activeprims detected, potential crash immenent"); + } + } + + public void addActiveGroups(OdePrim activatePrim) + { + lock (_activegroups) + { + if (!_activegroups.Contains(activatePrim)) + _activegroups.Add(activatePrim); } } @@ -1186,6 +1201,13 @@ namespace OpenSim.Region.Physics.OdePlugin _activeprims.Remove(deactivatePrim); } } + public void remActiveGroup(OdePrim deactivatePrim) + { + lock (_activegroups) + { + _activegroups.Remove(deactivatePrim); + } + } public override void RemovePrim(PhysicsActor prim) { @@ -1258,6 +1280,11 @@ namespace OpenSim.Region.Physics.OdePlugin { waitForSpaceUnlock(currentspace); d.SpaceRemove(currentspace, geom); + + if (d.SpaceGetSublevel(currentspace) > 2 && d.SpaceGetNumGeoms(currentspace) == 0) + { + d.SpaceDestroy(currentspace); + } } else { @@ -1274,6 +1301,12 @@ namespace OpenSim.Region.Physics.OdePlugin { waitForSpaceUnlock(currentspace); d.SpaceRemove(currentspace, geom); + + if (d.SpaceGetSublevel(currentspace) > 1 && d.SpaceGetNumGeoms(currentspace) == 0) + { + d.SpaceDestroy(currentspace); + } + } } } @@ -1577,42 +1610,14 @@ namespace OpenSim.Region.Physics.OdePlugin statchanges += Util.EnvironmentTickCountSubtract(statstart); - // Move characters - lock (_characters) - { - List defects = new List(); - 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); @@ -1642,8 +1647,35 @@ namespace OpenSim.Region.Physics.OdePlugin d.WorldQuickStep(world, ODE_STEPSIZE); statstep += Util.EnvironmentTickCountSubtract(statstart); - d.JointGroupEmpty(contactgroup); - totjcontact += m_global_contactcount; + + // Move characters + lock (_characters) + { + List defects = new List(); + 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 (_activegroups) + { + foreach (OdePrim aprim in _activegroups) + { + aprim.Move(); + } + } + //ode.dunlock(world); } catch (Exception e) @@ -1652,6 +1684,9 @@ namespace OpenSim.Region.Physics.OdePlugin // ode.dunlock(world); } + d.JointGroupEmpty(contactgroup); + totjcontact += m_global_contactcount; + step_time -= ODE_STEPSIZE; nodeframes++; } @@ -1686,10 +1721,10 @@ namespace OpenSim.Region.Physics.OdePlugin } statmovchar = Util.EnvironmentTickCountSubtract(statstart); - lock (_activeprims) + lock (_activegroups) { { - foreach (OdePrim actor in _activeprims) + foreach (OdePrim actor in _activegroups) { if (actor.IsPhysical) { @@ -1966,7 +2001,7 @@ namespace OpenSim.Region.Physics.OdePlugin 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); + d.GeomSetPosition(GroundGeom, pOffset.X + (float)Constants.RegionSize * 0.5f, pOffset.Y + (float)Constants.RegionSize * 0.5f, 0); RegionTerrain.Add(pOffset, GroundGeom, GroundGeom); // TerrainHeightFieldHeights.Add(GroundGeom, ODElandMap); TerrainHeightFieldHeights.Add(GroundGeom, _heightmap);