3 files changed, 424 insertions, 146 deletions

diff --git a/OpenSim/Region/PhysicsModules/ubOde/ODEPrim.cs b/OpenSim/Region/PhysicsModules/ubOde/ODEPrim.cs
index a2fbf41..9bf71f7 100644
--- a/OpenSim/Region/PhysicsModules/ubOde/ODEPrim.cs
+++ b/OpenSim/Region/PhysicsModules/ubOde/ODEPrim.cs
@@ -85,7 +85,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
         private Vector3 m_lastposition;
         private Vector3 m_rotationalVelocity;
         private Vector3 _size;
-        private Vector3 _acceleration;
+        private Vector3 m_acceleration;
         private IntPtr Amotor;
 
         internal Vector3 m_force;
@@ -109,8 +109,8 @@ namespace OpenSim.Region.PhysicsModule.ubOde
         private float m_waterHeight;
         private float m_buoyancy;                //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle.
 
-        private int body_autodisable_frames;
-        public int bodydisablecontrol = 0;
+        private int m_body_autodisable_frames;
+        public int m_bodydisablecontrol = 0;
         private float m_gravmod = 1.0f;
 
         // Default we're a Geometry
@@ -182,18 +182,21 @@ 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;
         private CollisionEventUpdate CollisionEventsThisFrame = null;
+        private CollisionEventUpdate CollisionVDTCEventsThisFrame = null;
         private bool SentEmptyCollisionsEvent;
 
         public volatile bool childPrim;
