3 files changed, 392 insertions, 301 deletions

diff --git a/OpenSim/Region/ClientStack/Linden/UDP/LLClientView.cs b/OpenSim/Region/ClientStack/Linden/UDP/LLClientView.cs
index 2efaa79..ea3c6a4 100644
--- a/OpenSim/Region/ClientStack/Linden/UDP/LLClientView.cs
+++ b/OpenSim/Region/ClientStack/Linden/UDP/LLClientView.cs
@@ -331,6 +331,7 @@ namespace OpenSim.Region.ClientStack.LindenUDP
         private Prioritizer m_prioritizer;
         private bool m_disableFacelights = false;
 
+        private bool m_VelocityInterpolate = false;
         private const uint MaxTransferBytesPerPacket = 600;
 
 
@@ -3991,6 +3992,7 @@ namespace OpenSim.Region.ClientStack.LindenUDP
     
             const float TIME_DILATION = 1.0f;
             ushort timeDilation = Utils.FloatToUInt16(avgTimeDilation, 0.0f, 1.0f);
+            
     
             if (terseAgentUpdateBlocks.IsValueCreated)
             {
@@ -4971,7 +4973,7 @@ namespace OpenSim.Region.ClientStack.LindenUDP
                 // in that direction, even though we don't model this on the server.  Implementing this in the future
                 // may improve movement smoothness.
 //                acceleration = new Vector3(1, 0, 0);
-                
+
                 angularVelocity = Vector3.Zero;
 
                 if (sendTexture)
@@ -5309,8 +5311,10 @@ namespace OpenSim.Region.ClientStack.LindenUDP
             // If AgentUpdate is ever handled asynchronously, then we will also need to construct a new AgentUpdateArgs
             // for each AgentUpdate packet.
             AddLocalPacketHandler(PacketType.AgentUpdate, HandleAgentUpdate, false);
-            
+
             AddLocalPacketHandler(PacketType.ViewerEffect, HandleViewerEffect, false);
+            AddLocalPacketHandler(PacketType.VelocityInterpolateOff, HandleVelocityInterpolateOff, false);
+            AddLocalPacketHandler(PacketType.VelocityInterpolateOn, HandleVelocityInterpolateOn, false);
             AddLocalPacketHandler(PacketType.AgentCachedTexture, HandleAgentTextureCached, false);
             AddLocalPacketHandler(PacketType.MultipleObjectUpdate, HandleMultipleObjUpdate, false);
             AddLocalPacketHandler(PacketType.MoneyTransferRequest, HandleMoneyTransferRequest, false);
@@ -5828,6 +5832,29 @@ namespace OpenSim.Region.ClientStack.LindenUDP
             return true;
         }
 
