2 files changed, 320 insertions, 57 deletions

diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs
index daf6c7c..4a98df4 100644
--- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs
@@ -79,6 +79,7 @@ namespace OpenSim.Region.Physics.OdePlugin
         private Vector3 _acceleration;
         private Vector3 m_rotationalVelocity;
         private Vector3 m_size;
+        private Vector3 m_collideNormal;
         private Quaternion m_orientation;
         private Quaternion m_orientation2D;
         private float m_mass = 80f;
@@ -109,6 +110,7 @@ namespace OpenSim.Region.Physics.OdePlugin
 
         private bool _zeroFlag = false;
 
+
         private uint m_localID = 0;
         public bool m_returnCollisions = false;
         // taints and their non-tainted counterparts
@@ -153,9 +155,7 @@ namespace OpenSim.Region.Physics.OdePlugin
         public UUID m_uuid;
         public bool bad = false;
 
-        float mu;
-
-        
+        float mu;       
 
         public OdeCharacter(uint localID, String avName, OdeScene parent_scene, Vector3 pos, Vector3 pSize, float pfeetOffset, float density, float walk_divisor, float rundivisor)
         {
@@ -864,11 +864,12 @@ namespace OpenSim.Region.Physics.OdePlugin
             x = tx * cos - y * sin;
             y = tx * sin + y * cos;
         }
-
-
-        public bool Collide(IntPtr me,IntPtr other, bool reverse, ref d.ContactGeom contact, ref bool feetcollision)
+      
+        public bool Collide(IntPtr me, IntPtr other, bool reverse, ref d.ContactGeom contact,
+                ref d.ContactGeom altContact , ref bool useAltcontact, ref bool feetcollision)
         {
             feetcollision = false;
+            useAltcontact = false;
 
             if (me == capsule)
             {
@@ -899,31 +900,77 @@ namespace OpenSim.Region.Physics.OdePlugin
                     {
                         feetcollision = true;
                         if (h < boneOff)
+                        {
+                            m_collideNormal.X = contact.normal.X;
+                            m_collideNormal.Y = contact.normal.Y;
+                            m_collideNormal.Z = contact.normal.Z;
                             IsColliding = true;
+                        }
                     }
                     return true;
-                }
+                }               
+
+                d.AABB aabb;
+                d.GeomGetAABB(other,out aabb);
+                float othertop = aabb.MaxZ - _position.Z;
+
+                if (offset.Z > 0 || othertop > -feetOff || contact.normal.Z > 0.35f)
+                {
+                    if (offset.Z <= 0)
+                    {
+                        feetcollision = true;
+                        if (h < boneOff)
+                        {
+                            m_collideNormal.X = contact.normal.X;
+                            m_collideNormal.Y = contact.normal.Y;
+                            m_collideNormal.Z = contact.normal.Z;
+                            IsColliding = true;
+                        }
+                    }
 
-                if (offset.Z > 0)
+                    if (contact.normal.Z < 0.2f)
+                    {
+                        contact.normal.Z = 0;
+                        float t = contact.normal.X * contact.normal.X + contact.normal.Y * contact.normal.Y;
+                        if (t > 0)
+                        {
+                            t = 1.0f / t;
+                            contact.normal.X *= t;
+                            contact.normal.Y *= t;
+                        }
+                    }
                     return true;
+                }
+
+                altContact = contact;
+                useAltcontact = true;
 
                 offset.Normalize();
 
+                if (contact.depth > 0.1f)
+                    contact.depth = 0.1f;
+
                 if (reverse)
                 {
-                    contact.normal.X = offset.X;
-                    contact.normal.Y = offset.Y;
-                    contact.normal.Z = offset.Z;
+                    altContact.normal.X = offset.X;
+                    altContact.normal.Y = offset.Y;
+                    altContact.normal.Z = offset.Z;
                 }
                 else
                 {
-                    contact.normal.X = -offset.X;
-                    contact.normal.Y = -offset.Y;
-                    contact.normal.Z = -offset.Z;
+                    altContact.normal.X = -offset.X;
+                    altContact.normal.Y = -offset.Y;
+                    altContact.normal.Z = -offset.Z;
                 }
+
                 feetcollision = true;
                 if (h < boneOff)
