* This is a very icky implementation of physical linkset prim using fixed joints. This will change quite drastically, however it's fun to play with.

* To play with this you must link your prim before setting it physical, otherwise they won't link in the physics engine properly.  This will also be fixed.
* Currently the linked prim are extremely unstable because I have yet to implement combining of forces with the same normal. This will also be fixed.   In fact, the whole PhysicsActor, ODEPrim relationship will be reworked to consider groups from the get-go.
* This implementation is better then it crashing your sim, so I'm commiting it for now.

10 files changed, 293 insertions, 124 deletions

diff --git a/OpenSim/Region/Environment/Scenes/Scene.cs b/OpenSim/Region/Environment/Scenes/Scene.cs
index f6e3d82..a20e466 100644
--- a/OpenSim/Region/Environment/Scenes/Scene.cs
+++ b/OpenSim/Region/Environment/Scenes/Scene.cs
@@ -120,8 +120,8 @@ namespace OpenSim.Region.Environment.Scenes
 
         private int m_update_physics = 1;
         private int m_update_entitymovement = 1;
-        private int m_update_entities = 20; // Run through all objects checking for updates
-        private int m_update_entitiesquick = 1; // Run through objects that have scheduled updates checking for updates
+        private int m_update_entities = 1; // Run through all objects checking for updates
+        private int m_update_entitiesquick = 200; // Run through objects that have scheduled updates checking for updates
         private int m_update_presences = 1; // Update scene presence movements
         private int m_update_events = 1;
         private int m_update_backup = 200;
diff --git a/OpenSim/Region/Environment/Scenes/SceneObjectGroup.cs b/OpenSim/Region/Environment/Scenes/SceneObjectGroup.cs
index 801e614..713c3ba 100644
--- a/OpenSim/Region/Environment/Scenes/SceneObjectGroup.cs
+++ b/OpenSim/Region/Environment/Scenes/SceneObjectGroup.cs
@@ -1758,10 +1758,13 @@ namespace OpenSim.Region.Environment.Scenes
             {
                 lock (m_parts)
                 {
+                    m_rootPart.ApplyPhysics(m_rootPart.ObjectFlags, m_physicalPrim);
                     foreach (SceneObjectPart part in m_parts.Values)
                     {
-                        part.ApplyPhysics(m_rootPart.ObjectFlags, m_physicalPrim);
-
+                        if (part.LocalID != m_rootPart.LocalID)
+                        {
+                            part.ApplyPhysics(m_rootPart.ObjectFlags, m_physicalPrim);
+                        }
                         // Hack to get the physics scene geometries in the right spot
                         ResetChildPrimPhysicsPositions();
 
diff --git a/OpenSim/Region/Environment/Scenes/SceneObjectPart.cs b/OpenSim/Region/Environment/Scenes/SceneObjectPart.cs
index 6da4c3e..04fa03e 100644
--- a/OpenSim/Region/Environment/Scenes/SceneObjectPart.cs
+++ b/OpenSim/Region/Environment/Scenes/SceneObjectPart.cs
@@ -1306,9 +1306,12 @@ namespace OpenSim.Region.Environment.Scenes
 
                         PhysActor.OnRequestTerseUpdate -= PhysicsRequestingTerseUpdate;
                         PhysActor.OnOutOfBounds -= PhysicsOutOfBounds;
+                        PhysActor.delink();
                     }
 
                     PhysActor.IsPhysical = UsePhysics;
+
+
                     // If we're not what we're supposed to be in the physics scene, recreate ourselves.
                     //m_parentGroup.Scene.PhysicsScene.RemovePrim(PhysActor);
                     /// that's not wholesome.  Had to make Scene public
@@ -1331,6 +1334,14 @@ namespace OpenSim.Region.Environment.Scenes
 
                             PhysActor.OnRequestTerseUpdate += PhysicsRequestingTerseUpdate;
                             PhysActor.OnOutOfBounds += PhysicsOutOfBounds;
+                            if (ParentID != 0 && ParentID != LocalID)
+                            {
+                                if (ParentGroup.RootPart.PhysActor != null)
+                                {
+                                    PhysActor.link(ParentGroup.RootPart.PhysActor);
+                                }
+                            }
+
                         }
                     }
                 }
diff --git a/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsPlugin.cs b/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsPlugin.cs
index e3b9ef3..c7ccef3 100644
--- a/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsPlugin.cs
+++ b/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsPlugin.cs
@@ -349,7 +349,15 @@ namespace OpenSim.Region.Physics.BasicPhysicsPlugin
             get { return true; }
             set { }
         }
