1 files changed, 792 insertions, 185 deletions

diff --git a/OpenSim/Region/Framework/Scenes/ScenePresence.cs b/OpenSim/Region/Framework/Scenes/ScenePresence.cs
index bfc1bd6..b340bda 100644
--- a/OpenSim/Region/Framework/Scenes/ScenePresence.cs
+++ b/OpenSim/Region/Framework/Scenes/ScenePresence.cs
@@ -26,6 +26,7 @@
  */
 
 using System;
+using System.Xml;
 using System.Collections.Generic;
 using System.Reflection;
 using System.Timers;
@@ -72,7 +73,7 @@ namespace OpenSim.Region.Framework.Scenes
 //        {
 //            m_log.Debug("[ScenePresence] Destructor called");
 //        }
-
+        
         private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
 
 //        private static readonly byte[] DEFAULT_TEXTURE = AvatarAppearance.GetDefaultTexture().GetBytes();
@@ -87,7 +88,9 @@ namespace OpenSim.Region.Framework.Scenes
         /// rotation, prim cut, prim twist, prim taper, and prim shear. See mantis
         /// issue #1716
         /// </summary>
-        private static readonly Vector3 SIT_TARGET_ADJUSTMENT = new Vector3(0.1f, 0.0f, 0.3f);
+//        private static readonly Vector3 SIT_TARGET_ADJUSTMENT = new Vector3(0.1f, 0.0f, 0.3f);
+                // Value revised by KF 091121 by comparison with SL.
+        private static readonly Vector3 SIT_TARGET_ADJUSTMENT = new Vector3(0.0f, 0.0f, 0.418f);
 
         public UUID currentParcelUUID = UUID.Zero;
 
@@ -121,8 +124,11 @@ namespace OpenSim.Region.Framework.Scenes
         public Vector3 lastKnownAllowedPosition;
         public bool sentMessageAboutRestrictedParcelFlyingDown;
         public Vector4 CollisionPlane = Vector4.UnitW;
-
+        
+                private Vector3 m_avInitialPos;         // used to calculate unscripted sit rotation
+                private Vector3 m_avUnscriptedSitPos;   // for non-scripted prims
         private Vector3 m_lastPosition;
+        private Vector3 m_lastWorldPosition;
         private Quaternion m_lastRotation;
         private Vector3 m_lastVelocity;
         //private int m_lastTerseSent;
@@ -153,9 +159,10 @@ namespace OpenSim.Region.Framework.Scenes
         private int m_perfMonMS;
 
         private bool m_setAlwaysRun;
-        
         private bool m_forceFly;
         private bool m_flyDisabled;
+        private bool m_flyingOld;               // add for fly velocity control
+        public bool m_wasFlying;                // add for fly velocity control
 
         private float m_speedModifier = 1.0f;
 
@@ -174,7 +181,8 @@ namespace OpenSim.Region.Framework.Scenes
         protected RegionInfo m_regionInfo;
         protected ulong crossingFromRegion;
 
-        private readonly Vector3[] Dir_Vectors = new Vector3[9];
+        private readonly Vector3[] Dir_Vectors = new Vector3[11];
+        private bool m_isNudging = false;
 
         // Position of agent's camera in world (region cordinates)
         protected Vector3 m_CameraCenter;
@@ -199,17 +207,25 @@ namespace OpenSim.Region.Framework.Scenes
         private bool m_autopilotMoving;
         private Vector3 m_autoPilotTarget;
         private bool m_sitAtAutoTarget;
+        private Vector3 m_initialSitTarget = Vector3.Zero;              //KF: First estimate of where to sit
 
         private string m_nextSitAnimation = String.Empty;
 
         //PauPaw:Proper PID Controler for autopilot************
         private bool m_moveToPositionInProgress;
         private Vector3 m_moveToPositionTarget;
+        private Quaternion m_offsetRotation = new Quaternion(0.0f, 0.0f, 0.0f, 1.0f);
 
         private bool m_followCamAuto;
 
         private int m_movementUpdateCount;
+        private int m_lastColCount = -1;                //KF: Look for Collision chnages
+        private int m_updateCount = 0;                  //KF: Update Anims for a while
+        private static readonly int UPDATE_COUNT = 10;          // how many frames to update for
         private const int NumMovementsBetweenRayCast = 5;
+        private List<uint> m_lastColliders = new List<uint>();
+
+        private object m_syncRoot = new Object();
 
         private bool CameraConstraintActive;
         //private int m_moveToPositionStateStatus;
@@ -236,7 +252,9 @@ namespace OpenSim.Region.Framework.Scenes
             DIR_CONTROL_FLAG_UP = AgentManager.ControlFlags.AGENT_CONTROL_UP_POS,
             DIR_CONTROL_FLAG_DOWN = AgentManager.ControlFlags.AGENT_CONTROL_UP_NEG,
             DIR_CONTROL_FLAG_FORWARD_NUDGE = AgentManager.ControlFlags.AGENT_CONTROL_NUDGE_AT_POS,
-            DIR_CONTROL_FLAG_BACKWARD_NUDGE = AgentManager.ControlFlags.AGENT_CONTROL_NUDGE_AT_NEG,
+            DIR_CONTROL_FLAG_BACK_NUDGE = AgentManager.ControlFlags.AGENT_CONTROL_NUDGE_AT_NEG,
+            DIR_CONTROL_FLAG_LEFT_NUDGE = AgentManager.ControlFlags.AGENT_CONTROL_NUDGE_LEFT_POS,
+            DIR_CONTROL_FLAG_RIGHT_NUDGE = AgentManager.ControlFlags.AGENT_CONTROL_NUDGE_LEFT_NEG,
             DIR_CONTROL_FLAG_DOWN_NUDGE = AgentManager.ControlFlags.AGENT_CONTROL_NUDGE_UP_NEG
         }
         
@@ -443,9 +461,18 @@ namespace OpenSim.Region.Framework.Scenes
             get
             {
                 PhysicsActor actor = m_physicsActor;
-                if (actor != null)
+//                if (actor != null)
+                if ((actor != null) && (m_parentID == 0))   // KF Do NOT update m_pos here if Av is sitting!
                     m_pos = actor.Position;
 
+                // If we're sitting, we need to update our position
+                if (m_parentID != 0)
+                {
+                    SceneObjectPart part = m_scene.GetSceneObjectPart(m_parentID);
+                    if (part != null)
+                        m_parentPosition = part.AbsolutePosition;
+                }
+
                 return m_parentPosition + m_pos;
             }
             set
@@ -464,7 +491,8 @@ namespace OpenSim.Region.Framework.Scenes
                     }
                 }
 
-                m_pos = value;
+                if (m_parentID == 0)   // KF Do NOT update m_pos here if Av is sitting!
+                    m_pos = value;
                 m_parentPosition = Vector3.Zero;
             }
         }
@@ -508,10 +536,39 @@ namespace OpenSim.Region.Framework.Scenes
             }
         }
 
+        public Quaternion OffsetRotation
+        {
+            get { return m_offsetRotation; }
+            set { m_offsetRotation = value; }
+        }
+
         public Quaternion Rotation
         {
-            get { return m_bodyRot; }
-            set { m_bodyRot = value; }
+            get {
+                if (m_parentID != 0)
+                {
+                    if (m_offsetRotation != null)
+                    {
+                        return m_offsetRotation;
+                    }
+                    else
+                    {
+                        return new Quaternion(0.0f, 0.0f, 0.0f, 1.0f);
+                    }
+                    
+                }
+                else
+                {
+                    return m_bodyRot;
+                }
+            }
+            set { 
+                m_bodyRot = value;
+                if (m_parentID != 0)
+                {
+                    m_offsetRotation = new Quaternion(0.0f, 0.0f, 0.0f, 1.0f);
+                }
+            }
         }
 
         public Quaternion PreviousRotation
@@ -536,11 +593,21 @@ namespace OpenSim.Region.Framework.Scenes
 
         private uint m_parentID;
 
+
+        private UUID m_linkedPrim;
+
         public uint ParentID
         {
             get { return m_parentID; }
             set { m_parentID = value; }
         }
+
+        public UUID LinkedPrim
+        {
+            get { return m_linkedPrim; }
+            set { m_linkedPrim = value; }
+        }
+
         public float Health
         {
             get { return m_health; }
@@ -662,7 +729,7 @@ namespace OpenSim.Region.Framework.Scenes
             CreateSceneViewer();
             m_animator = new ScenePresenceAnimator(this);
         }
