3 files changed, 104 insertions, 88 deletions

diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs
index ecd5474..daf6c7c 100644
--- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs
@@ -96,6 +96,8 @@ namespace OpenSim.Region.Physics.OdePlugin
         private float m_feetOffset = 0;
         private float feetOff = 0;
         private float boneOff = 0;
+        private float AvaAvaSizeXsq = 0.3f;
+        private float AvaAvaSizeYsq = 0.2f;
 
         public float walkDivisor = 1.3f;
         public float runDivisor = 0.8f;
@@ -136,7 +138,6 @@ namespace OpenSim.Region.Physics.OdePlugin
         public IntPtr Body = IntPtr.Zero;
         private OdeScene _parent_scene;
         private IntPtr capsule = IntPtr.Zero;
-        private IntPtr bbox = IntPtr.Zero;
         public IntPtr collider = IntPtr.Zero;
 
         public IntPtr Amotor = IntPtr.Zero;
@@ -709,6 +710,11 @@ namespace OpenSim.Region.Physics.OdePlugin
 
             feetOff = bot + feetsz;
 
+            AvaAvaSizeXsq = 0.4f * sx;
+            AvaAvaSizeXsq *= AvaAvaSizeXsq;
+            AvaAvaSizeYsq = 0.5f * sy;
+            AvaAvaSizeYsq *= AvaAvaSizeYsq;
+
             _parent_scene.waitForSpaceUnlock(_parent_scene.CharsSpace);
 
             collider = d.HashSpaceCreate(_parent_scene.CharsSpace);
@@ -723,9 +729,8 @@ namespace OpenSim.Region.Physics.OdePlugin
                 r = m_size.Y;
             float l = m_size.Z - r;
             r *= 0.5f;
-            capsule = d.CreateCapsule(collider, r, l);
 
-            bbox = d.CreateBox(collider, m_size.X, m_size.Y, m_size.Z);
+            capsule = d.CreateCapsule(collider, r, l);
 
             m_mass = m_density * m_size.X * m_size.Y * m_size.Z;  // update mass
 
@@ -750,8 +755,6 @@ namespace OpenSim.Region.Physics.OdePlugin
             _position.Z = npositionZ;
 
             d.BodySetMass(Body, ref ShellMass);
-
-            d.GeomSetBody(bbox, Body);
             d.GeomSetBody(capsule, Body);
 
             // The purpose of the AMotor here is to keep the avatar's physical
@@ -821,14 +824,6 @@ namespace OpenSim.Region.Physics.OdePlugin
                 capsule = 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)
             {
                 d.SpaceDestroy(collider);
@@ -871,20 +866,44 @@ namespace OpenSim.Region.Physics.OdePlugin
         }
 
 
-        public bool Collide(IntPtr me, bool reverse, ref d.ContactGeom contact, ref bool feetcollision)
+        public bool Collide(IntPtr me,IntPtr other, bool reverse, ref d.ContactGeom contact, ref bool feetcollision)
         {
             feetcollision = false;
 
-            Vector3 offset;
-
-            offset.X = contact.pos.X - _position.X;
-            offset.Y = contact.pos.Y - _position.Y;
-
             if (me == capsule)
             {
+                Vector3 offset;
+
                 float h = contact.pos.Z - _position.Z;
                 offset.Z = h - feetOff;
 
+                offset.X = contact.pos.X - _position.X;
+                offset.Y = contact.pos.Y - _position.Y;
+
+                d.GeomClassID gtype = d.GeomGetClass(other);
+                if (gtype == d.GeomClassID.CapsuleClass)
+                {
+                    Vector3 roff = offset * Quaternion.Inverse(m_orientation2D);
+                    float r = roff.X *roff.X / AvaAvaSizeXsq;
+                    r += (roff.Y * roff.Y) / AvaAvaSizeYsq;
+                    if (r > 1.0f)
+                        return false;
+
+                    float dp = 1.0f -(float)Math.Sqrt((double)r);
+                    if (dp > 0.05f)
+                        dp = 0.05f;
+
+                    contact.depth = dp;
+
+                    if (offset.Z < 0)
+                    {
+                        feetcollision = true;
+                        if (h < boneOff)
+                            IsColliding = true;
+                    }
+                    return true;
+                }
+
                 if (offset.Z > 0)
                     return true;
 
@@ -905,11 +924,9 @@ namespace OpenSim.Region.Physics.OdePlugin
                 feetcollision = true;
                 if (h < boneOff)
                     IsColliding = true;
+                return true;
             }
-            else
-                return false;
-
-            return true;
+            return false;
         }
 
         /// <summary>
