1 files changed, 727 insertions, 180 deletions

diff --git a/OpenSim/Region/Framework/Scenes/ScenePresence.cs b/OpenSim/Region/Framework/Scenes/ScenePresence.cs
index f6df3c3..9bf5bea 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("[SCENE PRESENCE] Destructor called");
 //        }
-
+        
         private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
 
         public PresenceType PresenceType { get; private set; }
@@ -89,7 +90,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);
 
         /// <summary>
         /// Movement updates for agents in neighboring regions are sent directly to clients.
@@ -117,6 +120,7 @@ namespace OpenSim.Region.Framework.Scenes
         /// TODO: For some reason, we effectively have a list both here and in Appearance.  Need to work out if this is
         /// necessary.
         /// </remarks>
+
         protected List<SceneObjectGroup> m_attachments = new List<SceneObjectGroup>();
 
         public Object AttachmentsSyncLock { get; private set; }
@@ -130,13 +134,21 @@ 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;
 
         private Vector3? m_forceToApply;
+        private int m_userFlags;
+        public int UserFlags
+        {
+            get { return m_userFlags; }
+        }
         private TeleportFlags m_teleportFlags;
         public TeleportFlags TeleportFlags
         {
@@ -159,6 +171,9 @@ namespace OpenSim.Region.Framework.Scenes
         private Vector3 m_lastChildAgentUpdatePosition;
         private Vector3 m_lastChildAgentUpdateCamPosition;
 
+        private bool m_flyingOld;               // add for fly velocity control
+        public bool m_wasFlying;                // add for fly velocity control
+
         private const int LAND_VELOCITYMAG_MAX = 12;
 
         public bool IsRestrictedToRegion;
@@ -169,7 +184,7 @@ namespace OpenSim.Region.Framework.Scenes
 
         protected ulong crossingFromRegion;
 
-        private readonly Vector3[] Dir_Vectors = new Vector3[9];
+        private readonly Vector3[] Dir_Vectors = new Vector3[11];
 
 
         protected Timer m_reprioritization_timer;
@@ -184,12 +199,14 @@ 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************
         public bool MovingToTarget { get; private set; }
         public Vector3 MoveToPositionTarget { get; private set; }
+        private Quaternion m_offsetRotation = new Quaternion(0.0f, 0.0f, 0.0f, 1.0f);
 
         /// <summary>
         /// Controls whether an avatar automatically moving to a target will land when it gets there (if flying).
@@ -199,7 +216,13 @@ namespace OpenSim.Region.Framework.Scenes
         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;
@@ -240,7 +263,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
         }
         
@@ -476,9 +501,11 @@ namespace OpenSim.Region.Framework.Scenes
         {
             get
             {
-                if (PhysicsActor != null)
-                {
-                    m_pos = PhysicsActor.Position;
+                PhysicsActor actor = m_physicsActor;
+//                if (actor != null)
+                if ((actor != null) && (m_parentID == 0))   // KF Do NOT update m_pos here if Av is sitting!
+                                {
+                    m_pos = actor.Position;
 
 //                    m_log.DebugFormat(
 //                        "[SCENE PRESENCE]: Set position {0} for {1} in {2} via getting AbsolutePosition!",
@@ -504,7 +531,7 @@ namespace OpenSim.Region.Framework.Scenes
                         SceneObjectPart part = m_scene.GetSceneObjectPart(ParentID);
                         if (part != null)
                         {
-                            return ParentPosition + (m_pos * part.GetWorldRotation());
+                            return part.AbsolutePosition + (m_pos * part.GetWorldRotation());
                         }
                         else
                         {
@@ -529,8 +556,10 @@ namespace OpenSim.Region.Framework.Scenes
                     }
                 }
 
-                m_pos = value;
-                ParentPosition = Vector3.Zero;
+//                              Changed this to update unconditionally to make npose work
+//                if (m_parentID == 0)   // KF Do NOT update m_pos here if Av is sitting!
+                    m_pos = value;
+                m_parentPosition = Vector3.Zero;
 
 //                m_log.DebugFormat(
 //                    "[ENTITY BASE]: In {0} set AbsolutePosition of {1} to {2}",
@@ -587,15 +616,39 @@ namespace OpenSim.Region.Framework.Scenes
             }
         }
 
+        public Quaternion OffsetRotation
+        {
+            get { return m_offsetRotation; }
+            set { m_offsetRotation = value; }
+        }
         private Quaternion m_bodyRot = Quaternion.Identity;
 
         public Quaternion Rotation
         {
-            get { return m_bodyRot; }
-            set
-            {
+            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;
-//                m_log.DebugFormat("[SCENE PRESENCE]: Body rot for {0} set to {1}", Name, m_bodyRot);
+                if (m_parentID != 0)
+                {
+                    m_offsetRotation = new Quaternion(0.0f, 0.0f, 0.0f, 1.0f);
+                }
             }
         }
 
@@ -613,12 +666,15 @@ namespace OpenSim.Region.Framework.Scenes
             set { m_isChildAgent = value; }
         }
 