-        
+
         private ScenePresence(IClientAPI client, Scene world, RegionInfo reginfo) : this()
         {
             m_rootRegionHandle = reginfo.RegionHandle;
@@ -694,10 +761,7 @@ namespace OpenSim.Region.Framework.Scenes
             m_reprioritization_timer.AutoReset = false;
 
             AdjustKnownSeeds();
-
-            // TODO: I think, this won't send anything, as we are still a child here...
             Animator.TrySetMovementAnimation("STAND"); 
-
             // we created a new ScenePresence (a new child agent) in a fresh region.
             // Request info about all the (root) agents in this region
             // Note: This won't send data *to* other clients in that region (children don't send)
@@ -746,25 +810,47 @@ namespace OpenSim.Region.Framework.Scenes
             Dir_Vectors[3] = -Vector3.UnitY; //RIGHT
             Dir_Vectors[4] = Vector3.UnitZ; //UP
             Dir_Vectors[5] = -Vector3.UnitZ; //DOWN
-            Dir_Vectors[8] = new Vector3(0f, 0f, -0.5f); //DOWN_Nudge
-            Dir_Vectors[6] = Vector3.UnitX*2; //FORWARD
-            Dir_Vectors[7] = -Vector3.UnitX; //BACK
+            Dir_Vectors[6] = new Vector3(0.5f, 0f, 0f); //FORWARD_NUDGE
+            Dir_Vectors[7] = new Vector3(-0.5f, 0f, 0f);  //BACK_NUDGE
+            Dir_Vectors[8] = new Vector3(0f, 0.5f, 0f);  //LEFT_NUDGE
+            Dir_Vectors[9] = new Vector3(0f, -0.5f, 0f);  //RIGHT_NUDGE
+            Dir_Vectors[10] = new Vector3(0f, 0f, -0.5f); //DOWN_Nudge
         }
 
         private Vector3[] GetWalkDirectionVectors()
         {
-            Vector3[] vector = new Vector3[9];
+            Vector3[] vector = new Vector3[11];
             vector[0] = new Vector3(m_CameraUpAxis.Z, 0f, -m_CameraAtAxis.Z); //FORWARD
             vector[1] = new Vector3(-m_CameraUpAxis.Z, 0f, m_CameraAtAxis.Z); //BACK
             vector[2] = Vector3.UnitY; //LEFT
             vector[3] = -Vector3.UnitY; //RIGHT
             vector[4] = new Vector3(m_CameraAtAxis.Z, 0f, m_CameraUpAxis.Z); //UP
             vector[5] = new Vector3(-m_CameraAtAxis.Z, 0f, -m_CameraUpAxis.Z); //DOWN
-            vector[8] = new Vector3(-m_CameraAtAxis.Z, 0f, -m_CameraUpAxis.Z); //DOWN_Nudge
-            vector[6] = (new Vector3(m_CameraUpAxis.Z, 0f, -m_CameraAtAxis.Z) * 2); //FORWARD Nudge
-            vector[7] = new Vector3(-m_CameraUpAxis.Z, 0f, m_CameraAtAxis.Z); //BACK Nudge
+            vector[6] = new Vector3(m_CameraUpAxis.Z, 0f, -m_CameraAtAxis.Z); //FORWARD_NUDGE
+            vector[7] = new Vector3(-m_CameraUpAxis.Z, 0f, m_CameraAtAxis.Z); //BACK_NUDGE
+            vector[8] = Vector3.UnitY; //LEFT_NUDGE
+            vector[9] = -Vector3.UnitY; //RIGHT_NUDGE
+            vector[10] = new Vector3(-m_CameraAtAxis.Z, 0f, -m_CameraUpAxis.Z); //DOWN_NUDGE
             return vector;
         }
+        
+        private bool[] GetDirectionIsNudge()
+        {
+            bool[] isNudge = new bool[11];
+            isNudge[0] = false; //FORWARD
+            isNudge[1] = false; //BACK
+            isNudge[2] = false; //LEFT
+            isNudge[3] = false; //RIGHT
+            isNudge[4] = false; //UP
+            isNudge[5] = false; //DOWN
+            isNudge[6] = true; //FORWARD_NUDGE
+            isNudge[7] = true; //BACK_NUDGE
+            isNudge[8] = true; //LEFT_NUDGE
+            isNudge[9] = true; //RIGHT_NUDGE
+            isNudge[10] = true; //DOWN_Nudge
+            return isNudge;
+        }
+
 
         #endregion
 
@@ -826,6 +912,52 @@ namespace OpenSim.Region.Framework.Scenes
                 pos.Y = crossedBorder.BorderLine.Z - 1;
             }
 
+            //If they're TP'ing in or logging in, we haven't had time to add any known child regions yet.
+            //This has the unfortunate consequence that if somebody is TP'ing who is already a child agent,
+            //they'll bypass the landing point. But I can't think of any decent way of fixing this.
+            if (KnownChildRegionHandles.Count == 0)
+            {
+                ILandObject land = m_scene.LandChannel.GetLandObject(pos.X, pos.Y);
+                if (land != null)
+                {
+                    //Don't restrict gods, estate managers, or land owners to the TP point. This behaviour mimics agni.
+                    if (land.LandData.LandingType == (byte)1 && land.LandData.UserLocation != Vector3.Zero && UserLevel < 200 && !m_scene.RegionInfo.EstateSettings.IsEstateManager(m_uuid) && land.LandData.OwnerID != m_uuid)
+                    {
+                        pos = land.LandData.UserLocation;
+                    }
+                }
+            }
+
+            if (pos.X < 0 || pos.Y < 0 || pos.Z < 0)
+            {
+                Vector3 emergencyPos = new Vector3(((int)Constants.RegionSize * 0.5f), ((int)Constants.RegionSize * 0.5f), 128);
+                
+                if (pos.X < 0)
+                {
+                    emergencyPos.X = (int)Constants.RegionSize + pos.X;
+                    if (!(pos.Y < 0))
+                        emergencyPos.Y = pos.Y;
+                    if (!(pos.Z < 0))
+                        emergencyPos.Z = pos.Z;
+                }
+                if (pos.Y < 0)
+                {
+                    emergencyPos.Y = (int)Constants.RegionSize + pos.Y;
+                    if (!(pos.X < 0))
+                        emergencyPos.X = pos.X;
+                    if (!(pos.Z < 0))
+                        emergencyPos.Z = pos.Z;
+                }
+                if (pos.Z < 0)
+                {
+                    emergencyPos.Z = 128;
+                    if (!(pos.Y < 0))
+                        emergencyPos.Y = pos.Y;
+                    if (!(pos.X < 0))
+                        emergencyPos.X = pos.X;
+                }
+            }
+
             if (pos.X < 0f || pos.Y < 0f || pos.Z < 0f)
             {
                 m_log.WarnFormat(
@@ -954,12 +1086,17 @@ namespace OpenSim.Region.Framework.Scenes
         {
             if (PhysicsActor != null)
             {
-                m_physicsActor.OnRequestTerseUpdate -= SendTerseUpdateToAllClients;
-                m_physicsActor.OnOutOfBounds -= OutOfBoundsCall;
-                m_scene.PhysicsScene.RemoveAvatar(PhysicsActor);
-                m_physicsActor.UnSubscribeEvents();
-                m_physicsActor.OnCollisionUpdate -= PhysicsCollisionUpdate;
-                PhysicsActor = null;
+                try
+                {
+                    m_physicsActor.OnRequestTerseUpdate -= SendTerseUpdateToAllClients;
+                    m_physicsActor.OnOutOfBounds -= OutOfBoundsCall;
+                    m_physicsActor.OnCollisionUpdate -= PhysicsCollisionUpdate;
+                    m_scene.PhysicsScene.RemoveAvatar(PhysicsActor);
+                    m_physicsActor.UnSubscribeEvents();
+                    PhysicsActor = null;
+                }
+                catch
+                { }
             }
         }
 
@@ -970,9 +1107,10 @@ namespace OpenSim.Region.Framework.Scenes
         public void Teleport(Vector3 pos)
         {
             bool isFlying = false;
+            
             if (m_physicsActor != null)
                 isFlying = m_physicsActor.Flying;
-            
+
             RemoveFromPhysicalScene();
             Velocity = Vector3.Zero;
             AbsolutePosition = pos;
@@ -984,6 +1122,7 @@ namespace OpenSim.Region.Framework.Scenes
             }
 
             SendTerseUpdateToAllClients();
+            
         }
 
         public void TeleportWithMomentum(Vector3 pos)
@@ -1202,6 +1341,7 @@ namespace OpenSim.Region.Framework.Scenes
                 m_log.Error("[AVATAR]: NonFinite Avatar position detected... Reset Position. Mantis this please. Error #9999902");
 
                 m_pos = m_LastFinitePos;
+                
                 if (!m_pos.IsFinite())
                 {
                     m_pos.X = 127f;
@@ -1268,7 +1408,6 @@ namespace OpenSim.Region.Framework.Scenes
                     m_scene.PhysicsScene.RaycastWorld(m_pos, Vector3.Normalize(m_CameraCenter - posAdjusted), Vector3.Distance(m_CameraCenter, posAdjusted) + 0.3f, RayCastCameraCallback);
                 }
             }
