8 files changed, 428 insertions, 228 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);
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs
index 8b5b989..326fe97 100644
--- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs
@@ -103,12 +103,13 @@ namespace OpenSim.Region.Physics.OdePlugin
 
         private float m_buoyancy = 0f;
 
+        private bool m_freemove = false;
         // private CollisionLocker ode;
 
         private string m_name = String.Empty;
         // other filter control
         int m_colliderfilter = 0;
-        //        int m_colliderGroundfilter = 0;
+        int m_colliderGroundfilter = 0;
         int m_colliderObjectfilter = 0;
 
         // Default we're a Character
@@ -280,7 +281,6 @@ namespace OpenSim.Region.Physics.OdePlugin
                     m_iscolliding = false;
                 else
                 {
-//                    SetPidStatus(false);
                     m_pidControllerActive = true;
                     m_iscolliding = true;
                 }
@@ -379,24 +379,21 @@ namespace OpenSim.Region.Physics.OdePlugin
             get { return _position; }
             set
             {
-                if (Body == IntPtr.Zero || Shell == IntPtr.Zero)
+                if (value.IsFinite())
                 {
-                    if (value.IsFinite())
+                    if (value.Z > 9999999f)
                     {
-                        if (value.Z > 9999999f)
-                        {
-                            value.Z = _parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
-                        }
-                        if (value.Z < -100f)
-                        {
-                            value.Z = _parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
-                        }
-                        AddChange(changes.Position, value);
+                        value.Z = _parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
                     }
-                    else
+                    if (value.Z < -100f)
                     {
-                        m_log.Warn("[PHYSICS]: Got a NaN Position from Scene on a Character");
+                        value.Z = _parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
                     }
+                    AddChange(changes.Position, value);
+                }
+                else
+                {
+                    m_log.Warn("[PHYSICS]: Got a NaN Position from Scene on a Character");
                 }
             }
         }
@@ -616,7 +613,7 @@ namespace OpenSim.Region.Physics.OdePlugin
             {
                 if (pushforce)
                 {
-                    AddChange(changes.Force, force * m_density / _parent_scene.ODE_STEPSIZE / 28f);
+                    AddChange(changes.Force, force * m_density / (_parent_scene.ODE_STEPSIZE * 28f));
                 }
                 else
                 {
@@ -687,6 +684,7 @@ namespace OpenSim.Region.Physics.OdePlugin
 
             _zeroFlag = false;
             m_pidControllerActive = true;
+            m_freemove = false;
 
             d.BodySetAutoDisableFlag(Body, false);
             d.BodySetPosition(Body, npositionX, npositionY, npositionZ);
@@ -828,11 +826,17 @@ namespace OpenSim.Region.Physics.OdePlugin
                 localpos.Y = _parent_scene.WorldExtents.Y - 0.1f;
             }
             if (fixbody)
+            {
+                m_freemove = false;
                 d.BodySetPosition(Body, localpos.X, localpos.Y, localpos.Z);
+            }
+
+            float breakfactor;
 
             Vector3 vec = Vector3.Zero;
             dtmp = d.BodyGetLinearVel(Body);
             Vector3 vel = new Vector3(dtmp.X, dtmp.Y, dtmp.Z);
+            float velLengthSquared = vel.LengthSquared();
 
             float movementdivisor = 1f;
             //Ubit change divisions into multiplications below
@@ -871,104 +875,148 @@ namespace OpenSim.Region.Physics.OdePlugin
 
                 if (depth < 0.1f)
                 {
-                    m_iscolliding = true;
-                    m_colliderfilter = 2;
-                    m_iscollidingGround = true;
-
-                    ContactPoint contact = new ContactPoint();
-                    contact.PenetrationDepth = depth;
-                    contact.Position.X = localpos.X;
-                    contact.Position.Y = localpos.Y;
-                    contact.Position.Z = chrminZ;
-                    contact.SurfaceNormal.X = 0f;
-                    contact.SurfaceNormal.Y = 0f;
-                    contact.SurfaceNormal.Z = -1f;
-                    AddCollisionEvent(0, contact);
-
-                    vec.Z *= 0.5f;
+                    m_colliderGroundfilter++;
+                    if (m_colliderGroundfilter > 2)
+                    {
+                        m_iscolliding = true;
+                        m_colliderfilter = 2;
+
+                        if (m_colliderGroundfilter > 10)
+                        {
+                            m_colliderGroundfilter = 10;
+                            m_freemove = false;
+                        }
+
+                        m_iscollidingGround = true;
+
+                        ContactPoint contact = new ContactPoint();
+                        contact.PenetrationDepth = depth;
+                        contact.Position.X = localpos.X;
+                        contact.Position.Y = localpos.Y;
+                        contact.Position.Z = chrminZ;
+                        contact.SurfaceNormal.X = 0f;
+                        contact.SurfaceNormal.Y = 0f;
+                        contact.SurfaceNormal.Z = -1f;
+                        AddCollisionEvent(0, contact);
+
+                        vec.Z *= 0.5f;
+                    }
                 }
 
                 else
+                {
+                    m_colliderGroundfilter = 0;
                     m_iscollidingGround = false;
+                }
             }
             else
+            {
+                m_colliderGroundfilter = 0;
                 m_iscollidingGround = false;
+            }
 
             //******************************************
 
