2 files changed, 243 insertions, 46 deletions

diff --git a/OpenSim/Region/PhysicsModules/SharedBase/PhysicsActor.cs b/OpenSim/Region/PhysicsModules/SharedBase/PhysicsActor.cs
index 33f0337..ad9b28f 100644
--- a/OpenSim/Region/PhysicsModules/SharedBase/PhysicsActor.cs
+++ b/OpenSim/Region/PhysicsModules/SharedBase/PhysicsActor.cs
@@ -55,6 +55,28 @@ namespace OpenSim.Region.PhysicsModules.SharedBase
         Absolute
     }
 
+    public class PhysicsInertiaData
+    {
+        public float TotalMass; // the total mass of a linkset
+        public Vector3 CenterOfMass;  // the center of mass position relative to root part position
+        public Vector3 Inertia; //  (Ixx, Iyy, Izz) moment of inertia relative to center of mass and principal axis in local coords
+        public Vector4 InertiaRotation; // if principal axis don't match local axis, the principal axis rotation
+                                        // or the upper triangle of the inertia tensor 
+                                        // Ixy (= Iyx), Ixz (= Izx), Iyz (= Izy))
+
+        public PhysicsInertiaData()
+        {
+        }
+
+        public PhysicsInertiaData(PhysicsInertiaData source)
+        {
+           TotalMass = source.TotalMass;
+           CenterOfMass = source.CenterOfMass;
+           Inertia = source.Inertia;
+           InertiaRotation = source.InertiaRotation;
+        }
+    }
+
     public struct CameraData
     {
         public Quaternion CameraRotation;
@@ -463,6 +485,20 @@ namespace OpenSim.Region.PhysicsModules.SharedBase
 
         public virtual void AddCollisionEvent(uint CollidedWith, ContactPoint contact) { }
 
+        public virtual PhysicsInertiaData GetInertiaData()
+        {
+            PhysicsInertiaData data = new PhysicsInertiaData();
+            data.TotalMass = this.Mass;
+            data.CenterOfMass = CenterOfMass - Position;
+            data.Inertia = Vector3.Zero;
+            data.InertiaRotation = Vector4.Zero;
+            return data;
+        }
+
+        public virtual void SetInertiaData(PhysicsInertiaData inertia)
+        {
+        }
+
         // Warning in a parent part it returns itself, not null
         public virtual PhysicsActor ParentActor { get { return this; } }
 
diff --git a/OpenSim/Region/PhysicsModules/ubOde/ODEPrim.cs b/OpenSim/Region/PhysicsModules/ubOde/ODEPrim.cs
index 45ef273..d560b41 100644
--- a/OpenSim/Region/PhysicsModules/ubOde/ODEPrim.cs
+++ b/OpenSim/Region/PhysicsModules/ubOde/ODEPrim.cs
@@ -182,14 +182,16 @@ namespace OpenSim.Region.PhysicsModule.ubOde
         private float m_streamCost;
 
         public d.Mass primdMass; // prim inertia information on it's own referencial
+        private PhysicsInertiaData m_InertiaOverride;
         float primMass; // prim own mass
         float primVolume; // prim own volume;
-        float _mass; // object mass acording to case
+        float m_mass; // object mass acording to case
 
         public int givefakepos;
         private Vector3 fakepos;
         public int givefakeori;
         private Quaternion fakeori;
+        private PhysicsInertiaData m_fakeInertiaOverride;
 
         private int m_eventsubscription;
         private int m_cureventsubscription;
@@ -465,10 +467,110 @@ namespace OpenSim.Region.PhysicsModule.ubOde
             }
         }
 