-
             lock (scriptedcontrols)
             {
                 if (scriptedcontrols.Count > 0)
@@ -1283,6 +1422,9 @@ namespace OpenSim.Region.Framework.Scenes
 
             if ((flags & AgentManager.ControlFlags.AGENT_CONTROL_SIT_ON_GROUND) != 0)
             {
+                m_updateCount = 0;  // Kill animation update burst so that the SIT_G.. will stick.
+                Animator.TrySetMovementAnimation("SIT_GROUND_CONSTRAINED");
+
                 // TODO: This doesn't prevent the user from walking yet.
                 // Setting parent ID would fix this, if we knew what value
                 // to use.  Or we could add a m_isSitting variable.
@@ -1331,12 +1473,20 @@ namespace OpenSim.Region.Framework.Scenes
                 if (actor.Flying != oldflying)
                     update_movementflag = true;
 
+                if (m_animator.m_jumping)     // add for jumping
+                    update_movementflag = true;
+
                 if (q != m_bodyRot)
                 {
                     m_bodyRot = q;
                     update_rotation = true;
                 }
 
+                //guilty until proven innocent..
+                bool Nudging = true;
+                //Basically, if there is at least one non-nudge control then we don't need
+                //to worry about stopping the avatar
+
                 if (m_parentID == 0)
                 {
                     bool bAllowUpdateMoveToPosition = false;
@@ -1351,9 +1501,12 @@ namespace OpenSim.Region.Framework.Scenes
                     else
                         dirVectors = Dir_Vectors;
 
-                    // The fact that m_movementflag is a byte needs to be fixed
-                    // it really should be a uint
-                    uint nudgehack = 250;
+                    bool[] isNudge = GetDirectionIsNudge();
+
+                    
+                    
+                    
+
                     foreach (Dir_ControlFlags DCF in DIR_CONTROL_FLAGS)
                     {
                         if (((uint)flags & (uint)DCF) != 0)
@@ -1363,40 +1516,28 @@ namespace OpenSim.Region.Framework.Scenes
                             try
                             {
                                 agent_control_v3 += dirVectors[i];
-                                //m_log.DebugFormat("[Motion]: {0}, {1}",i, dirVectors[i]);
+                                if (isNudge[i] == false)
+                                {
+                                    Nudging = false;
+                                }
                             }
                             catch (IndexOutOfRangeException)
                             {
                                 // Why did I get this?
                             }
 
-                            if ((m_movementflag & (byte)(uint)DCF) == 0)
+                            if ((m_movementflag & (uint)DCF) == 0)
                             {
-                                if (DCF == Dir_ControlFlags.DIR_CONTROL_FLAG_FORWARD_NUDGE || DCF == Dir_ControlFlags.DIR_CONTROL_FLAG_BACKWARD_NUDGE)
-                                {
-                                    m_movementflag |= (byte)nudgehack;
-                                }
                                 m_movementflag += (byte)(uint)DCF;
                                 update_movementflag = true;
                             }
                         }
                         else
                         {
-                            if ((m_movementflag & (byte)(uint)DCF) != 0 ||
-                                ((DCF == Dir_ControlFlags.DIR_CONTROL_FLAG_FORWARD_NUDGE || DCF == Dir_ControlFlags.DIR_CONTROL_FLAG_BACKWARD_NUDGE)
-                                && ((m_movementflag & (byte)nudgehack) == nudgehack))
-                                ) // This or is for Nudge forward
+                            if ((m_movementflag & (uint)DCF) != 0)
                             {
-                                m_movementflag -= ((byte)(uint)DCF);
-
+                                m_movementflag -= (byte)(uint)DCF;
                                 update_movementflag = true;
-                                /*
-                                    if ((DCF == Dir_ControlFlags.DIR_CONTROL_FLAG_FORWARD_NUDGE || DCF == Dir_ControlFlags.DIR_CONTROL_FLAG_BACKWARD_NUDGE)
-                                    && ((m_movementflag & (byte)nudgehack) == nudgehack))
-                                    {
-                                        m_log.Debug("Removed Hack flag");
-                                    }
-                                */
                             }
                             else
                             {
@@ -1405,7 +1546,6 @@ namespace OpenSim.Region.Framework.Scenes
                         }
                         i++;
                     }
-
                     //Paupaw:Do Proper PID for Autopilot here
                     if (bResetMoveToPosition)
                     {
@@ -1440,6 +1580,9 @@ namespace OpenSim.Region.Framework.Scenes
                                 // Ignore z component of vector
                                 Vector3 LocalVectorToTarget2D = new Vector3((float)(LocalVectorToTarget3D.X), (float)(LocalVectorToTarget3D.Y), 0f);
                                 LocalVectorToTarget2D.Normalize();
+                                
+                                //We're not nudging
+                                Nudging = false;
                                 agent_control_v3 += LocalVectorToTarget2D;
 
                                 // update avatar movement flags. the avatar coordinate system is as follows:
@@ -1530,13 +1673,13 @@ namespace OpenSim.Region.Framework.Scenes
                     //                    m_log.DebugFormat(
                     //                        "In {0} adding velocity to {1} of {2}", m_scene.RegionInfo.RegionName, Name, agent_control_v3);
 
-                    AddNewMovement(agent_control_v3, q);
+                    AddNewMovement(agent_control_v3, q, Nudging);
 
                     
                 }
             }
 
-            if (update_movementflag && ((flags & AgentManager.ControlFlags.AGENT_CONTROL_SIT_ON_GROUND) == 0) && (m_parentID == 0) && !SitGround)
+            if (update_movementflag && !SitGround)
                 Animator.UpdateMovementAnimations();
 
             m_scene.EventManager.TriggerOnClientMovement(this);
@@ -1551,7 +1694,6 @@ namespace OpenSim.Region.Framework.Scenes
             m_sitAtAutoTarget = false;
             PrimitiveBaseShape proxy = PrimitiveBaseShape.Default;
             //proxy.PCode = (byte)PCode.ParticleSystem;
-
             proxyObjectGroup = new SceneObjectGroup(UUID, Pos, Rotation, proxy);
             proxyObjectGroup.AttachToScene(m_scene);
             
@@ -1593,7 +1735,7 @@ namespace OpenSim.Region.Framework.Scenes
                 }
                 m_moveToPositionInProgress = true;
                 m_moveToPositionTarget = new Vector3(locx, locy, locz);
-            }
+                        }
             catch (Exception ex)
             {
                 //Why did I get this error?
@@ -1615,7 +1757,7 @@ namespace OpenSim.Region.Framework.Scenes
                         Velocity = Vector3.Zero;
                         SendFullUpdateToAllClients();
 
-                        //HandleAgentSit(ControllingClient, m_requestedSitTargetUUID);
+                        HandleAgentSit(ControllingClient, m_requestedSitTargetUUID);  //KF ??
                     }
                     //ControllingClient.SendSitResponse(m_requestedSitTargetID, m_requestedSitOffset, Quaternion.Identity, false, Vector3.Zero, Vector3.Zero, false);
                     m_requestedSitTargetUUID = UUID.Zero;
