fix llGetCenterOfMass ( checked with ubitODE only)

4 files changed, 101 insertions, 9 deletions

diff --git a/OpenSim/Region/Framework/Scenes/SceneObjectGroup.cs b/OpenSim/Region/Framework/Scenes/SceneObjectGroup.cs
index ba353c4..b0f2015 100644
--- a/OpenSim/Region/Framework/Scenes/SceneObjectGroup.cs
+++ b/OpenSim/Region/Framework/Scenes/SceneObjectGroup.cs
@@ -4041,6 +4041,39 @@ namespace OpenSim.Region.Framework.Scenes
             return retmass;
         }
 
+        // center of mass of full object
+        public Vector3 GetCenterOfMass()
+        {
+            PhysicsActor pa = RootPart.PhysActor;
+
+            if(((RootPart.Flags & PrimFlags.Physics) !=0) && pa !=null)
+            {
+                // physics knows better about center of mass of physical prims
+                Vector3 tmp = pa.CenterOfMass;
+                return tmp;
+            }
+            
+            Vector3 Ptot = Vector3.Zero;
+            float totmass = 0f;
+            float m;
+
+            SceneObjectPart[] parts = m_parts.GetArray();
+            for (int i = 0; i < parts.Length; i++)
+            {
+                m = parts[i].GetMass();
+                Ptot += parts[i].GetPartCenterOfMass() * m;
+                totmass += m;
+            }
+
+            if (totmass == 0)
+                totmass = 0;
+            else
+                totmass = 1 / totmass;
+            Ptot *= totmass;
+
+            return Ptot;
+        }
+
         /// <summary>
         /// If the object is a sculpt/mesh, retrieve the mesh data for each part and reinsert it into each shape so that
         /// the physics engine can use it.
diff --git a/OpenSim/Region/Framework/Scenes/SceneObjectPart.cs b/OpenSim/Region/Framework/Scenes/SceneObjectPart.cs
index c9659cb..b198ca7 100644
--- a/OpenSim/Region/Framework/Scenes/SceneObjectPart.cs
+++ b/OpenSim/Region/Framework/Scenes/SceneObjectPart.cs
@@ -1865,7 +1865,7 @@ namespace OpenSim.Region.Framework.Scenes
             {
                 if ((!isPhantom || isPhysical || _VolumeDetectActive) && !ParentGroup.IsAttachment
                                                 && !(Shape.PathCurve == (byte)Extrusion.Flexible))
-                    AddToPhysics(isPhysical, isPhantom, building, true);
+                    AddToPhysics(isPhysical, isPhantom, building, isPhysical);
                 else
                     PhysActor = null; // just to be sure
             }
@@ -2308,7 +2308,7 @@ namespace OpenSim.Region.Framework.Scenes
              */
         }
 
-         public float GetMass()
+        public float GetMass()
         {
             PhysicsActor pa = PhysActor;
 
@@ -2318,6 +2318,40 @@ namespace OpenSim.Region.Framework.Scenes
                 return 0;
         }
 
+        public Vector3 GetCenterOfMass()
+        {
+            if (ParentGroup.RootPart == this)
+            {
+                if (ParentGroup.IsDeleted)
+                    return AbsolutePosition;
+                return ParentGroup.GetCenterOfMass();
+            }
+
+            PhysicsActor pa = PhysActor;
+
+            if (pa != null)
+            {
+                Vector3 tmp = pa.CenterOfMass;
+                return tmp;
+            }
+            else
+                return AbsolutePosition;
+        }
+
+        public Vector3 GetPartCenterOfMass()
+        {
+            PhysicsActor pa = PhysActor;
+
+            if (pa != null)
+            {
+                Vector3 tmp = pa.CenterOfMass;
+                return tmp;
+            }
+            else
+                return AbsolutePosition;
+        }
+
+
         public Vector3 GetForce()
         {
             return Force;
@@ -4802,7 +4836,7 @@ namespace OpenSim.Region.Framework.Scenes
                 {
                     Velocity = velocity;
                     AngularVelocity = rotationalVelocity;
-                    pa.Velocity = velocity;
+//                    pa.Velocity = velocity;
                     pa.RotationalVelocity = rotationalVelocity;
 
                     // if not vehicle and root part apply force and torque
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs
index 2d587ab..c9a453d 100644
--- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs
@@ -450,7 +450,7 @@ namespace OpenSim.Region.Physics.OdePlugin
             get

             {

                 d.Vector3 dtmp;

-                if (IsPhysical && !childPrim && Body != IntPtr.Zero)

+                if (!childPrim && Body != IntPtr.Zero)

                 {

                     dtmp = d.BodyGetPosition(Body);

                     return new Vector3(dtmp.X, dtmp.Y, dtmp.Z);

@@ -465,12 +465,38 @@ namespace OpenSim.Region.Physics.OdePlugin
                     q.Z = dq.Z;

                     q.W = dq.W;

 

-                    Vector3 vtmp = primOOBoffset * q;

+                    Vector3 Ptot = primOOBoffset * q;

                     dtmp = d.GeomGetPosition(prim_geom);

-                    return new Vector3(dtmp.X + vtmp.X, dtmp.Y + vtmp.Y, dtmp.Z + vtmp.Z);

+                    Ptot.X += dtmp.X;

+                    Ptot.Y += dtmp.Y;

+                    Ptot.Z += dtmp.Z;

+

+//                    if(childPrim)  we only know about physical linksets

+                        return Ptot;

+/*

+                    float tmass = _mass;

+                    Ptot *= tmass;

+

+                    float m;

+

+                    foreach (OdePrim prm in childrenPrim)

+                    {

+                        m = prm._mass;

+                        Ptot += prm.CenterOfMass * m;

+                        tmass += m;

+                    }

+

+                    if (tmass == 0)

+                        tmass = 0;

+                    else

+                        tmass = 1.0f / tmass;

+

+                    Ptot *= tmass;

+                    return Ptot;

+ */

                 }

                 else

-                    return Vector3.Zero;

+                    return _position;

             }

         }

         /*

@@ -3490,7 +3516,6 @@ namespace OpenSim.Region.Physics.OdePlugin
                         {

                             d.BodySetPosition(Body, lpos.X, lpos.Y, m_targetHoverHeight);

                             d.BodySetLinearVel(Body, vel.X, vel.Y, 0);

-                            return;

                         }

                         else

                         {

diff --git a/OpenSim/Region/ScriptEngine/Shared/Api/Implementation/LSL_Api.cs b/OpenSim/Region/ScriptEngine/Shared/Api/Implementation/LSL_Api.cs
index 8e73eb1..6ee1a5d 100644
--- a/OpenSim/Region/ScriptEngine/Shared/Api/Implementation/LSL_Api.cs
+++ b/OpenSim/Region/ScriptEngine/Shared/Api/Implementation/LSL_Api.cs
@@ -5370,7 +5370,7 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
         public LSL_Vector llGetCenterOfMass()
         {
             m_host.AddScriptLPS(1);
-            Vector3 center = m_host.GetGeometricCenter();
+            Vector3 center = m_host.GetCenterOfMass();
             return new LSL_Vector(center.X,center.Y,center.Z);
         }