* First pass at collidable linksets

* There will be bugs, you can count on that. To avoid them, set the linksets phantom
* After region restart, the linksets restore in a non collidable state.
* Linksets can but shouldn't be made physical with the physical checkbox or when you unlink them, they tend to explode.
* After creating a linkset, you have to move the linkset or set it phantom and not phantom for it to become collidable.   
* There's a few ParentGroup references that need to be refactored.

4 files changed, 154 insertions, 41 deletions

diff --git a/OpenSim/Region/Environment/Scenes/InnerScene.cs b/OpenSim/Region/Environment/Scenes/InnerScene.cs
index 8c0ba3c..0116428 100644
--- a/OpenSim/Region/Environment/Scenes/InnerScene.cs
+++ b/OpenSim/Region/Environment/Scenes/InnerScene.cs
@@ -170,6 +170,7 @@ namespace OpenSim.Region.Environment.Scenes
             foreach (SceneObjectPart part in sceneObject.Children.Values)
             {
                 part.LocalID = m_parentScene.PrimIDAllocate();
+                
             }
             sceneObject.UpdateParentIDs();
             AddEntity(sceneObject);
diff --git a/OpenSim/Region/Environment/Scenes/SceneObjectGroup.cs b/OpenSim/Region/Environment/Scenes/SceneObjectGroup.cs
index cd8b8e4..879e4a3 100644
--- a/OpenSim/Region/Environment/Scenes/SceneObjectGroup.cs
+++ b/OpenSim/Region/Environment/Scenes/SceneObjectGroup.cs
@@ -146,13 +146,13 @@ namespace OpenSim.Region.Environment.Scenes
                         part.GroupPosition = val;
                     }
                 }
-                if (m_rootPart.PhysActor != null)
-                {
-                    m_rootPart.PhysActor.Position =
-                        new PhysicsVector(m_rootPart.GroupPosition.X, m_rootPart.GroupPosition.Y,
-                                          m_rootPart.GroupPosition.Z);
-                    m_scene.PhysicsScene.AddPhysicsActorTaint(m_rootPart.PhysActor);
-                }
+                //if (m_rootPart.PhysActor != null)
+                //{
+                    //m_rootPart.PhysActor.Position =
+                        //new PhysicsVector(m_rootPart.GroupPosition.X, m_rootPart.GroupPosition.Y,
+                                          //m_rootPart.GroupPosition.Z);
+                    //m_scene.PhysicsScene.AddPhysicsActorTaint(m_rootPart.PhysActor);
+                //}
             }
         }
 
@@ -844,7 +844,7 @@ namespace OpenSim.Region.Environment.Scenes
             }
             return false;
         }
-
+        
         #endregion
 
         #region Packet Handlers
@@ -881,12 +881,12 @@ namespace OpenSim.Region.Environment.Scenes
             m_parts.Add(linkPart.UUID, linkPart);
             linkPart.SetParent(this);
 
-            if (linkPart.PhysActor != null)
-            {
-                m_scene.PhysicsScene.RemovePrim(linkPart.PhysActor);
+            //if (linkPart.PhysActor != null)
+            //{
+               // m_scene.PhysicsScene.RemovePrim(linkPart.PhysActor);
 
-                linkPart.PhysActor = null;
-            }
+                //linkPart.PhysActor = null;
+            //}
 
             //TODO: rest of parts
             foreach (SceneObjectPart part in objectGroup.Children.Values)
@@ -902,6 +902,7 @@ namespace OpenSim.Region.Environment.Scenes
             m_scene.DeleteEntity(objectGroup.UUID);
 
             objectGroup.DeleteParts();
+            AbsolutePosition = AbsolutePosition;
             ScheduleGroupForFullUpdate();
         }
 