+                {
+                    m_collideNormal.X = contact.normal.X;
+                    m_collideNormal.Y = contact.normal.Y;
+                    m_collideNormal.Z = contact.normal.Z;
                     IsColliding = true;
+                }
                 return true;
             }
             return false;
@@ -1003,6 +1050,9 @@ namespace OpenSim.Region.Physics.OdePlugin
             Vector3 vel = new Vector3(dtmp.X, dtmp.Y, dtmp.Z);
             float velLengthSquared = vel.LengthSquared();
 
+
+            Vector3 ctz = _target_velocity;
+
             float movementdivisor = 1f;
             //Ubit change divisions into multiplications below
             if (!m_alwaysRun)
@@ -1010,13 +1060,16 @@ namespace OpenSim.Region.Physics.OdePlugin
             else
                 movementdivisor = 1 / runDivisor;
 
+            ctz.X *= movementdivisor;
+            ctz.Y *= movementdivisor;
+
             //******************************************
             // colide with land
 
             d.AABB aabb;
 //            d.GeomGetAABB(feetbox, out aabb);
             d.GeomGetAABB(capsule, out aabb);
-            float chrminZ = aabb.MinZ; ; // move up a bit
+            float chrminZ = aabb.MinZ; // move up a bit
             Vector3 posch = localpos;
 
             float ftmp;
@@ -1031,15 +1084,18 @@ namespace OpenSim.Region.Physics.OdePlugin
             float terrainheight = _parent_scene.GetTerrainHeightAtXY(posch.X, posch.Y);
             if (chrminZ < terrainheight)
             {
+                if (ctz.Z < 0)
+                    ctz.Z = 0;
+
+                Vector3 n = _parent_scene.GetTerrainNormalAtXY(posch.X, posch.Y);
                 float depth = terrainheight - chrminZ;
+
+                vec.Z = depth * PID_P * 50;
+
                 if (!flying)
-                {
-                    vec.Z = -vel.Z * PID_D * 1.5f + depth * PID_P * 50;
-                }
-                else
-                    vec.Z = depth * PID_P * 50;
+                    vec.Z += -vel.Z * PID_D;
 
-                if (depth < 0.1f)
+                if (depth < 0.2f)
                 {
                     m_colliderGroundfilter++;
                     if (m_colliderGroundfilter > 2)
@@ -1053,50 +1109,83 @@ namespace OpenSim.Region.Physics.OdePlugin
                             m_freemove = false;
                         }
 
+                        m_collideNormal.X = n.X;
+                        m_collideNormal.Y = n.Y;
+                        m_collideNormal.Z = n.Z;
+
                         m_iscollidingGround = true;
 
+
                         ContactPoint contact = new ContactPoint();
                         contact.PenetrationDepth = depth;
                         contact.Position.X = localpos.X;
                         contact.Position.Y = localpos.Y;
                         contact.Position.Z = terrainheight;
-                        contact.SurfaceNormal.X = 0.0f;
-                        contact.SurfaceNormal.Y = 0.0f;
-                        contact.SurfaceNormal.Z = -1f;
+                        contact.SurfaceNormal.X = -n.X;
+                        contact.SurfaceNormal.Y = -n.Y;
+                        contact.SurfaceNormal.Z = -n.Z;
                         contact.RelativeSpeed = -vel.Z;
                         contact.CharacterFeet = true;
                         AddCollisionEvent(0, contact);
 
-                        vec.Z *= 0.5f;
+//                        vec.Z *= 0.5f;
                     }
                 }
 
                 else
                 {
-                    m_colliderGroundfilter = 0;
-                    m_iscollidingGround = false;
+                    m_colliderGroundfilter -= 5;
+                    if (m_colliderGroundfilter <= 0)
+                    {
+                        m_colliderGroundfilter = 0;
+                        m_iscollidingGround = false;
+                    }
                 }
             }
             else
             {
-                m_colliderGroundfilter = 0;
-                m_iscollidingGround = false;
+                m_colliderGroundfilter -= 5;
+                if (m_colliderGroundfilter <= 0)
+                {
+                    m_colliderGroundfilter = 0;
+                    m_iscollidingGround = false;
+                }
             }
 
             
             //******************************************
+            if (!m_iscolliding)
+                m_collideNormal.Z = 0;
+
+            bool tviszero = (ctz.X == 0.0f && ctz.Y == 0.0f && ctz.Z == 0.0f);
+
 