-            //  if velocity is zero, use position control; otherwise, velocity control
-            if (_target_velocity.X == 0.0f && _target_velocity.Y == 0.0f && _target_velocity.Z == 0.0f
-                && m_iscolliding)
+            bool tviszero = (_target_velocity.X == 0.0f && _target_velocity.Y == 0.0f && _target_velocity.Z == 0.0f);
+
+            //            if (!tviszero || m_iscolliding || velLengthSquared <0.01)
+            if (!tviszero)
+                m_freemove = false;
+
+            if (!m_freemove)
             {
-                //  keep track of where we stopped.  No more slippin' & slidin'
-                if (!_zeroFlag)
-                {
-                    _zeroFlag = true;
-                    _zeroPosition = localpos;
-                }
-                if (m_pidControllerActive)
+
+                //  if velocity is zero, use position control; otherwise, velocity control
+                if (tviszero && m_iscolliding)
                 {
-                    // We only want to deactivate the PID Controller if we think we want to have our surrogate
-                    // react to the physics scene by moving it's position.
-                    // Avatar to Avatar collisions
-                    // Prim to avatar collisions
-
-                    vec.X = -vel.X * PID_D + (_zeroPosition.X - localpos.X) * (PID_P * 2);
-                    vec.Y = -vel.Y * PID_D + (_zeroPosition.Y - localpos.Y) * (PID_P * 2);
-                    if (flying)
+                    //  keep track of where we stopped.  No more slippin' & slidin'
+                    if (!_zeroFlag)
                     {
-                        vec.Z += -vel.Z * PID_D + (_zeroPosition.Z - localpos.Z) * PID_P;
+                        _zeroFlag = true;
+                        _zeroPosition = localpos;
                     }
-                }
-                //PidStatus = true;
-            }
-            else
-            {
-                m_pidControllerActive = true;
-                _zeroFlag = false;
-
-                if (m_iscolliding)
-                {
-                    if (!flying)
+                    if (m_pidControllerActive)
                     {
-                        if (_target_velocity.Z > 0.0f)
+                        // We only want to deactivate the PID Controller if we think we want to have our surrogate
+                        // react to the physics scene by moving it's position.
+                        // Avatar to Avatar collisions
+                        // Prim to avatar collisions
+
+                        vec.X = -vel.X * PID_D + (_zeroPosition.X - localpos.X) * (PID_P * 2);
+                        vec.Y = -vel.Y * PID_D + (_zeroPosition.Y - localpos.Y) * (PID_P * 2);
+                        if (flying)
                         {
-                            // We're colliding with something and we're not flying but we're moving
-                            // This means we're walking or running. JUMPING
-                            vec.Z += (_target_velocity.Z - vel.Z) * PID_D * 1.2f;// +(_zeroPosition.Z - localpos.Z) * PID_P;
+                            vec.Z += -vel.Z * PID_D + (_zeroPosition.Z - localpos.Z) * PID_P;
                         }
-                        // We're standing on something
-                        vec.X = ((_target_velocity.X * movementdivisor) - vel.X) * (PID_D);
-                        vec.Y = ((_target_velocity.Y * movementdivisor) - vel.Y) * (PID_D);
-                    }
-                    else
-                    {
-                        // We're flying and colliding with something
-                        vec.X = ((_target_velocity.X * movementdivisor) - vel.X) * (PID_D * 0.0625f);
-                        vec.Y = ((_target_velocity.Y * movementdivisor) - vel.Y) * (PID_D * 0.0625f);
-                        vec.Z += (_target_velocity.Z - vel.Z) * (PID_D);
                     }
+                    //PidStatus = true;
                 }
-                else // ie not colliding
+                else
                 {
-                    if (flying) //(!m_iscolliding && flying)
+                    m_pidControllerActive = true;
+                    _zeroFlag = false;
+
+                    if (m_iscolliding)
                     {
-                        // we're in mid air suspended
-                        vec.X = ((_target_velocity.X * movementdivisor) - vel.X) * (PID_D * 1.667f);
-                        vec.Y = ((_target_velocity.Y * movementdivisor) - vel.Y) * (PID_D * 1.667f);
-                        vec.Z += (_target_velocity.Z - vel.Z) * (PID_D);
+                        if (!flying)
+                        {
+                            if (_target_velocity.Z > 0.0f)
+                            {
+                                // We're colliding with something and we're not flying but we're moving
+                                // This means we're walking or running. JUMPING
+                                vec.Z += (_target_velocity.Z - vel.Z) * PID_D * 1.2f;// +(_zeroPosition.Z - localpos.Z) * PID_P;
+                            }
+                            // We're standing on something
+                            vec.X = ((_target_velocity.X * movementdivisor) - vel.X) * (PID_D);
+                            vec.Y = ((_target_velocity.Y * movementdivisor) - vel.Y) * (PID_D);
+                        }
+                        else
+                        {
+                            // We're flying and colliding with something
+                            vec.X = ((_target_velocity.X * movementdivisor) - vel.X) * (PID_D * 0.0625f);
+                            vec.Y = ((_target_velocity.Y * movementdivisor) - vel.Y) * (PID_D * 0.0625f);
+                            vec.Z += (_target_velocity.Z - vel.Z) * (PID_D);
+                        }
                     }
-
-                    else
+                    else // ie not colliding
                     {
-                        // we're not colliding and we're not flying so that means we're falling!
-                        // m_iscolliding includes collisions with the ground.
+                        if (flying) //(!m_iscolliding && flying)
+                        {
+                            // we're in mid air suspended
+                            vec.X = ((_target_velocity.X * movementdivisor) - vel.X) * (PID_D * 1.667f);
+                            vec.Y = ((_target_velocity.Y * movementdivisor) - vel.Y) * (PID_D * 1.667f);
+                            vec.Z += (_target_velocity.Z - vel.Z) * (PID_D);
+                        }
 
-                        // d.Vector3 pos = d.BodyGetPosition(Body);
-                        vec.X = (_target_velocity.X - vel.X) * PID_D * 0.833f;
-                        vec.Y = (_target_velocity.Y - vel.Y) * PID_D * 0.833f;
+                        else
+                        {
+                            // we're not colliding and we're not flying so that means we're falling!
+                            // m_iscolliding includes collisions with the ground.
+
+                            // d.Vector3 pos = d.BodyGetPosition(Body);
+                            vec.X = (_target_velocity.X - vel.X) * PID_D * 0.833f;
+                            vec.Y = (_target_velocity.Y - vel.Y) * PID_D * 0.833f;
+                        }
                     }
                 }
+
+                if (velLengthSquared > 2500.0f) // 50m/s apply breaks
+                {
+                    breakfactor = 0.16f * m_mass;
+                    vec.X -= breakfactor * vel.X;
+                    vec.Y -= breakfactor * vel.Y;
+                    vec.Z -= breakfactor * vel.Z;
+                }
+            }
+            else
+            {
+                breakfactor = m_mass;
+                vec.X -= breakfactor * vel.X;
+                vec.Y -= breakfactor * vel.Y;
+                if (flying)
+                    vec.Z -= breakfactor * vel.Z;
+                else
+                    vec.Z -= .5f* m_mass * vel.Z;
             }
 
             if (flying)
