ubitode: changes to vehicles servos

1 files changed, 154 insertions, 100 deletions

diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs
index 8f2feba..e88e559 100644
--- a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs
@@ -117,14 +117,23 @@ namespace OpenSim.Region.Physics.OdePlugin
 
 
         // auxiliar
-        private float m_lmEfect = 0;                                            // current linear motor eficiency
-        private float m_lmDecay = 1.0f;
+        private float m_lmEfect = 0f;                                            // current linear motor eficiency
+        private float m_lmDecay = 0f;                                            // current linear decay
+
         private float m_amEfect = 0;                                            // current angular motor eficiency
+        private float m_amDecay = 0f;                                            // current linear decay
+
         private float m_ffactor = 1.0f;
 
         private float m_timestep = 0.02f;
         private float m_invtimestep = 50;
 
+
+        float m_ampwr;
+        float m_amdampX;
+        float m_amdampY;
+        float m_amdampZ;
+
         public float FrictionFactor
         {
             get
@@ -146,6 +155,7 @@ namespace OpenSim.Region.Physics.OdePlugin
             m_type = vd.m_type;
             m_flags = vd.m_flags;
 
+            
             // Linear properties
             m_linearMotorDirection = vd.m_linearMotorDirection;
 
@@ -309,7 +319,10 @@ namespace OpenSim.Region.Physics.OdePlugin
                     len = m_angularMotorDirection.Length();
                     if (len > 12.566f)
                         m_angularMotorDirection *= (12.566f / len);
-                    m_amEfect = 1.0f; // turn it on
+
+                    m_amEfect = 1.0f / m_angularMotorTimescale; // turn it on
+                    m_amDecay = 1.0f - 1.0f / m_angularMotorDecayTimescale;
+
                     if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body)
                             && !rootPrim.m_isSelected && !rootPrim.m_disabled)
                         d.BodyEnable(rootPrim.Body);
@@ -323,8 +336,10 @@ namespace OpenSim.Region.Physics.OdePlugin
                     len = m_linearMotorDirection.Length();
                     if (len > 100.0f)
                         m_linearMotorDirection *= (100.0f / len);
+
                     m_lmDecay = 1.0f - 1.0f / m_linearMotorDecayTimescale;
                     m_lmEfect = 1.0f / m_linearMotorTimescale; // turn it on
+
                     m_ffactor = 0.01f;
                     if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body)
                             && !rootPrim.m_isSelected && !rootPrim.m_disabled)
@@ -358,7 +373,10 @@ namespace OpenSim.Region.Physics.OdePlugin
                     len = m_angularMotorDirection.Length();
                     if (len > 12.566f)
                         m_angularMotorDirection *= (12.566f / len);
-                    m_amEfect = 1.0f; // turn it on
+
+                    m_amEfect = 1.0f / m_angularMotorTimescale; // turn it on
+                    m_amDecay = 1.0f - 1.0f / m_angularMotorDecayTimescale;
+
                     if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body)
                             && !rootPrim.m_isSelected && !rootPrim.m_disabled)
                         d.BodyEnable(rootPrim.Body);
@@ -374,8 +392,12 @@ namespace OpenSim.Region.Physics.OdePlugin
                     len = m_linearMotorDirection.Length();
                     if (len > 100.0f)
                         m_linearMotorDirection *= (100.0f / len);
-                    m_lmDecay = 1.0f - 1.0f / m_linearMotorDecayTimescale;
+
                     m_lmEfect = 1.0f / m_linearMotorTimescale; // turn it on
+                    m_lmDecay = 1.0f - 1.0f / m_linearMotorDecayTimescale;
+
+
+
                     m_ffactor = 0.01f;
                     if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body)
                             && !rootPrim.m_isSelected && !rootPrim.m_disabled)
@@ -421,6 +443,7 @@ namespace OpenSim.Region.Physics.OdePlugin
         internal void ProcessTypeChange(Vehicle pType)
         {
             m_lmEfect = 0;
+
             m_amEfect = 0;
             m_ffactor = 1f;
 
@@ -607,15 +630,20 @@ namespace OpenSim.Region.Physics.OdePlugin
 //                    m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY |
 //                        VehicleFlag.HOVER_GLOBAL_HEIGHT);
                     break;
+
             }