@@ -1094,8 +1111,8 @@ namespace OpenSim.Region.Physics.OdePlugin
                         // Avatar to Avatar collisions
                         // Prim to avatar collisions
 
-                        vec.X = -vel.X * PID_D + (_zeroPosition.X - localpos.X) * (PID_P * 2);
-                        vec.Y = -vel.Y * PID_D + (_zeroPosition.Y - localpos.Y) * (PID_P * 2);
+                        vec.X = -vel.X * PID_D * 2 + (_zeroPosition.X - localpos.X) * (PID_P * 5);
+                        vec.Y = -vel.Y * PID_D * 2 + (_zeroPosition.Y - localpos.Y) * (PID_P * 5);
                         if (flying)
                         {
                             vec.Z += -vel.Z * PID_D + (_zeroPosition.Z - localpos.Z) * PID_P;
@@ -1389,10 +1406,6 @@ namespace OpenSim.Region.Physics.OdePlugin
                     AvatarGeomAndBodyCreation(_position.X, _position.Y, _position.Z);
 
                     _parent_scene.actor_name_map[collider] = (PhysicsActor)this;
-//                    _parent_scene.actor_name_map[feetbox] = (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.actor_name_map[capsule] = (PhysicsActor)this;
                     _parent_scene.AddCharacter(this);
                 }
@@ -1449,7 +1462,6 @@ namespace OpenSim.Region.Physics.OdePlugin
                     
 
                     _parent_scene.actor_name_map[collider] = (PhysicsActor)this;
-                    _parent_scene.actor_name_map[bbox] = (PhysicsActor)this;
                     _parent_scene.actor_name_map[capsule] = (PhysicsActor)this;
                 }
                 m_freemove = false;
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODESitAvatar.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODESitAvatar.cs
index e9023c3..672212f 100644
--- a/OpenSim/Region/Physics/UbitOdePlugin/ODESitAvatar.cs
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODESitAvatar.cs
@@ -78,11 +78,11 @@ namespace OpenSim.Region.Physics.OdePlugin
 
             IntPtr geom = ((OdePrim)actor).prim_geom;
 
-//            Vector3 geopos = d.GeomGetPositionOMV(geom);
-//            Quaternion geomOri = d.GeomGetQuaternionOMV(geom);
+            Vector3 geopos = d.GeomGetPositionOMV(geom);
+            Quaternion geomOri = d.GeomGetQuaternionOMV(geom);
 
-            Vector3 geopos = actor.Position;
-            Quaternion geomOri = actor.Orientation;
+//            Vector3 geopos = actor.Position;
+//            Quaternion geomOri = actor.Orientation;
 
             Quaternion geomInvOri = Quaternion.Conjugate(geomOri);
 
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs
index 8abf6cf..9bf2abe 100644
--- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs
+++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs
@@ -833,13 +833,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                         switch (p2.PhysicsActorType)
                         {
                             case (int)ActorTypes.Agent:
-                                p1.CollidingObj = true;
-                                p2.CollidingObj = true;
-                                break;
-
                             case (int)ActorTypes.Prim:
-                                if (p2.Velocity.LengthSquared() > 0.0f)
-                                    p2.CollidingObj = true;
                                 break;
 
                             default:
@@ -850,55 +844,53 @@ namespace OpenSim.Region.Physics.OdePlugin
                     }
 
                 case (int)ActorTypes.Prim:
-                    switch (p2.PhysicsActorType)
                     {
-                        case (int)ActorTypes.Agent:
-
-                            dop2ava = true;
-
-                            if (p1.Velocity.LengthSquared() > 0.0f)
-                                p1.CollidingObj = true;
-                            break;
-
-                        case (int)ActorTypes.Prim:
-                            Vector3 relV = p1.Velocity - p2.Velocity;
-                            float relVlenSQ = relV.LengthSquared();
-                            if (relVlenSQ > 0.0001f)
-                            {
-                                p1.CollidingObj = true;
-                                p2.CollidingObj = true;
-                            }
-                            p1.getContactData(ref contactdata1);
-                            p2.getContactData(ref contactdata2);
-                            bounce = contactdata1.bounce * contactdata2.bounce;
-                            mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu);
+                        switch (p2.PhysicsActorType)
+                        {
+                            case (int)ActorTypes.Agent:
+                                dop2ava = true;
+                                break;
 
-                            if (relVlenSQ > 0.01f)
-                                mu *= frictionMovementMult;
+                            case (int)ActorTypes.Prim:
+                                Vector3 relV = p1.Velocity - p2.Velocity;
+                                float relVlenSQ = relV.LengthSquared();
+                                if (relVlenSQ > 0.0001f)
+                                {
+                                    p1.CollidingObj = true;
+                                    p2.CollidingObj = true;
+                                }
+                                p1.getContactData(ref contactdata1);
+                                p2.getContactData(ref contactdata2);
+                                bounce = contactdata1.bounce * contactdata2.bounce;
+                                mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu);
 