@@ -465,6 +468,103 @@ 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
@@ -569,7 +669,10 @@ namespace OpenSim.Region.PhysicsModule.ubOde
             {
                 if (value.IsFinite())
                 {
-                    AddChange(changes.Velocity, value);
+                    if(m_outbounds)
+                        _velocity = value;
+                    else
+                        AddChange(changes.Velocity, value);
                 }
                 else
                 {
@@ -642,8 +745,12 @@ namespace OpenSim.Region.PhysicsModule.ubOde
 
         public override Vector3 Acceleration
         {
-            get { return _acceleration; }
-            set { }
+            get { return m_acceleration; }
+            set
+            {
+                if(m_outbounds)
+                    m_acceleration = value;
+            }
         }
 
         public override Vector3 RotationalVelocity
@@ -663,7 +770,10 @@ namespace OpenSim.Region.PhysicsModule.ubOde
             {
                 if (value.IsFinite())
                 {
-                    AddChange(changes.AngVelocity, value);
+                    if(m_outbounds)
+                        m_rotationalVelocity = value;
+                    else
+                        AddChange(changes.AngVelocity, value);
                 }
                 else
                 {
@@ -837,7 +947,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
         }
         public void SetAcceleration(Vector3 accel)
         {
-            _acceleration = accel;
+            m_acceleration = accel;
         }
 
         public override void AddForce(Vector3 force, bool pushforce)
@@ -873,31 +983,68 @@ namespace OpenSim.Region.PhysicsModule.ubOde
 
         public override void CrossingFailure()
         {
-            if (m_outbounds)
+            lock(_parent_scene.OdeLock)
+            {
+                if (m_outbounds)
+                {
+                    _position.X = Util.Clip(_position.X, 0.5f, _parent_scene.WorldExtents.X - 0.5f);
+                    _position.Y = Util.Clip(_position.Y, 0.5f, _parent_scene.WorldExtents.Y - 0.5f);
+                    _position.Z = Util.Clip(_position.Z + 0.2f, -100f, 50000f);
+
+                    m_lastposition = _position;
+                    _velocity.X = 0;
+                    _velocity.Y = 0;
+                    _velocity.Z = 0;
+
+                    d.AllocateODEDataForThread(0);
+
+                    m_lastVelocity = _velocity;
+                    if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE)
+                        m_vehicle.Stop();
+
+                    if(Body != IntPtr.Zero)
+                        d.BodySetLinearVel(Body, 0, 0, 0); // stop it
+                    if (prim_geom != IntPtr.Zero)
+                        d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
+
+                    m_outbounds = false;
+                    changeDisable(false);
+                    base.RequestPhysicsterseUpdate();
+                }
+            }
+        }
+
+        public override void CrossingStart()
+        {
+            lock(_parent_scene.OdeLock)
             {
-                _position.X = Util.Clip(_position.X, 0.5f, _parent_scene.WorldExtents.X - 0.5f);
-                _position.Y = Util.Clip(_position.Y, 0.5f, _parent_scene.WorldExtents.Y - 0.5f);
-                _position.Z = Util.Clip(_position.Z + 0.2f, -100f, 50000f);
+                if (m_outbounds || childPrim)
+                    return;
+
+                m_outbounds = true;
 
                 m_lastposition = _position;
-                _velocity.X = 0;
-                _velocity.Y = 0;
-                _velocity.Z = 0;
+                m_lastorientation = _orientation;
 
                 d.AllocateODEDataForThread(0);
+                if(Body != IntPtr.Zero)
+                {
+                    d.Vector3 dtmp = d.BodyGetAngularVel(Body);
+                    m_rotationalVelocity.X = dtmp.X;
+                    m_rotationalVelocity.Y = dtmp.Y;
+                    m_rotationalVelocity.Z = dtmp.Z;
 
-                m_lastVelocity = _velocity;
-                if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE)
-                    m_vehicle.Stop();
+                    dtmp = d.BodyGetLinearVel(Body);
+                    _velocity.X = dtmp.X;
+                    _velocity.Y = dtmp.Y;
+                    _velocity.Z = dtmp.Z;
 
-                if(Body != IntPtr.Zero)
                     d.BodySetLinearVel(Body, 0, 0, 0); // stop it
-                if (prim_geom != IntPtr.Zero)
-                    d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
-
-                m_outbounds = false;
-                changeDisable(false);
-                base.RequestPhysicsterseUpdate();
+                    d.BodySetAngularVel(Body, 0, 0, 0);
+                }
+                d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
+                disableBodySoft(); // stop collisions
+                UnSubscribeEvents();
             }
         }
 
@@ -920,8 +1067,10 @@ namespace OpenSim.Region.PhysicsModule.ubOde
             }
             set
             {
+                float old = m_density;
                 m_density = value / 100f;
-                // for not prim mass is not updated since this implies full rebuild of body inertia TODO
+ //               if(m_density != old)
+ //                   UpdatePrimBodyData();
             }
         }
         public override float GravModifier
@@ -989,11 +1138,18 @@ namespace OpenSim.Region.PhysicsModule.ubOde
             m_cureventsubscription = 0;
             if (CollisionEventsThisFrame == null)
                 CollisionEventsThisFrame = new CollisionEventUpdate();
