1 files changed, 379 insertions, 135 deletions

diff --git a/OpenSim/Region/Framework/Scenes/ScenePresence.cs b/OpenSim/Region/Framework/Scenes/ScenePresence.cs
index 289ba47..a3ad7ca 100644
--- a/OpenSim/Region/Framework/Scenes/ScenePresence.cs
+++ b/OpenSim/Region/Framework/Scenes/ScenePresence.cs
@@ -73,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[] BAKE_INDICES = new byte[] { 8, 9, 10, 11, 19, 20 };
@@ -89,7 +89,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;
 
@@ -113,8 +115,11 @@ namespace OpenSim.Region.Framework.Scenes
         public Vector3 lastKnownAllowedPosition;
         public bool sentMessageAboutRestrictedParcelFlyingDown;
         public Vector4 CollisionPlane = Vector4.UnitW;
-
-        private Vector3 m_lastPosition;
+        
+                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;
@@ -145,7 +150,6 @@ namespace OpenSim.Region.Framework.Scenes
         private int m_perfMonMS;
 
         private bool m_setAlwaysRun;
-        
         private bool m_forceFly;
         private bool m_flyDisabled;
 
@@ -169,7 +173,8 @@ namespace OpenSim.Region.Framework.Scenes
         protected RegionInfo m_regionInfo;
         protected ulong crossingFromRegion;
 
-        private readonly Vector3[] Dir_Vectors = new Vector3[6];
+        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;
@@ -194,6 +199,7 @@ namespace OpenSim.Region.Framework.Scenes
         private bool m_autopilotMoving;
         private Vector3 m_autoPilotTarget;
         private bool m_sitAtAutoTarget;
+        private Vector3 m_initialSitTarget;             //KF: First estimate of where to sit
 
         private string m_nextSitAnimation = String.Empty;
 
@@ -204,6 +210,9 @@ 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;
 
@@ -233,6 +242,10 @@ namespace OpenSim.Region.Framework.Scenes
             DIR_CONTROL_FLAG_RIGHT = AgentManager.ControlFlags.AGENT_CONTROL_LEFT_NEG,
             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_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
         }
         
@@ -658,10 +671,7 @@ namespace OpenSim.Region.Framework.Scenes
 
 
             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)
@@ -717,21 +727,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[5] = new Vector3(0f, 0f, -0.5f); //DOWN_Nudge
+            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[6];
+            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[5] = new Vector3(-m_CameraAtAxis.Z, 0f, -m_CameraUpAxis.Z); //DOWN_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
 
@@ -995,7 +1031,9 @@ namespace OpenSim.Region.Framework.Scenes
             {
                 AbsolutePosition = AbsolutePosition + new Vector3(0f, 0f, (1.56f / 6f));
             }
-
+            
+            m_updateCount = UPDATE_COUNT;                               //KF: Trigger Anim updates to catch falling anim. 
+            
             ControllingClient.SendAvatarTerseUpdate(new SendAvatarTerseData(m_rootRegionHandle, (ushort)(m_scene.TimeDilation * ushort.MaxValue), LocalId,
                     AbsolutePosition, Velocity, Vector3.Zero, m_bodyRot, new Vector4(0,0,1,AbsolutePosition.Z - 0.5f), m_uuid, null, GetUpdatePriority(ControllingClient)));
         }
@@ -1230,7 +1268,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)
@@ -1245,9 +1282,7 @@ namespace OpenSim.Region.Framework.Scenes
 
             if ((flags & AgentManager.ControlFlags.AGENT_CONTROL_SIT_ON_GROUND) != 0)
             {
-                // 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.
+                m_updateCount = 0;  // Kill animation update burst so that the SIT_G.. will stick.
                 Animator.TrySetMovementAnimation("SIT_GROUND_CONSTRAINED");
             }
 
@@ -1292,6 +1327,11 @@ namespace OpenSim.Region.Framework.Scenes
                     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;
@@ -1306,6 +1346,12 @@ namespace OpenSim.Region.Framework.Scenes
                     else
                         dirVectors = Dir_Vectors;
 
+                    bool[] isNudge = GetDirectionIsNudge();
+
+                    
+                    
+                    
+
                     foreach (Dir_ControlFlags DCF in DIR_CONTROL_FLAGS)
                     {
                         if (((uint)flags & (uint)DCF) != 0)
@@ -1315,6 +1361,10 @@ namespace OpenSim.Region.Framework.Scenes
                             try
                             {
                                 agent_control_v3 += dirVectors[i];
+                                if (isNudge[i] == false)
+                                {
+                                    Nudging = false;
+                                }
                             }
                             catch (IndexOutOfRangeException)
                             {
@@ -1376,6 +1426,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:
@@ -1464,7 +1517,7 @@ 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);
 
                     
                 }