@@ -1652,42 +1794,77 @@ namespace OpenSim.Region.Framework.Scenes
 
             if (m_parentID != 0)
             {
-                m_log.Debug("StandupCode Executed");
-                SceneObjectPart part = m_scene.GetSceneObjectPart(m_parentID);
+                SceneObjectPart part = m_scene.GetSceneObjectPart(m_requestedSitTargetID);
                 if (part != null)
                 {
+                    part.TaskInventory.LockItemsForRead(true);
                     TaskInventoryDictionary taskIDict = part.TaskInventory;
                     if (taskIDict != null)
                     {
-                        lock (taskIDict)
+                        foreach (UUID taskID in taskIDict.Keys)
                         {
-                            foreach (UUID taskID in taskIDict.Keys)
-                            {
-                                UnRegisterControlEventsToScript(LocalId, taskID);
-                                taskIDict[taskID].PermsMask &= ~(
-                                    2048 | //PERMISSION_CONTROL_CAMERA
-                                    4); // PERMISSION_TAKE_CONTROLS
-                            }
+                            UnRegisterControlEventsToScript(LocalId, taskID);
+                            taskIDict[taskID].PermsMask &= ~(
+                                2048 | //PERMISSION_CONTROL_CAMERA
+                                4); // PERMISSION_TAKE_CONTROLS
                         }
-
                     }
+                    part.TaskInventory.LockItemsForRead(false);
                     // Reset sit target.
                     if (part.GetAvatarOnSitTarget() == UUID)
                         part.SetAvatarOnSitTarget(UUID.Zero);
-
                     m_parentPosition = part.GetWorldPosition();
                     ControllingClient.SendClearFollowCamProperties(part.ParentUUID);
                 }
+                                // part.GetWorldRotation()   is the rotation of the object being sat on
+                                // Rotation  is the sittiing Av's rotation
+
+                        Quaternion partRot;
+//                  if (part.LinkNum == 1)
+//                  {                           // Root prim of linkset
+//                      partRot = part.ParentGroup.RootPart.RotationOffset;
+//                  }
+//                  else
+//                  {                           // single or child prim
+
+//                  }                       
+                if (part == null) //CW: Part may be gone. llDie() for example.
+                {
+                    partRot = new Quaternion(0.0f, 0.0f, 0.0f, 1.0f);
+                }
+                else
+                {
+                    partRot = part.GetWorldRotation();
+                }
+
+                    Quaternion partIRot = Quaternion.Inverse(partRot);
 
+                                Quaternion avatarRot = Quaternion.Inverse(Quaternion.Inverse(Rotation) * partIRot); // world or. of the av
+                                Vector3 avStandUp = new Vector3(1.0f, 0f, 0f) * avatarRot;              // 1M infront of av
+
+                
                 if (m_physicsActor == null)
                 {
                     AddToPhysicalScene(false);
                 }
-
-                m_pos += m_parentPosition + new Vector3(0.0f, 0.0f, 2.0f*m_sitAvatarHeight);
-                m_parentPosition = Vector3.Zero;
-
-                m_parentID = 0;
+                //CW: If the part isn't null then we can set the current position 
+                if (part != null)
+                {
+                    Vector3 avWorldStandUp = avStandUp + part.GetWorldPosition() + ((m_pos - part.OffsetPosition) * partRot);                   // + av sit offset!
+                    AbsolutePosition = avWorldStandUp;                   //KF: Fix stand up.
+                    part.IsOccupied = false;
+                    part.ParentGroup.DeleteAvatar(ControllingClient.AgentId);
+                }
+                else
+                {
+                    //CW: Since the part doesn't exist, a coarse standup position isn't an issue
+                    AbsolutePosition = m_lastWorldPosition;
+                }
+                
+                        m_parentPosition = Vector3.Zero;
+                                m_parentID = 0;
+                m_linkedPrim = UUID.Zero;
+                m_offsetRotation = new Quaternion(0.0f, 0.0f, 0.0f, 1.0f);
                 SendFullUpdateToAllClients();
                 m_requestedSitTargetID = 0;
                 if (m_physicsActor != null && m_appearance != null)
@@ -1696,7 +1873,6 @@ namespace OpenSim.Region.Framework.Scenes
                         SetHeight(m_appearance.AvatarHeight);
                 }
             }
-
             Animator.TrySetMovementAnimation("STAND");
         }
 
@@ -1727,13 +1903,9 @@ namespace OpenSim.Region.Framework.Scenes
                 Vector3 avSitOffSet = part.SitTargetPosition;
                 Quaternion avSitOrientation = part.SitTargetOrientation;
                 UUID avOnTargetAlready = part.GetAvatarOnSitTarget();