+        private uint m_parentID;
+
+
         public uint ParentID
         {
             get { return m_parentID; }
             set { m_parentID = value; }
         }
-        private uint m_parentID;
+
 
         public float Health
         {
@@ -751,6 +807,7 @@ namespace OpenSim.Region.Framework.Scenes
             m_localId = m_scene.AllocateLocalId();
 
             UserAccount account = m_scene.UserAccountService.GetUserAccount(m_scene.RegionInfo.ScopeID, m_uuid);
+            m_userFlags = account.UserFlags;
 
             if (account != null)
                 UserLevel = account.UserLevel;
@@ -769,10 +826,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)
@@ -813,26 +867,30 @@ 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];
-            vector[0] = new Vector3(m_CameraUpAxis.Z, 0f, -CameraAtAxis.Z); //FORWARD
-            vector[1] = new Vector3(-m_CameraUpAxis.Z, 0f, CameraAtAxis.Z); //BACK
+            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(CameraAtAxis.Z, 0f, m_CameraUpAxis.Z); //UP
-            vector[5] = new Vector3(-CameraAtAxis.Z, 0f, -m_CameraUpAxis.Z); //DOWN
-            vector[8] = new Vector3(-CameraAtAxis.Z, 0f, -m_CameraUpAxis.Z); //DOWN_Nudge
-            vector[6] = (new Vector3(m_CameraUpAxis.Z, 0f, -CameraAtAxis.Z) * 2); //FORWARD Nudge
-            vector[7] = new Vector3(-m_CameraUpAxis.Z, 0f, CameraAtAxis.Z); //BACK Nudge
+            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[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;
         }
-
+        
         #endregion
 
         public uint GenerateClientFlags(UUID ObjectID)
@@ -847,6 +905,8 @@ namespace OpenSim.Region.Framework.Scenes
         /// </summary>
         public void SendPrimUpdates()
         {
+            m_sceneViewer.SendPrimUpdates();
+
             SceneViewer.SendPrimUpdates();
         }
 
@@ -892,6 +952,62 @@ namespace OpenSim.Region.Framework.Scenes
                 pos.Y = crossedBorder.BorderLine.Z - 1;
             }
 
+            ILandObject land = m_scene.LandChannel.GetLandObject(pos.X, pos.Y);
+            if (land != null)
+            {
+                // If we come in via login, landmark or map, we want to
+                // honor landing points. If we come in via Lure, we want
+                // to ignore them.
+                if ((m_teleportFlags & (TeleportFlags.ViaLogin | TeleportFlags.ViaRegionID)) == (TeleportFlags.ViaLogin | TeleportFlags.ViaRegionID) ||
+                    (m_teleportFlags & TeleportFlags.ViaLandmark) != 0 ||
+                    (m_teleportFlags & TeleportFlags.ViaLocation) != 0)
+                {
+                    // Don't restrict gods, estate managers, or land owners to
+                    // the TP point. This behaviour mimics agni.
+                    if (land.LandData.LandingType == (byte)LandingType.LandingPoint &&
+                        land.LandData.UserLocation != Vector3.Zero &&
+                        GodLevel < 200 &&
+                        ((land.LandData.OwnerID != m_uuid &&
+                        (!m_scene.Permissions.IsGod(m_uuid)) &&
+                        (!m_scene.RegionInfo.EstateSettings.IsEstateManager(m_uuid))) || (m_teleportFlags & TeleportFlags.ViaLocation) != 0))
+                    {
+                        pos = land.LandData.UserLocation;
+                    }
+                }
+                
+                land.SendLandUpdateToClient(ControllingClient);
+            }
+
+            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(
@@ -1024,16 +1140,21 @@ namespace OpenSim.Region.Framework.Scenes
         /// <summary>
         /// Removes physics plugin scene representation of this agent if it exists.
         /// </summary>
-        private void RemoveFromPhysicalScene()
+        public void RemoveFromPhysicalScene()
         {
             if (PhysicsActor != null)
             {
-                PhysicsActor.OnRequestTerseUpdate -= SendTerseUpdateToAllClients;
-                PhysicsActor.OnOutOfBounds -= OutOfBoundsCall;
-                m_scene.PhysicsScene.RemoveAvatar(PhysicsActor);
-                PhysicsActor.UnSubscribeEvents();
-                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
+                { }
             }
         }
 
@@ -1049,10 +1170,12 @@ namespace OpenSim.Region.Framework.Scenes
             
             RemoveFromPhysicalScene();
             Velocity = Vector3.Zero;
+            CheckLandingPoint(ref pos);
             AbsolutePosition = pos;
             AddToPhysicalScene(isFlying);
 
             SendTerseUpdateToAllClients();
+            
         }
 
         public void TeleportWithMomentum(Vector3 pos)
