* Tweaks physics so that linked prim are a single body. This will make linked prim more stable and probably the last obstacle to vehicles physics wise.

* Fixed a bug that caused physics proxies to be scattered when you link an object.
* Single physical prim work exactly the same as before, just linked physical prim will have changed.

3 files changed, 687 insertions, 83 deletions

diff --git a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
index 79a7519..0a461e9 100644
--- a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
+++ b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
@@ -24,7 +24,6 @@
  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  */
-
 using System;
 using System.Collections.Generic;
 using System.Reflection;
@@ -118,6 +117,8 @@ namespace OpenSim.Region.Physics.OdePlugin
         private PhysicsActor _parent = null;
         private PhysicsActor m_taintparent = null;
 
+        private List<OdePrim> childrenPrim = new List<OdePrim>();
+
         private bool iscolliding = false;
         private bool m_isphysical = false;
         private bool m_isSelected = false;
@@ -147,6 +148,8 @@ namespace OpenSim.Region.Physics.OdePlugin
 
         private IntPtr m_linkJoint = (IntPtr)0;
 
+        public volatile bool childPrim = false;
+
         public OdePrim(String primName, OdeScene parent_scene, PhysicsVector pos, PhysicsVector size,
                        Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical, CollisionLocker dode)
         {
@@ -161,10 +164,10 @@ namespace OpenSim.Region.Physics.OdePlugin
             m_density = parent_scene.geomDefaultDensity;
             m_tensor = parent_scene.bodyMotorJointMaxforceTensor;
             body_autodisable_frames = parent_scene.bodyFramesAutoDisable;
-            
 
-            prim_geom = (IntPtr)0;
-            prev_geom = (IntPtr)0;
+
+            prim_geom = IntPtr.Zero;
+            prev_geom = IntPtr.Zero;
 
             if (size.X <= 0) size.X = 0.01f;
             if (size.Y <= 0) size.Y = 0.01f;
@@ -247,64 +250,83 @@ namespace OpenSim.Region.Physics.OdePlugin
         {
             prev_geom = prim_geom;
             prim_geom = geom;
-            if (prim_geom != (IntPtr)0)
+            if (prim_geom != IntPtr.Zero)
             {
                 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
                 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
             }
+
+            if (childPrim)
+            {
+                if (_parent != null && _parent is OdePrim)
+                {
+                    OdePrim parent = (OdePrim)_parent;
+                    parent.ChildSetGeom(this);
+                }
+            }
             //m_log.Warn("Setting Geom to: " + prim_geom);
         }
 
+        
+
         public void enableBodySoft()
         {
-            if (m_isphysical && Body != (IntPtr)0)
-                d.BodyEnable(Body);
+            if (!childPrim)
+            {
+                if (m_isphysical && Body != IntPtr.Zero)
+                    d.BodyEnable(Body);
 
-            m_disabled = false;
+                m_disabled = false;
+            }
         }
 
         public void disableBodySoft()
         {
             m_disabled = true;
 
-            if (m_isphysical && Body != (IntPtr)0)
+            if (m_isphysical && Body != IntPtr.Zero)
                 d.BodyDisable(Body);
         }
 
         public void enableBody()
         {
-            // Sets the geom to a body
-            Body = d.BodyCreate(_parent_scene.world);
+            // Don't enable this body if we're a child prim
+            // this should be taken care of in the parent function not here
+            if (!childPrim)
+            {
+                // Sets the geom to a body
+                Body = d.BodyCreate(_parent_scene.world);
 
-            setMass();
-            d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
-            d.Quaternion myrot = new d.Quaternion();
-            myrot.X = _orientation.X;
-            myrot.Y = _orientation.Y;
-            myrot.Z = _orientation.Z;
-            myrot.W = _orientation.W;
-            d.BodySetQuaternion(Body, ref myrot);
-            d.GeomSetBody(prim_geom, Body);
-            m_collisionCategories |= CollisionCategories.Body;
-            m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
+                setMass();
+                d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
+                d.Quaternion myrot = new d.Quaternion();
+                myrot.X = _orientation.X;
+                myrot.Y = _orientation.Y;
+                myrot.Z = _orientation.Z;
+                myrot.W = _orientation.W;
+                d.BodySetQuaternion(Body, ref myrot);
+                d.GeomSetBody(prim_geom, Body);
+                m_collisionCategories |= CollisionCategories.Body;
+                m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
 
-            d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
-            d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
+                d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
+                d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
 
-            d.BodySetAutoDisableFlag(Body, true);
-            d.BodySetAutoDisableSteps(Body, body_autodisable_frames);
+                d.BodySetAutoDisableFlag(Body, true);
+                d.BodySetAutoDisableSteps(Body, body_autodisable_frames);
 
-            m_interpenetrationcount = 0;
-            m_collisionscore = 0;
-            m_disabled = false;
+                m_interpenetrationcount = 0;
+                m_collisionscore = 0;
+                m_disabled = false;
 
-            // The body doesn't already have a finite rotation mode set here
-            if ((!m_angularlock.IsIdentical(PhysicsVector.Zero, 0)) && _parent == null)
-            {
-                createAMotor(m_angularlock);
-            }
+                // The body doesn't already have a finite rotation mode set here
+                if ((!m_angularlock.IsIdentical(PhysicsVector.Zero, 0)) && _parent == null)
+                {
+                    createAMotor(m_angularlock);
+                }
 
-            _parent_scene.addActivePrim(this);
+                _parent_scene.addActivePrim(this);
+            }
         }
 
         #region Mass Calculation
@@ -627,20 +649,41 @@ namespace OpenSim.Region.Physics.OdePlugin
             //this kills the body so things like 'mesh' can re-create it.
             lock (this)
             {
-                if (Body != (IntPtr)0)
+                if (!childPrim)
+                {
+                    if (Body != IntPtr.Zero)
+                    {
+                        _parent_scene.remActivePrim(this);
+                        
+                        m_collisionCategories &= ~CollisionCategories.Body;
+                        m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land);
+
+                        if (prim_geom != IntPtr.Zero)
+                        {
+                            d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
+                            d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
+                        }
+
+                        
+                        d.BodyDestroy(Body);
+                        Body = IntPtr.Zero;
+                    }
+                }
+                else
                 {
+                    _parent_scene.remActivePrim(this);
+                    
                     m_collisionCategories &= ~CollisionCategories.Body;
                     m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land);
 
-                    if (prim_geom != (IntPtr)0)
+                    if (prim_geom != IntPtr.Zero)
                     {
                         d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
                         d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
                     }
 
-                    _parent_scene.remActivePrim(this);
-                    d.BodyDestroy(Body);
-                    Body = (IntPtr)0;
+                    
+                    Body = IntPtr.Zero;
                 }
             }
             m_disabled = true;