+            if (CollisionVDTCEventsThisFrame == null)
+                CollisionVDTCEventsThisFrame = new CollisionEventUpdate();
             SentEmptyCollisionsEvent = false;
         }
 
         public override void UnSubscribeEvents()
         {
+            if (CollisionVDTCEventsThisFrame != null)
+            {
+                CollisionVDTCEventsThisFrame.Clear();
+                CollisionVDTCEventsThisFrame = null;
+            }
             if (CollisionEventsThisFrame != null)
             {
                 CollisionEventsThisFrame.Clear();
@@ -1012,37 +1168,67 @@ namespace OpenSim.Region.PhysicsModule.ubOde
             _parent_scene.AddCollisionEventReporting(this);
         }
 
+        public override void AddVDTCCollisionEvent(uint CollidedWith, ContactPoint contact)
+        {
+            if (CollisionVDTCEventsThisFrame == null)
+                CollisionVDTCEventsThisFrame = new CollisionEventUpdate();
+
+            CollisionVDTCEventsThisFrame.AddCollider(CollidedWith, contact);
+            _parent_scene.AddCollisionEventReporting(this);
+        }
+
         internal void SleeperAddCollisionEvents()
         {
-            if (CollisionEventsThisFrame == null)
-                return;
-            if(CollisionEventsThisFrame.m_objCollisionList.Count == 0)
-                return;
-            foreach(KeyValuePair<uint,ContactPoint> kvp in CollisionEventsThisFrame.m_objCollisionList)
+            if(CollisionEventsThisFrame != null && CollisionEventsThisFrame.m_objCollisionList.Count != 0)
+            {
+                foreach(KeyValuePair<uint,ContactPoint> kvp in CollisionEventsThisFrame.m_objCollisionList)
+                {
+                    if(kvp.Key == 0)
+                        continue;
+                    OdePrim other = _parent_scene.getPrim(kvp.Key);
+                    if(other == null)
+                        continue;
+                    ContactPoint cp = kvp.Value;
+                    cp.SurfaceNormal = - cp.SurfaceNormal;
+                    cp.RelativeSpeed = -cp.RelativeSpeed;
+                    other.AddCollisionEvent(ParentActor.LocalID,cp);
+                }
+            }
+            if(CollisionVDTCEventsThisFrame != null && CollisionVDTCEventsThisFrame.m_objCollisionList.Count != 0)
             {
-                OdePrim other = _parent_scene.getPrim(kvp.Key);
-                if(other == null)
-                    continue;
-                ContactPoint cp = kvp.Value;
-                cp.SurfaceNormal = - cp.SurfaceNormal;
-                cp.RelativeSpeed = -cp.RelativeSpeed;
-                other.AddCollisionEvent(ParentActor.LocalID,cp);
+                foreach(KeyValuePair<uint,ContactPoint> kvp in CollisionVDTCEventsThisFrame.m_objCollisionList)
+                {
+                    OdePrim other = _parent_scene.getPrim(kvp.Key);
+                    if(other == null)
+                        continue;
+                    ContactPoint cp = kvp.Value;
+                    cp.SurfaceNormal = - cp.SurfaceNormal;
+                    cp.RelativeSpeed = -cp.RelativeSpeed;
+                    other.AddCollisionEvent(ParentActor.LocalID,cp);
+                }
             }
         }
 
+        internal void clearSleeperCollisions()
+        {
+            if(CollisionVDTCEventsThisFrame != null && CollisionVDTCEventsThisFrame.Count >0 )
+                CollisionVDTCEventsThisFrame.Clear();
+        }
+
         public void SendCollisions(int timestep)
         {
             if (m_cureventsubscription < 50000)
                 m_cureventsubscription += timestep;
 
+
+            if (m_cureventsubscription < m_eventsubscription)
+                return;
+
             if (CollisionEventsThisFrame == null)
                 return;
 
             int ncolisions = CollisionEventsThisFrame.m_objCollisionList.Count;
 
-            if (m_cureventsubscription < m_eventsubscription)
-                return;
-
             if (!SentEmptyCollisionsEvent || ncolisions > 0)
             {
                 base.SendCollisionUpdate(CollisionEventsThisFrame);
@@ -1091,7 +1277,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
             m_invTimeStep = 1f / m_timeStep;
 
             m_density = parent_scene.geomDefaultDensity;
-            body_autodisable_frames = parent_scene.bodyFramesAutoDisable;
+            m_body_autodisable_frames = parent_scene.bodyFramesAutoDisable;
 
             prim_geom = IntPtr.Zero;
             collide_geom = IntPtr.Zero;
@@ -1714,26 +1900,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)
             {
@@ -1756,27 +1945,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)
@@ -1789,6 +1963,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
+                        }
                     }
                 }
             }
@@ -1797,25 +1989,66 @@ 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);
 
             d.BodySetAutoDisableFlag(Body, true);