@@ -1062,6 +1185,7 @@ namespace OpenSim.Region.Framework.Scenes
                 isFlying = PhysicsActor.Flying;
 
             RemoveFromPhysicalScene();
+            CheckLandingPoint(ref pos);
             AbsolutePosition = pos;
             AddToPhysicalScene(isFlying);
 
@@ -1269,11 +1393,12 @@ namespace OpenSim.Region.Framework.Scenes
         /// <summary>
         /// This is the event handler for client movement. If a client is moving, this event is triggering.
         /// </summary>
+        /// <summary>
+        /// This is the event handler for client movement. If a client is moving, this event is triggering.
+        /// </summary>
         public void HandleAgentUpdate(IClientAPI remoteClient, AgentUpdateArgs agentData)
         {
-//            m_log.DebugFormat(
-//                "[SCENE PRESENCE]: In {0} received agent update from {1}",
-//                Scene.RegionInfo.RegionName, remoteClient.Name);
+            //            m_log.DebugFormat("[SCENE PRESENCE]: Received agent update from {0}", remoteClient.Name);
 
             //if (IsChildAgent)
             //{
@@ -1348,6 +1473,10 @@ namespace OpenSim.Region.Framework.Scenes
 
             #endregion Inputs
 
+            // Make anims work for client side autopilot
+            if ((flags & AgentManager.ControlFlags.AGENT_CONTROL_AT_POS) != 0)
+                m_updateCount = UPDATE_COUNT;
+
             if ((flags & AgentManager.ControlFlags.AGENT_CONTROL_STAND_UP) != 0)
             {
                 StandUp();
@@ -1378,6 +1507,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.
@@ -1467,7 +1599,7 @@ namespace OpenSim.Region.Framework.Scenes
 
                             if ((MovementFlag & (byte)(uint)DCF) == 0)
                             {
-                                if (DCF == Dir_ControlFlags.DIR_CONTROL_FLAG_FORWARD_NUDGE || DCF == Dir_ControlFlags.DIR_CONTROL_FLAG_BACKWARD_NUDGE)
+                                if (DCF == Dir_ControlFlags.DIR_CONTROL_FLAG_FORWARD_NUDGE || DCF == Dir_ControlFlags.DIR_CONTROL_FLAG_BACK_NUDGE)
                                 {
                                     MovementFlag |= (byte)nudgehack;
                                 }
@@ -1479,13 +1611,13 @@ namespace OpenSim.Region.Framework.Scenes
                         }
                         else
                         {
-                            if ((MovementFlag & (byte)(uint)DCF) != 0 ||
-                                ((DCF == Dir_ControlFlags.DIR_CONTROL_FLAG_FORWARD_NUDGE || DCF == Dir_ControlFlags.DIR_CONTROL_FLAG_BACKWARD_NUDGE)
-                                && ((MovementFlag & (byte)nudgehack) == nudgehack))
+                            if ((m_movementflag & (byte)(uint)DCF) != 0 ||
+                                ((DCF == Dir_ControlFlags.DIR_CONTROL_FLAG_FORWARD_NUDGE || DCF == Dir_ControlFlags.DIR_CONTROL_FLAG_BACK_NUDGE)
+                                && ((m_movementflag & (byte)nudgehack) == nudgehack))
                                 ) // This or is for Nudge forward
                             {
-//                                m_log.DebugFormat("[SCENE PRESENCE]: Updating MovementFlag for {0} with lack of {1}", Name, DCF);
-                                MovementFlag -= ((byte)(uint)DCF);
+                                //                                m_log.DebugFormat("[SCENE PRESENCE]: Updating m_movementflag for {0} with lack of {1}", Name, DCF);
+                                m_movementflag -= ((byte)(uint)DCF);
                                 update_movementflag = true;
 
                                 /*
@@ -1546,21 +1678,21 @@ namespace OpenSim.Region.Framework.Scenes
                 // which occurs later in the main scene loop
                 if (update_movementflag || (update_rotation && DCFlagKeyPressed))
                 {
-//                    m_log.DebugFormat(
-//                        "[SCENE PRESENCE]: In {0} adding velocity of {1} to {2}, umf = {3}, ur = {4}",
-//                        m_scene.RegionInfo.RegionName, agent_control_v3, Name, update_movementflag, update_rotation);
+                    //                    m_log.DebugFormat(
+                    //                        "[SCENE PRESENCE]: In {0} adding velocity of {1} to {2}, umf = {3}, ur = {4}",
+                    //                        m_scene.RegionInfo.RegionName, agent_control_v3, Name, update_movementflag, update_rotation);
 
                     AddNewMovement(agent_control_v3);
                 }
-//                else
-//                {
-//                    if (!update_movementflag)
-//                    {
-//                        m_log.DebugFormat(
-//                            "[SCENE PRESENCE]: In {0} ignoring requested update of {1} for {2} as update_movementflag = false",
-//                            m_scene.RegionInfo.RegionName, agent_control_v3, Name);
-//                    }
-//                }
+                //                else
+                //                {
+                //                    if (!update_movementflag)
+                //                    {
+                //                        m_log.DebugFormat(
+                //                            "[SCENE PRESENCE]: In {0} ignoring requested update of {1} for {2} as update_movementflag = false",
+                //                            m_scene.RegionInfo.RegionName, agent_control_v3, Name);
+                //                    }
+                //                }
 
                 if (update_movementflag && ParentID == 0)
                     Animator.UpdateMovementAnimations();
@@ -1579,20 +1711,20 @@ namespace OpenSim.Region.Framework.Scenes
         /// <returns>True if movement has been updated in some way.  False otherwise.</returns>
         public bool HandleMoveToTargetUpdate(ref Vector3 agent_control_v3)
         {
-//            m_log.DebugFormat("[SCENE PRESENCE]: Called HandleMoveToTargetUpdate() for {0}", Name);
+            //            m_log.DebugFormat("[SCENE PRESENCE]: Called HandleMoveToTargetUpdate() for {0}", Name);
 
             bool updated = false;
 
-//            m_log.DebugFormat(
-//                "[SCENE PRESENCE]: bAllowUpdateMoveToPosition {0}, m_moveToPositionInProgress {1}, m_autopilotMoving {2}",
-//                allowUpdate, m_moveToPositionInProgress, m_autopilotMoving);
+            //            m_log.DebugFormat(
+            //                "[SCENE PRESENCE]: bAllowUpdateMoveToPosition {0}, m_moveToPositionInProgress {1}, m_autopilotMoving {2}",
+            //                allowUpdate, m_moveToPositionInProgress, m_autopilotMoving);
 
             if (!m_autopilotMoving)
             {
                 double distanceToTarget = Util.GetDistanceTo(AbsolutePosition, MoveToPositionTarget);
-//                        m_log.DebugFormat(
-//                            "[SCENE PRESENCE]: Abs pos of {0} is {1}, target {2}, distance {3}",
-//                            Name, AbsolutePosition, MoveToPositionTarget, distanceToTarget);
+                //                        m_log.DebugFormat(
+                //                            "[SCENE PRESENCE]: Abs pos of {0} is {1}, target {2}, distance {3}",
+                //                            Name, AbsolutePosition, MoveToPositionTarget, distanceToTarget);
 
                 // Check the error term of the current position in relation to the target position
                 if (distanceToTarget <= 1)
@@ -1615,7 +1747,7 @@ namespace OpenSim.Region.Framework.Scenes
                             (MoveToPositionTarget - AbsolutePosition) // vector from cur. pos to target in global coords
                             * Matrix4.CreateFromQuaternion(Quaternion.Inverse(Rotation)); // change to avatar coords
                         // Ignore z component of vector
-//                        Vector3 LocalVectorToTarget2D = new Vector3((float)(LocalVectorToTarget3D.X), (float)(LocalVectorToTarget3D.Y), 0f);
+                        //                        Vector3 LocalVectorToTarget2D = new Vector3((float)(LocalVectorToTarget3D.X), (float)(LocalVectorToTarget3D.Y), 0f);
                         LocalVectorToTarget3D.Normalize();
 
                         // update avatar movement flags. the avatar coordinate system is as follows:
@@ -1681,9 +1813,9 @@ namespace OpenSim.Region.Framework.Scenes
                             updated = true;
                         }
 
-//                        m_log.DebugFormat(
-//                            "[SCENE PRESENCE]: HandleMoveToTargetUpdate adding {0} to move vector {1} for {2}",
-//                            LocalVectorToTarget3D, agent_control_v3, Name);
+                        //                        m_log.DebugFormat(
+                        //                            "[SCENE PRESENCE]: HandleMoveToTargetUpdate adding {0} to move vector {1} for {2}",
+                        //                            LocalVectorToTarget3D, agent_control_v3, Name);
 
                         agent_control_v3 += LocalVectorToTarget3D;
                     }
@@ -1712,9 +1844,12 @@ namespace OpenSim.Region.Framework.Scenes
         /// </param>
         public void MoveToTarget(Vector3 pos, bool noFly, bool landAtTarget)
         {
-            m_log.DebugFormat(
-                "[SCENE PRESENCE]: Avatar {0} received request to move to position {1} in {2}",
-                Name, pos, m_scene.RegionInfo.RegionName);
+            if (SitGround)
+                StandUp();
+
+//            m_log.DebugFormat(
+//                "[SCENE PRESENCE]: Avatar {0} received request to move to position {1} in {2}",
+//                Name, pos, m_scene.RegionInfo.RegionName);
 
             if (pos.X < 0 || pos.X >= Constants.RegionSize
                 || pos.Y < 0 || pos.Y >= Constants.RegionSize
@@ -1740,9 +1875,9 @@ namespace OpenSim.Region.Framework.Scenes
             if (pos.Z - terrainHeight < 0.2)
                 pos.Z = terrainHeight;
 
-            m_log.DebugFormat(
-                "[SCENE PRESENCE]: Avatar {0} set move to target {1} (terrain height {2}) in {3}",
-                Name, pos, terrainHeight, m_scene.RegionInfo.RegionName);
+//            m_log.DebugFormat(
+//                "[SCENE PRESENCE]: Avatar {0} set move to target {1} (terrain height {2}) in {3}",
+//                Name, pos, terrainHeight, m_scene.RegionInfo.RegionName);
 
             if (noFly)
                 PhysicsActor.Flying = false;
@@ -1778,7 +1913,7 @@ namespace OpenSim.Region.Framework.Scenes
         /// </summary>
         public void ResetMoveToTarget()
         {
-            m_log.DebugFormat("[SCENE PRESENCE]: Resetting move to target for {0}", Name);
+//            m_log.DebugFormat("[SCENE PRESENCE]: Resetting move to target for {0}", Name);
 
             MovingToTarget = false;
             MoveToPositionTarget = Vector3.Zero;
@@ -1804,7 +1939,7 @@ namespace OpenSim.Region.Framework.Scenes
                         Velocity = Vector3.Zero;
                         SendAvatarDataToAllAgents();
 
-                        //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;
@@ -1841,24 +1976,22 @@ namespace OpenSim.Region.Framework.Scenes
 
             if (ParentID != 0)
             {
-                m_log.Debug("StandupCode Executed");
-                SceneObjectPart part = m_scene.GetSceneObjectPart(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.SitTargetAvatar == UUID)
@@ -1869,21 +2002,59 @@ namespace OpenSim.Region.Framework.Scenes
                     ParentPosition = part.GetWorldPosition();
                     ControllingClient.SendClearFollowCamProperties(part.ParentUUID);
                 }
-
-                if (PhysicsActor == null)
+                                // 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.
                 {
-                    AddToPhysicalScene(false);
+                    partRot = new Quaternion(0.0f, 0.0f, 0.0f, 1.0f);
+                }
+                else
+                {
+                    partRot = part.GetWorldRotation();
                 }
 
-                m_pos += ParentPosition + new Vector3(0.0f, 0.0f, 2.0f*m_sitAvatarHeight);
-                ParentPosition = Vector3.Zero;
+                    Quaternion partIRot = Quaternion.Inverse(partRot);
 
-                ParentID = 0;
+                                Quaternion avatarRot = Quaternion.Inverse(Quaternion.Inverse(Rotation) * partIRot); // world or. of the av
+                                Vector3 avStandUp = new Vector3(0.3f, 0f, 0f) * avatarRot;              // 0.3M infront of av
+
+                
+                if (m_physicsActor == null)
+                {
+                    AddToPhysicalScene(false);
+                }
+                //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_offsetRotation = new Quaternion(0.0f, 0.0f, 0.0f, 1.0f);
                 SendAvatarDataToAllAgents();
                 m_requestedSitTargetID = 0;
             }
 
-            Animator.TrySetMovementAnimation("STAND");
+            Animator.UpdateMovementAnimations();
         }
 
         private SceneObjectPart FindNextAvailableSitTarget(UUID targetID)
@@ -1915,9 +2086,7 @@ namespace OpenSim.Region.Framework.Scenes
                 UUID avOnTargetAlready = part.SitTargetAvatar;
 
                 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));
+                    bool SitTargetisSet = (Vector3.Zero != avSitOffSet);                //NB Latest SL Spec shows Sit Rotation setting is ignored.
 
                 if (SitTargetisSet && SitTargetUnOccupied)
                 {
@@ -1933,87 +2102,168 @@ 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)
+            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
             {
-                // 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.SitTargetAvatar;
-
-                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
-                       )
-                       ));
-
-//                m_log.DebugFormat("[SCENE PRESENCE]: {0} {1}", SitTargetisSet, SitTargetUnOccupied);
-
-                if (SitTargetisSet && SitTargetUnOccupied)
-                {
-                    part.SitTargetAvatar = UUID;
-                    offset = new Vector3(avSitOffSet.X, avSitOffSet.Y, avSitOffSet.Z);
-                    sitOrientation = avSitOrientation;
-                    autopilot = false;
-                }
-                part.ParentGroup.TriggerScriptChangedEvent(Changed.LINK);
-
-                pos = part.AbsolutePosition + offset;
-                //if (Math.Abs(part.AbsolutePosition.Z - AbsolutePosition.Z) > 1)
-                //{
-                   // offset = pos;
-                    //autopilot = false;
-                //}
-                if (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 = PhysicsActor.Size.Z;
-
-                    if (autopilot)
+                                if (!part.IsOccupied)
+                                {
+//Console.WriteLine("Scripted, unoccupied");
+                    part.SitTargetAvatar = 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.1f) || (Math.Abs(offset.Y) > 0.1f) ) // Changed 0.5M to 0.1M as they want to be able to sit close together 
+                {
+                        // 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
+
+            part.ParentGroup.TriggerScriptChangedEvent(Changed.LINK);
+
+            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();
             }
-
-            ControllingClient.SendSitResponse(targetID, offset, sitOrientation, autopilot, cameraAtOffset, cameraEyeOffset, forceMouselook);
-            m_requestedSitTargetUUID = targetID;
+            else return;    // physactor is null! 
+
+                        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);
         }
@@ -2279,45 +2529,127 @@ namespace OpenSim.Region.Framework.Scenes
                 {
                     if (part.SitTargetAvatar == 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;
-                        Rotation = sitTargetOrient;
-                        //Rotation = sitTargetOrient;
-                        ParentPosition = part.AbsolutePosition;
-
-                        //SendTerseUpdateToAllClients();
+                        if (!part.IsRoot)
+                        {
+                            m_pos *= part.RotationOffset;
+                        }
+                        m_bodyRot = sitTargetOrient;
+                        m_parentPosition = part.AbsolutePosition;
+                            part.IsOccupied = true;
+                        part.ParentGroup.AddAvatar(agentID);                                    
                     }
                     else
                     {
-                        m_pos -= part.AbsolutePosition;
-                        ParentPosition = 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;
                 }
             }
             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;
+            }
+
+            if (part.SitTargetAvatar != UUID)
+            {
+                m_offsetRotation = m_offsetRotation / part.RotationOffset;
+            }
             Velocity = Vector3.Zero;
             RemoveFromPhysicalScene();
-
             Animator.TrySetMovementAnimation(sitAnimation);
             SendAvatarDataToAllAgents();
-            // 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();
         }
+        
+        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
@@ -2347,6 +2679,19 @@ namespace OpenSim.Region.Framework.Scenes
             Vector3 direc = vec * Rotation;
             direc.Normalize();
 
+            if (PhysicsActor.Flying != m_flyingOld)                // add for fly velocity control
+            {
+                m_flyingOld = PhysicsActor.Flying;                 // add for fly velocity control
+                if (!PhysicsActor.Flying)
+                    m_wasFlying = true;      // add for fly velocity control
+            }
+
+            if (PhysicsActor.IsColliding == true)
+                m_wasFlying = false;        // add for fly velocity control
+
+            if ((vec.Z == 0f) && !PhysicsActor.Flying)
+                direc.Z = 0f; // Prevent camera WASD up.
+
             direc *= 0.03f * 128f * SpeedModifier;
 
             if (PhysicsActor != null)
@@ -2365,6 +2710,10 @@ 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;
+                }
                 else if (!PhysicsActor.Flying && PhysicsActor.IsColliding)
                 {
                     if (direc.Z > 2.0f)
@@ -2425,6 +2774,9 @@ namespace OpenSim.Region.Framework.Scenes
 
                 CheckForSignificantMovement(); // sends update to the modules.
             }
+            
+            //Sending prim updates AFTER the avatar terse updates are sent
+            SendPrimUpdates();
         }
 
         #endregion
@@ -3145,6 +3497,7 @@ namespace OpenSim.Region.Framework.Scenes
             m_callbackURI = cAgent.CallbackURI;
 
             m_pos = cAgent.Position;
+            
             m_velocity = cAgent.Velocity;
             CameraPosition = cAgent.Center;
             CameraAtAxis = cAgent.AtAxis;
@@ -3291,14 +3644,28 @@ namespace OpenSim.Region.Framework.Scenes
             //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: PhysicsActor.SubscribeEvents(
             // as of this comment the interval is set in AddToPhysicalScene
-            if (Animator != null)
-                Animator.UpdateMovementAnimations();
+            if (Animator!=null)
+            {
+                                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;
 
+            // No collisions at all means we may be flying. Update always
+            // to make falling work
+                        if (m_lastColCount != coldata.Count || coldata.Count == 0)
+                        {       
+                                m_updateCount = UPDATE_COUNT;
+                                m_lastColCount = coldata.Count;
+                        }
+                        
             if (coldata.Count != 0 && Animator != null)
             {
                 switch (Animator.CurrentMovementAnimation)
@@ -3328,6 +3695,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 GetAttachments())
+                        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 GetAttachments())
+                        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 (Invulnerable)
                 return;
             
@@ -3399,6 +3908,10 @@ namespace OpenSim.Region.Framework.Scenes
         {
             lock (m_attachments)
             {
+                // This may be true when the attachment comes back
+                // from serialization after login. Clear it.
+                gobj.IsDeleted = false;
+
                 m_attachments.Add(gobj);
             }
         }
@@ -3762,5 +4275,39 @@ 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));
+            }
+
+        private void CheckLandingPoint(ref Vector3 pos)
+        {
+            // Never constrain lures
+            if ((TeleportFlags & TeleportFlags.ViaLure) != 0)
+                return;
+
+            if (m_scene.RegionInfo.EstateSettings.AllowDirectTeleport)
+                return;
+
+            ILandObject land = m_scene.LandChannel.GetLandObject(pos.X, pos.Y);
+
+            if (land.LandData.LandingType == (byte)LandingType.LandingPoint &&
+                land.LandData.UserLocation != Vector3.Zero &&
+                land.LandData.OwnerID != m_uuid &&
+                (!m_scene.Permissions.IsGod(m_uuid)) &&
+                (!m_scene.RegionInfo.EstateSettings.IsEstateManager(m_uuid)))
+            {
+                float curr = Vector3.Distance(AbsolutePosition, pos);
+                if (Vector3.Distance(land.LandData.UserLocation, pos) < curr)
+                    pos = land.LandData.UserLocation;
+                else
+                    ControllingClient.SendAlertMessage("Can't teleport closer to destination");
+            }
+        }
     }
 }