8 files changed, 184 insertions, 5 deletions

diff --git a/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsPlugin.cs b/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsPlugin.cs
index 16ec66e..08fdd80 100644
--- a/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsPlugin.cs
+++ b/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsPlugin.cs
@@ -374,6 +374,11 @@ namespace OpenSim.Region.Physics.BasicPhysicsPlugin
         {
         }
 
+        public override void LockAngularMotion(PhysicsVector axis)
+        {
+
+        }
+
         public void SetAcceleration(PhysicsVector accel)
         {
             _acceleration = accel;
diff --git a/OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs b/OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs
index 1ad1201..40de537 100644
--- a/OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs
+++ b/OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs
@@ -809,6 +809,12 @@ namespace OpenSim.Region.Physics.BulletXPlugin
         {
 
         }
+
+        public override void LockAngularMotion(PhysicsVector axis)
+        {
+
+        }
+
         public override float Mass
         {
             get { return ActorMass; }
diff --git a/OpenSim/Region/Physics/Manager/PhysicsActor.cs b/OpenSim/Region/Physics/Manager/PhysicsActor.cs
index b40635c..f48f129 100644
--- a/OpenSim/Region/Physics/Manager/PhysicsActor.cs
+++ b/OpenSim/Region/Physics/Manager/PhysicsActor.cs
@@ -123,6 +123,8 @@ namespace OpenSim.Region.Physics.Manager
 
         public abstract void delink();
 
+        public abstract void LockAngularMotion(PhysicsVector axis);
+
         public virtual void RequestPhysicsterseUpdate()
         {
             // Make a temporary copy of the event to avoid possibility of
@@ -347,6 +349,9 @@ namespace OpenSim.Region.Physics.Manager
         {
         }
 
+        public override void LockAngularMotion(PhysicsVector axis)
+        {
+        }
 
         public override void AddForce(PhysicsVector force)
         {
diff --git a/OpenSim/Region/Physics/Manager/PhysicsVector.cs b/OpenSim/Region/Physics/Manager/PhysicsVector.cs
index a916e5e..4ec943c 100644
--- a/OpenSim/Region/Physics/Manager/PhysicsVector.cs
+++ b/OpenSim/Region/Physics/Manager/PhysicsVector.cs
@@ -149,7 +149,7 @@ namespace OpenSim.Region.Physics.Manager
         {
             PhysicsVector diff = this - v;
             float d = diff.length();
-            if (d < tolerance)
+            if (d <= tolerance)
                 return true;
 
             return false;
diff --git a/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs
index 042042c..9f6b14e 100644
--- a/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs
+++ b/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs
@@ -505,7 +505,14 @@ namespace OpenSim.Region.Physics.OdePlugin
 
         }
 
-// TODO: unused:
+        public override void LockAngularMotion(PhysicsVector axis)
+        {
+
+        }
+
+//      This code is very useful. Written by DanX0r. We're just not using it right now.  
+//      Commented out to prevent a warning.
+//
 //         private void standupStraight()
 //         {
 //             // The purpose of this routine here is to quickly stabilize the Body while it's popped up in the air.
diff --git a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
index 8e0640b..7a9734c 100644
--- a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
+++ b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
@@ -38,6 +38,10 @@ using OpenSim.Region.Physics.Manager;
 
 namespace OpenSim.Region.Physics.OdePlugin
 {
+    /// <summary>
+    /// Various properties that ODE uses for AMotors but isn't exposed in ODE.NET so we must define them ourselves.
+    /// </summary>
+
     public class OdePrim : PhysicsActor
     {
         private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
@@ -55,6 +59,9 @@ namespace OpenSim.Region.Physics.OdePlugin
         private PhysicsVector m_taintsize;
         private PhysicsVector m_taintVelocity = PhysicsVector.Zero;
         private Quaternion m_taintrot;
+        private PhysicsVector m_angularlock = new PhysicsVector(1f, 1f, 1f);
+        private PhysicsVector m_taintAngularLock = new PhysicsVector(1f, 1f, 1f);
+        private IntPtr Amotor = IntPtr.Zero;
 
         private PhysicsVector m_PIDTarget = new PhysicsVector(0, 0, 0);
         private float m_PIDTau = 0f;
@@ -309,6 +316,12 @@ namespace OpenSim.Region.Physics.OdePlugin
             m_collisionscore = 0;
             m_disabled = false;
 
+            // The body doesn't already have a finite rotation mode set here
+            if ((!m_angularlock.IsIdentical(PhysicsVector.Zero, 0)) && _parent == null)
+            {
+                createAMotor(m_angularlock);
+            }
+
             _parent_scene.addActivePrim(this);
         }
 
@@ -722,7 +735,7 @@ namespace OpenSim.Region.Physics.OdePlugin
             }
             if (prim_geom != (IntPtr)0)
             {
-                if (m_taintposition != _position)
+                if (!_position.IsIdentical(m_taintposition,0f))
                     changemove(timestep);
 
                 if (m_taintrot != _orientation)
@@ -733,7 +746,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                     changePhysicsStatus(timestep);
                 //
 
-                if (m_taintsize != _size)
+                if (!_size.IsIdentical(m_taintsize,0))
                     changesize(timestep);
                 //
 
@@ -750,7 +763,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                 if (m_taintselected != m_isSelected)
                     changeSelectedStatus(timestep);
 
-                if (m_taintVelocity != PhysicsVector.Zero)
+                if (!m_taintVelocity.IsIdentical(PhysicsVector.Zero,0))
                     changevelocity(timestep);
 
                 if (m_taintparent != _parent)
@@ -759,6 +772,8 @@ namespace OpenSim.Region.Physics.OdePlugin
                 if (m_taintCollidesWater != m_collidesWater)
                     changefloatonwater(timestep);
 
+                if (!m_angularlock.IsIdentical(m_taintAngularLock,0))
+                    changeAngularLock(timestep);
 
             }
             else
@@ -767,6 +782,35 @@ namespace OpenSim.Region.Physics.OdePlugin
             }
         }
 
+        private void changeAngularLock(float timestep)
+        {   
+            // do we have a Physical object?
+            if (Body != IntPtr.Zero)
+            {
+                //Check that we have a Parent
+                //If we have a parent then we're not authorative here
+                if (_parent == null)
+                {
+                    if (!m_taintAngularLock.IsIdentical(new PhysicsVector(1f,1f,1f), 0))
+                    {
+                        //d.BodySetFiniteRotationMode(Body, 0);
+                        //d.BodySetFiniteRotationAxis(Body,m_taintAngularLock.X,m_taintAngularLock.Y,m_taintAngularLock.Z);
+                        createAMotor(m_taintAngularLock);
+                    }
+                    else
+                    {
+                        if (Amotor != IntPtr.Zero)
+                        {
+                            d.JointDestroy(Amotor);
+                            Amotor = (IntPtr)0;
+                        }
+                    }
+                }
+            }
+            // Store this for later in case we get turned into a separate body
+            m_angularlock = new PhysicsVector(m_taintAngularLock.X,m_angularlock.Y,m_angularlock.Z);
+        }
+
         private void changelink(float timestep)
         {
             
@@ -1241,6 +1285,8 @@ namespace OpenSim.Region.Physics.OdePlugin
             if (m_isphysical && Body != (IntPtr) 0)
             {
                 d.BodySetQuaternion(Body, ref myrot);
+                if (!m_angularlock.IsIdentical(new PhysicsVector(1, 1, 1), 0))
+                    createAMotor(m_angularlock);
             }
             
             resetCollisionAccounting();
@@ -1880,6 +1926,17 @@ namespace OpenSim.Region.Physics.OdePlugin
             m_taintparent = null;
         }
 
+
+        public override void LockAngularMotion(PhysicsVector axis)
+        {
+            // reverse the zero/non zero values for ODE.
+
+            axis.X = (axis.X > 0) ? 1f : 0f;
+            axis.Y = (axis.Y > 0) ? 1f : 0f;
+            axis.Z = (axis.Z > 0) ? 1f : 0f;
+            m_taintAngularLock = new PhysicsVector(axis.X, axis.Y, axis.Z); ;
+        }
+
         public void UpdatePositionAndVelocity()
         { 
             //  no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit!
@@ -2078,5 +2135,83 @@ namespace OpenSim.Region.Physics.OdePlugin
         public override PhysicsVector PIDTarget { set { m_PIDTarget = value; ; } }
         public override bool PIDActive { set { m_usePID = value; } }
         public override float PIDTau { set { m_PIDTau = (value * 0.6f); } }
+
+        private void createAMotor(PhysicsVector axis)
+        {
+            if (Body == IntPtr.Zero)
+                return;
+
+            if (Amotor != IntPtr.Zero)
+            {
+                d.JointDestroy(Amotor);
+                Amotor = IntPtr.Zero;
+            }
+
+            float m_tensor = 0f;
+            if (Environment.OSVersion.Platform == PlatformID.Unix)
+            {
+                m_tensor = 2f;
+            }
+            else
+            {
+                m_tensor = 5f;
+            }
+            
+            float axisnum = 3;
+
+            axisnum = (axisnum - (axis.X + axis.Y + axis.Z));
+
+            if (axisnum <= 0)
+                return;
+            int dAMotorEuler = 1;
+
+            Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero);
+            d.JointAttach(Amotor, Body, IntPtr.Zero);
+            d.JointSetAMotorMode(Amotor, dAMotorEuler);
+
+            
+
+            d.JointSetAMotorNumAxes(Amotor,(int)axisnum);
+            int i = 0;
+
+            if (axis.X == 0)
+            {
+                d.JointSetAMotorAxis(Amotor, i, 0, 1, 0, 0);
+                i++;
+            }
+
+            if (axis.Y == 0)
+            {
+                d.JointSetAMotorAxis(Amotor, i, 0, 0, 1, 0);
+                i++;
+            }
+
+
+            if (axis.Z == 0)
+            {
+                d.JointSetAMotorAxis(Amotor, i, 0, 0, 0, 1);
+                i++;
+            }
+            for (int j = 0; j < (int)axisnum; j++)
+            {
+                //d.JointSetAMotorAngle(Amotor, j, 0);
+            }
+            //
+            //d.JointSetAMotorAngle(Amotor, 1, 0);
+            //d.JointSetAMotorAngle(Amotor, 2, 0);
+
+            // These lowstops and high stops are effectively (no wiggle room)
+            d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, -0.000000000001f);
+            d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -0.000000000001f);
+            d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, -0.000000000001f);
+            d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0.000000000001f);
+            d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0.000000000001f);
+            d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0.000000000001f);
+
+           
+            d.JointSetAMotorParam(Amotor, (int)dParam.FudgeFactor, 0f);
+            d.JointSetAMotorParam(Amotor, (int)dParam.FMax, m_tensor);
+
+        }
     }
 }