+        public override PhysicsInertiaData GetInertiaData()
+        {
+            PhysicsInertiaData inertia;
+            if(childPrim)
+            {
+                if(_parent != null)
+                    return _parent.GetInertiaData();
+                else
+                {
+                    inertia = new PhysicsInertiaData();
+                    inertia.TotalMass = -1;
+                    return inertia;
+                }
+            }
+
+            inertia = new PhysicsInertiaData();
+
+            // double buffering
+            if(m_fakeInertiaOverride != null)
+            { 
+                d.Mass objdmass = new d.Mass();
+                objdmass.I.M00 = m_fakeInertiaOverride.Inertia.X;
+                objdmass.I.M11 = m_fakeInertiaOverride.Inertia.Y;
+                objdmass.I.M22 = m_fakeInertiaOverride.Inertia.Z;
+
+                objdmass.mass = m_fakeInertiaOverride.TotalMass;
+                    
+                if(Math.Abs(m_fakeInertiaOverride.InertiaRotation.W) < 0.999)
+                {
+                    d.Matrix3 inertiarotmat = new d.Matrix3();
+                    d.Quaternion inertiarot = new d.Quaternion();
+
+                    inertiarot.X = m_fakeInertiaOverride.InertiaRotation.X;
+                    inertiarot.Y = m_fakeInertiaOverride.InertiaRotation.Y;
+                    inertiarot.Z = m_fakeInertiaOverride.InertiaRotation.Z;
+                    inertiarot.W = m_fakeInertiaOverride.InertiaRotation.W;
+                    d.RfromQ(out inertiarotmat, ref inertiarot);
+                    d.MassRotate(ref objdmass, ref inertiarotmat);
+                }
+
+                inertia.TotalMass = m_fakeInertiaOverride.TotalMass;
+                inertia.CenterOfMass = m_fakeInertiaOverride.CenterOfMass;
+                inertia.Inertia.X = objdmass.I.M00;
+                inertia.Inertia.Y = objdmass.I.M11;
+                inertia.Inertia.Z = objdmass.I.M22;
+                inertia.InertiaRotation.X =  objdmass.I.M01;
+                inertia.InertiaRotation.Y =  objdmass.I.M02;
+                inertia.InertiaRotation.Z =  objdmass.I.M12;
+                return inertia;
+            }
+
+            inertia.TotalMass = m_mass;
+
+            if(Body == IntPtr.Zero || prim_geom == IntPtr.Zero)
+            {
+                inertia.CenterOfMass = Vector3.Zero;
+                inertia.Inertia = Vector3.Zero;
+                inertia.InertiaRotation =  Vector4.Zero;
+                return inertia;
+            }
+
+            d.Vector3 dtmp;
+            d.Mass m = new d.Mass();
+            lock(_parent_scene.OdeLock)
+            {
+                d.AllocateODEDataForThread(0);
+                dtmp = d.GeomGetOffsetPosition(prim_geom);
+                d.BodyGetMass(Body, out m);
+            }
+
+            Vector3 cm = new Vector3(-dtmp.X, -dtmp.Y, -dtmp.Z);
+            inertia.CenterOfMass = cm;
+            inertia.Inertia = new Vector3(m.I.M00, m.I.M11, m.I.M22);
+            inertia.InertiaRotation = new Vector4(m.I.M01, m.I.M02 , m.I.M12, 0);
+
+            return inertia;
+        }
+
+        public override void SetInertiaData(PhysicsInertiaData inertia)
+        {
+            if(childPrim)
+            {
+                if(_parent != null)
+                    _parent.SetInertiaData(inertia);
+                return;
+            }
+
+            if(inertia.TotalMass > 0)
+                m_fakeInertiaOverride = new PhysicsInertiaData(inertia);
+            else
+                m_fakeInertiaOverride = null;
+
+            if (inertia.TotalMass > _parent_scene.maximumMassObject)
+                inertia.TotalMass = _parent_scene.maximumMassObject;
+            AddChange(changes.SetInertia,(object)m_fakeInertiaOverride);
+        }
+
         public override Vector3 CenterOfMass
         {
             get
             {
+                if(!childPrim && m_fakeInertiaOverride != null)
+                    return m_fakeInertiaOverride.CenterOfMass;
+
                 lock (_parent_scene.OdeLock)
                 {
                     d.AllocateODEDataForThread(0);
@@ -922,8 +1024,8 @@ namespace OpenSim.Region.PhysicsModule.ubOde
             {
                 float old = m_density;
                 m_density = value / 100f;
-                if(m_density != old)
-                    UpdatePrimBodyData();
+ //               if(m_density != old)
+ //                   UpdatePrimBodyData();
             }
         }
         public override float GravModifier
@@ -1716,26 +1818,29 @@ namespace OpenSim.Region.PhysicsModule.ubOde
                 m_log.Warn("[PHYSICS]: MakeBody root geom already had a body");
             }
 
-            d.Matrix3 mymat = new d.Matrix3();
-            d.Quaternion myrot = new d.Quaternion();
-            d.Mass objdmass = new d.Mass { };
+            bool noInertiaOverride = (m_InertiaOverride == null);
 
             Body = d.BodyCreate(_parent_scene.world);
 
-            objdmass = primdMass;
+            d.Matrix3 mymat = new d.Matrix3();
+            d.Quaternion myrot = new d.Quaternion();
+            d.Mass objdmass = new d.Mass { };
 
-            // rotate inertia
             myrot.X = _orientation.X;
             myrot.Y = _orientation.Y;
             myrot.Z = _orientation.Z;
             myrot.W = _orientation.W;
-
             d.RfromQ(out mymat, ref myrot);
-            d.MassRotate(ref objdmass, ref mymat);
 
             // set the body rotation
             d.BodySetRotation(Body, ref mymat);
 
+            if(noInertiaOverride)
+            {
+                objdmass = primdMass;
+                d.MassRotate(ref objdmass, ref mymat);
+            }
+    
             // recompute full object inertia if needed
             if (childrenPrim.Count > 0)
             {
@@ -1758,27 +1863,12 @@ namespace OpenSim.Region.PhysicsModule.ubOde
                             continue;
                         }
 
-                        tmpdmass = prm.primdMass;
-
-                        // apply prim current rotation to inertia
                         quat.X = prm._orientation.X;
                         quat.Y = prm._orientation.Y;
                         quat.Z = prm._orientation.Z;
                         quat.W = prm._orientation.W;
                         d.RfromQ(out mat, ref quat);
-                        d.MassRotate(ref tmpdmass, ref mat);
-
-                        Vector3 ppos = prm._position;
-                        ppos.X -= rcm.X;
-                        ppos.Y -= rcm.Y;
-                        ppos.Z -= rcm.Z;
-                        // refer inertia to root prim center of mass position
-                        d.MassTranslate(ref tmpdmass,
-                            ppos.X,
-                            ppos.Y,
-                            ppos.Z);
-
-                        d.MassAdd(ref objdmass, ref tmpdmass); // add to total object inertia
+
                         // fix prim colision cats
 
                         if (d.GeomGetBody(prm.prim_geom) != IntPtr.Zero)
@@ -1791,6 +1881,24 @@ namespace OpenSim.Region.PhysicsModule.ubOde
                         d.GeomSetBody(prm.prim_geom, Body);
                         prm.Body = Body;
                         d.GeomSetOffsetWorldRotation(prm.prim_geom, ref mat); // set relative rotation
+
+                        if(noInertiaOverride)
+                        {
+                            tmpdmass = prm.primdMass;
+
+                            d.MassRotate(ref tmpdmass, ref mat);
+                            Vector3 ppos = prm._position;
+                            ppos.X -= rcm.X;
+                            ppos.Y -= rcm.Y;
+                            ppos.Z -= rcm.Z;
+                            // refer inertia to root prim center of mass position
+                            d.MassTranslate(ref tmpdmass,
+                                ppos.X,
+                                ppos.Y,
+                                ppos.Z);
+
+                            d.MassAdd(ref objdmass, ref tmpdmass); // add to total object inertia
+                        }
                     }
                 }
             }
