2 files changed, 84 insertions, 8 deletions

diff --git a/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs
index dae19c3..ed1366c 100644
--- a/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs
+++ b/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs
@@ -52,8 +52,12 @@ namespace OpenSim.Region.Physics.OdePlugin
         StopCFM = 8,
         LoStop2 = 256,
         HiStop2 = 257,
+        Vel2 = 258,
+        FMax2 = 259,
         LoStop3 = 512,
-        HiStop3 = 513
+        HiStop3 = 513,
+        Vel3 = 514,
+        FMax3 = 515
     }
     public class OdeCharacter : PhysicsActor
     {
diff --git a/OpenSim/Region/Physics/OdePlugin/ODEVehicleSettings.cs b/OpenSim/Region/Physics/OdePlugin/ODEVehicleSettings.cs
index 04de9a7..81c41ad 100644
--- a/OpenSim/Region/Physics/OdePlugin/ODEVehicleSettings.cs
+++ b/OpenSim/Region/Physics/OdePlugin/ODEVehicleSettings.cs
@@ -62,6 +62,7 @@ namespace OpenSim.Region.Physics.OdePlugin
         private Quaternion m_referenceFrame = Quaternion.Identity;
         private Vector3 m_angularFrictionTimescale = Vector3.Zero;
         private Vector3 m_angularMotorDirection = Vector3.Zero;
+        private Vector3 m_angularMotorDirectionLASTSET = Vector3.Zero;
         private Vector3 m_linearFrictionTimescale = Vector3.Zero;
         private Vector3 m_linearMotorDirection = Vector3.Zero;
         private Vector3 m_linearMotorDirectionLASTSET = Vector3.Zero;
@@ -84,6 +85,7 @@ namespace OpenSim.Region.Physics.OdePlugin
         private float m_verticalAttractionEfficiency = 0;
         private float m_verticalAttractionTimescale = 0;
         private Vector3 m_lastLinearVelocityVector = Vector3.Zero;
+        private Vector3 m_lastAngularVelocityVector = Vector3.Zero;
         private VehicleFlag m_flags = (VehicleFlag) 0;
 
         private bool m_LinearMotorSetLastFrame = false;
@@ -168,6 +170,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                     break;
                 case Vehicle.ANGULAR_MOTOR_DIRECTION:
                     m_angularMotorDirection = new Vector3(pValue, pValue, pValue);
+                    m_angularMotorDirectionLASTSET = new Vector3(pValue, pValue, pValue);
                     break;
                 case Vehicle.LINEAR_FRICTION_TIMESCALE:
                     m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue);
@@ -193,7 +196,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                     break;
                 case Vehicle.ANGULAR_MOTOR_DIRECTION:
                     m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
-                    m_LinearMotorSetLastFrame = true;
+                    m_angularMotorDirectionLASTSET = new Vector3(pValue.X, pValue.Y, pValue.Z);
                     break;
                 case Vehicle.LINEAR_FRICTION_TIMESCALE:
                     m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
@@ -269,10 +272,12 @@ namespace OpenSim.Region.Physics.OdePlugin
                     d.JointAttach(m_lMotor1, Body, IntPtr.Zero);
                 }
 
-                Vector3 dirNorm = m_lastLinearVelocityVector;
-                dirNorm.Normalize();
-                //d.JointSetLMotorAxis(m_lMotor1, 0, 1, dirNorm.X, dirNorm.Y, dirNorm.Z);
-                //d.JointSetLMotorParam(m_lMotor1, (int)dParam.Vel, m_lastLinearVelocityVector.Length());
+                if (m_aMotor == IntPtr.Zero)
+                {
+                    m_aMotor = d.JointCreateAMotor(pParentScene.world, m_jointGroup);
+                    d.JointSetAMotorNumAxes(m_aMotor, 3);
+                    d.JointAttach(m_aMotor, Body, IntPtr.Zero);
+                }
             }
         }
 
@@ -296,6 +301,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                 return;
             VerticalAttractor(pTimestep);
             LinearMotor(pTimestep);
+            AngularMotor(pTimestep);
         }
 
         private void SetDefaultsForType(Vehicle pType)
@@ -490,7 +496,7 @@ namespace OpenSim.Region.Physics.OdePlugin
 
             //System.Console.WriteLine(m_linearMotorDirection + " " + m_lastLinearVelocityVector);
 