diff --git a/OpenSim/Region/Physics/POSPlugin/POSPlugin.cs b/OpenSim/Region/Physics/POSPlugin/POSPlugin.cs
index 12b2d8f..b7f38d4 100644
--- a/OpenSim/Region/Physics/POSPlugin/POSPlugin.cs
+++ b/OpenSim/Region/Physics/POSPlugin/POSPlugin.cs
@@ -502,6 +502,12 @@ namespace OpenSim.Region.Physics.POSPlugin
         {
 
         }
+
+        public override void LockAngularMotion(PhysicsVector axis)
+        {
+
+        }
+
         public void SetAcceleration(PhysicsVector accel)
         {
             _acceleration = accel;
@@ -706,6 +712,11 @@ namespace OpenSim.Region.Physics.POSPlugin
         {
         }
 
+        public override void LockAngularMotion(PhysicsVector axis)
+        {
+
+        }
+
         public override bool Selected
         {
             set { return; }
diff --git a/OpenSim/Region/Physics/PhysXPlugin/PhysXPlugin.cs b/OpenSim/Region/Physics/PhysXPlugin/PhysXPlugin.cs
index 2d00bdb..2d40134 100644
--- a/OpenSim/Region/Physics/PhysXPlugin/PhysXPlugin.cs
+++ b/OpenSim/Region/Physics/PhysXPlugin/PhysXPlugin.cs
@@ -392,6 +392,11 @@ namespace OpenSim.Region.Physics.PhysXPlugin
 
         }
 
+        public override void LockAngularMotion(PhysicsVector axis)
+        {
+
+        }
+
         public override void SetMomentum(PhysicsVector momentum)
         {
         }
@@ -630,6 +635,11 @@ namespace OpenSim.Region.Physics.PhysXPlugin
         {
         }
 
+        public override void LockAngularMotion(PhysicsVector axis)
+        {
+
+        }
+
         public override float Mass
         {
             get { return 0f; }