1 files changed, 64 insertions, 16 deletions

diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs
index c9d0909..d0b4546 100644
--- a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs
@@ -38,7 +38,7 @@
  * settings use.
  */
 
-// Ubit 2012
+// Extensive change Ubit 2012
 
 using System;
 using System.Collections.Generic;
@@ -119,7 +119,15 @@ namespace OpenSim.Region.Physics.OdePlugin
         // auxiliar
         private float m_lmEfect = 0;                                            // current linear motor eficiency
         private float m_amEfect = 0;                                            // current angular motor eficiency
+        private float m_ffactor = 1.0f;
 
+        public float FrictionFactor
+        {
+            get
+            {
+                return m_ffactor;
+            }
+        }
 
         public ODEDynamics(OdePrim rootp)
         {
@@ -127,7 +135,6 @@ namespace OpenSim.Region.Physics.OdePlugin
             _pParentScene = rootPrim._parent_scene;
         }
 
-
         public void DoSetVehicle(VehicleData vd)
         {
 
@@ -152,6 +159,7 @@ namespace OpenSim.Region.Physics.OdePlugin
             m_linearMotorTimescale = vd.m_linearMotorTimescale;
             if (m_linearMotorTimescale < timestep) m_linearMotorTimescale = timestep;
 
+
             m_linearMotorOffset = vd.m_linearMotorOffset;
 
             //Angular properties
@@ -201,6 +209,7 @@ namespace OpenSim.Region.Physics.OdePlugin
 
             m_lmEfect = 0;
             m_amEfect = 0;
+            m_ffactor = 1.0f;
         }
 
         internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue)
@@ -318,6 +327,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                     if (len > 30.0f)
                         m_linearMotorDirection *= (30.0f / len);
                     m_lmEfect = 1.0f; // turn it on
+                    m_ffactor = 0.01f;
                     if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body)
                             && !rootPrim.m_isSelected && !rootPrim.m_disabled)
                         d.BodyEnable(rootPrim.Body);
@@ -368,6 +378,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                     if (len > 30.0f)
                         m_linearMotorDirection *= (30.0f / len);
                     m_lmEfect = 1.0f; // turn it on
+                    m_ffactor = 0.01f;
                     if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body)
                             && !rootPrim.m_isSelected && !rootPrim.m_disabled)
                         d.BodyEnable(rootPrim.Body);
@@ -414,6 +425,7 @@ namespace OpenSim.Region.Physics.OdePlugin
             float invtimestep = _pParentScene.ODE_STEPSIZE;
             m_lmEfect = 0;
             m_amEfect = 0;
+            m_ffactor = 1f;
 
             m_linearMotorDirection = Vector3.Zero;
             m_angularMotorDirection = Vector3.Zero;
@@ -591,6 +603,7 @@ namespace OpenSim.Region.Physics.OdePlugin
         {
             m_lmEfect = 0;
             m_amEfect = 0;
+            m_ffactor = 1f;
         }
 
         public static Vector3 Xrot(Quaternion rot)
@@ -614,6 +627,7 @@ namespace OpenSim.Region.Physics.OdePlugin
             return vec;
         }
 
+        private const float pi = (float)Math.PI;
         private const float halfpi = 0.5f * (float)Math.PI;
 
         public static Vector3 ubitRot2Euler(Quaternion rot)
@@ -740,9 +754,14 @@ namespace OpenSim.Region.Physics.OdePlugin
                     force.Z += tmpV.Z;
                 }
                 m_lmEfect *= (1.0f - 1.0f / m_linearMotorDecayTimescale);
+
+                m_ffactor = 0.01f + 1e-4f * curVel.LengthSquared();
             }
             else
+            {
                 m_lmEfect = 0;
+                m_ffactor = 1f;
+            }
 
             // friction
             if (curLocalVel.X != 0 || curLocalVel.Y != 0 || curLocalVel.Z != 0)
@@ -884,35 +903,64 @@ namespace OpenSim.Region.Physics.OdePlugin
                 float ftmp = 1.0f / m_verticalAttractionTimescale / m_verticalAttractionTimescale / _pParentScene.ODE_STEPSIZE;
                 float ftmp2 = m_verticalAttractionEfficiency / _pParentScene.ODE_STEPSIZE;
 
-                if (Math.Abs(roll) > 0.01) // roll
+                if (roll > halfpi)
+                    roll = pi - roll;
+                else if (roll < -halfpi)
+                    roll = -pi - roll;                           
+
+                float effroll = pitch / halfpi;
+                effroll *= effroll;
+                effroll = 1 - effroll;
+                effroll *= roll;
+
+                if (Math.Abs(effroll) > 0.01) // roll
                 {
-                    torque.X -= -roll * ftmp + curLocalAngVel.X * ftmp2;
+                    torque.X -= -effroll * ftmp + curLocalAngVel.X * ftmp2;
                 }
 
-                if (Math.Abs(pitch) > 0.01 && ((m_flags & VehicleFlag.LIMIT_ROLL_ONLY) == 0)) // pitch
+                if ((m_flags & VehicleFlag.LIMIT_ROLL_ONLY) == 0)
                 {
-                    torque.Y -= -pitch * ftmp + curLocalAngVel.Y * ftmp2;
+                    float effpitch = roll / halfpi;
+                    effpitch *= effpitch;
+                    effpitch = 1 - effpitch;
+                    effpitch *= pitch;
+                    
+                    if (Math.Abs(effpitch) > 0.01) // pitch
+                    {
+                        torque.Y -= -effpitch * ftmp + curLocalAngVel.Y * ftmp2;
+                    }
                 }
 
-                if (m_bankingEfficiency != 0 && Math.Abs(roll) > 0.01)
+                if (m_bankingEfficiency != 0 && Math.Abs(effroll) > 0.01)
                 {
-                    float broll = roll * m_bankingEfficiency; ;
+
+                    float broll = effroll;
+/*
+                    if (broll > halfpi)
+                        broll = pi - broll;
+                    else if (broll < -halfpi)
+                        broll = -pi - broll;                           
+*/                    
+                    broll *= m_bankingEfficiency; 
                     if (m_bankingMix != 0)
                     {
                         float vfact = Math.Abs(curLocalVel.X) / 10.0f;
                         if (vfact > 1.0f) vfact = 1.0f;
+
                         if (curLocalVel.X >= 0)
-                            broll *= ((1 - m_bankingMix) + vfact);
+                            broll *= (1 + (vfact - 1) * m_bankingMix);
                         else
-                            broll *= -((1 - m_bankingMix) + vfact);                      
+                            broll *= -(1 + (vfact - 1) * m_bankingMix);                      
                     }
-                    broll = (broll - curLocalAngVel.Z) / m_bankingTimescale;
-                    //                    torque.Z += broll; 
-
                     // make z rot be in world Z not local as seems to be in sl
-                    tmpV.X = 0;
-                    tmpV.Y = 0;
-                    tmpV.Z = broll;
+
+                    broll = broll / m_bankingTimescale;
+
+                    ftmp = -Math.Abs(m_bankingEfficiency) / m_bankingTimescale;
+
+                    tmpV.X = ftmp * curAngVel.X;
+                    tmpV.Y = ftmp * curAngVel.Y;
+                    tmpV.Z = broll + ftmp * curAngVel.Z;
                     tmpV *= irotq;
 
                     torque.X += tmpV.X;