-
-                bool SitTargetUnOccupied = (!(avOnTargetAlready != UUID.Zero));
-                bool SitTargetisSet =
-                    (!(avSitOffSet.X == 0f && avSitOffSet.Y == 0f && avSitOffSet.Z == 0f && avSitOrientation.W == 1f &&
-                       avSitOrientation.X == 0f && avSitOrientation.Y == 0f && avSitOrientation.Z == 0f));
-
-                if (SitTargetisSet && SitTargetUnOccupied)
+                bool SitTargetOccupied = (avOnTargetAlready != UUID.Zero);
+                    bool SitTargetisSet = (Vector3.Zero != avSitOffSet);                //NB Latest SL Spec shows Sit Rotation setting is ignored.
+                if (SitTargetisSet && !SitTargetOccupied)
                 {
                     //switch the target to this prim
                     return part;
@@ -1747,84 +1919,164 @@ namespace OpenSim.Region.Framework.Scenes
         private void SendSitResponse(IClientAPI remoteClient, UUID targetID, Vector3 offset, Quaternion pSitOrientation)
         {
             bool autopilot = true;
+            Vector3 autopilotTarget = new Vector3();
+            Quaternion sitOrientation = Quaternion.Identity;
             Vector3 pos = new Vector3();
-            Quaternion sitOrientation = pSitOrientation;
             Vector3 cameraEyeOffset = Vector3.Zero;
             Vector3 cameraAtOffset = Vector3.Zero;
             bool forceMouselook = false;
 
             //SceneObjectPart part =  m_scene.GetSceneObjectPart(targetID);
             SceneObjectPart part =  FindNextAvailableSitTarget(targetID);
-            if (part != null)
-            {
-                // TODO: determine position to sit at based on scene geometry; don't trust offset from client
-                // see http://wiki.secondlife.com/wiki/User:Andrew_Linden/Office_Hours/2007_11_06 for details on how LL does it
-
-                // Is a sit target available?
-                Vector3 avSitOffSet = part.SitTargetPosition;
-                Quaternion avSitOrientation = part.SitTargetOrientation;
-                UUID avOnTargetAlready = part.GetAvatarOnSitTarget();
-
-                bool SitTargetUnOccupied = (!(avOnTargetAlready != UUID.Zero));
-                bool SitTargetisSet =
-                    (!(avSitOffSet.X == 0f && avSitOffSet.Y == 0f && avSitOffSet.Z == 0f &&
-                       (
-                           avSitOrientation.X == 0f && avSitOrientation.Y == 0f && avSitOrientation.Z == 0f && avSitOrientation.W == 1f // Valid Zero Rotation quaternion
-                           || avSitOrientation.X == 0f && avSitOrientation.Y == 0f && avSitOrientation.Z == 1f && avSitOrientation.W == 0f // W-Z Mapping was invalid at one point
-                           || avSitOrientation.X == 0f && avSitOrientation.Y == 0f && avSitOrientation.Z == 0f && avSitOrientation.W == 0f // Invalid Quaternion
-                       )
-                       ));
-
-                if (SitTargetisSet && SitTargetUnOccupied)
-                {
-                    part.SetAvatarOnSitTarget(UUID);
-                    offset = new Vector3(avSitOffSet.X, avSitOffSet.Y, avSitOffSet.Z);
-                    sitOrientation = avSitOrientation;
-                    autopilot = false;
-                }
-
-                pos = part.AbsolutePosition + offset;
-                //if (Math.Abs(part.AbsolutePosition.Z - AbsolutePosition.Z) > 1)
-                //{
-                   // offset = pos;
-                    //autopilot = false;
-                //}
-                if (m_physicsActor != null)
-                {
-                    // If we're not using the client autopilot, we're immediately warping the avatar to the location
-                    // We can remove the physicsActor until they stand up.
-                    m_sitAvatarHeight = m_physicsActor.Size.Z;
-
-                    if (autopilot)
+            if (part == null) return;
+            
+            // TODO: determine position to sit at based on scene geometry; don't trust offset from client
+            // see http://wiki.secondlife.com/wiki/User:Andrew_Linden/Office_Hours/2007_11_06 for details on how LL does it
+                
+                        // part is the prim to sit on
+                        // offset is the world-ref vector distance from that prim center to the click-spot
+                        // UUID  is the UUID of the Avatar doing the clicking
+                                
+                        m_avInitialPos = AbsolutePosition;                      // saved to calculate unscripted sit rotation
+                        
+            // Is a sit target available?
+            Vector3 avSitOffSet = part.SitTargetPosition;
+            Quaternion avSitOrientation = part.SitTargetOrientation;
+
+            bool SitTargetisSet = (Vector3.Zero != avSitOffSet);                //NB Latest SL Spec shows Sit Rotation setting is ignored.
+   //           Quaternion partIRot = Quaternion.Inverse(part.GetWorldRotation());
+                Quaternion partRot;
+//            if (part.LinkNum == 1)
+//            {                         // Root prim of linkset
+//                partRot = part.ParentGroup.RootPart.RotationOffset;
+//            }
+//            else
+//            {                         // single or child prim
+                partRot = part.GetWorldRotation();
+//            }                       
+                Quaternion partIRot = Quaternion.Inverse(partRot);
+//Console.WriteLine("SendSitResponse offset=" + offset + "  Occup=" + part.IsOccupied + "    TargSet=" + SitTargetisSet);
+                        // Sit analysis rewritten by KF 091125 
+            if (SitTargetisSet)                 // scipted sit
+            {
+                                if (!part.IsOccupied)
+                                {
+//Console.WriteLine("Scripted, unoccupied");
+                    part.SetAvatarOnSitTarget(UUID);            // set that Av will be on it
+                    offset = new Vector3(avSitOffSet.X, avSitOffSet.Y, avSitOffSet.Z);  // change ofset to the scripted one
+
+                    Quaternion nrot = avSitOrientation;
+                    if (!part.IsRoot)
                     {
-                        if (Util.GetDistanceTo(AbsolutePosition, pos) < 4.5)
-                        {
-                            autopilot = false;
-
-                            RemoveFromPhysicalScene();
-                            AbsolutePosition = pos + new Vector3(0.0f, 0.0f, m_sitAvatarHeight);
-                        }
+                        nrot = part.RotationOffset * avSitOrientation;
                     }
-                    else
+                    sitOrientation = nrot;              // Change rotatione to the scripted one
+                    OffsetRotation = nrot;
+                    autopilot = false;                                          // Jump direct to scripted llSitPos()
+                }
+                else
+                {
+//Console.WriteLine("Scripted, occupied");                      
+                         return;
+                }
+            }
+            else        // Not Scripted
+            {
+                if ( (Math.Abs(offset.X) > 0.5f) || (Math.Abs(offset.Y) > 0.5f) ) 
+                {
+                        // large prim & offset, ignore if other Avs sitting
+//                      offset.Z -= 0.05f;
+                        m_avUnscriptedSitPos = offset * partIRot;                                       // (non-zero) sit where clicked
+                        autopilotTarget = part.AbsolutePosition + offset;                       // World location of clicked point
+                        
+//Console.WriteLine(" offset ={0}", offset);            
+//Console.WriteLine(" UnscriptedSitPos={0}", m_avUnscriptedSitPos);            
+//Console.WriteLine(" autopilotTarget={0}", autopilotTarget);            
+                        
+                }
+                else            // small offset
+                {
+//Console.WriteLine("Small offset");                            
+                                        if (!part.IsOccupied)
+                                        {
+                                                m_avUnscriptedSitPos = Vector3.Zero;    // Zero = Sit on prim center
+                                                autopilotTarget = part.AbsolutePosition;
+//Console.WriteLine("UsSmall autopilotTarget={0}", autopilotTarget);                                            
+                        }
+                        else return;            // occupied small
+                }       // end large/small
+            } // end Scripted/not
+            cameraAtOffset = part.GetCameraAtOffset();
+            cameraEyeOffset = part.GetCameraEyeOffset();
+            forceMouselook = part.GetForceMouselook();
+            if(cameraAtOffset == Vector3.Zero) cameraAtOffset =  new Vector3(0f, 0f, 0.1f); // 
+            if(cameraEyeOffset == Vector3.Zero) cameraEyeOffset =  new Vector3(0f, 0f, 0.1f); // 
+
+            if (m_physicsActor != null)
+            {
+                // If we're not using the client autopilot, we're immediately warping the avatar to the location
+                // We can remove the physicsActor until they stand up.
+                m_sitAvatarHeight = m_physicsActor.Size.Z;
+                if (autopilot)
+                {                               // its not a scripted sit
+//                        if (Util.GetDistanceTo(AbsolutePosition, autopilotTarget) < 4.5)
+                                        if( (Math.Abs(AbsolutePosition.X - autopilotTarget.X) < 256.0f) && (Math.Abs(AbsolutePosition.Y - autopilotTarget.Y) < 256.0f) )
                     {
+                        autopilot = false;              // close enough
+                        m_lastWorldPosition = m_pos; /* CW - This give us a position to return the avatar to if the part is killed before standup.
+                                                             Not using the part's position because returning the AV to the last known standing
+                                                             position is likely to be more friendly, isn't it? */
                         RemoveFromPhysicalScene();
-                    }
+                        Velocity = Vector3.Zero;
+                        AbsolutePosition = autopilotTarget + new Vector3(0.0f, 0.0f, (m_sitAvatarHeight / 2.0f));  // Warp av to over sit target
+                    } // else the autopilot will get us close
+                }
+                else
+                {       // its a scripted sit
+                    m_lastWorldPosition = part.AbsolutePosition;  /* CW - This give us a position to return the avatar to if the part is killed before standup.
+                                                                          I *am* using the part's position this time because we have no real idea how far away
+                                                                          the avatar is from the sit target. */
+                    RemoveFromPhysicalScene();
+                    Velocity = Vector3.Zero;
                 }
-
-                cameraAtOffset = part.GetCameraAtOffset();
-                cameraEyeOffset = part.GetCameraEyeOffset();
-                forceMouselook = part.GetForceMouselook();
             }
+            else return;    // physactor is null! 
 
-            ControllingClient.SendSitResponse(targetID, offset, sitOrientation, autopilot, cameraAtOffset, cameraEyeOffset, forceMouselook);
-            m_requestedSitTargetUUID = targetID;
+                        Vector3 offsetr; // = offset * partIRot;        
+                        // KF: In a linkset, offsetr needs to be relative to the group root!  091208    
+        //              offsetr = (part.OffsetPosition * Quaternion.Inverse(part.ParentGroup.RootPart.RotationOffset)) + (offset * partIRot);
+ //           if (part.LinkNum < 2)    091216 All this was necessary because of the GetWorldRotation error.
+  //          {                         // Single, or Root prim of linkset, target is ClickOffset * RootRot
+                //offsetr = offset * partIRot;
+//
+  //          else
+    //        { // Child prim, offset is (ChildOffset * RootRot) + (ClickOffset * ChildRot)
+        //      offsetr = //(part.OffsetPosition * Quaternion.Inverse(part.ParentGroup.RootPart.RotationOffset)) +      
+      //                                (offset * partRot);
+    //        }
+
+//Console.WriteLine(" ");
+//Console.WriteLine("link number ={0}", part.LinkNum);          
+//Console.WriteLine("Prim offset ={0}", part.OffsetPosition );                  
+//Console.WriteLine("Root Rotate ={0}", part.ParentGroup.RootPart.RotationOffset);
+//Console.WriteLine("Click offst ={0}", offset);
+//Console.WriteLine("Prim Rotate ={0}", part.GetWorldRotation());
+//Console.WriteLine("offsetr     ={0}", offsetr);
+//Console.WriteLine("Camera At   ={0}", cameraAtOffset);
+//Console.WriteLine("Camera Eye  ={0}", cameraEyeOffset);
+
+            //NOTE: SendSitResponse should be relative to the GROUP *NOT* THE PRIM if we're sitting on a child
+            ControllingClient.SendSitResponse(part.ParentGroup.UUID, ((offset * part.RotationOffset) + part.OffsetPosition), sitOrientation, autopilot, cameraAtOffset, cameraEyeOffset, forceMouselook);
+            
+            m_requestedSitTargetUUID = part.UUID;               //KF: Correct autopilot target
             // This calls HandleAgentSit twice, once from here, and the client calls
             // HandleAgentSit itself after it gets to the location
             // It doesn't get to the location until we've moved them there though
             // which happens in HandleAgentSit :P
             m_autopilotMoving = autopilot;
-            m_autoPilotTarget = pos;
+            m_autoPilotTarget = autopilotTarget;
             m_sitAtAutoTarget = autopilot;
+            m_initialSitTarget = autopilotTarget;
             if (!autopilot)
                 HandleAgentSit(remoteClient, UUID);
         }
@@ -2119,47 +2371,130 @@ namespace OpenSim.Region.Framework.Scenes
             {
                 if (part != null)
                 {
+//Console.WriteLine("Link #{0}, Rot {1}", part.LinkNum, part.GetWorldRotation());                
                     if (part.GetAvatarOnSitTarget() == UUID)
                     {
+//Console.WriteLine("Scripted Sit");
+                        // Scripted sit
                         Vector3 sitTargetPos = part.SitTargetPosition;
                         Quaternion sitTargetOrient = part.SitTargetOrientation;
-
-                        //Quaternion vq = new Quaternion(sitTargetPos.X, sitTargetPos.Y+0.2f, sitTargetPos.Z+0.2f, 0);
-                        //Quaternion nq = new Quaternion(-sitTargetOrient.X, -sitTargetOrient.Y, -sitTargetOrient.Z, sitTargetOrient.w);
-
-                        //Quaternion result = (sitTargetOrient * vq) * nq;
-
                         m_pos = new Vector3(sitTargetPos.X, sitTargetPos.Y, sitTargetPos.Z);
                         m_pos += SIT_TARGET_ADJUSTMENT;
+                        if (!part.IsRoot)
+                        {
+                            m_pos *= part.RotationOffset;
+                        }
                         m_bodyRot = sitTargetOrient;
-                        //Rotation = sitTargetOrient;
                         m_parentPosition = part.AbsolutePosition;
-
-                        //SendTerseUpdateToAllClients();
+                            part.IsOccupied = true;
+                        part.ParentGroup.AddAvatar(agentID);                                    
                     }
                     else
                     {
-                        m_pos -= part.AbsolutePosition;
+                                                // if m_avUnscriptedSitPos is zero then Av sits above center
+                                                // Else Av sits at m_avUnscriptedSitPos                                         
+                                                
+                        // Non-scripted sit by Kitto Flora 21Nov09
+                        // Calculate angle of line from prim to Av
+                        Quaternion partIRot;
+//                      if (part.LinkNum == 1)
+//                      {                               // Root prim of linkset
+//                              partIRot = Quaternion.Inverse(part.ParentGroup.RootPart.RotationOffset);
+//                      }
+//                      else
+//                      {                               // single or child prim
+                                partIRot = Quaternion.Inverse(part.GetWorldRotation());
+//                      }
+                        Vector3 sitTargetPos= part.AbsolutePosition + m_avUnscriptedSitPos;
+                                float y_diff = (m_avInitialPos.Y - sitTargetPos.Y);
+                        float x_diff = ( m_avInitialPos.X - sitTargetPos.X);
+                        if(Math.Abs(x_diff) < 0.001f) x_diff = 0.001f;                  // avoid div by 0
+                        if(Math.Abs(y_diff) < 0.001f) y_diff = 0.001f;                  // avoid pol flip at 0
+                        float sit_angle = (float)Math.Atan2( (double)y_diff, (double)x_diff);
+                                                // NOTE: when sitting m_ pos and m_bodyRot are *relative* to the prim location/rotation, not 'World'.
+                        //  Av sits at world euler <0,0, z>, translated by part rotation 
+                        m_bodyRot = partIRot * Quaternion.CreateFromEulers(0f, 0f, sit_angle);          // sit at 0,0,inv-click
+                        
                         m_parentPosition = part.AbsolutePosition;
-                    }
+                        part.IsOccupied = true;
+                        part.ParentGroup.AddAvatar(agentID);
+                        m_pos = new Vector3(0f, 0f, 0.05f) +                                    // corrections to get Sit Animation
+                                        (new Vector3(0.0f, 0f, 0.61f) * partIRot) +     // located on center
+                                        (new Vector3(0.34f, 0f, 0.0f) * m_bodyRot) +
+                                        m_avUnscriptedSitPos;                                                   // adds click offset, if any
+                            //Set up raytrace to find top surface of prim
+                                        Vector3 size = part.Scale;
+                                                float mag = 2.0f; // 0.1f + (float)Math.Sqrt((size.X * size.X) + (size.Y * size.Y) + (size.Z * size.Z));
+                                                Vector3 start =  part.AbsolutePosition + new Vector3(0f, 0f, mag);
+                                                Vector3 down = new Vector3(0f, 0f, -1f);    
+//Console.WriteLine("st={0}  do={1}  ma={2}", start, down, mag);                                                
+                                m_scene.PhysicsScene.RaycastWorld(
+                                        start,          // Vector3 position,
+                                        down,           // Vector3 direction,
+                                        mag,            // float length,
+                                        SitAltitudeCallback);           // retMethod
+                        } // end scripted/not
                 }
-                else
+                else  // no Av
                 {
                     return;
                 }
             }
-            m_parentID = m_requestedSitTargetID;
 
+            //We want our offsets to reference the root prim, not the child we may have sat on
+            if (!part.IsRoot)
+            {
+                m_parentID = part.ParentGroup.RootPart.LocalId;
+                m_pos += part.OffsetPosition;
+            }
+            else
+            {
+                m_parentID = m_requestedSitTargetID;
+            }
+
+            m_linkedPrim = part.UUID;
+            if (part.GetAvatarOnSitTarget() != UUID)
+            {
+                m_offsetRotation = m_offsetRotation / part.RotationOffset;
+            }
             Velocity = Vector3.Zero;
             RemoveFromPhysicalScene();
-
             Animator.TrySetMovementAnimation(sitAnimation);
             SendFullUpdateToAllClients();
-            // This may seem stupid, but Our Full updates don't send avatar rotation :P
-            // So we're also sending a terse update (which has avatar rotation)
-            // [Update] We do now.
-            //SendTerseUpdateToAllClients();
+            SendTerseUpdateToAllClients();
         }
