1 files changed, 120 insertions, 69 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);