+        public override void link(PhysicsActor obj)
+        {
+
+        }
 
+        public override void delink()
+        {
+
+        }
         public void SetAcceleration(PhysicsVector accel)
         {
             _acceleration = accel;
diff --git a/OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs b/OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs
index 8a4e4f4..12e2ea2 100644
--- a/OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs
+++ b/OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs
@@ -832,7 +832,15 @@ namespace OpenSim.Region.Physics.BulletXPlugin
                 }
             }
         }
+        public override void link(PhysicsActor obj)
+        {
+
+        }
 
+        public override void delink()
+        {
+
+        }
         public override float Mass
         {
             get { return ActorMass; }
diff --git a/OpenSim/Region/Physics/Manager/PhysicsActor.cs b/OpenSim/Region/Physics/Manager/PhysicsActor.cs
index aa6cf43..e31a08d 100644
--- a/OpenSim/Region/Physics/Manager/PhysicsActor.cs
+++ b/OpenSim/Region/Physics/Manager/PhysicsActor.cs
@@ -132,6 +132,10 @@ namespace OpenSim.Region.Physics.Manager
 
         public abstract void CrossingFailure();
 
+        public abstract void link(PhysicsActor obj);
+
+        public abstract void delink();
+
         public virtual void RequestPhysicsterseUpdate()
         {
             // Make a temporary copy of the event to avoid possibility of
@@ -353,6 +357,16 @@ namespace OpenSim.Region.Physics.Manager
             set { return; }
         }
 
+        public override void link(PhysicsActor obj)
+        {
+
+        }
+
+        public override void delink()
+        {
+
+        }
+
         public override void AddForce(PhysicsVector force)
         {
             return;
diff --git a/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs
index 2efca3b..f335246 100644
--- a/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs
+++ b/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs
@@ -490,7 +490,15 @@ namespace OpenSim.Region.Physics.OdePlugin
                 return m_density*AVvolume;
             }
         }
+        public override void link(PhysicsActor obj)
+        {
+
+        }
 
+        public override void delink()
+        {
+
+        }
         private void standupStraight()
         {
 
diff --git a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
index 34c5cfd..eb90cf4 100644
--- a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
+++ b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
@@ -90,6 +90,11 @@ namespace OpenSim.Region.Physics.OdePlugin
         public IntPtr prev_geom;
         public IntPtr _triMeshData;
 
+        private IntPtr _linkJointGroup = (IntPtr)0;
+        private PhysicsActor _parent = null;
+        private PhysicsActor m_taintparent = null;
+        
+
         private bool iscolliding = false;
         private bool m_isphysical = false;
         private bool m_isSelected = false;
@@ -113,6 +118,8 @@ namespace OpenSim.Region.Physics.OdePlugin
         private PhysicsVector _target_velocity;
         public d.Mass pMass;
 
+        private IntPtr m_linkJoint = (IntPtr)0;
+
         private int debugcounter = 0;
 
         public OdePrim(String primName, OdeScene parent_scene, PhysicsVector pos, PhysicsVector size,
@@ -719,6 +726,45 @@ namespace OpenSim.Region.Physics.OdePlugin
 
             if (m_taintVelocity != PhysicsVector.Zero)
                 changevelocity(timestep);
+
+            if (m_taintparent != _parent)
+                changelink(timestep);
+        }
+
+        private void changelink(float timestep)
+        {
+            while (ode.lockquery())
+            {
+            }
+            ode.dlock(_parent_scene.world);
+
+            if (_parent == null && m_taintparent != null)
+            {
+                if (m_taintparent.PhysicsActorType == (int)ActorTypes.Prim)
+                {
+                    OdePrim obj = (OdePrim)m_taintparent;
+                    if (obj.Body != (IntPtr)0 && Body != (IntPtr)0)
+                    {
+                        _linkJointGroup = d.JointGroupCreate(0);
+                        m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup);
+                        d.JointAttach(m_linkJoint, obj.Body, Body);
+                        d.JointSetFixed(m_linkJoint);
+                    }
+                }
+
+            }
+            else if (_parent != null && m_taintparent == null)
+            {
+                if (Body != (IntPtr)0 && _linkJointGroup != (IntPtr)0)
+                    d.JointGroupDestroy(_linkJointGroup);
+
+                _linkJointGroup = (IntPtr)0;
+                m_linkJoint = (IntPtr)0;
+
+            }
+            ode.dunlock(_parent_scene.world);
+
+            _parent = m_taintparent;
         }
 
         private void changeSelectedStatus(float timestep)
