3 files changed, 104 insertions, 206 deletions

diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs
index 9c1b87b..ec4be58 100644
--- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs
@@ -683,7 +683,7 @@ namespace OpenSim.Region.Physics.OdePlugin
             PID_D *= m_mass / _parent_scene.ODE_STEPSIZE;
             PID_P /= 50 * 80;
             PID_P *= m_mass / _parent_scene.ODE_STEPSIZE;
-
+            
             Body = d.BodyCreate(_parent_scene.world);
 
             d.BodySetAutoDisableFlag(Body, false);
@@ -771,16 +771,10 @@ namespace OpenSim.Region.Physics.OdePlugin
         /// <param name="timeStep"></param>
         public void Move(float timeStep, List<OdeCharacter> defects)
         {
-            //  no lock; for now it's only called from within Simulate()
-
-            // If the PID Controller isn't active then we set our force
-            // calculating base velocity to the current position
-
             if (Body == IntPtr.Zero)
                 return;
 
-            d.Vector3 dtmp;
-            d.BodyCopyPosition(Body, out dtmp);
+            d.Vector3 dtmp = d.BodyGetPosition(Body);
             Vector3 localpos = new Vector3(dtmp.X, dtmp.Y, dtmp.Z);
 
             // the Amotor still lets avatar rotation to drift during colisions
@@ -797,22 +791,43 @@ namespace OpenSim.Region.Physics.OdePlugin
             {
                 _zeroPosition = localpos;
             }
-            //PidStatus = true;
-
 
             if (!localpos.IsFinite())
             {
-
                 m_log.Warn("[PHYSICS]: Avatar Position is non-finite!");
                 defects.Add(this);
                 // _parent_scene.RemoveCharacter(this);
 
                 // destroy avatar capsule and related ODE data
                 AvatarGeomAndBodyDestroy();
-
                 return;
             }
 
+            // check outbounds forcing to be in world
+            bool fixbody = false;
+            if (localpos.X < 0.0f)
+            {
+                fixbody = true;
+                localpos.X = 0.1f;
+            }
+            else if (localpos.X > _parent_scene.WorldExtents.X - 0.1f)
+            {
+                fixbody = true;
+                localpos.X = _parent_scene.WorldExtents.X - 0.1f;
+            }
+            if (localpos.Y < 0.0f)
+            {
+                fixbody = true;
+                localpos.Y = 0.1f;
+            }
+            else if (localpos.Y > _parent_scene.WorldExtents.Y - 0.1)
+            {
+                fixbody = true;
+                localpos.Y = _parent_scene.WorldExtents.Y - 0.1f;
+            }
+            if (fixbody)
+                d.BodySetPosition(Body, localpos.X, localpos.Y, localpos.Z);
+
             Vector3 vec = Vector3.Zero;
             dtmp = d.BodyGetLinearVel(Body);
             Vector3 vel = new Vector3(dtmp.X, dtmp.Y, dtmp.Z);
@@ -820,16 +835,12 @@ namespace OpenSim.Region.Physics.OdePlugin
             float movementdivisor = 1f;
             //Ubit change divisions into multiplications below
             if (!m_alwaysRun)
-            {
                 movementdivisor = 1 / walkDivisor;
-            }
             else
-            {
                 movementdivisor = 1 / runDivisor;
-            }
 
+            //******************************************
             // colide with land
-
             d.AABB aabb;
             d.GeomGetAABB(Shell, out aabb);
             float chrminZ = aabb.MinZ;
@@ -851,32 +862,12 @@ namespace OpenSim.Region.Physics.OdePlugin
                 float depth = terrainheight - chrminZ;
                 if (!flying)
                 {
-                    vec.Z = -vel.Z * PID_D * 1.5f + depth * PID_P * 50;
+                    vec.Z = -vel.Z * PID_D * 1.5f + depth * PID_P * 60;
                 }
                 else
-                    vec.Z = depth * PID_P * 50;
-
-                /*
-                                Vector3 vtmp;
-                                vtmp.X = _target_velocity.X * timeStep;
-                                vtmp.Y = _target_velocity.Y * timeStep;
-                                // fake and avoid squares
-                                float k = (Math.Abs(vtmp.X) + Math.Abs(vtmp.Y));
-                                if (k > 0)
-                                    {
-                                    posch.X += vtmp.X;
-                                    posch.Y += vtmp.Y;
-                                    terrainheight -= _parent_scene.GetTerrainHeightAtXY(posch.X, posch.Y);
-                                    k = 1 + Math.Abs(terrainheight) / k;
-                                    movementdivisor /= k;
-
-                                    if (k < 1)
-                                        k = 1;
-                                    }
-                */
+                    vec.Z = depth * PID_P * 60;
 
-
-                if (depth < 0.1f)
+                if (depth < 0.2f)
                 {
                     m_iscolliding = true;
                     m_colliderfilter = 2;
@@ -901,6 +892,7 @@ namespace OpenSim.Region.Physics.OdePlugin
             else
                 m_iscollidingGround = false;
 
+            //******************************************
 
             //  if velocity is zero, use position control; otherwise, velocity control
             if (_target_velocity.X == 0.0f && _target_velocity.Y == 0.0f && _target_velocity.Z == 0.0f
@@ -1012,97 +1004,31 @@ namespace OpenSim.Region.Physics.OdePlugin
                 // _parent_scene.RemoveCharacter(this);
                 // destroy avatar capsule and related ODE data
                 AvatarGeomAndBodyDestroy();
+                return;
             }
+
+            // update our local ideia of position velocity and aceleration
+            _position = localpos;
+            _acceleration = _velocity; // previus velocity
+            _velocity = vel;
+            _acceleration = (vel - _acceleration) / timeStep;
+          
         }
 
         /// <summary>
-        /// Updates the reported position and velocity.  This essentially sends the data up to ScenePresence.
+        /// Updates the reported position and velocity.
+        /// Used to copy variables from unmanaged space at heartbeat rate and also trigger scene updates acording
+        /// also outbounds checking
+        /// copy and outbounds now done in move(..) at ode rate
+        /// 
         /// </summary>
         public void UpdatePositionAndVelocity()
         {
-            //  no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit!
-            if (Body == IntPtr.Zero)
-                return;
-
-            d.Vector3 vec;
-            try
-            {
-                d.BodyCopyPosition(Body, out vec);
-            }
-            catch (NullReferenceException)
-            {
-                bad = true;
-                _parent_scene.BadCharacter(this);
-                vec = new d.Vector3(_position.X, _position.Y, _position.Z);
-                base.RaiseOutOfBounds(_position); // Tells ScenePresence that there's a problem!
-                m_log.WarnFormat("[ODEPLUGIN]: Avatar Null reference for Avatar {0}, physical actor {1}", m_name, m_uuid);
-            }
-
-            _position.X = vec.X;
-            _position.Y = vec.Y;
-            _position.Z = vec.Z;
+            return;
 
-            bool fixbody = false;
-
-            if (_position.X < 0.0f)
-            {
-                fixbody = true;
-                _position.X = 0.1f;
-            }
-            else if (_position.X > (int)_parent_scene.WorldExtents.X - 0.1f)
-            {
-                fixbody = true;
-                _position.X = (int)_parent_scene.WorldExtents.X - 0.1f;
-            }
+//            if (Body == IntPtr.Zero)
+//                return;
 
-            if (_position.Y < 0.0f)
-            {
-                fixbody = true;
-                _position.Y = 0.1f;
-            }
-            else if (_position.Y > (int)_parent_scene.WorldExtents.Y - 0.1)
-            {
-                fixbody = true;
-                _position.Y = (int)_parent_scene.WorldExtents.Y - 0.1f;
-            }
-
-            if (fixbody)
-                d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
-
-            // Did we move last? = zeroflag
-            // This helps keep us from sliding all over
-/*
-            if (_zeroFlag)
-            {
-                _velocity.X = 0.0f;
-                _velocity.Y = 0.0f;
-                _velocity.Z = 0.0f;
-
-                // Did we send out the 'stopped' message?
-                if (!m_lastUpdateSent)
-                {
-                    m_lastUpdateSent = true;
-                    base.RequestPhysicsterseUpdate();
-                }
-            }
-            else
-            {
-                m_lastUpdateSent = false;
- */
-                try
-                {
-                    vec = d.BodyGetLinearVel(Body);
-                }
-                catch (NullReferenceException)
-                {
-                    vec.X = _velocity.X;
-                    vec.Y = _velocity.Y;
-                    vec.Z = _velocity.Z;
-                }
-                _velocity.X = (vec.X);
-                _velocity.Y = (vec.Y);
-                _velocity.Z = (vec.Z);
- //           }
         }
 
         /// <summary>
@@ -1245,7 +1171,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                         CAPSULE_LENGTH = caplen;
 
                         AvatarGeomAndBodyCreation(_position.X, _position.Y,
-                                          _position.Z + (Math.Abs(CAPSULE_LENGTH - prevCapsule) * 2));
+                                          _position.Z + (CAPSULE_LENGTH - prevCapsule) * 0.5f);
 
                         Velocity = Vector3.Zero;
 
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs
index 32c4722..5467b9f 100644
--- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs
@@ -3228,17 +3228,13 @@ namespace OpenSim.Region.Physics.OdePlugin
         {

             if (!childPrim && m_isphysical && Body != IntPtr.Zero &&

                 !m_disabled && !m_isSelected && !m_building && !m_outbounds)

-            //                  !m_disabled && !m_isSelected && !m_building && !m_outbounds)

             {

-                //                if (!d.BodyIsEnabled(Body)) d.BodyEnable(Body); // KF add 161009

-

                 if (d.BodyIsEnabled(Body))

                 {

                     float timestep = _parent_scene.ODE_STEPSIZE;

 

                     // check outside region

-                    d.Vector3 lpos;

-                    d.GeomCopyPosition(prim_geom, out lpos); // root position that is seem by rest of simulator

+                    d.Vector3 lpos = d.GeomGetPosition(prim_geom); // root position that is seem by rest of simulator

 

                     if (lpos.Z < -100 || lpos.Z > 100000f)

                     {

@@ -3321,12 +3317,9 @@ namespace OpenSim.Region.Physics.OdePlugin
                     {

                         // 'VEHICLES' are dealt with in ODEDynamics.cs

                         m_vehicle.Step();

-                        return;

                     }

-

                     else

                     {

-

                         float fx = 0;

                         float fy = 0;

                         float fz = 0;

@@ -3512,10 +3505,39 @@ namespace OpenSim.Region.Physics.OdePlugin
                         {

                             d.BodyAddTorque(Body, trq.X, trq.Y, trq.Z);

                         }

-

                     }

+

+                    // update our ideia of velocities and acelerations

+                    d.Quaternion ori;

+                    d.Vector3 dtmpu;

+

+                    _position.X = lpos.X;

+                    _position.Y = lpos.Y;

+                    _position.Z = lpos.Z;

+

+                    d.GeomCopyQuaternion(prim_geom, out ori);

+                    _orientation.X = ori.X;

+                    _orientation.Y = ori.Y;

+                    _orientation.Z = ori.Z;

+                    _orientation.W = ori.W;

+

+                    _acceleration = _velocity;

+

+                    dtmpu = d.BodyGetLinearVel(Body);

+                    _velocity.X = dtmpu.X;

+                    _velocity.Y = dtmpu.Y;

+                    _velocity.Z = dtmpu.Z;

+

+                    float invts = 1 / timestep;

+                    _acceleration = (_velocity - _acceleration) * invts;

+

+                    dtmpu = d.BodyGetAngularVel(Body);

+                    m_rotationalVelocity.X = dtmpu.X;

+                    m_rotationalVelocity.Y = dtmpu.Y;

+                    m_rotationalVelocity.Z = dtmpu.Z;

                 }

-                else // body disabled

+

+                else // body disabled/sleeping

                 {

                     // let vehicles sleep

                     if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE)

@@ -3546,36 +3568,23 @@ namespace OpenSim.Region.Physics.OdePlugin
             {

                 if (Body != IntPtr.Zero)

                 {

-                    Vector3 pv = Vector3.Zero;

                     bool lastZeroFlag = _zeroFlag;

 

-                    d.Vector3 lpos;

-                    d.GeomCopyPosition(prim_geom, out lpos); // root position that is seem by rest of simulator

-

-

-                    d.Quaternion ori;

-                    d.GeomCopyQuaternion(prim_geom, out ori);

-                    d.Vector3 vel = d.BodyGetLinearVel(Body);

-                    d.Vector3 rotvel = d.BodyGetAngularVel(Body);

-

-                    if ((Math.Abs(m_lastposition.X - lpos.X) < 0.01)

-                        && (Math.Abs(m_lastposition.Y - lpos.Y) < 0.01)

-                        && (Math.Abs(m_lastposition.Z - lpos.Z) < 0.01)

-                        && (Math.Abs(m_lastorientation.X - ori.X) < 0.0001)

-                        && (Math.Abs(m_lastorientation.Y - ori.Y) < 0.0001)

-                        && (Math.Abs(m_lastorientation.Z - ori.Z) < 0.0001)

+                    if ((Math.Abs(m_lastposition.X - _position.X) < 0.01)

+                        && (Math.Abs(m_lastposition.Y - _position.Y) < 0.01)

+                        && (Math.Abs(m_lastposition.Z - _position.Z) < 0.01)

+                        && (Math.Abs(m_lastorientation.X - _orientation.X) < 0.0001)

+                        && (Math.Abs(m_lastorientation.Y - _orientation.Y) < 0.0001)

+                        && (Math.Abs(m_lastorientation.Z - _orientation.Z) < 0.0001)

                         )

                     {

                         _zeroFlag = true;

-                        //Console.WriteLine("ZFT 2");

                         m_throttleUpdates = false;

                     }

                     else

                     {

-                        //m_log.Debug(Math.Abs(m_lastposition.X - l_position.X).ToString());

                         _zeroFlag = false;

                         m_lastUpdateSent = false;

-                        //m_throttleUpdates = false;

                     }

 

                     if (_zeroFlag)

@@ -3583,22 +3592,14 @@ namespace OpenSim.Region.Physics.OdePlugin
                         m_lastposition = _position;

                         m_lastorientation = _orientation;

 

-                        _velocity.X = 0.0f;

-                        _velocity.Y = 0.0f;

-                        _velocity.Z = 0.0f;

+                        _velocity = Vector3.Zero;

+                        _acceleration = Vector3.Zero;

+                        m_rotationalVelocity = Vector3.Zero;

 

-                        _acceleration.X = 0;

-                        _acceleration.Y = 0;

-                        _acceleration.Z = 0;

-

-                        m_rotationalVelocity.X = 0;

-                        m_rotationalVelocity.Y = 0;

-                        m_rotationalVelocity.Z = 0;

                         if (!m_lastUpdateSent)

                         {

                             m_throttleUpdates = false;

                             throttleCounter = 0;

-                            m_rotationalVelocity = pv;

 

                             base.RequestPhysicsterseUpdate();

 

@@ -3612,39 +3613,12 @@ namespace OpenSim.Region.Physics.OdePlugin
                             base.RequestPhysicsterseUpdate();

                         }

 

-                        m_lastVelocity = _velocity;

-

-                        _position.X = lpos.X;

-                        _position.Y = lpos.Y;

-                        _position.Z = lpos.Z;

-

-                        _velocity.X = vel.X;

-                        _velocity.Y = vel.Y;

-                        _velocity.Z = vel.Z;

-

-                        _orientation.X = ori.X;

-                        _orientation.Y = ori.Y;

-                        _orientation.Z = ori.Z;

-                        _orientation.W = ori.W;

-

-                        _acceleration = ((_velocity - m_lastVelocity) / simulatedtime);

-

-                        if (m_rotationalVelocity.ApproxEquals(pv, 0.0001f))

-                        {

-                            m_rotationalVelocity = pv;

-                        }

-                        else

-                        {

-                            m_rotationalVelocity.X = rotvel.X;

-                            m_rotationalVelocity.Y = rotvel.Y;

-                            m_rotationalVelocity.Z = rotvel.Z;

-                        }

-

                         m_lastUpdateSent = false;

                         if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate)

                         {

                             m_lastposition = _position;

                             m_lastorientation = _orientation;

+                            m_lastVelocity = _velocity;

                             base.RequestPhysicsterseUpdate();

                         }

                         else

@@ -3656,17 +3630,11 @@ namespace OpenSim.Region.Physics.OdePlugin
                 else if (!m_lastUpdateSent || !_zeroFlag)

                 {

                     // Not a body..   so Make sure the client isn't interpolating

-                    _velocity.X = 0;

-                    _velocity.Y = 0;

-                    _velocity.Z = 0;

-

-                    _acceleration.X = 0;

-                    _acceleration.Y = 0;

-                    _acceleration.Z = 0;

+                    _velocity = Vector3.Zero;

+                    _acceleration = Vector3.Zero;

+                    m_rotationalVelocity = Vector3.Zero;

+                    m_lastVelocity = Vector3.Zero;

 

-                    m_rotationalVelocity.X = 0;

-                    m_rotationalVelocity.Y = 0;

-                    m_rotationalVelocity.Z = 0;

                     _zeroFlag = true;

 

                     if (!m_lastUpdateSent)

diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs
index 7632e25..9ca2d3f 100644
--- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs
+++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs
@@ -455,15 +455,15 @@ namespace OpenSim.Region.Physics.OdePlugin
 
                     geomDefaultDensity = physicsconfig.GetFloat("geometry_default_density", geomDefaultDensity);
                     bodyFramesAutoDisable = physicsconfig.GetInt("body_frames_auto_disable", bodyFramesAutoDisable);
-
+/*
                     bodyPIDD = physicsconfig.GetFloat("body_pid_derivative", bodyPIDD);
                     bodyPIDG = physicsconfig.GetFloat("body_pid_gain", bodyPIDG);
-
+*/
                     forceSimplePrimMeshing = physicsconfig.GetBoolean("force_simple_prim_meshing", forceSimplePrimMeshing);
                     meshSculptedPrim = physicsconfig.GetBoolean("mesh_sculpted_prim", meshSculptedPrim);
                     meshSculptLOD = physicsconfig.GetFloat("mesh_lod", meshSculptLOD);
                     MeshSculptphysicalLOD = physicsconfig.GetFloat("mesh_physical_lod", MeshSculptphysicalLOD);
-
+/*
                     if (Environment.OSVersion.Platform == PlatformID.Unix)
                     {
                         avPIDD = physicsconfig.GetFloat("av_pid_derivative_linux", avPIDD);
@@ -471,10 +471,11 @@ namespace OpenSim.Region.Physics.OdePlugin
                     }
                     else
                     {
+ 
                         avPIDD = physicsconfig.GetFloat("av_pid_derivative_win", avPIDD);
                         avPIDP = physicsconfig.GetFloat("av_pid_proportional_win", avPIDP);
                     }
-
+*/
                     physics_logging = physicsconfig.GetBoolean("physics_logging", false);
                     physics_logging_interval = physicsconfig.GetInt("physics_logging_interval", 0);
                     physics_logging_append_existing_logfile = physicsconfig.GetBoolean("physics_logging_append_existing_logfile", false);
@@ -1860,6 +1861,9 @@ namespace OpenSim.Region.Physics.OdePlugin
 
                 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)
@@ -1873,7 +1877,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                         }
                     }
                 }
-
+*/
                 lock (_badCharacter)
                 {
                     if (_badCharacter.Count > 0)