+
             m_lmDecay = (1.0f - 1.0f / m_linearMotorDecayTimescale);
+            m_amDecay = 1.0f - 1.0f / m_angularMotorDecayTimescale;
+
         }//end SetDefaultsForType
 
         internal void Stop()
         {
             m_lmEfect = 0;
-            m_lmDecay = 1.0f;
+            m_lmDecay = 0f;
             m_amEfect = 0;
+            m_amDecay = 0;
             m_ffactor = 1f;
         }
 
@@ -642,6 +670,7 @@ namespace OpenSim.Region.Physics.OdePlugin
 
         private const float pi = (float)Math.PI;
         private const float halfpi = 0.5f * (float)Math.PI;
+        private const float twopi = 2.0f * pi;
 
         public static Vector3 ubitRot2Euler(Quaternion rot)
         {
@@ -744,6 +773,8 @@ namespace OpenSim.Region.Physics.OdePlugin
             curAngVel.Z = dvtmp.Z;
             Vector3 curLocalAngVel = curAngVel * irotq; // current angular velocity in  local
 
+            float ldampZ = 0;
+            
             // linear motor
             if (m_lmEfect > 0.001 && m_linearMotorTimescale < 1000)
             {
@@ -766,9 +797,11 @@ namespace OpenSim.Region.Physics.OdePlugin
                     force.Y += tmpV.Y;
                     force.Z += tmpV.Z;
                 }
+
                 m_lmEfect *= m_lmDecay;
 
-                m_ffactor = 0.01f + 1e-4f * curVel.LengthSquared();
+                //                m_ffactor = 0.01f + 1e-4f * curVel.LengthSquared();
+                m_ffactor = 0;
             }
             else
             {
@@ -776,18 +809,6 @@ namespace OpenSim.Region.Physics.OdePlugin
                 m_ffactor = 1f;
             }
 
-            // friction
-            if (curLocalVel.X != 0 || curLocalVel.Y != 0 || curLocalVel.Z != 0)
-            {
-                tmpV.X = -curLocalVel.X / m_linearFrictionTimescale.X;
-                tmpV.Y = -curLocalVel.Y / m_linearFrictionTimescale.Y;
-                tmpV.Z = -curLocalVel.Z / m_linearFrictionTimescale.Z;
-                tmpV *= rotq; // to world
-                force.X += tmpV.X;
-                force.Y += tmpV.Y;
-                force.Z += tmpV.Z;
-            }
-
             // hover
             if (m_VhoverTimescale < 300 && rootPrim.prim_geom != IntPtr.Zero)
             {
@@ -823,10 +844,16 @@ namespace OpenSim.Region.Physics.OdePlugin
                 else if (t > m_VhoverHeight)
                         perr = t - pos.Z; ;
 
-                if ((m_flags & VehicleFlag.HOVER_UP_ONLY) == 0 || perr > 0)
+                if ((m_flags & VehicleFlag.HOVER_UP_ONLY) == 0 || perr > -0.1)
                 {
-                    //                    force.Z += (perr / m_VhoverTimescale / m_VhoverTimescale - curVel.Z * m_VhoverEfficiency) / m_timestep;
-                    force.Z += (perr / m_VhoverTimescale - curVel.Z * m_VhoverEfficiency);// * m_invtimestep);
+                    ldampZ = m_VhoverEfficiency * m_invtimestep;
+
+                    perr *= (1.0f + ldampZ) / m_VhoverTimescale;
+
+                    //                    force.Z += perr - curVel.Z * tmp;
+                    force.Z += perr;
+                    ldampZ *= -curVel.Z;
+
                     force.Z += _pParentScene.gravityz * (1f - m_VehicleBuoyancy);
                 }
                 else // no buoyancy
@@ -844,7 +871,6 @@ namespace OpenSim.Region.Physics.OdePlugin
                 float len = curVel.Length();
                 if (len > 0.01) // if moving
                 {
-
                     Vector3 atAxis;
                     atAxis = Xrot(rotq); // where are we pointing to
                     atAxis *= len; // make it same size as world velocity vector
@@ -870,56 +896,19 @@ namespace OpenSim.Region.Physics.OdePlugin
                 }
             }
 
