8 files changed, 160 insertions, 3 deletions

diff --git a/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsPlugin.cs b/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsPlugin.cs
index 9954798..013c9cf 100644
--- a/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsPlugin.cs
+++ b/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsPlugin.cs
@@ -381,6 +381,12 @@ namespace OpenSim.Region.Physics.BasicPhysicsPlugin
             set { _velocity = value; }
         }
 
+        public override PhysicsVector Torque
+        {
+            get { return PhysicsVector.Zero; }
+            set { return; }
+        }
+
         public override float CollisionScore
         {
             get { return 0f; }
@@ -426,6 +432,10 @@ namespace OpenSim.Region.Physics.BasicPhysicsPlugin
         {
         }
 
+        public override void AddAngularForce(PhysicsVector force, bool pushforce)
+        {
+        }
+
         public override void SetMomentum(PhysicsVector momentum)
         {
         }
diff --git a/OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs b/OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs
index 20c556f..4f1afdd 100644
--- a/OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs
+++ b/OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs
@@ -1134,6 +1134,14 @@ namespace OpenSim.Region.Physics.BulletXPlugin
         public override void AddForce(PhysicsVector force, bool pushforce)
         {
         }
+        public override PhysicsVector Torque
+        {
+            get { return PhysicsVector.Zero; }
+            set { return; }
+        }
+        public override void AddAngularForce(PhysicsVector force, bool pushforce)
+        {
+        }
 
         public override void SetMomentum(PhysicsVector momentum)
         {
diff --git a/OpenSim/Region/Physics/Manager/PhysicsActor.cs b/OpenSim/Region/Physics/Manager/PhysicsActor.cs
index 3c094ad..fd02057 100644
--- a/OpenSim/Region/Physics/Manager/PhysicsActor.cs
+++ b/OpenSim/Region/Physics/Manager/PhysicsActor.cs
@@ -185,6 +185,7 @@ namespace OpenSim.Region.Physics.Manager
         public abstract PhysicsVector GeometricCenter { get; }
         public abstract PhysicsVector CenterOfMass { get; }
         public abstract PhysicsVector Velocity { get; set; }
+        public abstract PhysicsVector Torque { get; set; }
         public abstract float CollisionScore { get; set;}
         public abstract PhysicsVector Acceleration { get; }
         public abstract Quaternion Orientation { get; set; }
@@ -204,6 +205,7 @@ namespace OpenSim.Region.Physics.Manager
         public abstract bool PIDActive { set;}
         public abstract float PIDTau { set; }
         public abstract void AddForce(PhysicsVector force, bool pushforce);
+        public abstract void AddAngularForce(PhysicsVector force, bool pushforce);
         public abstract void SetMomentum(PhysicsVector momentum);
         public abstract void SubscribeEvents(int ms);
         public abstract void UnSubscribeEvents();
@@ -331,6 +333,12 @@ namespace OpenSim.Region.Physics.Manager
             set { return; }
         }
 
+        public override PhysicsVector Torque
+        {
+            get { return PhysicsVector.Zero; }
+            set { return; }
+        }
+
         public override float CollisionScore
         {
             get { return 0f; }
@@ -404,6 +412,11 @@ namespace OpenSim.Region.Physics.Manager
         {
         }
 
+        public override void AddAngularForce(PhysicsVector force, bool pushforce)
+        {
+            
+        }
+
         public override PhysicsVector RotationalVelocity
         {
             get { return PhysicsVector.Zero; }
diff --git a/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs
index de09691..f2906cf 100644
--- a/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs
+++ b/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs
@@ -605,6 +605,12 @@ namespace OpenSim.Region.Physics.OdePlugin
             }
         }
 
+        public override PhysicsVector Torque
+        {
+            get { return PhysicsVector.Zero; }
+            set { return; }
+        }
+
         public override float CollisionScore
         {
             get { return 0f; }
@@ -665,6 +671,11 @@ namespace OpenSim.Region.Physics.OdePlugin
             //m_lastUpdateSent = false;
         }
 
+        public override void AddAngularForce(PhysicsVector force, bool pushforce)
+        {
+
+        }
+
         /// <summary>
         /// After all of the forces add up with 'add force' we apply them with doForce
         /// </summary>
diff --git a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
index eeef893..6f5abfa 100644
--- a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
+++ b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
@@ -47,6 +47,7 @@ namespace OpenSim.Region.Physics.OdePlugin
 
         public PhysicsVector _position;
         private PhysicsVector _velocity;
+        private PhysicsVector _torque = new PhysicsVector(0,0,0);
         private PhysicsVector m_lastVelocity = new PhysicsVector(0.0f, 0.0f, 0.0f);
         private PhysicsVector m_lastposition = new PhysicsVector(0.0f, 0.0f, 0.0f);
         private PhysicsVector m_rotationalVelocity;
@@ -56,7 +57,8 @@ namespace OpenSim.Region.Physics.OdePlugin
         private Quaternion _orientation;
         private PhysicsVector m_taintposition;
         private PhysicsVector m_taintsize;
-        private PhysicsVector m_taintVelocity = PhysicsVector.Zero;
+        private PhysicsVector m_taintVelocity = new PhysicsVector(0, 0, 0);
+        private PhysicsVector m_taintTorque = new PhysicsVector(0, 0, 0);
         private Quaternion m_taintrot;
         private PhysicsVector m_angularlock = new PhysicsVector(1f, 1f, 1f);
         private PhysicsVector m_taintAngularLock = new PhysicsVector(1f, 1f, 1f);
@@ -102,8 +104,10 @@ namespace OpenSim.Region.Physics.OdePlugin
         private CollisionLocker ode;
 
         private bool m_taintforce = false;
+        private bool m_taintaddangularforce = false;
         private PhysicsVector m_force = new PhysicsVector(0.0f, 0.0f, 0.0f);
         private List<PhysicsVector> m_forcelist = new List<PhysicsVector>();
+        private List<PhysicsVector> m_angularforcelist = new List<PhysicsVector>();
 
         private IMesh _mesh;
         private PrimitiveBaseShape _pbs;
@@ -838,6 +842,12 @@ namespace OpenSim.Region.Physics.OdePlugin
                 if (m_taintforce)
                     changeAddForce(timestep);
 
+                if (m_taintaddangularforce)
+                    changeAddAngularForce(timestep);
+
+                if (!m_taintTorque.IsIdentical(PhysicsVector.Zero, 0.001f))
+                    changeSetTorque(timestep);
+
                 if (m_taintdisable)
                     changedisable(timestep);
 
@@ -2058,6 +2068,49 @@ namespace OpenSim.Region.Physics.OdePlugin
 
         }
 
