1 files changed, 109 insertions, 106 deletions

diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs
index 9ca2d3f..cf74f14 100644
--- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs
+++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs
@@ -189,9 +189,12 @@ namespace OpenSim.Region.Physics.OdePlugin
         private const uint m_regionHeight = Constants.RegionSize;
 
         public float ODE_STEPSIZE = 0.020f;
+        public float HalfOdeStep = 0.01f;
         private float metersInSpace = 25.6f;
         private float m_timeDilation = 1.0f;
 
+        DateTime m_lastframe;
+
         public float gravityx = 0f;
         public float gravityy = 0f;
         public float gravityz = -9.8f;
@@ -485,6 +488,8 @@ namespace OpenSim.Region.Physics.OdePlugin
                 }
             }
 
+            HalfOdeStep = ODE_STEPSIZE * 0.5f;
+
             ContactgeomsArray = Marshal.AllocHGlobal(contactsPerCollision * d.ContactGeom.unmanagedSizeOf);
             GlobalContactsArray = GlobalContactsArray = Marshal.AllocHGlobal(maxContactsbeforedeath * d.Contact.unmanagedSizeOf);
 
@@ -521,7 +526,7 @@ namespace OpenSim.Region.Physics.OdePlugin
             d.WorldSetAngularDamping(world, 0.001f);
             d.WorldSetAngularDampingThreshold(world, 0f);
             d.WorldSetLinearDampingThreshold(world, 0f);
-            d.WorldSetMaxAngularSpeed(world, 256f);
+            d.WorldSetMaxAngularSpeed(world, 100f);
 
             d.WorldSetCFM(world,1e-6f); // a bit harder than default
             //d.WorldSetCFM(world, 1e-4f); // a bit harder than default
@@ -564,6 +569,7 @@ namespace OpenSim.Region.Physics.OdePlugin
             // let this now be real maximum values
             spaceGridMaxX--;
             spaceGridMaxY--;
+            m_lastframe = DateTime.UtcNow;
         }
 
         internal void waitForSpaceUnlock(IntPtr space)
@@ -1685,35 +1691,30 @@ namespace OpenSim.Region.Physics.OdePlugin
         /// <returns></returns>
         public override float Simulate(float timeStep)
         {
-            int statstart;
-            int statchanges = 0;
-            int statchmove = 0;
-            int statactmove = 0;
-            int statray = 0;
-            int statcol = 0;
-            int statstep = 0;
-            int statmovchar = 0;
-            int statmovprim;
-            int totjcontact = 0;
 
+            DateTime now = DateTime.UtcNow;
+            TimeSpan SinceLastFrame = now - m_lastframe;
+            m_lastframe = now;
+            timeStep = (float)SinceLastFrame.TotalSeconds;
+            
             // acumulate time so we can reduce error
             step_time += timeStep;
 
-            if (step_time < ODE_STEPSIZE)
+            if (step_time < HalfOdeStep)
                 return 0;
 
-            if (framecount >= int.MaxValue)
+            if (framecount < 0)
                 framecount = 0;
 
             framecount++;
 
-            int curphysiteractions = m_physicsiterations;
+            int curphysiteractions;
 
+            // if in trouble reduce step resolution
             if (step_time >= m_SkipFramesAtms)
-            {
-                // if in trouble reduce step resolution
-                curphysiteractions /= 2;
-            }
+                curphysiteractions = m_physicsiterations / 2;
+            else
+                curphysiteractions = m_physicsiterations;
 
             int nodeframes = 0;
 
@@ -1733,13 +1734,10 @@ namespace OpenSim.Region.Physics.OdePlugin
                     base.TriggerPhysicsBasedRestart();
                 }
 
