Merge branch 'master' into vehicles

9 files changed, 521 insertions, 411 deletions

diff --git a/OpenSim/Region/Framework/Scenes/AnimationSet.cs b/OpenSim/Region/Framework/Scenes/AnimationSet.cs
index aa0c8b8..740d168 100644
--- a/OpenSim/Region/Framework/Scenes/AnimationSet.cs
+++ b/OpenSim/Region/Framework/Scenes/AnimationSet.cs
@@ -30,6 +30,8 @@ using System.Collections.Generic;
 using OpenSim.Framework;
 using OpenMetaverse;
 
+using Animation = OpenSim.Framework.Animation;
+
 namespace OpenSim.Region.Framework.Scenes
 {
     [Serializable]
diff --git a/OpenSim/Region/Framework/Scenes/Scene.PacketHandlers.cs b/OpenSim/Region/Framework/Scenes/Scene.PacketHandlers.cs
index ac89f7b..1a91f0c 100644
--- a/OpenSim/Region/Framework/Scenes/Scene.PacketHandlers.cs
+++ b/OpenSim/Region/Framework/Scenes/Scene.PacketHandlers.cs
@@ -394,7 +394,7 @@ namespace OpenSim.Region.Framework.Scenes
         void ProcessViewerEffect(IClientAPI remoteClient, List<ViewerEffectEventHandlerArg> args)
         {
             // TODO: don't create new blocks if recycling an old packet
-            List<ViewerEffectPacket.EffectBlock> effectBlock = new List<ViewerEffectPacket.EffectBlock>();
+            ViewerEffectPacket.EffectBlock[] effectBlockArray = new ViewerEffectPacket.EffectBlock[args.Count];
             for (int i = 0; i < args.Count; i++)
             {
                 ViewerEffectPacket.EffectBlock effect = new ViewerEffectPacket.EffectBlock();
@@ -404,11 +404,10 @@ namespace OpenSim.Region.Framework.Scenes
                 effect.ID = args[i].ID;
                 effect.Type = args[i].Type;
                 effect.TypeData = args[i].TypeData;
-                effectBlock.Add(effect);
+                effectBlockArray[i] = effect;
             }
-            ViewerEffectPacket.EffectBlock[] effectBlockArray = effectBlock.ToArray();
 
-            ClientManager.ForEach(
+            ForEachClient(
                 delegate(IClientAPI client)
                 {
                     if (client.AgentId != remoteClient.AgentId)
diff --git a/OpenSim/Region/Framework/Scenes/Scene.cs b/OpenSim/Region/Framework/Scenes/Scene.cs
index 5005ac9..42051d0 100644
--- a/OpenSim/Region/Framework/Scenes/Scene.cs
+++ b/OpenSim/Region/Framework/Scenes/Scene.cs
@@ -61,6 +61,7 @@ namespace OpenSim.Region.Framework.Scenes
             Time = 0,
             Distance = 1,
             SimpleAngularDistance = 2,
+            FrontBack = 3,
         }
 
         public delegate void SynchronizeSceneHandler(Scene scene);
@@ -81,8 +82,6 @@ namespace OpenSim.Region.Framework.Scenes
 
         protected Timer m_restartWaitTimer = new Timer();
 
-        protected Thread m_updateEntitiesThread;
-
         public SimStatsReporter StatsReporter;
 
         protected List<RegionInfo> m_regionRestartNotifyList = new List<RegionInfo>();
@@ -107,11 +106,11 @@ namespace OpenSim.Region.Framework.Scenes
         public bool m_physicalPrim;
         public float m_maxNonphys = 256;
         public float m_maxPhys = 10;
-        public bool m_clampPrimSize = false;
-        public bool m_trustBinaries = false;
-        public bool m_allowScriptCrossings = false;
-        public bool m_useFlySlow = false;
-        public bool m_usePreJump = false;
+        public bool m_clampPrimSize;
+        public bool m_trustBinaries;
+        public bool m_allowScriptCrossings;
+        public bool m_useFlySlow;
+        public bool m_usePreJump;
         public bool m_seeIntoRegionFromNeighbor;
         // TODO: need to figure out how allow client agents but deny
         // root agents when ACL denies access to root agent
@@ -119,11 +118,11 @@ namespace OpenSim.Region.Framework.Scenes
         public int MaxUndoCount = 5;
         private int m_RestartTimerCounter;
         private readonly Timer m_restartTimer = new Timer(15000); // Wait before firing
-        private int m_incrementsof15seconds = 0;
-        private volatile bool m_backingup = false;
+        private int m_incrementsof15seconds;
+        private volatile bool m_backingup;
+        private bool m_useAsyncWhenPossible;
 
         private Dictionary<UUID, ReturnInfo> m_returns = new Dictionary<UUID, ReturnInfo>();
-        
         private Dictionary<UUID, SceneObjectGroup> m_groupsWithTargets = new Dictionary<UUID, SceneObjectGroup>();
 
         protected string m_simulatorVersion = "OpenSimulator Server";
@@ -143,8 +142,8 @@ namespace OpenSim.Region.Framework.Scenes
 
         public IXfer XferManager;
 
-        protected IAssetService m_AssetService = null;
-        protected IAuthorizationService m_AuthorizationService = null;
+        protected IAssetService m_AssetService;
+        protected IAuthorizationService m_AuthorizationService;
 
         private Object m_heartbeatLock = new Object();
 
@@ -185,7 +184,7 @@ namespace OpenSim.Region.Framework.Scenes
             }
         }
 
-        protected IInventoryService m_InventoryService = null;
+        protected IInventoryService m_InventoryService;
 
         public IInventoryService InventoryService
         {
@@ -205,7 +204,7 @@ namespace OpenSim.Region.Framework.Scenes
             }
         }
 
-        protected IGridService m_GridService = null;
+        protected IGridService m_GridService;
 
         public IGridService GridService
         {
@@ -253,9 +252,9 @@ namespace OpenSim.Region.Framework.Scenes
         // Central Update Loop
 
         protected int m_fps = 10;
-        protected int m_frame = 0;
+        protected int m_frame;
         protected float m_timespan = 0.089f;
-        protected DateTime m_lastupdate = DateTime.Now;
+        protected DateTime m_lastupdate = DateTime.UtcNow;
 
         private int m_update_physics = 1;
         private int m_update_entitymovement = 1;
@@ -266,17 +265,17 @@ namespace OpenSim.Region.Framework.Scenes
         private int m_update_terrain = 50;
         private int m_update_land = 1;
 
-        private int frameMS = 0;
-        private int physicsMS2 = 0;
-        private int physicsMS = 0;
-        private int otherMS = 0;
+        private int frameMS;
+        private int physicsMS2;
+        private int physicsMS;
+        private int otherMS;
 
         private bool m_physics_enabled = true;
         private bool m_scripts_enabled = true;
         private string m_defaultScriptEngine;
-        private int m_LastLogin = 0;
-        private Thread HeartbeatThread = null;
-        private volatile bool shuttingdown = false;
+        private int m_LastLogin;
+        private Thread HeartbeatThread;
+        private volatile bool shuttingdown;
 
         private int m_lastUpdate = Environment.TickCount;
         private bool m_firstHeartbeat = true;
@@ -480,6 +479,9 @@ namespace OpenSim.Region.Framework.Scenes
                 //
                 IConfig startupConfig = m_config.Configs["Startup"];
 
+                // Should we try to run loops synchronously or asynchronously?
+                m_useAsyncWhenPossible = startupConfig.GetBoolean("use_async_when_possible", false);
+
                 //Animation states
                 m_useFlySlow = startupConfig.GetBoolean("enableflyslow", false);
                 // TODO: Change default to true once the feature is supported
@@ -542,6 +544,9 @@ namespace OpenSim.Region.Framework.Scenes
                         case "simpleangulardistance":
                             m_update_prioritization_scheme = UpdatePrioritizationSchemes.SimpleAngularDistance;
                             break;
+                        case "frontback":
+                            m_update_prioritization_scheme = UpdatePrioritizationSchemes.FrontBack;
+                            break;
                         default:
                             m_log.Warn("[SCENE]: UpdatePrioritizationScheme was not recognized, setting to default settomg of Time");
                             m_update_prioritization_scheme = UpdatePrioritizationSchemes.Time;
@@ -889,6 +894,9 @@ namespace OpenSim.Region.Framework.Scenes
         {
             m_log.InfoFormat("[SCENE]: Closing down the single simulator: {0}", RegionInfo.RegionName);
 
+            m_restartTimer.Stop();
+            m_restartTimer.Close();
+
             // Kick all ROOT agents with the message, 'The simulator is going down'
             ForEachScenePresence(delegate(ScenePresence avatar)
                                  {
@@ -945,11 +953,8 @@ namespace OpenSim.Region.Framework.Scenes
                 HeartbeatThread = null;
             }
             m_lastUpdate = Environment.TickCount;
-            HeartbeatThread = new Thread(new ParameterizedThreadStart(Heartbeat));
-            HeartbeatThread.SetApartmentState(ApartmentState.MTA);
-            HeartbeatThread.Name = string.Format("Heartbeat for region {0}", RegionInfo.RegionName);
-            HeartbeatThread.Priority = ThreadPriority.AboveNormal;
-            HeartbeatThread.Start();
+
+            HeartbeatThread = Watchdog.StartThread(Heartbeat, "Heartbeat for region " + RegionInfo.RegionName, ThreadPriority.Normal, false);
         }
 
         /// <summary>
@@ -976,12 +981,13 @@ namespace OpenSim.Region.Framework.Scenes
         /// <summary>
         /// Performs per-frame updates regularly
         /// </summary>
-        /// <param name="sender"></param>
-        /// <param name="e"></param>
-        private void Heartbeat(object sender)
+        private void Heartbeat()
         {
             if (!Monitor.TryEnter(m_heartbeatLock))
+            {
+                Watchdog.RemoveThread();
                 return;
+            }
 
             try
             {
@@ -998,6 +1004,8 @@ namespace OpenSim.Region.Framework.Scenes
                 Monitor.Pulse(m_heartbeatLock);
                 Monitor.Exit(m_heartbeatLock);
             }
+
+            Watchdog.RemoveThread();
         }
 
         /// <summary>
@@ -1016,10 +1024,11 @@ namespace OpenSim.Region.Framework.Scenes
 //#endif
                 maintc = Environment.TickCount;
 
-                TimeSpan SinceLastFrame = DateTime.Now - m_lastupdate;
+                TimeSpan SinceLastFrame = DateTime.UtcNow - m_lastupdate;
                 float physicsFPS = 0;
 
                 frameMS = Environment.TickCount;
+
                 try
                 {
                     // Increment the frame counter
@@ -1101,6 +1110,11 @@ namespace OpenSim.Region.Framework.Scenes
                     }
                     if (loginsdisabled && (m_frame > 20))
                     {
+                        // In 99.9% of cases it is a bad idea to manually force garbage collection. However,
+                        // this is a rare case where we know we have just went through a long cycle of heap
+                        // allocations, and there is no more work to be done until someone logs in
+                        GC.Collect();
+
                         m_log.Debug("[REGION]: Enabling Logins");
                         loginsdisabled = false;
                     }
@@ -1139,13 +1153,16 @@ namespace OpenSim.Region.Framework.Scenes
                     }
                     m_timedilation = tmpval;
 
-                    m_lastupdate = DateTime.Now;
+                    m_lastupdate = DateTime.UtcNow;
                 }
                 maintc = Environment.TickCount - maintc;
                 maintc = (int)(m_timespan * 1000) - maintc;
 
                 if ((maintc < (m_timespan * 1000)) && maintc > 0)
                     Thread.Sleep(maintc);
+
+                // Tell the watchdog that this thread is still alive
+                Watchdog.UpdateThread();
             }
         }
 