+
+
+        public void changeSetTorque(float timestamp)
+        {
+            if (!m_isSelected)
+            {
+                if (IsPhysical && Body != IntPtr.Zero)
+                {
+                    d.BodySetTorque(Body, m_taintTorque.X, m_taintTorque.Y, m_taintTorque.Z);
+                }
+            }
+            
+            m_taintTorque = new PhysicsVector(0, 0, 0);
+        }
+
+        public void changeAddAngularForce(float timestamp)
+        {
+            if (!m_isSelected)
+            {
+                lock (m_angularforcelist)
+                {
+                    //m_log.Info("[PHYSICS]: dequeing forcelist");
+                    if (IsPhysical)
+                    {
+                        PhysicsVector iforce = new PhysicsVector();
+                        for (int i = 0; i < m_angularforcelist.Count; i++)
+                        {
+                            iforce = iforce + (m_angularforcelist[i] * 100);
+                        }
+                        d.BodyEnable(Body);
+                        d.BodyAddTorque(Body, iforce.X, iforce.Y, iforce.Z);
+                        
+                    }
+                    m_angularforcelist.Clear();
+                }
+
+                m_collisionscore = 0;
+                m_interpenetrationcount = 0;
+            }
+
+            m_taintaddangularforce = false;
+        }
+
         private void changevelocity(float timestep)
         {
             if (!m_isSelected)
@@ -2070,7 +2123,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                         d.BodySetLinearVel(Body, m_taintVelocity.X, m_taintVelocity.Y, m_taintVelocity.Z);
                     }
                 }
-
+                
                 //resetCollisionAccounting();
             }
             m_taintVelocity = PhysicsVector.Zero;
@@ -2216,6 +2269,23 @@ namespace OpenSim.Region.Physics.OdePlugin
             }
         }
 
+        public override PhysicsVector Torque
+        {
+            get
+            {
+                if (!m_isphysical || Body == IntPtr.Zero)
+                    return new PhysicsVector(0,0,0);
+
+                return _torque;
+            }
+
+            set
+            {
+                m_taintTorque = value;
+                _parent_scene.AddPhysicsActorTaint(this);
+            }
+        }
+
         public override float CollisionScore
         {
             get { return m_collisionscore; }
@@ -2252,6 +2322,12 @@ namespace OpenSim.Region.Physics.OdePlugin
             //m_log.Info("[PHYSICS]: Added Force:" + force.ToString() +  " to prim at " + Position.ToString());
         }
 