@@ -970,8 +1016,25 @@ namespace OpenSim.Region.Physics.OdePlugin
                     enableBody();
                 //Prim auto disable after 20 frames, 
                 //if you move it, re-enable the prim manually.
-               
+                if (_parent != null)
+                {
+                    if (m_linkJoint != (IntPtr)0)
+                    {
+                        d.JointDestroy(m_linkJoint);
+                        m_linkJoint = (IntPtr)0;
+                    }
+                }
                 d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
+                if (_parent != null)
+                {
+                    OdePrim odParent = (OdePrim)_parent;
+                    if (Body != (IntPtr)0 && odParent.Body != (IntPtr)0)
+                    {
+                        m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup);
+                        d.JointAttach(m_linkJoint, Body, odParent.Body);
+                        d.JointSetFixed(m_linkJoint);
+                    }
+                }
                 d.BodyEnable(Body);
                 
             }
@@ -1652,154 +1715,172 @@ namespace OpenSim.Region.Physics.OdePlugin
                 m_log.Warn("[PHYSICS]: Too many crossing failures for: " + m_primName);
             }
         }
+
+        public override void link(PhysicsActor obj)
+        {
+            m_taintparent = obj;
+        }
+
+        public override void delink()
+        {
+            m_taintparent = null;
+        }
+
         public void UpdatePositionAndVelocity()
         { 
             //  no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit!
-            PhysicsVector pv = new PhysicsVector(0, 0, 0);
-            bool lastZeroFlag = _zeroFlag;
-            if (Body != (IntPtr) 0)
+            if (_parent != null)
             {
-                d.Vector3 vec = d.BodyGetPosition(Body);
-                d.Quaternion ori = d.BodyGetQuaternion(Body);
-                d.Vector3 vel = d.BodyGetLinearVel(Body);
-                d.Vector3 rotvel = d.BodyGetAngularVel(Body);
 
-                PhysicsVector l_position = new PhysicsVector();
+            }
+            else
+            {
+                PhysicsVector pv = new PhysicsVector(0, 0, 0);
+                bool lastZeroFlag = _zeroFlag;
+                if (Body != (IntPtr)0)
+                {
+                    d.Vector3 vec = d.BodyGetPosition(Body);
+                    d.Quaternion ori = d.BodyGetQuaternion(Body);
+                    d.Vector3 vel = d.BodyGetLinearVel(Body);
+                    d.Vector3 rotvel = d.BodyGetAngularVel(Body);
 
-                
-                //  kluge to keep things in bounds.  ODE lets dead avatars drift away (they should be removed!)
-                //if (vec.X < 0.0f) { vec.X = 0.0f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); }
-                //if (vec.Y < 0.0f) { vec.Y = 0.0f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); }
-                //if (vec.X > 255.95f) { vec.X = 255.95f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); }
-                //if (vec.Y > 255.95f) { vec.Y = 255.95f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); }
+                    PhysicsVector l_position = new PhysicsVector();
 
-                m_lastposition = _position;
 
-                l_position.X = vec.X;
-                l_position.Y = vec.Y;
-                l_position.Z = vec.Z;
+                    //  kluge to keep things in bounds.  ODE lets dead avatars drift away (they should be removed!)
+                    //if (vec.X < 0.0f) { vec.X = 0.0f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); }
+                    //if (vec.Y < 0.0f) { vec.Y = 0.0f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); }
+                    //if (vec.X > 255.95f) { vec.X = 255.95f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); }
+                    //if (vec.Y > 255.95f) { vec.Y = 255.95f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); }
 
-                if (l_position.X > 255.95f || l_position.X < 0f || l_position.Y > 255.95f || l_position.Y < 0f)
-                {
-                    base.RaiseOutOfBounds(_position);
-                }
+                    m_lastposition = _position;
+
+                    l_position.X = vec.X;
+                    l_position.Y = vec.Y;
+                    l_position.Z = vec.Z;
+
+                    if (l_position.X > 255.95f || l_position.X < 0f || l_position.Y > 255.95f || l_position.Y < 0f)
+                    {
+                        base.RaiseOutOfBounds(_position);
+                    }
                     //if (m_crossingfailures < 5)
                     //{
-                        //base.RequestPhysicsterseUpdate();
+                    //base.RequestPhysicsterseUpdate();
+                    //}
                     //}
-                //}
-
-                if (l_position.Z < 0)
-                {
-                    // This is so prim that get lost underground don't fall forever and suck up 
-                    // 
-                    // Sim resources and memory.
-                    // Disables the prim's movement physics....  
-                    // It's a hack and will generate a console message if it fails.
-
-
-                    //IsPhysical = false;
-                    base.RaiseOutOfBounds(_position);
-                    _velocity.X = 0;
-                    _velocity.Y = 0;
-                    _velocity.Z = 0;
-                    m_rotationalVelocity.X = 0;
-                    m_rotationalVelocity.Y = 0;
-                    m_rotationalVelocity.Z = 0;
-                    base.RequestPhysicsterseUpdate();
-                    m_throttleUpdates = false;
-                    throttleCounter = 0;
-                    _zeroFlag = true;
-                    //outofBounds = true;
-                }
-
-                if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02)
-                    && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02)
-                    && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02))
-                {
-                    _zeroFlag = true;
-                    m_throttleUpdates = false;
-                }
-                else
-                {
-                    //System.Console.WriteLine(Math.Abs(m_lastposition.X - l_position.X).ToString());
-                    _zeroFlag = false;
-                }
-
 
