* 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.

2 files changed, 209 insertions, 120 deletions

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;
             }
         }