+        public override void AddAngularForce(PhysicsVector force, bool pushforce)
+        {
+            m_angularforcelist.Add(force);
+            m_taintaddangularforce = true;
+        }
+
         public override PhysicsVector RotationalVelocity
         {
             get
@@ -2323,7 +2399,8 @@ namespace OpenSim.Region.Physics.OdePlugin
                     d.Quaternion ori = d.BodyGetQuaternion(Body);
                     d.Vector3 vel = d.BodyGetLinearVel(Body);
                     d.Vector3 rotvel = d.BodyGetAngularVel(Body);
-
+                    d.Vector3 torque = d.BodyGetTorque(Body);
+                    _torque.setValues(torque.X, torque.Y, torque.Z);
                     PhysicsVector l_position = new PhysicsVector();
 
                     //  kluge to keep things in bounds.  ODE lets dead avatars drift away (they should be removed!)
diff --git a/OpenSim/Region/Physics/POSPlugin/POSCharacter.cs b/OpenSim/Region/Physics/POSPlugin/POSCharacter.cs
index ab66d4c..4230a57 100644
--- a/OpenSim/Region/Physics/POSPlugin/POSCharacter.cs
+++ b/OpenSim/Region/Physics/POSPlugin/POSCharacter.cs
@@ -211,6 +211,12 @@ namespace OpenSim.Region.Physics.POSPlugin
             set { _target_velocity = value; }
         }
 
+        public override PhysicsVector Torque
+        {
+            get { return PhysicsVector.Zero; }
+            set { return; }
+        }
+
         public override float CollisionScore
         {
             get { return 0f; }
@@ -255,6 +261,10 @@ namespace OpenSim.Region.Physics.POSPlugin
         {
         }
 
+        public override void AddAngularForce(PhysicsVector force, bool pushforce)
+        {
+        }
+
         public override void SetMomentum(PhysicsVector momentum)
         {
         }
diff --git a/OpenSim/Region/Physics/POSPlugin/POSPrim.cs b/OpenSim/Region/Physics/POSPlugin/POSPrim.cs
index fdd095f..bf96c35 100644
--- a/OpenSim/Region/Physics/POSPlugin/POSPrim.cs
+++ b/OpenSim/Region/Physics/POSPlugin/POSPrim.cs
@@ -211,6 +211,16 @@ namespace OpenSim.Region.Physics.POSPlugin
         {
         }
 
+        public override void AddAngularForce(PhysicsVector force, bool pushforce)
+        {
+        }
+
+        public override PhysicsVector Torque
+        {
+            get { return PhysicsVector.Zero; }
+            set { return; }
+        }
+
         public override void SetMomentum(PhysicsVector momentum)
         {
         }
diff --git a/OpenSim/Region/Physics/PhysXPlugin/PhysXPlugin.cs b/OpenSim/Region/Physics/PhysXPlugin/PhysXPlugin.cs
index 940c9bc..6502827 100644
--- a/OpenSim/Region/Physics/PhysXPlugin/PhysXPlugin.cs
+++ b/OpenSim/Region/Physics/PhysXPlugin/PhysXPlugin.cs
@@ -417,6 +417,14 @@ namespace OpenSim.Region.Physics.PhysXPlugin
         public override void AddForce(PhysicsVector force, bool pushforce)
         {
         }
+        public override PhysicsVector Torque
+        {
+            get { return PhysicsVector.Zero; }
+            set { return; }
+        }
+        public override void AddAngularForce(PhysicsVector force, bool pushforce)
+        {
+        }
 
         public override void link(PhysicsActor obj)
         {
@@ -625,6 +633,12 @@ namespace OpenSim.Region.Physics.PhysXPlugin
             set { _velocity = value; }
         }
 
+        public override PhysicsVector Torque
+        {
+            get { return PhysicsVector.Zero; }
+            set { return; }
+        }
+
         public override float CollisionScore
         {
             get { return 0f; }
@@ -666,6 +680,10 @@ namespace OpenSim.Region.Physics.PhysXPlugin
         {
         }
 
+        public override void AddAngularForce(PhysicsVector force, bool pushforce)
+        {
+        }
+
         public override void SetMomentum(PhysicsVector momentum)
         {
         }