@@ -985,14 +1033,6 @@ namespace OpenSim.Region.Physics.OdePlugin
                 // end add Kitto Flora
             }
 
-            if (vel.X * vel.X + vel.Y * vel.Y + vel.Z * vel.Z > 2500.0f) // 50m/s apply breaks
-            {
-                float breakfactor = 0.16f * m_mass; // will give aprox 60m/s terminal velocity at free fall
-                vec.X -= breakfactor * vel.X;
-                vec.Y -= breakfactor * vel.Y;
-                vec.Z -= breakfactor * vel.Z;
-            }
-
             if (vec.IsFinite())
             {
                 if (vec.X != 0 || vec.Y !=0 || vec.Z !=0)
@@ -1210,6 +1250,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                 if (Body != IntPtr.Zero)
                     d.BodySetPosition(Body, newPos.X, newPos.Y, newPos.Z);
                 _position = newPos;
+                m_pidControllerActive = true;               
             }
 
         private void changeOrientation(Quaternion newOri)
@@ -1256,11 +1297,30 @@ namespace OpenSim.Region.Physics.OdePlugin
 
         private void changeBuilding(bool arg)
         {
-        }                                 
+        }
+
+        private void setFreeMove()
+        {
+            m_pidControllerActive = true;
+            _zeroFlag = false;
+            _target_velocity = Vector3.Zero;
+            m_freemove = true;
+            m_colliderfilter = -2;
+            m_colliderObjectfilter = -2;
+            m_colliderGroundfilter = -2;
+
+            m_iscolliding = false;
+            m_iscollidingGround = false;
+            m_iscollidingObj = false;
+
+            CollisionEventsThisFrame = new CollisionEventUpdate();
+            m_eventsubscription = 0;
+        }
 
         private void changeForce(Vector3 newForce)
         {
-            m_pidControllerActive = false;
+            setFreeMove();
+
             if (Body != IntPtr.Zero)
             {
                 if (newForce.X != 0f || newForce.Y != 0f || newForce.Z != 0)
@@ -1272,8 +1332,8 @@ namespace OpenSim.Region.Physics.OdePlugin
         private void changeMomentum(Vector3 newmomentum)
         {
             _velocity = newmomentum;
-            _target_velocity = newmomentum;
-            m_pidControllerActive = true;
+            setFreeMove();
+
             if (Body != IntPtr.Zero)
                 d.BodySetLinearVel(Body, newmomentum.X, newmomentum.Y, newmomentum.Z);
         }
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs
index dcd02e2..d106677 100644
--- a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs
@@ -121,6 +121,9 @@ namespace OpenSim.Region.Physics.OdePlugin
         private float m_amEfect = 0;                                            // current angular motor eficiency
         private float m_ffactor = 1.0f;
 
+        private float m_timestep = 0.02f;
+        private float m_invtimestep = 50;
+
         public float FrictionFactor
         {
             get
@@ -133,14 +136,12 @@ namespace OpenSim.Region.Physics.OdePlugin
         {
             rootPrim = rootp;
             _pParentScene = rootPrim._parent_scene;
+            m_timestep = _pParentScene.ODE_STEPSIZE;
+            m_invtimestep = 1.0f / m_timestep;
         }
 
         public void DoSetVehicle(VehicleData vd)
         {
-
-            float timestep = _pParentScene.ODE_STEPSIZE;
-            float invtimestep = 1.0f / timestep;
-
             m_type = vd.m_type;
             m_flags = vd.m_flags;
 
@@ -148,61 +149,60 @@ namespace OpenSim.Region.Physics.OdePlugin
             m_linearMotorDirection = vd.m_linearMotorDirection;
 
             m_linearFrictionTimescale = vd.m_linearFrictionTimescale;
-            if (m_linearFrictionTimescale.X < timestep) m_linearFrictionTimescale.X = timestep;
-            if (m_linearFrictionTimescale.Y < timestep) m_linearFrictionTimescale.Y = timestep;
-            if (m_linearFrictionTimescale.Z < timestep) m_linearFrictionTimescale.Z = timestep;
+            if (m_linearFrictionTimescale.X < m_timestep) m_linearFrictionTimescale.X = m_timestep;
+            if (m_linearFrictionTimescale.Y < m_timestep) m_linearFrictionTimescale.Y = m_timestep;
+            if (m_linearFrictionTimescale.Z < m_timestep) m_linearFrictionTimescale.Z = m_timestep;
 
             m_linearMotorDecayTimescale = vd.m_linearMotorDecayTimescale;
-            if (m_linearMotorDecayTimescale < timestep) m_linearMotorDecayTimescale = timestep;
-            m_linearMotorDecayTimescale *= invtimestep;
+            if (m_linearMotorDecayTimescale < m_timestep) m_linearMotorDecayTimescale = m_timestep;
+            m_linearMotorDecayTimescale *= m_invtimestep;
 
             m_linearMotorTimescale = vd.m_linearMotorTimescale;
-            if (m_linearMotorTimescale < timestep) m_linearMotorTimescale = timestep;
-
+            if (m_linearMotorTimescale < m_timestep) m_linearMotorTimescale = m_timestep;
 
             m_linearMotorOffset = vd.m_linearMotorOffset;
 
             //Angular properties
             m_angularMotorDirection = vd.m_angularMotorDirection;
             m_angularMotorTimescale = vd.m_angularMotorTimescale;
-            if (m_angularMotorTimescale < timestep) m_angularMotorTimescale = timestep;
+            if (m_angularMotorTimescale < m_timestep) m_angularMotorTimescale = m_timestep;
 
             m_angularMotorDecayTimescale = vd.m_angularMotorDecayTimescale;
-            if (m_angularMotorDecayTimescale < timestep) m_angularMotorDecayTimescale = timestep;
-            m_angularMotorDecayTimescale *= invtimestep;
+            if (m_angularMotorDecayTimescale < m_timestep) m_angularMotorDecayTimescale = m_timestep;
+            m_angularMotorDecayTimescale *= m_invtimestep;
 
             m_angularFrictionTimescale = vd.m_angularFrictionTimescale;
-            if (m_angularFrictionTimescale.X < timestep) m_angularFrictionTimescale.X = timestep;
-            if (m_angularFrictionTimescale.Y < timestep) m_angularFrictionTimescale.Y = timestep;
-            if (m_angularFrictionTimescale.Z < timestep) m_angularFrictionTimescale.Z = timestep;
+            if (m_angularFrictionTimescale.X < m_timestep) m_angularFrictionTimescale.X = m_timestep;
+            if (m_angularFrictionTimescale.Y < m_timestep) m_angularFrictionTimescale.Y = m_timestep;
+            if (m_angularFrictionTimescale.Z < m_timestep) m_angularFrictionTimescale.Z = m_timestep;
 
             //Deflection properties
             m_angularDeflectionEfficiency = vd.m_angularDeflectionEfficiency;
             m_angularDeflectionTimescale = vd.m_angularDeflectionTimescale;
-            if (m_angularDeflectionTimescale < timestep) m_angularDeflectionTimescale = timestep;
+            if (m_angularDeflectionTimescale < m_timestep) m_angularDeflectionTimescale = m_timestep;
 
             m_linearDeflectionEfficiency = vd.m_linearDeflectionEfficiency;
             m_linearDeflectionTimescale = vd.m_linearDeflectionTimescale;
-            if (m_linearDeflectionTimescale < timestep) m_linearDeflectionTimescale = timestep;
+            if (m_linearDeflectionTimescale < m_timestep) m_linearDeflectionTimescale = m_timestep;
 
             //Banking properties
             m_bankingEfficiency = vd.m_bankingEfficiency;
             m_bankingMix = vd.m_bankingMix;
             m_bankingTimescale = vd.m_bankingTimescale;
-            if (m_bankingTimescale < timestep) m_bankingTimescale = timestep;
+            if (m_bankingTimescale < m_timestep) m_bankingTimescale = m_timestep;
 
             //Hover and Buoyancy properties
             m_VhoverHeight = vd.m_VhoverHeight;
             m_VhoverEfficiency = vd.m_VhoverEfficiency;
             m_VhoverTimescale = vd.m_VhoverTimescale;
-            if (m_VhoverTimescale < timestep) m_VhoverTimescale = timestep;
+            if (m_VhoverTimescale < m_timestep) m_VhoverTimescale = m_timestep;
 
             m_VehicleBuoyancy = vd.m_VehicleBuoyancy;
 
             //Attractor properties
             m_verticalAttractionEfficiency = vd.m_verticalAttractionEfficiency;
             m_verticalAttractionTimescale = vd.m_verticalAttractionTimescale;
-            if (m_verticalAttractionTimescale < timestep) m_verticalAttractionTimescale = timestep;
+            if (m_verticalAttractionTimescale < m_timestep) m_verticalAttractionTimescale = m_timestep;
 
             // Axis
             m_referenceFrame = vd.m_referenceFrame;
@@ -215,8 +215,6 @@ namespace OpenSim.Region.Physics.OdePlugin
         internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue)
         {
             float len;
-            float invtimestep = 1.0f / _pParentScene.ODE_STEPSIZE;
-            float timestep = _pParentScene.ODE_STEPSIZE;
 
             switch (pParam)
             {
@@ -226,18 +224,16 @@ namespace OpenSim.Region.Physics.OdePlugin
                     m_angularDeflectionEfficiency = pValue;
                     break;
                 case Vehicle.ANGULAR_DEFLECTION_TIMESCALE:
-                    if (pValue < timestep) pValue = timestep;
+                    if (pValue < m_timestep) pValue = m_timestep;
                     m_angularDeflectionTimescale = pValue;
                     break;
                 case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE:
-                    if (pValue < timestep) pValue = timestep;
-                    // try to make impulses to work a bit better
-//                    if (pValue < 0.5f) pValue = 0.5f;
+                    if (pValue < m_timestep) pValue = m_timestep;
                     else if (pValue > 120) pValue = 120;
-                    m_angularMotorDecayTimescale = pValue * invtimestep;
+                    m_angularMotorDecayTimescale = pValue * m_invtimestep;
                     break;
                 case Vehicle.ANGULAR_MOTOR_TIMESCALE:
-                    if (pValue < timestep) pValue = timestep;
+                    if (pValue < m_timestep) pValue = m_timestep;
                     m_angularMotorTimescale = pValue;
                     break;
                 case Vehicle.BANKING_EFFICIENCY:
@@ -251,7 +247,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                     m_bankingMix = pValue;
                     break;
                 case Vehicle.BANKING_TIMESCALE:
-                    if (pValue < timestep) pValue = timestep;
+                    if (pValue < m_timestep) pValue = m_timestep;
                     m_bankingTimescale = pValue;
                     break;
                 case Vehicle.BUOYANCY:
@@ -268,7 +264,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                     m_VhoverHeight = pValue;
                     break;
                 case Vehicle.HOVER_TIMESCALE:
-                    if (pValue < timestep) pValue = timestep;
+                    if (pValue < m_timestep) pValue = m_timestep;
                     m_VhoverTimescale = pValue;
                     break;
                 case Vehicle.LINEAR_DEFLECTION_EFFICIENCY:
@@ -277,18 +273,16 @@ namespace OpenSim.Region.Physics.OdePlugin
                     m_linearDeflectionEfficiency = pValue;
                     break;
                 case Vehicle.LINEAR_DEFLECTION_TIMESCALE:
-                    if (pValue < timestep) pValue = timestep;
+                    if (pValue < m_timestep) pValue = m_timestep;
                     m_linearDeflectionTimescale = pValue;
                     break;
                 case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE:
-                    if (pValue < timestep) pValue = timestep;
-                    // try to make impulses to work a bit better
-                    //if (pValue < 0.5f) pValue = 0.5f;
+                    if (pValue < m_timestep) pValue = m_timestep;
                     else if (pValue > 120) pValue = 120;
-                    m_linearMotorDecayTimescale = pValue * invtimestep;
+                    m_linearMotorDecayTimescale = pValue * m_invtimestep;
                     break;
                 case Vehicle.LINEAR_MOTOR_TIMESCALE:
-                    if (pValue < timestep) pValue = timestep;
+                    if (pValue < m_timestep) pValue = m_timestep;
                     m_linearMotorTimescale = pValue;
                     break;
                 case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY:
@@ -297,14 +291,14 @@ namespace OpenSim.Region.Physics.OdePlugin
                     m_verticalAttractionEfficiency = pValue;
                     break;
                 case Vehicle.VERTICAL_ATTRACTION_TIMESCALE:
-                    if (pValue < timestep) pValue = timestep;
+                    if (pValue < m_timestep) pValue = m_timestep;
                     m_verticalAttractionTimescale = pValue;
                     break;
 
                 // These are vector properties but the engine lets you use a single float value to
                 // set all of the components to the same value
                 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
-                    if (pValue < timestep) pValue = timestep;
+                    if (pValue < m_timestep) pValue = m_timestep;
                     m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue);
                     break;
                 case Vehicle.ANGULAR_MOTOR_DIRECTION:
@@ -318,7 +312,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                         d.BodyEnable(rootPrim.Body);
                     break;
                 case Vehicle.LINEAR_FRICTION_TIMESCALE:
-                    if (pValue < timestep) pValue = timestep;
+                    if (pValue < m_timestep) pValue = m_timestep;
                     m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue);
                     break;
                 case Vehicle.LINEAR_MOTOR_DIRECTION:
@@ -344,14 +338,13 @@ namespace OpenSim.Region.Physics.OdePlugin
         internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue)
         {
             float len;
-            float invtimestep = 1.0f / _pParentScene.ODE_STEPSIZE;
-            float timestep = _pParentScene.ODE_STEPSIZE;
+
             switch (pParam)
             {
                 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
-                    if (pValue.X < timestep) pValue.X = timestep;
-                    if (pValue.Y < timestep) pValue.Y = timestep;
-                    if (pValue.Z < timestep) pValue.Z = timestep;
+                    if (pValue.X < m_timestep) pValue.X = m_timestep;
+                    if (pValue.Y < m_timestep) pValue.Y = m_timestep;
+                    if (pValue.Z < m_timestep) pValue.Z = m_timestep;
 
                     m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
                     break;
@@ -367,9 +360,9 @@ namespace OpenSim.Region.Physics.OdePlugin
                         d.BodyEnable(rootPrim.Body);
                     break;
                 case Vehicle.LINEAR_FRICTION_TIMESCALE:
-                    if (pValue.X < timestep) pValue.X = timestep;
-                    if (pValue.Y < timestep) pValue.Y = timestep;
-                    if (pValue.Z < timestep) pValue.Z = timestep;
+                    if (pValue.X < m_timestep) pValue.X = m_timestep;
+                    if (pValue.Y < m_timestep) pValue.Y = m_timestep;
+                    if (pValue.Z < m_timestep) pValue.Z = m_timestep;
                     m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
                     break;
                 case Vehicle.LINEAR_MOTOR_DIRECTION:
@@ -422,7 +415,6 @@ namespace OpenSim.Region.Physics.OdePlugin
 
         internal void ProcessTypeChange(Vehicle pType)
         {
-            float invtimestep = _pParentScene.ODE_STEPSIZE;
             m_lmEfect = 0;
             m_amEfect = 0;
             m_ffactor = 1f;
@@ -444,9 +436,9 @@ namespace OpenSim.Region.Physics.OdePlugin
                     m_linearFrictionTimescale = new Vector3(1000, 1000, 1000);
                     m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
                     m_linearMotorTimescale = 1000;
-                    m_linearMotorDecayTimescale = 120 * invtimestep;
+                    m_linearMotorDecayTimescale = 120 * m_invtimestep;
                     m_angularMotorTimescale = 1000;
-                    m_angularMotorDecayTimescale = 1000  * invtimestep;
+                    m_angularMotorDecayTimescale = 1000  * m_invtimestep;
                     m_VhoverHeight = 0;
                     m_VhoverEfficiency = 1;
                     m_VhoverTimescale = 1000;
@@ -468,9 +460,9 @@ namespace OpenSim.Region.Physics.OdePlugin
                     m_linearFrictionTimescale = new Vector3(30, 1, 1000);
                     m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
                     m_linearMotorTimescale = 1000;
-                    m_linearMotorDecayTimescale = 120 * invtimestep;
+                    m_linearMotorDecayTimescale = 120 * m_invtimestep;
                     m_angularMotorTimescale = 1000;
-                    m_angularMotorDecayTimescale = 120 * invtimestep;
+                    m_angularMotorDecayTimescale = 120 * m_invtimestep;
                     m_VhoverHeight = 0;
                     m_VhoverEfficiency = 1;
                     m_VhoverTimescale = 10;
@@ -491,9 +483,9 @@ namespace OpenSim.Region.Physics.OdePlugin
                     m_linearFrictionTimescale = new Vector3(100, 2, 1000);
                     m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
                     m_linearMotorTimescale = 1;
-                    m_linearMotorDecayTimescale = 60 * invtimestep;
+                    m_linearMotorDecayTimescale = 60 * m_invtimestep;
                     m_angularMotorTimescale = 1;
-                    m_angularMotorDecayTimescale = 0.8f * invtimestep;
+                    m_angularMotorDecayTimescale = 0.8f * m_invtimestep;
                     m_VhoverHeight = 0;
                     m_VhoverEfficiency = 0;
                     m_VhoverTimescale = 1000;
@@ -515,9 +507,9 @@ namespace OpenSim.Region.Physics.OdePlugin
                     m_linearFrictionTimescale = new Vector3(10, 3, 2);
                     m_angularFrictionTimescale = new Vector3(10, 10, 10);
                     m_linearMotorTimescale = 5;
-                    m_linearMotorDecayTimescale = 60 * invtimestep;
+                    m_linearMotorDecayTimescale = 60 * m_invtimestep;
                     m_angularMotorTimescale = 4;
-                    m_angularMotorDecayTimescale = 4 * invtimestep;
+                    m_angularMotorDecayTimescale = 4 * m_invtimestep;
                     m_VhoverHeight = 0;
                     m_VhoverEfficiency = 0.5f;
                     m_VhoverTimescale = 2;
@@ -543,9 +535,9 @@ namespace OpenSim.Region.Physics.OdePlugin
                     m_linearFrictionTimescale = new Vector3(200, 10, 5);
                     m_angularFrictionTimescale = new Vector3(20, 20, 20);
                     m_linearMotorTimescale = 2;
-                    m_linearMotorDecayTimescale = 60 * invtimestep;
+                    m_linearMotorDecayTimescale = 60 * m_invtimestep;
                     m_angularMotorTimescale = 4;
-                    m_angularMotorDecayTimescale = 8 * invtimestep;
+                    m_angularMotorDecayTimescale = 8 * m_invtimestep;
                     m_VhoverHeight = 0;
                     m_VhoverEfficiency = 0.5f;
                     m_VhoverTimescale = 1000;
@@ -571,15 +563,15 @@ namespace OpenSim.Region.Physics.OdePlugin
                     m_linearFrictionTimescale = new Vector3(5, 5, 5);
                     m_angularFrictionTimescale = new Vector3(10, 10, 10);
                     m_linearMotorTimescale = 5;
-                    m_linearMotorDecayTimescale = 60 * invtimestep;
+                    m_linearMotorDecayTimescale = 60 * m_invtimestep;
                     m_angularMotorTimescale = 6;
-                    m_angularMotorDecayTimescale = 10 * invtimestep;
+                    m_angularMotorDecayTimescale = 10 * m_invtimestep;
                     m_VhoverHeight = 5;
                     m_VhoverEfficiency = 0.8f;
                     m_VhoverTimescale = 10;
                     m_VehicleBuoyancy = 1;
                     m_linearDeflectionEfficiency = 0;
-                    m_linearDeflectionTimescale = 5 * invtimestep;
+                    m_linearDeflectionTimescale = 5 * m_invtimestep;
                     m_angularDeflectionEfficiency = 0;
                     m_angularDeflectionTimescale = 5;
                     m_verticalAttractionEfficiency = 0f;
@@ -701,7 +693,7 @@ namespace OpenSim.Region.Physics.OdePlugin
             return ;
         }        
         
-        internal void Step()//float pTimestep)
+        internal void Step()
         {
             IntPtr Body = rootPrim.Body;
 
@@ -780,30 +772,37 @@ namespace OpenSim.Region.Physics.OdePlugin
             {
                 d.Vector3 pos = d.BodyGetPosition(Body);
 
-                // default to global
-                float perr = m_VhoverHeight - pos.Z;;
-                
-                if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0)
-                {
-                    perr += _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y);
-                }
-                else if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) != 0)
-                {
-                    perr += _pParentScene.GetWaterLevel();
-                }
-                else if ((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == 0)
+                float t = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y);
+                float perr;
+
+                // default to global but don't go underground
+                if (t < m_VhoverHeight)
+                    perr = m_VhoverHeight - pos.Z;
+                else
+                    perr = t - pos.Z; ;
+
+                if ((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == 0)
                 {
-                    float t = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y);
-                    float w = _pParentScene.GetWaterLevel();
-                    if (t > w)
+                    if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) != 0)
+                    {
+                        perr += _pParentScene.GetWaterLevel();
+                    }
+                    else if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0)
+                    {
                         perr += t;
+                    }
                     else
