3 files changed, 75 insertions, 38 deletions

diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs
index 78b15be..ef2dccc 100644
--- a/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs
+++ b/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs
@@ -83,6 +83,12 @@ namespace OpenSim.Region.Physics.OdePlugin
 //        private IntPtr m_jointGroup = IntPtr.Zero;
 //        private IntPtr m_aMotor = IntPtr.Zero;
  
+                // Correction factors, to match Sl
+                private static float m_linearVelocityFactor = 0.9f;
+                private static float m_linearAttackFactor = 0.4f;
+                private static float m_linearDecayFactor = 0.5f;
+                private static float m_linearFrictionFactor = 1.2f;
+
 
         // Vehicle properties
         private Vehicle m_type = Vehicle.TYPE_NONE;                                             // If a 'VEHICLE', and what kind
@@ -98,7 +104,7 @@ namespace OpenSim.Region.Physics.OdePlugin
         
         // Linear properties
         private Vector3 m_linearMotorDirection = Vector3.Zero;                  // velocity requested by LSL, decayed by time
-        private Vector3 m_linearMotorDirectionLASTSET = Vector3.Zero;   // velocity requested by LSL
+        private Vector3 m_linearMotorDirectionLASTSET = Vector3.Zero;   // velocity requested by LSL, for max limiting
         private Vector3 m_dir = Vector3.Zero;                                                   // velocity applied to body
         private Vector3 m_linearFrictionTimescale = Vector3.Zero;
         private float m_linearMotorDecayTimescale = 0;
@@ -267,8 +273,9 @@ namespace OpenSim.Region.Physics.OdePlugin
                     m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
                     break;
                 case Vehicle.LINEAR_MOTOR_DIRECTION:
-                    m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
-                    m_linearMotorDirectionLASTSET = new Vector3(pValue.X, pValue.Y, pValue.Z);
+                        pValue *= m_linearVelocityFactor;
+                    m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);                 // velocity requested by LSL, decayed by time
+                    m_linearMotorDirectionLASTSET = new Vector3(pValue.X, pValue.Y, pValue.Z);  // velocity requested by LSL, for max limiting
                     break;
                 case Vehicle.LINEAR_MOTOR_OFFSET:
                     // m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z);
@@ -453,6 +460,17 @@ namespace OpenSim.Region.Physics.OdePlugin
             MoveAngular(pTimestep);
         }// end Step
 
+                internal void Halt()
+                {       // Kill all motions, when non-physical
+                        m_linearMotorDirection = Vector3.Zero;
+                        m_linearMotorDirectionLASTSET = Vector3.Zero;
+                        m_dir = Vector3.Zero;                                           
+                        m_lastLinearVelocityVector = Vector3.Zero;
+                        m_angularMotorDirection = Vector3.Zero;         
+                        m_angularMotorVelocity = Vector3.Zero;          
+                        m_lastAngularVelocity = Vector3.Zero;           
+                }
+
         private void MoveLinear(float pTimestep, OdeScene _pParentScene)
         {
             if (!m_linearMotorDirection.ApproxEquals(Vector3.Zero, 0.01f))              // requested m_linearMotorDirection is significant
@@ -460,9 +478,15 @@ namespace OpenSim.Region.Physics.OdePlugin
                  if(!d.BodyIsEnabled (Body))  d.BodyEnable (Body);
 
                 // add drive to body
-                Vector3 addAmount = m_linearMotorDirection/(m_linearMotorTimescale/pTimestep);
-                m_lastLinearVelocityVector += (addAmount*10);                                   // lastLinearVelocityVector is the current body velocity vector?
-        
+                float linfactor = m_linearMotorTimescale/pTimestep;
+                // Linear accel
+                Vector3 addAmount1 = (m_linearMotorDirection/linfactor) * 0.8f; 
+                // Differential accel
+                Vector3 addAmount2 = ((m_linearMotorDirection - m_lastLinearVelocityVector)/linfactor) * 1.6f;
+                // SL correction
+                Vector3 addAmount = (addAmount1 + addAmount2) * m_linearAttackFactor;
+                m_lastLinearVelocityVector += addAmount;                                        // lastLinearVelocityVector is the current body velocity vector
+//if(frcount == 0) Console.WriteLine("AL {0}  +  AD {1}    AS{2}    V {3}", addAmount1, addAmount2, addAmount, m_lastLinearVelocityVector);   
                 // This will work temporarily, but we really need to compare speed on an axis
                 // KF: Limit body velocity to applied velocity?
                 if (Math.Abs(m_lastLinearVelocityVector.X) > Math.Abs(m_linearMotorDirectionLASTSET.X))
@@ -475,7 +499,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                 // decay applied velocity
                 Vector3 decayfraction = ((Vector3.One/(m_linearMotorDecayTimescale/pTimestep)));
                 //Console.WriteLine("decay: " + decayfraction);
-                m_linearMotorDirection -= m_linearMotorDirection * decayfraction * 0.5f;
+                m_linearMotorDirection -= m_linearMotorDirection * decayfraction * m_linearDecayFactor;
                 //Console.WriteLine("actual: " + m_linearMotorDirection);
             }
             else