+        private bool HandleVelocityInterpolateOff(IClientAPI sender, Packet Pack)
+        {
+            VelocityInterpolateOffPacket p = (VelocityInterpolateOffPacket)Pack;
+            if (p.AgentData.SessionID != SessionId ||
+                p.AgentData.AgentID != AgentId)
+                return true;
+
+            m_VelocityInterpolate = false;
+            return true;
+        }
+
+        private bool HandleVelocityInterpolateOn(IClientAPI sender, Packet Pack)
+        {
+            VelocityInterpolateOnPacket p = (VelocityInterpolateOnPacket)Pack;
+            if (p.AgentData.SessionID != SessionId ||
+                p.AgentData.AgentID != AgentId)
+                return true;
+
+            m_VelocityInterpolate = true;
+            return true;
+        }
+
+
         private bool HandleAvatarPropertiesRequest(IClientAPI sender, Packet Pack)
         {
             AvatarPropertiesRequestPacket avatarProperties = (AvatarPropertiesRequestPacket)Pack;
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs
index 15bdc57..1b25faf 100644
--- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs
@@ -74,7 +74,6 @@ namespace OpenSim.Region.Physics.OdePlugin
 
         private Vector3 _position;
         private Vector3 _zeroPosition;
-        private bool _zeroFlag = false;
         private Vector3 _velocity;
         private Vector3 _target_velocity;
         private Vector3 _acceleration;
@@ -90,11 +89,15 @@ namespace OpenSim.Region.Physics.OdePlugin
         public float PID_D;
         public float PID_P;
 
+        private float timeStep;
+        private float invtimeStep;
+
         private float m_feetOffset = 0;
         private float feetOff = 0;
         private float feetSZ = 0.5f;
         const float feetScale = 0.8f;
         private float boneOff = 0;
+        private float m_lastVelocitySqr = 0;
 
         public float walkDivisor = 1.3f;
         public float runDivisor = 0.8f;
@@ -103,6 +106,9 @@ namespace OpenSim.Region.Physics.OdePlugin
         private bool m_iscollidingGround = false;
         private bool m_iscollidingObj = false;
         private bool m_alwaysRun = false;
+
+        private bool _zeroFlag = false;
+
         private int m_requestedUpdateFrequency = 0;
         private uint m_localID = 0;
         public bool m_returnCollisions = false;
@@ -120,6 +126,7 @@ namespace OpenSim.Region.Physics.OdePlugin
         int m_colliderfilter = 0;
         int m_colliderGroundfilter = 0;
         int m_colliderObjectfilter = 0;
+        bool m_collisionException = false;
 
         // Default we're a Character
         private CollisionCategories m_collisionCategories = (CollisionCategories.Character);
@@ -132,10 +139,11 @@ namespace OpenSim.Region.Physics.OdePlugin
         // we do land collisions not ode                | CollisionCategories.Land);
         public IntPtr Body = IntPtr.Zero;
         private OdeScene _parent_scene;
-        public IntPtr topbox = IntPtr.Zero;
-        public IntPtr midbox = IntPtr.Zero;
-        public IntPtr feetbox = IntPtr.Zero;
-        public IntPtr bonebox = IntPtr.Zero;
+        private IntPtr topbox = IntPtr.Zero;
+        private IntPtr midbox = IntPtr.Zero;
+        private IntPtr feetbox = IntPtr.Zero;
+        private IntPtr bbox = IntPtr.Zero;
+        public IntPtr collider = IntPtr.Zero;
 
         public IntPtr Amotor = IntPtr.Zero;
 
@@ -143,6 +151,7 @@ namespace OpenSim.Region.Physics.OdePlugin
 
 
 
+
         public int m_eventsubscription = 0;
         private int m_cureventsubscription = 0;
         private CollisionEventUpdate CollisionEventsThisFrame = null;
@@ -160,6 +169,9 @@ namespace OpenSim.Region.Physics.OdePlugin
         {
             m_uuid = UUID.Random();
 
+            timeStep = parent_scene.ODE_STEPSIZE;
+            invtimeStep = 1 / timeStep;
+
             if (pos.IsFinite())
             {
                 if (pos.Z > 99999f)
@@ -208,8 +220,8 @@ namespace OpenSim.Region.Physics.OdePlugin
 
             m_mass = m_density * m_size.X * m_size.Y * m_size.Z; ; // sure we have a default           
 
-            PID_D = basePID_D * m_mass / parent_scene.ODE_STEPSIZE;
-            PID_P = basePID_P * m_mass / parent_scene.ODE_STEPSIZE;
+            PID_D = basePID_D * m_mass * invtimeStep;
+            PID_P = basePID_P * m_mass * invtimeStep;
 
             m_isPhysical = false; // current status: no ODE information exists
 
@@ -292,7 +304,7 @@ namespace OpenSim.Region.Physics.OdePlugin
             set
             {
                 flying = value;
-                //                m_log.DebugFormat("[PHYSICS]: Set OdeCharacter Flying to {0}", flying);
+//                m_log.DebugFormat("[PHYSICS]: Set OdeCharacter Flying to {0}", flying);
             }
         }
 
@@ -336,25 +348,25 @@ namespace OpenSim.Region.Physics.OdePlugin
             get { return m_iscollidingGround; }
             set
             {
-                /*  we now control this
-                                if (value)
-                                    {
-                                    m_colliderGroundfilter += 2;
-                                    if (m_colliderGroundfilter > 2)
-                                        m_colliderGroundfilter = 2;
-                                    }
-                                else
-                                    {
-                                    m_colliderGroundfilter--;
-                                    if (m_colliderGroundfilter < 0)
-                                        m_colliderGroundfilter = 0;
-                                    }
-
-                                if (m_colliderGroundfilter == 0)
-                                    m_iscollidingGround = false;
-                                else
-                                    m_iscollidingGround = true;
-                 */
+/*  we now control this
+                if (value)
+                    {
+                    m_colliderGroundfilter += 2;
+                    if (m_colliderGroundfilter > 2)
+                        m_colliderGroundfilter = 2;
+                    }
+                else
+                    {
+                    m_colliderGroundfilter--;
+                    if (m_colliderGroundfilter < 0)
+                        m_colliderGroundfilter = 0;
+                    }
+
+                if (m_colliderGroundfilter == 0)
+                    m_iscollidingGround = false;
+                else
+                    m_iscollidingGround = true;
+ */
             }
 
         }
@@ -386,7 +398,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                 else
                     m_iscollidingObj = true;
 
-                //            m_iscollidingObj = value;
+//            m_iscollidingObj = value;
 
                 if (m_iscollidingObj)
                     m_pidControllerActive = false;
@@ -634,8 +646,8 @@ namespace OpenSim.Region.Physics.OdePlugin
             get { return m_orientation; }
             set
             {
-                //                fakeori = value;
-                //                givefakeori++;
+//                fakeori = value;
+//                givefakeori++;
 
                 value.Normalize();
                 AddChange(changes.Orientation, value);
@@ -690,6 +702,46 @@ namespace OpenSim.Region.Physics.OdePlugin
                 AddChange(changes.Momentum, momentum);
         }
 
+        private void ajustCollider()
+        {
+            float vq = _velocity.LengthSquared();
+            if (m_lastVelocitySqr != vq)
+            {
+                m_lastVelocitySqr = vq;
+                if (vq > 100.0f)
+                {
+                    Vector3 off = _velocity;
+                    float t = 0.5f * timeStep;
+                    off = off * t;
+                    d.GeomSetOffsetPosition(bbox, off.X, off.Y, off.Z);
+                    off.X = 2.0f * (m_size.X + Math.Abs(off.X));
+                    off.Y = 2.0f * (m_size.Y + Math.Abs(off.Y));
+                    off.Z = m_size.Z + 2.0f * Math.Abs(off.Z);
+                    d.GeomBoxSetLengths(bbox, off.X, off.Y, off.Z);
+
+                    d.GeomSetCategoryBits(bbox, (uint)m_collisionCategories);
+                    d.GeomSetCollideBits(bbox, (uint)m_collisionFlags);
+                    d.GeomSetCategoryBits(topbox, 0);
+                    d.GeomSetCollideBits(topbox, 0);
+                    d.GeomSetCategoryBits(midbox, 0);
+                    d.GeomSetCollideBits(midbox, 0);
+                    d.GeomSetCategoryBits(feetbox, 0);
+                    d.GeomSetCollideBits(feetbox, 0);
+                }
+                else
+                {
+                    d.GeomSetCategoryBits(bbox, 0);
+                    d.GeomSetCollideBits(bbox, 0);
+                    d.GeomSetCategoryBits(topbox, (uint)m_collisionCategories);
+                    d.GeomSetCollideBits(topbox, (uint)m_collisionFlags);
+                    d.GeomSetCategoryBits(midbox, (uint)m_collisionCategories);
+                    d.GeomSetCollideBits(midbox, (uint)m_collisionFlags);
+                    d.GeomSetCategoryBits(feetbox, (uint)m_collisionCategories);
+                    d.GeomSetCollideBits(feetbox, (uint)m_collisionFlags);
+                }
+            }
+        }
+
         private void AvatarGeomAndBodyCreation(float npositionX, float npositionY, float npositionZ)
         {
             // sizes  one day should came from visual parameters
@@ -697,7 +749,6 @@ namespace OpenSim.Region.Physics.OdePlugin
             float sy = m_size.Y;
             float sz = m_size.Z;
 
-
             float topsx = sx * 0.9f;
             float midsx = sx;
             float feetsx = sx * feetScale;
@@ -732,21 +783,17 @@ namespace OpenSim.Region.Physics.OdePlugin
 
             _parent_scene.waitForSpaceUnlock(_parent_scene.CharsSpace);
 
-            feetbox = d.CreateBox(_parent_scene.CharsSpace, feetsx, feetsy, feetsz);
-            d.GeomSetCategoryBits(feetbox, (uint)m_collisionCategories);
-            d.GeomSetCollideBits(feetbox, (uint)m_collisionFlags);
-
-            midbox = d.CreateBox(_parent_scene.CharsSpace, midsx, midsy, midsz);
-            d.GeomSetCategoryBits(midbox, (uint)m_collisionCategories);
-            d.GeomSetCollideBits(midbox, (uint)m_collisionFlags);
+            collider = d.HashSpaceCreate(_parent_scene.CharsSpace);
+            d.HashSpaceSetLevels(collider, -4, 3);
+            d.SpaceSetSublevel(collider, 3);
+            d.SpaceSetCleanup(collider, false);
+            d.GeomSetCategoryBits(collider, (uint)m_collisionCategories);
+            d.GeomSetCollideBits(collider, (uint)m_collisionFlags);
 
-            topbox = d.CreateBox(_parent_scene.CharsSpace, topsx, topsy, topsz);
-            d.GeomSetCategoryBits(topbox, (uint)m_collisionCategories);
-            d.GeomSetCollideBits(topbox, (uint)m_collisionFlags);
-
-            bonebox = d.CreateBox(_parent_scene.CharsSpace, bonesx, bonesy, bonesz);
-            d.GeomSetCategoryBits(bonebox, (uint)m_collisionCategories);
-            d.GeomSetCollideBits(bonebox, (uint)m_collisionFlags);
+            feetbox = d.CreateBox(collider, feetsx, feetsy, feetsz);
+            midbox = d.CreateBox(collider, midsx, midsy, midsz);
+            topbox = d.CreateBox(collider, topsx, topsy, topsz);
+            bbox = d.CreateBox(collider, m_size.X, m_size.Y, m_size.Z);
 
             m_mass = m_density * m_size.X * m_size.Y * m_size.Z;  // update mass
 
@@ -758,9 +805,13 @@ namespace OpenSim.Region.Physics.OdePlugin
             Body = d.BodyCreate(_parent_scene.world);
 
             _zeroFlag = false;
+            m_collisionException = false;
             m_pidControllerActive = true;
             m_freemove = false;
 
+            _velocity = Vector3.Zero;
+            m_lastVelocitySqr = 0;
+
             d.BodySetAutoDisableFlag(Body, false);
             d.BodySetPosition(Body, npositionX, npositionY, npositionZ);
 
@@ -772,12 +823,14 @@ namespace OpenSim.Region.Physics.OdePlugin
             d.GeomSetBody(feetbox, Body);
             d.GeomSetBody(midbox, Body);
             d.GeomSetBody(topbox, Body);
-            d.GeomSetBody(bonebox, Body);
+            d.GeomSetBody(bbox, Body);
 
             d.GeomSetOffsetPosition(feetbox, 0, 0, feetz);
             d.GeomSetOffsetPosition(midbox, 0, 0, midz);
             d.GeomSetOffsetPosition(topbox, 0, 0, topz);
-            d.GeomSetOffsetPosition(bonebox, 0, 0, m_feetOffset);
+
+            ajustCollider();
+
 
             // The purpose of the AMotor here is to keep the avatar's physical
             // surrogate from rotating while moving
@@ -841,44 +894,109 @@ namespace OpenSim.Region.Physics.OdePlugin
             if (topbox != IntPtr.Zero)
             {
                 _parent_scene.actor_name_map.Remove(topbox);
-                _parent_scene.waitForSpaceUnlock(_parent_scene.CharsSpace);
+                _parent_scene.waitForSpaceUnlock(collider);
                 d.GeomDestroy(topbox);
                 topbox = IntPtr.Zero;
             }
             if (midbox != IntPtr.Zero)
             {
                 _parent_scene.actor_name_map.Remove(midbox);
-                _parent_scene.waitForSpaceUnlock(_parent_scene.CharsSpace);
+                _parent_scene.waitForSpaceUnlock(collider);
                 d.GeomDestroy(midbox);
                 midbox = IntPtr.Zero;
             }
             if (feetbox != IntPtr.Zero)
             {
                 _parent_scene.actor_name_map.Remove(feetbox);
-                _parent_scene.waitForSpaceUnlock(_parent_scene.CharsSpace);
+                _parent_scene.waitForSpaceUnlock(collider);
                 d.GeomDestroy(feetbox);
                 feetbox = IntPtr.Zero;
             }
 
-            if (bonebox != IntPtr.Zero)
+            if (bbox != IntPtr.Zero)
+            {
+                _parent_scene.actor_name_map.Remove(bbox);
+                _parent_scene.waitForSpaceUnlock(collider);
+                d.GeomDestroy(bbox);
+                bbox = IntPtr.Zero;
+            }
+
+            if (collider != IntPtr.Zero)
             {
-                _parent_scene.actor_name_map.Remove(bonebox);
-                _parent_scene.waitForSpaceUnlock(_parent_scene.CharsSpace);
-                d.GeomDestroy(bonebox);
-                bonebox = IntPtr.Zero;
+                d.SpaceDestroy(collider);
+                collider = IntPtr.Zero;
             }
 
         }
 
+        //in place 2D rotation around Z assuming rot is normalised and is a rotation around Z
+        public void RotateXYonZ(ref float x, ref float y, ref Quaternion rot)
+        {
+            float sin = 2.0f * rot.Z * rot.W;
+            float cos = rot.W * rot.W - rot.Z * rot.Z;
+            float tx = x;
+
+            x = tx * cos - y * sin;
+            y = tx * sin + y * cos;
+        }
+        public void RotateXYonZ(ref float x, ref float y, ref float sin, ref float cos)
+        {
+            float tx = x;
+            x = tx * cos - y * sin;
+            y = tx * sin + y * cos;
+        }
+        public void invRotateXYonZ(ref float x, ref float y, ref float sin, ref float cos)
+        {
+            float tx = x;
+            x = tx * cos + y * sin;
+            y = -tx * sin + y * cos;
+        }
+
+        public void invRotateXYonZ(ref float x, ref float y, ref Quaternion rot)
+        {
+            float sin = - 2.0f * rot.Z * rot.W;
+            float cos = rot.W * rot.W - rot.Z * rot.Z;
+            float tx = x;
+
+            x = tx * cos - y * sin;
+            y = tx * sin + y * cos;
+        }
+
+
         public bool Collide(IntPtr me, bool reverse, ref d.ContactGeom contact, ref bool feetcollision)
         {
             feetcollision = false;
+            if (m_collisionException)
+                return false;
 
-            if (me == bonebox) // inner bone
+            if (me == bbox) // if moving fast
             {
-                if (contact.pos.Z - _position.Z < boneOff)
-                    IsColliding = true;
-                return true;
+                // force a full inelastic collision
+                m_collisionException = true;
+
+                Vector3 off = m_size * 0.5f;
+                off.X += contact.depth;
+                off.Y += contact.depth;
+                off.Z += contact.depth;
+                if (reverse)
+                {
+                    off.X *= -contact.normal.X;
+                    off.Y *= -contact.normal.Y;
+                    off.Z *= -contact.normal.Z;
+                }
+                else
+                {
+                    off.X *= contact.normal.X;
+                    off.Y *= contact.normal.Y;
+                    off.Z *= contact.normal.Z;
+                }
+
+                off.X += contact.pos.X;
+                off.Y += contact.pos.Y;
+                off.Z += contact.pos.Z;
+
+                _position = off;
+                return false;
             }
 
             if (me == topbox) // keep a box head
@@ -890,6 +1008,21 @@ namespace OpenSim.Region.Physics.OdePlugin
 
             if (me == midbox)
             {
+                if (Math.Abs(contact.normal.Z) > 0.95f)
+                {
+                    float nz = contact.normal.Z;
+                    if (!reverse)
+                        nz = -nz;
+
+                    if (nz > 0)
+                        return true; // missed head TODO
+
+                    // missed feet collision?
+
+
+                    return true;
+                }
+
                 t = offx * offx + offy * offy;
                 t = (float)Math.Sqrt(t);
                 t = 1 / t;
@@ -917,12 +1050,19 @@ namespace OpenSim.Region.Physics.OdePlugin
 
                 if (Math.Abs(contact.normal.Z) > 0.95f)
                 {
+                    if (contact.normal.Z > 0)
+                        contact.normal.Z = 1.0f;
+                    else
+                        contact.normal.Z = -1.0f;
+                    contact.normal.X = 0.0f;
+                    contact.normal.Y = 0.0f;
                     feetcollision = true;
                     if (h < boneOff)
                         IsColliding = true;
                     return true;
                 }
 
+
                 float offz = h - feetOff; // distance from top of feetbox
 
                 if (offz > 0)
@@ -971,11 +1111,28 @@ namespace OpenSim.Region.Physics.OdePlugin
         /// This is the avatar's movement control + PID Controller
         /// </summary>
         /// <param name="timeStep"></param>
-        public void Move(float timeStep, List<OdeCharacter> defects)
+        public void Move(List<OdeCharacter> defects)
         {
             if (Body == IntPtr.Zero)
                 return;
 
+            if (m_collisionException)
+            {
+                d.BodySetPosition(Body,_position.X, _position.Y, _position.Z);
+                d.BodySetLinearVel(Body, 0, 0, 0);
+
+                float v = _velocity.Length();
+                if (v != 0)
+                {
+                    v = 6.0f / v;
+                    _velocity = _velocity * v;
+                    d.BodySetLinearVel(Body, _velocity.X, _velocity.Y, _velocity.Z);
+                }
+                ajustCollider();
+                m_collisionException = false;
+                return;
+            }
+
             d.Vector3 dtmp = d.BodyGetPosition(Body);
             Vector3 localpos = new Vector3(dtmp.X, dtmp.Y, dtmp.Z);
 
@@ -1049,6 +1206,7 @@ namespace OpenSim.Region.Physics.OdePlugin
 
             //******************************************
             // colide with land
+
             d.AABB aabb;
             d.GeomGetAABB(feetbox, out aabb);
             float chrminZ = aabb.MinZ - 0.02f; // move up a bit
@@ -1095,8 +1253,8 @@ namespace OpenSim.Region.Physics.OdePlugin
                         contact.Position.X = localpos.X;
                         contact.Position.Y = localpos.Y;
                         contact.Position.Z = chrminZ;
-                        contact.SurfaceNormal.X = 0f;
-                        contact.SurfaceNormal.Y = 0f;
+                        contact.SurfaceNormal.X = 0.0f;
+                        contact.SurfaceNormal.Y = 0.0f;
                         contact.SurfaceNormal.Z = -1f;
                         contact.RelativeSpeed = -vel.Z;
                         contact.CharacterFeet = true;
@@ -1118,6 +1276,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                 m_iscollidingGround = false;
             }
 
+            
             //******************************************
 
             bool tviszero = (_target_velocity.X == 0.0f && _target_velocity.Y == 0.0f && _target_velocity.Z == 0.0f);
@@ -1253,21 +1412,58 @@ namespace OpenSim.Region.Physics.OdePlugin
             }
 
             // update our local ideia of position velocity and aceleration
+            //            _position = localpos;
             _position = localpos;
+
             if (_zeroFlag)
             {
                 _velocity = Vector3.Zero;
                 _acceleration = Vector3.Zero;
+                m_rotationalVelocity = Vector3.Zero;
             }
             else
             {
-                _acceleration = _velocity; // previus velocity
-                _velocity = vel;
-                _acceleration = (vel - _acceleration) / timeStep;
+                Vector3 a =_velocity; // previus velocity
+                SetSmooth(ref _velocity, ref vel, 2);
+                a = (_velocity - a) * invtimeStep;
+                SetSmooth(ref _acceleration, ref a, 2);
+
+                dtmp = d.BodyGetAngularVel(Body);
+                m_rotationalVelocity.X = 0f;
+                m_rotationalVelocity.Y = 0f;
+                m_rotationalVelocity.Z = dtmp.Z;
+                Math.Round(m_rotationalVelocity.Z,3);
             }
-          
+            ajustCollider();
         }
 
+        public void round(ref Vector3 v, int digits)
+        {
+            v.X = (float)Math.Round(v.X, digits);
+            v.Y = (float)Math.Round(v.Y, digits);
+            v.Z = (float)Math.Round(v.Z, digits);
+         }
+
+        public void SetSmooth(ref Vector3 dst, ref Vector3 value)
+        {
+            dst.X = 0.1f * dst.X + 0.9f * value.X;
+            dst.Y = 0.1f * dst.Y + 0.9f * value.Y;
+            dst.Z = 0.1f * dst.Z + 0.9f * value.Z;
+        }
+
+        public void SetSmooth(ref Vector3 dst, ref Vector3 value, int rounddigits)
+        {
+            dst.X = 0.4f * dst.X + 0.6f * value.X;
+            dst.X = (float)Math.Round(dst.X, rounddigits);
+
+            dst.Y = 0.4f * dst.Y + 0.6f * value.Y;
+            dst.Y = (float)Math.Round(dst.Y, rounddigits);
+
+            dst.Z = 0.4f * dst.Z + 0.6f * value.Z;
+            dst.Z = (float)Math.Round(dst.Z, rounddigits);
+        }
+
+
         /// <summary>
         /// Updates the reported position and velocity.
         /// Used to copy variables from unmanaged space at heartbeat rate and also trigger scene updates acording
@@ -1394,10 +1590,11 @@ namespace OpenSim.Region.Physics.OdePlugin
 
                     AvatarGeomAndBodyCreation(_position.X, _position.Y, _position.Z);
 
-                    _parent_scene.actor_name_map[topbox] = (PhysicsActor)this;
-                    _parent_scene.actor_name_map[midbox] = (PhysicsActor)this;
+                    _parent_scene.actor_name_map[collider] = (PhysicsActor)this;
                     _parent_scene.actor_name_map[feetbox] = (PhysicsActor)this;
-                    _parent_scene.actor_name_map[bonebox] = (PhysicsActor)this;
+                    _parent_scene.actor_name_map[midbox] = (PhysicsActor)this;
+                    _parent_scene.actor_name_map[topbox] = (PhysicsActor)this;
+                    _parent_scene.actor_name_map[bbox] = (PhysicsActor)this;
                     _parent_scene.AddCharacter(this);
                 }
                 else