-                        perr += w;
+                    {
+                        float w = _pParentScene.GetWaterLevel();
+                        if (t > w)
+                            perr += t;
+                        else
+                            perr += w;
+                    }
                 }
-
                 if ((m_flags & VehicleFlag.HOVER_UP_ONLY) == 0 || perr > 0)
                 {
-                    force.Z += (perr / m_VhoverTimescale / m_VhoverTimescale - curVel.Z * m_VhoverEfficiency) / _pParentScene.ODE_STEPSIZE;
+                    force.Z += (perr / m_VhoverTimescale / m_VhoverTimescale - curVel.Z * m_VhoverEfficiency) / m_timestep;
                     force.Z += _pParentScene.gravityz * (1f - m_VehicleBuoyancy);
                 }
                 else // no buoyancy
@@ -811,7 +810,7 @@ namespace OpenSim.Region.Physics.OdePlugin
             }
             else
             {
-                // default gravity and buoancy
+                // default gravity and Buoyancy
                 force.Z += _pParentScene.gravityz * (1f - m_VehicleBuoyancy);
             }
 
@@ -819,24 +818,31 @@ namespace OpenSim.Region.Physics.OdePlugin
             if (m_linearDeflectionEfficiency > 0)
             {
                 float len = curVel.Length();
-                Vector3 atAxis;
-                atAxis = Xrot(rotq); // where are we pointing to
-                atAxis *= len; // make it same size as world velocity vector
-                tmpV = -atAxis; // oposite direction
-                atAxis -= curVel; // error to one direction
-                len = atAxis.LengthSquared();
-                tmpV -= curVel; // error to oposite
-                float lens = tmpV.LengthSquared();
-                if (len > 0.01 || lens > 0.01) // do nothing if close enougth
+                if (len > 0.01) // if moving
                 {
-                    if (len < lens)
-                        tmpV = atAxis;
 
-                    tmpV *= (m_linearDeflectionEfficiency / m_linearDeflectionTimescale); // error to correct in this timestep
-                    force.X += tmpV.X;
-                    force.Y += tmpV.Y;
-                    if ((m_flags & VehicleFlag.NO_DEFLECTION_UP) == 0)
-                        force.Z += tmpV.Z;
+                    Vector3 atAxis;
+                    atAxis = Xrot(rotq); // where are we pointing to
+                    atAxis *= len; // make it same size as world velocity vector
+
+                    tmpV = -atAxis; // oposite direction
+                    atAxis -= curVel; // error to one direction
+                    len = atAxis.LengthSquared();
+
+                    tmpV -= curVel; // error to oposite
+                    float lens = tmpV.LengthSquared();
+
+                    if (len > 0.01 || lens > 0.01) // do nothing if close enougth
+                    {
+                        if (len < lens)
+                            tmpV = atAxis;
+
+                        tmpV *= (m_linearDeflectionEfficiency / m_linearDeflectionTimescale); // error to correct in this timestep
+                        force.X += tmpV.X;
+                        force.Y += tmpV.Y;
+                        if ((m_flags & VehicleFlag.NO_DEFLECTION_UP) == 0)
+                            force.Z += tmpV.Z;
+                    }
                 }
             }
 