@@ -655,9 +698,20 @@ namespace OpenSim.Region.Physics.OdePlugin
             Thread.Sleep(10);
 
             //Kill Body so that mesh can re-make the geom
-            if (IsPhysical && Body != (IntPtr) 0)
+            if (IsPhysical && Body != IntPtr.Zero)
             {
-                disableBody();
+                if (childPrim)
+                {
+                    if (_parent != null)
+                    {
+                        OdePrim parent = (OdePrim)_parent;
+                        parent.ChildDelink(this);
+                    }
+                }
+                else
+                {
+                    disableBody();
+                }
             }
 
             IMesh oldMesh = primMesh;
@@ -682,7 +736,7 @@ namespace OpenSim.Region.Physics.OdePlugin
 
             try
             {
-                if (prim_geom == (IntPtr)0)
+                if (prim_geom == IntPtr.Zero)
                 {
                     SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, parent_scene.triCallback, null, null));
                 }
@@ -715,7 +769,7 @@ namespace OpenSim.Region.Physics.OdePlugin
             {
                 changeadd(timestep);
             }
-            if (prim_geom != (IntPtr)0)
+            if (prim_geom != IntPtr.Zero)
             {
                 if (!_position.IsIdentical(m_taintposition,0f))
                     changemove(timestep);
@@ -724,7 +778,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                     rotate(timestep);
                 //
 
-                if (m_taintPhysics != m_isphysical)
+                if (m_taintPhysics != m_isphysical && !(m_taintparent != _parent))
                     changePhysicsStatus(timestep);
                 //
 
@@ -783,7 +837,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                         if (Amotor != IntPtr.Zero)
                         {
                             d.JointDestroy(Amotor);
-                            Amotor = (IntPtr)0;
+                            Amotor = IntPtr.Zero;
                         }
                     }
                 }