-            bool tviszero = (_target_velocity.X == 0.0f && _target_velocity.Y == 0.0f && _target_velocity.Z == 0.0f);
 
-            //            if (!tviszero || m_iscolliding || velLengthSquared <0.01)
             if (!tviszero)
+            {
                 m_freemove = false;
 
+                // movement relative to surface if moving on it
+                // dont disturbe vertical movement, ie jumps
+                if (m_iscolliding && !flying && ctz.Z == 0 && m_collideNormal.Z > 0.2f && m_collideNormal.Z < 0.94f)
+                {
+                    float p = ctz.X * m_collideNormal.X + ctz.Y * m_collideNormal.Y;
+                    ctz.X *= (float)Math.Sqrt(1 - m_collideNormal.X * m_collideNormal.X);
+                    ctz.Y *= (float)Math.Sqrt(1 - m_collideNormal.Y * m_collideNormal.Y);
+                    ctz.Z -= p;
+                    if (ctz.Z < 0)
+                        ctz.Z *= 2;
+
+                }
+
+            }
+
+
             if (!m_freemove)
             {
 
                 //  if velocity is zero, use position control; otherwise, velocity control
-                if (tviszero && m_iscolliding)
+                if (tviszero && m_iscolliding && !flying)
                 {
                     //  keep track of where we stopped.  No more slippin' & slidin'
                     if (!_zeroFlag)
@@ -1129,22 +1218,48 @@ namespace OpenSim.Region.Physics.OdePlugin
                     {
                         if (!flying)
                         {
-                            if (_target_velocity.Z > 0.0f)
+                            // we are on a surface
+                            if (ctz.Z > 0f)
                             {
-                                // We're colliding with something and we're not flying but we're moving
-                                // This means we're walking or running. JUMPING
-                                vec.Z += (_target_velocity.Z - vel.Z) * PID_D * 1.2f;// +(_zeroPosition.Z - localpos.Z) * PID_P;
+                                // moving up or JUMPING
+                                vec.Z += (ctz.Z - vel.Z) * PID_D * 1.2f;// +(_zeroPosition.Z - localpos.Z) * PID_P;
+                                vec.X += (ctz.X - vel.X) * (PID_D);
+                                vec.Y += (ctz.Y - vel.Y) * (PID_D);
                             }
+                            else
+                            {
+                                // we are moving down on a surface
+                                if (ctz.Z == 0)
+                                {
+                                    if (vel.Z > 0)
+                                        vec.Z -= vel.Z * PID_D * 2.0f;
+                                    vec.X += (ctz.X - vel.X) * (PID_D);
+                                    vec.Y += (ctz.Y - vel.Y) * (PID_D);
+                                }
+                                // intencionally going down
+                                else
+                                {
+                                    if (ctz.Z < vel.Z)
+                                        vec.Z += (ctz.Z - vel.Z) * PID_D * 2.0f;
+                                    else
+                                    {
+                                    }
+
+                                    if (Math.Abs(ctz.X) > Math.Abs(vel.X))
+                                        vec.X += (ctz.X - vel.X) * (PID_D);
+                                    if (Math.Abs(ctz.Y) > Math.Abs(vel.Y))
+                                        vec.Y += (ctz.Y - vel.Y) * (PID_D);
+                                }
+                            }
+
                             // We're standing on something
-                            vec.X = ((_target_velocity.X * movementdivisor) - vel.X) * (PID_D);
-                            vec.Y = ((_target_velocity.Y * movementdivisor) - vel.Y) * (PID_D);
                         }
                         else
                         {
                             // We're flying and colliding with something
-                            vec.X = ((_target_velocity.X * movementdivisor) - vel.X) * (PID_D * 0.0625f);
-                            vec.Y = ((_target_velocity.Y * movementdivisor) - vel.Y) * (PID_D * 0.0625f);
-                            vec.Z += (_target_velocity.Z - vel.Z) * (PID_D);
+                            vec.X += (ctz.X - vel.X) * (PID_D * 0.0625f);
+                            vec.Y += (ctz.Y - vel.Y) * (PID_D * 0.0625f);
+                            vec.Z += (ctz.Z - vel.Z) * (PID_D);
                         }
                     }
                     else // ie not colliding
@@ -1152,9 +1267,9 @@ namespace OpenSim.Region.Physics.OdePlugin
                         if (flying) //(!m_iscolliding && flying)
                         {
                             // we're in mid air suspended
-                            vec.X = ((_target_velocity.X * movementdivisor) - vel.X) * (PID_D * 1.667f);
-                            vec.Y = ((_target_velocity.Y * movementdivisor) - vel.Y) * (PID_D * 1.667f);
-                            vec.Z += (_target_velocity.Z - vel.Z) * (PID_D);
+                            vec.X += (ctz.X - vel.X) * (PID_D * 1.667f);
+                            vec.Y += (ctz.Y - vel.Y) * (PID_D * 1.667f);
+                            vec.Z += (ctz.Z - vel.Z) * (PID_D);
                         }
 
                         else
@@ -1163,8 +1278,11 @@ namespace OpenSim.Region.Physics.OdePlugin
                             // m_iscolliding includes collisions with the ground.
 
                             // d.Vector3 pos = d.BodyGetPosition(Body);
-                            vec.X = (_target_velocity.X - vel.X) * PID_D * 0.833f;
-                            vec.Y = (_target_velocity.Y - vel.Y) * PID_D * 0.833f;
+                            vec.X += (ctz.X - vel.X) * PID_D * 0.833f;
+                            vec.Y += (ctz.Y - vel.Y) * PID_D * 0.833f;
+                            // hack for  breaking on fall
+                            if (ctz.Z == -9999f)
+                                vec.Z += -vel.Z * PID_D - _parent_scene.gravityz * m_mass;
                         }
                     }
                 }
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs
index 9bf2abe..2adbe01 100644
--- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs
+++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs
@@ -935,18 +935,22 @@ namespace OpenSim.Region.Physics.OdePlugin
             SharedTmpcontact.surface.mu = mu;
             SharedTmpcontact.surface.bounce = bounce;
 
