10 files changed, 193 insertions, 6 deletions

diff --git a/OpenSim/Region/Environment/Scenes/SceneObjectGroup.cs b/OpenSim/Region/Environment/Scenes/SceneObjectGroup.cs
index 2875874..2f2a1be 100644
--- a/OpenSim/Region/Environment/Scenes/SceneObjectGroup.cs
+++ b/OpenSim/Region/Environment/Scenes/SceneObjectGroup.cs
@@ -694,6 +694,25 @@ namespace OpenSim.Region.Environment.Scenes
             }
         }
 
+        public void moveToTarget(LLVector3 target, float tau)
+        {
+            SceneObjectPart rootpart = m_rootPart;
+            if (rootpart != null)
+            {
+                rootpart.PhysActor.PIDTarget = new PhysicsVector(target.X, target.Y, target.Z);
+                rootpart.PhysActor.PIDTau = tau;
+                rootpart.PhysActor.PIDActive = true;
+            }
+        }
+        public void stopMoveToTarget()
+        {
+            SceneObjectPart rootpart = m_rootPart;
+            if (rootpart != null)
+            {
+                rootpart.PhysActor.PIDActive = false;
+            }
+        }
+
         public void SetRootPartOwner(SceneObjectPart part, LLUUID cAgentID, LLUUID cGroupID)
         {
             part.LastOwnerID = part.OwnerID;
@@ -2019,5 +2038,7 @@ namespace OpenSim.Region.Environment.Scenes
                 }
             }
         }
+
+        
     }
 }
diff --git a/OpenSim/Region/Environment/Scenes/SceneObjectPart.cs b/OpenSim/Region/Environment/Scenes/SceneObjectPart.cs
index 48177dc..cbefc19 100644
--- a/OpenSim/Region/Environment/Scenes/SceneObjectPart.cs
+++ b/OpenSim/Region/Environment/Scenes/SceneObjectPart.cs
@@ -586,6 +586,24 @@ namespace OpenSim.Region.Environment.Scenes
             }
         }
 
+        public void MoveToTarget(LLVector3 target, float tau)
+        {
+            if (tau > 0)
+            {
+                m_parentGroup.moveToTarget(target, tau);
+            }
+            else
+            {
+                StopMoveToTarget();
+            }
+
+        }
+
+        public void StopMoveToTarget()
+        {
+            m_parentGroup.stopMoveToTarget();
+        }
+
         public void TriggerScriptChangedEvent(Changed val)
         {
             if (m_parentGroup != null)
diff --git a/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsPlugin.cs b/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsPlugin.cs
index 8459fbc..16ec66e 100644
--- a/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsPlugin.cs
+++ b/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsPlugin.cs
@@ -390,5 +390,8 @@ namespace OpenSim.Region.Physics.BasicPhysicsPlugin
         public override void CrossingFailure()
         {
         }
+        public override PhysicsVector PIDTarget { set { return; } }
+        public override bool PIDActive { set { return; } }
+        public override float PIDTau { set { return; } }
     }
 }
diff --git a/OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs b/OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs
index f100ab0..bc777e2 100644
--- a/OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs
+++ b/OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs
@@ -992,6 +992,9 @@ namespace OpenSim.Region.Physics.BulletXPlugin
         {
 
         }
+        public override PhysicsVector PIDTarget { set { return; } }
+        public override bool PIDActive { set { return; } }
+        public override float PIDTau { set { return; } }
     }
 
     /// <summary>
diff --git a/OpenSim/Region/Physics/Manager/PhysicsActor.cs b/OpenSim/Region/Physics/Manager/PhysicsActor.cs
index c63e1f7..b40635c 100644
--- a/OpenSim/Region/Physics/Manager/PhysicsActor.cs
+++ b/OpenSim/Region/Physics/Manager/PhysicsActor.cs
@@ -180,6 +180,10 @@ namespace OpenSim.Region.Physics.Manager
         public abstract PhysicsVector RotationalVelocity { get; set; }
         public abstract bool Kinematic { get; set; }
         public abstract float Buoyancy { get; set; }
+        public abstract PhysicsVector PIDTarget { set;}
+        public abstract bool PIDActive { set;}
+        public abstract float PIDTau { set; }
+
 
         public abstract void AddForce(PhysicsVector force);
         public abstract void SetMomentum(PhysicsVector momentum);
@@ -343,6 +347,7 @@ namespace OpenSim.Region.Physics.Manager
         {
         }
 
+
         public override void AddForce(PhysicsVector force)
         {
         }
@@ -353,6 +358,10 @@ namespace OpenSim.Region.Physics.Manager
             set { return; }
         }
 
+        public override PhysicsVector PIDTarget { set { return; } }
+        public override bool PIDActive { set { return; } }
+        public override float PIDTau { set { return; } }
+
         public override void SetMomentum(PhysicsVector momentum)
         {
         }