@@ -1450,13 +1647,16 @@ namespace OpenSim.Region.Physics.OdePlugin
                                       _position.Z + (m_size.Z - oldsz) * 0.5f);
 
                     Velocity = Vector3.Zero;
+                    
 
-                    _parent_scene.actor_name_map[topbox] = (PhysicsActor)this;
-                    _parent_scene.actor_name_map[midbox] = (PhysicsActor)this;
+                    _parent_scene.actor_name_map[collider] = (PhysicsActor)this;
                     _parent_scene.actor_name_map[feetbox] = (PhysicsActor)this;
-                    _parent_scene.actor_name_map[bonebox] = (PhysicsActor)this;
+                    _parent_scene.actor_name_map[midbox] = (PhysicsActor)this;
+                    _parent_scene.actor_name_map[topbox] = (PhysicsActor)this;
+                    _parent_scene.actor_name_map[bbox] = (PhysicsActor)this;
                 }
                 m_freemove = false;
+                m_collisionException = false;
                 m_pidControllerActive = true;
             }
             else
@@ -1565,6 +1765,7 @@ namespace OpenSim.Region.Physics.OdePlugin
 
             if (Body != IntPtr.Zero)
                 d.BodySetLinearVel(Body, newmomentum.X, newmomentum.Y, newmomentum.Z);