-            // angular motor
-            if (m_amEfect > 0.01 && m_angularMotorTimescale < 1000)
-            {
-                tmpV = m_angularMotorDirection - curLocalAngVel; // velocity error
-                tmpV *= m_amEfect / m_angularMotorTimescale; // error to correct in this timestep
-                torque.X += tmpV.X;
-                torque.Y += tmpV.Y;
-                torque.Z += tmpV.Z;
-                m_amEfect *= (1 - 1.0f / m_angularMotorDecayTimescale);
-            }
-            else
-                m_amEfect = 0;
-
-            // angular friction
-            if (curLocalAngVel.X != 0 || curLocalAngVel.Y != 0 || curLocalAngVel.Z != 0)
-            {
-                torque.X -= curLocalAngVel.X / m_angularFrictionTimescale.X;
-                torque.Y -= curLocalAngVel.Y / m_angularFrictionTimescale.Y;
-                torque.Z -= curLocalAngVel.Z / m_angularFrictionTimescale.Z;
-            }
-
-            // angular deflection
-            if (m_angularDeflectionEfficiency > 0)
+            // linear friction/damping
+            if (curLocalVel.X != 0 || curLocalVel.Y != 0 || curLocalVel.Z != 0)
             {
-                Vector3 dirv;
-                
-                if (curLocalVel.X > 0.01f)
-                    dirv = curLocalVel;
-                else if (curLocalVel.X < -0.01f)
-                    // use oposite 
-                    dirv = -curLocalVel;
-                else
-                {
-                    // make it fall into small positive x case
-                    dirv.X = 0.01f;
-                    dirv.Y = curLocalVel.Y;
-                    dirv.Z = curLocalVel.Z;
-                }
-
-                float ftmp = m_angularDeflectionEfficiency / m_angularDeflectionTimescale;
-
-                if (Math.Abs(dirv.Z) > 0.01)
-                {
-                    torque.Y += - (float)Math.Atan2(dirv.Z, dirv.X) * ftmp;
-                }
+                tmpV.X = -curLocalVel.X / m_linearFrictionTimescale.X;
+                tmpV.Y = -curLocalVel.Y / m_linearFrictionTimescale.Y;
+                tmpV.Z = -curLocalVel.Z / m_linearFrictionTimescale.Z;
+                tmpV *= rotq; // to world
 
-                if (Math.Abs(dirv.Y) > 0.01)
-                {
-                    torque.Z += (float)Math.Atan2(dirv.Y, dirv.X) * ftmp;
-                }
+                if(ldampZ != 0 && Math.Abs(ldampZ) > Math.Abs(tmpV.Z))
+                    tmpV.Z = ldampZ;
+                force.X += tmpV.X;
+                force.Y += tmpV.Y;
+                force.Z += tmpV.Z;
             }
 
             // vertical atractor
@@ -928,29 +917,30 @@ namespace OpenSim.Region.Physics.OdePlugin
                 float roll;
                 float pitch;
 
-                GetRollPitch(irotq, out roll, out pitch);
 
-                float ftmp = 1.0f / m_verticalAttractionTimescale / m_verticalAttractionTimescale * m_invtimestep;
+
+                float ftmp = m_invtimestep / m_verticalAttractionTimescale / m_verticalAttractionTimescale;
+
                 float ftmp2;
-                if (m_bankingEfficiency == 0)
-                    ftmp2 = m_verticalAttractionEfficiency * m_invtimestep;
-                else
-                    ftmp2 = 0;
+                ftmp2 = 0.5f * m_verticalAttractionEfficiency * m_invtimestep;
+                m_amdampX = ftmp2;
+
+                m_ampwr = 1.0f - 0.8f * m_verticalAttractionEfficiency;
+
+                GetRollPitch(irotq, out roll, out pitch);
 
                 if (roll > halfpi)
                     roll = pi - roll;
                 else if (roll < -halfpi)
-                    roll = -pi - roll;                           
+                    roll = -pi - roll;
 
                 float effroll = pitch / halfpi;
                 effroll *= effroll;
                 effroll = 1 - effroll;
                 effroll *= roll;
 