@@ -1485,7 +1538,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);
             
@@ -1527,7 +1579,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?
@@ -1549,7 +1601,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;
@@ -1587,46 +1639,79 @@ namespace OpenSim.Region.Framework.Scenes
                 SceneObjectPart part = m_scene.GetSceneObjectPart(m_parentID);
                 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);
                 }
-
-                if (m_physicsActor == null)
-                {
-                    AddToPhysicalScene(false);
+                                // 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);

+                }
+                //CW: If the part isn't null then we can set the current position 

+                if (part != null)

+                {

+                    Vector3 avWorldStandUp = avStandUp + part.GetWorldPosition() + (m_pos * partRot);                   // + av sit offset!

+                    AbsolutePosition = avWorldStandUp;                   //KF: Fix stand up.

+                    part.IsOccupied = false;

+                }

+                else

+                {

+                    //CW: Since the part doesn't exist, a coarse standup position isn't an issue

+                    AbsolutePosition = m_lastWorldPosition;

                 }
-
-                m_pos += m_parentPosition + new Vector3(0.0f, 0.0f, 2.0f*m_sitAvatarHeight);
-                m_parentPosition = Vector3.Zero;
-
-                m_parentID = 0;
+                
+                        m_parentPosition = Vector3.Zero;
+                                m_parentID = 0;
                 SendFullUpdateToAllClients();
                 m_requestedSitTargetID = 0;
+
                 if ((m_physicsActor != null) && (m_avHeight > 0))
                 {
                     SetHeight(m_avHeight);
                 }
             }
-
             Animator.TrySetMovementAnimation("STAND");
         }
 
@@ -1657,13 +1742,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;
@@ -1677,84 +1758,152 @@ 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 (Util.GetDistanceTo(AbsolutePosition, pos) < 4.5)
-                        {
-                            autopilot = false;
+            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
+                    sitOrientation = avSitOrientation;          // Change rotatione to the scripted one
+                    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;
+                        }
+                        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); // 
 
-                            RemoveFromPhysicalScene();
-                            AbsolutePosition = pos + new Vector3(0.0f, 0.0f, m_sitAvatarHeight);
-                        }
-                    }
-                    else
+            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) < 2.0f) && (Math.Abs(AbsolutePosition.Y - autopilotTarget.Y) < 2.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();
-                    }
+                        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();
                 }
-
-                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);
+
+            ControllingClient.SendSitResponse(part.UUID, offsetr, 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);
         }
@@ -2049,31 +2198,65 @@ 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;
                         m_bodyRot = sitTargetOrient;
-                        //Rotation = sitTargetOrient;
                         m_parentPosition = part.AbsolutePosition;
-
-                        //SendTerseUpdateToAllClients();
+                            part.IsOccupied = true;                        
                     }
                     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;
-                    }
-                }
-                else
+                        part.IsOccupied = true;
+                        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  // no Av
                 {
                     return;
                 }
@@ -2085,11 +2268,36 @@ namespace OpenSim.Region.Framework.Scenes
 
             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();
         }
+        
+        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());
+//              }                       
+                                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
@@ -2119,7 +2327,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)
             {
@@ -2193,7 +2401,7 @@ namespace OpenSim.Region.Framework.Scenes
 
             // 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));
         }
 
@@ -2208,7 +2416,7 @@ namespace OpenSim.Region.Framework.Scenes
             const float POSITION_TOLERANCE = 0.05f;
             //const int TIME_MS_TOLERANCE = 3000;
 
-            SendPrimUpdates();
+            
 
             if (m_newCoarseLocations)
             {
@@ -2244,6 +2452,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
@@ -3097,14 +3308,25 @@ namespace OpenSim.Region.Framework.Scenes
         {
             if (m_forceToApply.HasValue)
             {
-                Vector3 force = m_forceToApply.Value;
 
+                Vector3 force = m_forceToApply.Value;
                 m_updateflag = true;
-//                movementvector = force;
                 Velocity = force;
 
                 m_forceToApply = null;
             }
+            else
+            {
+                if (m_isNudging)
+                {
+                    Vector3 force = Vector3.Zero;
+
+                    m_updateflag = true;
+                    Velocity = force;
+                    m_isNudging = false;
+                        m_updateCount = UPDATE_COUNT;                   //KF: Update anims to pickup "STAND"
+                }
+            }
         }
 
         public override void SetText(string text, Vector3 color, double alpha)
@@ -3156,18 +3378,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)
@@ -3815,5 +4048,16 @@ 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));
+            }
+
+
     }
-}
\ No newline at end of file
+}