+        
+        public void SitAltitudeCallback(bool hitYN, Vector3 collisionPoint, uint localid, float distance, Vector3 normal)
+        {
+                        // KF: 091202 There appears to be a bug in Prim Edit Size - the process sometimes make a prim that RayTrace no longer
+                        //     sees. Take/re-rez, or sim restart corrects the condition. Result of bug is incorrect sit height.
+                        if(hitYN)
+                        {
+                                // m_pos = Av offset from prim center to make look like on center
+                                // m_parentPosition = Actual center pos of prim
+                                // collisionPoint = spot on prim where we want to sit
+                                // collisionPoint.Z = global sit surface height
+                                SceneObjectPart part = m_scene.GetSceneObjectPart(localid);
+                                Quaternion partIRot;
+//                  if (part.LinkNum == 1)
+///             {                               // Root prim of linkset
+//                  partIRot = Quaternion.Inverse(part.ParentGroup.RootPart.RotationOffset);
+//              }
+//              else
+//              {                               // single or child prim
+                    partIRot = Quaternion.Inverse(part.GetWorldRotation());
+//              }          
+                if (m_initialSitTarget != null)
+                {
+                    float offZ = collisionPoint.Z - m_initialSitTarget.Z;
+                    Vector3 offset = new Vector3(0.0f, 0.0f, offZ) * partIRot; // Altitude correction
+                    //Console.WriteLine("sitPoint={0},  offset={1}",  sitPoint,  offset);
+                    m_pos += offset;
+                    //  ControllingClient.SendClearFollowCamProperties(part.UUID);
+                }
+                                
+                        }
+                } // End SitAltitudeCallback  KF.
 
         /// <summary>
         /// Event handler for the 'Always run' setting on the client
@@ -2189,7 +2524,7 @@ namespace OpenSim.Region.Framework.Scenes
         /// </summary>
         /// <param name="vec">The vector in which to move.  This is relative to the rotation argument</param>
         /// <param name="rotation">The direction in which this avatar should now face.