-            SetMotorProperties();
+            SetLinearMotorProperties();
 
             Vector3 decayamount = Vector3.One / (m_linearFrictionTimescale / pTimestep);
             m_lastLinearVelocityVector -= m_lastLinearVelocityVector * decayamount;
@@ -499,7 +505,7 @@ namespace OpenSim.Region.Physics.OdePlugin
 
         }
 
-        private void SetMotorProperties()
+        private void SetLinearMotorProperties()
         {
             Vector3 dirNorm = m_lastLinearVelocityVector;
             dirNorm.Normalize();
@@ -519,5 +525,71 @@ namespace OpenSim.Region.Physics.OdePlugin
             }
 
         }
+
+        private void AngularMotor(float pTimestep)
+        {
+            if (!m_angularMotorDirection.ApproxEquals(Vector3.Zero, 0.01f))
+            {
+
+                Vector3 addAmount = m_angularMotorDirection / (m_angularMotorTimescale / pTimestep);
+                m_lastAngularVelocityVector += (addAmount * 10);
+
+                // This will work temporarily, but we really need to compare speed on an axis
+                if (Math.Abs(m_lastAngularVelocityVector.X) > Math.Abs(m_angularMotorDirectionLASTSET.X))
+                    m_lastAngularVelocityVector.X = m_angularMotorDirectionLASTSET.X;
+                if (Math.Abs(m_lastAngularVelocityVector.Y) > Math.Abs(m_angularMotorDirectionLASTSET.Y))
+                    m_lastAngularVelocityVector.Y = m_angularMotorDirectionLASTSET.Y;
+                if (Math.Abs(m_lastAngularVelocityVector.Z) > Math.Abs(m_angularMotorDirectionLASTSET.Z))
+                    m_lastAngularVelocityVector.Z = m_angularMotorDirectionLASTSET.Z;
+                //Console.WriteLine("add: " + addAmount);
+
+                Vector3 decayfraction = ((Vector3.One / (m_angularMotorDecayTimescale / pTimestep)));
+                //Console.WriteLine("decay: " + decayfraction);
+
+                m_angularMotorDirection -= m_angularMotorDirection * decayfraction;
+                //Console.WriteLine("actual: " + m_linearMotorDirection);
+            }
+
+            //System.Console.WriteLine(m_linearMotorDirection + " " + m_lastLinearVelocityVector);
+
+            SetAngularMotorProperties();
+
+            Vector3 decayamount = Vector3.One / (m_angularFrictionTimescale / pTimestep);
+            m_lastAngularVelocityVector -= m_lastAngularVelocityVector * decayamount;
+
+            //m_linearMotorDirection  *= decayamount;
+
+        }
+        private void SetAngularMotorProperties()
+        {
+            
+            
+
+            d.Mass objMass;
+            d.BodyGetMass(Body, out objMass);
+            d.Quaternion rot = d.BodyGetQuaternion(Body);
+            Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);
+            Vector3 axis0 = Vector3.UnitX;
+            Vector3 axis1 = Vector3.UnitY;
+            Vector3 axis2 = Vector3.UnitZ;
+            axis0 *= rotq;
+            axis1 *= rotq;
+            axis2 *= rotq;
+
+
+            if (m_aMotor != IntPtr.Zero)
+            {
+                d.JointSetAMotorAxis(m_aMotor, 0, 1, axis0.X, axis0.Y, axis0.Z);
+                d.JointSetAMotorAxis(m_aMotor, 1, 1, axis1.X, axis1.Y, axis1.Z);
+                d.JointSetAMotorAxis(m_aMotor, 2, 1, axis2.X, axis2.Y, axis2.Z);
+                d.JointSetAMotorParam(m_aMotor, (int)dParam.FMax, 30*objMass.mass);
+                d.JointSetAMotorParam(m_aMotor, (int)dParam.FMax2, 30*objMass.mass);
+                d.JointSetAMotorParam(m_aMotor, (int)dParam.FMax3, 30 * objMass.mass);
+                d.JointSetAMotorParam(m_aMotor, (int)dParam.Vel, m_lastAngularVelocityVector.X);
+                d.JointSetAMotorParam(m_aMotor, (int)dParam.Vel2, m_lastAngularVelocityVector.Y);
+                d.JointSetAMotorParam(m_aMotor, (int)dParam.Vel3, m_lastAngularVelocityVector.Z);
+
+            }
+        }
     }
 }