update ubitODE to current working state

2 files changed, 199 insertions, 113 deletions

diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs
index 0ccdbc0..17f38e8 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 6e4c373..ccb4c23 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<OdeCharacter> _characters = new HashSet<OdeCharacter>();
         private readonly HashSet<OdePrim> _prims = new HashSet<OdePrim>();
         private readonly HashSet<OdePrim> _activeprims = new HashSet<OdePrim>();
+        private readonly HashSet<OdePrim> _activegroups = new HashSet<OdePrim>();
 
         public OpenSim.Framework.LocklessQueue<ODEchangeitem> ChangesQueue = new OpenSim.Framework.LocklessQueue<ODEchangeitem>();
 
@@ -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<OdeCharacter> defects = new List<OdeCharacter>();
-                            foreach (OdeCharacter actor in _characters)
-                            {
-                                if (actor != null)
-                                    actor.Move(ODE_STEPSIZE, defects);
-                            }
-                            if (defects.Count != 0)
-                            {
-                                foreach (OdeCharacter defect in defects)
-                                {
-                                    RemoveCharacter(defect);
-                                }
-                            }
-                        }
-                        statchmove += Util.EnvironmentTickCountSubtract(statstart);
-
-                        // Move other active objects
-                        lock (_activeprims)
-                        {
-                            foreach (OdePrim aprim in _activeprims)
-                            {
-                                aprim.CollisionScore = 0;
-                                aprim.IsColliding = false;
-                                aprim.Move();
-                            }
-                        }
-
                         statactmove += Util.EnvironmentTickCountSubtract(statstart);
                         //if ((framecount % m_randomizeWater) == 0)
                         // randomizeWater(waterlevel);
 
                         m_rayCastManager.ProcessQueuedRequests();
 
+
+
                         statray += Util.EnvironmentTickCountSubtract(statstart);
                         collision_optimized();
                         statcol += Util.EnvironmentTickCountSubtract(statstart);
@@ -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<OdeCharacter> defects = new List<OdeCharacter>();
+                            foreach (OdeCharacter actor in _characters)
+                            {
+                                if (actor != null)
+                                    actor.Move(ODE_STEPSIZE, defects);
+                            }
+                            if (defects.Count != 0)
+                            {
+                                foreach (OdeCharacter defect in defects)
+                                {
+                                    RemoveCharacter(defect);
+                                }
+                            }
+                        }
+                        statchmove += Util.EnvironmentTickCountSubtract(statstart);
+
+                        // Move other active objects
+                        lock (_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);