@@ -918,7 +919,7 @@ namespace OpenSim.Region.Environment.Scenes
             {
                 // Remove the part from this object
                 m_parts.Remove(linkPart.UUID);
-
+                linkPart.ParentID = 0;
                 // We need to reset the child part's position 
                 // ready for life as a separate object after being a part of another object
                 Quaternion parentRot
@@ -951,19 +952,19 @@ namespace OpenSim.Region.Environment.Scenes
                 // Add physics information back to delinked part if appropriate
                 // XXX This is messy and should be refactorable with the similar section in
                 // SceneObjectPart.UpdatePrimFlags()
-                if (m_rootPart.PhysActor != null)
-                {
-                    linkPart.PhysActor = m_scene.PhysicsScene.AddPrimShape(
-                        linkPart.Name,
-                        linkPart.Shape,
-                        new PhysicsVector(linkPart.AbsolutePosition.X, linkPart.AbsolutePosition.Y,
-                                          linkPart.AbsolutePosition.Z),
-                        new PhysicsVector(linkPart.Scale.X, linkPart.Scale.Y, linkPart.Scale.Z),
-                        new Quaternion(linkPart.RotationOffset.W, linkPart.RotationOffset.X,
-                                       linkPart.RotationOffset.Y, linkPart.RotationOffset.Z),
-                        m_rootPart.PhysActor.IsPhysical);
-                    m_rootPart.DoPhysicsPropertyUpdate(m_rootPart.PhysActor.IsPhysical, true);
-                }
+                //if (m_rootPart.PhysActor != null)
+                //{
+                    //linkPart.PhysActor = m_scene.PhysicsScene.AddPrimShape(
+                        //linkPart.Name,
+                        //linkPart.Shape,
+                        //new PhysicsVector(linkPart.AbsolutePosition.X, linkPart.AbsolutePosition.Y,
+                                          //linkPart.AbsolutePosition.Z),
+                        //new PhysicsVector(linkPart.Scale.X, linkPart.Scale.Y, linkPart.Scale.Z),
+                        //new Quaternion(linkPart.RotationOffset.W, linkPart.RotationOffset.X,
+                                       //linkPart.RotationOffset.Y, linkPart.RotationOffset.Z),
+                        //m_rootPart.PhysActor.IsPhysical);
+                    //m_rootPart.DoPhysicsPropertyUpdate(m_rootPart.PhysActor.IsPhysical, true);
+                //}
 
                 SceneObjectGroup objectGroup = new SceneObjectGroup(m_scene, m_regionHandle, linkPart);
 
@@ -1139,7 +1140,18 @@ namespace OpenSim.Region.Environment.Scenes
             SceneObjectPart part = GetChildPart(localID);
             if (part != null)
             {
-                part.UpdatePrimFlags(type, inUse, data);
+                // If we have children
+                if (m_parts.Count > 1)
+                {
+                    foreach (SceneObjectPart parts in m_parts.Values)
+                    {
+                        parts.UpdatePrimFlags(type, inUse, data);
+                    }
+                }
+                else
+                {
+                    part.UpdatePrimFlags(type, inUse, data);
+                }
             }
         }
 
@@ -1639,7 +1651,17 @@ namespace OpenSim.Region.Environment.Scenes
 
         public void ApplyPhysics()
         {
-            m_rootPart.ApplyPhysics();
+            if (m_parts.Count > 1)
+            {
+                foreach (SceneObjectPart parts in m_parts.Values)
+                {
+                    parts.ApplyPhysics();
+                }
+            }
+            else
+            {
+                m_rootPart.ApplyPhysics();
+            }
         }
     }
 }
diff --git a/OpenSim/Region/Environment/Scenes/SceneObjectPart.cs b/OpenSim/Region/Environment/Scenes/SceneObjectPart.cs
index 5a02ad4..4456182 100644
--- a/OpenSim/Region/Environment/Scenes/SceneObjectPart.cs
+++ b/OpenSim/Region/Environment/Scenes/SceneObjectPart.cs
@@ -153,12 +153,65 @@ namespace OpenSim.Region.Environment.Scenes
         //unkown if this will be kept, added as a way of removing the group position from the group class
         protected LLVector3 m_groupPosition;
 