-        public void AddNewMovement(Vector3 vec, Quaternion rotation)
+        public void AddNewMovement(Vector3 vec, Quaternion rotation, bool Nudging)
         {
             if (m_isChildAgent)
             {
@@ -2230,15 +2565,26 @@ namespace OpenSim.Region.Framework.Scenes
             Rotation = rotation;
             Vector3 direc = vec * rotation;
             direc.Normalize();
+            PhysicsActor actor = m_physicsActor;
+
+            if (actor.Flying != m_flyingOld)                // add for fly velocity control
+            {
+                m_flyingOld = actor.Flying;                     // add for fly velocity control
+                if (!actor.Flying) m_wasFlying = true;      // add for fly velocity control
+            }
+
+            if (m_physicsActor.IsColliding == true) m_wasFlying = false;            // add for fly velocity control
+
+            if ((vec.Z == 0f) && !actor.Flying) direc.Z = 0f; // Prevent camera WASD up.
 
             direc *= 0.03f * 128f * m_speedModifier;
 
-            PhysicsActor actor = m_physicsActor;
             if (actor != null)
             {
                 if (actor.Flying)
                 {
-                    direc *= 4.0f;
+// rm speed mod                    direc *= 4.0f;
+                    direc *= 5.2f;      // for speed mod
                     //bool controlland = (((m_AgentControlFlags & (uint)AgentManager.ControlFlags.AGENT_CONTROL_UP_NEG) != 0) || ((m_AgentControlFlags & (uint)AgentManager.ControlFlags.AGENT_CONTROL_NUDGE_UP_NEG) != 0));
                     //bool colliding = (m_physicsActor.IsColliding==true);
                     //if (controlland)
@@ -2251,22 +2597,34 @@ namespace OpenSim.Region.Framework.Scenes
                     //    m_log.Info("[AGENT]: Stop FLying");
                     //}
                 }
+                if (Animator.m_falling && m_wasFlying)    // if falling from flying, disable motion add
+                {
+                    direc *= 0.0f;
+                }
+ /* This jumping section removed to SPA
                 else if (!actor.Flying && actor.IsColliding)
                 {
                     if (direc.Z > 2.0f)
                     {
-                        direc.Z *= 3.0f;
-
-                        // TODO: PreJump and jump happen too quickly.  Many times prejump gets ignored.
-                        Animator.TrySetMovementAnimation("PREJUMP");
-                        Animator.TrySetMovementAnimation("JUMP");
+                        if(m_animator.m_animTickJump == -1)
+                                                {
+                                direc.Z *= 3.0f;        // jump
+                        }
+                        else
+                        {
+                                direc.Z *= 0.1f;        // prejump
+                        }
+                        / * Animations are controlled via  GetMovementAnimation() in ScenePresenceAnimator.cs
+                           Animator.TrySetMovementAnimation("PREJUMP");
+                           Animator.TrySetMovementAnimation("JUMP");
+                        * /
                     }
-                }
+                }  */
             }
 
             // TODO: Add the force instead of only setting it to support multiple forces per frame?
             m_forceToApply = direc;
-
+            m_isNudging = Nudging;
             m_scene.StatsReporter.AddAgentTime(Util.EnvironmentTickCountSubtract(m_perfMonMS));
         }
 
@@ -2281,7 +2639,7 @@ namespace OpenSim.Region.Framework.Scenes
             const float POSITION_TOLERANCE = 0.05f;
             //const int TIME_MS_TOLERANCE = 3000;
 
-            SendPrimUpdates();
+            
 
             if (m_isChildAgent == false)
             {
@@ -2311,6 +2669,9 @@ namespace OpenSim.Region.Framework.Scenes
                     CheckForBorderCrossing();
                 CheckForSignificantMovement(); // sends update to the modules.
             }
+            
+            //Sending prim updates AFTER the avatar terse updates are sent
+            SendPrimUpdates();
         }
 
         #endregion
@@ -3058,6 +3419,7 @@ namespace OpenSim.Region.Framework.Scenes
             m_callbackURI = cAgent.CallbackURI;
 
             m_pos = cAgent.Position;
+            
             m_velocity = cAgent.Velocity;
             m_CameraCenter = cAgent.Center;
             m_CameraAtAxis = cAgent.AtAxis;
@@ -3176,17 +3538,46 @@ namespace OpenSim.Region.Framework.Scenes
         /// </summary>
         public override void UpdateMovement()
         {
-            if (m_forceToApply.HasValue)
-            {
-                Vector3 force = m_forceToApply.Value;
+            if (Animator!=null)             // add for jumping
+            {                               // add for jumping
+                // if (!m_animator.m_jumping)  // add for jumping
+                // {                           // add for jumping
 
-                m_updateflag = true;
-//                movementvector = force;
-                Velocity = force;
+                    if (m_forceToApply.HasValue)    // this section realigned
+                    {
 
-                m_forceToApply = null;
-            }
-        }
+                        Vector3 force = m_forceToApply.Value;
+                        m_updateflag = true;
+if (m_animator.m_jumping) force.Z = m_animator.m_jumpVelocity;     // add for jumping
+                        Velocity = force;
+//Console.WriteLine("UM1 {0}", Velocity);
+                        m_forceToApply = null;
+                    }
+                    else
+                    {
+                        if (m_isNudging)
+                        {
+                            Vector3 force = Vector3.Zero;
+
+                            m_updateflag = true;
+if (m_animator.m_jumping) force.Z = m_animator.m_jumpVelocity;     // add for jumping
+                            Velocity = force;
+//Console.WriteLine("UM2 {0}", Velocity);
+                            m_isNudging = false;
+                                m_updateCount = UPDATE_COUNT;                   //KF: Update anims to pickup "STAND"
+                        }
+                        else      // add for jumping
+                        {  // add for jumping
+                            Vector3 force = Vector3.Zero;     // add for jumping
+if (m_animator.m_jumping) force.Z = m_animator.m_jumpVelocity;     // add for jumping
+//Console.WriteLine("UM3 {0}", Velocity);
+                            Velocity = force;     // add for jumping
+                        }
+
+                    }
+              //  }                                   // end realign
+            }                               // add for jumping
+        }                                   // add for jumping
 
         /// <summary>
         /// Adds a physical representation of the avatar to the Physics plugin
@@ -3200,10 +3591,8 @@ namespace OpenSim.Region.Framework.Scenes
 
             Vector3 pVec = AbsolutePosition;
 
-            // Old bug where the height was in centimeters instead of meters
             m_physicsActor = scene.AddAvatar(Firstname + "." + Lastname, pVec,
                                                  new Vector3(0f, 0f, m_appearance.AvatarHeight), isFlying);
-
             scene.AddPhysicsActorTaint(m_physicsActor);
             //m_physicsActor.OnRequestTerseUpdate += SendTerseUpdateToAllClients;
             m_physicsActor.OnCollisionUpdate += PhysicsCollisionUpdate;
@@ -3227,18 +3616,29 @@ namespace OpenSim.Region.Framework.Scenes
         {
             if (e == null)
                 return;
-
-            //if ((Math.Abs(Velocity.X) > 0.1e-9f) || (Math.Abs(Velocity.Y) > 0.1e-9f))
-            // The Physics Scene will send updates every 500 ms grep: m_physicsActor.SubscribeEvents(
+                
+            // The Physics Scene will send (spam!) updates every 500 ms grep: m_physicsActor.SubscribeEvents(
             // as of this comment the interval is set in AddToPhysicalScene
             if (Animator!=null)
-                Animator.UpdateMovementAnimations();
+            {
+                                if (m_updateCount > 0)                  //KF: DO NOT call UpdateMovementAnimations outside of the m_updateCount wrapper,
+                                {                                                               //  else its will lock out other animation changes, like ground sit.
+                        Animator.UpdateMovementAnimations();
+                        m_updateCount--;
+                                }
+                        }
 
             CollisionEventUpdate collisionData = (CollisionEventUpdate)e;
             Dictionary<uint, ContactPoint> coldata = collisionData.m_objCollisionList;
 
             CollisionPlane = Vector4.UnitW;
 
+                        if (m_lastColCount != coldata.Count)
+                        {       
+                                m_updateCount = UPDATE_COUNT;
+                                m_lastColCount = coldata.Count;
+                        }
+                        
             if (coldata.Count != 0 && Animator != null)
             {
                 switch (Animator.CurrentMovementAnimation)
@@ -3268,6 +3668,148 @@ namespace OpenSim.Region.Framework.Scenes
                 }
             }
 