@@ -794,11 +848,18 @@ namespace OpenSim.Region.Physics.OdePlugin
 
         private void changelink(float timestep)
         {
+            // If the newly set parent is not null
+            // create link
             if (_parent == null && m_taintparent != null)
             {
                 if (m_taintparent.PhysicsActorType == (int)ActorTypes.Prim)
                 {
                     OdePrim obj = (OdePrim)m_taintparent;
+                    //obj.disableBody();
+
+                    obj.ParentPrim(this);
+
+                    /*
                     if (obj.Body != (IntPtr)0 && Body != (IntPtr)0 && obj.Body != Body)
                     {
                         _linkJointGroup = d.JointGroupCreate(0);
@@ -806,18 +867,245 @@ namespace OpenSim.Region.Physics.OdePlugin
                         d.JointAttach(m_linkJoint, obj.Body, Body);
                         d.JointSetFixed(m_linkJoint);
                     }
+                     */
                 }
             }
+            // If the newly set parent is null
+            // destroy link
             else if (_parent != null && m_taintparent == null)
             {
-                if (Body != (IntPtr)0 && _linkJointGroup != (IntPtr)0)
+                if (_parent is OdePrim)
+                {
+                    OdePrim obj = (OdePrim)_parent;
+                    obj.ChildDelink(this);
+                    childPrim = false;
+                    //_parent = null;
+                }
+                
+                /*
+                    if (Body != (IntPtr)0 && _linkJointGroup != (IntPtr)0)
                     d.JointGroupDestroy(_linkJointGroup);
-
-                _linkJointGroup = (IntPtr)0;
-                m_linkJoint = (IntPtr)0;
+                        
+                    _linkJointGroup = (IntPtr)0;
+                    m_linkJoint = (IntPtr)0;
+                */
             }
 
             _parent = m_taintparent;
+            m_taintPhysics = m_isphysical;
+        }
+
+        // I'm the parent
+        // prim is the child
+        public void ParentPrim(OdePrim prim)
+        {
+            if (this.m_localID != prim.m_localID)
+            {
+                if (Body == IntPtr.Zero)
+                {
+                    Body = d.BodyCreate(_parent_scene.world);
+                }
+                if (Body != IntPtr.Zero)
+                {
+                    lock (childrenPrim)
+                    {
+                        if (!childrenPrim.Contains(prim))
+                        {
+                            childrenPrim.Add(prim);
+                            
+                            foreach (OdePrim prm in childrenPrim)
+                            {
+                                d.Mass m2;
+                                d.MassSetZero(out m2);
+                                d.MassSetBoxTotal(out m2, prim.CalculateMass(), prm._size.X, prm._size.Y, prm._size.Z);
+
+
+                                d.Quaternion quat = new d.Quaternion();
+                                quat.W = prm._orientation.W;
+                                quat.X = prm._orientation.X;
+                                quat.Y = prm._orientation.Y;
+                                quat.Z = prm._orientation.Z;
+
+                                d.Matrix3 mat = new d.Matrix3();
+                                d.RfromQ(out mat, ref quat);
+                                d.MassRotate(out m2, ref mat);
+                                d.MassTranslate(out m2, Position.X - prm.Position.X, Position.Y - prm.Position.Y, Position.Z - prm.Position.Z);
+                                d.MassAdd(ref pMass, ref m2);
+                            }
+                            foreach (OdePrim prm in childrenPrim)
+                            {
+                                prm.m_collisionCategories |= CollisionCategories.Body;
+                                prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
+
+                                d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories);
+                                d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags);
+
+
+                                d.Quaternion quat = new d.Quaternion();
+                                quat.W = prm._orientation.W;
+                                quat.X = prm._orientation.X;
+                                quat.Y = prm._orientation.Y;
+                                quat.Z = prm._orientation.Z;
+
+                                d.Matrix3 mat = new d.Matrix3();
+                                d.RfromQ(out mat, ref quat);
+                                if (Body != IntPtr.Zero)
+                                {
+                                    d.GeomSetBody(prm.prim_geom, Body);
+                                    prm.childPrim = true;
+                                    d.GeomSetOffsetWorldPosition(prm.prim_geom, prm.Position.X , prm.Position.Y, prm.Position.Z);
+                                    //d.GeomSetOffsetPosition(prim.prim_geom,
+                                    //    (Position.X - prm.Position.X) - pMass.c.X,
+                                    //    (Position.Y - prm.Position.Y) - pMass.c.Y,
+                                    //    (Position.Z - prm.Position.Z) - pMass.c.Z);
+                                    d.GeomSetOffsetWorldRotation(prm.prim_geom, ref mat);
+                                    //d.GeomSetOffsetRotation(prm.prim_geom, ref mat);
+                                    d.MassTranslate(out pMass, -pMass.c.X, -pMass.c.Y, -pMass.c.Z);
+                                    d.BodySetMass(Body, ref pMass);
+                                }
+                                else
+                                {
+                                    m_log.Debug("[PHYSICS]:I ain't got no boooooooooddy, no body");
+                                }
+
+
+                                prm.m_interpenetrationcount = 0;
+                                prm.m_collisionscore = 0;
+                                prm.m_disabled = false;
+
+                                // The body doesn't already have a finite rotation mode set here
+                                if ((!m_angularlock.IsIdentical(PhysicsVector.Zero, 0)) && _parent == null)
+                                {
+                                    prm.createAMotor(m_angularlock);
+                                }
+                                prm.Body = Body;
+                                _parent_scene.addActivePrim(prm);
+                            }
+
+                            m_collisionCategories |= CollisionCategories.Body;
+                            m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
+
+                            d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
+                            d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
+
+
+                            d.Quaternion quat2 = new d.Quaternion();
+                            quat2.W = _orientation.W;
+                            quat2.X = _orientation.X;
+                            quat2.Y = _orientation.Y;
+                            quat2.Z = _orientation.Z;
+
+                            d.Matrix3 mat2 = new d.Matrix3();
+                            d.RfromQ(out mat2, ref quat2);
+                            d.GeomSetBody(prim_geom, Body);
+                            d.GeomSetOffsetWorldPosition(prim_geom, Position.X - pMass.c.X, Position.Y - pMass.c.Y, Position.Z - pMass.c.Z);
+                            //d.GeomSetOffsetPosition(prim.prim_geom,
+                            //    (Position.X - prm.Position.X) - pMass.c.X,
+                            //    (Position.Y - prm.Position.Y) - pMass.c.Y,
+                            //    (Position.Z - prm.Position.Z) - pMass.c.Z);
+                            //d.GeomSetOffsetRotation(prim_geom, ref mat2);
+                            d.MassTranslate(out pMass, -pMass.c.X, -pMass.c.Y, -pMass.c.Z);
+                            d.BodySetMass(Body, ref pMass);
+                            
+                            d.BodySetAutoDisableFlag(Body, true);
+                            d.BodySetAutoDisableSteps(Body, body_autodisable_frames);
+
+
+                            m_interpenetrationcount = 0;
+                            m_collisionscore = 0;
+                            m_disabled = false;
+
+                            // The body doesn't already have a finite rotation mode set here
+                            if ((!m_angularlock.IsIdentical(PhysicsVector.Zero, 0)) && _parent == null)
+                            {
+                                createAMotor(m_angularlock);
+                            }
+                            d.BodySetPosition(Body, Position.X, Position.Y, Position.Z);
+
+                            _parent_scene.addActivePrim(this);
+                        }
+                    }
+                }
+            }
+
+        }
+
+        private void ChildSetGeom(OdePrim odePrim)
+        {
+            //if (m_isphysical && Body != IntPtr.Zero)
+            lock (childrenPrim)
+            {
+                foreach (OdePrim prm in childrenPrim)
+                {
+                    //prm.childPrim = true;
+                    prm.disableBody();
+                    //prm.m_taintparent = null;
+                    //prm._parent = null;
+                    //prm.m_taintPhysics = false;
+                    //prm.m_disabled = true;
+                    //prm.childPrim = false;
+                }
+            }
+            disableBody();
+
+
+            if (Body != IntPtr.Zero)
+            {
+                _parent_scene.remActivePrim(this);
+            }
+
+            lock (childrenPrim)
+            {
+                foreach (OdePrim prm in childrenPrim)
+                {
+                    ParentPrim(prm);
+                }
+            }
+            
+        }
+
+        private void ChildDelink(OdePrim odePrim)
+        {
+            // Okay, we have a delinked child..   need to rebuild the body.
+            lock (childrenPrim)
+            {
+                foreach (OdePrim prm in childrenPrim)
+                {
+                    prm.childPrim = true;
+                    prm.disableBody();
+                    //prm.m_taintparent = null;
+                    //prm._parent = null;
+                    //prm.m_taintPhysics = false;
+                    //prm.m_disabled = true;
+                    //prm.childPrim = false;
+                }
+            }
+            disableBody();
+
+            lock (childrenPrim)
+            {
+                childrenPrim.Remove(odePrim);
+            }
+            
+            
+            
+
+            if (Body != IntPtr.Zero)
+            {
+                _parent_scene.remActivePrim(this);
+            }
+
+            
+
+            lock (childrenPrim)
+            {
+                foreach (OdePrim prm in childrenPrim)
+                {
+                    ParentPrim(prm);
+                }
+            }
+
+           
         }
 
         private void changeSelectedStatus(float timestep)