-
-                while (step_time >= ODE_STEPSIZE && nodeframes < 10) //limit number of steps so we don't say here for ever
+                while (step_time >= HalfOdeStep && nodeframes < 10) //limit number of steps so we don't say here for ever
                 {
                     try
                     {
-                        statstart = Util.EnvironmentTickCount();
-
                         // clear pointer/counter to contacts to pass into joints
                         m_global_contactcount = 0;
 
@@ -1778,17 +1776,39 @@ namespace OpenSim.Region.Physics.OdePlugin
 
                         }
 
-                        statchanges += Util.EnvironmentTickCountSubtract(statstart);
+                        // Move characters
+                        lock (_characters)
+                        {
+                            List<OdeCharacter> defects = new List<OdeCharacter>();
+                            foreach (OdeCharacter actor in _characters)
+                            {
+                                if (actor != null)
+                                    actor.Move(ODE_STEPSIZE, defects);
+                            }
+                            if (defects.Count != 0)
+                            {
+                                foreach (OdeCharacter defect in defects)
+                                {
+                                    RemoveCharacter(defect);
+                                }
+                            }
+                        }
+
+                        // Move other active objects
+                        lock (_activegroups)
+                        {
+                            foreach (OdePrim aprim in _activegroups)
+                            {
+                                aprim.Move();
+                            }
+                        }
 
-                        statactmove += Util.EnvironmentTickCountSubtract(statstart);
                         //if ((framecount % m_randomizeWater) == 0)
                         // randomizeWater(waterlevel);
 
                         m_rayCastManager.ProcessQueuedRequests();
 
-                        statray += Util.EnvironmentTickCountSubtract(statstart);
                         collision_optimized();
-                        statcol += Util.EnvironmentTickCountSubtract(statstart);
 
                         lock (_collisionEventPrim)
                         {
@@ -1813,38 +1833,39 @@ namespace OpenSim.Region.Physics.OdePlugin
                             }
                         }
 
+                        // do a ode simulation step
                         d.WorldQuickStep(world, ODE_STEPSIZE);
-                        statstep += Util.EnvironmentTickCountSubtract(statstart);
+                        d.JointGroupEmpty(contactgroup);
+
+                        // update managed ideia of physical data and do updates to core
+                        /*
+                                        lock (_characters)
+                                        {
+                                            foreach (OdeCharacter actor in _characters)
+                                            {
+                                                if (actor != null)
+                                                {
+                                                    if (actor.bad)
+                                                        m_log.WarnFormat("[PHYSICS]: BAD Actor {0} in _characters list was not removed?", actor.m_uuid);
+
+                                                    actor.UpdatePositionAndVelocity();
+                                                }
+                                            }
+                                        }
+                        */
 
-                        // Move characters
-                        lock (_characters)
+                        lock (_activegroups)
                         {
-                            List<OdeCharacter> defects = new List<OdeCharacter>();
-                            foreach (OdeCharacter actor in _characters)
-                            {
-                                if (actor != null)
-                                    actor.Move(ODE_STEPSIZE, defects);
-                            }
-                            if (defects.Count != 0)
                             {
-                                foreach (OdeCharacter defect in defects)
+                                foreach (OdePrim actor in _activegroups)
                                 {
-                                    RemoveCharacter(defect);
+                                    if (actor.IsPhysical)
+                                    {
+                                        actor.UpdatePositionAndVelocity();
+                                    }
                                 }
                             }
                         }
-                        statchmove += Util.EnvironmentTickCountSubtract(statstart);
-
-                        // Move other active objects
-                        lock (_activegroups)
-                        {
-                            foreach (OdePrim aprim in _activegroups)
-                            {
-                                aprim.Move();
-                            }
-                        }
-
-                        //ode.dunlock(world);
                     }
                     catch (Exception e)
                     {
@@ -1852,32 +1873,11 @@ namespace OpenSim.Region.Physics.OdePlugin
 //                        ode.dunlock(world);
                     }
 
-                    d.JointGroupEmpty(contactgroup);
-                    totjcontact += m_global_contactcount;
 
                     step_time -= ODE_STEPSIZE;
                     nodeframes++;
                 }
 
-                statstart = Util.EnvironmentTickCount();
-
-/*
-// now included in characters move() and done at ode rate
-//  maybe be needed later if we need to do any extra work at hearbeat rate
-                lock (_characters)
-                {
-                    foreach (OdeCharacter actor in _characters)
-                    {
-                        if (actor != null)
-                        {
-                            if (actor.bad)
-                                m_log.WarnFormat("[PHYSICS]: BAD Actor {0} in _characters list was not removed?", actor.m_uuid);
-
-                            actor.UpdatePositionAndVelocity();
-                        }
-                    }
-                }
-*/
                 lock (_badCharacter)
                 {
                     if (_badCharacter.Count > 0)
@@ -1890,22 +1890,6 @@ namespace OpenSim.Region.Physics.OdePlugin
                         _badCharacter.Clear();
                     }
                 }
-                statmovchar = Util.EnvironmentTickCountSubtract(statstart);
-
-                lock (_activegroups)
-                {
-                    {
-                        foreach (OdePrim actor in _activegroups)
-                        {
-                            if (actor.IsPhysical)
-                            {
-                                actor.UpdatePositionAndVelocity((float)nodeframes * ODE_STEPSIZE);
-                            }
-                        }
-                    }
-                }
-
-                statmovprim = Util.EnvironmentTickCountSubtract(statstart);
 
                 int nactivegeoms = d.SpaceGetNumGeoms(ActiveSpace);
                 int nstaticgeoms = d.SpaceGetNumGeoms(StaticSpace);