-                if (Math.Abs(effroll) > 0.01) // roll
-                {
-                    torque.X -= -effroll * ftmp + curLocalAngVel.X * ftmp2;
-                }
+
+                torque.X += effroll * ftmp;
 
                 if ((m_flags & VehicleFlag.LIMIT_ROLL_ONLY) == 0)
                 {
@@ -958,24 +948,21 @@ namespace OpenSim.Region.Physics.OdePlugin
                     effpitch *= effpitch;
                     effpitch = 1 - effpitch;
                     effpitch *= pitch;
-                    
-                    if (Math.Abs(effpitch) > 0.01) // pitch
-                    {
-                        torque.Y -= -effpitch * ftmp + curLocalAngVel.Y * ftmp2;
-                    }
+
+                    torque.Y += effpitch * ftmp;
                 }
 
                 if (m_bankingEfficiency != 0 && Math.Abs(effroll) > 0.01)
                 {
 
                     float broll = effroll;
-/*
-                    if (broll > halfpi)
-                        broll = pi - broll;
-                    else if (broll < -halfpi)
-                        broll = -pi - broll;                           
-*/                    
-                    broll *= m_bankingEfficiency; 
+                    /*
+                                        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;
@@ -984,24 +971,91 @@ namespace OpenSim.Region.Physics.OdePlugin
                         if (curLocalVel.X >= 0)
                             broll *= (1 + (vfact - 1) * m_bankingMix);
                         else
-                            broll *= -(1 + (vfact - 1) * m_bankingMix);                      
+                            broll *= -(1 + (vfact - 1) * m_bankingMix);
                     }
                     // make z rot be in world Z not local as seems to be in sl
 
                     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;
+                    tmpV = Zrot(irotq);
+                    tmpV *= broll;
 
                     torque.X += tmpV.X;
                     torque.Y += tmpV.Y;
                     torque.Z += tmpV.Z;
+
+                    m_amdampZ = Math.Abs(m_bankingEfficiency) / m_bankingTimescale;
+                    m_amdampY = m_amdampZ;
+
+                }
+                else
+                {
+                    m_amdampZ = 1 / m_angularFrictionTimescale.Z;
+                    m_amdampY = m_amdampX;
+                }
+            }
+            else
+            {
+                m_ampwr = 1.0f;
+                m_amdampX = 1 / m_angularFrictionTimescale.X;
+                m_amdampY = 1 / m_angularFrictionTimescale.Y;
+                m_amdampZ = 1 / m_angularFrictionTimescale.Z;
+            }
+
+            // angular motor
+            if (m_amEfect > 0.01 && m_angularMotorTimescale < 1000)
+            {
+                tmpV = m_angularMotorDirection - curLocalAngVel; // velocity error
+                tmpV *= m_amEfect; // error to correct in this timestep
+                torque.X += tmpV.X * m_ampwr;
+                torque.Y += tmpV.Y * m_ampwr;
+                torque.Z += tmpV.Z;
+
+                m_amEfect *= m_amDecay;
+            }
+            else
+                m_amEfect = 0;
+
+            // angular deflection
+            if (m_angularDeflectionEfficiency > 0)
+            {
+                Vector3 dirv;
+                
+                if (curLocalVel.X > 0.01f)
+                    dirv = curLocalVel;
+                else if (curLocalVel.X < -0.01f)
+                    // use oposite 
+                    dirv = -curLocalVel;
+                else
+                {
+                    // make it fall into small positive x case
+                    dirv.X = 0.01f;
+                    dirv.Y = curLocalVel.Y;
+                    dirv.Z = curLocalVel.Z;
+                }
+
+                float ftmp = m_angularDeflectionEfficiency / m_angularDeflectionTimescale;
+
+                if (Math.Abs(dirv.Z) > 0.01)
+                {
+                    torque.Y += - (float)Math.Atan2(dirv.Z, dirv.X) * ftmp;
+                }
+
+                if (Math.Abs(dirv.Y) > 0.01)
+                {
+                    torque.Z += (float)Math.Atan2(dirv.Y, dirv.X) * ftmp;
                 }
             }
+
+            // angular friction
+            if (curLocalAngVel.X != 0 || curLocalAngVel.Y != 0 || curLocalAngVel.Z != 0)
+            {
+                torque.X -= curLocalAngVel.X * m_amdampX;
+                torque.Y -= curLocalAngVel.Y * m_amdampY;
+                torque.Z -= curLocalAngVel.Z * m_amdampZ;
+            }
+
             
             d.Mass dmass;
             d.BodyGetMass(Body,out dmass);