3 files changed, 97 insertions, 11 deletions

diff --git a/OpenSim/Region/Physics/Manager/PhysicsVector.cs b/OpenSim/Region/Physics/Manager/PhysicsVector.cs
index 1f5d0dc..4b2e756 100644
--- a/OpenSim/Region/Physics/Manager/PhysicsVector.cs
+++ b/OpenSim/Region/Physics/Manager/PhysicsVector.cs
@@ -46,7 +46,12 @@ namespace OpenSim.Region.Physics.Manager
             Y = y;
             Z = z;
         }
-
+        public void setValues(float x, float y, float z)
+        {
+            X = x;
+            Y = y;
+            Z = z;
+        }
         public static readonly PhysicsVector Zero = new PhysicsVector(0f, 0f, 0f);
 
         public override string ToString()
@@ -54,6 +59,56 @@ namespace OpenSim.Region.Physics.Manager
             return "<" + X + "," + Y + "," + Z + ">";
         }
 
+        /// <summary>
+        /// These routines are the easiest way to store XYZ values in an LLVector3 without requiring 3 calls.
+        /// </summary>
+        /// <returns></returns>
+        public byte[] GetBytes()
+        {
+            byte[] byteArray = new byte[12];
+
+            Buffer.BlockCopy(BitConverter.GetBytes(X), 0, byteArray, 0, 4);
+            Buffer.BlockCopy(BitConverter.GetBytes(Y), 0, byteArray, 4, 4);
+            Buffer.BlockCopy(BitConverter.GetBytes(Z), 0, byteArray, 8, 4);
+
+            if (!BitConverter.IsLittleEndian)
+            {
+                Array.Reverse(byteArray, 0, 4);
+                Array.Reverse(byteArray, 4, 4);
+                Array.Reverse(byteArray, 8, 4);
+            }
+
+            return byteArray;
+        }
+
+        public void FromBytes(byte[] byteArray, int pos)
+        {
+            byte[] conversionBuffer = null;
+            if (!BitConverter.IsLittleEndian)
+            {
+                // Big endian architecture
+                if (conversionBuffer == null)
+                    conversionBuffer = new byte[12];
+
+                Buffer.BlockCopy(byteArray, pos, conversionBuffer, 0, 12);
+
+                Array.Reverse(conversionBuffer, 0, 4);
+                Array.Reverse(conversionBuffer, 4, 4);
+                Array.Reverse(conversionBuffer, 8, 4);
+
+                X = BitConverter.ToSingle(conversionBuffer, 0);
+                Y = BitConverter.ToSingle(conversionBuffer, 4);
+                Z = BitConverter.ToSingle(conversionBuffer, 8);
+            }
+            else
+            {
+                // Little endian architecture
+                X = BitConverter.ToSingle(byteArray, pos);
+                Y = BitConverter.ToSingle(byteArray, pos + 4);
+                Z = BitConverter.ToSingle(byteArray, pos + 8);
+            }
+        }
+
         // Operations
         public static PhysicsVector operator +(PhysicsVector a, PhysicsVector b)
         {
diff --git a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
index 12d7694..819d823 100644
--- a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
+++ b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
@@ -53,6 +53,7 @@ namespace OpenSim.Region.Physics.OdePlugin
         private bool m_taintshape = false;
         private bool m_taintPhysics = false;
         public bool m_taintremove = false;
+        public bool m_taintdisable = false;
 
         private bool m_taintforce = false;
         private List<PhysicsVector> m_forcelist = new List<PhysicsVector>();
@@ -451,6 +452,9 @@ namespace OpenSim.Region.Physics.OdePlugin
 
             if (m_taintforce)
                 changeAddForce(timestep);
+
+            if (m_taintdisable)
+                changedisable(timestep);
         }
 
         public void Move(float timestep)
@@ -494,6 +498,13 @@ namespace OpenSim.Region.Physics.OdePlugin
 
             m_taintrot = _orientation;
         }
+        public void changedisable(float timestep)
+        {
+            if (Body != (IntPtr) 0)
+                d.BodyDisable(Body);
+
+            m_taintdisable = false;
+        }
 
         public void changePhysicsStatus(float timestap)
         {
@@ -862,7 +873,16 @@ namespace OpenSim.Region.Physics.OdePlugin
 
         public override PhysicsVector RotationalVelocity
         {
-            get { return m_rotationalVelocity; }
+            get {
+                if (_zeroFlag)
+                    return PhysicsVector.Zero;
+                m_lastUpdateSent = false;
+                
+                if (m_rotationalVelocity.IsIdentical(PhysicsVector.Zero, 0.2f))
+                    return PhysicsVector.Zero;
+
+                return m_rotationalVelocity; 
+            }
             set { m_rotationalVelocity = value; }
         }
 
@@ -917,6 +937,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                     && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02))
                 {
                     _zeroFlag = true;
+                    m_throttleUpdates = false;
                 }
                 else
                 {
@@ -944,9 +965,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                     {
                         m_throttleUpdates = false;
                         throttleCounter = 0;
-                        m_rotationalVelocity.X = 0;
-                        m_rotationalVelocity.Y = 0;
-                        m_rotationalVelocity.Z = 0;
+                        m_rotationalVelocity = PhysicsVector.Zero;
                         base.RequestPhysicsterseUpdate();
                         m_lastUpdateSent = true;
                     }
@@ -960,10 +979,15 @@ namespace OpenSim.Region.Physics.OdePlugin
                     _velocity.X = vel.X;
                     _velocity.Y = vel.Y;
                     _velocity.Z = vel.Z;
-
-                    m_rotationalVelocity.X = rotvel.X;
-                    m_rotationalVelocity.Y = rotvel.Y;
-                    m_rotationalVelocity.Z = rotvel.Z;
+                    if (_velocity.IsIdentical(PhysicsVector.Zero, 0.5f))
+                    {
+                        m_rotationalVelocity = PhysicsVector.Zero;
+                    }
+                    else
+                    {
+                        m_rotationalVelocity.setValues(rotvel.X, rotvel.Y, rotvel.Z);
+                    }
+                    
                     //System.Console.WriteLine("ODE: " + m_rotationalVelocity.ToString());
                     _orientation.w = ori.W;
                     _orientation.x = ori.X;
diff --git a/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs
index f1d8232..1aa141b 100644
--- a/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs
+++ b/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs
@@ -405,8 +405,15 @@ namespace OpenSim.Region.Physics.OdePlugin
                         // If you interpenetrate a prim with another prim
                         if (p1.PhysicsActorType == (int) ActorTypes.Prim && p2.PhysicsActorType == (int) ActorTypes.Prim)
                         {
-                            // Don't collide, one or both prim will explode.
-                            contacts[i].depth = 0f;
+                            if (contacts[i].depth >= 0.25f)
+                            {
+                                // Don't collide, one or both prim will explode.
+                                ((OdePrim)p1).m_taintdisable = true;
+                                AddPhysicsActorTaint(p1);
+                                ((OdePrim)p2).m_taintdisable = true;
+                                AddPhysicsActorTaint(p2);
+                                contacts[i].depth = 0f;
+                            }
                         }
                         if (contacts[i].depth >= 1.00f)
                         {