@@ -900,10 +906,10 @@ namespace OpenSim.Region.Physics.OdePlugin
 
                 GetRollPitch(irotq, out roll, out pitch);
 
-                float ftmp = 1.0f / m_verticalAttractionTimescale / m_verticalAttractionTimescale / _pParentScene.ODE_STEPSIZE;
+                float ftmp = 1.0f / m_verticalAttractionTimescale / m_verticalAttractionTimescale * m_invtimestep;
                 float ftmp2;
                 if (m_bankingEfficiency == 0)
-                    ftmp2 = m_verticalAttractionEfficiency / _pParentScene.ODE_STEPSIZE;
+                    ftmp2 = m_verticalAttractionEfficiency * m_invtimestep;
                 else
                     ftmp2 = 0;
 
@@ -985,7 +991,7 @@ namespace OpenSim.Region.Physics.OdePlugin
             if (torque.X != 0 || torque.Y != 0 || torque.Z != 0)
             {
                 torque *= m_referenceFrame; // to object frame
-                dtorque.X = torque.X;
+                dtorque.X = torque.X ;
                 dtorque.Y = torque.Y;
                 dtorque.Z = torque.Z;
 
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/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs
index cf74f14..fa3d33e 100644
--- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs
+++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs
@@ -1020,12 +1020,16 @@ namespace OpenSim.Region.Physics.OdePlugin
 
         private void collision_accounting_events(PhysicsActor p1, PhysicsActor p2, ContactPoint contact)
             {
-            // obj1LocalID = 0;
-            //returncollisions = false;
             obj2LocalID = 0;
-            //ctype = 0;
-            //cStartStop = 0;
-            if (!(p2.SubscribedEvents() || p1.SubscribedEvents()))
+            bool p1events = p1.SubscribedEvents();
+            bool p2events = p2.SubscribedEvents();
+
+            if (p1 is OdePrim && p1.IsVolumeDtc)
+                p2events = false;
+            if (p2 is OdePrim && p2.IsVolumeDtc)
+                p1events = false;
+
+            if (!(p2events || p1events))
                 return;
 
             switch ((ActorTypes)p1.PhysicsActorType)
@@ -1037,7 +1041,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                         case ActorTypes.Agent:
                             cc2 = (OdeCharacter)p2;
                             obj2LocalID = cc2.m_localID;
-                            if (p2.SubscribedEvents())
+                            if (p2events)
                                 cc2.AddCollisionEvent(cc1.m_localID, contact);
                             break;
 
@@ -1046,7 +1050,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                                 {
                                 cp2 = (OdePrim)p2;
                                 obj2LocalID = cp2.m_localID;
-                                if (p2.SubscribedEvents())
+                                if (p2events)
                                     cp2.AddCollisionEvent(cc1.m_localID, contact);
                                 }
                             break;
@@ -1057,7 +1061,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                             obj2LocalID = 0;
                             break;
                         }
-                    if (p1.SubscribedEvents())
+                    if (p1events)
                         {
                         contact.SurfaceNormal = -contact.SurfaceNormal;
                         cc1.AddCollisionEvent(obj2LocalID, contact);
@@ -1078,7 +1082,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                                     {
                                     cc2 = (OdeCharacter)p2;
                                     obj2LocalID = cc2.m_localID;
-                                    if (p2.SubscribedEvents())
+                                    if (p2events)
                                         cc2.AddCollisionEvent(cp1.m_localID, contact);
                                     }
                                 break;
@@ -1088,7 +1092,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                                     {
                                     cp2 = (OdePrim)p2;
                                     obj2LocalID = cp2.m_localID;
-                                    if (p2.SubscribedEvents())
+                                    if (p2events)
                                         cp2.AddCollisionEvent(cp1.m_localID, contact);
                                     }
                                 break;
@@ -1099,7 +1103,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                                 obj2LocalID = 0;
                                 break;
                             }
-                        if (p1.SubscribedEvents())
+                        if (p1events)
                             {
                             contact.SurfaceNormal = -contact.SurfaceNormal;
                             cp1.AddCollisionEvent(obj2LocalID, contact);
@@ -1734,7 +1738,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                     base.TriggerPhysicsBasedRestart();
                 }
 
-                while (step_time >= HalfOdeStep && nodeframes < 10) //limit number of steps so we don't say here for ever
+                while (step_time > HalfOdeStep && nodeframes < 10) //limit number of steps so we don't say here for ever
                 {
                     try
                     {
diff --git a/OpenSim/Region/ScriptEngine/Shared/Api/Implementation/LSL_Api.cs b/OpenSim/Region/ScriptEngine/Shared/Api/Implementation/LSL_Api.cs
index 5a5f2e1..3fe5780 100644
--- a/OpenSim/Region/ScriptEngine/Shared/Api/Implementation/LSL_Api.cs
+++ b/OpenSim/Region/ScriptEngine/Shared/Api/Implementation/LSL_Api.cs
@@ -5237,7 +5237,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);
         }