@@ -1799,19 +1907,60 @@ namespace OpenSim.Region.PhysicsModule.ubOde
             // associate root geom with body
             d.GeomSetBody(prim_geom, Body);
 
-            d.BodySetPosition(Body, _position.X + objdmass.c.X, _position.Y + objdmass.c.Y, _position.Z + objdmass.c.Z);
+            if(noInertiaOverride)
+                d.BodySetPosition(Body, _position.X + objdmass.c.X, _position.Y + objdmass.c.Y, _position.Z + objdmass.c.Z);
+            else
+            {
+                Vector3 ncm =  m_InertiaOverride.CenterOfMass * _orientation;
+                d.BodySetPosition(Body,
+                    _position.X + ncm.X,
+                    _position.Y + ncm.Y,
+                    _position.Z + ncm.Z);
+            }
+
             d.GeomSetOffsetWorldPosition(prim_geom, _position.X, _position.Y, _position.Z);
 
-            d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body
-            myrot.X = -myrot.X;
-            myrot.Y = -myrot.Y;
-            myrot.Z = -myrot.Z;
+            if(noInertiaOverride)
+            {
+                d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body
+                myrot.X = -myrot.X;
+                myrot.Y = -myrot.Y;
+                myrot.Z = -myrot.Z;
 
-            d.RfromQ(out mymat, ref myrot);
-            d.MassRotate(ref objdmass, ref mymat);
+                d.RfromQ(out mymat, ref myrot);
+                d.MassRotate(ref objdmass, ref mymat);
 
-            d.BodySetMass(Body, ref objdmass);
-            _mass = objdmass.mass;
+                d.BodySetMass(Body, ref objdmass);
+                m_mass = objdmass.mass;
+            }
+            else
+            {
+                objdmass.c.X = 0;
+                objdmass.c.Y = 0;
+                objdmass.c.Z = 0;
+
+                objdmass.I.M00 = m_InertiaOverride.Inertia.X;
+                objdmass.I.M11 = m_InertiaOverride.Inertia.Y;
+                objdmass.I.M22 = m_InertiaOverride.Inertia.Z;
+
+                objdmass.mass = m_InertiaOverride.TotalMass;
+
+                if(Math.Abs(m_InertiaOverride.InertiaRotation.W) < 0.999)
+                {
+                    d.Matrix3 inertiarotmat = new d.Matrix3();
+                    d.Quaternion inertiarot = new d.Quaternion();
+
+                    inertiarot.X = m_InertiaOverride.InertiaRotation.X;
+                    inertiarot.Y = m_InertiaOverride.InertiaRotation.Y;
+                    inertiarot.Z = m_InertiaOverride.InertiaRotation.Z;
+                    inertiarot.W = m_InertiaOverride.InertiaRotation.W;
+                    d.RfromQ(out inertiarotmat, ref inertiarot);
+                    d.MassRotate(ref objdmass, ref inertiarotmat);
+                }
+                d.BodySetMass(Body, ref objdmass);
+
+                m_mass = objdmass.mass;
+            }
 
             // disconnect from world gravity so we can apply buoyancy
             d.BodySetGravityMode(Body, false);