@@ -1932,15 +1916,17 @@ namespace OpenSim.Region.Physics.OdePlugin
                     d.WorldExportDIF(world, fname, physics_logging_append_existing_logfile, prefix);
                 }
                 
-                // think time dilation is not a physics issue alone..  but ok let's fake something
-                if (step_time < ODE_STEPSIZE) // we did the required loops
+                // think time dilation as to do with dinamic step size that we dont' have
+                // even so tell something to world
+                if (nodeframes < 10) // we did the requested loops
                     m_timeDilation = 1.0f;
-                else
-                { // we didn't forget the lost ones and let user know something
-                    m_timeDilation = 1 - step_time / timeStep;
-                    if (m_timeDilation < 0)
-                        m_timeDilation = 0;
-                    step_time = 0;
+                else if (step_time > 0)
+                {
+                    m_timeDilation = timeStep / step_time;
+                    if (m_timeDilation > 1)
+                        m_timeDilation = 1;
+                    if (step_time > m_SkipFramesAtms)
+                        step_time = 0;
                 }
             }
 
@@ -2007,7 +1993,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                 }
                 else // out world use external height
                 {
-                    ix = regsize - 1;
+                    ix = regsize - 2;
                     dx = 0;
                 }
                 if (y < regsize - 1)
@@ -2017,7 +2003,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                 }
                 else
                 {
-                    iy = regsize - 1;
+                    iy = regsize - 2;
                     dy = 0;
                 }
             }
@@ -2034,7 +2020,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                 }
                 else // out world use external height
                 {
-                    iy = regsize - 1;
+                    iy = regsize - 2;
                     dy = 0;
                 }
                 if (y < regsize - 1)
@@ -2044,7 +2030,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                 }
                 else
                 {
-                    ix = regsize - 1;
+                    ix = regsize - 2;
                     dx = 0;
                 }
             }
@@ -2057,18 +2043,35 @@ namespace OpenSim.Region.Physics.OdePlugin
             iy += ix; // all indexes have iy + ix
 
             float[] heights = TerrainHeightFieldHeights[heightFieldGeom];
+            /*
+                        if ((dx + dy) <= 1.0f)
+                        {
+                            h0 = ((float)heights[iy]); // 0,0 vertice
+                            h1 = (((float)heights[iy + 1]) - h0) * dx; // 1,0 vertice minus 0,0
+                            h2 = (((float)heights[iy + regsize]) - h0) * dy; // 0,1 vertice minus 0,0
+                        }
+                        else
+                        {
+                            h0 = ((float)heights[iy + regsize + 1]); // 1,1 vertice
+                            h1 = (((float)heights[iy + 1]) - h0) * (1 - dy); // 1,1 vertice minus 1,0
+                            h2 = (((float)heights[iy + regsize]) - h0) * (1 - dx); // 1,1 vertice minus 0,1
+                        }
+            */
+            h0 = ((float)heights[iy]); // 0,0 vertice
 
-            if ((dx + dy) <= 1.0f)
+            if ((dy > dx))
             {
-                h0 = ((float)heights[iy]); // 0,0 vertice
-                h1 = (((float)heights[iy + 1]) - h0) * dx; // 1,0 vertice minus 0,0
-                h2 = (((float)heights[iy + regsize]) - h0) * dy; // 0,1 vertice minus 0,0
+                iy += regsize;
+                h2 = (float)heights[iy]; // 0,1 vertice
+                h1 = (h2 - h0) * dy; // 0,1 vertice minus 0,0
+                h2 = ((float)heights[iy + 1] - h2) * dx; // 1,1 vertice minus 0,1
             }
             else
             {
-                h0 = ((float)heights[iy + regsize + 1]); // 1,1 vertice
-                h1 = (((float)heights[iy + 1]) - h0) * (1 - dy); // 1,1 vertice minus 1,0
-                h2 = (((float)heights[iy + regsize]) - h0) * (1 - dx); // 1,1 vertice minus 0,1
+                iy++;
+                h2 = (float)heights[iy]; // vertice 1,0
+                h1 = (h2 - h0) * dx; // 1,0 vertice minus 0,0
+                h2 = (((float)heights[iy + regsize]) - h2) * dy; // 1,1 vertice minus 1,0
             }
 
             return h0 + h1 + h2;