@@ -1219,10 +1236,7 @@ namespace OpenSim.Region.Framework.Scenes
             if (!m_backingup)
             {
                 m_backingup = true;
-
-                System.ComponentModel.BackgroundWorker backupWorker = new System.ComponentModel.BackgroundWorker();
-                backupWorker.DoWork += delegate(object sender, System.ComponentModel.DoWorkEventArgs e) { Backup(); };
-                backupWorker.RunWorkerAsync();
+                Util.FireAndForget(BackupWaitCallback);
             }
         }
 
@@ -1235,6 +1249,14 @@ namespace OpenSim.Region.Framework.Scenes
         }
 
         /// <summary>
+        /// Wrapper for Backup() that can be called with Util.FireAndForget()
+        /// </summary>
+        private void BackupWaitCallback(object o)
+        {
+            Backup();
+        }
+
+        /// <summary>
         /// Backup the scene.  This acts as the main method of the backup thread.
         /// </summary>
         /// <returns></returns>
@@ -2460,7 +2482,7 @@ namespace OpenSim.Region.Framework.Scenes
         /// <param name="client"></param>
         public override void AddNewClient(IClientAPI client)
         {
-            ClientManager.Add(client);
+            m_clientManager.Add(client);
 
             CheckHeartbeat();
             SubscribeToClientEvents(client);
@@ -3099,7 +3121,7 @@ namespace OpenSim.Region.Framework.Scenes
 
                 // Remove the avatar from the scene
                 m_sceneGraph.RemoveScenePresence(agentID);
-                ClientManager.Remove(agentID);
+                m_clientManager.Remove(agentID);
 
                 try
                 {
@@ -3496,9 +3518,7 @@ namespace OpenSim.Region.Framework.Scenes
         public virtual void AgentCrossing(UUID agentID, Vector3 position, bool isFlying)
         {
             ScenePresence presence;
-
-            lock (m_sceneGraph.ScenePresences)
-                m_sceneGraph.ScenePresences.TryGetValue(agentID, out presence);
+            m_sceneGraph.TryGetAvatar(agentID, out presence);
 
             if (presence != null)
             {
@@ -3709,8 +3729,7 @@ namespace OpenSim.Region.Framework.Scenes
                                             Vector3 lookAt, uint teleportFlags)
         {
             ScenePresence sp;
-            lock (m_sceneGraph.ScenePresences)
-                m_sceneGraph.ScenePresences.TryGetValue(remoteClient.AgentId, out sp);
+            m_sceneGraph.TryGetAvatar(remoteClient.AgentId, out sp);
 
             if (sp != null)
             {
@@ -4112,7 +4131,7 @@ namespace OpenSim.Region.Framework.Scenes
         /// This list is a new object, so it can be iterated over without locking.
         /// </summary>
         /// <returns></returns>
-        public List<ScenePresence> GetScenePresences()
+        public ScenePresence[] GetScenePresences()
         {
             return m_sceneGraph.GetScenePresences();
         }
@@ -4159,15 +4178,13 @@ namespace OpenSim.Region.Framework.Scenes
         public void ForEachScenePresence(Action<ScenePresence> action)
         {
             // We don't want to try to send messages if there are no avatars.
-            if (m_sceneGraph != null && m_sceneGraph.ScenePresences != null)
+            if (m_sceneGraph != null)
             {
                 try
                 {
-                    List<ScenePresence> presenceList = GetScenePresences();
-                    foreach (ScenePresence presence in presenceList)
-                    {
-                        action(presence);
-                    }
+                    ScenePresence[] presences = GetScenePresences();
+                    for (int i = 0; i < presences.Length; i++)
+                        action(presences[i]);
                 }
                 catch (Exception e)
                 {
@@ -4239,7 +4256,28 @@ namespace OpenSim.Region.Framework.Scenes
 
         public void ForEachClient(Action<IClientAPI> action)
         {
-            ClientManager.ForEach(action);
+            ForEachClient(action, m_useAsyncWhenPossible);
+        }
+
+        public void ForEachClient(Action<IClientAPI> action, bool doAsynchronous)
+        {
+            // FIXME: Asynchronous iteration is disabled until we have a threading model that
+            // can support calling this function from an async packet handler without
+            // potentially deadlocking
+            //if (doAsynchronous)
+            //    m_clientManager.ForEach(action);
+            //else
+            //    m_clientManager.ForEachSync(action);
+        }
+
+        public bool TryGetClient(UUID avatarID, out IClientAPI client)
+        {
+            return m_clientManager.TryGetValue(avatarID, out client);
+        }
+
+        public bool TryGetClient(System.Net.IPEndPoint remoteEndPoint, out IClientAPI client)
+        {
+            return m_clientManager.TryGetValue(remoteEndPoint, out client);
         }
 
         public void ForEachSOG(Action<SceneObjectGroup> action)
@@ -4571,7 +4609,7 @@ namespace OpenSim.Region.Framework.Scenes
             {
                 case PhysicsJointType.Ball:
                     {
-                        PhysicsVector jointAnchor = PhysicsScene.GetJointAnchor(joint);
+                        Vector3 jointAnchor = PhysicsScene.GetJointAnchor(joint);
                         Vector3 proxyPos = new Vector3(jointAnchor.X, jointAnchor.Y, jointAnchor.Z);
                         jointProxyObject.ParentGroup.UpdateGroupPosition(proxyPos); // schedules the entire group for a terse update
                     }
@@ -4579,7 +4617,7 @@ namespace OpenSim.Region.Framework.Scenes
 
                 case PhysicsJointType.Hinge:
                     {
-                        PhysicsVector jointAnchor = PhysicsScene.GetJointAnchor(joint);
+                        Vector3 jointAnchor = PhysicsScene.GetJointAnchor(joint);
 
                         // Normally, we would just ask the physics scene to return the axis for the joint.
                         // Unfortunately, ODE sometimes returns <0,0,0> for the joint axis, which should
diff --git a/OpenSim/Region/Framework/Scenes/SceneBase.cs b/OpenSim/Region/Framework/Scenes/SceneBase.cs
index cf5c3c8..82731d1 100644
--- a/OpenSim/Region/Framework/Scenes/SceneBase.cs
+++ b/OpenSim/Region/Framework/Scenes/SceneBase.cs
@@ -102,12 +102,7 @@ namespace OpenSim.Region.Framework.Scenes
 
         private readonly Mutex _primAllocateMutex = new Mutex(false);
         
-        private readonly ClientManager m_clientManager = new ClientManager();
-
-        public ClientManager ClientManager
-        {
-            get { return m_clientManager; }
-        }
+        protected readonly ClientManager m_clientManager = new ClientManager();
 
         public float TimeDilation
         {
diff --git a/OpenSim/Region/Framework/Scenes/SceneGraph.cs b/OpenSim/Region/Framework/Scenes/SceneGraph.cs
index deee6c3..db055f9 100644
--- a/OpenSim/Region/Framework/Scenes/SceneGraph.cs
+++ b/OpenSim/Region/Framework/Scenes/SceneGraph.cs
@@ -66,7 +66,9 @@ namespace OpenSim.Region.Framework.Scenes
 
         #region Fields
 
-        protected internal Dictionary<UUID, ScenePresence> ScenePresences = new Dictionary<UUID, ScenePresence>();
+        protected Dictionary<UUID, ScenePresence> m_scenePresences = new Dictionary<UUID, ScenePresence>();
+        protected ScenePresence[] m_scenePresenceArray = new ScenePresence[0];
+
         // SceneObjects is not currently populated or used.
         //public Dictionary<UUID, SceneObjectGroup> SceneObjects;
         protected internal EntityManager Entities = new EntityManager();
@@ -126,10 +128,12 @@ namespace OpenSim.Region.Framework.Scenes
 
         protected internal void Close()
         {
-            lock (ScenePresences)
+            lock (m_scenePresences)
             {
-                ScenePresences.Clear();
+                m_scenePresences.Clear();
+                m_scenePresenceArray = new ScenePresence[0];
             }
+
             lock (m_dictionary_lock)
             {
                 SceneObjectGroupsByFullID.Clear();
@@ -157,11 +161,9 @@ namespace OpenSim.Region.Framework.Scenes
 
         protected internal void UpdatePresences()
         {
-            List<ScenePresence> updateScenePresences = GetScenePresences();
-            foreach (ScenePresence pres in updateScenePresences)
-            {
-                pres.Update();
-            }
+            ScenePresence[] updateScenePresences = GetScenePresences();
+            for (int i = 0; i < updateScenePresences.Length; i++)
+                updateScenePresences[i].Update();
         }
 
         protected internal float UpdatePhysics(double elapsed)
@@ -190,15 +192,9 @@ namespace OpenSim.Region.Framework.Scenes
 
         protected internal void UpdateScenePresenceMovement()
         {
-            List<ScenePresence> moveEntities = GetScenePresences();
-
-            foreach (EntityBase entity in moveEntities)
-            {
-                //cfk. This throws occaisional exceptions on a heavily used region
-                //and I added this null check to try to preclude the exception.
-                if (entity != null)
-                    entity.UpdateMovement();
-            }
+            ScenePresence[] moveEntities = GetScenePresences();
+            for (int i = 0; i < moveEntities.Length; i++)
+                moveEntities[i].UpdateMovement();
         }
 
         #endregion
@@ -645,9 +641,34 @@ namespace OpenSim.Region.Framework.Scenes
 
             Entities[presence.UUID] = presence;
 
-            lock (ScenePresences)
+            lock (m_scenePresences)
             {
-                ScenePresences[presence.UUID] = presence;
+                if (!m_scenePresences.ContainsKey(presence.UUID))
+                {
+                    m_scenePresences.Add(presence.UUID, presence);
+
+                    // Create a new array of ScenePresence references
+                    int oldLength = m_scenePresenceArray.Length;
+                    ScenePresence[] newArray = new ScenePresence[oldLength + 1];
+                    Array.Copy(m_scenePresenceArray, newArray, oldLength);
+                    newArray[oldLength] = presence;
+                    m_scenePresenceArray = newArray;
+                }
+                else
+                {
+                    m_scenePresences[presence.UUID] = presence;
+                    
+                    // Do a linear search through the array of ScenePresence references
+                    // and update the modified entry
+                    for (int i = 0; i < m_scenePresenceArray.Length; i++)
+                    {
+                        if (m_scenePresenceArray[i].UUID == presence.UUID)
+                        {
+                            m_scenePresenceArray[i] = presence;
+                            break;
+                        }
+                    }
+                }
             }
         }
 
@@ -663,16 +684,30 @@ namespace OpenSim.Region.Framework.Scenes
                     agentID);
             }
 
-            lock (ScenePresences)
+            lock (m_scenePresences)
             {
-                if (!ScenePresences.Remove(agentID))
+                if (m_scenePresences.Remove(agentID))
+                {
+                    // Copy all of the elements from the previous array
+                    // into the new array except the removed element
+                    int oldLength = m_scenePresenceArray.Length;
+                    ScenePresence[] newArray = new ScenePresence[oldLength - 1];
+                    int j = 0;
+                    for (int i = 0; i < m_scenePresenceArray.Length; i++)
+                    {
+                        ScenePresence presence = m_scenePresenceArray[i];
+                        if (presence.UUID != agentID)
+                        {
+                            newArray[j] = presence;
+                            ++j;
+                        }
+                    }
+                    m_scenePresenceArray = newArray;
+                }
+                else
                 {
                     m_log.WarnFormat("[SCENE] Tried to remove non-existent scene presence with agent ID {0} from scene ScenePresences list", agentID);
                 }
-//                else
-//                {
-//                    m_log.InfoFormat("[SCENE] Removed scene presence {0} from scene presences list", agentID);
-//                }
             }
         }
 
@@ -704,20 +739,21 @@ namespace OpenSim.Region.Framework.Scenes
 
         public void RecalculateStats()
         {
-            List<ScenePresence> SPList = GetScenePresences();
+            ScenePresence[] presences = GetScenePresences();
             int rootcount = 0;
             int childcount = 0;
 
-            foreach (ScenePresence user in SPList)
+            for (int i = 0; i < presences.Length; i++)
             {
+                ScenePresence user = presences[i];
                 if (user.IsChildAgent)
-                    childcount++;
+                    ++childcount;
                 else
-                    rootcount++;
+                    ++rootcount;
             }
+
             m_numRootAgents = rootcount;
             m_numChildAgents = childcount;
-            
         }
 
         public int GetChildAgentCount()
@@ -767,12 +803,9 @@ namespace OpenSim.Region.Framework.Scenes
         /// locking is required to iterate over it.
         /// </summary>
         /// <returns></returns>
-        protected internal List<ScenePresence> GetScenePresences()
+        protected internal ScenePresence[] GetScenePresences()
         {
-            lock (ScenePresences)
-            {
-                return new List<ScenePresence>(ScenePresences.Values);
-            }
+            return m_scenePresenceArray;
         }
 
         protected internal List<ScenePresence> GetAvatars()
@@ -817,14 +850,13 @@ namespace OpenSim.Region.Framework.Scenes
             // No locking of scene presences here since we're passing back a list...
 
             List<ScenePresence> result = new List<ScenePresence>();
-            List<ScenePresence> ScenePresencesList = GetScenePresences();
+            ScenePresence[] scenePresences = GetScenePresences();
 
-            foreach (ScenePresence avatar in ScenePresencesList)
+            for (int i = 0; i < scenePresences.Length; i++)
             {
+                ScenePresence avatar = scenePresences[i];
                 if (filter(avatar))
-                {
                     result.Add(avatar);
-                }
             }
 
             return result;
@@ -839,9 +871,9 @@ namespace OpenSim.Region.Framework.Scenes
         {
             ScenePresence sp;
             
-            lock (ScenePresences)
+            lock (m_scenePresences)
             {
-                ScenePresences.TryGetValue(agentID, out sp);
+                m_scenePresences.TryGetValue(agentID, out sp);
             }
 
             return sp;
@@ -1000,48 +1032,24 @@ namespace OpenSim.Region.Framework.Scenes
 
         protected internal bool TryGetAvatar(UUID avatarId, out ScenePresence avatar)
         {
-            ScenePresence presence;
-            
-            lock (ScenePresences)
-            {
-                if (ScenePresences.TryGetValue(avatarId, out presence))
-                {
-                    avatar = presence;
-                    return true;
-
-                    //if (!presence.IsChildAgent)
-                    //{
-                    //    avatar = presence;
-                    //    return true;
-                    //}
-                    //else
-                    //{
-                    //    m_log.WarnFormat(
-                    //        "[INNER SCENE]: Requested avatar {0} could not be found in scene {1} since it is only registered as a child agent!",
-                    //        avatarId, m_parentScene.RegionInfo.RegionName);
-                    //}
-                }
-            }
-
-            avatar = null;
-            return false;
+            lock (m_scenePresences)
+                return m_scenePresences.TryGetValue(avatarId, out avatar);
         }
 
         protected internal bool TryGetAvatarByName(string avatarName, out ScenePresence avatar)
         {
-            lock (ScenePresences)
+            ScenePresence[] presences = GetScenePresences();
+
+            for (int i = 0; i < presences.Length; i++)
             {
-                foreach (ScenePresence presence in ScenePresences.Values)
+                ScenePresence presence = presences[i];
+
+                if (!presence.IsChildAgent)
                 {
-                    if (!presence.IsChildAgent)
+                    if (String.Compare(avatarName, presence.ControllingClient.Name, true) == 0)
                     {
-                        string name = presence.ControllingClient.Name;
-
-                        if (String.Compare(avatarName, name, true) == 0)
-                        {
-                            avatar = presence;
-                            return true;
-                        }
+                        avatar = presence;
+                        return true;
                     }
                 }
             }
diff --git a/OpenSim/Region/Framework/Scenes/SceneManager.cs b/OpenSim/Region/Framework/Scenes/SceneManager.cs
index 3097929..dfaa7ea 100644
--- a/OpenSim/Region/Framework/Scenes/SceneManager.cs
+++ b/OpenSim/Region/Framework/Scenes/SceneManager.cs
@@ -411,41 +411,46 @@ namespace OpenSim.Region.Framework.Scenes
         /// <param name="newDebug"></param>
         public void SetDebugPacketLevelOnCurrentScene(int newDebug)
         {
-            ForEachCurrentScene(delegate(Scene scene)
-                                {
-                                    List<ScenePresence> scenePresences = scene.GetScenePresences();
+            ForEachCurrentScene(
+                delegate(Scene scene)
+                {
+                    ScenePresence[] scenePresences = scene.GetScenePresences();
+
+                    for (int i = 0; i < scenePresences.Length; i++)
+                    {
+                        ScenePresence scenePresence = scenePresences[i];
 
-                                    foreach (ScenePresence scenePresence in scenePresences)
-                                    {
-                                        if (!scenePresence.IsChildAgent)
-                                        {
-                                            m_log.ErrorFormat("Packet debug for {0} {1} set to {2}",
-                                                              scenePresence.Firstname,
-                                                              scenePresence.Lastname,
-                                                              newDebug);
+                        if (!scenePresence.IsChildAgent)
+                        {
+                            m_log.ErrorFormat("Packet debug for {0} {1} set to {2}",
+                                              scenePresence.Firstname,
+                                              scenePresence.Lastname,
+                                              newDebug);
 
-                                            scenePresence.ControllingClient.SetDebugPacketLevel(newDebug);
-                                        }
-                                    }
-                                });
+                            scenePresence.ControllingClient.SetDebugPacketLevel(newDebug);
+                        }
+                    }
+                }
+            );
         }
 
         public List<ScenePresence> GetCurrentSceneAvatars()
         {
             List<ScenePresence> avatars = new List<ScenePresence>();
 
-            ForEachCurrentScene(delegate(Scene scene)
-            {
-                List<ScenePresence> scenePresences = scene.GetScenePresences();
-
-                foreach (ScenePresence scenePresence in scenePresences)
+            ForEachCurrentScene(
+                delegate(Scene scene)
                 {
-                    if (!scenePresence.IsChildAgent)
+                    ScenePresence[] scenePresences = scene.GetScenePresences();
+
+                    for (int i = 0; i < scenePresences.Length; i++)
                     {
-                        avatars.Add(scenePresence);
+                        ScenePresence scenePresence = scenePresences[i];
+                        if (!scenePresence.IsChildAgent)
+                            avatars.Add(scenePresence);
                     }
                 }
-            });
+            );
 
             return avatars;
         }
@@ -456,7 +461,7 @@ namespace OpenSim.Region.Framework.Scenes
 
             ForEachCurrentScene(delegate(Scene scene)
             {
-                List<ScenePresence> scenePresences = scene.GetScenePresences();
+                ScenePresence[] scenePresences = scene.GetScenePresences();
                 presences.AddRange(scenePresences);
             });
 
diff --git a/OpenSim/Region/Framework/Scenes/SceneObjectGroup.cs b/OpenSim/Region/Framework/Scenes/SceneObjectGroup.cs
index 69b3ded..38a0cff 100644
--- a/OpenSim/Region/Framework/Scenes/SceneObjectGroup.cs
+++ b/OpenSim/Region/Framework/Scenes/SceneObjectGroup.cs
@@ -493,8 +493,8 @@ namespace OpenSim.Region.Framework.Scenes
 
         public Vector3 GroupScale()
         {
-            Vector3 minScale = new Vector3(Constants.RegionSize,Constants.RegionSize,Constants.RegionSize);
-            Vector3 maxScale = new Vector3(0f,0f,0f);
+            Vector3 minScale = new Vector3(Constants.RegionSize, Constants.RegionSize, Constants.RegionSize);
+            Vector3 maxScale = Vector3.Zero;
             Vector3 finalScale = new Vector3(0.5f, 0.5f, 0.5f);
 
             lock (m_parts)
@@ -577,7 +577,6 @@ namespace OpenSim.Region.Framework.Scenes
             {
                 foreach (SceneObjectPart part in m_parts.Values)
                 {
-
                     Vector3 worldPos = part.GetWorldPosition();
                     Vector3 offset = worldPos - AbsolutePosition;
                     Quaternion worldRot;
@@ -1182,8 +1181,8 @@ namespace OpenSim.Region.Framework.Scenes
                 {
 //                    part.Inventory.RemoveScriptInstances();
 
-                    List<ScenePresence> avatars = Scene.GetScenePresences();
-                    for (int i = 0; i < avatars.Count; i++)
+                    ScenePresence[] avatars = Scene.GetScenePresences();
+                    for (int i = 0; i < avatars.Length; i++)
                     {
                         if (avatars[i].ParentID == LocalId)
                         {
@@ -1335,7 +1334,7 @@ namespace OpenSim.Region.Framework.Scenes
                                 (parcel.LandData.GroupID != GroupID ||
                                 parcel.LandData.GroupID == UUID.Zero))
                         {
-                            if ((DateTime.Now - RootPart.Rezzed).TotalMinutes >
+                            if ((DateTime.UtcNow - RootPart.Rezzed).TotalMinutes >
                                     parcel.LandData.OtherCleanTime)
                             {
                                 DetachFromBackup();
@@ -1480,8 +1479,8 @@ namespace OpenSim.Region.Framework.Scenes
                 dupe.RootPart.PhysActor = m_scene.PhysicsScene.AddPrimShape(
                     dupe.RootPart.Name,
                     pbs,
-                    new PhysicsVector(dupe.RootPart.AbsolutePosition.X, dupe.RootPart.AbsolutePosition.Y, dupe.RootPart.AbsolutePosition.Z),
-                    new PhysicsVector(dupe.RootPart.Scale.X, dupe.RootPart.Scale.Y, dupe.RootPart.Scale.Z),
+                    dupe.RootPart.AbsolutePosition,
+                    dupe.RootPart.Scale,
                     dupe.RootPart.RotationOffset,
                     dupe.RootPart.PhysActor.IsPhysical);
 
@@ -1596,7 +1595,7 @@ namespace OpenSim.Region.Framework.Scenes
             */
         }
 
-        public void applyImpulse(PhysicsVector impulse)
+        public void applyImpulse(Vector3 impulse)
         {
             // We check if rootpart is null here because scripts don't delete if you delete the host.
             // This means that unfortunately, we can pass a null physics actor to Simulate!
@@ -1623,7 +1622,7 @@ namespace OpenSim.Region.Framework.Scenes
             }
         }
 
-        public void applyAngularImpulse(PhysicsVector impulse)
+        public void applyAngularImpulse(Vector3 impulse)
         {
             // We check if rootpart is null here because scripts don't delete if you delete the host.
             // This means that unfortunately, we can pass a null physics actor to Simulate!
@@ -1642,7 +1641,7 @@ namespace OpenSim.Region.Framework.Scenes
             }
         }
 
-        public void setAngularImpulse(PhysicsVector impulse)
+        public void setAngularImpulse(Vector3 impulse)
         {
             // We check if rootpart is null here because scripts don't delete if you delete the host.
             // This means that unfortunately, we can pass a null physics actor to Simulate!
@@ -1673,8 +1672,8 @@ namespace OpenSim.Region.Framework.Scenes
                 {
                     if (!IsAttachment)
                     {
-                        PhysicsVector torque = rootpart.PhysActor.Torque;
-                        return new Vector3(torque.X, torque.Y, torque.Z);
+                        Vector3 torque = rootpart.PhysActor.Torque;
+                        return torque;
                     }
                 }
             }
@@ -1707,7 +1706,7 @@ namespace OpenSim.Region.Framework.Scenes
                 {
                     if (rootpart.PhysActor != null)
                     {
-                        rootpart.PhysActor.PIDTarget = new PhysicsVector(target.X, target.Y, target.Z);
+                        rootpart.PhysActor.PIDTarget = target;
                         rootpart.PhysActor.PIDTau = tau;
                         rootpart.PhysActor.PIDActive = true;
                     }
@@ -2375,7 +2374,7 @@ namespace OpenSim.Region.Framework.Scenes
                     if (m_rootPart.PhysActor.IsPhysical)
                     {
                         Vector3 llmoveforce = pos - AbsolutePosition;
-                        PhysicsVector grabforce = new PhysicsVector(llmoveforce.X, llmoveforce.Y, llmoveforce.Z);
+                        Vector3 grabforce = llmoveforce;
                         grabforce = (grabforce / 10) * m_rootPart.PhysActor.Mass;
                         m_rootPart.PhysActor.AddForce(grabforce,true);
                         m_scene.PhysicsScene.AddPhysicsActorTaint(m_rootPart.PhysActor);
@@ -2480,7 +2479,7 @@ namespace OpenSim.Region.Framework.Scenes
                           rotationAxis.Normalize();
 
                           //m_log.Error("SCENE OBJECT GROUP]: rotation axis is " + rotationAxis);
-                          PhysicsVector spinforce = new PhysicsVector(rotationAxis.X, rotationAxis.Y, rotationAxis.Z);
+                          Vector3 spinforce = new Vector3(rotationAxis.X, rotationAxis.Y, rotationAxis.Z);
                           spinforce = (spinforce/8) * m_rootPart.PhysActor.Mass; // 8 is an arbitrary torque scaling factor
                           m_rootPart.PhysActor.AddAngularForce(spinforce,true);
                           m_scene.PhysicsScene.AddPhysicsActorTaint(m_rootPart.PhysActor);
@@ -2707,8 +2706,7 @@ namespace OpenSim.Region.Framework.Scenes
                         if (scale.Z > m_scene.m_maxPhys)
                             scale.Z = m_scene.m_maxPhys;
                     }
-                    part.PhysActor.Size =
-                        new PhysicsVector(scale.X, scale.Y, scale.Z);
+                    part.PhysActor.Size = scale;
                     m_scene.PhysicsScene.AddPhysicsActorTaint(part.PhysActor);
                 }
                 //if (part.UUID != m_rootPart.UUID)
@@ -2852,8 +2850,7 @@ namespace OpenSim.Region.Framework.Scenes
 
                 if (part.PhysActor != null)
                 {
-                    part.PhysActor.Size =
-                        new PhysicsVector(prevScale.X, prevScale.Y, prevScale.Z);
+                    part.PhysActor.Size = prevScale;
                     m_scene.PhysicsScene.AddPhysicsActorTaint(part.PhysActor);
                 }
 
@@ -3366,6 +3363,8 @@ namespace OpenSim.Region.Framework.Scenes
                     return GetPriorityByDistance(client);
                 case Scene.UpdatePrioritizationSchemes.SimpleAngularDistance:
                     return GetPriorityBySimpleAngularDistance(client);
+                case Scenes.Scene.UpdatePrioritizationSchemes.FrontBack:
+                    return GetPriorityByFrontBack(client);
                 default:
                     throw new InvalidOperationException("UpdatePrioritizationScheme not defined");
             }
@@ -3398,6 +3397,16 @@ namespace OpenSim.Region.Framework.Scenes
             return double.NaN;
         }
 
+        private double GetPriorityByFrontBack(IClientAPI client)
+        {
+            ScenePresence presence = Scene.GetScenePresence(client.AgentId);
+            if (presence != null)
+            {
+                return GetPriorityByFrontBack(presence.CameraPosition, presence.CameraAtAxis);
+            }
+            return double.NaN;
+        }
+
         public double GetPriorityByDistance(Vector3 position)
         {
             return Vector3.Distance(AbsolutePosition, position);
@@ -3427,5 +3436,21 @@ namespace OpenSim.Region.Framework.Scenes
             else
                 return double.MinValue;
         }
+
+        public double GetPriorityByFrontBack(Vector3 camPosition, Vector3 camAtAxis)
+        {
+            // Distance
+            double priority = Vector3.Distance(camPosition, AbsolutePosition);
+
+            // Scale
+            //priority -= GroupScale().Length();
+
+            // Plane equation
+            float d = -Vector3.Dot(camPosition, camAtAxis);
+            float p = Vector3.Dot(camAtAxis, AbsolutePosition) + d;
+            if (p < 0.0f) priority *= 2.0f;
+
+            return priority;
+        }
     }
 }
diff --git a/OpenSim/Region/Framework/Scenes/SceneObjectPart.cs b/OpenSim/Region/Framework/Scenes/SceneObjectPart.cs
index 32171a0..70f3112 100644
--- a/OpenSim/Region/Framework/Scenes/SceneObjectPart.cs
+++ b/OpenSim/Region/Framework/Scenes/SceneObjectPart.cs
@@ -102,16 +102,16 @@ namespace OpenSim.Region.Framework.Scenes
 
         #region Fields
 
-        public bool AllowedDrop = false;
+        public bool AllowedDrop;
 
         [XmlIgnore]
-        public bool DIE_AT_EDGE = false;
+        public bool DIE_AT_EDGE;
 
         // TODO: This needs to be persisted in next XML version update!
         [XmlIgnore]
-        public int[] PayPrice = {-2,-2,-2,-2,-2};
+        public readonly int[] PayPrice = {-2,-2,-2,-2,-2};
         [XmlIgnore]
-        public PhysicsActor PhysActor = null;
+        public PhysicsActor PhysActor;
 
         //Xantor 20080528 Sound stuff:
         //  Note: This isn't persisted in the database right now, as the fields for that aren't just there yet.
@@ -130,55 +130,56 @@ namespace OpenSim.Region.Framework.Scenes
         public double SoundRadius;
         
         [XmlIgnore]
-        public uint TimeStampFull = 0;
+        public uint TimeStampFull;
         
         [XmlIgnore]
-        public uint TimeStampLastActivity = 0; // Will be used for AutoReturn
+        public uint TimeStampLastActivity; // Will be used for AutoReturn
         
         [XmlIgnore]
-        public uint TimeStampTerse = 0;
-        
+        public uint TimeStampTerse;
+
         [XmlIgnore]
-        public UUID FromItemID = UUID.Zero;
+        public UUID FromItemID;
                
         /// <value>
         /// The UUID of the user inventory item from which this object was rezzed if this is a root part.
         /// If UUID.Zero then either this is not a root part or there is no connection with a user inventory item.
         /// </value>
-        private UUID m_fromUserInventoryItemID = UUID.Zero;
+        private UUID m_fromUserInventoryItemID;
         
         [XmlIgnore]
         public UUID FromUserInventoryItemID
         {
             get { return m_fromUserInventoryItemID; }
         }
-        
+
         [XmlIgnore]
-        public bool IsAttachment = false;
-        
+        public bool IsAttachment;
+
         [XmlIgnore]
-        public scriptEvents AggregateScriptEvents = 0;
-        
+        public scriptEvents AggregateScriptEvents;
+
         [XmlIgnore]
-        public UUID AttachedAvatar = UUID.Zero;
-        
+        public UUID AttachedAvatar;
+
         [XmlIgnore]
-        public Vector3 AttachedPos = Vector3.Zero;
-        
+        public Vector3 AttachedPos;
+
         [XmlIgnore]
-        public uint AttachmentPoint = (byte)0;
-        
+        public uint AttachmentPoint;
+
         [XmlIgnore]
-        public PhysicsVector RotationAxis = new PhysicsVector(1f,1f,1f);
+        public Vector3 RotationAxis = Vector3.One;
 
         [XmlIgnore]
-        public bool VolumeDetectActive = false; // XmlIgnore set to avoid problems with persistance until I come to care for this
-                                                // Certainly this must be a persistant setting finally
+        public bool VolumeDetectActive; // XmlIgnore set to avoid problems with persistance until I come to care for this
+                                        // Certainly this must be a persistant setting finally
 
         [XmlIgnore]
-        public bool IsWaitingForFirstSpinUpdatePacket = false;
+        public bool IsWaitingForFirstSpinUpdatePacket;
+
         [XmlIgnore]
-        public Quaternion SpinOldOrientation = new Quaternion();
+        public Quaternion SpinOldOrientation = Quaternion.Identity;
 
         /// <summary>
         /// This part's inventory
@@ -191,34 +192,32 @@ namespace OpenSim.Region.Framework.Scenes
         protected SceneObjectPartInventory m_inventory;
 
         [XmlIgnore]
-        public bool Undoing = false;
+        public bool Undoing;
 
         [XmlIgnore]
-        private PrimFlags LocalFlags = 0;
+        private PrimFlags LocalFlags;
         [XmlIgnore]
         private float m_damage = -1.0f;
         private byte[] m_TextureAnimation;
-        private byte m_clickAction = 0;
+        private byte m_clickAction;
         private Color m_color = Color.Black;
         private string m_description = String.Empty;
         private readonly List<uint> m_lastColliders = new List<uint>();
-        // private PhysicsVector m_lastRotationalVelocity = PhysicsVector.Zero;
-        private int m_linkNum = 0;
+        private int m_linkNum;
         [XmlIgnore]
-        private int m_scriptAccessPin = 0;
+        private int m_scriptAccessPin;
         [XmlIgnore]
         private readonly Dictionary<UUID, scriptEvents> m_scriptEvents = new Dictionary<UUID, scriptEvents>();
         private string m_sitName = String.Empty;
         private Quaternion m_sitTargetOrientation = Quaternion.Identity;
-        private Vector3 m_sitTargetPosition = Vector3.Zero;
+        private Vector3 m_sitTargetPosition;
         private string m_sitAnimation = "SIT";
         private string m_text = String.Empty;
         private string m_touchName = String.Empty;
         private readonly UndoStack<UndoState> m_undo = new UndoStack<UndoState>(5);
         private UUID _creatorID;
 
-
-        private bool m_passTouches = false;
+        private bool m_passTouches;
 
         /// <summary>
         /// Only used internally to schedule client updates.
@@ -236,28 +235,34 @@ namespace OpenSim.Region.Framework.Scenes
         //unkown if this will be kept, added as a way of removing the group position from the group class
         protected Vector3 m_groupPosition;
         protected uint m_localId;
-        protected Material m_material = (Material)3; // Wood
+        protected Material m_material = OpenMetaverse.Material.Wood;
         protected string m_name;
         protected Vector3 m_offsetPosition;
 
         // FIXME, TODO, ERROR: 'ParentGroup' can't be in here, move it out.
         protected SceneObjectGroup m_parentGroup;
-        protected byte[] m_particleSystem = new byte[0];
+        protected byte[] m_particleSystem = Utils.EmptyBytes;
         protected ulong m_regionHandle;
         protected Quaternion m_rotationOffset;
-        protected PrimitiveBaseShape m_shape = null;
+        protected PrimitiveBaseShape m_shape;
         protected UUID m_uuid;
         protected Vector3 m_velocity;
 
+        protected Vector3 m_lastPosition;
+        protected Quaternion m_lastRotation;
+        protected Vector3 m_lastVelocity;
+        protected Vector3 m_lastAcceleration;
+        protected Vector3 m_lastAngularVelocity;
+
         // TODO: Those have to be changed into persistent properties at some later point,
         // or sit-camera on vehicles will break on sim-crossing.
-        private Vector3 m_cameraEyeOffset = new Vector3(0.0f, 0.0f, 0.0f);
-        private Vector3 m_cameraAtOffset = new Vector3(0.0f, 0.0f, 0.0f);
-        private bool m_forceMouselook = false;
+        private Vector3 m_cameraEyeOffset;
+        private Vector3 m_cameraAtOffset;
+        private bool m_forceMouselook;
 
         // TODO: Collision sound should have default.
-        private UUID m_collisionSound = UUID.Zero;
-        private float m_collisionSoundVolume = 0.0f;
+        private UUID m_collisionSound;
+        private float m_collisionSoundVolume;
 
         #endregion Fields
 
@@ -269,9 +274,9 @@ namespace OpenSim.Region.Framework.Scenes
         public SceneObjectPart()
         {
             // It's not necessary to persist this
-            m_TextureAnimation = new byte[0];
-            m_particleSystem = new byte[0];
-            Rezzed = DateTime.Now;
+            m_TextureAnimation = Utils.EmptyBytes;
+            m_particleSystem = Utils.EmptyBytes;
+            Rezzed = DateTime.UtcNow;
             
             m_inventory = new SceneObjectPartInventory(this);
         }
@@ -290,8 +295,8 @@ namespace OpenSim.Region.Framework.Scenes
         {
             m_name = "Primitive";
 
-            Rezzed = DateTime.Now;
-            _creationDate = (Int32) (DateTime.UtcNow - new DateTime(1970, 1, 1)).TotalSeconds;
+            Rezzed = DateTime.UtcNow;
+            _creationDate = (int)Utils.DateTimeToUnixTime(Rezzed);
             _ownerID = ownerID;
             _creatorID = _ownerID;
             _lastOwnerID = UUID.Zero;
@@ -299,19 +304,19 @@ namespace OpenSim.Region.Framework.Scenes
             Shape = shape;
             // Todo: Add More Object Parameter from above!
             _ownershipCost = 0;
-            _objectSaleType = (byte) 0;
+            _objectSaleType = 0;
             _salePrice = 0;
-            _category = (uint) 0;
+            _category = 0;
             _lastOwnerID = _creatorID;
             // End Todo: ///
             GroupPosition = groupPosition;
             OffsetPosition = offsetPosition;
             RotationOffset = rotationOffset;
-            Velocity = new Vector3(0, 0, 0);
-            AngularVelocity = new Vector3(0, 0, 0);
-            Acceleration = new Vector3(0, 0, 0);
-            m_TextureAnimation = new byte[0];
-            m_particleSystem = new byte[0];
+            Velocity = Vector3.Zero;
+            AngularVelocity = Vector3.Zero;
+            Acceleration = Vector3.Zero;
+            m_TextureAnimation = Utils.EmptyBytes;
+            m_particleSystem = Utils.EmptyBytes;
 
             // Prims currently only contain a single folder (Contents).  From looking at the Second Life protocol,
             // this appears to have the same UUID (!) as the prim.  If this isn't the case, one can't drag items from
@@ -532,13 +537,13 @@ namespace OpenSim.Region.Framework.Scenes
                         // Root prim actually goes at Position
                         if (_parentID == 0)
                         {
-                            PhysActor.Position = new PhysicsVector(value.X, value.Y, value.Z);
+                            PhysActor.Position = value;
                         }
                         else
                         {
                             // To move the child prim in respect to the group position and rotation we have to calculate
                             Vector3 resultingposition = GetWorldPosition();
-                            PhysActor.Position = new PhysicsVector(resultingposition.X, resultingposition.Y, resultingposition.Z);
+                            PhysActor.Position = resultingposition;
                             Quaternion resultingrot = GetWorldRotation();
                             PhysActor.Orientation = resultingrot;
                         }
@@ -580,7 +585,7 @@ namespace OpenSim.Region.Framework.Scenes
                      if (_parentID != 0 && PhysActor != null)
                     {
                         Vector3 resultingposition = GetWorldPosition();
-                        PhysActor.Position = new PhysicsVector(resultingposition.X, resultingposition.Y, resultingposition.Z);
+                        PhysActor.Position = resultingposition;
                         Quaternion resultingrot = GetWorldRotation();
                         PhysActor.Orientation = resultingrot;
 
@@ -670,7 +675,7 @@ namespace OpenSim.Region.Framework.Scenes
                 {
                     if (PhysActor.IsPhysical)
                     {
-                        PhysActor.Velocity = new PhysicsVector(value.X, value.Y, value.Z);
+                        PhysActor.Velocity = value;
                         m_parentGroup.Scene.PhysicsScene.AddPhysicsActorTaint(PhysActor);
                     }
                 }
@@ -812,7 +817,7 @@ if (m_shape != null) {
                     {
                         if (m_parentGroup.Scene.PhysicsScene != null)
                         {
-                            PhysActor.Size = new PhysicsVector(m_shape.Scale.X, m_shape.Scale.Y, m_shape.Scale.Z);
+                            PhysActor.Size = m_shape.Scale;
                             m_parentGroup.Scene.PhysicsScene.AddPhysicsActorTaint(PhysActor);
                         }
                     }
@@ -1077,8 +1082,8 @@ if (m_shape != null) {
 
         private void SendObjectPropertiesToClient(UUID AgentID)
         {
-            List<ScenePresence> avatars = m_parentGroup.Scene.GetScenePresences();
-            for (int i = 0; i < avatars.Count; i++)
+            ScenePresence[] avatars = m_parentGroup.Scene.GetScenePresences();
+            for (int i = 0; i < avatars.Length; i++)
             {
                 // Ugly reference :(
                 if (avatars[i].UUID == AgentID)
@@ -1140,8 +1145,8 @@ if (m_shape != null) {
         /// </summary>
         public void AddFullUpdateToAllAvatars()
         {
-            List<ScenePresence> avatars = m_parentGroup.Scene.GetScenePresences();
-            for (int i = 0; i < avatars.Count; i++)
+            ScenePresence[] avatars = m_parentGroup.Scene.GetScenePresences();
+            for (int i = 0; i < avatars.Length; i++)
             {
                 avatars[i].SceneViewer.QueuePartForUpdate(this);
             }
@@ -1165,8 +1170,8 @@ if (m_shape != null) {
         /// Terse updates
         public void AddTerseUpdateToAllAvatars()
         {
-            List<ScenePresence> avatars = m_parentGroup.Scene.GetScenePresences();
-            for (int i = 0; i < avatars.Count; i++)
+            ScenePresence[] avatars = m_parentGroup.Scene.GetScenePresences();
+            for (int i = 0; i < avatars.Length; i++)
             {
                 avatars[i].SceneViewer.QueuePartForUpdate(this);
             }
@@ -1220,7 +1225,7 @@ if (m_shape != null) {
         /// <param name="localGlobalTF">true for the local frame, false for the global frame</param>
         public void ApplyImpulse(Vector3 impulsei, bool localGlobalTF)
         {
-            PhysicsVector impulse = new PhysicsVector(impulsei.X, impulsei.Y, impulsei.Z);
+            Vector3 impulse = impulsei;
 
             if (localGlobalTF)
             {
@@ -1228,7 +1233,7 @@ if (m_shape != null) {
                 Quaternion AXgrot = grot;
                 Vector3 AXimpulsei = impulsei;
                 Vector3 newimpulse = AXimpulsei * AXgrot;
-                impulse = new PhysicsVector(newimpulse.X, newimpulse.Y, newimpulse.Z);
+                impulse = newimpulse;
             }
 
             if (m_parentGroup != null)
@@ -1246,7 +1251,7 @@ if (m_shape != null) {
         /// <param name="localGlobalTF">true for the local frame, false for the global frame</param>
         public void ApplyAngularImpulse(Vector3 impulsei, bool localGlobalTF)
         {
-            PhysicsVector impulse = new PhysicsVector(impulsei.X, impulsei.Y, impulsei.Z);
+            Vector3 impulse = impulsei;
 
             if (localGlobalTF)
             {
@@ -1254,7 +1259,7 @@ if (m_shape != null) {
                 Quaternion AXgrot = grot;
                 Vector3 AXimpulsei = impulsei;
                 Vector3 newimpulse = AXimpulsei * AXgrot;
-                impulse = new PhysicsVector(newimpulse.X, newimpulse.Y, newimpulse.Z);
+                impulse = newimpulse;
             }
 
             if (m_parentGroup != null)
@@ -1272,7 +1277,7 @@ if (m_shape != null) {
         /// <param name="localGlobalTF">true for the local frame, false for the global frame</param>
         public void SetAngularImpulse(Vector3 impulsei, bool localGlobalTF)
         {
-            PhysicsVector impulse = new PhysicsVector(impulsei.X, impulsei.Y, impulsei.Z);
+            Vector3 impulse = impulsei;
 
             if (localGlobalTF)
             {
@@ -1280,7 +1285,7 @@ if (m_shape != null) {
                 Quaternion AXgrot = grot;
                 Vector3 AXimpulsei = impulsei;
                 Vector3 newimpulse = AXimpulsei * AXgrot;
-                impulse = new PhysicsVector(newimpulse.X, newimpulse.Y, newimpulse.Z);
+                impulse = newimpulse;
             }
 
             if (m_parentGroup != null)
@@ -1328,8 +1333,8 @@ if (m_shape != null) {
                     PhysActor = m_parentGroup.Scene.PhysicsScene.AddPrimShape(
                         Name,
                         Shape,
-                        new PhysicsVector(AbsolutePosition.X, AbsolutePosition.Y, AbsolutePosition.Z),
-                        new PhysicsVector(Scale.X, Scale.Y, Scale.Z),
+                        AbsolutePosition,
+                        Scale,
                         RotationOffset,
                         RigidBody);
 
@@ -1518,7 +1523,7 @@ if (m_shape != null) {
                     PhysicsJoint joint;
 
                     joint = m_parentGroup.Scene.PhysicsScene.RequestJointCreation(Name, jointType,
-                        new PhysicsVector(AbsolutePosition.X, AbsolutePosition.Y, AbsolutePosition.Z),
+                        AbsolutePosition,
                         this.RotationOffset,
                         Description,
                         bodyNames,
@@ -1703,12 +1708,12 @@ if (m_shape != null) {
             }
         }
 
-        public PhysicsVector GetForce()
+        public Vector3 GetForce()
         {
             if (PhysActor != null)
                 return PhysActor.Force;
             else
-                return new PhysicsVector();
+                return Vector3.Zero;
         }
 
         public void GetProperties(IClientAPI client)
@@ -1894,24 +1899,24 @@ if (m_shape != null) {
                         }
                         else
                         {
-                            List<ScenePresence> avlist = m_parentGroup.Scene.GetScenePresences();
-                            if (avlist != null)
+                            ScenePresence[] avlist = m_parentGroup.Scene.GetScenePresences();
+
+                            for (int i = 0; i < avlist.Length; i++)
                             {
-                                foreach (ScenePresence av in avlist)
+                                ScenePresence av = avlist[i];
+
+                                if (av.LocalId == localId)
                                 {
-                                    if (av.LocalId == localId)
-                                    {
-                                        DetectedObject detobj = new DetectedObject();
-                                        detobj.keyUUID = av.UUID;
-                                        detobj.nameStr = av.ControllingClient.Name;
-                                        detobj.ownerUUID = av.UUID;
-                                        detobj.posVector = av.AbsolutePosition;
-                                        detobj.rotQuat = av.Rotation;
-                                        detobj.velVector = av.Velocity;
-                                        detobj.colliderType = 0;
-                                        detobj.groupUUID = av.ControllingClient.ActiveGroupId;
-                                        colliding.Add(detobj);
-                                    }
+                                    DetectedObject detobj = new DetectedObject();
+                                    detobj.keyUUID = av.UUID;
+                                    detobj.nameStr = av.ControllingClient.Name;
+                                    detobj.ownerUUID = av.UUID;
+                                    detobj.posVector = av.AbsolutePosition;
+                                    detobj.rotQuat = av.Rotation;
+                                    detobj.velVector = av.Velocity;
+                                    detobj.colliderType = 0;
+                                    detobj.groupUUID = av.ControllingClient.ActiveGroupId;
+                                    colliding.Add(detobj);
                                 }
                             }
                         }
@@ -1965,26 +1970,25 @@ if (m_shape != null) {
                         }
                         else
                         {
-                            List<ScenePresence> avlist = m_parentGroup.Scene.GetScenePresences();
-                            if (avlist != null)
+                            ScenePresence[] avlist = m_parentGroup.Scene.GetScenePresences();
+                            
+                            for (int i = 0; i < avlist.Length; i++)
                             {
-                                foreach (ScenePresence av in avlist)
+                                ScenePresence av = avlist[i];
+
+                                if (av.LocalId == localId)
                                 {
-                                    if (av.LocalId == localId)
-                                    {
-                                        DetectedObject detobj = new DetectedObject();
-                                        detobj.keyUUID = av.UUID;
-                                        detobj.nameStr = av.Name;
-                                        detobj.ownerUUID = av.UUID;
-                                        detobj.posVector = av.AbsolutePosition;
-                                        detobj.rotQuat = av.Rotation;
-                                        detobj.velVector = av.Velocity;
-                                        detobj.colliderType = 0;
-                                        detobj.groupUUID = av.ControllingClient.ActiveGroupId;
-                                        colliding.Add(detobj);
-                                    }
+                                    DetectedObject detobj = new DetectedObject();
+                                    detobj.keyUUID = av.UUID;
+                                    detobj.nameStr = av.Name;
+                                    detobj.ownerUUID = av.UUID;
+                                    detobj.posVector = av.AbsolutePosition;
+                                    detobj.rotQuat = av.Rotation;
+                                    detobj.velVector = av.Velocity;
+                                    detobj.colliderType = 0;
+                                    detobj.groupUUID = av.ControllingClient.ActiveGroupId;
+                                    colliding.Add(detobj);
                                 }
-
                             }
                         }
                     }
@@ -2035,24 +2039,24 @@ if (m_shape != null) {
                         }
                         else
                         {
-                            List<ScenePresence> avlist = m_parentGroup.Scene.GetScenePresences();
-                            if (avlist != null)
+                            ScenePresence[] avlist = m_parentGroup.Scene.GetScenePresences();
+
+                            for (int i = 0; i < avlist.Length; i++)
                             {
-                                foreach (ScenePresence av in avlist)
+                                ScenePresence av = avlist[i];
+
+                                if (av.LocalId == localId)
                                 {
-                                    if (av.LocalId == localId)
-                                    {
-                                        DetectedObject detobj = new DetectedObject();
-                                        detobj.keyUUID = av.UUID;
-                                        detobj.nameStr = av.Name;
-                                        detobj.ownerUUID = av.UUID;
-                                        detobj.posVector = av.AbsolutePosition;
-                                        detobj.rotQuat = av.Rotation;
-                                        detobj.velVector = av.Velocity;
-                                        detobj.colliderType = 0;
-                                        detobj.groupUUID = av.ControllingClient.ActiveGroupId;
-                                        colliding.Add(detobj);
-                                    }
+                                    DetectedObject detobj = new DetectedObject();
+                                    detobj.keyUUID = av.UUID;
+                                    detobj.nameStr = av.Name;
+                                    detobj.ownerUUID = av.UUID;
+                                    detobj.posVector = av.AbsolutePosition;
+                                    detobj.rotQuat = av.Rotation;
+                                    detobj.velVector = av.Velocity;
+                                    detobj.colliderType = 0;
+                                    detobj.groupUUID = av.ControllingClient.ActiveGroupId;
+                                    colliding.Add(detobj);
                                 }
                             }
                         }
@@ -2074,7 +2078,7 @@ if (m_shape != null) {
             }
         }
 
-        public void PhysicsOutOfBounds(PhysicsVector pos)
+        public void PhysicsOutOfBounds(Vector3 pos)
         {
             m_log.Error("[PHYSICS]: Physical Object went out of bounds.");
             
@@ -2312,8 +2316,8 @@ if (m_shape != null) {
         /// </summary>
         public void SendFullUpdateToAllClients()
         {
-            List<ScenePresence> avatars = m_parentGroup.Scene.GetScenePresences();
-            for (int i = 0; i < avatars.Count; i++)
+            ScenePresence[] avatars = m_parentGroup.Scene.GetScenePresences();
+            for (int i = 0; i < avatars.Length; i++)
             {
                 // Ugly reference :(
                 m_parentGroup.SendPartFullUpdate(avatars[i].ControllingClient, this,
@@ -2323,8 +2327,8 @@ if (m_shape != null) {
 
         public void SendFullUpdateToAllClientsExcept(UUID agentID)
         {
-            List<ScenePresence> avatars = m_parentGroup.Scene.GetScenePresences();
-            for (int i = 0; i < avatars.Count; i++)
+            ScenePresence[] avatars = m_parentGroup.Scene.GetScenePresences();
+            for (int i = 0; i < avatars.Length; i++)
             {
                 // Ugly reference :(
                 if (avatars[i].UUID != agentID)
@@ -2389,18 +2393,36 @@ if (m_shape != null) {
         /// </summary>
         public void SendScheduledUpdates()
         {
-            if (m_updateFlag == 1) //some change has been made so update the clients
+            const float VELOCITY_TOLERANCE = 0.01f;
+            const float POSITION_TOLERANCE = 10.0f;
+
+            if (m_updateFlag == 1)
             {
-                AddTerseUpdateToAllAvatars();
-                ClearUpdateSchedule();
+                // Throw away duplicate or insignificant updates
+                if (RotationOffset != m_lastRotation ||
+                    Acceleration != m_lastAcceleration ||
+                    (Velocity - m_lastVelocity).Length() > VELOCITY_TOLERANCE ||
+                    (RotationalVelocity - m_lastAngularVelocity).Length() > VELOCITY_TOLERANCE ||
+                    (OffsetPosition - m_lastPosition).Length() > POSITION_TOLERANCE)
+                {
+                    AddTerseUpdateToAllAvatars();
+                    ClearUpdateSchedule();
 
-                // This causes the Scene to 'poll' physical objects every couple of frames
-                // bad, so it's been replaced by an event driven method.
-                //if ((ObjectFlags & (uint)PrimFlags.Physics) != 0)
-                //{
-                // Only send the constant terse updates on physical objects!
-                //ScheduleTerseUpdate();
-                //}
+                    // This causes the Scene to 'poll' physical objects every couple of frames
+                    // bad, so it's been replaced by an event driven method.
+                    //if ((ObjectFlags & (uint)PrimFlags.Physics) != 0)
+                    //{
+                    // Only send the constant terse updates on physical objects!
+                    //ScheduleTerseUpdate();
+                    //}
+
+                    // Update the "last" values
+                    m_lastPosition = OffsetPosition;
+                    m_lastRotation = RotationOffset;
+                    m_lastVelocity = Velocity;
+                    m_lastAcceleration = Acceleration;
+                    m_lastAngularVelocity = RotationalVelocity;
+                }
             }
             else
             {
@@ -2467,8 +2489,8 @@ if (m_shape != null) {
         /// </summary>
         public void SendTerseUpdateToAllClients()
         {
-            List<ScenePresence> avatars = m_parentGroup.Scene.GetScenePresences();
-            for (int i = 0; i < avatars.Count; i++)
+            ScenePresence[] avatars = m_parentGroup.Scene.GetScenePresences();
+            for (int i = 0; i < avatars.Length; i++)
             {
                 SendTerseUpdateToClient(avatars[i].ControllingClient);
             }
@@ -2542,7 +2564,7 @@ if (m_shape != null) {
             }
         }
 
-        public void SetForce(PhysicsVector force)
+        public void SetForce(Vector3 force)
         {
             if (PhysActor != null)
             {
@@ -2566,7 +2588,7 @@ if (m_shape != null) {
             }
         }
 
-        public void SetVehicleVectorParam(int param, PhysicsVector value)
+        public void SetVehicleVectorParam(int param, Vector3 value)
         {
             if (PhysActor != null)
             {
@@ -3408,8 +3430,8 @@ if (m_shape != null) {
                     PhysActor = m_parentGroup.Scene.PhysicsScene.AddPrimShape(
                         Name,
                         Shape,
-                        new PhysicsVector(AbsolutePosition.X, AbsolutePosition.Y, AbsolutePosition.Z),
-                        new PhysicsVector(Scale.X, Scale.Y, Scale.Z),
+                        AbsolutePosition,
+                        Scale,
                         RotationOffset,
                         UsePhysics);
 
@@ -3549,7 +3571,7 @@ if (m_shape != null) {
             // in SL.
             //
             if (ParentGroup.RootPart != this)
-                ParentGroup.RootPart.Rezzed = DateTime.Now;
+                ParentGroup.RootPart.Rezzed = DateTime.UtcNow;
 
             ParentGroup.HasGroupChanged = true;
             ScheduleFullUpdate();
diff --git a/OpenSim/Region/Framework/Scenes/ScenePresence.cs b/OpenSim/Region/Framework/Scenes/ScenePresence.cs
index d7113bf..87fac0c 100644
--- a/OpenSim/Region/Framework/Scenes/ScenePresence.cs
+++ b/OpenSim/Region/Framework/Scenes/ScenePresence.cs
@@ -93,12 +93,13 @@ namespace OpenSim.Region.Framework.Scenes
         public Vector3 lastKnownAllowedPosition;
         public bool sentMessageAboutRestrictedParcelFlyingDown;
 
-        
+        private Vector3 m_lastPosition;
+        private Quaternion m_lastRotation;
+        private Vector3 m_lastVelocity;
 
         private bool m_updateflag;
         private byte m_movementflag;
         private readonly List<NewForce> m_forcesList = new List<NewForce>();
-        private short m_updateCount;
         private uint m_requestedSitTargetID;
         private UUID m_requestedSitTargetUUID = UUID.Zero;
         private SendCourseLocationsMethod m_sendCourseLocationsMethod;
@@ -145,12 +146,9 @@ namespace OpenSim.Region.Framework.Scenes
         public string JID = string.Empty;
 
         // Agent moves with a PID controller causing a force to be exerted.
-        private bool m_newForce;
         private bool m_newCoarseLocations = true;
         private float m_health = 100f;
 
-        private Vector3 m_lastVelocity = Vector3.Zero;
-
         // Default AV Height
         private float m_avHeight = 127.0f;
 
@@ -158,16 +156,6 @@ namespace OpenSim.Region.Framework.Scenes
         protected ulong crossingFromRegion;
 
         private readonly Vector3[] Dir_Vectors = new Vector3[6];
-        
-        /// <value>
-        /// The avatar position last sent to clients
-        /// </value>
-        private Vector3 lastPhysPos = Vector3.Zero;
-        
-        /// <value>
-        /// The avatar body rotation last sent to clients 
-        /// </value>
-        private Quaternion lastPhysRot = Quaternion.Identity;
 
         // Position of agent's camera in world (region cordinates)
         protected Vector3 m_CameraCenter = Vector3.Zero;
@@ -297,6 +285,21 @@ namespace OpenSim.Region.Framework.Scenes
             get { return Util.Axes2Rot(m_CameraAtAxis, m_CameraLeftAxis, m_CameraUpAxis); }
         }
 
+        public Vector3 CameraAtAxis
+        {
+            get { return m_CameraAtAxis; }
+        }
+
+        public Vector3 CameraLeftAxis
+        {
+            get { return m_CameraLeftAxis; }
+        }
+
+        public Vector3 CameraUpAxis
+        {
+            get { return m_CameraUpAxis; }
+        }
+
         public Vector3 Lookat
         {
             get
@@ -431,7 +434,7 @@ namespace OpenSim.Region.Framework.Scenes
                     {
                         lock (m_scene.SyncRoot)
                         {
-                            m_physicsActor.Position = new PhysicsVector(value.X, value.Y, value.Z);
+                            m_physicsActor.Position = value;
                         }
                     }
                     catch (Exception e)
@@ -471,7 +474,7 @@ namespace OpenSim.Region.Framework.Scenes
                     {
                         lock (m_scene.SyncRoot)
                         {
-                            m_physicsActor.Velocity = new PhysicsVector(value.X, value.Y, value.Z);
+                            m_physicsActor.Velocity = value;
                         }
                     }
                     catch (Exception e)
@@ -869,14 +872,16 @@ namespace OpenSim.Region.Framework.Scenes
 
             m_isChildAgent = false;
 
-            List<ScenePresence> AnimAgents = m_scene.GetScenePresences();
-            foreach (ScenePresence p in AnimAgents)
+            ScenePresence[] animAgents = m_scene.GetScenePresences();
+            for (int i = 0; i < animAgents.Length; i++)
             {
-                if (p != this)
-                    p.SendAnimPackToClient(ControllingClient);
+                ScenePresence presence = animAgents[i];
+
+                if (presence != this)
+                    presence.SendAnimPackToClient(ControllingClient);
             }
-            m_scene.EventManager.TriggerOnMakeRootAgent(this);
 
+            m_scene.EventManager.TriggerOnMakeRootAgent(this);
         }
 
         /// <summary>
@@ -1041,7 +1046,7 @@ namespace OpenSim.Region.Framework.Scenes
             m_avHeight = height;
             if (PhysicsActor != null && !IsChildAgent)
             {
-                PhysicsVector SetSize = new PhysicsVector(0.45f, 0.6f, m_avHeight);
+                Vector3 SetSize = new Vector3(0.45f, 0.6f, m_avHeight);
                 PhysicsActor.Size = SetSize;
             }
         }
@@ -1106,18 +1111,18 @@ namespace OpenSim.Region.Framework.Scenes
                     CameraConstraintActive = true;
                     //m_log.DebugFormat("[RAYCASTRESULT]: {0}, {1}, {2}, {3}", hitYN, collisionPoint, localid, distance);
                     
-                    Vector3 normal = Vector3.Normalize(new Vector3(0,0,collisionPoint.Z) - collisionPoint);
+                    Vector3 normal = Vector3.Normalize(new Vector3(0f, 0f, collisionPoint.Z) - collisionPoint);
                     ControllingClient.SendCameraConstraint(new Vector4(normal.X, normal.Y, normal.Z, -1 * Vector3.Distance(new Vector3(0,0,collisionPoint.Z),collisionPoint)));
                 }
                 else 
                 {
-                    if (((Util.GetDistanceTo(lastPhysPos, AbsolutePosition) > 0.02)
-                         || (Util.GetDistanceTo(m_lastVelocity, m_velocity) > 0.02)
-                         || lastPhysRot != m_bodyRot))
+                    if ((m_pos - m_lastPosition).Length() > 0.02f ||
+                        (m_velocity - m_lastVelocity).Length() > 0.02f ||
+                        m_bodyRot != m_lastRotation)
                     {
                         if (CameraConstraintActive)
                         {
-                            ControllingClient.SendCameraConstraint(new Vector4(0, 0.5f, 0.9f, -3000f));
+                            ControllingClient.SendCameraConstraint(new Vector4(0f, 0.5f, 0.9f, -3000f));
                             CameraConstraintActive = false;
                         }
                     }
@@ -2356,6 +2361,9 @@ namespace OpenSim.Region.Framework.Scenes
 
         public override void Update()
         {
+            const float VELOCITY_TOLERANCE = 0.01f;
+            const float POSITION_TOLERANCE = 10.0f;
+
             SendPrimUpdates();
 
             if (m_newCoarseLocations)
@@ -2366,28 +2374,17 @@ namespace OpenSim.Region.Framework.Scenes
 
             if (m_isChildAgent == false)
             {
-                if (m_newForce) // user movement 'forces' (ie commands to move)
-                {
-                    SendTerseUpdateToAllClients();
-                    m_updateCount = 0;
-                }
-                else if (m_movementflag != 0) // scripted movement (?)
-                {
-                    m_updateCount++;
-                    if (m_updateCount > 3)
-                    {
-                        SendTerseUpdateToAllClients();
-                        m_updateCount = 0;
-                    }
-                }
-                else if ((Util.GetDistanceTo(lastPhysPos, AbsolutePosition) > 0.02) 
-                         || (Util.GetDistanceTo(m_lastVelocity, m_velocity) > 0.02)
-                         || lastPhysRot != m_bodyRot)
+                // Throw away duplicate or insignificant updates
+                if (m_bodyRot != m_lastRotation ||
+                    (m_velocity - m_lastVelocity).Length() > VELOCITY_TOLERANCE ||
+                    (m_pos - m_lastPosition).Length() > POSITION_TOLERANCE)
                 {
-                    // Send Terse Update to all clients updates lastPhysPos and m_lastVelocity
-                    // doing the above assures us that we know what we sent the clients last
                     SendTerseUpdateToAllClients();
-                    m_updateCount = 0;
+
+                    // Update the "last" values
+                    m_lastPosition = m_pos;
+                    m_lastRotation = m_bodyRot;
+                    m_lastVelocity = m_velocity;
                 }
 
                 // followed suggestion from mic bowman. reversed the two lines below.
@@ -2417,7 +2414,7 @@ namespace OpenSim.Region.Framework.Scenes
                 pos.Z -= m_appearance.HipOffset;
 
                 remoteClient.SendAvatarTerseUpdate(new SendAvatarTerseData(m_regionHandle, (ushort)(m_scene.TimeDilation * ushort.MaxValue), LocalId,
-                    pos, m_velocity, Vector3.Zero, m_rotation, Vector4.Zero, m_uuid, null, GetUpdatePriority(remoteClient)));
+                    pos, m_velocity, Vector3.Zero, m_bodyRot, Vector4.UnitW, m_uuid, null, GetUpdatePriority(remoteClient)));
 
                 m_scene.StatsReporter.AddAgentTime(Environment.TickCount - m_perfMonMS);
                 m_scene.StatsReporter.AddAgentUpdates(1);
@@ -2430,15 +2427,10 @@ namespace OpenSim.Region.Framework.Scenes
         public void SendTerseUpdateToAllClients()
         {
             m_perfMonMS = Environment.TickCount;
-
+            
             m_scene.ForEachClient(SendTerseUpdateToClient);
 
-            m_lastVelocity = m_velocity;
-            lastPhysPos = AbsolutePosition;
-            lastPhysRot = m_bodyRot;
-
             m_scene.StatsReporter.AddAgentTime(Environment.TickCount - m_perfMonMS);
-
         }
 
         public void SendCoarseLocations()
@@ -2517,15 +2509,12 @@ namespace OpenSim.Region.Framework.Scenes
             if (m_appearance.Texture == null)
                 return;
 
-            // Note: because Quaternion is a struct, it can't be null
-            Quaternion rot = m_bodyRot;
-
             Vector3 pos = m_pos;
             pos.Z -= m_appearance.HipOffset;
 
             remoteAvatar.m_controllingClient.SendAvatarData(new SendAvatarData(m_regionInfo.RegionHandle, m_firstname, m_lastname, m_grouptitle, m_uuid,
-                                                            LocalId, m_pos, m_appearance.Texture.GetBytes(),
-                                                            m_parentID, rot));
+                                                            LocalId, pos, m_appearance.Texture.GetBytes(),
+                                                            m_parentID, m_bodyRot));
             m_scene.StatsReporter.AddAgentUpdates(1);
         }
 
@@ -2536,9 +2525,12 @@ namespace OpenSim.Region.Framework.Scenes
         {
             m_perfMonMS = Environment.TickCount;
 
-            List<ScenePresence> avatars = m_scene.GetScenePresences();
-            foreach (ScenePresence avatar in avatars)
+            ScenePresence[] avatars = m_scene.GetScenePresences();
+
+            for (int i = 0; i < avatars.Length; i++)
             {
+                ScenePresence avatar = avatars[i];
+
                 // only send if this is the root (children are only "listening posts" in a foreign region)
                 if (!IsChildAgent)
                 {
@@ -2556,7 +2548,7 @@ namespace OpenSim.Region.Framework.Scenes
                 }
             }
 
-            m_scene.StatsReporter.AddAgentUpdates(avatars.Count);
+            m_scene.StatsReporter.AddAgentUpdates(avatars.Length);
             m_scene.StatsReporter.AddAgentTime(Environment.TickCount - m_perfMonMS);
 
             //SendAnimPack();
@@ -2588,14 +2580,11 @@ namespace OpenSim.Region.Framework.Scenes
             // the inventory arrives
             // m_scene.GetAvatarAppearance(m_controllingClient, out m_appearance);
 
-            // Note: because Quaternion is a struct, it can't be null
-            Quaternion rot = m_bodyRot;
-
             Vector3 pos = m_pos;
             pos.Z -= m_appearance.HipOffset;
 
             m_controllingClient.SendAvatarData(new SendAvatarData(m_regionInfo.RegionHandle, m_firstname, m_lastname, m_grouptitle, m_uuid, LocalId,
-                                               m_pos, m_appearance.Texture.GetBytes(), m_parentID, rot));
+                                               pos, m_appearance.Texture.GetBytes(), m_parentID, m_bodyRot));
 
             if (!m_isChildAgent)
             {
@@ -2679,7 +2668,7 @@ namespace OpenSim.Region.Framework.Scenes
                     {
                         if (m_scene.AssetService.Get(face.TextureID.ToString()) == null)
                         {
-                            m_log.Warn("[APPEARANCE]: Missing baked texture " + face.TextureID + " (" + (AppearanceManager.TextureIndex)j + ") for avatar " + this.Name);
+                            m_log.Warn("[APPEARANCE]: Missing baked texture " + face.TextureID + " (" + j + ") for avatar " + this.Name);
                             this.ControllingClient.SendRebakeAvatarTextures(face.TextureID);
                         }
                     }
@@ -2700,9 +2689,11 @@ namespace OpenSim.Region.Framework.Scenes
                 m_startAnimationSet = true;
             }
 
-            Quaternion rot = m_bodyRot;
+            Vector3 pos = m_pos;
+            pos.Z -= m_appearance.HipOffset;
+
             m_controllingClient.SendAvatarData(new SendAvatarData(m_regionInfo.RegionHandle, m_firstname, m_lastname, m_grouptitle, m_uuid, LocalId,
-                m_pos, m_appearance.Texture.GetBytes(), m_parentID, rot));
+                pos, m_appearance.Texture.GetBytes(), m_parentID, m_bodyRot));
 
         }
 
@@ -3300,7 +3291,6 @@ namespace OpenSim.Region.Framework.Scenes
         /// </summary>
         public override void UpdateMovement()
         {
-            m_newForce = false;
             lock (m_forcesList)
             {
                 if (m_forcesList.Count > 0)
@@ -3322,7 +3312,6 @@ namespace OpenSim.Region.Framework.Scenes
                         // Ignoring this causes no movement to be sent to the physics engine...
                         // which when the scene is moving at 1 frame every 10 seconds, it doesn't really matter!
                     }
-                    m_newForce = true;
 
                     m_forcesList.Clear();
                 }
@@ -3356,20 +3345,18 @@ namespace OpenSim.Region.Framework.Scenes
             
             PhysicsScene scene = m_scene.PhysicsScene;
 
-            PhysicsVector pVec =
-                new PhysicsVector(AbsolutePosition.X, AbsolutePosition.Y,
-                                  AbsolutePosition.Z);
+            Vector3 pVec = AbsolutePosition;
 
             // Old bug where the height was in centimeters instead of meters
             if (m_avHeight == 127.0f)
             {
-                m_physicsActor = scene.AddAvatar(Firstname + "." + Lastname, pVec, new PhysicsVector(0, 0, 1.56f),
+                m_physicsActor = scene.AddAvatar(Firstname + "." + Lastname, pVec, new Vector3(0f, 0f, 1.56f),
                                                  isFlying);
             }
             else
             {
                 m_physicsActor = scene.AddAvatar(Firstname + "." + Lastname, pVec,
-                                                 new PhysicsVector(0, 0, m_avHeight), isFlying);
+                                                 new Vector3(0f, 0f, m_avHeight), isFlying);
             }
             scene.AddPhysicsActorTaint(m_physicsActor);
             //m_physicsActor.OnRequestTerseUpdate += SendTerseUpdateToAllClients;
@@ -3380,7 +3367,7 @@ namespace OpenSim.Region.Framework.Scenes
             
         }
 
-        private void OutOfBoundsCall(PhysicsVector pos)
+        private void OutOfBoundsCall(Vector3 pos)
         {
             //bool flying = m_physicsActor.Flying;
             //RemoveFromPhysicalScene();
@@ -3603,7 +3590,7 @@ namespace OpenSim.Region.Framework.Scenes
             */
         }
 
-        internal void PushForce(PhysicsVector impulse)
+        internal void PushForce(Vector3 impulse)
         {
             if (PhysicsActor != null)
             {
@@ -3866,6 +3853,8 @@ namespace OpenSim.Region.Framework.Scenes
                     return GetPriorityByDistance(client);
                 case Scene.UpdatePrioritizationSchemes.SimpleAngularDistance:
                     return GetPriorityByDistance(client);
+                case Scenes.Scene.UpdatePrioritizationSchemes.FrontBack:
+                    return GetPriorityByFrontBack(client);
                 default:
                     throw new InvalidOperationException("UpdatePrioritizationScheme not defined.");
             }
@@ -3887,11 +3876,34 @@ namespace OpenSim.Region.Framework.Scenes
             return double.NaN;
         }
 
+        private double GetPriorityByFrontBack(IClientAPI client)
+        {
+            ScenePresence presence = Scene.GetScenePresence(client.AgentId);
+            if (presence != null)
+            {
+                return GetPriorityByFrontBack(presence.CameraPosition, presence.CameraAtAxis);
+            }
+            return double.NaN;
+        }
+
         private double GetPriorityByDistance(Vector3 position)
         {
             return Vector3.Distance(AbsolutePosition, position);
         }
 
+        private double GetPriorityByFrontBack(Vector3 camPosition, Vector3 camAtAxis)
+        {
+            // Distance
+            double priority = Vector3.Distance(camPosition, AbsolutePosition);
+
+            // Plane equation
+            float d = -Vector3.Dot(camPosition, camAtAxis);
+            float p = Vector3.Dot(camAtAxis, AbsolutePosition) + d;
+            if (p < 0.0f) priority *= 2.0f;
+
+            return priority;
+        }
+
         private double GetSOGUpdatePriority(SceneObjectGroup sog)
         {
             switch (Scene.UpdatePrioritizationScheme)
@@ -3902,6 +3914,8 @@ namespace OpenSim.Region.Framework.Scenes
                     return sog.GetPriorityByDistance((IsChildAgent) ? AbsolutePosition : CameraPosition);
                 case Scene.UpdatePrioritizationSchemes.SimpleAngularDistance:
                     return sog.GetPriorityBySimpleAngularDistance((IsChildAgent) ? AbsolutePosition : CameraPosition);
+                case Scenes.Scene.UpdatePrioritizationSchemes.FrontBack:
+                    return sog.GetPriorityByFrontBack(CameraPosition, CameraAtAxis);
                 default:
                     throw new InvalidOperationException("UpdatePrioritizationScheme not defined");
             }
@@ -3928,6 +3942,8 @@ namespace OpenSim.Region.Framework.Scenes
                     case Scene.UpdatePrioritizationSchemes.Distance:
                     case Scene.UpdatePrioritizationSchemes.SimpleAngularDistance:
                         return GetPriorityByDistance((IsChildAgent) ? AbsolutePosition : CameraPosition);
+                    case Scenes.Scene.UpdatePrioritizationSchemes.FrontBack:
+                        return GetPriorityByFrontBack(CameraPosition, CameraAtAxis);
                     default:
                         throw new InvalidOperationException("UpdatePrioritizationScheme not defined");
                 }