+            d.ContactGeom altContact = new d.ContactGeom();
+            bool useAltcontact = false;
+            bool noskip = true;
+
             while (true)
             {
 //                if (!(IgnoreNegSides && curContact.side1 < 0))
                 {
-                    bool noskip = true;
+                    noskip = true;
+                    useAltcontact = false;
+
                     if (dop1ava)
                     {
-                        if (!(((OdeCharacter)p1).Collide(g1, g2, false, ref curContact, ref FeetCollision)))
-                            noskip = false;
-                        else
+                        if ((((OdeCharacter)p1).Collide(g1, g2, false, ref curContact, ref altContact , ref useAltcontact, ref FeetCollision)))
                         {
-                            if(p2.PhysicsActorType == (int)ActorTypes.Agent)
+                            if (p2.PhysicsActorType == (int)ActorTypes.Agent)
                             {
                                 p1.CollidingObj = true;
                                 p2.CollidingObj = true;
@@ -954,18 +958,32 @@ namespace OpenSim.Region.Physics.OdePlugin
                             else if (p2.Velocity.LengthSquared() > 0.0f)
                                     p2.CollidingObj = true;
                         }
+                        else
+                            noskip = false;
                     }
                     else if (dop2ava)
                     {
-                        if (!(((OdeCharacter)p2).Collide(g2,g1, true, ref curContact, ref FeetCollision)))
+                        if ((((OdeCharacter)p2).Collide(g2, g1, true, ref curContact, ref altContact , ref useAltcontact, ref FeetCollision)))
+                        {
+                            if (p1.PhysicsActorType == (int)ActorTypes.Agent)
+                            {
+                                p1.CollidingObj = true;
+                                p2.CollidingObj = true;
+                            }
+                            else if (p2.Velocity.LengthSquared() > 0.0f)
+                                p1.CollidingObj = true;
+                        }
+                        else
                             noskip = false;
-                        else if (p1.Velocity.LengthSquared() > 0.0f)
-                            p1.CollidingObj = true;
                     }
 
                     if (noskip)
                     {
-                        Joint = CreateContacJoint(ref curContact);
+                        if(useAltcontact)
+                            Joint = CreateContacJoint(ref altContact);
+                        else
+                            Joint = CreateContacJoint(ref curContact);
+
                         if (Joint == IntPtr.Zero)
                             break;
 
@@ -1924,12 +1942,12 @@ namespace OpenSim.Region.Physics.OdePlugin
                     dy = 0;
                 }
             }
-
             else
             {
                 // we  still have square fixed size regions
                 // also flip x and y because of how map is done for ODE fliped axis
                 // so ix,iy,dx and dy are inter exchanged
+
                 if (x < regsize - 1)
                 {
                     iy = (int)x;
@@ -1976,7 +1994,7 @@ namespace OpenSim.Region.Physics.OdePlugin
             */
             h0 = ((float)heights[iy]); // 0,0 vertice
 
-            if ((dy > dx))
+            if (dy>dx)
             {
                 iy += regsize;
                 h2 = (float)heights[iy]; // 0,1 vertice
@@ -1994,6 +2012,133 @@ namespace OpenSim.Region.Physics.OdePlugin
             return h0 + h1 + h2;
         }
 
+        public Vector3 GetTerrainNormalAtXY(float x, float y)
+        {
+            int offsetX = ((int)(x / (int)Constants.RegionSize)) * (int)Constants.RegionSize;
+            int offsetY = ((int)(y / (int)Constants.RegionSize)) * (int)Constants.RegionSize;
+
+            IntPtr heightFieldGeom = IntPtr.Zero;
+            Vector3 norm = new Vector3(0, 0, 1);
+
+            // get region map
+            if (!RegionTerrain.TryGetValue(new Vector3(offsetX, offsetY, 0), out heightFieldGeom))
+                return norm;
+
+            if (heightFieldGeom == IntPtr.Zero)
+                return norm;
+
+            if (!TerrainHeightFieldHeights.ContainsKey(heightFieldGeom))
+                return norm;
+
+            // TerrainHeightField for ODE as offset 1m
+            x += 1f - offsetX;
+            y += 1f - offsetY;
+
+            // make position fit into array
+            if (x < 0)
+                x = 0;
+            if (y < 0)
+                y = 0;
+
+            // integer indexs
+            int ix;
+            int iy;
+            //  interpolators offset
+            float dx;
+            float dy;
+
+
+            int regsize = (int)Constants.RegionSize + 3; // map size see setterrain number of samples
+            int xstep = 1;
+            int ystep = regsize;
+            bool firstTri = false;
+
+            if (OdeUbitLib)
+            {
+                if (x < regsize - 1)
+                {
+                    ix = (int)x;
+                    dx = x - (float)ix;
+                }
+                else // out world use external height
+                {
+                    ix = regsize - 2;
+                    dx = 0;
+                }
+                if (y < regsize - 1)
+                {
+                    iy = (int)y;
+                    dy = y - (float)iy;
+                }
+                else
+                {
+                    iy = regsize - 2;
+                    dy = 0;
+                }
+                firstTri = dy > dx;
+            }
+
+            else
+            {
+                xstep = regsize;
+                ystep = 1;
+                // we  still have square fixed size regions
+                // also flip x and y because of how map is done for ODE fliped axis
+                // so ix,iy,dx and dy are inter exchanged
+                if (x < regsize - 1)
+                {
+                    iy = (int)x;
+                    dy = x - (float)iy;
+                }
+                else // out world use external height
+                {
+                    iy = regsize - 2;
+                    dy = 0;
+                }
+                if (y < regsize - 1)
+                {
+                    ix = (int)y;
+                    dx = y - (float)ix;
+                }
+                else
+                {
+                    ix = regsize - 2;
+                    dx = 0;
+                }
+                firstTri = dx > dy;
+            }
+
+            float h0;
+            float h1;
+            float h2;
+
+            iy *= regsize;
+            iy += ix; // all indexes have iy + ix
+
+            float[] heights = TerrainHeightFieldHeights[heightFieldGeom];
+
+            if (firstTri)
+            {
+                h1 = ((float)heights[iy]); // 0,0 vertice
+                iy += ystep;
+                h0 = (float)heights[iy]; // 0,1
+                h2 = (float)heights[iy+xstep]; // 1,1 vertice
+                norm.X = h0 - h2;
+                norm.Y = h1 - h0;              
+            }
+            else
+            {
+                h2 = ((float)heights[iy]); // 0,0 vertice
+                iy += xstep;
+                h0 = ((float)heights[iy]); // 1,0 vertice
+                h1 = (float)heights[iy+ystep]; // vertice 1,1
+                norm.X = h2 - h0;
+                norm.Y = h0 - h1;
+            }
+            norm.Z = 1;
+            norm.Normalize();
+            return norm;
+        }
 
         public override void SetTerrain(float[] heightMap)
         {