3 files changed, 113 insertions, 8 deletions

diff --git a/OpenSim/Region/Framework/Scenes/SceneObjectGroup.cs b/OpenSim/Region/Framework/Scenes/SceneObjectGroup.cs
index 66a6520..b5e1a52 100644
--- a/OpenSim/Region/Framework/Scenes/SceneObjectGroup.cs
+++ b/OpenSim/Region/Framework/Scenes/SceneObjectGroup.cs
@@ -2671,11 +2671,11 @@ namespace OpenSim.Region.Framework.Scenes
 
             Vector3 axPos = linkPart.OffsetPosition;
             Quaternion parentRot = m_rootPart.RotationOffset;
-            axPos *= Quaternion.Inverse(parentRot);
+            axPos *= Quaternion.Conjugate(parentRot);
             linkPart.OffsetPosition = axPos;
 
             Quaternion oldRot = linkPart.RotationOffset;
-            Quaternion newRot = Quaternion.Inverse(parentRot) * oldRot;
+            Quaternion newRot = Quaternion.Conjugate(parentRot) * oldRot;
             linkPart.RotationOffset = newRot;
 
 //            linkPart.ParentID = m_rootPart.LocalId; done above
@@ -2943,12 +2943,12 @@ namespace OpenSim.Region.Framework.Scenes
             Quaternion rootRotation = m_rootPart.RotationOffset;
 
             Vector3 pos = part.OffsetPosition;
-            pos *= Quaternion.Inverse(rootRotation);
+            pos *= Quaternion.Conjugate(rootRotation);
             part.OffsetPosition = pos;
 
             parentRot = m_rootPart.RotationOffset;
             oldRot = part.RotationOffset;
-            Quaternion newRot = Quaternion.Inverse(parentRot) * worldRot;
+            Quaternion newRot = Quaternion.Conjugate(parentRot) * worldRot;
             part.RotationOffset = newRot;
 
             part.UpdatePrimFlags(UsesPhysics, IsTemporary, IsPhantom, IsVolumeDetect, false);
@@ -3548,13 +3548,16 @@ namespace OpenSim.Region.Framework.Scenes
         /// <param name="rot"></param>
         public void UpdateGroupRotationR(Quaternion rot)
         {
+            m_rootPart.UpdateRotation(rot);
+        
+/* this is done by rootpart RotationOffset set called by UpdateRotation
             PhysicsActor actor = m_rootPart.PhysActor;
             if (actor != null)
             {
                 actor.Orientation = m_rootPart.RotationOffset;
                 m_scene.PhysicsScene.AddPhysicsActorTaint(actor);
             }
-
+*/
             HasGroupChanged = true;
             ScheduleGroupForTerseUpdate();
         }
@@ -4063,6 +4066,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 37b5554..ffb86f3 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
             }
@@ -2311,7 +2311,7 @@ namespace OpenSim.Region.Framework.Scenes
              */
         }
 
-         public float GetMass()
+        public float GetMass()
         {
             PhysicsActor pa = PhysActor;
 
@@ -2321,6 +2321,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;
@@ -4804,7 +4838,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/Framework/Scenes/ScenePresence.cs b/OpenSim/Region/Framework/Scenes/ScenePresence.cs
index 311ba6a..1e717d9 100644
--- a/OpenSim/Region/Framework/Scenes/ScenePresence.cs
+++ b/OpenSim/Region/Framework/Scenes/ScenePresence.cs
@@ -1088,6 +1088,41 @@ namespace OpenSim.Region.Framework.Scenes
             SendTerseUpdateToAllClients();
         }
 
+        public void avnLocalTeleport(Vector3 newpos, Vector3? newvel, bool rotateToVelXY)
+        {
+            CheckLandingPoint(ref newpos);
+            AbsolutePosition = newpos;
+
+            if (newvel.HasValue)
+            {
+                if ((Vector3)newvel == Vector3.Zero)
+                {
+                    if (PhysicsActor != null)
+                        PhysicsActor.SetMomentum(Vector3.Zero);
+                    m_velocity = Vector3.Zero;
+                }
+                else
+                {
+                    if (PhysicsActor != null)
+                        PhysicsActor.SetMomentum((Vector3)newvel);
+                    m_velocity = (Vector3)newvel;
+
+                    if (rotateToVelXY)
+                    {
+                        Vector3 lookAt = (Vector3)newvel;
+                        lookAt.Z = 0;
+                        lookAt.Normalize();
+                        ControllingClient.SendLocalTeleport(newpos, lookAt, (uint)TeleportFlags.ViaLocation);
+                        return;
+                    }
+                }
+            }
+
+            SendTerseUpdateToAllClients();
+        }
+
+
+
         public void StopFlying()
         {
             ControllingClient.StopFlying(this);