11 files changed, 287 insertions, 8 deletions

diff --git a/OpenSim/Region/Environment/Scenes/SceneObjectGroup.cs b/OpenSim/Region/Environment/Scenes/SceneObjectGroup.cs
index fabf276..cc99929 100644
--- a/OpenSim/Region/Environment/Scenes/SceneObjectGroup.cs
+++ b/OpenSim/Region/Environment/Scenes/SceneObjectGroup.cs
@@ -1466,6 +1466,64 @@ namespace OpenSim.Region.Environment.Scenes
             }
         }
 
+        public void applyAngularImpulse(PhysicsVector impulse)
+        {
+            // We check if rootpart is null here because scripts don't delete if you delete the host.
+            // This means that unfortunately, we can pass a null physics actor to Simulate!
+            // Make sure we don't do that!
+            SceneObjectPart rootpart = m_rootPart;
+            if (rootpart != null)
+            {
+                if (rootpart.PhysActor != null)
+                {
+                    if (!IsAttachment)
+                    {
+                        rootpart.PhysActor.AddAngularForce(impulse, true);
+                        m_scene.PhysicsScene.AddPhysicsActorTaint(rootpart.PhysActor);
+                    }
+                }
+            }
+        }
+
+        public void setAngularImpulse(PhysicsVector impulse)
+        {
+            // We check if rootpart is null here because scripts don't delete if you delete the host.
+            // This means that unfortunately, we can pass a null physics actor to Simulate!
+            // Make sure we don't do that!
+            SceneObjectPart rootpart = m_rootPart;
+            if (rootpart != null)
+            {
+                if (rootpart.PhysActor != null)
+                {
+                    if (!IsAttachment)
+                    {
+                        rootpart.PhysActor.Torque = impulse;
+                        m_scene.PhysicsScene.AddPhysicsActorTaint(rootpart.PhysActor);
+                    }
+                }
+            }
+        }
+
+        public Vector3 GetTorque()
+        {
+            // We check if rootpart is null here because scripts don't delete if you delete the host.
+            // This means that unfortunately, we can pass a null physics actor to Simulate!
+            // Make sure we don't do that!
+            SceneObjectPart rootpart = m_rootPart;
+            if (rootpart != null)
+            {
+                if (rootpart.PhysActor != null)
+                {
+                    if (!IsAttachment)
+                    {
+                        PhysicsVector torque = rootpart.PhysActor.Torque;
+                        return new Vector3(torque.X, torque.Y, torque.Z);
+                    }
+                }
+            }
+            return Vector3.Zero;
+        }
+
         public void moveToTarget(Vector3 target, float tau)
         {
             SceneObjectPart rootpart = m_rootPart;
diff --git a/OpenSim/Region/Environment/Scenes/SceneObjectPart.cs b/OpenSim/Region/Environment/Scenes/SceneObjectPart.cs
index efc9289..5a43df6 100644
--- a/OpenSim/Region/Environment/Scenes/SceneObjectPart.cs
+++ b/OpenSim/Region/Environment/Scenes/SceneObjectPart.cs
@@ -1180,6 +1180,68 @@ if (m_shape != null) {
             }
         }
 
+
+        /// <summary>
+        /// hook to the physics scene to apply angular impulse
+        /// This is sent up to the group, which then finds the root prim
+        /// and applies the force on the root prim of the group
+        /// </summary>
+        /// <param name="impulsei">Vector force</param>
+        /// <param name="localGlobalTF">true for the local frame, false for the global frame</param>
+        public void ApplyAngularImpulse(Vector3 impulsei, bool localGlobalTF)
+        {
+            PhysicsVector impulse = new PhysicsVector(impulsei.X, impulsei.Y, impulsei.Z);
+
+            if (localGlobalTF)
+            {
+                Quaternion grot = GetWorldRotation();
+                Quaternion AXgrot = grot;
+                Vector3 AXimpulsei = impulsei;
+                Vector3 newimpulse = AXimpulsei * AXgrot;
+                impulse = new PhysicsVector(newimpulse.X, newimpulse.Y, newimpulse.Z);
+            }
+
+            if (m_parentGroup != null)
+            {
+                m_parentGroup.applyAngularImpulse(impulse);
+            }
+        }
+
+        /// <summary>
+        /// hook to the physics scene to apply angular impulse
+        /// This is sent up to the group, which then finds the root prim
+        /// and applies the force on the root prim of the group
+        /// </summary>
+        /// <param name="impulsei">Vector force</param>
+        /// <param name="localGlobalTF">true for the local frame, false for the global frame</param>
+        public void SetAngularImpulse(Vector3 impulsei, bool localGlobalTF)
+        {
+            PhysicsVector impulse = new PhysicsVector(impulsei.X, impulsei.Y, impulsei.Z);
+
+            if (localGlobalTF)
+            {
+                Quaternion grot = GetWorldRotation();
+                Quaternion AXgrot = grot;
+                Vector3 AXimpulsei = impulsei;
+                Vector3 newimpulse = AXimpulsei * AXgrot;
+                impulse = new PhysicsVector(newimpulse.X, newimpulse.Y, newimpulse.Z);
+            }
+
+            if (m_parentGroup != null)
+            {
+                m_parentGroup.setAngularImpulse(impulse);
+            }
+        }
+
+        public Vector3 GetTorque()
+        {
+            if (m_parentGroup != null)
+            {
+                m_parentGroup.GetTorque();
+            }
+            return Vector3.Zero;
+        }
+
         /// <summary>
         /// Apply physics to this part.
         /// </summary>
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)
         {
         }
diff --git a/OpenSim/Region/ScriptEngine/Shared/Api/Implementation/LSL_Api.cs b/OpenSim/Region/ScriptEngine/Shared/Api/Implementation/LSL_Api.cs
index 68b92a4..84def93 100644
--- a/OpenSim/Region/ScriptEngine/Shared/Api/Implementation/LSL_Api.cs
+++ b/OpenSim/Region/ScriptEngine/Shared/Api/Implementation/LSL_Api.cs
@@ -1951,26 +1951,28 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
         public void llApplyRotationalImpulse(LSL_Vector force, int local)
         {
             m_host.AddScriptLPS(1);
-            NotImplemented("llApplyRotationalImpulse");
+            m_host.ApplyAngularImpulse(new Vector3((float)force.x, (float)force.y, (float)force.z), local != 0);
+
         }
 
         public void llSetTorque(LSL_Vector torque, int local)
         {
             m_host.AddScriptLPS(1);
-            NotImplemented("llSetTorque");
+            m_host.SetAngularImpulse(new Vector3((float)torque.x, (float)torque.y, (float)torque.z), local != 0);
         }
 
         public LSL_Vector llGetTorque()
         {
             m_host.AddScriptLPS(1);
-            NotImplemented("llGetTorque");
-            return new LSL_Vector();
+            Vector3 torque = m_host.GetTorque();
+            return new LSL_Vector(torque.X,torque.Y,torque.Z);
         }
 
         public void llSetForceAndTorque(LSL_Vector force, LSL_Vector torque, int local)
         {
             m_host.AddScriptLPS(1);
-            NotImplemented("llSetForceAndTorque");
+            llSetForce(force, local);
+            llSetTorque(torque, local);
         }
 
         public LSL_Vector llGetVel()