diff --git a/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs
index 30f0b06..b870a77 100644
--- a/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs
+++ b/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs
@@ -852,5 +852,8 @@ namespace OpenSim.Region.Physics.OdePlugin
         public override void CrossingFailure()
         {
         }
+        public override PhysicsVector PIDTarget { set { return; } }
+        public override bool PIDActive { set { return; } }
+        public override float PIDTau { set { return; } }
     }
 }
diff --git a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
index ddadc98..43a7272 100644
--- a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
+++ b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
@@ -46,11 +46,17 @@ namespace OpenSim.Region.Physics.OdePlugin
         private PhysicsVector m_rotationalVelocity;
         private PhysicsVector _size;
         private PhysicsVector _acceleration;
+        private d.Vector3 _zeroPosition = new d.Vector3(0.0f, 0.0f, 0.0f);
         private Quaternion _orientation;
         private PhysicsVector m_taintposition;
         private PhysicsVector m_taintsize;
         private PhysicsVector m_taintVelocity = PhysicsVector.Zero;
         private Quaternion m_taintrot;
+
+        private PhysicsVector m_PIDTarget = new PhysicsVector(0, 0, 0);
+        private float m_PIDTau = 0f;
+        private bool m_usePID = false;
+
         private const CollisionCategories m_default_collisionFlags = (CollisionCategories.Geom
                                                         | CollisionCategories.Space
                                                         | CollisionCategories.Body
@@ -1079,31 +1085,138 @@ namespace OpenSim.Region.Physics.OdePlugin
 
         public void Move(float timestep)
         {
+            float fx = 0;
+            float fy = 0;
+            float fz = 0;
 
             if (IsPhysical && Body != (IntPtr)0 && !m_isSelected)
             {
+                float PID_D = 2200.0f;
+                float PID_P = 900.0f;
+                
+                
                 float m_mass = CalculateMass();
+                
+                fz = 0f;
                     //m_log.Info(m_collisionFlags.ToString());
+
+               
+
+
                 if (m_buoyancy != 0)
                 {
-                    float buoyancy = 0f;
+                    
                     if (m_buoyancy > 0)
                     {
-                         buoyancy = ((9.8f * m_buoyancy) * m_mass);
+                         fz = ((9.8f * m_buoyancy) * m_mass);
                          
                         //d.Vector3 l_velocity = d.BodyGetLinearVel(Body);
                         //m_log.Info("Using Buoyancy: " + buoyancy + " G: " + (9.8f * m_buoyancy) + "mass:" + m_mass + "  Pos: " + Position.ToString());
                     }
                     else 
                     {
-                        buoyancy = (-1 * ((9.8f * (-1 * m_buoyancy)) * m_mass));
+                        fz = (-1 * ((9.8f * (-1 * m_buoyancy)) * m_mass));
+                    }
+                    
+
+                }
+
+                if (m_usePID)
+                {
+
+                    // If we're using the PID controller, then we have no gravity             
+                    fz = ((9.8f) * this.Mass );
+
+                    //  no lock; for now it's only called from within Simulate()
+
+                    // If the PID Controller isn't active then we set our force 
+                    // calculating base velocity to the current position
+                    if (System.Environment.OSVersion.Platform == PlatformID.Unix)
+                    {
+                        PID_D = 3200.0f;
+                        PID_P = 1400.0f;
+                    }
+                    else
+                    {
+                        PID_D = 2200.0f;
+                        PID_P = 900.0f;
+                    }
+                    PID_D = 1.0f;
+                    PID_P = 1.0f;
+
+                    
+                    //PidStatus = true;
+
+                    PhysicsVector vec = new PhysicsVector();
+                    d.Vector3 vel = d.BodyGetLinearVel(Body);
+                    
+
+                    d.Vector3 pos = d.BodyGetPosition(Body);
+                    _target_velocity = 
+                        new PhysicsVector(
+                            (m_PIDTarget.X - pos.X) / m_PIDTau,
+                            (m_PIDTarget.Y - pos.Y) / m_PIDTau, 
+                            (m_PIDTarget.Z - pos.Z) / m_PIDTau
+                            );
+
+
+                    //  if velocity is zero, use position control; otherwise, velocity control
+                    
+                    if (_target_velocity.IsIdentical(PhysicsVector.Zero,0.1f))
+                    {
+                        //  keep track of where we stopped.  No more slippin' & slidin'
+                        
+                       
+                        // We only want to deactivate the PID Controller if we think we want to have our surrogate
+                        // react to the physics scene by moving it's position.
+                        // Avatar to Avatar collisions
+                        // Prim to avatar collisions
+
+                        
+                        //fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2);
+                        //fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2);
+                        //fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P;
+                        d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);
+                        d.BodySetLinearVel(Body, 0, 0, 0);
+                        d.BodyAddForce(Body, 0, 0, fz);
+                        return;
+
+                    }
+                    else
+                    {
+                       
+                        _zeroFlag = false;
+                        
+                        // We're flying and colliding with something
+                        fx = ((_target_velocity.X / m_PIDTau) - vel.X) * (PID_D / 6);
+                        fy = ((_target_velocity.Y / m_PIDTau) - vel.Y) * (PID_D / 6);
+                    
+
+                   
+                        
+                       // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P;
+
+                        fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
                     }
-                    d.BodyAddForce(Body, 0, 0, buoyancy);
 
+                } 
+
+                fx *= m_mass;
+                fy *= m_mass;
+                //fz *= m_mass;
+                
+                //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString());
+                if (fx != 0 || fy != 0 || fz != 0)
+                {
+                    //m_taintdisable = true;
+                    //base.RaiseOutOfBounds(Position);
+                    //d.BodySetLinearVel(Body, fx, fy, 0f);
+                    d.BodyAddForce(Body, fx, fy, fz);
                 }
             }
             else
             {
+                _zeroPosition = d.BodyGetPosition(Body);
                 return;
             }
         }