-                            break;
+                                if (relVlenSQ > 0.01f)
+                                    mu *= frictionMovementMult;
 
-                        case (int)ActorTypes.Ground:
-                            p1.getContactData(ref contactdata1);
-                            bounce = contactdata1.bounce * TerrainBounce;
-                            mu = (float)Math.Sqrt(contactdata1.mu * TerrainFriction);
+                                break;
 
-                            if (Math.Abs(p1.Velocity.X) > 0.1f || Math.Abs(p1.Velocity.Y) > 0.1f)
-                                mu *= frictionMovementMult;
-                            p1.CollidingGround = true;
-/*
-                            if (d.GeomGetClass(g1) == d.GeomClassID.TriMeshClass)
-                            {
-                                if (curContact.side1 > 0)
-                                    IgnoreNegSides = true;
-                            }
- */
-                            break;
+                            case (int)ActorTypes.Ground:
+                                p1.getContactData(ref contactdata1);
+                                bounce = contactdata1.bounce * TerrainBounce;
+                                mu = (float)Math.Sqrt(contactdata1.mu * TerrainFriction);
+
+                                if (Math.Abs(p1.Velocity.X) > 0.1f || Math.Abs(p1.Velocity.Y) > 0.1f)
+                                    mu *= frictionMovementMult;
+                                p1.CollidingGround = true;
+                                /*
+                                                            if (d.GeomGetClass(g1) == d.GeomClassID.TriMeshClass)
+                                                            {
+                                                                if (curContact.side1 > 0)
+                                                                    IgnoreNegSides = true;
+                                                            }
+                                 */
+                                break;
 
-                        case (int)ActorTypes.Water:
-                        default:
-                            ignore = true;
-                            break;
+                            case (int)ActorTypes.Water:
+                            default:
+                                ignore = true;
+                                break;
+                        }
                     }
                     break;
 
@@ -950,14 +942,25 @@ namespace OpenSim.Region.Physics.OdePlugin
                     bool noskip = true;
                     if (dop1ava)
                     {
-                        if (!(((OdeCharacter)p1).Collide(g1, false, ref curContact, ref FeetCollision)))
-
+                        if (!(((OdeCharacter)p1).Collide(g1, g2, false, ref curContact, ref FeetCollision)))
                             noskip = false;
+                        else
+                        {
+                            if(p2.PhysicsActorType == (int)ActorTypes.Agent)
+                            {
+                                p1.CollidingObj = true;
+                                p2.CollidingObj = true;
+                            }
+                            else if (p2.Velocity.LengthSquared() > 0.0f)
+                                    p2.CollidingObj = true;
+                        }
                     }
                     else if (dop2ava)
                     {
-                        if (!(((OdeCharacter)p2).Collide(g2, true, ref curContact, ref FeetCollision)))
+                        if (!(((OdeCharacter)p2).Collide(g2,g1, true, ref curContact, ref FeetCollision)))
                             noskip = false;
+                        else if (p1.Velocity.LengthSquared() > 0.0f)
+                            p1.CollidingObj = true;
                     }
 
                     if (noskip)
@@ -1095,10 +1098,11 @@ namespace OpenSim.Region.Physics.OdePlugin
                         // do colisions with static space
                         d.SpaceCollide2(chr.collider, StaticSpace, IntPtr.Zero, nearCallback);
 
-                        // chars with chars
-                        d.SpaceCollide(CharsSpace, IntPtr.Zero, nearCallback);
                         // no coll with gnd
                     }
+                    // chars with chars
+                    d.SpaceCollide(CharsSpace, IntPtr.Zero, nearCallback);
+
                 }
                 catch (AccessViolationException)
                 {