2 files changed, 67 insertions, 13 deletions

diff --git a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
index 391f644..adb559a 100644
--- a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
+++ b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
@@ -66,6 +66,7 @@ namespace OpenSim.Region.Physics.OdePlugin
         private IntPtr Amotor = IntPtr.Zero;
 
         private PhysicsVector m_PIDTarget = new PhysicsVector(0, 0, 0);
+        private PhysicsVector m_taintPIDTarget = new PhysicsVector(0, 0, 0);
         private float m_PIDTau = 0f;
         private float PID_D = 35f;
         private float PID_G = 25f;
@@ -908,6 +909,8 @@ namespace OpenSim.Region.Physics.OdePlugin
 
                 if (!m_angularlock.IsIdentical(m_taintAngularLock,0))
                     changeAngularLock(timestep);
+                
+                    
             }
             else
             {
@@ -915,6 +918,7 @@ namespace OpenSim.Region.Physics.OdePlugin
             }
         }
 
+
         private void changeAngularLock(float timestep)
         {
             // do we have a Physical object?
@@ -1470,6 +1474,10 @@ namespace OpenSim.Region.Physics.OdePlugin
                             }
                         }
                         d.BodyEnable(Body);
+                        if (m_vehicle.Type != Vehicle.TYPE_NONE)
+                        {
+                            m_vehicle.Enable(Body, _parent_scene);
+                        }
                     }
                     else
                     {
@@ -1550,6 +1558,7 @@ namespace OpenSim.Region.Physics.OdePlugin
 
                 if (m_usePID)
                 {
+                    
                     //if (!d.BodyIsEnabled(Body))
                     //d.BodySetForce(Body, 0f, 0f, 0f);
                     // If we're using the PID controller, then we have no gravity
@@ -1737,6 +1746,8 @@ namespace OpenSim.Region.Physics.OdePlugin
                         fy = nmin;
                     d.BodyAddForce(Body, fx, fy, fz);
                 }
+                if (m_vehicle.Body == IntPtr.Zero && m_vehicle.Type != Vehicle.TYPE_NONE)
+                    m_vehicle.Enable(Body, _parent_scene);
 
                 m_vehicle.Step(timestep);
             }
@@ -1747,6 +1758,8 @@ namespace OpenSim.Region.Physics.OdePlugin
             }
         }
 
+
+
         public void rotate(float timestep)
         {
             d.Quaternion myrot = new d.Quaternion();
@@ -2701,8 +2714,10 @@ namespace OpenSim.Region.Physics.OdePlugin
             set
             {
                 if (PhysicsVector.isFinite(value))
-                    m_PIDTarget = value; 
-                else 
+                {
+                    m_PIDTarget = value;
+                }
+                else
                     m_log.Warn("[PHYSICS]: Got NaN PIDTarget from Scene on Object");
             } 
         }
@@ -3062,15 +3077,7 @@ namespace OpenSim.Region.Physics.OdePlugin
             return det;
 
         }
-        private static float Determinant(Matrix4 matrix)
-        {
-            float det = 0;
-
-            for (int j = 0; j < 4; j++)
-                det += (matrix[0, j] * Determinant(Minor(matrix, 0, j)) * (int)System.Math.Pow(-1, 0 + j));
-            return det;
-        }
-
+        
         private static void DMassCopy(ref d.Mass src, ref d.Mass dst)
         {
             dst.c.W = src.c.W;
diff --git a/OpenSim/Region/Physics/OdePlugin/ODEVehicleSettings.cs b/OpenSim/Region/Physics/OdePlugin/ODEVehicleSettings.cs
index c4c3044..3a99b48 100644
--- a/OpenSim/Region/Physics/OdePlugin/ODEVehicleSettings.cs
+++ b/OpenSim/Region/Physics/OdePlugin/ODEVehicleSettings.cs
@@ -44,6 +44,11 @@ namespace OpenSim.Region.Physics.OdePlugin
             get { return m_type; }
         }
 