@@ -560,7 +584,7 @@ namespace OpenSim.Region.Physics.OdePlugin
 
                         // apply friction
             Vector3 decayamount = Vector3.One / (m_linearFrictionTimescale / pTimestep);
-            m_lastLinearVelocityVector -= m_lastLinearVelocityVector * decayamount;
+            m_lastLinearVelocityVector -= m_lastLinearVelocityVector * decayamount  * m_linearFrictionFactor;
         } // end MoveLinear()
         
         private void MoveAngular(float pTimestep)
diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
index 0179240..6e6b44f 100644
--- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
+++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
@@ -2296,11 +2296,15 @@ Console.WriteLine(" JointCreateFixed");
         public override bool IsPhysical
         {
             get { return m_isphysical; }
-            set { 
+            set 
+            { 
                   m_isphysical = value;
-                  if (!m_isphysical) // Zero the remembered last velocity
-                      m_lastVelocity = Vector3.Zero;
-                }
+                  if (!m_isphysical)
+                  {             // Zero the remembered last velocity
+                                m_lastVelocity = Vector3.Zero;
+                            if (m_vehicle.Type != Vehicle.TYPE_NONE) m_vehicle.Halt();
+                  }
+            }
         }
 
         public void setPrimForRemoval()
diff --git a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs
index f48649e..60786d4 100644
--- a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs
+++ b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs
@@ -229,7 +229,7 @@ namespace OpenSim.Region.Physics.OdePlugin
 
         public int bodyFramesAutoDisable = 20;
 
-        
+        protected DateTime m_lastframe = DateTime.UtcNow;
 
         private float[] _watermap;
         private bool m_filterCollisions = true;
@@ -2639,13 +2639,20 @@ namespace OpenSim.Region.Physics.OdePlugin
         {
             if (framecount >= int.MaxValue)
                 framecount = 0;
-
             //if (m_worldOffset != Vector3.Zero)
             //    return 0;
 
             framecount++;
-
-            float fps = 0;
+            
+            DateTime now = DateTime.UtcNow;
+                        TimeSpan SinceLastFrame = now - m_lastframe;
+                        m_lastframe = now;
+                        float realtime = (float)SinceLastFrame.TotalSeconds;
+// Console.WriteLine("ts={0}    rt={1}",        timeStep,       realtime);      
+                        timeStep = realtime;
+                        
+  //          float fps = 1.0f / realtime;
+            float fps = 0.0f;           // number of ODE steps in this Simulate step
             //m_log.Info(timeStep.ToString());
             step_time += timeStep;
             
@@ -2691,11 +2698,11 @@ namespace OpenSim.Region.Physics.OdePlugin
                 // Figure out the Frames Per Second we're going at.
                 //(step_time == 0.004f, there's 250 of those per second.   Times the step time/step size
 
-                fps = (step_time / ODE_STEPSIZE) * 1000;
+ //               fps = (step_time / ODE_STEPSIZE) * 1000;
                 // HACK: Using a time dilation of 1.0 to debug rubberbanding issues
                 //m_timeDilation = Math.Min((step_time / ODE_STEPSIZE) / (0.09375f / ODE_STEPSIZE), 1.0f);
 
-                step_time = 0.09375f;
+ //               step_time = 0.09375f;
 
                 while (step_time > 0.0f)
                 {
@@ -2716,7 +2723,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                                         foreach (OdeCharacter character in _taintedActors)
                                         {
 
-                                            character.ProcessTaints(timeStep);
+                                            character.ProcessTaints(ODE_STEPSIZE);
 
                                             processedtaints = true;
                                             //character.m_collisionscore = 0;
@@ -2725,7 +2732,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                                         if (processedtaints)
                                             _taintedActors.Clear();
                                     }
-                                }
+                                } // end lock _taintedActors
 
                                 // Modify other objects in the scene.
                                 processedtaints = false;