-                if (_zeroFlag)
-                {
-                    // Supposedly this is supposed to tell SceneObjectGroup that 
-                    // no more updates need to be sent..  
-                    // but it seems broken.
-                    _velocity.X = 0.0f;
-                    _velocity.Y = 0.0f;
-                    _velocity.Z = 0.0f;
-                    //_orientation.w = 0f;
-                    //_orientation.x = 0f;
-                    //_orientation.y = 0f;
-                    //_orientation.z = 0f;
-                    m_rotationalVelocity.X = 0;
-                    m_rotationalVelocity.Y = 0;
-                    m_rotationalVelocity.Z = 0;
-                    if (!m_lastUpdateSent)
+                    if (l_position.Z < 0)
                     {
+                        // This is so prim that get lost underground don't fall forever and suck up 
+                        // 
+                        // Sim resources and memory.
+                        // Disables the prim's movement physics....  
+                        // It's a hack and will generate a console message if it fails.
+
+
+                        //IsPhysical = false;
+                        base.RaiseOutOfBounds(_position);
+                        _velocity.X = 0;
+                        _velocity.Y = 0;
+                        _velocity.Z = 0;
+                        m_rotationalVelocity.X = 0;
+                        m_rotationalVelocity.Y = 0;
+                        m_rotationalVelocity.Z = 0;
+                        base.RequestPhysicsterseUpdate();
                         m_throttleUpdates = false;
                         throttleCounter = 0;
-                        m_rotationalVelocity = pv;
-                        base.RequestPhysicsterseUpdate();
-                        m_lastUpdateSent = true;
+                        _zeroFlag = true;
+                        //outofBounds = true;
                     }
-                }
-                else
-                {
-                    if (lastZeroFlag != _zeroFlag)
-                        base.RequestPhysicsterseUpdate();
-
-                    m_lastVelocity = _velocity;
 
-                    _position = l_position;
-
-                    _velocity.X = vel.X;
-                    _velocity.Y = vel.Y;
-                    _velocity.Z = vel.Z;
-                    if (_velocity.IsIdentical(pv, 0.5f))
+                    if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02)
+                        && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02)
+                        && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02))
                     {
-                        m_rotationalVelocity = pv;
+                        _zeroFlag = true;
+                        m_throttleUpdates = false;
                     }
                     else
                     {
-                        m_rotationalVelocity.setValues(rotvel.X, rotvel.Y, rotvel.Z);
+                        //System.Console.WriteLine(Math.Abs(m_lastposition.X - l_position.X).ToString());
+                        _zeroFlag = false;
                     }