@@ -1990,7 +2139,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
                                 SetInStaticSpace(prm);
                             }
                             prm.Body = IntPtr.Zero;
-                            prm._mass = prm.primMass;
+                            prm.m_mass = prm.primMass;
                             prm.m_collisionscore = 0;
                         }
                     }
@@ -2004,7 +2153,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
                 }
                 Body = IntPtr.Zero;
             }
-            _mass = primMass;
+            m_mass = primMass;
             m_collisionscore = 0;
         }
 
@@ -2081,7 +2230,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
             d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z);
             d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body
             d.BodySetMass(Body, ref objdmass);
-            _mass = objdmass.mass;
+            m_mass = objdmass.mass;
         }
 
         private void FixInertia(Vector3 NewPos)
@@ -2145,7 +2294,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
             d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z);
             d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body
             d.BodySetMass(Body, ref objdmass);
-            _mass = objdmass.mass;
+            m_mass = objdmass.mass;
         }
 
         private void FixInertia(Quaternion newrot)
@@ -2211,7 +2360,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
             d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z);
             d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body
             d.BodySetMass(Body, ref objdmass);
-            _mass = objdmass.mass;
+            m_mass = objdmass.mass;
         }
 
 
@@ -2226,7 +2375,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
             if (primMass > _parent_scene.maximumMassObject)
                 primMass = _parent_scene.maximumMassObject;
 
-            _mass = primMass; // just in case
+            m_mass = primMass; // just in case
 
             d.MassSetBoxTotal(out primdMass, primMass, 2.0f * m_OBB.X, 2.0f * m_OBB.Y, 2.0f * m_OBB.Z);
 
@@ -3306,6 +3455,15 @@ namespace OpenSim.Region.PhysicsModule.ubOde
             m_useHoverPID = active;
         }
 
+        private void changeInertia(PhysicsInertiaData inertia)
+        {
+            m_InertiaOverride = inertia;
+
+            if (Body != IntPtr.Zero)
+                DestroyBody();
+            MakeBody();
+        }
+
         #endregion
 
         public void Move()
@@ -3346,7 +3504,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
                 float fy = 0;
                 float fz = 0;
 
-                float m_mass = _mass;
+                float mass = m_mass;
 
                 if (m_usePID && m_PIDTau > 0)
                 {
@@ -3453,9 +3611,9 @@ namespace OpenSim.Region.PhysicsModule.ubOde
                     fz = _parent_scene.gravityz * b;
                 }
 
-                fx *= m_mass;
-                fy *= m_mass;
-                fz *= m_mass;
+                fx *= mass;
+                fy *= mass;
+                fz *= mass;
 
                 // constant force
                 fx += m_force.X;
@@ -3941,6 +4099,10 @@ namespace OpenSim.Region.PhysicsModule.ubOde
                     changePIDHoverActive((bool)arg);
                     break;
 
+                case changes.SetInertia:
+                    changeInertia((PhysicsInertiaData) arg);
+                    break;
+
                 case changes.Null:
                     donullchange();
                     break;
@@ -3957,7 +4119,6 @@ namespace OpenSim.Region.PhysicsModule.ubOde
             _parent_scene.AddChange((PhysicsActor) this, what, arg);
         }
 
-
         private struct strVehicleBoolParam
         {
             public int param;