+            ajustCollider();
         }
 
         private void donullchange()
@@ -1573,7 +1774,7 @@ namespace OpenSim.Region.Physics.OdePlugin
 
         public bool DoAChange(changes what, object arg)
         {
-            if (topbox == IntPtr.Zero && what != changes.Add && what != changes.Remove)
+            if (collider == IntPtr.Zero && what != changes.Add && what != changes.Remove)
             {
                 return false;
             }
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs
index 2ba5940..f8d7195 100644
--- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs
+++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs
@@ -178,7 +178,9 @@ namespace OpenSim.Region.Physics.OdePlugin
         public changes what;
         public Object arg;
     }
-     
+
+    
+
     public class OdeScene : PhysicsScene
     {
         private readonly ILog m_log;
@@ -301,6 +303,8 @@ namespace OpenSim.Region.Physics.OdePlugin
         public IntPtr StaticSpace; // space for the static things around
         public IntPtr GroundSpace; // space for ground
 
+        public IntPtr SharedRay;
+
         // some speedup variables
         private int spaceGridMaxX;
         private int spaceGridMaxY;
@@ -428,6 +432,8 @@ namespace OpenSim.Region.Physics.OdePlugin
                 contactgroup = d.JointGroupCreate(0);
                 //contactgroup
 
+                SharedRay = d.CreateRay(TopSpace, 1.0f);
+
                 d.WorldSetAutoDisableFlag(world, false);
             }
         }