+            List<uint> thisHitColliders = new List<uint>();
+            List<uint> endedColliders = new List<uint>();
+            List<uint> startedColliders = new List<uint>();
+
+            foreach (uint localid in coldata.Keys)
+            {
+                thisHitColliders.Add(localid);
+                if (!m_lastColliders.Contains(localid))
+                {
+                    startedColliders.Add(localid);
+                }
+                //m_log.Debug("[SCENE PRESENCE]: Collided with:" + localid.ToString() + " at depth of: " + collissionswith[localid].ToString());
+            }
+            
+            // calculate things that ended colliding
+            foreach (uint localID in m_lastColliders)
+            {
+                if (!thisHitColliders.Contains(localID))
+                {
+                    endedColliders.Add(localID);
+                }
+            }
+            //add the items that started colliding this time to the last colliders list.
+            foreach (uint localID in startedColliders)
+            {
+                m_lastColliders.Add(localID);
+            }
+            // remove things that ended colliding from the last colliders list
+            foreach (uint localID in endedColliders)
+            {
+                m_lastColliders.Remove(localID);
+            }
+
+            // do event notification
+            if (startedColliders.Count > 0)
+            {
+                ColliderArgs StartCollidingMessage = new ColliderArgs();
+                List<DetectedObject> colliding = new List<DetectedObject>();
+                foreach (uint localId in startedColliders)
+                {
+                    if (localId == 0)
+                        continue;
+
+                    SceneObjectPart obj = Scene.GetSceneObjectPart(localId);
+                    string data = "";
+                    if (obj != null)
+                    {
+                        DetectedObject detobj = new DetectedObject();
+                        detobj.keyUUID = obj.UUID;
+                        detobj.nameStr = obj.Name;
+                        detobj.ownerUUID = obj.OwnerID;
+                        detobj.posVector = obj.AbsolutePosition;
+                        detobj.rotQuat = obj.GetWorldRotation();
+                        detobj.velVector = obj.Velocity;
+                        detobj.colliderType = 0;
+                        detobj.groupUUID = obj.GroupID;
+                        colliding.Add(detobj);
+                    }
+                }
+
+                if (colliding.Count > 0)
+                {
+                    StartCollidingMessage.Colliders = colliding;
+
+                    foreach (SceneObjectGroup att in Attachments)
+                        Scene.EventManager.TriggerScriptCollidingStart(att.LocalId, StartCollidingMessage);
+                }
+            }
+
+            if (endedColliders.Count > 0)
+            {
+                ColliderArgs EndCollidingMessage = new ColliderArgs();
+                List<DetectedObject> colliding = new List<DetectedObject>();
+                foreach (uint localId in endedColliders)
+                {
+                    if (localId == 0)
+                        continue;
+
+                    SceneObjectPart obj = Scene.GetSceneObjectPart(localId);
+                    string data = "";
+                    if (obj != null)
+                    {
+                        DetectedObject detobj = new DetectedObject();
+                        detobj.keyUUID = obj.UUID;
+                        detobj.nameStr = obj.Name;
+                        detobj.ownerUUID = obj.OwnerID;
+                        detobj.posVector = obj.AbsolutePosition;
+                        detobj.rotQuat = obj.GetWorldRotation();
+                        detobj.velVector = obj.Velocity;
+                        detobj.colliderType = 0;
+                        detobj.groupUUID = obj.GroupID;
+                        colliding.Add(detobj);
+                    }
+                }
+
+                if (colliding.Count > 0)
+                {
+                    EndCollidingMessage.Colliders = colliding;
+
+                    foreach (SceneObjectGroup att in Attachments)
+                        Scene.EventManager.TriggerScriptCollidingEnd(att.LocalId, EndCollidingMessage);
+                }
+            }
+
+            if (thisHitColliders.Count > 0)
+            {
+                ColliderArgs CollidingMessage = new ColliderArgs();
+                List<DetectedObject> colliding = new List<DetectedObject>();
+                foreach (uint localId in thisHitColliders)
+                {
+                    if (localId == 0)
+                        continue;
+
+                    SceneObjectPart obj = Scene.GetSceneObjectPart(localId);
+                    string data = "";
+                    if (obj != null)
+                    {
+                        DetectedObject detobj = new DetectedObject();
+                        detobj.keyUUID = obj.UUID;
+                        detobj.nameStr = obj.Name;
+                        detobj.ownerUUID = obj.OwnerID;
+                        detobj.posVector = obj.AbsolutePosition;
+                        detobj.rotQuat = obj.GetWorldRotation();
+                        detobj.velVector = obj.Velocity;
+                        detobj.colliderType = 0;
+                        detobj.groupUUID = obj.GroupID;
+                        colliding.Add(detobj);
+                    }
+                }
+
+                if (colliding.Count > 0)
+                {
+                    CollidingMessage.Colliders = colliding;
+
+                    lock (m_attachments)
+                    {
+                        foreach (SceneObjectGroup att in m_attachments)
+                            Scene.EventManager.TriggerScriptColliding(att.LocalId, CollidingMessage);
+                    }
+                }
+            }
+
             if (m_invulnerable)
                 return;
             
@@ -3693,6 +4235,39 @@ namespace OpenSim.Region.Framework.Scenes
                 return;
             }
 
+            XmlDocument doc = new XmlDocument();
+            string stateData = String.Empty;
+
+            IAttachmentsService attServ = m_scene.RequestModuleInterface<IAttachmentsService>();
+            if (attServ != null)
+            {
+                m_log.DebugFormat("[ATTACHMENT]: Loading attachment data from attachment service");
+                stateData = attServ.Get(ControllingClient.AgentId.ToString());
+                if (stateData != String.Empty)
+                {
+                    try
+                    {
+                        doc.LoadXml(stateData);
+                    }
+                    catch { }
+                }
+            }
+
+            Dictionary<UUID, string> itemData = new Dictionary<UUID, string>();
+
+            XmlNodeList nodes = doc.GetElementsByTagName("Attachment");
+            if (nodes.Count > 0)
+            {
+                foreach (XmlNode n in nodes)
+                {
+                    XmlElement elem = (XmlElement)n;
+                    string itemID = elem.GetAttribute("ItemID");
+                    string xml = elem.InnerXml;
+
+                    itemData[new UUID(itemID)] = xml;
+                }
+            }
+
             List<AvatarAttachment> attachments = m_appearance.GetAttachments();
             foreach (AvatarAttachment attach in attachments)
             {
@@ -3713,7 +4288,30 @@ namespace OpenSim.Region.Framework.Scenes
 
                 try
                 {
-                    m_scene.AttachmentsModule.RezSingleAttachmentFromInventory(ControllingClient, itemID, (uint)p);
+                    string xmlData;
+                    XmlDocument d = new XmlDocument();
+                    UUID asset;
+                    if (itemData.TryGetValue(itemID, out xmlData))
+                    {
+                        d.LoadXml(xmlData);
+                        m_log.InfoFormat("[ATTACHMENT]: Found saved state for item {0}, loading it", itemID);
+
+                        // Rez from inventory
+                        asset 
+                            = m_scene.AttachmentsModule.RezSingleAttachmentFromInventory(ControllingClient, itemID, (uint)p, true, d);
+
+                    }
+                    else
+                    {
+                        // Rez from inventory (with a null doc to let
+                        // CHANGED_OWNER happen)
+                        asset 
+                            = m_scene.AttachmentsModule.RezSingleAttachmentFromInventory(ControllingClient, itemID, (uint)p, true, null);
+                    }
+
+                    m_log.InfoFormat(
+                        "[ATTACHMENT]: Rezzed attachment in point {0} from item {1} and asset {2}",
+                        p, itemID, asset);
                 }
                 catch (Exception e)
                 {
@@ -3746,6 +4344,15 @@ namespace OpenSim.Region.Framework.Scenes
                 m_reprioritization_called = false;
             }
         }
+        
+        private Vector3 Quat2Euler(Quaternion rot){
+                float x = Utils.RAD_TO_DEG * (float)Math.Atan2((double)((2.0f * rot.X * rot.W) - (2.0f * rot.Y * rot.Z)) , 
+                                                                                                                (double)(1 - (2.0f * rot.X * rot.X) - (2.0f * rot.Z * rot.Z)));
+                float y = Utils.RAD_TO_DEG * (float)Math.Asin ((double)((2.0f * rot.X * rot.Y) + (2.0f * rot.Z * rot.W)));
+                float z = Utils.RAD_TO_DEG * (float)Math.Atan2(((double)(2.0f * rot.Y * rot.W) - (2.0f * rot.X * rot.Z)) , 
+                                                                                                                (double)(1 - (2.0f * rot.Y * rot.Y) - (2.0f * rot.Z * rot.Z)));
+                return(new Vector3(x,y,z));
+            }
 
         public void SaveChangedAttachments()
         {