+        public LLVector3 GetWorldPosition()
+        {
+
+            Quaternion parentRot = new Quaternion(
+            ParentGroup.RootPart.RotationOffset.W,
+            ParentGroup.RootPart.RotationOffset.X,
+            ParentGroup.RootPart.RotationOffset.Y,
+            ParentGroup.RootPart.RotationOffset.Z);
+
+            Vector3 axPos
+                = new Vector3(
+                    OffsetPosition.X,
+                    OffsetPosition.Y,
+                    OffsetPosition.Z);
+
+            axPos = parentRot * axPos;
+            LLVector3 translationOffsetPosition = new LLVector3(axPos.x, axPos.y, axPos.z);
+            return GroupPosition + translationOffsetPosition;
+
+            //return (new LLVector3(axiomPos.x, axiomPos.y, axiomPos.z) + AbsolutePosition);
+        }
+
+        public LLQuaternion GetWorldRotation()
+        {
+            Quaternion newRot;
+            
+            if (this.LinkNum == 0)
+            {
+                newRot = new Quaternion(RotationOffset.W,RotationOffset.X,RotationOffset.Y,RotationOffset.Z);
+               
+            }
+            else
+            {
+                Quaternion parentRot = new Quaternion(
+                ParentGroup.RootPart.RotationOffset.W,
+                ParentGroup.RootPart.RotationOffset.X,
+                ParentGroup.RootPart.RotationOffset.Y,
+                ParentGroup.RootPart.RotationOffset.Z);
+
+                Quaternion oldRot
+                    = new Quaternion(
+                        RotationOffset.W,
+                        RotationOffset.X,
+                        RotationOffset.Y,
+                        RotationOffset.Z);
+
+                newRot = parentRot * oldRot;
+            }
+            return new LLQuaternion(newRot.x, newRot.y, newRot.z, newRot.w);
+
+            //return new LLQuaternion(axiomPartRotation.x, axiomPartRotation.y, axiomPartRotation.z, axiomPartRotation.w);
+
+        }
 
         public LLVector3 GroupPosition
         {
             get
             {
-                if (PhysActor != null)
+                if (PhysActor != null && ParentID == 0)
                 {
                     m_groupPosition.X = PhysActor.Position.X;
                     m_groupPosition.Y = PhysActor.Position.Y;
@@ -167,23 +220,35 @@ namespace OpenSim.Region.Environment.Scenes
                 return m_groupPosition;
             }
             set
-            {
+            {   
+                m_groupPosition = value;
+
                 if (PhysActor != null)
                 {
                     try
                     {
-                        //lock (m_parentGroup.Scene.SyncRoot)
-                        //{
-                        PhysActor.Position = new PhysicsVector(value.X, value.Y, value.Z);
+
+                        if (ParentID == 0)
+                        {
+                            PhysActor.Position = new PhysicsVector(value.X, value.Y, value.Z);
+
+                        }
+                        else
+                        {
+                            LLVector3 resultingposition = GetWorldPosition();
+                            PhysActor.Position = new PhysicsVector(resultingposition.X, resultingposition.Y, resultingposition.Z);
+                            LLQuaternion resultingrot = GetWorldRotation();
+                            PhysActor.Orientation = new Quaternion(resultingrot.W, resultingrot.X, resultingrot.Y, resultingrot.Z);
+                        }
+
                         m_parentGroup.Scene.PhysicsScene.AddPhysicsActorTaint(PhysActor);
-                        //}
                     }
                     catch (Exception e)
                     {
                         Console.WriteLine(e.Message);
                     }
                 }
-                m_groupPosition = value;
+               
             }
         }
 
@@ -192,7 +257,18 @@ namespace OpenSim.Region.Environment.Scenes
         public LLVector3 OffsetPosition
         {
             get { return m_offsetPosition; }
-            set { m_offsetPosition = value; }
+            set { m_offsetPosition = value;
+            try
+            {
+                // Hack to get the child prim to update positions in the physics engine
+                ParentGroup.AbsolutePosition = ParentGroup.AbsolutePosition;
+            }
+            catch (System.NullReferenceException)
+            {
+                // Ignore, and skip over.
+            }
+            //MainLog.Instance.Verbose("PART", "OFFSET:" + m_offsetPosition, ToString());
+            }
         }
 
         public LLVector3 AbsolutePosition
@@ -206,7 +282,7 @@ namespace OpenSim.Region.Environment.Scenes
         {
             get
             {
-                if (PhysActor != null)
+                if (PhysActor != null && ParentID == 0)
                 {
                     if (PhysActor.Orientation.x != 0 || PhysActor.Orientation.y != 0
                         || PhysActor.Orientation.z != 0 || PhysActor.Orientation.w != 0)
@@ -221,13 +297,25 @@ namespace OpenSim.Region.Environment.Scenes
             }
             set
             {
+                m_rotationOffset = value;
+
                 if (PhysActor != null)
                 {
                     try
                     {
                         //lock (Scene.SyncRoot)
                         //{
-                        PhysActor.Orientation = new Quaternion(value.W, value.X, value.Y, value.Z);
+                        if (ParentID == 0)
+                        {
+                            PhysActor.Orientation = new Quaternion(value.W, value.X, value.Y, value.Z);
+                            //MainLog.Instance.Verbose("PART", "RO1:" + PhysActor.Orientation.ToString());
+                        }
+                        else
+                        {
+                            LLQuaternion resultingrotation = GetWorldRotation();
+                            PhysActor.Orientation = new Quaternion(resultingrotation.W, resultingrotation.X, resultingrotation.Y, resultingrotation.Z);
+                            //MainLog.Instance.Verbose("PART", "RO2:" + PhysActor.Orientation.ToString());
+                        }
                         m_parentGroup.Scene.PhysicsScene.AddPhysicsActorTaint(PhysActor);
                         //}
                     }
@@ -236,7 +324,7 @@ namespace OpenSim.Region.Environment.Scenes
                         Console.WriteLine(ex.Message);
                     }
                 }
-                m_rotationOffset = value;
+                
             }
         }
 
diff --git a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
index 5fef47d..6d08f98 100644
--- a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
+++ b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
@@ -587,7 +587,9 @@ namespace OpenSim.Region.Physics.OdePlugin
         {
             get { return _position; }
 
-            set { _position = value; }
+            set { _position = value; 
+                //OpenSim.Framework.Console.MainLog.Instance.Verbose("PHYSICS", _position.ToString());
+            }
         }
 
         public override PhysicsVector Size