-                    
-                    //System.Console.WriteLine("ODE: " + m_rotationalVelocity.ToString());
-                    _orientation.w = ori.W;
-                    _orientation.x = ori.X;
-                    _orientation.y = ori.Y;
-                    _orientation.z = ori.Z;
-                    m_lastUpdateSent = false;
-                    if (!m_throttleUpdates || throttleCounter > 15)
+
+
+                    if (_zeroFlag)
                     {
-                        
-                        base.RequestPhysicsterseUpdate();
+                        // Supposedly this is supposed to tell SceneObjectGroup that 
+                        // no more updates need to be sent..  
+                        // but it seems broken.
+                        _velocity.X = 0.0f;
+                        _velocity.Y = 0.0f;
+                        _velocity.Z = 0.0f;
+                        //_orientation.w = 0f;
+                        //_orientation.x = 0f;
+                        //_orientation.y = 0f;
+                        //_orientation.z = 0f;
+                        m_rotationalVelocity.X = 0;
+                        m_rotationalVelocity.Y = 0;
+                        m_rotationalVelocity.Z = 0;
+                        if (!m_lastUpdateSent)
+                        {
+                            m_throttleUpdates = false;
+                            throttleCounter = 0;
+                            m_rotationalVelocity = pv;
+                            base.RequestPhysicsterseUpdate();
+                            m_lastUpdateSent = true;
+                        }
                     }
                     else
                     {
-                        throttleCounter++;
+                        if (lastZeroFlag != _zeroFlag)
+                            base.RequestPhysicsterseUpdate();
+
+                        m_lastVelocity = _velocity;
+
+                        _position = l_position;
+
+                        _velocity.X = vel.X;
+                        _velocity.Y = vel.Y;
+                        _velocity.Z = vel.Z;
+                        if (_velocity.IsIdentical(pv, 0.5f))
+                        {
+                            m_rotationalVelocity = pv;
+                        }
+                        else
+                        {
+                            m_rotationalVelocity.setValues(rotvel.X, rotvel.Y, rotvel.Z);
+                        }
+
+                        //System.Console.WriteLine("ODE: " + m_rotationalVelocity.ToString());
+                        _orientation.w = ori.W;
+                        _orientation.x = ori.X;
+                        _orientation.y = ori.Y;
+                        _orientation.z = ori.Z;
+                        m_lastUpdateSent = false;
+                        if (!m_throttleUpdates || throttleCounter > 15)
+                        {
+
+                            base.RequestPhysicsterseUpdate();
+                        }
+                        else
+                        {
+                            throttleCounter++;
+                        }
                     }
+                    m_lastposition = l_position;
+                }
+                else
+                {
+                    // Not a body..   so Make sure the client isn't interpolating
+                    _velocity.X = 0;
+                    _velocity.Y = 0;
+                    _velocity.Z = 0;
+                    m_rotationalVelocity.X = 0;
+                    m_rotationalVelocity.Y = 0;
+                    m_rotationalVelocity.Z = 0;
+                    _zeroFlag = true;
                 }
-                m_lastposition = l_position;
-            }
-            else
-            {
-                // Not a body..   so Make sure the client isn't interpolating
-                _velocity.X = 0;
-                _velocity.Y = 0;
-                _velocity.Z = 0;
-                m_rotationalVelocity.X = 0;
-                m_rotationalVelocity.Y = 0;
-                m_rotationalVelocity.Z = 0;
-                _zeroFlag = true;
             }
         }
 
diff --git a/OpenSim/Region/Physics/POSPlugin/POSPlugin.cs b/OpenSim/Region/Physics/POSPlugin/POSPlugin.cs
index 9d70ac8..ac63a12 100644
--- a/OpenSim/Region/Physics/POSPlugin/POSPlugin.cs
+++ b/OpenSim/Region/Physics/POSPlugin/POSPlugin.cs
@@ -477,7 +477,15 @@ namespace OpenSim.Region.Physics.POSPlugin
             get { return true; }
             set { }
         }
+        public override void link(PhysicsActor obj)
+        {
+
+        }
 
+        public override void delink()
+        {
+
+        }
         public void SetAcceleration(PhysicsVector accel)
         {
             _acceleration = accel;
@@ -659,7 +667,15 @@ namespace OpenSim.Region.Physics.POSPlugin
         {
             set { return; }
         }
+        public override void link(PhysicsActor obj)
+        {
+
+        }
 
+        public override void delink()
+        {
+
+        }
         public override bool Selected
         {
             set { return; }
diff --git a/OpenSim/Region/Physics/PhysXPlugin/PhysXPlugin.cs b/OpenSim/Region/Physics/PhysXPlugin/PhysXPlugin.cs
index 83930c3..0c42260 100644
--- a/OpenSim/Region/Physics/PhysXPlugin/PhysXPlugin.cs
+++ b/OpenSim/Region/Physics/PhysXPlugin/PhysXPlugin.cs
@@ -366,6 +366,16 @@ namespace OpenSim.Region.Physics.PhysXPlugin
         {
         }
 
+        public override void link(PhysicsActor obj)
+        {
+
+        }
+
+        public override void delink()
+        {
+
+        }
+
         public override void SetMomentum(PhysicsVector momentum)
         {
         }
@@ -582,6 +592,16 @@ namespace OpenSim.Region.Physics.PhysXPlugin
             set { }
         }
 
+        public override void link(PhysicsActor obj)
+        {
+
+        }
+
+        public override void delink()
+        {
+
+        }
+
         public override float Mass
         {
             get { return 0f; }