+        public IntPtr Body
+        {
+            get { return m_body; }
+        }
+
         private Vehicle m_type = Vehicle.TYPE_NONE;
         private OdeScene m_parentScene = null;
         private IntPtr m_body = IntPtr.Zero;
@@ -75,6 +80,7 @@ namespace OpenSim.Region.Physics.OdePlugin
         private float m_linearMotorTimescale = 0;
         private float m_verticalAttractionEfficiency = 0;
         private float m_verticalAttractionTimescale = 0;
+        private Vector3 m_lastVector = Vector3.Zero;
         private VehicleFlag m_flags = (VehicleFlag) 0;
 
 
@@ -222,8 +228,9 @@ namespace OpenSim.Region.Physics.OdePlugin
 
         internal void Enable(IntPtr pBody, OdeScene pParentScene)
         {
-            if (pBody == IntPtr.Zero || m_type == Vehicle.TYPE_NONE)
+            if (m_type == Vehicle.TYPE_NONE)
                 return;
+
             m_body = pBody;
             m_parentScene = pParentScene;
         }
@@ -246,11 +253,13 @@ namespace OpenSim.Region.Physics.OdePlugin
         {
             if (m_body == IntPtr.Zero || m_type == Vehicle.TYPE_NONE)
                 return;
-            
+            VerticalAttractor(pTimestep);
+            LinearMotor(pTimestep);
         }
 
         private void SetDefaultsForType(Vehicle pType)
         {
+            m_type = pType;
             switch (pType)
             {
                 case Vehicle.TYPE_SLED:
@@ -388,5 +397,43 @@ namespace OpenSim.Region.Physics.OdePlugin
 
             }
         }
+        
+        private void VerticalAttractor(float pTimestep)
+        {
+            // The purpose of this routine here is to quickly stabilize the Body while it's popped up in the air.
+            // The amotor needs a few seconds to stabilize so without it, the avatar shoots up sky high when you
+            // change appearance and when you enter the simulator
+            // After this routine is done, the amotor stabilizes much quicker
+            d.Mass objMass;
+            d.BodyGetMass(Body, out objMass);
+            //d.BodyGetS
+
+            d.Vector3 feet;
+            d.Vector3 head;
+            d.BodyGetRelPointPos(m_body, 0.0f, 0.0f, -1.0f, out feet);
+            d.BodyGetRelPointPos(m_body, 0.0f, 0.0f, 1.0f, out head);
+            float posture = head.Z - feet.Z;
+            
+            // restoring force proportional to lack of posture:
+            float servo = (2.5f - posture) * (objMass.mass * m_verticalAttractionEfficiency / (m_verticalAttractionTimescale * pTimestep)) * objMass.mass;
+            d.BodyAddForceAtRelPos(m_body, 0.0f, 0.0f, servo, 0.0f, 0.0f, 1.0f);
+            d.BodyAddForceAtRelPos(m_body, 0.0f, 0.0f, -servo, 0.0f, 0.0f, -1.0f);
+            //d.Matrix3 bodyrotation = d.BodyGetRotation(Body);
+            //m_log.Info("[PHYSICSAV]: Rotation: " + bodyrotation.M00 + " : " + bodyrotation.M01 + " : " + bodyrotation.M02 + " : " + bodyrotation.M10 + " : " + bodyrotation.M11 + " : " + bodyrotation.M12 + " : " + bodyrotation.M20 + " : " + bodyrotation.M21 + " : " + bodyrotation.M22);
+        }
+
+        private void LinearMotor(float pTimestep)
+        {
+
+            /*
+            float decayval = (m_linearMotorDecayTimescale * pTimestep);
+            m_linearMotorDirection  *= decayval;
+            m_lastVector += m_linearMotorDirection
+
+            
+            m_lin
+            m_lastVector
+             * */
+        }
     }
 }