@@ -1952,5 +2065,8 @@ namespace OpenSim.Region.Physics.OdePlugin
         public override void SetMomentum(PhysicsVector momentum)
         {
         }
+        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); } }
     }
 }
diff --git a/OpenSim/Region/Physics/POSPlugin/POSPlugin.cs b/OpenSim/Region/Physics/POSPlugin/POSPlugin.cs
index 57fcbe4..12b2d8f 100644
--- a/OpenSim/Region/Physics/POSPlugin/POSPlugin.cs
+++ b/OpenSim/Region/Physics/POSPlugin/POSPlugin.cs
@@ -518,6 +518,9 @@ namespace OpenSim.Region.Physics.POSPlugin
         {
 
         }
+        public override PhysicsVector PIDTarget { set { return; } }
+        public override bool PIDActive { set { return; } }
+        public override float PIDTau { set { return; } }
     }
 
     public class POSPrim : PhysicsActor
@@ -711,5 +714,8 @@ namespace OpenSim.Region.Physics.POSPlugin
         public override void CrossingFailure()
         {
         }
+        public override PhysicsVector PIDTarget { set { return; } }
+        public override bool PIDActive { set { return; } }
+        public override float PIDTau { set { return; } }
     }
 }
diff --git a/OpenSim/Region/Physics/PhysXPlugin/PhysXPlugin.cs b/OpenSim/Region/Physics/PhysXPlugin/PhysXPlugin.cs
index 3541b5c..2d00bdb 100644
--- a/OpenSim/Region/Physics/PhysXPlugin/PhysXPlugin.cs
+++ b/OpenSim/Region/Physics/PhysXPlugin/PhysXPlugin.cs
@@ -433,6 +433,9 @@ namespace OpenSim.Region.Physics.PhysXPlugin
         {
 
         }
+        public override PhysicsVector PIDTarget { set { return; } }
+        public override bool PIDActive { set { return; } }
+        public override float PIDTau { set { return; } }
     }
 
 
@@ -650,5 +653,8 @@ namespace OpenSim.Region.Physics.PhysXPlugin
         public override void CrossingFailure()
         {
         }
+        public override PhysicsVector PIDTarget { set { return; } }
+        public override bool PIDActive { set { return; } }
+        public override float PIDTau { set { return; } }
     }
 }
diff --git a/OpenSim/Region/ScriptEngine/Common/LSL_BuiltIn_Commands.cs b/OpenSim/Region/ScriptEngine/Common/LSL_BuiltIn_Commands.cs
index 82d67d4..73d901d 100644
--- a/OpenSim/Region/ScriptEngine/Common/LSL_BuiltIn_Commands.cs
+++ b/OpenSim/Region/ScriptEngine/Common/LSL_BuiltIn_Commands.cs
@@ -1097,13 +1097,15 @@ namespace OpenSim.Region.ScriptEngine.Common
         public void llMoveToTarget(LSL_Types.Vector3 target, double tau)
         {
             m_host.AddScriptLPS(1);
-            NotImplemented("llMoveToTarget");
+            m_host.MoveToTarget(new LLVector3((float)target.x, (float)target.y, (float)target.z), (float)tau);
+            //NotImplemented("llMoveToTarget");
         }
 
         public void llStopMoveToTarget()
         {
             m_host.AddScriptLPS(1);
-            NotImplemented("llStopMoveToTarget");
+            m_host.StopMoveToTarget();
+            //NotImplemented("llStopMoveToTarget");
         }
 
         public void llApplyImpulse(LSL_Types.Vector3 force, int local)