@@ -733,35 +739,35 @@ namespace OpenSim.Region.Physics.OdePlugin
 
                 if (b1 != IntPtr.Zero && b2 != IntPtr.Zero && d.AreConnectedExcluding(b1, b2, d.JointType.Contact))
                     return;
-/*
-// debug
-                PhysicsActor dp2;
-                if (d.GeomGetClass(g1) == d.GeomClassID.HeightfieldClass)
-                {
-                    d.AABB aabb;
-                    d.GeomGetAABB(g2, out aabb);
-                    float x = aabb.MaxX - aabb.MinX;
-                    float y = aabb.MaxY - aabb.MinY;
-                    float z = aabb.MaxZ - aabb.MinZ;
-                    if (x > 60.0f || y > 60.0f || z > 60.0f)
-                    {
-                        if (!actor_name_map.TryGetValue(g2, out dp2))
-                            m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 2");
-                        else
-                            m_log.WarnFormat("[PHYSICS]: land versus large prim geo {0},size {1}, AABBsize <{2},{3},{4}>, at {5} ori {6},({7})",
-                                dp2.Name, dp2.Size, x, y, z,
-                                dp2.Position.ToString(),
-                                dp2.Orientation.ToString(),
-                                dp2.Orientation.Length());
-                        return;
-                    }
-                }
-//
-*/
+                /*
+                // debug
+                                PhysicsActor dp2;
+                                if (d.GeomGetClass(g1) == d.GeomClassID.HeightfieldClass)
+                                {
+                                    d.AABB aabb;
+                                    d.GeomGetAABB(g2, out aabb);
+                                    float x = aabb.MaxX - aabb.MinX;
+                                    float y = aabb.MaxY - aabb.MinY;
+                                    float z = aabb.MaxZ - aabb.MinZ;
+                                    if (x > 60.0f || y > 60.0f || z > 60.0f)
+                                    {
+                                        if (!actor_name_map.TryGetValue(g2, out dp2))
+                                            m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 2");
+                                        else
+                                            m_log.WarnFormat("[PHYSICS]: land versus large prim geo {0},size {1}, AABBsize <{2},{3},{4}>, at {5} ori {6},({7})",
+                                                dp2.Name, dp2.Size, x, y, z,
+                                                dp2.Position.ToString(),
+                                                dp2.Orientation.ToString(),
+                                                dp2.Orientation.Length());
+                                        return;
+                                    }
+                                }
+                //
+                */
 
 
-                if(d.GeomGetCategoryBits(g1) == (uint)CollisionCategories.VolumeDtc ||
-                    d.GeomGetCategoryBits(g1) == (uint)CollisionCategories.VolumeDtc)
+                if (d.GeomGetCategoryBits(g1) == (uint)CollisionCategories.VolumeDtc ||
+                    d.GeomGetCategoryBits(g2) == (uint)CollisionCategories.VolumeDtc)
                 {
                     int cflags;
                     unchecked
@@ -776,7 +782,7 @@ namespace OpenSim.Region.Physics.OdePlugin
             catch (SEHException)
             {
                 m_log.Error("[PHYSICS]: The Operating system shut down ODE because of corrupt memory.  This could be a result of really irregular terrain.  If this repeats continuously, restart using Basic Physics and terrain fill your terrain.  Restarting the sim.");
-//                ode.drelease(world);
+                //                ode.drelease(world);
                 base.TriggerPhysicsBasedRestart();
             }
             catch (Exception e)
@@ -816,26 +822,25 @@ namespace OpenSim.Region.Physics.OdePlugin
 
             // get first contact
             d.ContactGeom curContact = new d.ContactGeom();
+
             if (!GetCurContactGeom(0, ref curContact))
                 return;
-            // for now it's the one with max depth
-            ContactPoint maxDepthContact = new ContactPoint(
-                        new Vector3(curContact.pos.X, curContact.pos.Y, curContact.pos.Z),
-                        new Vector3(curContact.normal.X, curContact.normal.Y, curContact.normal.Z),
-                        curContact.depth
-                        );
+
             // do volume detection case
-            if (
-                (p1.IsVolumeDtc || p2.IsVolumeDtc))
+            if ((p1.IsVolumeDtc || p2.IsVolumeDtc))
             {
+                ContactPoint maxDepthContact = new ContactPoint(
+                    new Vector3(curContact.pos.X, curContact.pos.Y, curContact.pos.Z),
+                    new Vector3(curContact.normal.X, curContact.normal.Y, curContact.normal.Z),
+                    curContact.depth, false
+                    );
+
                 collision_accounting_events(p1, p2, maxDepthContact);
                 return;
             }
 
             // big messy collision analises
 
-            Vector3 normoverride = Vector3.Zero; //damm c#
-
             float mu = 0;
             float bounce = 0;
             float cfm = 0.0001f;
@@ -846,36 +851,15 @@ namespace OpenSim.Region.Physics.OdePlugin
             ContactData contactdata1 = new ContactData(0, 0, false);
             ContactData contactdata2 = new ContactData(0, 0, false);
 
-            bool dop1foot = false;
-            bool dop2foot = false;
+            bool dop1ava = false;
+            bool dop2ava = false;
             bool ignore = false;
-            bool AvanormOverride = false;
 
             switch (p1.PhysicsActorType)
             {
                 case (int)ActorTypes.Agent:
                     {
-                        dop1foot = true;
-
-                        AvanormOverride = true;
-                        Vector3 tmp = p2.Position - p1.Position;
-                        normoverride = p2.Velocity - p1.Velocity;
-                        mu = normoverride.LengthSquared();
-
-                        if (mu > 1e-6)
-                        {
-                            mu = 1.0f / (float)Math.Sqrt(mu);
-                            normoverride *= mu;
-                            mu = Vector3.Dot(tmp, normoverride);
-                            if (mu > 0)
-                                normoverride *= -1;
-                        }
-                        else
-                        {
-                            tmp.Normalize();
-                            normoverride = -tmp;
-                        }
-
+                        dop1ava = true;
                         switch (p2.PhysicsActorType)
                         {
                             case (int)ActorTypes.Agent:
@@ -886,7 +870,6 @@ namespace OpenSim.Region.Physics.OdePlugin
                             case (int)ActorTypes.Prim:
                                 if (p2.Velocity.LengthSquared() > 0.0f)
                                     p2.CollidingObj = true;
-                                dop1foot = true;
                                 break;
 
                             default:
@@ -901,33 +884,8 @@ namespace OpenSim.Region.Physics.OdePlugin
                     {
                         case (int)ActorTypes.Agent:
 
+                            dop2ava = true;
 
-                            dop2foot = true;
-
-                            AvanormOverride = true;
-
-                            Vector3 tmp = p2.Position - p1.Position;
-                            normoverride = p2.Velocity - p1.Velocity;
-                            mu = normoverride.LengthSquared();
-                            if (mu > 1e-6)
-                            {
-                                mu = 1.0f / (float)Math.Sqrt(mu);
-                                normoverride *= mu;
-                                mu = Vector3.Dot(tmp, normoverride);
-                                if (mu > 0)
-                                    normoverride *= -1;
-                            }
-                            else
-                            {
-                                tmp.Normalize();
-                                normoverride = -tmp;
-                            }
-
-                            bounce = 0;
-                            mu = 0;
-                            cfm = 0.0001f;
-
-                            dop2foot = true;
                             if (p1.Velocity.LengthSquared() > 0.0f)
                                 p1.CollidingObj = true;
                             break;
@@ -1032,170 +990,78 @@ namespace OpenSim.Region.Physics.OdePlugin
                 default:
                     break;
             }
+
             if (ignore)
                 return;
 
-            IntPtr Joint;
 
-            bool FeetCollision = false;
+            d.ContactGeom maxContact = curContact;
+//            if (IgnoreNegSides && curContact.side1 < 0)
+//                maxContact.depth = float.MinValue;
+
+            d.ContactGeom minContact = curContact;
+//            if (IgnoreNegSides && curContact.side1 < 0)
+//                minContact.depth = float.MaxValue;
 
-            int i = 0;
+            IntPtr Joint;
+            bool FeetCollision = false;
             int ncontacts = 0;
-            while(true)
-            {
 
-                if (IgnoreNegSides && curContact.side1 < 0)
-                {
-                    if (++i >= count)
-                        break;
 
-                    if (!GetCurContactGeom(i, ref curContact))
-                        break;
-                }
-                else
+                int i = 0;
 
+                while (true)
                 {
-                    if(dop1foot)
+                    if (m_global_contactcount >= maxContactsbeforedeath)
+                        break;
+
+//                    if (!(IgnoreNegSides && curContact.side1 < 0))
                     {
-                        if (!(((OdeCharacter)p1).Collide(g1, false, ref curContact, ref FeetCollision)))
+                        bool noskip = true;
+                        if (dop1ava)
                         {
-                            if (++i >= count)
-                                break;
-                            else
-                                continue;
+                            if (!(((OdeCharacter)p1).Collide(g1,false, ref curContact, ref FeetCollision)))
+
+                                noskip = false;
                         }
-                    }
-                    else if(dop2foot)
-                    {
-                        if (!(((OdeCharacter)p2).Collide(g2, true, ref curContact, ref FeetCollision)))
+                        else if (dop2ava)
                         {
-                            if (++i >= count)
-                                break;
-                            else
-                                continue;
+                            if (!(((OdeCharacter)p2).Collide(g2,true, ref curContact, ref FeetCollision)))
+                                noskip = false;
                         }
-                    }
 
-/*
-                    if (AvanormOverride)
-                    {
-                        if (curContact.depth > 0.3f)
+                        if (noskip)
                         {
-                            if (dop1foot && (p1.Position.Z - curContact.pos.Z) > (p1.Size.Z - avCapRadius) * 0.5f)
-                                p1.IsColliding = true;
-                            if (dop2foot && (p2.Position.Z - curContact.pos.Z) > (p2.Size.Z - avCapRadius) * 0.5f)
-                                p2.IsColliding = true;
-                            curContact.normal.X = normoverride.X;
-                            curContact.normal.Y = normoverride.Y;
-                            curContact.normal.Z = normoverride.Z;
-                        }
+                            m_global_contactcount++;
+                            ncontacts++;
 
-                        else
-                        {
-                            if (dop1foot)
-                            {
-                                float sz = p1.Size.Z;
-                                Vector3 vtmp = p1.Position;
-                                float ppos = curContact.pos.Z - vtmp.Z + (sz - avCapRadius) * 0.5f;
-                                if (ppos > 0f)
-                                {
-                                    if (!p1.Flying)
-                                    {
-                                        d.AABB aabb;
-                                        d.GeomGetAABB(g2, out aabb);
-                                        float tmp = vtmp.Z - sz * .18f;
-
-                                        if (aabb.MaxZ < tmp)
-                                        {
-                                            vtmp.X = curContact.pos.X - vtmp.X;
-                                            vtmp.Y = curContact.pos.Y - vtmp.Y;
-                                            vtmp.Z = -0.2f;
-                                            vtmp.Normalize();
-                                            curContact.normal.X = vtmp.X;
-                                            curContact.normal.Y = vtmp.Y;
-                                            curContact.normal.Z = vtmp.Z;
-                                        }
-                                    }
-                                }
-                                else
-                                    p1.IsColliding = true;
+                            Joint = CreateContacJoint(ref curContact, mu, bounce, cfm, erpscale, dscale);
+                            d.JointAttach(Joint, b1, b2);
 
-                            }
+                            if (curContact.depth > maxContact.depth)
+                                maxContact = curContact;
 
-                            if (dop2foot)
-                            {
-                                float sz = p2.Size.Z;
-                                Vector3 vtmp = p2.Position;
-                                vtmp.Z -= sz * 0.5f;
-                                vtmp.Z += 0.5f;
-                                float ppos = vtmp.Z - curContact.pos.Z;
-                                if (ppos > 0f)
-                                {
-                                    if (!p2.Flying)
-                                    {
-                                        float tmp = vtmp.Z - sz * .18f;
-                                        vtmp.X = curContact.pos.X - vtmp.X;
-                                        vtmp.Y = curContact.pos.Y - vtmp.Y;
-                                        vtmp.Z = curContact.pos.Z - vtmp.Z;
-                                        vtmp.Normalize();
-                                        curContact.normal.X = vtmp.X;
-                                        curContact.normal.Y = vtmp.Y;
-                                        curContact.normal.Z = vtmp.Z;
-                                    }
-                                }
-//                                else
-                                    p2.IsColliding = true;
-
-                            }
+                            if (curContact.depth < minContact.depth)
+                                minContact = curContact;
                         }
                     }
-*/
-                    ncontacts++;
-                    Joint = CreateContacJoint(ref curContact, mu, bounce, cfm, erpscale, dscale);
-                    d.JointAttach(Joint, b1, b2);
-
-                    if (++m_global_contactcount >= maxContactsbeforedeath)
-                        break;
 
                     if (++i >= count)
                         break;
 
                     if (!GetCurContactGeom(i, ref curContact))
                         break;
-
-                    if (curContact.depth > maxDepthContact.PenetrationDepth)
-                    {
-                        maxDepthContact.Position.X = curContact.pos.X;
-                        maxDepthContact.Position.Y = curContact.pos.Y;
-                        maxDepthContact.Position.Z = curContact.pos.Z;
-                        maxDepthContact.SurfaceNormal.X = curContact.normal.X;
-                        maxDepthContact.SurfaceNormal.Y = curContact.normal.Y;
-                        maxDepthContact.SurfaceNormal.Z = curContact.normal.Z;
-                        maxDepthContact.PenetrationDepth = curContact.depth;
-                    }
                 }
-            }
 
             if (ncontacts > 0)
             {
-                maxDepthContact.CharacterFeet = FeetCollision;
+                ContactPoint maxDepthContact = new ContactPoint(
+                            new Vector3(maxContact.pos.X, maxContact.pos.Y, maxContact.pos.Z),
+                            new Vector3(minContact.normal.X, minContact.normal.Y, minContact.normal.Z),
+                            maxContact.depth, FeetCollision
+                            );
                 collision_accounting_events(p1, p2, maxDepthContact);
             }
-/*
-            if (notskipedcount > geomContactPointsStartthrottle)
-            {
-                // If there are more then 3 contact points, it's likely
-                // that we've got a pile of objects, so ...
-                // We don't want to send out hundreds of terse updates over and over again
-                // so lets throttle them and send them again after it's somewhat sorted out.
-                 this needs checking so out for now
-                                if (b1 != IntPtr.Zero)
-                                    p1.ThrottleUpdates = true;
-                                if (b2 != IntPtr.Zero)
-                                    p2.ThrottleUpdates = true;
-                
-            }
- */
         }
 
         private void collision_accounting_events(PhysicsActor p1, PhysicsActor p2, ContactPoint contact)
@@ -1286,10 +1152,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                         //                    chr.CollidingGround = false; not done here
                         chr.CollidingObj = false;
                         // do colisions with static space
-                        d.SpaceCollide2(StaticSpace, chr.topbox, IntPtr.Zero, nearCallback);
-                        d.SpaceCollide2(StaticSpace, chr.midbox, IntPtr.Zero, nearCallback);
-                        d.SpaceCollide2(StaticSpace, chr.feetbox, IntPtr.Zero, nearCallback);
-                        d.SpaceCollide2(StaticSpace, chr.bonebox, IntPtr.Zero, nearCallback);
+                        d.SpaceCollide2(chr.collider, StaticSpace, IntPtr.Zero, nearCallback);
 
                         // chars with chars
                         d.SpaceCollide(CharsSpace, IntPtr.Zero, nearCallback);
@@ -1346,7 +1209,7 @@ namespace OpenSim.Region.Physics.OdePlugin
             // and with chars
             try
             {
-                d.SpaceCollide2(ActiveSpace, CharsSpace,IntPtr.Zero, nearCallback);
+                d.SpaceCollide2(CharsSpace,ActiveSpace, IntPtr.Zero, nearCallback);
             }
             catch (AccessViolationException)
             {
@@ -1837,7 +1700,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                             foreach (OdeCharacter actor in _characters)
                             {
                                 if (actor != null)
-                                    actor.Move(ODE_STEPSIZE, defects);
+                                    actor.Move(defects);
                             }
                             if (defects.Count != 0)
                             {