@@ -2742,7 +2749,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                                         else
                                         {
                                             //Console.WriteLine("Simulate calls ProcessTaints");                                        
-                                            prim.ProcessTaints(timeStep);
+                                            prim.ProcessTaints(ODE_STEPSIZE);
                                         }
                                         processedtaints = true;
                                         prim.m_collisionscore = 0;
@@ -2767,7 +2774,8 @@ namespace OpenSim.Region.Physics.OdePlugin
                                             foreach (PhysicsJoint joint in pendingJoints)
                                             {
                                                 //DoJointErrorMessage(joint, "taint: time to create joint with parms: " + joint.RawParams);
-                                                string[] jointParams = joint.RawParams.Split(" ".ToCharArray(), System.StringSplitOptions.RemoveEmptyEntries);
+                                                string[] jointParams = joint.RawParams.Split(" ".ToCharArray(), 
+                                                                                                System.StringSplitOptions.RemoveEmptyEntries);
                                                 List<IntPtr> jointBodies = new List<IntPtr>();
                                                 bool allJointBodiesAreReady = true;
                                                 foreach (string jointParam in jointParams)
@@ -2934,13 +2942,13 @@ namespace OpenSim.Region.Physics.OdePlugin
                                                 //DoJointErrorMessage(successfullyProcessedJoint, "done");
                                             }
                                         }
-                                    }
+                                    } // end SupportsNINJAJoints 
 
                                     if (processedtaints)
 //Console.WriteLine("Simulate calls Clear of _taintedPrim list");                                      
-                                        _taintedPrimH.Clear();
+                                        _taintedPrimH.Clear();                          // ??? if this only ???
                                         _taintedPrimL.Clear();
-                                }
+                                } // end lock _taintedPrimLock
 
                                 // Move characters
                                 lock (_characters)
@@ -2949,7 +2957,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                                     foreach (OdeCharacter actor in _characters)
                                     {
                                         if (actor != null)
-                                            actor.Move(timeStep, defects);
+                                            actor.Move(ODE_STEPSIZE, defects);
                                     }
                                     if (0 != defects.Count)
                                     {
@@ -2958,7 +2966,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                                             RemoveCharacter(defect);
                                         }
                                     }
-                                }
+                                } // end lock _characters
 
                                 // Move other active objects
                                 lock (_activeprims)
@@ -2966,9 +2974,9 @@ namespace OpenSim.Region.Physics.OdePlugin
                                     foreach (OdePrim prim in _activeprims)
                                     {
                                         prim.m_collisionscore = 0;
-                                        prim.Move(timeStep);
+                                        prim.Move(ODE_STEPSIZE);
                                     }
-                                }
+                                } // end lock _activeprims
 
                                 //if ((framecount % m_randomizeWater) == 0)
                                    // randomizeWater(waterlevel);
@@ -2976,7 +2984,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                                 //int RayCastTimeMS = m_rayCastManager.ProcessQueuedRequests();
                                 m_rayCastManager.ProcessQueuedRequests();
 
-                                collision_optimized(timeStep);
+                                collision_optimized(ODE_STEPSIZE);
 
                                 lock (_collisionEventPrim)
                                 {
@@ -2998,7 +3006,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                                                 break;
                                         }
                                     }
-                                }
+                                } // end lock _collisionEventPrim
 
                                 //if (m_global_contactcount > 5)
                                 //{
@@ -3009,8 +3017,9 @@ namespace OpenSim.Region.Physics.OdePlugin
                                 
                                 d.WorldQuickStep(world, ODE_STEPSIZE);
                                 d.JointGroupEmpty(contactgroup);
+                                fps++;
                                 //ode.dunlock(world);
-                            }
+                            } // end try
                             catch (Exception e)
                             {
                                 m_log.ErrorFormat("[PHYSICS]: {0}, {1}, {2}", e.Message, e.TargetSite, e);
@@ -3025,7 +3034,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                             //fps = 0;
                         //}
                     //}
-                }
+                }  // end while (step_time > 0.0f)
 
                 lock (_characters)
                 {
@@ -3090,7 +3099,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                             }
                         }
                     }
-                }
+                } // end lock _activeprims
 
                 //DumpJointInfo();
 
@@ -3111,10 +3120,10 @@ namespace OpenSim.Region.Physics.OdePlugin
                     }
                     d.WorldExportDIF(world, fname, physics_logging_append_existing_logfile, prefix);
                 }
-            }
-
-            return fps;
-        }
+            } // end lock OdeLock
+            
+            return fps * 1000.0f;               //NB This is a FRAME COUNT, not a time! AND is divide by 1000 in SimStatusReporter!
+        } // end Simulate
 
         public override void GetResults()
         {