-            d.BodySetAutoDisableSteps(Body, body_autodisable_frames);
+            d.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
             d.BodySetAutoDisableAngularThreshold(Body, 0.05f);
             d.BodySetAutoDisableLinearThreshold(Body, 0.05f);
             d.BodySetDamping(Body, .004f, .001f);
@@ -1909,8 +2142,9 @@ namespace OpenSim.Region.PhysicsModule.ubOde
             {
                 d.BodySetAngularVel(Body, m_rotationalVelocity.X, m_rotationalVelocity.Y, m_rotationalVelocity.Z);
                 d.BodySetLinearVel(Body, _velocity.X, _velocity.Y, _velocity.Z);
+
                 _zeroFlag = false;
-                bodydisablecontrol = 0;
+                m_bodydisablecontrol = 0;
             }
             _parent_scene.addActiveGroups(this);
         }
@@ -1988,7 +2222,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;
                         }
                     }
@@ -2002,7 +2236,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
                 }
                 Body = IntPtr.Zero;
             }
-            _mass = primMass;
+            m_mass = primMass;
             m_collisionscore = 0;
         }
 
@@ -2079,7 +2313,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)
@@ -2143,7 +2377,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)
@@ -2209,7 +2443,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;
         }
 
 
@@ -2224,7 +2458,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);
 
@@ -2514,7 +2748,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
                 m_angularForceacc = Vector3.Zero;
 //                m_torque = Vector3.Zero;
                 _velocity = Vector3.Zero;
-                _acceleration = Vector3.Zero;
+                m_acceleration = Vector3.Zero;
                 m_rotationalVelocity = Vector3.Zero;
                 _target_velocity = Vector3.Zero;
                 if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE)
@@ -2767,8 +3001,12 @@ namespace OpenSim.Region.PhysicsModule.ubOde
                         myrot.W = newOri.W;
                         d.GeomSetQuaternion(prim_geom, ref myrot);
                         _orientation = newOri;