@@ -837,7 +1125,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                     disableBodySoft();
                 }
 
-                if (prim_geom != (IntPtr)0)
+                if (prim_geom != IntPtr.Zero)
                 {
                     d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
                     d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
@@ -862,7 +1150,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                 if (m_collidesWater)
                     m_collisionFlags |= CollisionCategories.Water;
 
-                if (prim_geom != (IntPtr)0)
+                if (prim_geom != IntPtr.Zero)
                 {
                     d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
                     d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
@@ -993,7 +1281,7 @@ namespace OpenSim.Region.Physics.OdePlugin
             {
                 CreateGeom(m_targetSpace, _mesh);
 
-                if (prim_geom != (IntPtr) 0)
+                if (prim_geom != IntPtr.Zero)
                 {
                     d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
                     d.Quaternion myrot = new d.Quaternion();
@@ -1004,7 +1292,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                     d.GeomSetQuaternion(prim_geom, ref myrot);
                 }
 
-                if (m_isphysical && Body == (IntPtr)0)
+                if (m_isphysical && Body == IntPtr.Zero)
                 {
                     enableBody();
                 }
@@ -1023,16 +1311,16 @@ namespace OpenSim.Region.Physics.OdePlugin
             if (m_isphysical)
             {
                 // This is a fallback..   May no longer be necessary.
-                if (Body == (IntPtr) 0)
+                if (Body == IntPtr.Zero)
                     enableBody();
                 //Prim auto disable after 20 frames,
                 //if you move it, re-enable the prim manually.
                 if (_parent != null)
                 {
-                    if (m_linkJoint != (IntPtr)0)
+                    if (m_linkJoint != IntPtr.Zero)
                     {
                         d.JointDestroy(m_linkJoint);
-                        m_linkJoint = (IntPtr)0;
+                        m_linkJoint = IntPtr.Zero;
                     }
                 }
                 d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
@@ -1058,7 +1346,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                 m_targetSpace = tempspace;
 
                 _parent_scene.waitForSpaceUnlock(m_targetSpace);
-                if (prim_geom != (IntPtr) 0)
+                if (prim_geom != IntPtr.Zero)
                 {
                     d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
 
@@ -1079,7 +1367,7 @@ namespace OpenSim.Region.Physics.OdePlugin
             float fy = 0;
             float fz = 0;
 
-            if (IsPhysical && Body != (IntPtr)0 && !m_isSelected)
+            if (IsPhysical && Body != IntPtr.Zero && !m_isSelected)
             {
                 //float PID_P = 900.0f;
 
@@ -1201,7 +1489,7 @@ namespace OpenSim.Region.Physics.OdePlugin
             myrot.Z = _orientation.Z;
             myrot.W = _orientation.W;
             d.GeomSetQuaternion(prim_geom, ref myrot);
-            if (m_isphysical && Body != (IntPtr) 0)
+            if (m_isphysical && Body != IntPtr.Zero)
             {
                 d.BodySetQuaternion(Body, ref myrot);
                 if (!m_angularlock.IsIdentical(new PhysicsVector(1, 1, 1), 0))
@@ -1222,10 +1510,10 @@ namespace OpenSim.Region.Physics.OdePlugin
         public void changedisable(float timestep)
         {
             m_disabled = true;
-            if (Body != (IntPtr)0)
+            if (Body != IntPtr.Zero)
             {
                 d.BodyDisable(Body);
-                Body = (IntPtr)0;
+                Body = IntPtr.Zero;
             }
 
             m_taintdisable = false;
@@ -1235,7 +1523,7 @@ namespace OpenSim.Region.Physics.OdePlugin
         {
             if (m_isphysical == true)
             {
-                if (Body == (IntPtr)0)
+                if (Body == IntPtr.Zero)
                 {
                     if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim)
                     {
@@ -1249,7 +1537,7 @@ namespace OpenSim.Region.Physics.OdePlugin
             }
             else
             {
-                if (Body != (IntPtr)0)
+                if (Body != IntPtr.Zero)
                 {
                     if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim)
                     {
@@ -1272,7 +1560,18 @@ namespace OpenSim.Region.Physics.OdePlugin
 
                         changeadd(2f);
                     }
-                    disableBody();
+                    if (childPrim)
+                    {
+                        if (_parent != null)
+                        {
+                            OdePrim parent = (OdePrim)_parent;
+                            parent.ChildDelink(this);
+                        }
+                    }
+                    else
+                    {
+                        disableBody();
+                    }
                 }
             }
 
@@ -1301,9 +1600,20 @@ namespace OpenSim.Region.Physics.OdePlugin
                 // Cleanup meshing here
             }
             //kill body to rebuild
-            if (IsPhysical && Body != (IntPtr)0)
+            if (IsPhysical && Body != IntPtr.Zero)
             {
-                disableBody();
+                if (childPrim)
+                {
+                    if (_parent != null)
+                    {
+                        OdePrim parent = (OdePrim)_parent;
+                        parent.ChildDelink(this);
+                    }
+                }
+                else
+                {
+                    disableBody();
+                }
             }
             if (d.SpaceQuery(m_targetSpace, prim_geom))
             {
@@ -1311,7 +1621,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                 d.SpaceRemove(m_targetSpace, prim_geom);
             }
             d.GeomDestroy(prim_geom);
-            prim_geom = (IntPtr)0;
+            prim_geom = IntPtr.Zero;
             // we don't need to do space calculation because the client sends a position update also.
 
             // Construction of new prim
@@ -1349,7 +1659,7 @@ namespace OpenSim.Region.Physics.OdePlugin
             d.GeomSetQuaternion(prim_geom, ref myrot);
 
             //d.GeomBoxSetLengths(prim_geom, _size.X, _size.Y, _size.Z);
-            if (IsPhysical && Body == (IntPtr)0)
+            if (IsPhysical && Body == IntPtr.Zero && !childPrim)
             {
                 // Re creates body on size.
                 // EnableBody also does setMass()
@@ -1360,7 +1670,14 @@ namespace OpenSim.Region.Physics.OdePlugin
             _parent_scene.geom_name_map[prim_geom] = oldname;
 
             changeSelectedStatus(timestamp);
-
+            if (childPrim)
+            {
+                if (_parent is OdePrim)
+                {
+                    OdePrim parent = (OdePrim)_parent;
+                    parent.ChildSetGeom(this);
+                }
+            }
             resetCollisionAccounting();
             m_taintsize = _size;
         }
@@ -1541,7 +1858,7 @@ namespace OpenSim.Region.Physics.OdePlugin
         {
             m_collidesWater = m_taintCollidesWater;
 
-            if (prim_geom != (IntPtr)0)
+            if (prim_geom != IntPtr.Zero)
             {
                 if (m_collidesWater)
                 {
@@ -1560,9 +1877,20 @@ namespace OpenSim.Region.Physics.OdePlugin
             string oldname = _parent_scene.geom_name_map[prim_geom];
 
             // Cleanup of old prim geometry and Bodies
-            if (IsPhysical && Body != (IntPtr) 0)
+            if (IsPhysical && Body != IntPtr.Zero)
             {
-                disableBody();
+                if (childPrim)
+                {
+                    if (_parent != null)
+                    {
+                        OdePrim parent = (OdePrim)_parent;
+                        parent.ChildDelink(this);
+                    }
+                }
+                else
+                {
+                    disableBody();
+                }
             }
             try
             {
@@ -1573,7 +1901,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                 prim_geom = IntPtr.Zero;
                 m_log.Error("[PHYSICS]: PrimGeom dead");
             }
-            prim_geom = (IntPtr) 0;
+            prim_geom = IntPtr.Zero;
             // we don't need to do space calculation because the client sends a position update also.
             if (_size.X <= 0) _size.X = 0.01f;
             if (_size.Y <= 0) _size.Y = 0.01f;
@@ -1608,7 +1936,7 @@ namespace OpenSim.Region.Physics.OdePlugin
             d.GeomSetQuaternion(prim_geom, ref myrot);
 
             //d.GeomBoxSetLengths(prim_geom, _size.X, _size.Y, _size.Z);
-            if (IsPhysical && Body == (IntPtr)0)
+            if (IsPhysical && Body == IntPtr.Zero)
             {
                 // Re creates body on size.
                 // EnableBody also does setMass()
@@ -1618,7 +1946,14 @@ namespace OpenSim.Region.Physics.OdePlugin
             _parent_scene.geom_name_map[prim_geom] = oldname;
 
             changeSelectedStatus(timestamp);
-
+            if (childPrim)
+            {
+                if (_parent is OdePrim)
+                {
+                    OdePrim parent = (OdePrim)_parent;
+                    parent.ChildSetGeom(this);
+                }
+            }
             resetCollisionAccounting();
             m_taintshape = false;
         }
@@ -1658,7 +1993,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                 Thread.Sleep(20);
                 if (IsPhysical)
                 {
-                    if (Body != (IntPtr)0)
+                    if (Body != IntPtr.Zero)
                     {
                         d.BodySetLinearVel(Body, m_taintVelocity.X, m_taintVelocity.Y, m_taintVelocity.Z);
                     }
diff --git a/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs
index 0dab05c..a50c5ae 100644
--- a/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs
+++ b/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs
@@ -24,6 +24,7 @@
  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  */
+//#define USE_DRAWSTUFF
 
 using System;
 using System.Collections.Generic;
@@ -34,6 +35,9 @@ using System.IO;
 using log4net;
 using Nini.Config;
 using Ode.NET;
+#if USE_DRAWSTUFF
+using Drawstuff.NET;
+#endif 
 using OpenSim.Framework;
 using OpenSim.Region.Physics.Manager;
 using OpenMetaverse;
@@ -257,6 +261,9 @@ namespace OpenSim.Region.Physics.OdePlugin
         public int physics_logging_interval = 0;
         public bool physics_logging_append_existing_logfile = false;
 
+        public d.Vector3 xyz = new d.Vector3(2.1640f, -1.3079f, 1.7600f);
+        public d.Vector3 hpr = new d.Vector3(125.5000f, -17.0000f, 0.0000f);
+
         /// <summary>
         /// Initiailizes the scene
         /// Sets many properties that ODE requires to be stable
@@ -280,6 +287,11 @@ namespace OpenSim.Region.Physics.OdePlugin
                 //contactgroup
 
                 d.WorldSetAutoDisableFlag(world, false);
+                #if USE_DRAWSTUFF
+                
+                Thread viewthread = new Thread(new ParameterizedThreadStart(startvisualization));
+                viewthread.Start();
+                #endif
             }
 
             // zero out a heightmap array float array (single dimention [flattened]))
@@ -290,6 +302,21 @@ namespace OpenSim.Region.Physics.OdePlugin
             // we can hit test less.
         }
 
+#if USE_DRAWSTUFF
+        public void startvisualization(object o)
+        {
+            ds.Functions fn;
+            fn.version = ds.VERSION;
+            fn.start = new ds.CallbackFunction(start);
+            fn.step = new ds.CallbackFunction(step);
+            fn.command = new ds.CallbackFunction(command);
+            fn.stop = null;
+            fn.path_to_textures = "./textures";
+            string[] args = new string[0];
+            ds.SimulationLoop(args.Length, args, 352, 288, ref fn);
+        }
+#endif
+
         // Initialize the mesh plugin
         public override void Initialise(IMesher meshmerizer, IConfigSource config)
         {
@@ -787,6 +814,10 @@ namespace OpenSim.Region.Physics.OdePlugin
                             _perloopContact.Add(contacts[i]);
                             joint = d.JointCreateContact(world, contactgroup, ref TerrainContact);
                         }
+                        //if (p2.PhysicsActorType == (int)ActorTypes.Prim)
+                        //{
+                            //m_log.Debug("[PHYSICS]: prim contacting with ground");
+                        //}
                     }
                     else if (name1 == "Water" || name2 == "Water")
                     {
@@ -1137,7 +1168,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                 List<OdePrim> removeprims = null;
                 foreach (OdePrim chr in _activeprims)
                 {
-                    if (d.BodyIsEnabled(chr.Body) && (!chr.m_disabled))
+                    if (chr.Body != IntPtr.Zero && d.BodyIsEnabled(chr.Body) && (!chr.m_disabled))
                     {
                         try
                         {
@@ -1243,7 +1274,8 @@ namespace OpenSim.Region.Physics.OdePlugin
             {
                 newPrim = new OdePrim(name, this, pos, siz, rot, mesh, pbs, isphysical, ode);
 
-                _prims.Add(newPrim);
+                lock (_prims)
+                    _prims.Add(newPrim);
             }
 
             return newPrim;
@@ -1252,8 +1284,13 @@ namespace OpenSim.Region.Physics.OdePlugin
         public void addActivePrim(OdePrim activatePrim)
         {
             // adds active prim..   (ones that should be iterated over in collisions_optimized
-
-            _activeprims.Add(activatePrim);
+            lock (_activeprims)
+            {
+                if (!_activeprims.Contains(activatePrim))
+                    _activeprims.Add(activatePrim);
+                //else
+                  //  m_log.Warn("[PHYSICS]: Double Entry in _activeprims detected, potential crash immenent");
+            }
         }
 
         public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, PhysicsVector position,
@@ -1334,6 +1371,15 @@ namespace OpenSim.Region.Physics.OdePlugin
                         if (prim.IsPhysical)
                         {
                             prim.disableBody();
+                            if (prim.childPrim)
+                            {
+                                prim.childPrim = false;
+                                prim.Body = IntPtr.Zero;
+                                prim.m_disabled = true;
+                                prim.IsPhysical = false;
+                            }
+
+
                         }
                         // we don't want to remove the main space
 
@@ -1376,6 +1422,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                         {
                             m_log.Info("[PHYSICS]: Couldn't remove prim from physics scene, it was already be removed.");
                         }
+                        lock (_prims)
                         _prims.Remove(prim);
 
                         //If there are no more geometries in the sub-space, we don't need it in the main space anymore
@@ -2376,9 +2423,12 @@ namespace OpenSim.Region.Physics.OdePlugin
         {
             lock (OdeLock)
             {
-                foreach (OdePrim prm in _prims)
+                lock (_prims)
                 {
-                    RemovePrim(prm);
+                    foreach (OdePrim prm in _prims)
+                    {
+                        RemovePrim(prm);
+                    }
                 }
 
                 //foreach (OdeCharacter act in _characters)
@@ -2411,5 +2461,126 @@ namespace OpenSim.Region.Physics.OdePlugin
             }
             return returncolliders;
         }
+#if USE_DRAWSTUFF
+        // Keyboard callback
+        public void command(int cmd)
+        {
+            IntPtr geom;
+            d.Mass mass;
+            d.Vector3 sides = new d.Vector3(d.RandReal() * 0.5f + 0.1f, d.RandReal() * 0.5f + 0.1f, d.RandReal() * 0.5f + 0.1f);
+
+            
+
+            Char ch = Char.ToLower((Char)cmd);
+            switch ((Char)ch)
+            {
+                case 'w':
+                    Vector3 rotate = (new Vector3(1, 0, 0) * Quaternion.CreateFromEulers(hpr.Z * Utils.DEG_TO_RAD, hpr.Y * Utils.DEG_TO_RAD, hpr.X * Utils.DEG_TO_RAD));
+
+                    xyz.X += rotate.X; xyz.Y += rotate.Y; xyz.Z += rotate.Z;
+                    ds.SetViewpoint(ref xyz, ref hpr);
+                    break;
+
+                case 'a':
+                    hpr.X++;
+                    ds.SetViewpoint(ref xyz, ref hpr);
+                    break;
+
+                case 's':
+                    Vector3 rotate2 = (new Vector3(-1, 0, 0) * Quaternion.CreateFromEulers(hpr.Z * Utils.DEG_TO_RAD, hpr.Y * Utils.DEG_TO_RAD, hpr.X * Utils.DEG_TO_RAD));
+
+                    xyz.X += rotate2.X; xyz.Y += rotate2.Y; xyz.Z += rotate2.Z;
+                    ds.SetViewpoint(ref xyz, ref hpr);
+                    break;
+                case 'd':
+                    hpr.X--;
+                    ds.SetViewpoint(ref xyz, ref hpr);
+                    break;
+                case 'r':
+                    xyz.Z++;
+                    ds.SetViewpoint(ref xyz, ref hpr);
+                    break;
+                case 'f':
+                    xyz.Z--;
+                    ds.SetViewpoint(ref xyz, ref hpr);
+                    break;
+                case 'e':
+                    xyz.Y++;
+                    ds.SetViewpoint(ref xyz, ref hpr);
+                    break;
+                case 'q':
+                    xyz.Y--;
+                    ds.SetViewpoint(ref xyz, ref hpr);
+                    break;
+            }
+        }
+
+        public void step(int pause)
+        {
+            
+            ds.SetColor(1.0f, 1.0f, 0.0f);
+            ds.SetTexture(ds.Texture.Wood);
+            lock (_prims)
+            {
+                foreach (OdePrim prm in _prims)
+                {
+                    //IntPtr body = d.GeomGetBody(prm.prim_geom);
+                    if (prm.prim_geom != IntPtr.Zero)
+                    {
+                        d.Vector3 pos;
+                        d.GeomCopyPosition(prm.prim_geom, out pos);
+                        //d.BodyCopyPosition(body, out pos);
+
+                        d.Matrix3 R;
+                        d.GeomCopyRotation(prm.prim_geom, out R);
+                        //d.BodyCopyRotation(body, out R);
+
+
+                        d.Vector3 sides = new d.Vector3();
+                        sides.X = prm.Size.X;
+                        sides.Y = prm.Size.Y;
+                        sides.Z = prm.Size.Z;
+
+                        ds.DrawBox(ref pos, ref R, ref sides);
+                    }
+                }
+            }
+            ds.SetColor(1.0f, 0.0f, 0.0f);
+            lock (_characters)
+            {
+                foreach (OdeCharacter chr in _characters)
+                {
+                    if (chr.Shell != IntPtr.Zero)
+                    {
+                        IntPtr body = d.GeomGetBody(chr.Shell);
+
+                        d.Vector3 pos;
+                        d.GeomCopyPosition(chr.Shell, out pos);
+                        //d.BodyCopyPosition(body, out pos);
+
+                        d.Matrix3 R;
+                        d.GeomCopyRotation(chr.Shell, out R);
+                        //d.BodyCopyRotation(body, out R);
+
+                        ds.DrawCapsule(ref pos, ref R, chr.Size.Z, 0.35f);
+                        d.Vector3 sides = new d.Vector3();
+                        sides.X = 0.5f;
+                        sides.Y = 0.5f;
+                        sides.Z = 0.5f;
+
+                        ds.DrawBox(ref pos, ref R, ref sides);
+
+
+                    }
+                }
+            }
+        }
+
+        public void start(int unused)
+        {
+            
+            ds.SetViewpoint(ref xyz, ref hpr);
+        }
+#endif
     }
 }
diff --git a/OpenSim/Region/Physics/OdePlugin/drawstuff.cs b/OpenSim/Region/Physics/OdePlugin/drawstuff.cs
new file mode 100644
index 0000000..73afe12
--- /dev/null
+++ b/OpenSim/Region/Physics/OdePlugin/drawstuff.cs
@@ -0,0 +1,98 @@
+/*
+ * Copyright ODE
+ * Ode.NET - .NET bindings for ODE
+ *  Jason Perkins (starkos@industriousone.com)
+ *  Licensed under the New BSD
+ *  Part of the OpenDynamicsEngine
+Open Dynamics Engine
+Copyright (c) 2001-2007, Russell L. Smith.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions
+are met:
+
+Redistributions of source code must retain the above copyright notice,
+this list of conditions and the following disclaimer.
+
+Redistributions in binary form must reproduce the above copyright notice,
+this list of conditions and the following disclaimer in the documentation
+and/or other materials provided with the distribution.
+
+Neither the names of ODE's copyright owner nor the names of its
+contributors may be used to endorse or promote products derived from
+this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ * 
+ * 
+ */
+
+using System;
+using System.Runtime.InteropServices;
+using Ode.NET;
+
+namespace Drawstuff.NET
+{
+#if dDOUBLE
+        using dReal = System.Double;
+#else
+    using dReal = System.Single;
+#endif
+
+    public static class ds
+    {
+        public const int VERSION = 2;
+
+        public enum Texture
+        {
+            None,
+            Wood
+        }
+
+        [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
+        public delegate void CallbackFunction(int arg);
+
+        [StructLayout(LayoutKind.Sequential)]
+        public struct Functions
+        {
+            public int version;
+            public CallbackFunction start;
+            public CallbackFunction step;
+            public CallbackFunction command;
+            public CallbackFunction stop;
+            public string path_to_textures;
+        }
+
+        [DllImport("drawstuff", EntryPoint = "dsDrawBox")]
+        public static extern void DrawBox(ref d.Vector3 pos, ref d.Matrix3 R, ref d.Vector3 sides);
+
+        [DllImport("drawstuff", EntryPoint = "dsDrawCapsule")]
+        public static extern void DrawCapsule(ref d.Vector3 pos, ref d.Matrix3 R, dReal length, dReal radius);
+
+        [DllImport("drawstuff", EntryPoint = "dsDrawConvex")]
+        public static extern void DrawConvex(ref d.Vector3 pos, ref d.Matrix3 R, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons);
+
+        [DllImport("drawstuff", EntryPoint = "dsSetColor")]
+        public static extern void SetColor(float red, float green, float blue);
+
+        [DllImport("drawstuff", EntryPoint = "dsSetTexture")]
+        public static extern void SetTexture(Texture texture);
+
+        [DllImport("drawstuff", EntryPoint = "dsSetViewpoint")]
+        public static extern void SetViewpoint(ref d.Vector3 xyz, ref d.Vector3 hpr);
+
+        [DllImport("drawstuff", EntryPoint = "dsSimulationLoop")]
+        public static extern void SimulationLoop(int argc, string[] argv, int window_width, int window_height, ref Functions fn);
+    }
+}
\ No newline at end of file