-                        if (Body != IntPtr.Zero && m_angularlocks != 0)
-                            createAMotor(m_angularlocks);
+                        
+                        if (Body != IntPtr.Zero)
+                        {
+                            if(m_angularlocks != 0)
+                                createAMotor(m_angularlocks);
+                        }
                     }
                     if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
                     {
@@ -3064,7 +3302,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
 
         private void changeSetTorque(Vector3 newtorque)
         {
-            if (!m_isSelected)
+            if (!m_isSelected && !m_outbounds)
             {
                 if (m_isphysical && Body != IntPtr.Zero)
                 {
@@ -3081,14 +3319,14 @@ namespace OpenSim.Region.PhysicsModule.ubOde
         private void changeForce(Vector3 force)
         {
             m_force = force;
-            if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
+            if (!m_isSelected && !m_outbounds && Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
                 d.BodyEnable(Body);
         }
 
         private void changeAddForce(Vector3 theforce)
         {
             m_forceacc += theforce;
-            if (!m_isSelected)
+            if (!m_isSelected && !m_outbounds)
             {
                 lock (this)
                 {
@@ -3109,7 +3347,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
         private void changeAddAngularImpulse(Vector3 aimpulse)
         {
             m_angularForceacc += aimpulse * m_invTimeStep;
-            if (!m_isSelected)
+            if (!m_isSelected && !m_outbounds)
             {
                 lock (this)
                 {
@@ -3134,7 +3372,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
                 newVel *= len;
             }
 
-            if (!m_isSelected)
+            if (!m_isSelected && !m_outbounds)
             {
                 if (Body != IntPtr.Zero)
                 {
@@ -3142,7 +3380,6 @@ namespace OpenSim.Region.PhysicsModule.ubOde
                         enableBodySoft();
                     else if (!d.BodyIsEnabled(Body))
                         d.BodyEnable(Body);
-
                     d.BodySetLinearVel(Body, newVel.X, newVel.Y, newVel.Z);
                 }
                 //resetCollisionAccounting();
@@ -3159,7 +3396,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
                 newAngVel *= len;
             }
 
-            if (!m_isSelected)
+            if (!m_isSelected && !m_outbounds)
             {
                 if (Body != IntPtr.Zero)
                 {
@@ -3167,8 +3404,6 @@ namespace OpenSim.Region.PhysicsModule.ubOde
                         enableBodySoft();
                     else if (!d.BodyIsEnabled(Body))
                         d.BodyEnable(Body);
-
-
                     d.BodySetAngularVel(Body, newAngVel.X, newAngVel.Y, newAngVel.Z);
                 }
                 //resetCollisionAccounting();
@@ -3304,6 +3539,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()
@@ -3317,7 +3561,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
                     if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE)
                         return;
 
-                    if (++bodydisablecontrol < 50)
+                    if (++m_bodydisablecontrol < 50)
                         return;
 
                     // clear residuals
@@ -3325,11 +3569,11 @@ namespace OpenSim.Region.PhysicsModule.ubOde
                     d.BodySetLinearVel(Body,0f,0f,0f);
                     _zeroFlag = true;
                     d.BodyEnable(Body);
-                    bodydisablecontrol = -4;
+                    m_bodydisablecontrol = -4;
                 }
 
-                if(bodydisablecontrol < 0)
-                    bodydisablecontrol ++;
+                if(m_bodydisablecontrol < 0)
+                    m_bodydisablecontrol ++;
 
                 d.Vector3 lpos = d.GeomGetPosition(prim_geom); // root position that is seem by rest of simulator
 
@@ -3344,7 +3588,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)
                 {
@@ -3451,9 +3695,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;
@@ -3498,7 +3742,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
             {
                 bool bodyenabled = d.BodyIsEnabled(Body);
 
-                if(bodydisablecontrol < 0)
+                if(m_bodydisablecontrol < 0)
                     return;
 
                 if (bodyenabled || !_zeroFlag)
@@ -3513,9 +3757,9 @@ namespace OpenSim.Region.PhysicsModule.ubOde
                         m_outbounds = true;
 
                         lpos.Z = Util.Clip(lpos.Z, -100f, 100000f);
-                        _acceleration.X = 0;
-                        _acceleration.Y = 0;
-                        _acceleration.Z = 0;
+                        m_acceleration.X = 0;
+                        m_acceleration.Y = 0;
+                        m_acceleration.Z = 0;
 
                         _velocity.X = 0;
                         _velocity.Y = 0;
@@ -3638,19 +3882,19 @@ namespace OpenSim.Region.PhysicsModule.ubOde
                         _orientation.W = ori.W;
                     }
 
-                    // update velocities and aceleration
+                    // update velocities and acceleration
                     if (_zeroFlag || lastZeroFlag)
                     {
                          // disable interpolators
                         _velocity = Vector3.Zero;
-                        _acceleration = Vector3.Zero;
+                        m_acceleration = Vector3.Zero;
                          m_rotationalVelocity = Vector3.Zero;
                     }
                     else
                     {
                         d.Vector3 vel = d.BodyGetLinearVel(Body);
 
-                        _acceleration = _velocity;
+                        m_acceleration = _velocity;
 
                         if ((Math.Abs(vel.X) < 0.005f) &&
                             (Math.Abs(vel.Y) < 0.005f) &&
@@ -3658,28 +3902,28 @@ namespace OpenSim.Region.PhysicsModule.ubOde
                         {
                             _velocity = Vector3.Zero;
                             float t = -m_invTimeStep;
-                            _acceleration = _acceleration * t;
+                            m_acceleration = m_acceleration * t;
                         }
                         else
                         {
                             _velocity.X = vel.X;
                             _velocity.Y = vel.Y;
                             _velocity.Z = vel.Z;
-                            _acceleration = (_velocity - _acceleration) * m_invTimeStep;
+                            m_acceleration = (_velocity - m_acceleration) * m_invTimeStep;
                         }
 
-                        if ((Math.Abs(_acceleration.X) < 0.01f) &&
-                            (Math.Abs(_acceleration.Y) < 0.01f) &&
-                            (Math.Abs(_acceleration.Z) < 0.01f))
+                        if ((Math.Abs(m_acceleration.X) < 0.01f) &&
+                            (Math.Abs(m_acceleration.Y) < 0.01f) &&
+                            (Math.Abs(m_acceleration.Z) < 0.01f))
                         {
-                            _acceleration = Vector3.Zero;
+                            m_acceleration = Vector3.Zero;
                         }
 
                         vel = d.BodyGetAngularVel(Body);
                         if ((Math.Abs(vel.X) < 0.0001) &&
-                           (Math.Abs(vel.Y) < 0.0001) &&
-                           (Math.Abs(vel.Z) < 0.0001)
-                           )
+                            (Math.Abs(vel.Y) < 0.0001) &&
+                            (Math.Abs(vel.Z) < 0.0001)
+                            )
                         {
                             m_rotationalVelocity = Vector3.Zero;
                         }
@@ -3939,6 +4183,10 @@ namespace OpenSim.Region.PhysicsModule.ubOde
                     changePIDHoverActive((bool)arg);
                     break;
 
+                case changes.SetInertia:
+                    changeInertia((PhysicsInertiaData) arg);
+                    break;
+
                 case changes.Null:
                     donullchange();
                     break;
@@ -3955,7 +4203,6 @@ namespace OpenSim.Region.PhysicsModule.ubOde
             _parent_scene.AddChange((PhysicsActor) this, what, arg);
         }
 
-
         private struct strVehicleBoolParam
         {
             public int param;
diff --git a/OpenSim/Region/PhysicsModules/ubOde/ODEScene.cs b/OpenSim/Region/PhysicsModules/ubOde/ODEScene.cs
index bed66cc..86d41ea 100644
--- a/OpenSim/Region/PhysicsModules/ubOde/ODEScene.cs
+++ b/OpenSim/Region/PhysicsModules/ubOde/ODEScene.cs
@@ -155,6 +155,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
         VehicleRotationParam,
         VehicleFlags,
         SetVehicle,
+        SetInertia,
 
         Null             //keep this last used do dim the methods array. does nothing but pulsing the prim
     }
@@ -185,7 +186,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
 
         float frictionMovementMult = 0.8f;
 
-        float TerrainBounce = 0.1f;
+        float TerrainBounce = 0.001f;
         float TerrainFriction = 0.3f;
 
         public float AvatarFriction = 0;// 0.9f * 0.5f;
@@ -502,7 +503,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
 
             d.WorldSetGravity(world, gravityx, gravityy, gravityz);
 
-            d.WorldSetLinearDamping(world, 0.002f);
+            d.WorldSetLinearDamping(world, 0.001f);
             d.WorldSetAngularDamping(world, 0.002f);
             d.WorldSetAngularDampingThreshold(world, 0f);
             d.WorldSetLinearDampingThreshold(world, 0f);
@@ -528,6 +529,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
             SharedTmpcontact.surface.mode = comumContactFlags;
             SharedTmpcontact.surface.mu = 0;
             SharedTmpcontact.surface.bounce = 0;
+            SharedTmpcontact.surface.bounce_vel = 1.5f;
             SharedTmpcontact.surface.soft_cfm = comumContactCFM;
             SharedTmpcontact.surface.soft_erp = comumContactERP;
             SharedTmpcontact.surface.slip1 = comumContactSLIP;
@@ -726,8 +728,8 @@ namespace OpenSim.Region.PhysicsModule.ubOde
                 if (g1 == g2)
                     return; // Can't collide with yourself
 
-                if (b1 != IntPtr.Zero && b2 != IntPtr.Zero && d.AreConnectedExcluding(b1, b2, d.JointType.Contact))
-                    return;
+//                if (b1 != IntPtr.Zero && b2 != IntPtr.Zero && d.AreConnectedExcluding(b1, b2, d.JointType.Contact))
+//                    return;
                 /*
                 // debug
                                 PhysicsActor dp2;
@@ -1082,9 +1084,12 @@ namespace OpenSim.Region.PhysicsModule.ubOde
                         case ActorTypes.Prim:
                             if (p2events)
                             {
-                                AddCollisionEventReporting(p2);
+                                //AddCollisionEventReporting(p2);
                                 p2.AddCollisionEvent(p1.ParentActor.LocalID, contact);
                             }
+                            else if(p1.IsVolumeDtc)
+                                p2.AddVDTCCollisionEvent(p1.ParentActor.LocalID, contact);
+
                             obj2LocalID = p2.ParentActor.LocalID;
                             break;
 
@@ -1098,9 +1103,16 @@ namespace OpenSim.Region.PhysicsModule.ubOde
                     {
                         contact.SurfaceNormal = -contact.SurfaceNormal;
                         contact.RelativeSpeed = -contact.RelativeSpeed;
-                        AddCollisionEventReporting(p1);
+                        //AddCollisionEventReporting(p1);
                         p1.AddCollisionEvent(obj2LocalID, contact);
                     }
+                    else if(p2.IsVolumeDtc)
+                    {
+                        contact.SurfaceNormal = -contact.SurfaceNormal;
+                        contact.RelativeSpeed = -contact.RelativeSpeed;
+                        //AddCollisionEventReporting(p1);
+                        p1.AddVDTCCollisionEvent(obj2LocalID, contact);
+                    }
                     break;
                 }
                 case ActorTypes.Ground:
@@ -1109,7 +1121,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
                 {
                     if (p2events && !p2.IsVolumeDtc)
                     {
-                        AddCollisionEventReporting(p2);
+                        //AddCollisionEventReporting(p2);
                         p2.AddCollisionEvent(0, contact);
                     }
                     break;
@@ -1161,8 +1173,11 @@ namespace OpenSim.Region.PhysicsModule.ubOde
                 {
                     aprim.CollisionScore = 0;
                     aprim.IsColliding = false;
+                    if(!aprim.m_outbounds && d.BodyIsEnabled(aprim.Body))
+                        aprim.clearSleeperCollisions();
                 }
             }
+
             lock (_activegroups)
             {
                 try
@@ -1657,11 +1672,15 @@ namespace OpenSim.Region.PhysicsModule.ubOde
 
 //                d.WorldSetQuickStepNumIterations(world, curphysiteractions);
 
-                int loopstartMS = Util.EnvironmentTickCount();
-                int looptimeMS = 0;
-                int changestimeMS = 0;
-                int maxChangestime = (int)(reqTimeStep * 500f); // half the time
-                int maxLoopTime = (int)(reqTimeStep * 1200f); // 1.2 the time
+                double loopstartMS = Util.GetTimeStampMS();
+                double looptimeMS = 0;
+                double changestimeMS = 0;
+                double maxChangestime = (int)(reqTimeStep * 500f); // half the time
+                double maxLoopTime = (int)(reqTimeStep * 1200f); // 1.2 the time
+
+//                double collisionTime = 0;
+//                double qstepTIme = 0;
+//                double tmpTime = 0;
 
                 d.AllocateODEDataForThread(~0U);
 
@@ -1684,7 +1703,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
                                     item.actor.Name, item.what.ToString());
                             }
                         }
-                        changestimeMS = Util.EnvironmentTickCountSubtract(loopstartMS);
+                        changestimeMS = Util.GetTimeStampMS() - loopstartMS;
                         if (changestimeMS > maxChangestime)
                             break;
                     }
@@ -1729,9 +1748,19 @@ namespace OpenSim.Region.PhysicsModule.ubOde
 
                         m_rayCastManager.ProcessQueuedRequests();
 
+//                        tmpTime =  Util.GetTimeStampMS();
                         collision_optimized();
-                        List<OdePrim> sleepers = new List<OdePrim>();
+//                        collisionTime += Util.GetTimeStampMS() - tmpTime;
+
+                        lock(_collisionEventPrimRemove)
+                        {
+                            foreach (PhysicsActor obj in _collisionEventPrimRemove)
+                                _collisionEventPrim.Remove(obj);
 
+                            _collisionEventPrimRemove.Clear();
+                        }
+
+                        List<OdePrim> sleepers = new List<OdePrim>();
                         foreach (PhysicsActor obj in _collisionEventPrim)
                         {
                             if (obj == null)
@@ -1761,18 +1790,12 @@ namespace OpenSim.Region.PhysicsModule.ubOde
                         foreach(OdePrim prm in sleepers)
                             prm.SleeperAddCollisionEvents();
                         sleepers.Clear();
-
-                        lock(_collisionEventPrimRemove)
-                        {
-                            foreach (PhysicsActor obj in _collisionEventPrimRemove)
-                                _collisionEventPrim.Remove(obj);
-
-                            _collisionEventPrimRemove.Clear();
-                        }
-
+ 
                         // do a ode simulation step
+//                        tmpTime =  Util.GetTimeStampMS();
                         d.WorldQuickStep(world, ODE_STEPSIZE);
                         d.JointGroupEmpty(contactgroup);
+//                        qstepTIme += Util.GetTimeStampMS() - tmpTime;
 
                         // update managed ideia of physical data and do updates to core
         /*
@@ -1813,7 +1836,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
                     step_time -= ODE_STEPSIZE;
                     nodeframes++;
 
-                    looptimeMS = Util.EnvironmentTickCountSubtract(loopstartMS);
+                    looptimeMS = Util.GetTimeStampMS() - loopstartMS;
                     if (looptimeMS > maxLoopTime)
                         break;
                 }
@@ -1882,6 +1905,14 @@ namespace OpenSim.Region.PhysicsModule.ubOde
                 int nbodies = d.NTotalBodies;
                 int ngeoms = d.NTotalGeoms;
 */
+/*
+                looptimeMS /= nodeframes;
+                if(looptimeMS > 0.080)
+                {
+                    collisionTime /= nodeframes;
+                    qstepTIme /= nodeframes;    
+                }
+*/
                 // Finished with all sim stepping. If requested, dump world state to file for debugging.
                 // TODO: This call to the export function is already inside lock (OdeLock) - but is an extra lock needed?
                 // TODO: This overwrites all dump files in-place. Should this be a growing logfile, or separate snapshots?
diff --git a/OpenSim/Region/PhysicsModules/ubOdeMeshing/Meshmerizer.cs b/OpenSim/Region/PhysicsModules/ubOdeMeshing/Meshmerizer.cs
index 163f439..0117800 100644
--- a/OpenSim/Region/PhysicsModules/ubOdeMeshing/Meshmerizer.cs
+++ b/OpenSim/Region/PhysicsModules/ubOdeMeshing/Meshmerizer.cs
@@ -454,7 +454,7 @@ namespace OpenSim.Region.PhysicsModule.ubODEMeshing
 
                 if (physicsParms == null)
                 {
-                    m_log.WarnFormat("[MESH]: unknown mesh type for prim {0}",primName);
+                    //m_log.WarnFormat("[MESH]: unknown mesh type for prim {0}",primName);
                     return false;
                 }
 
@@ -712,7 +712,7 @@ namespace OpenSim.Region.PhysicsModule.ubODEMeshing
                         else
                         {
                             // if neither mesh or decomposition present, warn and use convex
-                            m_log.WarnFormat("[MESH]: Data for PRIM shape type ( mesh or decomposition) not found for prim {0}",primName);
+                            //m_log.WarnFormat("[MESH]: Data for PRIM shape type ( mesh or decomposition) not found for prim {0}",primName);
                         }
                     }
                     vs.Clear();