48 files changed, 12812 insertions, 1641 deletions

diff --git a/OpenSim/ApplicationPlugins/RemoteController/RemoteAdminPlugin.cs b/OpenSim/ApplicationPlugins/RemoteController/RemoteAdminPlugin.cs
index c59ea28..0c1e3ca 100644
--- a/OpenSim/ApplicationPlugins/RemoteController/RemoteAdminPlugin.cs
+++ b/OpenSim/ApplicationPlugins/RemoteController/RemoteAdminPlugin.cs
@@ -125,6 +125,7 @@ namespace OpenSim.ApplicationPlugins.RemoteController
                     availableMethods["admin_region_query"] = XmlRpcRegionQueryMethod;
                     availableMethods["admin_shutdown"] = XmlRpcShutdownMethod;
                     availableMethods["admin_broadcast"] = XmlRpcAlertMethod;
+                    availableMethods["admin_dialog"] = XmlRpcDialogMethod;
                     availableMethods["admin_restart"] = XmlRpcRestartMethod;
                     availableMethods["admin_load_heightmap"] = XmlRpcLoadHeightmapMethod;
                     // User management
@@ -214,9 +215,59 @@ namespace OpenSim.ApplicationPlugins.RemoteController
                 if (!m_app.SceneManager.TryGetScene(regionID, out rebootedScene))
                     throw new Exception("region not found");
 
+                int timeout = 30000;
+                string message;
+
+                if (requestData.ContainsKey("restart")
+                    && ((string)requestData["restart"] == "delayed")
+                    && requestData.ContainsKey("milliseconds"))
+                {
+                    timeout = Int32.Parse(requestData["milliseconds"].ToString());
+
+                    if (timeout < 15000)
+                    {
+                        //It must be at least 15 seconds or we'll cancel the reboot request
+                        timeout = 15000;
+                    }
+
+                    message
+                        = "Region is restarting in " + ((int)(timeout / 1000)).ToString()
+                          + " second(s). Please save what you are doing and log out.";
+                }
+                else
+                {
+                    message = "Region is restarting in 30 second(s). Please save what you are doing and log out.";
+                }
+
+                if (requestData.ContainsKey("noticetype")
+                    && ((string)requestData["noticetype"] == "dialog"))
+                {
+                    m_app.SceneManager.ForEachScene(
+                    delegate(Scene scene)
+                    {
+                        IDialogModule dialogModule = scene.RequestModuleInterface<IDialogModule>();
+                        if (dialogModule != null)
+                            dialogModule.SendNotificationToUsersInRegion(UUID.Zero, "System", message);
+                    });
+                }
+                else
+                {
+                    if (!requestData.ContainsKey("noticetype")
+                    || ((string)requestData["noticetype"] != "none"))
+                    {
+                        m_app.SceneManager.ForEachScene(
+                        delegate(Scene scene)
+                        {
+                            IDialogModule dialogModule = scene.RequestModuleInterface<IDialogModule>();
+                            if (dialogModule != null)
+                                dialogModule.SendGeneralAlert(message);
+                        });
+                    }
+                }
+
                 responseData["rebooting"] = true;
                 response.Value = responseData;
-                rebootedScene.Restart(30);
+                rebootedScene.Restart(timeout / 1000,false);
             }
             catch (Exception e)
             {
@@ -279,6 +330,53 @@ namespace OpenSim.ApplicationPlugins.RemoteController
             m_log.Info("[RADMIN]: Alert request complete");
             return response;
         }
+        public XmlRpcResponse XmlRpcDialogMethod(XmlRpcRequest request, IPEndPoint remoteClient)
+        {
+            XmlRpcResponse response = new XmlRpcResponse();
+            Hashtable responseData = new Hashtable();
+
+            m_log.Info("[RADMIN]: Dialog request started");
+
+            try
+            {
+                Hashtable requestData = (Hashtable)request.Params[0];
+
+                checkStringParameters(request, new string[] { "password", "from", "message" });
+
+                if (m_requiredPassword != String.Empty &&
+                    (!requestData.Contains("password") || (string)requestData["password"] != m_requiredPassword))
+                    throw new Exception("wrong password");
+
+                string message = (string)requestData["message"];
+                string fromuuid = (string)requestData["from"];
+                m_log.InfoFormat("[RADMIN]: Broadcasting: {0}", message);
+
+                responseData["accepted"] = true;
+                responseData["success"] = true;
+                response.Value = responseData;
+
+                m_app.SceneManager.ForEachScene(
+                    delegate(Scene scene)
+                    {
+                        IDialogModule dialogModule = scene.RequestModuleInterface<IDialogModule>();
+                        if (dialogModule != null)
+                            dialogModule.SendNotificationToUsersInRegion(UUID.Zero, fromuuid, message);
+                    });
+            }
+            catch (Exception e)
+            {
+                m_log.ErrorFormat("[RADMIN]: Broadcasting: failed: {0}", e.Message);
+                m_log.DebugFormat("[RADMIN]: Broadcasting: failed: {0}", e.ToString());
+
+                responseData["accepted"] = false;
+                responseData["success"] = false;
+                responseData["error"] = e.Message;
+                response.Value = responseData;
+            }
+
+            m_log.Info("[RADMIN]: Alert request complete");
+            return response;
+        }
 
         public XmlRpcResponse XmlRpcLoadHeightmapMethod(XmlRpcRequest request, IPEndPoint remoteClient)
         {
@@ -386,13 +484,33 @@ namespace OpenSim.ApplicationPlugins.RemoteController
                     message = "Region is going down now.";
                 }
 
-                m_app.SceneManager.ForEachScene(
+                if (requestData.ContainsKey("noticetype")
+                    && ((string) requestData["noticetype"] == "dialog"))
+                {
+                    m_app.SceneManager.ForEachScene(
                     delegate(Scene scene)
                         {
                             IDialogModule dialogModule = scene.RequestModuleInterface<IDialogModule>();
                             if (dialogModule != null)
+                                dialogModule.SendNotificationToUsersInRegion(UUID.Zero, "System", message);
+                        });
+                }
+                else
+                {
+                    if (!requestData.ContainsKey("noticetype")
+                    || ((string)requestData["noticetype"] != "none"))
+                    {
+                        m_app.SceneManager.ForEachScene(
+                        delegate(Scene scene)
+                        {
+                            IDialogModule dialogModule = scene.RequestModuleInterface<IDialogModule>();
+                            if (dialogModule != null)
                                 dialogModule.SendGeneralAlert(message);
                         });
+                    }
+                }
+
+                
 
                 // Perform shutdown
                 System.Timers.Timer shutdownTimer = new System.Timers.Timer(timeout); // Wait before firing
diff --git a/OpenSim/Data/MySQL/Resources/032_RegionStore.sql b/OpenSim/Data/MySQL/Resources/032_RegionStore.sql
index dca5de7..02ac1f5 100644
--- a/OpenSim/Data/MySQL/Resources/032_RegionStore.sql
+++ b/OpenSim/Data/MySQL/Resources/032_RegionStore.sql
@@ -1,3 +1,71 @@
 BEGIN;
+
+CREATE TABLE `regionwindlight` (
+  `region_id` varchar(36) NOT NULL DEFAULT '000000-0000-0000-0000-000000000000',
+  `water_color_r` float(9,6) unsigned NOT NULL DEFAULT '4.000000',
+  `water_color_g` float(9,6) unsigned NOT NULL DEFAULT '38.000000',
+  `water_color_b` float(9,6) unsigned NOT NULL DEFAULT '64.000000',
+  `water_fog_density_exponent` float(3,1) unsigned NOT NULL DEFAULT '4.0',
+  `underwater_fog_modifier` float(3,2) unsigned NOT NULL DEFAULT '0.25',
+  `reflection_wavelet_scale_1` float(3,1) unsigned NOT NULL DEFAULT '2.0',
+  `reflection_wavelet_scale_2` float(3,1) unsigned NOT NULL DEFAULT '2.0',
+  `reflection_wavelet_scale_3` float(3,1) unsigned NOT NULL DEFAULT '2.0',
+  `fresnel_scale` float(3,2) unsigned NOT NULL DEFAULT '0.40',
+  `fresnel_offset` float(3,2) unsigned NOT NULL DEFAULT '0.50',
+  `refract_scale_above` float(3,2) unsigned NOT NULL DEFAULT '0.03',
+  `refract_scale_below` float(3,2) unsigned NOT NULL DEFAULT '0.20',
+  `blur_multiplier` float(4,3) unsigned NOT NULL DEFAULT '0.040',
+  `big_wave_direction_x` float(3,2) NOT NULL DEFAULT '1.05',
+  `big_wave_direction_y` float(3,2) NOT NULL DEFAULT '-0.42',
+  `little_wave_direction_x` float(3,2) NOT NULL DEFAULT '1.11',
+  `little_wave_direction_y` float(3,2) NOT NULL DEFAULT '-1.16',
+  `normal_map_texture` varchar(36) NOT NULL DEFAULT '822ded49-9a6c-f61c-cb89-6df54f42cdf4',
+  `horizon_r` float(3,2) unsigned NOT NULL DEFAULT '0.25',
+  `horizon_g` float(3,2) unsigned NOT NULL DEFAULT '0.25',
+  `horizon_b` float(3,2) unsigned NOT NULL DEFAULT '0.32',
+  `horizon_i` float(3,2) unsigned NOT NULL DEFAULT '0.32',
+  `haze_horizon` float(3,2) unsigned NOT NULL DEFAULT '0.19',
+  `blue_density_r` float(3,2) unsigned NOT NULL DEFAULT '0.12',
+  `blue_density_g` float(3,2) unsigned NOT NULL DEFAULT '0.22',
+  `blue_density_b` float(3,2) unsigned NOT NULL DEFAULT '0.38',
+  `blue_density_i` float(3,2) unsigned NOT NULL DEFAULT '0.38',
+  `haze_density` float(3,2) unsigned NOT NULL DEFAULT '0.70',
+  `density_multiplier` float(3,2) unsigned NOT NULL DEFAULT '0.18',
+  `distance_multiplier` float(4,1) unsigned NOT NULL DEFAULT '0.8',
+  `max_altitude` int(4) unsigned NOT NULL DEFAULT '1605',
+  `sun_moon_color_r` float(3,2) unsigned NOT NULL DEFAULT '0.24',
+  `sun_moon_color_g` float(3,2) unsigned NOT NULL DEFAULT '0.26',
+  `sun_moon_color_b` float(3,2) unsigned NOT NULL DEFAULT '0.30',
+  `sun_moon_color_i` float(3,2) unsigned NOT NULL DEFAULT '0.30',
+  `sun_moon_position` float(4,3) unsigned NOT NULL DEFAULT '0.317',
+  `ambient_r` float(3,2) unsigned NOT NULL DEFAULT '0.35',
+  `ambient_g` float(3,2) unsigned NOT NULL DEFAULT '0.35',
+  `ambient_b` float(3,2) unsigned NOT NULL DEFAULT '0.35',
+  `ambient_i` float(3,2) unsigned NOT NULL DEFAULT '0.35',
+  `east_angle` float(3,2) unsigned NOT NULL DEFAULT '0.00',
+  `sun_glow_focus` float(3,2) unsigned NOT NULL DEFAULT '0.10',
+  `sun_glow_size` float(3,2) unsigned NOT NULL DEFAULT '1.75',
+  `scene_gamma` float(4,2) unsigned NOT NULL DEFAULT '1.00',
+  `star_brightness` float(3,2) unsigned NOT NULL DEFAULT '0.00',
+  `cloud_color_r` float(3,2) unsigned NOT NULL DEFAULT '0.41',
+  `cloud_color_g` float(3,2) unsigned NOT NULL DEFAULT '0.41',
+  `cloud_color_b` float(3,2) unsigned NOT NULL DEFAULT '0.41',
+  `cloud_color_i` float(3,2) unsigned NOT NULL DEFAULT '0.41',
+  `cloud_x` float(3,2) unsigned NOT NULL DEFAULT '1.00',
+  `cloud_y` float(3,2) unsigned NOT NULL DEFAULT '0.53',
+  `cloud_density` float(3,2) unsigned NOT NULL DEFAULT '1.00',
+  `cloud_coverage` float(3,2) unsigned NOT NULL DEFAULT '0.27',
+  `cloud_scale` float(3,2) unsigned NOT NULL DEFAULT '0.42',
+  `cloud_detail_x` float(3,2) unsigned NOT NULL DEFAULT '1.00',
+  `cloud_detail_y` float(3,2) unsigned NOT NULL DEFAULT '0.53',
+  `cloud_detail_density` float(3,2) unsigned NOT NULL DEFAULT '0.12',
+  `cloud_scroll_x` float(3,2) unsigned NOT NULL DEFAULT '0.20',
+  `cloud_scroll_x_lock` tinyint(1) unsigned NOT NULL DEFAULT '0',
+  `cloud_scroll_y` float(3,2) unsigned NOT NULL DEFAULT '0.01',
+  `cloud_scroll_y_lock` tinyint(1) unsigned NOT NULL DEFAULT '0',
+  `draw_classic_clouds` tinyint(1) unsigned NOT NULL DEFAULT '1',
+  PRIMARY KEY (`region_id`)
+);
+
 ALTER TABLE estate_settings AUTO_INCREMENT = 100;
 COMMIT;
diff --git a/OpenSim/Framework/Servers/VersionInfo.cs b/OpenSim/Framework/Servers/VersionInfo.cs
index 90b5f57..cd2f384 100644
--- a/OpenSim/Framework/Servers/VersionInfo.cs
+++ b/OpenSim/Framework/Servers/VersionInfo.cs
@@ -29,9 +29,8 @@ namespace OpenSim
 {
     public class VersionInfo
     {
-        private const string VERSION_NUMBER = "0.7";
+        private const string VERSION_NUMBER = "0.7CM";
         private const Flavour VERSION_FLAVOUR = Flavour.Dev;
-
         public enum Flavour
         {
             Unknown,
@@ -71,4 +70,4 @@ namespace OpenSim
         /// </value>
         public readonly static int MajorInterfaceVersion = 6;
     }
-}
\ No newline at end of file
+}
diff --git a/OpenSim/Framework/TaskInventoryDictionary.cs b/OpenSim/Framework/TaskInventoryDictionary.cs
index 25ae6b0..4b9a509 100644
--- a/OpenSim/Framework/TaskInventoryDictionary.cs
+++ b/OpenSim/Framework/TaskInventoryDictionary.cs
@@ -27,9 +27,12 @@
 
 using System;
 using System.Collections.Generic;
+using System.Threading;
+using System.Reflection;
 using System.Xml;
 using System.Xml.Schema;
 using System.Xml.Serialization;
+using log4net;
 using OpenMetaverse;
 
 namespace OpenSim.Framework
@@ -45,6 +48,111 @@ namespace OpenSim.Framework
         // private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
 
         private static XmlSerializer tiiSerializer = new XmlSerializer(typeof (TaskInventoryItem));
+        private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
+        
+        private Thread LockedByThread;
+        /// <value>
+        /// An advanced lock for inventory data
+        /// </value>
+        private System.Threading.ReaderWriterLockSlim m_itemLock = new System.Threading.ReaderWriterLockSlim();
+
+        /// <summary>
+        /// Are we readlocked by the calling thread?
+        /// </summary>
+        public bool IsReadLockedByMe()
+        {
+            if (m_itemLock.RecursiveReadCount > 0)
+            {
+                return true;
+            }
+            else
+            {
+                return false;
+            }
+        }
+
+        /// <summary>
+        /// Lock our inventory list for reading (many can read, one can write)
+        /// </summary>
+        public void LockItemsForRead(bool locked)
+        {
+            if (locked)
+            {
+                if (m_itemLock.IsWriteLockHeld && LockedByThread != null)
+                {
+                    if (!LockedByThread.IsAlive)
+                    {
+                        //Locked by dead thread, reset.
+                        m_itemLock = new System.Threading.ReaderWriterLockSlim(); 
+                    }
+                }
+
+                if (m_itemLock.RecursiveReadCount > 0)
+                {
+                    m_log.Error("[TaskInventoryDictionary] Recursive read lock requested. This should not happen and means something needs to be fixed. For now though, it's safe to continue.");
+                    m_itemLock.ExitReadLock();
+                }
+                if (m_itemLock.RecursiveWriteCount > 0)
+                {
+                    m_log.Error("[TaskInventoryDictionary] Recursive write lock requested. This should not happen and means something needs to be fixed.");
+                    m_itemLock.ExitWriteLock();
+                }
+                
+                while (!m_itemLock.TryEnterReadLock(60000))
+                {
+                    m_log.Error("Thread lock detected while trying to aquire READ lock in TaskInventoryDictionary. Locked by thread " + LockedByThread.Name + ". I'm going to try to solve the thread lock automatically to preserve region stability, but this needs to be fixed.");
+                    if (m_itemLock.IsWriteLockHeld)
+                    {
+                        m_itemLock = new System.Threading.ReaderWriterLockSlim();
+                    }
+                }
+            }
+            else
+            {
+                if (m_itemLock.RecursiveReadCount>0)
+                {
+                    m_itemLock.ExitReadLock();
+                }
+            }
+        }
+
+        /// <summary>
+        /// Lock our inventory list for writing (many can read, one can write)
+        /// </summary>
+        public void LockItemsForWrite(bool locked)
+        {
+            if (locked)
+            {
+                //Enter a write lock, wait indefinately for one to open.
+                if (m_itemLock.RecursiveReadCount > 0)
+                {
+                    m_log.Error("[TaskInventoryDictionary] Recursive read lock requested. This should not happen and means something needs to be fixed. For now though, it's safe to continue.");
+                    m_itemLock.ExitReadLock();
+                }
+                if (m_itemLock.RecursiveWriteCount > 0)
+                {
+                    m_log.Error("[TaskInventoryDictionary] Recursive write lock requested. This should not happen and means something needs to be fixed.");
+                    m_itemLock.ExitWriteLock();
+                }
+                while (!m_itemLock.TryEnterWriteLock(60000))
+                {
+                    m_log.Error("Thread lock detected while trying to aquire WRITE lock in TaskInventoryDictionary. Locked by thread " + LockedByThread.Name + ". I'm going to try to solve the thread lock automatically to preserve region stability, but this needs to be fixed.");
+                    if (m_itemLock.IsWriteLockHeld)
+                    {
+                        m_itemLock = new System.Threading.ReaderWriterLockSlim();
+                    }
+                }
+
+                LockedByThread = Thread.CurrentThread;
+            }
+            else
+            {
+                if (m_itemLock.RecursiveWriteCount > 0)
+                {
+                    m_itemLock.ExitWriteLock();
+                }
+            }
+        }
 
         #region ICloneable Members
 
@@ -52,13 +160,12 @@ namespace OpenSim.Framework
         {
             TaskInventoryDictionary clone = new TaskInventoryDictionary();
 
-            lock (this)
+            m_itemLock.EnterReadLock();
+            foreach (UUID uuid in Keys)
             {
-                foreach (UUID uuid in Keys)
-                {
-                    clone.Add(uuid, (TaskInventoryItem) this[uuid].Clone());
-                }
+                clone.Add(uuid, (TaskInventoryItem) this[uuid].Clone());
             }
+            m_itemLock.ExitReadLock();
 
             return clone;
         }
diff --git a/OpenSim/Framework/Util.cs b/OpenSim/Framework/Util.cs
index 64f6118..8cb9127 100644
--- a/OpenSim/Framework/Util.cs
+++ b/OpenSim/Framework/Util.cs
@@ -997,19 +997,19 @@ namespace OpenSim.Framework
         {
             string os = String.Empty;
 
-            if (Environment.OSVersion.Platform != PlatformID.Unix)
-            {
-                os = Environment.OSVersion.ToString();
-            }
-            else
-            {
-                os = ReadEtcIssue();
-            }
-                      
-            if (os.Length > 45)
-            {
-                os = os.Substring(0, 45);
-            }
+//            if (Environment.OSVersion.Platform != PlatformID.Unix)
+//            {
+//                os = Environment.OSVersion.ToString();
+//            }
+//            else
+//            {
+//                os = ReadEtcIssue();
+//            }
+//                      
+//            if (os.Length > 45)
+//            {
+//                os = os.Substring(0, 45);
+//            }
             
             return os;
         }
diff --git a/OpenSim/Region/ClientStack/LindenUDP/LLClientView.cs b/OpenSim/Region/ClientStack/LindenUDP/LLClientView.cs
index 25f6ef0..773baf5 100644
--- a/OpenSim/Region/ClientStack/LindenUDP/LLClientView.cs
+++ b/OpenSim/Region/ClientStack/LindenUDP/LLClientView.cs
@@ -1054,6 +1054,10 @@ namespace OpenSim.Region.ClientStack.LindenUDP
         public virtual void SendLayerData(float[] map)
         {
             Util.FireAndForget(DoSendLayerData, map);
+
+            // Send it sync, and async. It's not that much data
+            // and it improves user experience just so much!
+            DoSendLayerData(map);
         }
 
         /// <summary>
@@ -1066,16 +1070,13 @@ namespace OpenSim.Region.ClientStack.LindenUDP
 
             try
             {
-                //for (int y = 0; y < 16; y++)
-                //{
-                //    for (int x = 0; x < 16; x++)
-                //    {
-                //        SendLayerData(x, y, map);
-                //    }
-                //}
-
-                // Send LayerData in a spiral pattern. Fun!
-                SendLayerTopRight(map, 0, 0, 15, 15);
+                for (int y = 0; y < 16; y++)
+                {
+                    for (int x = 0; x < 16; x+=4)
+                    {
+                        SendLayerPacket(x, y, map);
+                    }
+                }
             }
             catch (Exception e)
             {
@@ -1083,51 +1084,35 @@ namespace OpenSim.Region.ClientStack.LindenUDP
             }
         }
 
-        private void SendLayerTopRight(float[] map, int x1, int y1, int x2, int y2)
-        {
-            // Row
-            for (int i = x1; i <= x2; i++)
-                SendLayerData(i, y1, map);
-
-            // Column
-            for (int j = y1 + 1; j <= y2; j++)
-                SendLayerData(x2, j, map);
-
-            if (x2 - x1 > 0)
-                SendLayerBottomLeft(map, x1, y1 + 1, x2 - 1, y2);
-        }
-
-        void SendLayerBottomLeft(float[] map, int x1, int y1, int x2, int y2)
-        {
-            // Row in reverse
-            for (int i = x2; i >= x1; i--)
-                SendLayerData(i, y2, map);
-
-            // Column in reverse
-            for (int j = y2 - 1; j >= y1; j--)
-                SendLayerData(x1, j, map);
-
-            if (x2 - x1 > 0)
-                SendLayerTopRight(map, x1 + 1, y1, x2, y2 - 1);
-        }
-
         /// <summary>
         /// Sends a set of four patches (x, x+1, ..., x+3) to the client
         /// </summary>
         /// <param name="map">heightmap</param>
         /// <param name="px">X coordinate for patches 0..12</param>
         /// <param name="py">Y coordinate for patches 0..15</param>
-        // private void SendLayerPacket(float[] map, int y, int x)
-        // {
-        //     int[] patches = new int[4];
-        //     patches[0] = x + 0 + y * 16;
-        //     patches[1] = x + 1 + y * 16;
-        //     patches[2] = x + 2 + y * 16;
-        //     patches[3] = x + 3 + y * 16;
-
-        //     Packet layerpack = LLClientView.TerrainManager.CreateLandPacket(map, patches);
-        //     OutPacket(layerpack, ThrottleOutPacketType.Land);
-        // }
+        private void SendLayerPacket(int x, int y, float[] map)
+        {
+            int[] patches = new int[4];
+            patches[0] = x + 0 + y * 16;
+            patches[1] = x + 1 + y * 16;
+            patches[2] = x + 2 + y * 16;
+            patches[3] = x + 3 + y * 16;
+
+            float[] heightmap = (map.Length == 65536) ?
+                map :
+                LLHeightFieldMoronize(map);
+
+            try
+            {
+                Packet layerpack = TerrainCompressor.CreateLandPacket(heightmap, patches);
+                OutPacket(layerpack, ThrottleOutPacketType.Land);
+            }
+            catch
+            {
+                for (int px = x ; px < x + 4 ; px++)
+                    SendLayerData(px, y, map);
+            }
+        }
 
         /// <summary>
         /// Sends a specified patch to a client
@@ -1147,7 +1132,7 @@ namespace OpenSim.Region.ClientStack.LindenUDP
                 LayerDataPacket layerpack = TerrainCompressor.CreateLandPacket(heightmap, patches);
                 layerpack.Header.Reliable = true;
 
-                OutPacket(layerpack, ThrottleOutPacketType.Land);
+                OutPacket(layerpack, ThrottleOutPacketType.Task);
             }
             catch (Exception e)
             {
@@ -3419,7 +3404,6 @@ namespace OpenSim.Region.ClientStack.LindenUDP
 
             objupdate.ObjectData = new ObjectUpdatePacket.ObjectDataBlock[1];
             objupdate.ObjectData[0] = CreateAvatarUpdateBlock(data);
-
             OutPacket(objupdate, ThrottleOutPacketType.Task);
         }
 
@@ -3470,8 +3454,7 @@ namespace OpenSim.Region.ClientStack.LindenUDP
                     terse.ObjectData[i] = m_avatarTerseUpdates.Dequeue();
             }
 
-            // HACK: Using the task category until the tiered reprioritization code is in
-            OutPacket(terse, ThrottleOutPacketType.Task);
+            OutPacket(terse, ThrottleOutPacketType.State);
         }
 
         public void SendCoarseLocationUpdate(List<UUID> users, List<Vector3> CoarseLocations)
@@ -3961,6 +3944,9 @@ namespace OpenSim.Region.ClientStack.LindenUDP
             {
                 m_propertiesPacketTimer.Stop();
 
+                if (m_propertiesBlocks.Count == 0)
+                    return;
+
                 proper.ObjectData = new ObjectPropertiesPacket.ObjectDataBlock[m_propertiesBlocks.Count];
 
                 int index = 0;
@@ -4898,6 +4884,7 @@ namespace OpenSim.Region.ClientStack.LindenUDP
                         (x.CameraLeftAxis != lastarg.CameraLeftAxis) ||
                         (x.CameraUpAxis != lastarg.CameraUpAxis) ||
                         (x.ControlFlags != lastarg.ControlFlags) ||
+                        (x.ControlFlags != 0) ||
                         (x.Far != lastarg.Far) ||
                         (x.Flags != lastarg.Flags) ||
                         (x.State != lastarg.State) ||
diff --git a/OpenSim/Region/CoreModules/Avatar/Chat/ChatModule.cs b/OpenSim/Region/CoreModules/Avatar/Chat/ChatModule.cs
index 6dacbba..acc3a78 100644
--- a/OpenSim/Region/CoreModules/Avatar/Chat/ChatModule.cs
+++ b/OpenSim/Region/CoreModules/Avatar/Chat/ChatModule.cs
@@ -49,7 +49,8 @@ namespace OpenSim.Region.CoreModules.Avatar.Chat
         private int m_shoutdistance = 100;
         private int m_whisperdistance = 10;
         private List<Scene> m_scenes = new List<Scene>();
-
+        private List<string> FreezeCache = new List<string>();
+        private string m_adminPrefix = "";
         internal object m_syncy = new object();
 
         internal IConfig m_config;
@@ -76,6 +77,7 @@ namespace OpenSim.Region.CoreModules.Avatar.Chat
             m_whisperdistance = config.Configs["Chat"].GetInt("whisper_distance", m_whisperdistance);
             m_saydistance = config.Configs["Chat"].GetInt("say_distance", m_saydistance);
             m_shoutdistance = config.Configs["Chat"].GetInt("shout_distance", m_shoutdistance);
+            m_adminPrefix = config.Configs["Chat"].GetString("admin_prefix", "");
         }
 
         public virtual void AddRegion(Scene scene)
@@ -171,7 +173,15 @@ namespace OpenSim.Region.CoreModules.Avatar.Chat
                 return;
             }
 
-            DeliverChatToAvatars(ChatSourceType.Agent, c);
+            if (FreezeCache.Contains(c.Sender.AgentId.ToString()))
+            {
+                if (c.Type != ChatTypeEnum.StartTyping || c.Type != ChatTypeEnum.StopTyping)
+                    c.Sender.SendAgentAlertMessage("You may not talk as you are frozen.", false);
+            }
+            else
+            {
+                DeliverChatToAvatars(ChatSourceType.Agent, c);
+            }
         }
 
         public virtual void OnChatFromWorld(Object sender, OSChatMessage c)
@@ -185,6 +195,7 @@ namespace OpenSim.Region.CoreModules.Avatar.Chat
         protected virtual void DeliverChatToAvatars(ChatSourceType sourceType, OSChatMessage c)
         {
             string fromName = c.From;
+            string fromNamePrefix = "";
             UUID fromID = UUID.Zero;
             string message = c.Message;
             IScene scene = c.Scene;
@@ -207,7 +218,10 @@ namespace OpenSim.Region.CoreModules.Avatar.Chat
                 fromPos = avatar.AbsolutePosition;
                 fromName = avatar.Name;
                 fromID = c.Sender.AgentId;
-
+                if (avatar.GodLevel > 200)
+                {
+                    fromNamePrefix = m_adminPrefix;
+                }
                 break;
 
             case ChatSourceType.Object:
@@ -227,7 +241,15 @@ namespace OpenSim.Region.CoreModules.Avatar.Chat
                 s.ForEachScenePresence(
                     delegate(ScenePresence presence)
                     {
-                        TrySendChatMessage(presence, fromPos, regionPos, fromID, fromName, c.Type, message, sourceType);
+                        ILandObject Presencecheck = s.LandChannel.GetLandObject(presence.AbsolutePosition.X, presence.AbsolutePosition.Y);
+                        if (Presencecheck != null)
+                        {
+                            if (Presencecheck.IsEitherBannedOrRestricted(c.SenderUUID) != true)
+                            {
+                                TrySendChatMessage(presence, fromPos, regionPos, fromID, fromNamePrefix+fromName, c.Type, message, sourceType);
+                            }
+                        }
+
                     }
                 );
             }
@@ -266,25 +288,29 @@ namespace OpenSim.Region.CoreModules.Avatar.Chat
             }
             
             // m_log.DebugFormat("[CHAT] Broadcast: fromID {0} fromName {1}, cType {2}, sType {3}", fromID, fromName, cType, sourceType);
-
-            ((Scene)c.Scene).ForEachScenePresence(
-                delegate(ScenePresence presence)
-                {
-                    // ignore chat from child agents
-                    if (presence.IsChildAgent) return;
-                    
-                    IClientAPI client = presence.ControllingClient;
-                    
-                    // don't forward SayOwner chat from objects to
-                    // non-owner agents
-                    if ((c.Type == ChatTypeEnum.Owner) &&
-                        (null != c.SenderObject) &&
-                        (((SceneObjectPart)c.SenderObject).OwnerID != client.AgentId))
-                        return;
-                    
-                    client.SendChatMessage(c.Message, (byte)cType, CenterOfRegion, fromName, fromID, 
-                                           (byte)sourceType, (byte)ChatAudibleLevel.Fully);
-                });
+            if (c.Scene != null)
+            {
+                ((Scene)c.Scene).ForEachScenePresence
+                (
+                    delegate(ScenePresence presence)
+                    {
+                        // ignore chat from child agents
+                        if (presence.IsChildAgent) return;
+                        
+                        IClientAPI client = presence.ControllingClient;
+                        
+                        // don't forward SayOwner chat from objects to
+                        // non-owner agents
+                        if ((c.Type == ChatTypeEnum.Owner) &&
+                            (null != c.SenderObject) &&
+                            (((SceneObjectPart)c.SenderObject).OwnerID != client.AgentId))
+                            return;
+                        
+                        client.SendChatMessage(c.Message, (byte)cType, CenterOfRegion, fromName, fromID, 
+                                               (byte)sourceType, (byte)ChatAudibleLevel.Fully);
+                    }
+                );
+             }
         }
 
 
@@ -313,5 +339,35 @@ namespace OpenSim.Region.CoreModules.Avatar.Chat
             presence.ControllingClient.SendChatMessage(message, (byte) type, fromPos, fromName,
                                                        fromAgentID,(byte)src,(byte)ChatAudibleLevel.Fully);
         }
+
+        Dictionary<UUID, System.Threading.Timer> Timers = new Dictionary<UUID, System.Threading.Timer>();
+        public void ParcelFreezeUser(IClientAPI client, UUID parcelowner, uint flags, UUID target)
+        {
+            System.Threading.Timer Timer;
+            if (flags == 0)
+            {
+                FreezeCache.Add(target.ToString());
+                System.Threading.TimerCallback timeCB = new System.Threading.TimerCallback(OnEndParcelFrozen);
+                Timer = new System.Threading.Timer(timeCB, target, 30000, 0);
+                Timers.Add(target, Timer);
+            }
+            else
+            {
+                FreezeCache.Remove(target.ToString());
+                Timers.TryGetValue(target, out Timer);
+                Timers.Remove(target);
+                Timer.Dispose();
+            }
+        }
+
+        private void OnEndParcelFrozen(object avatar)
+        {
+            UUID target = (UUID)avatar;
+            FreezeCache.Remove(target.ToString());
+            System.Threading.Timer Timer;
+            Timers.TryGetValue(target, out Timer);
+            Timers.Remove(target);
+            Timer.Dispose();
+        }
     }
 }
diff --git a/OpenSim/Region/CoreModules/Avatar/InstantMessage/OfflineMessageModule.cs b/OpenSim/Region/CoreModules/Avatar/InstantMessage/OfflineMessageModule.cs
index fdfcd10..9412735 100644
--- a/OpenSim/Region/CoreModules/Avatar/InstantMessage/OfflineMessageModule.cs
+++ b/OpenSim/Region/CoreModules/Avatar/InstantMessage/OfflineMessageModule.cs
@@ -172,13 +172,16 @@ namespace OpenSim.Region.CoreModules.Avatar.InstantMessage
 
         private void RetrieveInstantMessages(IClientAPI client)
         {
-            if (m_RestURL != "")
-            {
-                m_log.DebugFormat("[OFFLINE MESSAGING] Retrieving stored messages for {0}", client.AgentId);
+            if (m_RestURL == String.Empty)
+                return;
 
-                List<GridInstantMessage> msglist = SynchronousRestObjectPoster.BeginPostObject<UUID, List<GridInstantMessage>>(
+            m_log.DebugFormat("[OFFLINE MESSAGING] Retrieving stored messages for {0}", client.AgentId);
+
+            List<GridInstantMessage> msglist = SynchronousRestObjectPoster.BeginPostObject<UUID, List<GridInstantMessage>>(
                         "POST", m_RestURL + "/RetrieveMessages/", client.AgentId);
 
+            if (msglist != null)
+            {
                 foreach (GridInstantMessage im in msglist)
                 {
                     // client.SendInstantMessage(im);
diff --git a/OpenSim/Region/CoreModules/Avatar/Inventory/Transfer/InventoryTransferModule.cs b/OpenSim/Region/CoreModules/Avatar/Inventory/Transfer/InventoryTransferModule.cs
index be89740..7142442 100644
--- a/OpenSim/Region/CoreModules/Avatar/Inventory/Transfer/InventoryTransferModule.cs
+++ b/OpenSim/Region/CoreModules/Avatar/Inventory/Transfer/InventoryTransferModule.cs
@@ -417,7 +417,7 @@ namespace OpenSim.Region.CoreModules.Avatar.Inventory.Transfer
         {
             // Check if this is ours to handle
             //
-            m_log.Info("OnFridInstantMessage");
+            //m_log.Info("OnFridInstantMessage");
             if (msg.dialog != (byte) InstantMessageDialog.InventoryOffered)
                 return;
 
diff --git a/OpenSim/Region/CoreModules/World/Archiver/ArchiveReadRequest.cs b/OpenSim/Region/CoreModules/World/Archiver/ArchiveReadRequest.cs
index b1b2336f8..14bab6e 100644
--- a/OpenSim/Region/CoreModules/World/Archiver/ArchiveReadRequest.cs
+++ b/OpenSim/Region/CoreModules/World/Archiver/ArchiveReadRequest.cs
@@ -256,10 +256,11 @@ namespace OpenSim.Region.CoreModules.World.Archiver
                     // Fix ownership/creator of inventory items
                     // Not doing so results in inventory items
                     // being no copy/no mod for everyone
-                    lock (part.TaskInventory)
+                    part.TaskInventory.LockItemsForRead(true);
+                    TaskInventoryDictionary inv = part.TaskInventory;
+                    foreach (KeyValuePair<UUID, TaskInventoryItem> kvp in inv)
                     {
-                        TaskInventoryDictionary inv = part.TaskInventory;
-                        foreach (KeyValuePair<UUID, TaskInventoryItem> kvp in inv)
+                        if (!ResolveUserUuid(kvp.Value.OwnerID))
                         {
                             if (!ResolveUserUuid(kvp.Value.OwnerID))
                             {
@@ -271,6 +272,7 @@ namespace OpenSim.Region.CoreModules.World.Archiver
                             }
                         }
                     }
+                    part.TaskInventory.LockItemsForRead(false);
                 }
 
                 if (m_scene.AddRestoredSceneObject(sceneObject, true, false))
diff --git a/OpenSim/Region/CoreModules/World/Land/LandManagementModule.cs b/OpenSim/Region/CoreModules/World/Land/LandManagementModule.cs
index 5750aa4..7f6a0ad 100644
--- a/OpenSim/Region/CoreModules/World/Land/LandManagementModule.cs
+++ b/OpenSim/Region/CoreModules/World/Land/LandManagementModule.cs
@@ -82,6 +82,8 @@ namespace OpenSim.Region.CoreModules.World.Land
         private int m_lastLandLocalID = LandChannel.START_LAND_LOCAL_ID - 1;
 
         private bool m_allowedForcefulBans = true;
+        private UUID DefaultGodParcelGroup;
+        private string DefaultGodParcelName;
 
         // caches ExtendedLandData
         private Cache parcelInfoCache;
@@ -96,6 +98,12 @@ namespace OpenSim.Region.CoreModules.World.Land
 
         public void Initialise(IConfigSource source)
         {
+            IConfig cnf = source.Configs["LandManagement"];
+            if (cnf != null)
+            {
+                DefaultGodParcelGroup = new UUID(cnf.GetString("DefaultAdministratorGroupUUID", UUID.Zero.ToString()));
+                DefaultGodParcelName = cnf.GetString("DefaultAdministratorParcelName", "Default Parcel");
+            }
         }
 
         public void AddRegion(Scene scene)
@@ -1504,5 +1512,321 @@ namespace OpenSim.Region.CoreModules.World.Land
 
             UpdateLandObject(localID, land.LandData);
         }
+
+        public void ClientOnParcelGodMark(IClientAPI client, UUID god, int landID)
+        {
+            ILandObject land = null;
+            List<ILandObject> Land = ((Scene)client.Scene).LandChannel.AllParcels();
+            foreach (ILandObject landObject in Land)
+            {
+                if (landObject.LandData.LocalID == landID)
+                {
+                    land = landObject;
+                }
+            }
+            land.DeedToGroup(DefaultGodParcelGroup);
+            land.LandData.Name = DefaultGodParcelName;
+            land.SendLandUpdateToAvatarsOverMe();
+        }
+
+        private void ClientOnSimWideDeletes(IClientAPI client, UUID agentID, int flags, UUID targetID)
+        {
+            ScenePresence SP;
+            ((Scene)client.Scene).TryGetScenePresence(client.AgentId, out SP);
+            List<SceneObjectGroup> returns = new List<SceneObjectGroup>();
+            if (SP.GodLevel != 0)
+            {
+                if (flags == 0) //All parcels, scripted or not
+                {
+                    ((Scene)client.Scene).ForEachSOG(delegate(SceneObjectGroup e)
+                    {
+                        if (e.OwnerID == targetID)
+                        {
+                            returns.Add(e);
+                        }
+                    }
+                                                    );
+                }
+                if (flags == 4) //All parcels, scripted object
+                {
+                    ((Scene)client.Scene).ForEachSOG(delegate(SceneObjectGroup e)
+                    {
+                        if (e.OwnerID == targetID)
+                        {
+                            if (e.scriptScore >= 0.01)
+                            {
+                                returns.Add(e);
+                            }
+                        }
+                    }
+                                                    );
+                }
+                if (flags == 4) //not target parcel, scripted object
+                {
+                    ((Scene)client.Scene).ForEachSOG(delegate(SceneObjectGroup e)
+                    {
+                        if (e.OwnerID == targetID)
+                        {
+                            ILandObject landobject = ((Scene)client.Scene).LandChannel.GetLandObject(e.AbsolutePosition.X, e.AbsolutePosition.Y);
+                            if (landobject.LandData.OwnerID != e.OwnerID)
+                            {
+                                if (e.scriptScore >= 0.01)
+                                {
+                                    returns.Add(e);
+                                }
+                            }
+                        }
+                    }
+                                                    );
+                }
+                foreach (SceneObjectGroup ol in returns)
+                {
+                    ReturnObject(ol, client);
+                }
+            }
+        }
+        public void ReturnObject(SceneObjectGroup obj, IClientAPI client)
+        {
+            SceneObjectGroup[] objs = new SceneObjectGroup[1];
+            objs[0] = obj;
+            ((Scene)client.Scene).returnObjects(objs, client.AgentId);
+        }
+
+        Dictionary<UUID, System.Threading.Timer> Timers = new Dictionary<UUID, System.Threading.Timer>();
+
+        public void ClientOnParcelFreezeUser(IClientAPI client, UUID parcelowner, uint flags, UUID target)
+        {
+            ScenePresence targetAvatar = null;
+            ((Scene)client.Scene).TryGetScenePresence(target, out targetAvatar);
+            ScenePresence parcelManager = null;
+            ((Scene)client.Scene).TryGetScenePresence(client.AgentId, out parcelManager);
+            System.Threading.Timer Timer;
+
+            if (targetAvatar.GodLevel == 0)
+            {
+                ILandObject land = ((Scene)client.Scene).LandChannel.GetLandObject(targetAvatar.AbsolutePosition.X, targetAvatar.AbsolutePosition.Y);
+                if (!((Scene)client.Scene).Permissions.CanEditParcel(client.AgentId, land))
+                    return;
+                if (flags == 0)
+                {
+                    targetAvatar.AllowMovement = false;
+                    targetAvatar.ControllingClient.SendAlertMessage(parcelManager.Firstname + " " + parcelManager.Lastname + " has frozen you for 30 seconds.  You cannot move or interact with the world.");
+                    parcelManager.ControllingClient.SendAlertMessage("Avatar Frozen.");
+                    System.Threading.TimerCallback timeCB = new System.Threading.TimerCallback(OnEndParcelFrozen);
+                    Timer = new System.Threading.Timer(timeCB, targetAvatar, 30000, 0);
+                    Timers.Add(targetAvatar.UUID, Timer);
+                }
+                else
+                {
+                    targetAvatar.AllowMovement = true;
+                    targetAvatar.ControllingClient.SendAlertMessage(parcelManager.Firstname + " " + parcelManager.Lastname + " has unfrozen you.");
+                    parcelManager.ControllingClient.SendAlertMessage("Avatar Unfrozen.");
+                    Timers.TryGetValue(targetAvatar.UUID, out Timer);
+                    Timers.Remove(targetAvatar.UUID);
+                    Timer.Dispose();
+                }
+            }
+        }
+        private void OnEndParcelFrozen(object avatar)
+        {
+            ScenePresence targetAvatar = (ScenePresence)avatar;
+            targetAvatar.AllowMovement = true;
+            System.Threading.Timer Timer;
+            Timers.TryGetValue(targetAvatar.UUID, out Timer);
+            Timers.Remove(targetAvatar.UUID);
+            targetAvatar.ControllingClient.SendAgentAlertMessage("The freeze has worn off; you may go about your business.", false);
+        }
+
+
+        public void ClientOnParcelEjectUser(IClientAPI client, UUID parcelowner, uint flags, UUID target)
+        {
+            ScenePresence targetAvatar = null;
+            ((Scene)client.Scene).TryGetScenePresence(target, out targetAvatar);
+            ScenePresence parcelManager = null;
+            ((Scene)client.Scene).TryGetScenePresence(client.AgentId, out parcelManager);
+            //Just eject
+            if (flags == 0)
+            {
+                if (targetAvatar.GodLevel == 0)
+                {
+                    ILandObject land = ((Scene)client.Scene).LandChannel.GetLandObject(targetAvatar.AbsolutePosition.X, targetAvatar.AbsolutePosition.Y);
+                    if (!((Scene)client.Scene).Permissions.CanEditParcel(client.AgentId, land))
+                        return;
+
+                    Vector3 position = new Vector3(0, 0, 0);
+                    List<ILandObject> allParcels = new List<ILandObject>();
+                    allParcels = AllParcels();
+                    if (allParcels.Count != 1)
+                    {
+                        foreach (ILandObject parcel in allParcels)
+                        {
+                            if (parcel.LandData.GlobalID != land.LandData.GlobalID)
+                            {
+                                if (parcel.IsEitherBannedOrRestricted(targetAvatar.UUID) != true)
+                                {
+                                    for (int x = 1; x <= Constants.RegionSize; x += 2)
+                                    {
+                                        for (int y = 1; y <= Constants.RegionSize; y += 2)
+                                        {
+                                            if (parcel.ContainsPoint(x, y))
+                                            {
+                                                position = new Vector3(x, y, targetAvatar.AbsolutePosition.Z);
+                                                targetAvatar.TeleportWithMomentum(position);
+                                                targetAvatar.ControllingClient.SendAlertMessage("You have been ejected by " + parcelManager.Firstname + " " + parcelManager.Lastname);
+                                                parcelManager.ControllingClient.SendAlertMessage("Avatar Ejected.");
+                                                return;
+                                            }
+                                        }
+                                    }
+                                }
+                            }
+                        }
+                    }
+                    Vector3 targetVector;
+                    if (targetAvatar.AbsolutePosition.X > targetAvatar.AbsolutePosition.Y)
+                    {
+                        if (targetAvatar.AbsolutePosition.X > .5 * Constants.RegionSize)
+                        {
+                            targetVector = new Vector3(Constants.RegionSize, targetAvatar.AbsolutePosition.Y, targetAvatar.AbsolutePosition.Z); ;
+                            targetAvatar.TeleportWithMomentum(targetVector);
+                            targetAvatar.ControllingClient.SendAlertMessage("You have been ejected by " + parcelManager.Firstname + " " + parcelManager.Lastname);
+                            parcelManager.ControllingClient.SendAlertMessage("Avatar Ejected.");
+                            return;
+                        }
+                        else
+                        {
+                            targetVector = new Vector3(0, targetAvatar.AbsolutePosition.Y, targetAvatar.AbsolutePosition.Z); ;
+                            targetAvatar.TeleportWithMomentum(targetVector);
+                            targetAvatar.ControllingClient.SendAlertMessage("You have been ejected by " + parcelManager.Firstname + " " + parcelManager.Lastname);
+                            parcelManager.ControllingClient.SendAlertMessage("Avatar Ejected.");
+                            return;
+                        }
+                    }
+                    else
+                    {
+                        if (targetAvatar.AbsolutePosition.Y > .5 * Constants.RegionSize)
+                        {
+                            targetVector = new Vector3(targetAvatar.AbsolutePosition.X, Constants.RegionSize, targetAvatar.AbsolutePosition.Z); ;
+                            targetAvatar.TeleportWithMomentum(targetVector);
+                            targetAvatar.ControllingClient.SendAlertMessage("You have been ejected by " + parcelManager.Firstname + " " + parcelManager.Lastname);
+                            parcelManager.ControllingClient.SendAlertMessage("Avatar Ejected.");
+                            return;
+                        }
+                        else
+                        {
+                            targetVector = new Vector3(targetAvatar.AbsolutePosition.X, 0, targetAvatar.AbsolutePosition.Z); ;
+                            targetAvatar.TeleportWithMomentum(targetVector);
+                            targetAvatar.ControllingClient.SendAlertMessage("You have been ejected by " + parcelManager.Firstname + " " + parcelManager.Lastname);
+                            parcelManager.ControllingClient.SendAlertMessage("Avatar Ejected.");
+                            return;
+                        }
+                    }
+                }
+            }
+            //Eject and ban
+            if (flags == 1)
+            {
+                if (targetAvatar.GodLevel == 0)
+                {
+                    ILandObject land = ((Scene)client.Scene).LandChannel.GetLandObject(targetAvatar.AbsolutePosition.X, targetAvatar.AbsolutePosition.Y);
+                    if (!((Scene)client.Scene).Permissions.CanEditParcel(client.AgentId, land))
+                        return;
+
+                    Vector3 position = new Vector3(0, 0, 0);
+                    List<ILandObject> allParcels = new List<ILandObject>();
+                    allParcels = AllParcels();
+                    if (allParcels.Count != 1)
+                    {
+                        foreach (ILandObject parcel in allParcels)
+                        {
+                            if (parcel.LandData.GlobalID != land.LandData.GlobalID)
+                            {
+                                if (parcel.IsEitherBannedOrRestricted(targetAvatar.UUID) != true)
+                                {
+                                    for (int x = 1; x <= Constants.RegionSize; x += 2)
+                                    {
+                                        for (int y = 1; y <= Constants.RegionSize; y += 2)
+                                        {
+                                            if (parcel.ContainsPoint(x, y))
+                                            {
+                                                position = new Vector3(x, y, targetAvatar.AbsolutePosition.Z);
+                                                targetAvatar.TeleportWithMomentum(position);
+                                                targetAvatar.ControllingClient.SendAlertMessage("You have been ejected and banned by " + parcelManager.Firstname + " " + parcelManager.Lastname);
+                                                parcelManager.ControllingClient.SendAlertMessage("Avatar Ejected and Banned.");
+                                                ParcelManager.ParcelAccessEntry entry = new ParcelManager.ParcelAccessEntry();
+                                                entry.AgentID = targetAvatar.UUID;
+                                                entry.Flags = AccessList.Ban;
+                                                entry.Time = new DateTime();
+                                                land.LandData.ParcelAccessList.Add(entry);
+                                                return;
+                                            }
+                                        }
+                                    }
+                                }
+                            }
+                        }
+                    }
+                    Vector3 targetVector;
+                    if (targetAvatar.AbsolutePosition.X > targetAvatar.AbsolutePosition.Y)
+                    {
+                        if (targetAvatar.AbsolutePosition.X > .5 * Constants.RegionSize)
+                        {
+                            targetVector = new Vector3(Constants.RegionSize, targetAvatar.AbsolutePosition.Y, targetAvatar.AbsolutePosition.Z); ;
+                            targetAvatar.TeleportWithMomentum(targetVector);
+                            targetAvatar.ControllingClient.SendAlertMessage("You have been ejected and banned by " + parcelManager.Firstname + " " + parcelManager.Lastname);
+                            parcelManager.ControllingClient.SendAlertMessage("Avatar Ejected and Banned.");
+                            ParcelManager.ParcelAccessEntry entry = new ParcelManager.ParcelAccessEntry();
+                            entry.AgentID = targetAvatar.UUID;
+                            entry.Flags = AccessList.Ban;
+                            entry.Time = new DateTime();
+                            land.LandData.ParcelAccessList.Add(entry);
+                            return;
+                        }
+                        else
+                        {
+                            targetVector = new Vector3(0, targetAvatar.AbsolutePosition.Y, targetAvatar.AbsolutePosition.Z); ;
+                            targetAvatar.TeleportWithMomentum(targetVector);
+                            targetAvatar.ControllingClient.SendAlertMessage("You have been ejected and banned by " + parcelManager.Firstname + " " + parcelManager.Lastname);
+                            parcelManager.ControllingClient.SendAlertMessage("Avatar Ejected and Banned.");
+                            ParcelManager.ParcelAccessEntry entry = new ParcelManager.ParcelAccessEntry();
+                            entry.AgentID = targetAvatar.UUID;
+                            entry.Flags = AccessList.Ban;
+                            entry.Time = new DateTime();
+                            land.LandData.ParcelAccessList.Add(entry);
+                            return;
+                        }
+                    }
+                    else
+                    {
+                        if (targetAvatar.AbsolutePosition.Y > .5 * Constants.RegionSize)
+                        {
+                            targetVector = new Vector3(targetAvatar.AbsolutePosition.X, Constants.RegionSize, targetAvatar.AbsolutePosition.Z); ;
+                            targetAvatar.TeleportWithMomentum(targetVector);
+                            targetAvatar.ControllingClient.SendAlertMessage("You have been ejected and banned by " + parcelManager.Firstname + " " + parcelManager.Lastname);
+                            parcelManager.ControllingClient.SendAlertMessage("Avatar Ejected and Banned.");
+                            ParcelManager.ParcelAccessEntry entry = new ParcelManager.ParcelAccessEntry();
+                            entry.AgentID = targetAvatar.UUID;
+                            entry.Flags = AccessList.Ban;
+                            entry.Time = new DateTime();
+                            land.LandData.ParcelAccessList.Add(entry);
+                            return;
+                        }
+                        else
+                        {
+                            targetVector = new Vector3(targetAvatar.AbsolutePosition.X, 0, targetAvatar.AbsolutePosition.Z); ;
+                            targetAvatar.TeleportWithMomentum(targetVector);
+                            targetAvatar.ControllingClient.SendAlertMessage("You have been ejected and banned by " + parcelManager.Firstname + " " + parcelManager.Lastname);
+                            parcelManager.ControllingClient.SendAlertMessage("Avatar Ejected and Banned.");
+                            ParcelManager.ParcelAccessEntry entry = new ParcelManager.ParcelAccessEntry();
+                            entry.AgentID = targetAvatar.UUID;
+                            entry.Flags = AccessList.Ban;
+                            entry.Time = new DateTime();
+                            land.LandData.ParcelAccessList.Add(entry);
+                            return;
+                        }
+                    }
+                }
+            }
+        }
     }
 }
diff --git a/OpenSim/Region/Framework/Scenes/Animation/ScenePresenceAnimator.cs b/OpenSim/Region/Framework/Scenes/Animation/ScenePresenceAnimator.cs
index 7307662..fd7d44f 100644
--- a/OpenSim/Region/Framework/Scenes/Animation/ScenePresenceAnimator.cs
+++ b/OpenSim/Region/Framework/Scenes/Animation/ScenePresenceAnimator.cs
@@ -53,8 +53,8 @@ namespace OpenSim.Region.Framework.Scenes.Animation
         {
             get { return m_movementAnimation; }
         }
-        protected string m_movementAnimation = "DEFAULT";
-
+ //       protected string m_movementAnimation = "DEFAULT";     //KF: 'DEFAULT' does not exist!
+        protected string m_movementAnimation = "CROUCH";                //KF: CROUCH ensures reliable Av Anim. init.
         private int m_animTickFall;
         private int m_animTickJump;
         
@@ -123,17 +123,22 @@ namespace OpenSim.Region.Framework.Scenes.Animation
         /// </summary>
         public void TrySetMovementAnimation(string anim)
         {
-            //m_log.DebugFormat("Updating movement animation to {0}", anim);
+//Console.WriteLine("Updating movement animation to {0}", anim);
 
             if (!m_scenePresence.IsChildAgent)
             {
                 if (m_animations.TrySetDefaultAnimation(
                     anim, m_scenePresence.ControllingClient.NextAnimationSequenceNumber, m_scenePresence.UUID))
                 {
+//Console.WriteLine("TSMA {0} success.", anim);    
                     // 16384 is CHANGED_ANIMATION
                     m_scenePresence.SendScriptEventToAttachments("changed", new Object[] { 16384 });
                     SendAnimPack();
                 }
+                else
+                {
+//Console.WriteLine("TSMA {0} fail.", anim);                   
+                                }
             }
         }
 
@@ -146,10 +151,7 @@ namespace OpenSim.Region.Framework.Scenes.Animation
             const float PREJUMP_DELAY = 0.25f;
 
             #region Inputs
-            if (m_scenePresence.SitGround)
-            {
-                return "SIT_GROUND_CONSTRAINED";
-            }
+
             AgentManager.ControlFlags controlFlags = (AgentManager.ControlFlags)m_scenePresence.AgentControlFlags;
             PhysicsActor actor = m_scenePresence.PhysicsActor;
 
@@ -159,11 +161,10 @@ namespace OpenSim.Region.Framework.Scenes.Animation
             Vector3 left = Vector3.Transform(Vector3.UnitY, rotMatrix);
 
             // Check control flags
-            bool heldForward = 
-                (((controlFlags & AgentManager.ControlFlags.AGENT_CONTROL_AT_POS) == AgentManager.ControlFlags.AGENT_CONTROL_AT_POS) || ((controlFlags & AgentManager.ControlFlags.AGENT_CONTROL_NUDGE_AT_POS) == AgentManager.ControlFlags.AGENT_CONTROL_NUDGE_AT_POS));
-            bool heldBack = (controlFlags & AgentManager.ControlFlags.AGENT_CONTROL_AT_NEG) == AgentManager.ControlFlags.AGENT_CONTROL_AT_NEG;
-            bool heldLeft = (controlFlags & AgentManager.ControlFlags.AGENT_CONTROL_LEFT_POS) == AgentManager.ControlFlags.AGENT_CONTROL_LEFT_POS;
-            bool heldRight = (controlFlags & AgentManager.ControlFlags.AGENT_CONTROL_LEFT_NEG) == AgentManager.ControlFlags.AGENT_CONTROL_LEFT_NEG;
+            bool heldForward = ((controlFlags & AgentManager.ControlFlags.AGENT_CONTROL_AT_POS) == AgentManager.ControlFlags.AGENT_CONTROL_AT_POS || (controlFlags & AgentManager.ControlFlags.AGENT_CONTROL_NUDGE_AT_POS) == AgentManager.ControlFlags.AGENT_CONTROL_NUDGE_AT_POS);
+            bool heldBack = ((controlFlags & AgentManager.ControlFlags.AGENT_CONTROL_AT_NEG) == AgentManager.ControlFlags.AGENT_CONTROL_AT_NEG || (controlFlags & AgentManager.ControlFlags.AGENT_CONTROL_NUDGE_AT_NEG) == AgentManager.ControlFlags.AGENT_CONTROL_NUDGE_AT_NEG);
+            bool heldLeft = ((controlFlags & AgentManager.ControlFlags.AGENT_CONTROL_LEFT_POS) == AgentManager.ControlFlags.AGENT_CONTROL_LEFT_POS || (controlFlags & AgentManager.ControlFlags.AGENT_CONTROL_NUDGE_LEFT_POS) == AgentManager.ControlFlags.AGENT_CONTROL_NUDGE_LEFT_POS);
+            bool heldRight = ((controlFlags & AgentManager.ControlFlags.AGENT_CONTROL_LEFT_NEG) == AgentManager.ControlFlags.AGENT_CONTROL_LEFT_NEG || (controlFlags & AgentManager.ControlFlags.AGENT_CONTROL_NUDGE_LEFT_NEG) == AgentManager.ControlFlags.AGENT_CONTROL_NUDGE_LEFT_NEG);
             //bool heldTurnLeft = (controlFlags & AgentManager.ControlFlags.AGENT_CONTROL_TURN_LEFT) == AgentManager.ControlFlags.AGENT_CONTROL_TURN_LEFT;
             //bool heldTurnRight = (controlFlags & AgentManager.ControlFlags.AGENT_CONTROL_TURN_RIGHT) == AgentManager.ControlFlags.AGENT_CONTROL_TURN_RIGHT;
             bool heldUp = (controlFlags & AgentManager.ControlFlags.AGENT_CONTROL_UP_POS) == AgentManager.ControlFlags.AGENT_CONTROL_UP_POS;
@@ -316,7 +317,7 @@ namespace OpenSim.Region.Framework.Scenes.Animation
         public void UpdateMovementAnimations()
         {
             m_movementAnimation = GetMovementAnimation();
-
+//Console.WriteLine("UMA got {0}", m_movementAnimation);
             if (m_movementAnimation == "PREJUMP" && !m_scenePresence.Scene.m_usePreJump)
             {
                 // This was the previous behavior before PREJUMP
@@ -451,4 +452,4 @@ namespace OpenSim.Region.Framework.Scenes.Animation
             m_scenePresence = null;
         }
     }
-}
\ No newline at end of file
+}
diff --git a/OpenSim/Region/Framework/Scenes/EntityManager.cs b/OpenSim/Region/Framework/Scenes/EntityManager.cs
index 099fcce..c246e32 100644
--- a/OpenSim/Region/Framework/Scenes/EntityManager.cs
+++ b/OpenSim/Region/Framework/Scenes/EntityManager.cs
@@ -40,7 +40,7 @@ namespace OpenSim.Region.Framework.Scenes
         private readonly Dictionary<UUID,EntityBase> m_eb_uuid = new Dictionary<UUID, EntityBase>();
         private readonly Dictionary<uint, EntityBase> m_eb_localID = new Dictionary<uint, EntityBase>();
         //private readonly Dictionary<UUID, ScenePresence> m_pres_uuid = new Dictionary<UUID, ScenePresence>();
-        private readonly Object m_lock = new Object();
+        private System.Threading.ReaderWriterLockSlim m_lock = new System.Threading.ReaderWriterLockSlim();
 
         [Obsolete("Use Add() instead.")]
         public void Add(UUID id, EntityBase eb)
@@ -50,7 +50,8 @@ namespace OpenSim.Region.Framework.Scenes
 
         public void Add(EntityBase entity)
         {
-            lock (m_lock)
+            m_lock.EnterWriteLock();
+            try
             {
                 try
                 {
@@ -62,11 +63,16 @@ namespace OpenSim.Region.Framework.Scenes
                     m_log.ErrorFormat("Add Entity failed: {0}", e.Message);
                 }
             }
+            finally
+            {
+                m_lock.ExitWriteLock();
+            }
         }
 
         public void InsertOrReplace(EntityBase entity)
         {
-            lock (m_lock)
+            m_lock.EnterWriteLock();
+            try
             {
                 try
                 {
@@ -78,15 +84,24 @@ namespace OpenSim.Region.Framework.Scenes
                     m_log.ErrorFormat("Insert or Replace Entity failed: {0}", e.Message);
                 }
             }
+            finally
+            {
+                m_lock.ExitWriteLock();
+            }
         }
 
         public void Clear()
         {
-            lock (m_lock)
+            m_lock.EnterWriteLock();
+            try
             {
                 m_eb_uuid.Clear();
                 m_eb_localID.Clear();
             }
+            finally
+            {
+                m_lock.ExitWriteLock();
+            }
         }
 
         public int Count
@@ -123,7 +138,8 @@ namespace OpenSim.Region.Framework.Scenes
 
         public bool Remove(uint localID)
         {
-            lock (m_lock)
+            m_lock.EnterWriteLock();
+            try
             {
                 try
                 {
@@ -141,11 +157,16 @@ namespace OpenSim.Region.Framework.Scenes
                     return false;
                 }
             }
+            finally
+            {
+                m_lock.ExitWriteLock();
+            }
         }
 
         public bool Remove(UUID id)
         {
-            lock (m_lock)
+            m_lock.EnterWriteLock();
+            try
             {
                 try 
                 {
@@ -163,13 +184,18 @@ namespace OpenSim.Region.Framework.Scenes
                     return false;
                 }
             }
+            finally
+            {
+                m_lock.ExitWriteLock();
+            }
         }
 
         public List<EntityBase> GetAllByType<T>()
         {
             List<EntityBase> tmp = new List<EntityBase>();
 
-            lock (m_lock)
+            m_lock.EnterReadLock();
+            try
             {
                 try
                 {
@@ -187,23 +213,33 @@ namespace OpenSim.Region.Framework.Scenes
                     tmp = null;
                 }
             }
+            finally
+            {
+                m_lock.ExitReadLock();
+            }
 
             return tmp;
         }
 
         public List<EntityBase> GetEntities()
         {
-            lock (m_lock)
+            m_lock.EnterReadLock();
+            try
             {
                 return new List<EntityBase>(m_eb_uuid.Values);
             }
+            finally
+            {
+                m_lock.ExitReadLock();
+            }
         }
 
         public EntityBase this[UUID id]
         {
             get
             {
-                lock (m_lock)
+                m_lock.EnterReadLock();
+                try
                 {
                     EntityBase entity;
                     if (m_eb_uuid.TryGetValue(id, out entity))
@@ -211,6 +247,10 @@ namespace OpenSim.Region.Framework.Scenes
                     else
                         return null;
                 }
+                finally
+                {
+                    m_lock.ExitReadLock();
+                }
             }
             set
             {
@@ -222,7 +262,8 @@ namespace OpenSim.Region.Framework.Scenes
         {
             get
             {
-                lock (m_lock)
+                m_lock.EnterReadLock();
+                try
                 {
                     EntityBase entity;
                     if (m_eb_localID.TryGetValue(localID, out entity))
@@ -230,6 +271,10 @@ namespace OpenSim.Region.Framework.Scenes
                     else
                         return null;
                 }
+                finally
+                {
+                    m_lock.ExitReadLock();
+                }
             }
             set
             {
@@ -239,18 +284,28 @@ namespace OpenSim.Region.Framework.Scenes
 
         public bool TryGetValue(UUID key, out EntityBase obj)
         {
-            lock (m_lock)
+            m_lock.EnterReadLock();
+            try
             {
                 return m_eb_uuid.TryGetValue(key, out obj);
             }
+            finally
+            {
+                m_lock.ExitReadLock();
+            }
         }
 
         public bool TryGetValue(uint key, out EntityBase obj)
         {
-            lock (m_lock)
+            m_lock.EnterReadLock();
+            try
             {
                 return m_eb_localID.TryGetValue(key, out obj);
             }
+            finally
+            {
+                m_lock.ExitReadLock();
+            }
         }
 
         /// <summary>
diff --git a/OpenSim/Region/Framework/Scenes/Scene.Inventory.cs b/OpenSim/Region/Framework/Scenes/Scene.Inventory.cs
index 31c0be1..1875c48 100644
--- a/OpenSim/Region/Framework/Scenes/Scene.Inventory.cs
+++ b/OpenSim/Region/Framework/Scenes/Scene.Inventory.cs
@@ -782,8 +782,12 @@ namespace OpenSim.Region.Framework.Scenes
         public void RemoveTaskInventory(IClientAPI remoteClient, UUID itemID, uint localID)
         {
             SceneObjectPart part = GetSceneObjectPart(localID);
-            SceneObjectGroup group = part.ParentGroup;
-            if (group != null)
+            SceneObjectGroup group = null;
+            if (part != null)
+            {
+                group = part.ParentGroup;
+            }            
+            if (part != null && group != null)
             {
                 TaskInventoryItem item = group.GetInventoryItem(localID, itemID);
                 if (item == null)
diff --git a/OpenSim/Region/Framework/Scenes/Scene.cs b/OpenSim/Region/Framework/Scenes/Scene.cs
index fc915a3..1f604c5 100644
--- a/OpenSim/Region/Framework/Scenes/Scene.cs
+++ b/OpenSim/Region/Framework/Scenes/Scene.cs
@@ -602,6 +602,8 @@ namespace OpenSim.Region.Framework.Scenes
 
             // Load region settings
             m_regInfo.RegionSettings = m_storageManager.DataStore.LoadRegionSettings(m_regInfo.RegionID);
+            m_regInfo.WindlightSettings = m_storageManager.DataStore.LoadRegionWindlightSettings(m_regInfo.RegionID);
+
             if (m_storageManager.EstateDataStore != null)
             {
                 m_regInfo.EstateSettings = m_storageManager.EstateDataStore.LoadEstateSettings(m_regInfo.RegionID, false);
@@ -1016,6 +1018,15 @@ namespace OpenSim.Region.Framework.Scenes
         /// <param name="seconds">float indicating duration before restart.</param>
         public virtual void Restart(float seconds)
         {
+            Restart(seconds, true);
+        }
+
+        /// <summary>
+        /// Given float seconds, this will restart the region. showDialog will optionally alert the users.
+        /// </summary>
+        /// <param name="seconds">float indicating duration before restart.</param>
+        public virtual void Restart(float seconds, bool showDialog)
+        {
             // notifications are done in 15 second increments
             // so ..   if the number of seconds is less then 15 seconds, it's not really a restart request
             // It's a 'Cancel restart' request.
@@ -1036,8 +1047,11 @@ namespace OpenSim.Region.Framework.Scenes
                 m_restartTimer.Elapsed += new ElapsedEventHandler(RestartTimer_Elapsed);
                 m_log.Info("[REGION]: Restarting Region in " + (seconds / 60) + " minutes");
                 m_restartTimer.Start();
-                m_dialogModule.SendNotificationToUsersInRegion(
+                if (showDialog)
+                {
+                    m_dialogModule.SendNotificationToUsersInRegion(
                     UUID.Random(), String.Empty, RegionInfo.RegionName + String.Format(": Restarting in {0} Minutes", (int)(seconds / 60.0)));
+                }
             }
         }
 
@@ -1393,16 +1407,16 @@ namespace OpenSim.Region.Framework.Scenes
                     // Check if any objects have reached their targets
                     CheckAtTargets();
 
-                    // Update SceneObjectGroups that have scheduled themselves for updates
-                    // Objects queue their updates onto all scene presences
-                    if (m_frame % m_update_objects == 0)
-                        m_sceneGraph.UpdateObjectGroups();
-
                     // Run through all ScenePresences looking for updates
                     // Presence updates and queued object updates for each presence are sent to clients
                     if (m_frame % m_update_presences == 0)
                         m_sceneGraph.UpdatePresences();
 
+                    // Update SceneObjectGroups that have scheduled themselves for updates
+                    // Objects queue their updates onto all scene presences
+                    if (m_frame % m_update_objects == 0)
+                        m_sceneGraph.UpdateObjectGroups();
+
                     int tmpPhysicsMS2 = Util.EnvironmentTickCount();
                     if ((m_frame % m_update_physics == 0) && m_physics_enabled)
                         m_sceneGraph.UpdatePreparePhysics();
@@ -1713,7 +1727,7 @@ namespace OpenSim.Region.Framework.Scenes
         public void SaveTerrain()
         {
             m_storageManager.DataStore.StoreTerrain(Heightmap.GetDoubles(), RegionInfo.RegionID);
-        }
+        }

 
         public void StoreWindlightProfile(RegionLightShareData wl)
         {
@@ -3413,6 +3427,9 @@ namespace OpenSim.Region.Framework.Scenes
 
             CapsModule.AddCapsHandler(agent.AgentID);
 
+            if ((teleportFlags & ((uint)TeleportFlags.ViaLandmark | (uint)TeleportFlags.ViaLocation | (uint)TeleportFlags.ViaLandmark | (uint)TeleportFlags.Default)) != 0)
+                System.Threading.Thread.Sleep(2000);
+
             if (!agent.child)
             {
                 if (TestBorderCross(agent.startpos,Cardinals.E))
@@ -3471,6 +3488,8 @@ namespace OpenSim.Region.Framework.Scenes
                     }
                 }
                 // Honor parcel landing type and position.
+                /*
+                ILandObject land = LandChannel.GetLandObject(agent.startpos.X, agent.startpos.Y);
                 if (land != null)
                 {
                     if (land.LandData.LandingType == (byte)1 && land.LandData.UserLocation != Vector3.Zero)
@@ -3478,6 +3497,7 @@ namespace OpenSim.Region.Framework.Scenes
                         agent.startpos = land.LandData.UserLocation;
                     }
                 }
+                */// This is now handled properly in ScenePresence.MakeRootAgent
             }
 
             agent.teleportFlags = teleportFlags;
diff --git a/OpenSim/Region/Framework/Scenes/SceneCommunicationService.cs b/OpenSim/Region/Framework/Scenes/SceneCommunicationService.cs
index bd8ccce..9d0e6f4 100644
--- a/OpenSim/Region/Framework/Scenes/SceneCommunicationService.cs
+++ b/OpenSim/Region/Framework/Scenes/SceneCommunicationService.cs
@@ -300,7 +300,7 @@ namespace OpenSim.Region.Framework.Scenes
                               d);
             }
         }
-       
+
         public List<GridRegion> RequestNamedRegions(string name, int maxNumber)
         {
             return m_scene.GridService.GetRegionsByName(UUID.Zero, name, maxNumber);
diff --git a/OpenSim/Region/Framework/Scenes/SceneGraph.cs b/OpenSim/Region/Framework/Scenes/SceneGraph.cs
index 23a4ee9..3a1962c 100644
--- a/OpenSim/Region/Framework/Scenes/SceneGraph.cs
+++ b/OpenSim/Region/Framework/Scenes/SceneGraph.cs
@@ -225,6 +225,30 @@ namespace OpenSim.Region.Framework.Scenes
         protected internal bool AddRestoredSceneObject(
             SceneObjectGroup sceneObject, bool attachToBackup, bool alreadyPersisted)
         {
+                // KF: Check for out-of-region, move inside and make static.
+                        Vector3 npos = new Vector3(sceneObject.RootPart.GroupPosition.X,
+                                                           sceneObject.RootPart.GroupPosition.Y,
+                                                           sceneObject.RootPart.GroupPosition.Z);
+                        if (!(((sceneObject.RootPart.Shape.PCode == (byte)PCode.Prim) && (sceneObject.RootPart.Shape.State != 0))) && (npos.X < 0.0 || npos.Y < 0.0 || npos.Z < 0.0 ||
+                            npos.X > Constants.RegionSize ||
+                            npos.Y > Constants.RegionSize))
+                        {
+                                if (npos.X < 0.0) npos.X = 1.0f;
+                                if (npos.Y < 0.0) npos.Y = 1.0f;
+                                if (npos.Z < 0.0) npos.Z = 0.0f;
+                                if (npos.X > Constants.RegionSize) npos.X = Constants.RegionSize - 1.0f;
+                                if (npos.Y > Constants.RegionSize) npos.Y = Constants.RegionSize - 1.0f;
+ 
+                foreach (SceneObjectPart part in sceneObject.Children.Values)
+                {
+                        part.GroupPosition = npos;
+                }
+                sceneObject.RootPart.Velocity = Vector3.Zero;
+                sceneObject.RootPart.AngularVelocity = Vector3.Zero;
+                sceneObject.RootPart.Acceleration = Vector3.Zero;
+                sceneObject.RootPart.Velocity = Vector3.Zero;
+                        }
+                        
             if (!alreadyPersisted)
             {
                 sceneObject.ForceInventoryPersistence();
diff --git a/OpenSim/Region/Framework/Scenes/SceneObjectGroup.Inventory.cs b/OpenSim/Region/Framework/Scenes/SceneObjectGroup.Inventory.cs
index 71354b4..8b58b3e 100644
--- a/OpenSim/Region/Framework/Scenes/SceneObjectGroup.Inventory.cs
+++ b/OpenSim/Region/Framework/Scenes/SceneObjectGroup.Inventory.cs
@@ -46,12 +46,12 @@ namespace OpenSim.Region.Framework.Scenes
         /// </summary>
         public void ForceInventoryPersistence()
         {
-            lock (m_parts)
+            lockPartsForRead(true);
+            List<SceneObjectPart> values = new List<SceneObjectPart>(m_parts.Values);
+            lockPartsForRead(false);
+            foreach (SceneObjectPart part in values)
             {
-                foreach (SceneObjectPart part in m_parts.Values)
-                {
-                    part.Inventory.ForceInventoryPersistence();
-                }
+                part.Inventory.ForceInventoryPersistence();
             }
         }
 
@@ -74,19 +74,17 @@ namespace OpenSim.Region.Framework.Scenes
         /// <summary>
         /// Stop the scripts contained in all the prims in this group
         /// </summary>
-        /// <param name="sceneObjectBeingDeleted">
-        /// Should be true if these scripts are being removed because the scene
-        /// object is being deleted.  This will prevent spurious updates to the client.
-        /// </param>
         public void RemoveScriptInstances(bool sceneObjectBeingDeleted)
         {
-            lock (m_parts)
+            lockPartsForRead(true);
+            List<SceneObjectPart> values = new List<SceneObjectPart>(m_parts.Values);
+            lockPartsForRead(false);
+            
+            foreach (SceneObjectPart part in values)
             {
-                foreach (SceneObjectPart part in m_parts.Values)
-                {
-                    part.Inventory.RemoveScriptInstances(sceneObjectBeingDeleted);
-                }
+                part.Inventory.RemoveScriptInstances(sceneObjectBeingDeleted);
             }
+            
         }
 
         /// <summary>
diff --git a/OpenSim/Region/Framework/Scenes/SceneObjectGroup.cs b/OpenSim/Region/Framework/Scenes/SceneObjectGroup.cs
index 8aefd50..7ed29a5 100644
--- a/OpenSim/Region/Framework/Scenes/SceneObjectGroup.cs
+++ b/OpenSim/Region/Framework/Scenes/SceneObjectGroup.cs
@@ -106,6 +106,72 @@ namespace OpenSim.Region.Framework.Scenes
         private bool m_hasGroupChanged = false;
         private long timeFirstChanged;
         private long timeLastChanged;
+        private System.Threading.ReaderWriterLockSlim m_partsLock = new System.Threading.ReaderWriterLockSlim();
+
+        public void lockPartsForRead(bool locked)
+        {
+            if (locked)
+            {
+                if (m_partsLock.RecursiveReadCount > 0)
+                {
+                    m_log.Error("[SceneObjectGroup.m_parts] Recursive read lock requested. This should not happen and means something needs to be fixed. For now though, it's safe to continue.");
+                    m_partsLock.ExitReadLock();
+                }
+                if (m_partsLock.RecursiveWriteCount > 0)
+                {
+                    m_log.Error("[SceneObjectGroup.m_parts] Recursive read lock requested. This should not happen and means something needs to be fixed.");
+                    m_partsLock.ExitWriteLock();
+                }
+
+                while (!m_partsLock.TryEnterReadLock(60000))
+                {
+                    m_log.Error("[SceneObjectGroup.m_parts] Thread lock detected while trying to aquire READ lock of m_parts in SceneObjectGroup. I'm going to try to solve the thread lock automatically to preserve region stability, but this needs to be fixed.");
+                    if (m_partsLock.IsWriteLockHeld)
+                    {
+                        m_partsLock = new System.Threading.ReaderWriterLockSlim();
+                    }
+                }
+            }
+            else
+            {
+                if (m_partsLock.RecursiveReadCount > 0)
+                {
+                    m_partsLock.ExitReadLock();
+                }
+            }
+        }
+        public void lockPartsForWrite(bool locked)
+        {
+            if (locked)
+            {
+                if (m_partsLock.RecursiveReadCount > 0)
+                {
+                    m_log.Error("[SceneObjectGroup.m_parts] Recursive write lock requested. This should not happen and means something needs to be fixed. For now though, it's safe to continue.");
+                    m_partsLock.ExitReadLock();
+                }
+                if (m_partsLock.RecursiveWriteCount > 0)
+                {
+                    m_log.Error("[SceneObjectGroup.m_parts] Recursive write lock requested. This should not happen and means something needs to be fixed.");
+                    m_partsLock.ExitWriteLock();
+                }
+
+                while (!m_partsLock.TryEnterWriteLock(60000))
+                {
+                    m_log.Error("[SceneObjectGroup.m_parts] Thread lock detected while trying to aquire WRITE lock of m_scripts in XEngine. I'm going to try to solve the thread lock automatically to preserve region stability, but this needs to be fixed.");
+                    if (m_partsLock.IsWriteLockHeld)
+                    {
+                        m_partsLock = new System.Threading.ReaderWriterLockSlim();
+                    }
+                }
+            }
+            else
+            {
+                if (m_partsLock.RecursiveWriteCount > 0)
+                {
+                    m_partsLock.ExitWriteLock();
+                }
+            }
+        }
 
         public bool HasGroupChanged
         {
@@ -258,13 +324,16 @@ namespace OpenSim.Region.Framework.Scenes
             set
             {
                 m_regionHandle = value;
-                lock (m_parts)
+                lockPartsForRead(true);
                 {
                     foreach (SceneObjectPart part in m_parts.Values)
                     {
+                        
                         part.RegionHandle = m_regionHandle;
+                        
                     }
                 }
+                lockPartsForRead(false);
             }
         }
 
@@ -298,6 +367,9 @@ namespace OpenSim.Region.Framework.Scenes
                 {
                     m_scene.CrossPrimGroupIntoNewRegion(val, this, true);
                 }
+
+                lockPartsForRead(true);
+
                 if (RootPart.GetStatusSandbox())
                 {
                     if (Util.GetDistanceTo(RootPart.StatusSandboxPos, value) > 10)
@@ -305,17 +377,18 @@ namespace OpenSim.Region.Framework.Scenes
                         RootPart.ScriptSetPhysicsStatus(false);
                         Scene.SimChat(Utils.StringToBytes("Hit Sandbox Limit"),
                               ChatTypeEnum.DebugChannel, 0x7FFFFFFF, RootPart.AbsolutePosition, Name, UUID, false);
+                        lockPartsForRead(false);
                         return;
                     }
                 }
-                lock (m_parts)
+
+                foreach (SceneObjectPart part in m_parts.Values)
                 {
-                    foreach (SceneObjectPart part in m_parts.Values)
-                    {
-                        part.GroupPosition = val;
-                    }
+                    part.GroupPosition = val;
                 }
 
+                lockPartsForRead(false);
+
                 //if (m_rootPart.PhysActor != null)
                 //{
                 //m_rootPart.PhysActor.Position =
@@ -504,13 +577,16 @@ namespace OpenSim.Region.Framework.Scenes
 
         public void SetFromItemID(UUID AssetId)
         {
-            lock (m_parts)
+            lockPartsForRead(true);
             {
                 foreach (SceneObjectPart part in m_parts.Values)
                 {
+                    
                     part.FromItemID = AssetId;
+                    
                 }
             }
+            lockPartsForRead(false);
         }
 
         public UUID GetFromItemID()
@@ -579,10 +655,11 @@ namespace OpenSim.Region.Framework.Scenes
             Vector3 maxScale = Vector3.Zero;
             Vector3 finalScale = new Vector3(0.5f, 0.5f, 0.5f);
 
-            lock (m_parts)
+            lockPartsForRead(true);
             {
                 foreach (SceneObjectPart part in m_parts.Values)
                 {
+                    
                     Vector3 partscale = part.Scale;
                     Vector3 partoffset = part.OffsetPosition;
 
@@ -593,8 +670,11 @@ namespace OpenSim.Region.Framework.Scenes
                     maxScale.X = (partscale.X + partoffset.X > maxScale.X) ? partscale.X + partoffset.X : maxScale.X;
                     maxScale.Y = (partscale.Y + partoffset.Y > maxScale.Y) ? partscale.Y + partoffset.Y : maxScale.Y;
                     maxScale.Z = (partscale.Z + partoffset.Z > maxScale.Z) ? partscale.Z + partoffset.Z : maxScale.Z;
+                    
                 }
             }
+            lockPartsForRead(false);
+
             finalScale.X = (minScale.X > maxScale.X) ? minScale.X : maxScale.X;
             finalScale.Y = (minScale.Y > maxScale.Y) ? minScale.Y : maxScale.Y;
             finalScale.Z = (minScale.Z > maxScale.Z) ? minScale.Z : maxScale.Z;
@@ -610,10 +690,11 @@ namespace OpenSim.Region.Framework.Scenes
 
             EntityIntersection result = new EntityIntersection();
 
-            lock (m_parts)
+            lockPartsForRead(true);
             {
                 foreach (SceneObjectPart part in m_parts.Values)
                 {
+                    
                     // Temporary commented to stop compiler warning
                     //Vector3 partPosition =
                     //    new Vector3(part.AbsolutePosition.X, part.AbsolutePosition.Y, part.AbsolutePosition.Z);
@@ -641,8 +722,10 @@ namespace OpenSim.Region.Framework.Scenes
                             result.distance = inter.distance;
                         }
                     }
+                    
                 }
             }
+            lockPartsForRead(false);
             return result;
         }
 
@@ -655,10 +738,11 @@ namespace OpenSim.Region.Framework.Scenes
         public Vector3 GetAxisAlignedBoundingBox(out float offsetHeight)
         {
             float maxX = -256f, maxY = -256f, maxZ = -256f, minX = 256f, minY = 256f, minZ = 256f;
-            lock (m_parts)
+            lockPartsForRead(true);
             {
                 foreach (SceneObjectPart part in m_parts.Values)
                 {
+                    
                     Vector3 worldPos = part.GetWorldPosition();
                     Vector3 offset = worldPos - AbsolutePosition;
                     Quaternion worldRot;
@@ -717,6 +801,8 @@ namespace OpenSim.Region.Framework.Scenes
                     backBottomRight.Y = orig.Y + (part.Scale.Y / 2);
                     backBottomRight.Z = orig.Z - (part.Scale.Z / 2);
 
+                    
+
                     //m_log.InfoFormat("pre corner 1 is {0} {1} {2}", frontTopLeft.X, frontTopLeft.Y, frontTopLeft.Z);
                     //m_log.InfoFormat("pre corner 2 is {0} {1} {2}", frontTopRight.X, frontTopRight.Y, frontTopRight.Z);
                     //m_log.InfoFormat("pre corner 3 is {0} {1} {2}", frontBottomRight.X, frontBottomRight.Y, frontBottomRight.Z);
@@ -888,6 +974,7 @@ namespace OpenSim.Region.Framework.Scenes
                         minZ = backBottomLeft.Z;
                 }
             }
+            lockPartsForRead(false);
 
             Vector3 boundingBox = new Vector3(maxX - minX, maxY - minY, maxZ - minZ);
 
@@ -916,17 +1003,20 @@ namespace OpenSim.Region.Framework.Scenes
             Dictionary<UUID,string> states = new Dictionary<UUID,string>();
 
             // Capture script state while holding the lock
-            lock (m_parts)
+            lockPartsForRead(true);
             {
                 foreach (SceneObjectPart part in m_parts.Values)
                 {
+                    
                     Dictionary<UUID,string> pstates = part.Inventory.GetScriptStates();
                     foreach (UUID itemid in pstates.Keys)
                     {
                         states.Add(itemid, pstates[itemid]);
                     }
+                    
                 }
             }
+            lockPartsForRead(false);
 
             if (states.Count > 0)
             {
@@ -1094,13 +1184,16 @@ namespace OpenSim.Region.Framework.Scenes
 
         public override void UpdateMovement()
         {
-            lock (m_parts)
+            lockPartsForRead(true);
             {
                 foreach (SceneObjectPart part in m_parts.Values)
                 {
+                    
                     part.UpdateMovement();
+                    
                 }
             }
+            lockPartsForRead(false);
         }
 
         public ushort GetTimeDilation()
@@ -1144,7 +1237,7 @@ namespace OpenSim.Region.Framework.Scenes
         /// <param name="part"></param>
         public void AddPart(SceneObjectPart part)
         {
-            lock (m_parts)
+            lockPartsForWrite(true);
             {
                 part.SetParent(this);
                 m_parts.Add(part.UUID, part);
@@ -1154,6 +1247,7 @@ namespace OpenSim.Region.Framework.Scenes
                 if (part.LinkNum == 2 && RootPart != null)
                     RootPart.LinkNum = 1;
             }
+            lockPartsForWrite(false);
         }
 
         /// <summary>
@@ -1161,28 +1255,33 @@ namespace OpenSim.Region.Framework.Scenes
         /// </summary>
         private void UpdateParentIDs()
         {
-            lock (m_parts)
+            lockPartsForRead(true);
             {
                 foreach (SceneObjectPart part in m_parts.Values)
                 {
+                    
                     if (part.UUID != m_rootPart.UUID)
                     {
                         part.ParentID = m_rootPart.LocalId;
                     }
+                    
                 }
             }
+            lockPartsForRead(false);
         }
 
         public void RegenerateFullIDs()
         {
-            lock (m_parts)
+            lockPartsForRead(true);
             {
                 foreach (SceneObjectPart part in m_parts.Values)
                 {
+                    
                     part.UUID = UUID.Random();
-
+                    
                 }
             }
+            lockPartsForRead(false);
         }
 
         // helper provided for parts.
@@ -1263,27 +1362,32 @@ namespace OpenSim.Region.Framework.Scenes
 
             DetachFromBackup();
 
-            lock (m_parts)
+            lockPartsForRead(true);
+            List<SceneObjectPart> values = new List<SceneObjectPart>(m_parts.Values);
+            lockPartsForRead(false);
+            
+            foreach (SceneObjectPart part in values)
             {
-                foreach (SceneObjectPart part in m_parts.Values)
-                {
 //                    part.Inventory.RemoveScriptInstances();
-                    Scene.ForEachScenePresence(delegate(ScenePresence avatar)
+                
+                Scene.ForEachScenePresence(delegate (ScenePresence sp)
+                {
+                    if (sp.ParentID == LocalId)
                     {
-                        if (avatar.ParentID == LocalId)
-                        {
-                            avatar.StandUp();
-                        }
+                        sp.StandUp();
+                    }
 
-                        if (!silent)
-                        {
-                            part.UpdateFlag = 0;
-                            if (part == m_rootPart)
-                                avatar.ControllingClient.SendKillObject(m_regionHandle, part.LocalId);
-                        }
-                    });
-                }
+                    if (!silent)
+                    {
+                        part.UpdateFlag = 0;
+                        if (part == m_rootPart)
+                            sp.ControllingClient.SendKillObject(m_regionHandle, part.LocalId);
+                    }
+                });
+                
             }
+            
+          
         }
 
         public void AddScriptLPS(int count)
@@ -1308,17 +1412,20 @@ namespace OpenSim.Region.Framework.Scenes
 
             scriptEvents aggregateScriptEvents=0;
 
-            lock (m_parts)
+            lockPartsForRead(true);
             {
                 foreach (SceneObjectPart part in m_parts.Values)
                 {
+                    
                     if (part == null)
                         continue;
                     if (part != RootPart)
                         part.ObjectFlags = objectflagupdate;
                     aggregateScriptEvents |= part.AggregateScriptEvents;
+                    
                 }
             }
+            lockPartsForRead(false);
 
             m_scriptListens_atTarget = ((aggregateScriptEvents & scriptEvents.at_target) != 0);
             m_scriptListens_notAtTarget = ((aggregateScriptEvents & scriptEvents.not_at_target) != 0);
@@ -1360,42 +1467,52 @@ namespace OpenSim.Region.Framework.Scenes
         /// <param name="m_physicalPrim"></param>
         public void ApplyPhysics(bool m_physicalPrim)
         {
-            lock (m_parts)
+            lockPartsForRead(true);
+            
+            if (m_parts.Count > 1)
             {
-                if (m_parts.Count > 1)
+                List<SceneObjectPart> values = new List<SceneObjectPart>(m_parts.Values);
+                lockPartsForRead(false);
+                m_rootPart.ApplyPhysics(m_rootPart.GetEffectiveObjectFlags(), m_rootPart.VolumeDetectActive, m_physicalPrim);
+                foreach (SceneObjectPart part in values)
                 {
-                    m_rootPart.ApplyPhysics(m_rootPart.GetEffectiveObjectFlags(), m_rootPart.VolumeDetectActive, m_physicalPrim);
-                    foreach (SceneObjectPart part in m_parts.Values)
+                    
+                    if (part.LocalId != m_rootPart.LocalId)
                     {
-                        if (part.LocalId != m_rootPart.LocalId)
-                        {
-                            part.ApplyPhysics(m_rootPart.GetEffectiveObjectFlags(), part.VolumeDetectActive, m_physicalPrim);
-                        }
+                        part.ApplyPhysics(m_rootPart.GetEffectiveObjectFlags(), part.VolumeDetectActive, m_physicalPrim);
                     }
                     
-                    // Hack to get the physics scene geometries in the right spot
-                    ResetChildPrimPhysicsPositions();
-                }
-                else
-                {
-                    m_rootPart.ApplyPhysics(m_rootPart.GetEffectiveObjectFlags(), m_rootPart.VolumeDetectActive, m_physicalPrim);
                 }
+                // Hack to get the physics scene geometries in the right spot
+                ResetChildPrimPhysicsPositions();
+            }
+            else
+            {
+                lockPartsForRead(false);
+                m_rootPart.ApplyPhysics(m_rootPart.GetEffectiveObjectFlags(), m_rootPart.VolumeDetectActive, m_physicalPrim);
             }
         }
 
         public void SetOwnerId(UUID userId)
         {
-            ForEachPart(delegate(SceneObjectPart part) { part.OwnerID = userId; });
+                ForEachPart(delegate(SceneObjectPart part) 
+                {
+                    
+                    part.OwnerID = userId;
+                    
+                });
         }
 
         public void ForEachPart(Action<SceneObjectPart> whatToDo)
         {
-            lock (m_parts)
+            lockPartsForRead(true);
+            List<SceneObjectPart> values = new List<SceneObjectPart>(m_parts.Values);
+            lockPartsForRead(false);    
+            foreach (SceneObjectPart part in values)
             {
-                foreach (SceneObjectPart part in m_parts.Values)
-                {
-                    whatToDo(part);
-                }
+                
+                whatToDo(part);
+                
             }
         }
 
@@ -1493,10 +1610,11 @@ namespace OpenSim.Region.Framework.Scenes
             RootPart.SendFullUpdate(
                 remoteClient, m_scene.Permissions.GenerateClientFlags(remoteClient.AgentId, RootPart.UUID));
 
-            lock (m_parts)
+            lockPartsForRead(true);
             {
                 foreach (SceneObjectPart part in m_parts.Values)
                 {
+                    
                     if (part != RootPart)
                         part.SendFullUpdate(
                             remoteClient, m_scene.Permissions.GenerateClientFlags(remoteClient.AgentId, part.UUID));
@@ -1570,10 +1688,11 @@ namespace OpenSim.Region.Framework.Scenes
             
             List<SceneObjectPart> partList;
 
-            lock (m_parts)
-            {
-                partList = new List<SceneObjectPart>(m_parts.Values);
-            }
+            lockPartsForRead(true);
+            
+            partList = new List<SceneObjectPart>(m_parts.Values);
+            
+            lockPartsForRead(false);
             
             partList.Sort(delegate(SceneObjectPart p1, SceneObjectPart p2)
                 {
@@ -1796,13 +1915,40 @@ namespace OpenSim.Region.Framework.Scenes
             }
         }
         
+        public void rotLookAt(Quaternion target, float strength, float damping)
+        {
+            SceneObjectPart rootpart = m_rootPart;
+            if (rootpart != null)
+            {
+                if (IsAttachment)
+                {
+                /*
+                    ScenePresence avatar = m_scene.GetScenePresence(rootpart.AttachedAvatar);
+                    if (avatar != null)
+                    {
+                    Rotate the Av?
+                    } */
+                }
+                else
+                {
+                    if (rootpart.PhysActor != null)
+                    {                                                                   // APID must be implemented in your physics system for this to function.
+                        rootpart.PhysActor.APIDTarget = new Quaternion(target.X, target.Y, target.Z, target.W);
+                        rootpart.PhysActor.APIDStrength = strength;
+                        rootpart.PhysActor.APIDDamping = damping;
+                        rootpart.PhysActor.APIDActive = true;
+                    }
+                }
+            }
+        }
+
         public void stopLookAt()
         {
             SceneObjectPart rootpart = m_rootPart;
             if (rootpart != null)
             {
                 if (rootpart.PhysActor != null)
-                {
+                {                                                       // APID must be implemented in your physics system for this to function.
                     rootpart.PhysActor.APIDActive = false;
                 }
             }
@@ -1870,10 +2016,11 @@ namespace OpenSim.Region.Framework.Scenes
             SceneObjectPart newPart = part.Copy(m_scene.AllocateLocalId(), OwnerID, GroupID, m_parts.Count, userExposed);
             newPart.SetParent(this);
 
-            lock (m_parts)
+            lockPartsForWrite(true);
             {
                 m_parts.Add(newPart.UUID, newPart);
             }
+            lockPartsForWrite(false);
 
             SetPartAsNonRoot(newPart);
 
@@ -1936,7 +2083,7 @@ namespace OpenSim.Region.Framework.Scenes
             //if ((RootPart.Flags & PrimFlags.TemporaryOnRez) != 0)
             //    return;
 
-            lock (m_parts)
+            lockPartsForRead(true);
             {
                 bool UsePhysics = ((RootPart.Flags & PrimFlags.Physics) != 0);
 
@@ -1956,9 +2103,12 @@ namespace OpenSim.Region.Framework.Scenes
                 {
                     if (!IsSelected)
                         part.UpdateLookAt();
+
                     part.SendScheduledUpdates();
+                    
                 }
             }
+            lockPartsForRead(false);
         }
 
         public void ScheduleFullUpdateToAvatar(ScenePresence presence)
@@ -1967,27 +2117,29 @@ namespace OpenSim.Region.Framework.Scenes
             
             RootPart.AddFullUpdateToAvatar(presence);
 
-            lock (m_parts)
+            lockPartsForRead(true);
             {
                 foreach (SceneObjectPart part in m_parts.Values)
                 {
+                    
                     if (part != RootPart)
                         part.AddFullUpdateToAvatar(presence);
+                    
                 }
             }
+            lockPartsForRead(false);
         }
 
         public void ScheduleTerseUpdateToAvatar(ScenePresence presence)
         {
-//            m_log.DebugFormat("[SOG]: Scheduling terse update for {0} {1} just to avatar {2}", Name, UUID, presence.Name);
-            
-            lock (m_parts)
+            lockPartsForRead(true);
+
+            foreach (SceneObjectPart part in m_parts.Values)
             {
-                foreach (SceneObjectPart part in m_parts.Values)
-                {
-                    part.AddTerseUpdateToAvatar(presence);
-                }
+                part.AddTerseUpdateToAvatar(presence);
             }
+
+            lockPartsForRead(false);
         }
 
         /// <summary>
@@ -2000,14 +2152,17 @@ namespace OpenSim.Region.Framework.Scenes
             checkAtTargets();
             RootPart.ScheduleFullUpdate();
 
-            lock (m_parts)
+            lockPartsForRead(true);
             {
                 foreach (SceneObjectPart part in m_parts.Values)
                 {
+                    
                     if (part != RootPart)
                         part.ScheduleFullUpdate();
+                    
                 }
             }
+            lockPartsForRead(false);
         }
 
         /// <summary>
@@ -2015,15 +2170,14 @@ namespace OpenSim.Region.Framework.Scenes
         /// </summary>
         public void ScheduleGroupForTerseUpdate()
         {
-//            m_log.DebugFormat("[SOG]: Scheduling terse update for {0} {1}", Name, UUID);
-            
-            lock (m_parts)
+            lockPartsForRead(true);
+
+            foreach (SceneObjectPart part in m_parts.Values)
             {
-                foreach (SceneObjectPart part in m_parts.Values)
-                {
-                    part.ScheduleTerseUpdate();
-                }
+                part.ScheduleTerseUpdate();
             }
+
+            lockPartsForRead(false);
         }
 
         /// <summary>
@@ -2038,14 +2192,17 @@ namespace OpenSim.Region.Framework.Scenes
             
             RootPart.SendFullUpdateToAllClients();
 
-            lock (m_parts)
+            lockPartsForRead(true);
             {
                 foreach (SceneObjectPart part in m_parts.Values)
                 {
+                    
                     if (part != RootPart)
                         part.SendFullUpdateToAllClients();
+                    
                 }
             }
+            lockPartsForRead(false);
         }
 
         /// <summary>
@@ -2077,14 +2234,15 @@ namespace OpenSim.Region.Framework.Scenes
         {
             if (IsDeleted)
                 return;
-            
-            lock (m_parts)
+
+            lockPartsForRead(true);
             {
                 foreach (SceneObjectPart part in m_parts.Values)
                 {
                     part.SendTerseUpdateToAllClients();
                 }
             }
+            lockPartsForRead(false);
         }
 
         #endregion
@@ -2098,16 +2256,18 @@ namespace OpenSim.Region.Framework.Scenes
         /// <returns>null if no child part with that linknum or child part</returns>
         public SceneObjectPart GetLinkNumPart(int linknum)
         {
-            lock (m_parts)
+            lockPartsForRead(true);
             {
                 foreach (SceneObjectPart part in m_parts.Values)
                 {
                     if (part.LinkNum == linknum)
                     {
+                        lockPartsForRead(false);
                         return part;
                     }
                 }
             }
+            lockPartsForRead(false);
 
             return null;
         }
@@ -2135,17 +2295,19 @@ namespace OpenSim.Region.Framework.Scenes
         public SceneObjectPart GetChildPart(uint localID)
         {
             //m_log.DebugFormat("Entered looking for {0}", localID);
-            lock (m_parts)
+            lockPartsForRead(true);
             {
                 foreach (SceneObjectPart part in m_parts.Values)
                 {
                     //m_log.DebugFormat("Found {0}", part.LocalId);
                     if (part.LocalId == localID)
                     {
+                        lockPartsForRead(false);
                         return part;
                     }
                 }
             }
+            lockPartsForRead(false);
 
             return null;
         }
@@ -2175,17 +2337,19 @@ namespace OpenSim.Region.Framework.Scenes
         public bool HasChildPrim(uint localID)
         {
             //m_log.DebugFormat("Entered HasChildPrim looking for {0}", localID);
-            lock (m_parts)
+            lockPartsForRead(true);
             {
                 foreach (SceneObjectPart part in m_parts.Values)
                 {
                     //m_log.DebugFormat("Found {0}", part.LocalId);
                     if (part.LocalId == localID)
                     {
+                        lockPartsForRead(false);
                         return true;
                     }
                 }
             }
+            lockPartsForRead(false);
 
             return false;
         }
@@ -2235,53 +2399,57 @@ namespace OpenSim.Region.Framework.Scenes
             if (m_rootPart.LinkNum == 0)
                 m_rootPart.LinkNum = 1;
 
-            lock (m_parts)
-            {
-                m_parts.Add(linkPart.UUID, linkPart);
+            lockPartsForWrite(true);
+            
+            m_parts.Add(linkPart.UUID, linkPart);
+
+            lockPartsForWrite(false);
 
-                // Insert in terms of link numbers, the new links
-                // before the current ones (with the exception of 
-                // the root prim. Shuffle the old ones up
-                foreach (KeyValuePair<UUID, SceneObjectPart> kvp in m_parts) 
+            // Insert in terms of link numbers, the new links
+            // before the current ones (with the exception of 
+            // the root prim. Shuffle the old ones up
+            lockPartsForRead(true);
+            foreach (KeyValuePair<UUID, SceneObjectPart> kvp in m_parts) 
+            {
+                if (kvp.Value.LinkNum != 1)
                 {
-                    if (kvp.Value.LinkNum != 1)
-                    {
-                        // Don't update root prim link number
-                        kvp.Value.LinkNum += objectGroup.PrimCount;
-                    }
+                    // Don't update root prim link number
+                    kvp.Value.LinkNum += objectGroup.PrimCount;
                 }
+            }
+            lockPartsForRead(false);
 
-                linkPart.LinkNum = 2;
+            linkPart.LinkNum = 2;
 
-                linkPart.SetParent(this);
-                linkPart.AddFlag(PrimFlags.CreateSelected);
+            linkPart.SetParent(this);
+            linkPart.AddFlag(PrimFlags.CreateSelected);
 
-                //if (linkPart.PhysActor != null)
-                //{
-                // m_scene.PhysicsScene.RemovePrim(linkPart.PhysActor);
-        
-                //linkPart.PhysActor = null;
-                //}
+            //if (linkPart.PhysActor != null)
+            //{
+            // m_scene.PhysicsScene.RemovePrim(linkPart.PhysActor);
+    
+            //linkPart.PhysActor = null;
+            //}
 
-                //TODO: rest of parts
-                int linkNum = 3;
-                foreach (SceneObjectPart part in objectGroup.Children.Values)
+            //TODO: rest of parts
+            int linkNum = 3;
+            foreach (SceneObjectPart part in objectGroup.Children.Values)
+            {
+                if (part.UUID != objectGroup.m_rootPart.UUID)
                 {
-                    if (part.UUID != objectGroup.m_rootPart.UUID)
-                    {
-                        LinkNonRootPart(part, oldGroupPosition, oldRootRotation, linkNum++);
-                    }
-                    part.ClearUndoState();
+                    LinkNonRootPart(part, oldGroupPosition, oldRootRotation, linkNum++);
                 }
+                part.ClearUndoState();
             }
 
             m_scene.UnlinkSceneObject(objectGroup.UUID, true);
             objectGroup.m_isDeleted = true;
+
+            objectGroup.lockPartsForWrite(true);
             
-            lock (objectGroup.m_parts)
-            {
-                objectGroup.m_parts.Clear();
-            }
+            objectGroup.m_parts.Clear();
+            
+            objectGroup.lockPartsForWrite(false);
             
             // Can't do this yet since backup still makes use of the root part without any synchronization
 //            objectGroup.m_rootPart = null;
@@ -2351,11 +2519,12 @@ namespace OpenSim.Region.Framework.Scenes
             Quaternion worldRot = linkPart.GetWorldRotation();
 
             // Remove the part from this object
-            lock (m_parts)
+            lockPartsForWrite(true);
             {
                 m_parts.Remove(linkPart.UUID);
             }
-
+            lockPartsForWrite(false);
+            lockPartsForRead(true);
             if (m_parts.Count == 1 && RootPart != null) //Single prim is left
                 RootPart.LinkNum = 0;
             else
@@ -2366,6 +2535,7 @@ namespace OpenSim.Region.Framework.Scenes
                         p.LinkNum--;
                 }
             }
+            lockPartsForRead(false);
 
             linkPart.ParentID = 0;
             linkPart.LinkNum = 0;
@@ -2687,9 +2857,12 @@ namespace OpenSim.Region.Framework.Scenes
 
             if (selectionPart != null)
             {
-                lock (m_parts)
+                lockPartsForRead(true);
+                List<SceneObjectPart> parts = new List<SceneObjectPart>(m_parts.Values);
+                lockPartsForRead(false);
+                foreach (SceneObjectPart part in parts)
                 {
-                    foreach (SceneObjectPart part in m_parts.Values)
+                    if (part.Scale.X > 10.0 || part.Scale.Y > 10.0 || part.Scale.Z > 10.0)
                     {
                         if (part.Scale.X > m_scene.RegionInfo.PhysPrimMax || 
                             part.Scale.Y > m_scene.RegionInfo.PhysPrimMax ||
@@ -2699,12 +2872,13 @@ namespace OpenSim.Region.Framework.Scenes
                             break;
                         }
                     }
+                }
 
-                    foreach (SceneObjectPart part in m_parts.Values)
-                    {
-                        part.UpdatePrimFlags(UsePhysics, IsTemporary, IsPhantom, IsVolumeDetect);
-                    }
+                foreach (SceneObjectPart part in parts)
+                {
+                    part.UpdatePrimFlags(UsePhysics, IsTemporary, IsPhantom, IsVolumeDetect);
                 }
+                
             }
         }
 
@@ -2790,11 +2964,9 @@ namespace OpenSim.Region.Framework.Scenes
                 scale.Y = m_scene.m_maxNonphys;
             if (scale.Z > m_scene.m_maxNonphys)
                 scale.Z = m_scene.m_maxNonphys;
-
             SceneObjectPart part = GetChildPart(localID);
             if (part != null)
             {
-                part.Resize(scale);
                 if (part.PhysActor != null)
                 {
                     if (part.PhysActor.IsPhysical)
@@ -2809,7 +2981,7 @@ namespace OpenSim.Region.Framework.Scenes
                     part.PhysActor.Size = scale;
                     m_scene.PhysicsScene.AddPhysicsActorTaint(part.PhysActor);
                 }
-                //if (part.UUID != m_rootPart.UUID)
+                part.Resize(scale);
 
                 HasGroupChanged = true;
                 ScheduleGroupForFullUpdate();
@@ -2851,73 +3023,71 @@ namespace OpenSim.Region.Framework.Scenes
                 float y = (scale.Y / part.Scale.Y);
                 float z = (scale.Z / part.Scale.Z);
 
-                lock (m_parts)
+                lockPartsForRead(true);
+                if (x > 1.0f || y > 1.0f || z > 1.0f)
                 {
-                    if (x > 1.0f || y > 1.0f || z > 1.0f)
+                    foreach (SceneObjectPart obPart in m_parts.Values)
                     {
-                        foreach (SceneObjectPart obPart in m_parts.Values)
+                        if (obPart.UUID != m_rootPart.UUID)
                         {
-                            if (obPart.UUID != m_rootPart.UUID)
-                            {
-                                obPart.IgnoreUndoUpdate = true;
-                                Vector3 oldSize = new Vector3(obPart.Scale);
+                            Vector3 oldSize = new Vector3(obPart.Scale);
+                            obPart.IgnoreUndoUpdate = true;
 
-                                float f = 1.0f;
-                                float a = 1.0f;
+                            float f = 1.0f;
+                            float a = 1.0f;
 
-                                if (part.PhysActor != null && part.PhysActor.IsPhysical)
+                            if (part.PhysActor != null && part.PhysActor.IsPhysical)
+                            {
+                                if (oldSize.X*x > m_scene.m_maxPhys)
                                 {
-                                    if (oldSize.X*x > m_scene.m_maxPhys)
-                                    {
-                                        f = m_scene.m_maxPhys / oldSize.X;
-                                        a = f / x;
-                                        x *= a;
-                                        y *= a;
-                                        z *= a;
-                                    }
-                                    if (oldSize.Y*y > m_scene.m_maxPhys)
-                                    {
-                                        f = m_scene.m_maxPhys / oldSize.Y;
-                                        a = f / y;
-                                        x *= a;
-                                        y *= a;
-                                        z *= a;
-                                    }
-                                    if (oldSize.Z*z > m_scene.m_maxPhys)
-                                    {
-                                        f = m_scene.m_maxPhys / oldSize.Z;
-                                        a = f / z;
-                                        x *= a;
-                                        y *= a;
-                                        z *= a;
-                                    }
+                                    f = m_scene.m_maxPhys / oldSize.X;
+                                    a = f / x;
+                                    x *= a;
+                                    y *= a;
+                                    z *= a;
                                 }
-                                else
+                                if (oldSize.Y*y > m_scene.m_maxPhys)
+                                {
+                                    f = m_scene.m_maxPhys / oldSize.Y;
+                                    a = f / y;
+                                    x *= a;
+                                    y *= a;
+                                    z *= a;
+                                }
+                                if (oldSize.Z*z > m_scene.m_maxPhys)
+                                {
+                                    f = m_scene.m_maxPhys / oldSize.Z;
+                                    a = f / z;
+                                    x *= a;
+                                    y *= a;
+                                    z *= a;
+                                }
+                            }
+                            else
+                            {
+                                if (oldSize.X*x > m_scene.m_maxNonphys)
+                                {
+                                    f = m_scene.m_maxNonphys / oldSize.X;
+                                    a = f / x;
+                                    x *= a;
+                                    y *= a;
+                                    z *= a;
+                                }
+                                if (oldSize.Y*y > m_scene.m_maxNonphys)
                                 {
-                                    if (oldSize.X*x > m_scene.m_maxNonphys)
-                                    {
-                                        f = m_scene.m_maxNonphys / oldSize.X;
-                                        a = f / x;
-                                        x *= a;
-                                        y *= a;
-                                        z *= a;
-                                    }
-                                    if (oldSize.Y*y > m_scene.m_maxNonphys)
-                                    {
-                                        f = m_scene.m_maxNonphys / oldSize.Y;
-                                        a = f / y;
-                                        x *= a;
-                                        y *= a;
-                                        z *= a;
-                                    }
-                                    if (oldSize.Z*z > m_scene.m_maxNonphys)
-                                    {
-                                        f = m_scene.m_maxNonphys / oldSize.Z;
-                                        a = f / z;
-                                        x *= a;
-                                        y *= a;
-                                        z *= a;
-                                    }
+                                    f = m_scene.m_maxNonphys / oldSize.Y;
+                                    a = f / y;
+                                    x *= a;
+                                    y *= a;
+                                    z *= a;
+                                }
+                                if (oldSize.Z*z > m_scene.m_maxNonphys)
+                                {
+                                    f = m_scene.m_maxNonphys / oldSize.Z;
+                                    a = f / z;
+                                    x *= a;
+                                    y *= a;
+                                    z *= a;
                                 }
                                 obPart.IgnoreUndoUpdate = false;
                                 obPart.StoreUndoState();
@@ -2925,6 +3095,7 @@ namespace OpenSim.Region.Framework.Scenes
                         }
                     }
                 }
+                lockPartsForRead(false);
 
                 Vector3 prevScale = part.Scale;
                 prevScale.X *= x;
@@ -2932,7 +3103,7 @@ namespace OpenSim.Region.Framework.Scenes
                 prevScale.Z *= z;
                 part.Resize(prevScale);
 
-                lock (m_parts)
+                lockPartsForRead(true);
                 {
                     foreach (SceneObjectPart obPart in m_parts.Values)
                     {
@@ -2954,6 +3125,7 @@ namespace OpenSim.Region.Framework.Scenes
                         obPart.StoreUndoState();
                     }
                 }
+                lockPartsForRead(false);
 
                 if (part.PhysActor != null)
                 {
@@ -3056,7 +3228,7 @@ namespace OpenSim.Region.Framework.Scenes
             axDiff *= Quaternion.Inverse(partRotation);
             diff = axDiff;
 
-            lock (m_parts)
+            lockPartsForRead(true);
             {
                 foreach (SceneObjectPart obPart in m_parts.Values)
                 {
@@ -3066,6 +3238,7 @@ namespace OpenSim.Region.Framework.Scenes
                     }
                 }
             }
+            lockPartsForRead(false);
 
             AbsolutePosition = newPos;
 
@@ -3199,25 +3372,25 @@ namespace OpenSim.Region.Framework.Scenes
                 m_scene.PhysicsScene.AddPhysicsActorTaint(m_rootPart.PhysActor);
             }
 
-            lock (m_parts)
+            lockPartsForRead(true);
+
+            foreach (SceneObjectPart prim in m_parts.Values)
             {
-                foreach (SceneObjectPart prim in m_parts.Values)
+                if (prim.UUID != m_rootPart.UUID)
                 {
-                    if (prim.UUID != m_rootPart.UUID)
-                    {
-                        prim.IgnoreUndoUpdate = true;
-                        Vector3 axPos = prim.OffsetPosition;
-                        axPos *= oldParentRot;
-                        axPos *= Quaternion.Inverse(axRot);
-                        prim.OffsetPosition = axPos;
-                        Quaternion primsRot = prim.RotationOffset;
-                        Quaternion newRot = primsRot * oldParentRot;
-                        newRot *= Quaternion.Inverse(axRot);
-                        prim.RotationOffset = newRot;
-                        prim.ScheduleTerseUpdate();
-                    }
+                    prim.IgnoreUndoUpdate = true;
+                    Vector3 axPos = prim.OffsetPosition;
+                    axPos *= oldParentRot;
+                    axPos *= Quaternion.Inverse(axRot);
+                    prim.OffsetPosition = axPos;
+                    Quaternion primsRot = prim.RotationOffset;
+                    Quaternion newRot = primsRot * oldParentRot;
+                    newRot *= Quaternion.Inverse(axRot);
+                    prim.RotationOffset = newRot;
+                    prim.ScheduleTerseUpdate();
                 }
             }
+
             foreach (SceneObjectPart childpart in Children.Values)
             {
                 if (childpart != m_rootPart)
@@ -3226,6 +3399,9 @@ namespace OpenSim.Region.Framework.Scenes
                     childpart.StoreUndoState();
                 }
             }
+
+            lockPartsForRead(false);
+
             m_rootPart.ScheduleTerseUpdate();
         }
 
@@ -3347,7 +3523,7 @@ namespace OpenSim.Region.Framework.Scenes
                     if (atTargets.Count > 0)
                     {
                         uint[] localids = new uint[0];
-                        lock (m_parts)
+                        lockPartsForRead(true);
                         {
                             localids = new uint[m_parts.Count];
                             int cntr = 0;
@@ -3357,6 +3533,7 @@ namespace OpenSim.Region.Framework.Scenes
                                 cntr++;
                             }
                         }
+                        lockPartsForRead(false);
                         
                         for (int ctr = 0; ctr < localids.Length; ctr++)
                         {
@@ -3375,7 +3552,7 @@ namespace OpenSim.Region.Framework.Scenes
                     {
                         //trigger not_at_target
                         uint[] localids = new uint[0];
-                        lock (m_parts)
+                        lockPartsForRead(true);
                         {
                             localids = new uint[m_parts.Count];
                             int cntr = 0;
@@ -3385,7 +3562,8 @@ namespace OpenSim.Region.Framework.Scenes
                                 cntr++;
                             }
                         }
-                        
+                        lockPartsForRead(false);    
+
                         for (int ctr = 0; ctr < localids.Length; ctr++)
                         {
                             m_scene.EventManager.TriggerNotAtTargetEvent(localids[ctr]);
@@ -3477,19 +3655,20 @@ namespace OpenSim.Region.Framework.Scenes
         public float GetMass()
         {
             float retmass = 0f;
-            lock (m_parts)
+            lockPartsForRead(true);
             {
                 foreach (SceneObjectPart part in m_parts.Values)
                 {
                     retmass += part.GetMass();
                 }
             }
+            lockPartsForRead(false);
             return retmass;
         }
         
         public void CheckSculptAndLoad()
         {
-            lock (m_parts)
+            lockPartsForRead(true);
             {
                 if (!IsDeleted)
                 {
@@ -3514,6 +3693,7 @@ namespace OpenSim.Region.Framework.Scenes
                     }
                 }
             }
+            lockPartsForRead(false);
         }
 
         protected void AssetReceived(string id, Object sender, AssetBase asset)
@@ -3534,7 +3714,7 @@ namespace OpenSim.Region.Framework.Scenes
         /// <param name="client"></param>
         public void SetGroup(UUID GroupID, IClientAPI client)
         {
-            lock (m_parts)
+            lockPartsForRead(true);
             {
                 foreach (SceneObjectPart part in m_parts.Values)
                 {
@@ -3544,6 +3724,7 @@ namespace OpenSim.Region.Framework.Scenes
 
                 HasGroupChanged = true;
             }
+            lockPartsForRead(false);
 
             // Don't trigger the update here - otherwise some client issues occur when multiple updates are scheduled
             // for the same object with very different properties.  The caller must schedule the update.
@@ -3565,11 +3746,12 @@ namespace OpenSim.Region.Framework.Scenes
 
         public void SetAttachmentPoint(byte point)
         {
-            lock (m_parts)
+            lockPartsForRead(true);
             {
                 foreach (SceneObjectPart part in m_parts.Values)
                     part.SetAttachmentPoint(point);
             }
+            lockPartsForRead(false);
         }
 
         #region ISceneObject
diff --git a/OpenSim/Region/Framework/Scenes/SceneObjectPart.cs b/OpenSim/Region/Framework/Scenes/SceneObjectPart.cs
index 46eadee..4b2641c 100644
--- a/OpenSim/Region/Framework/Scenes/SceneObjectPart.cs
+++ b/OpenSim/Region/Framework/Scenes/SceneObjectPart.cs
@@ -147,7 +147,7 @@ namespace OpenSim.Region.Framework.Scenes
 
         // TODO: This needs to be persisted in next XML version update!
         [XmlIgnore]
-        public readonly int[] PayPrice = {-2,-2,-2,-2,-2};
+        public int[] PayPrice = {-2,-2,-2,-2,-2};
         [XmlIgnore]
         public PhysicsActor PhysActor;
 
@@ -274,6 +274,7 @@ namespace OpenSim.Region.Framework.Scenes
         private Quaternion m_sitTargetOrientation = Quaternion.Identity;
         private Vector3 m_sitTargetPosition;
         private string m_sitAnimation = "SIT";
+        private bool m_occupied;                                        // KF if any av is sitting on this prim
         private string m_text = String.Empty;
         private string m_touchName = String.Empty;
         private readonly UndoStack<UndoState> m_undo = new UndoStack<UndoState>(5);
@@ -452,12 +453,16 @@ namespace OpenSim.Region.Framework.Scenes
         }
 
         /// <value>
-        /// Access should be via Inventory directly - this property temporarily remains for xml serialization purposes
+        /// Get the inventory list
         /// </value>
         public TaskInventoryDictionary TaskInventory
         {
-            get { return m_inventory.Items; }
-            set { m_inventory.Items = value; }
+            get {
+                return m_inventory.Items;
+            }
+            set {
+                m_inventory.Items = value;
+            }
         }
 
         public uint ObjectFlags
@@ -586,14 +591,12 @@ namespace OpenSim.Region.Framework.Scenes
             set { m_LoopSoundSlavePrims = value; }
         }
 
-        [XmlIgnore]
         public Byte[] TextureAnimation
         {
             get { return m_TextureAnimation; }
             set { m_TextureAnimation = value; }
         }
 
-        [XmlIgnore]
         public Byte[] ParticleSystem
         {
             get { return m_particleSystem; }
@@ -647,7 +650,6 @@ namespace OpenSim.Region.Framework.Scenes
             set
             {
                 m_groupPosition = value;
-
                 PhysicsActor actor = PhysActor;
                 if (actor != null)
                 {
@@ -816,7 +818,16 @@ namespace OpenSim.Region.Framework.Scenes
         /// <summary></summary>
         public Vector3 Acceleration
         {
-            get { return m_acceleration; }
+                        get 
+                        { 
+                PhysicsActor actor = PhysActor;
+                if (actor != null)
+                                {
+                                        m_acceleration = actor.Acceleration;
+                                }
+                                return m_acceleration;
+                        } 
+            
             set { m_acceleration = value; }
         }
 
@@ -960,7 +971,8 @@ namespace OpenSim.Region.Framework.Scenes
                 if (IsAttachment)
                     return GroupPosition;
 
-                return m_offsetPosition + m_groupPosition; }
+//                return m_offsetPosition + m_groupPosition; }
+                return m_groupPosition + (m_offsetPosition * ParentGroup.RootPart.RotationOffset) ; }  //KF: Rotation was ignored!
         }
 
         public SceneObjectGroup ParentGroup
@@ -1112,6 +1124,13 @@ namespace OpenSim.Region.Framework.Scenes
             get { return _flags; }
             set { _flags = value; }
         }
+        
+        [XmlIgnore]
+        public bool IsOccupied                          // KF If an av is sittingon this prim
+        {
+                get { return m_occupied; }
+                set { m_occupied = value; }
+        }
 
         [XmlIgnore]
         public UUID SitTargetAvatar
@@ -1187,14 +1206,6 @@ namespace OpenSim.Region.Framework.Scenes
             }
         }
 
-        /// <summary>
-        /// Clear all pending updates of parts to clients
-        /// </summary>
-        private void ClearUpdateSchedule()
-        {
-            m_updateFlag = 0;
-        }
-
         private void SendObjectPropertiesToClient(UUID AgentID)
         {
             m_parentGroup.Scene.ForEachScenePresence(delegate(ScenePresence avatar)
@@ -1934,12 +1945,17 @@ namespace OpenSim.Region.Framework.Scenes
         public Vector3 GetWorldPosition()
         {
             Quaternion parentRot = ParentGroup.RootPart.RotationOffset;
-
             Vector3 axPos = OffsetPosition;
-
             axPos *= parentRot;
             Vector3 translationOffsetPosition = axPos;
-            return GroupPosition + translationOffsetPosition;
+            if(_parentID == 0)
+            {
+               return GroupPosition;
+            }
+            else
+            {
+                return ParentGroup.AbsolutePosition + translationOffsetPosition; //KF: Fix child prim position
+            }
         }
 
         /// <summary>
@@ -1950,7 +1966,7 @@ namespace OpenSim.Region.Framework.Scenes
         {
             Quaternion newRot;
 
-            if (this.LinkNum == 0)
+            if (this.LinkNum < 2)                       //KF Single or root prim
             {
                 newRot = RotationOffset;
             }
@@ -2596,17 +2612,18 @@ namespace OpenSim.Region.Framework.Scenes
                 //Trys to fetch sound id from prim's inventory.
                 //Prim's inventory doesn't support non script items yet
                 
-                lock (TaskInventory)
+                TaskInventory.LockItemsForRead(true);
+
+                foreach (KeyValuePair<UUID, TaskInventoryItem> item in TaskInventory)
                 {
-                    foreach (KeyValuePair<UUID, TaskInventoryItem> item in TaskInventory)
+                    if (item.Value.Name == sound)
                     {
-                        if (item.Value.Name == sound)
-                        {
-                            soundID = item.Value.ItemID;
-                            break;
-                        }
+                        soundID = item.Value.ItemID;
+                        break;
                     }
                 }
+
+                TaskInventory.LockItemsForRead(false);
             }
 
             m_parentGroup.Scene.ForEachScenePresence(delegate(ScenePresence sp)
@@ -2675,38 +2692,7 @@ namespace OpenSim.Region.Framework.Scenes
         
         public void RotLookAt(Quaternion target, float strength, float damping)
         {
-            rotLookAt(target, strength, damping);
-        }
-
-        public void rotLookAt(Quaternion target, float strength, float damping)
-        {
-            if (IsAttachment)
-            {
-                /*
-                    ScenePresence avatar = m_scene.GetScenePresence(rootpart.AttachedAvatar);
-                    if (avatar != null)
-                    {
-                    Rotate the Av?
-                    } */
-            }
-            else
-            {
-                APIDDamp = damping;
-                APIDStrength = strength;
-                APIDTarget = target;
-            }
-        }
-
-        public void startLookAt(Quaternion rot, float damp, float strength)
-        {
-            APIDDamp = damp;
-            APIDStrength = strength;
-            APIDTarget = rot;
-        }
-
-        public void stopLookAt()
-        {
-            APIDTarget = Quaternion.Identity;
+                m_parentGroup.rotLookAt(target, strength, damping);  // This calls method in SceneObjectGroup.
         }
 
         /// <summary>
@@ -2939,8 +2925,8 @@ namespace OpenSim.Region.Framework.Scenes
         {
             const float ROTATION_TOLERANCE = 0.01f;
             const float VELOCITY_TOLERANCE = 0.001f;
-            const float POSITION_TOLERANCE = 0.05f;
-            const int TIME_MS_TOLERANCE = 3000;
+            const float POSITION_TOLERANCE = 0.05f; // I don't like this, but I suppose it's necessary
+            const int TIME_MS_TOLERANCE = 200; //llSetPos has a 200ms delay. This should NOT be 3 seconds.
 
             if (m_updateFlag == 1)
             {
@@ -2954,7 +2940,7 @@ namespace OpenSim.Region.Framework.Scenes
                     Environment.TickCount - m_lastTerseSent > TIME_MS_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.
@@ -2972,16 +2958,18 @@ namespace OpenSim.Region.Framework.Scenes
                     m_lastAngularVelocity = AngularVelocity;
                     m_lastTerseSent = Environment.TickCount;
                 }
+                //Moved this outside of the if clause so updates don't get blocked.. *sigh*
+                m_updateFlag = 0; //Why were we calling a function to do this? Inefficient! *screams* 
             }
             else
             {
                 if (m_updateFlag == 2) // is a new prim, just created/reloaded or has major changes
                 {
                     AddFullUpdateToAllAvatars();
-                    ClearUpdateSchedule();
+                    m_updateFlag = 0; //Same here
                 }
             }
-            ClearUpdateSchedule();
+            m_updateFlag = 0;
         }
 
         /// <summary>
@@ -3008,17 +2996,16 @@ namespace OpenSim.Region.Framework.Scenes
             if (!UUID.TryParse(sound, out soundID))
             {
                 // search sound file from inventory
-                lock (TaskInventory)
+                TaskInventory.LockItemsForRead(true);
+                foreach (KeyValuePair<UUID, TaskInventoryItem> item in TaskInventory)
                 {
-                    foreach (KeyValuePair<UUID, TaskInventoryItem> item in TaskInventory)
+                    if (item.Value.Name == sound && item.Value.Type == (int)AssetType.Sound)
                     {
-                        if (item.Value.Name == sound && item.Value.Type == (int)AssetType.Sound)
-                        {
-                            soundID = item.Value.ItemID;
-                            break;
-                        }
+                        soundID = item.Value.ItemID;
+                        break;
                     }
                 }
+                TaskInventory.LockItemsForRead(false);
             }
 
             if (soundID == UUID.Zero)
@@ -3453,7 +3440,7 @@ namespace OpenSim.Region.Framework.Scenes
         
         public void StopLookAt()
         {
-            m_parentGroup.stopLookAt();
+            m_parentGroup.stopLookAt();    // This calls method in SceneObjectGroup.
 
             m_parentGroup.ScheduleGroupForTerseUpdate();
         }
diff --git a/OpenSim/Region/Framework/Scenes/SceneObjectPartInventory.cs b/OpenSim/Region/Framework/Scenes/SceneObjectPartInventory.cs
index 77bf6fe..836622d 100644
--- a/OpenSim/Region/Framework/Scenes/SceneObjectPartInventory.cs
+++ b/OpenSim/Region/Framework/Scenes/SceneObjectPartInventory.cs
@@ -81,7 +81,9 @@ namespace OpenSim.Region.Framework.Scenes
         /// </value>
         protected internal TaskInventoryDictionary Items
         {
-            get { return m_items; }
+            get { 
+                return m_items; 
+            }
             set
             {
                 m_items = value;
@@ -117,22 +119,25 @@ namespace OpenSim.Region.Framework.Scenes
         /// <param name="linkNum">Link number for the part</param>
         public void ResetInventoryIDs()
         {
-            lock (Items)
+            m_items.LockItemsForWrite(true);
+
+            if (0 == Items.Count)
             {
-                if (0 == Items.Count)
-                    return;
+                m_items.LockItemsForWrite(false);
+                return;
+            }
 
-                HasInventoryChanged = true;
-                m_part.ParentGroup.HasGroupChanged = true;
-                IList<TaskInventoryItem> items = new List<TaskInventoryItem>(Items.Values);
-                Items.Clear();
+            HasInventoryChanged = true;
+            m_part.ParentGroup.HasGroupChanged = true;
+            IList<TaskInventoryItem> items = new List<TaskInventoryItem>(Items.Values);
+            Items.Clear();
 
-                foreach (TaskInventoryItem item in items)
-                {
-                    item.ResetIDs(m_part.UUID);
-                    Items.Add(item.ItemID, item);
-                }
+            foreach (TaskInventoryItem item in items)
+            {
+                item.ResetIDs(m_part.UUID);
+                Items.Add(item.ItemID, item);
             }
+            m_items.LockItemsForWrite(false);
         }
 
         /// <summary>
@@ -141,25 +146,25 @@ namespace OpenSim.Region.Framework.Scenes
         /// <param name="ownerId"></param>
         public void ChangeInventoryOwner(UUID ownerId)
         {
-            lock (Items)
+            m_items.LockItemsForWrite(true);
+            if (0 == Items.Count)
             {
-                if (0 == Items.Count)
-                {
-                    return;
-                }
+                m_items.LockItemsForWrite(false);
+                return;
+            }
 
-                HasInventoryChanged = true;
-                m_part.ParentGroup.HasGroupChanged = true;
-                IList<TaskInventoryItem> items = new List<TaskInventoryItem>(Items.Values);
-                foreach (TaskInventoryItem item in items)
+            HasInventoryChanged = true;
+            m_part.ParentGroup.HasGroupChanged = true;
+            IList<TaskInventoryItem> items = new List<TaskInventoryItem>(Items.Values);
+            foreach (TaskInventoryItem item in items)
+            {
+                if (ownerId != item.OwnerID)
                 {
-                    if (ownerId != item.OwnerID)
-                    {
-                        item.LastOwnerID = item.OwnerID;
-                        item.OwnerID = ownerId;
-                    }
+                    item.LastOwnerID = item.OwnerID;
+                    item.OwnerID = ownerId;
                 }
             }
+            m_items.LockItemsForWrite(false);
         }
 
         /// <summary>
@@ -168,24 +173,24 @@ namespace OpenSim.Region.Framework.Scenes
         /// <param name="groupID"></param>
         public void ChangeInventoryGroup(UUID groupID)
         {
-            lock (Items)
+            m_items.LockItemsForWrite(true);
+            if (0 == Items.Count)
             {
-                if (0 == Items.Count)
-                {
-                    return;
-                }
+                m_items.LockItemsForWrite(false);
+                return;
+            }
 
-                HasInventoryChanged = true;
-                m_part.ParentGroup.HasGroupChanged = true;
-                IList<TaskInventoryItem> items = new List<TaskInventoryItem>(Items.Values);
-                foreach (TaskInventoryItem item in items)
+            HasInventoryChanged = true;
+            m_part.ParentGroup.HasGroupChanged = true;
+            IList<TaskInventoryItem> items = new List<TaskInventoryItem>(Items.Values);
+            foreach (TaskInventoryItem item in items)
+            {
+                if (groupID != item.GroupID)
                 {
-                    if (groupID != item.GroupID)
-                    {
-                        item.GroupID = groupID;
-                    }
+                    item.GroupID = groupID;
                 }
             }
+            m_items.LockItemsForWrite(false);
         }
 
         /// <summary>
@@ -193,14 +198,14 @@ namespace OpenSim.Region.Framework.Scenes
         /// </summary>
         public void CreateScriptInstances(int startParam, bool postOnRez, string engine, int stateSource)
         {
-            lock (m_items)
+            Items.LockItemsForRead(true);
+            IList<TaskInventoryItem> items = new List<TaskInventoryItem>(Items.Values);
+            Items.LockItemsForRead(false);
+            foreach (TaskInventoryItem item in items)
             {
-                foreach (TaskInventoryItem item in Items.Values)
+                if ((int)InventoryType.LSL == item.InvType)
                 {
-                    if ((int)InventoryType.LSL == item.InvType)
-                    {
-                        CreateScriptInstance(item, startParam, postOnRez, engine, stateSource);
-                    }
+                    CreateScriptInstance(item, startParam, postOnRez, engine, stateSource);
                 }
             }
         }
@@ -235,16 +240,20 @@ namespace OpenSim.Region.Framework.Scenes
         /// </param>
         public void RemoveScriptInstances(bool sceneObjectBeingDeleted)
         {
-            lock (Items)
+            Items.LockItemsForRead(true);
+            IList<TaskInventoryItem> items = new List<TaskInventoryItem>(Items.Values);
+            Items.LockItemsForRead(false);
+
+            foreach (TaskInventoryItem item in items)
             {
-                foreach (TaskInventoryItem item in Items.Values)
+                if ((int)InventoryType.LSL == item.InvType)
                 {
-                    if ((int)InventoryType.LSL == item.InvType)
-                    {
-                        RemoveScriptInstance(item.ItemID, sceneObjectBeingDeleted);
-                    }
+                    RemoveScriptInstance(item.ItemID, sceneObjectBeingDeleted);
+                    m_part.RemoveScriptEvents(item.ItemID);
                 }
             }
+
+            
         }
 
         /// <summary>
@@ -269,12 +278,10 @@ namespace OpenSim.Region.Framework.Scenes
                 if (stateSource == 1 && // Prim crossing
                         m_part.ParentGroup.Scene.m_trustBinaries)
                 {
-                    lock (m_items)
-                    {
-                        m_items[item.ItemID].PermsMask = 0;
-                        m_items[item.ItemID].PermsGranter = UUID.Zero;
-                    }
-                    
+                    m_items.LockItemsForWrite(true);
+                    m_items[item.ItemID].PermsMask = 0;
+                    m_items[item.ItemID].PermsGranter = UUID.Zero;
+                    m_items.LockItemsForWrite(false);
                     m_part.ParentGroup.Scene.EventManager.TriggerRezScript(
                         m_part.LocalId, item.ItemID, String.Empty, startParam, postOnRez, engine, stateSource);
                     m_part.ParentGroup.AddActiveScriptCount(1);
@@ -282,36 +289,31 @@ namespace OpenSim.Region.Framework.Scenes
                     return;
                 }
 
-                m_part.ParentGroup.Scene.AssetService.Get(
-                    item.AssetID.ToString(), this, delegate(string id, object sender, AssetBase asset)
-                    {
-                        if (null == asset)
-                        {
-                            m_log.ErrorFormat(
-                                "[PRIM INVENTORY]: " +
-                                "Couldn't start script {0}, {1} at {2} in {3} since asset ID {4} could not be found",
-                                item.Name, item.ItemID, m_part.AbsolutePosition, 
-                                m_part.ParentGroup.Scene.RegionInfo.RegionName, item.AssetID);
-                        }
-                        else
-                        {
-                            if (m_part.ParentGroup.m_savedScriptState != null)
-                                RestoreSavedScriptState(item.OldItemID, item.ItemID);
-
-                            lock (m_items)
-                            {
-                                m_items[item.ItemID].PermsMask = 0;
-                                m_items[item.ItemID].PermsGranter = UUID.Zero;
-                            }
-                        
-                            string script = Utils.BytesToString(asset.Data);
-                            m_part.ParentGroup.Scene.EventManager.TriggerRezScript(
-                                m_part.LocalId, item.ItemID, script, startParam, postOnRez, engine, stateSource);
-                            m_part.ParentGroup.AddActiveScriptCount(1);
-                            m_part.ScheduleFullUpdate();
-                        }
-                    }
-                );
+                m_part.ParentGroup.Scene.AssetService.Get(item.AssetID.ToString(), this, delegate(string id, object sender, AssetBase asset)
+                               {
+                                   if (null == asset)
+                                   {
+                                       m_log.ErrorFormat(
+                                           "[PRIM INVENTORY]: " +
+                                           "Couldn't start script {0}, {1} at {2} in {3} since asset ID {4} could not be found",
+                                           item.Name, item.ItemID, m_part.AbsolutePosition, 
+                                           m_part.ParentGroup.Scene.RegionInfo.RegionName, item.AssetID);
+                                   }
+                                   else
+                                   {
+                                       if (m_part.ParentGroup.m_savedScriptState != null)
+                                           RestoreSavedScriptState(item.OldItemID, item.ItemID);
+                                       m_items.LockItemsForWrite(true);
+                                       m_items[item.ItemID].PermsMask = 0;
+                                       m_items[item.ItemID].PermsGranter = UUID.Zero;
+                                       m_items.LockItemsForWrite(false);
+                                       string script = Utils.BytesToString(asset.Data);
+                                       m_part.ParentGroup.Scene.EventManager.TriggerRezScript(
+                                            m_part.LocalId, item.ItemID, script, startParam, postOnRez, engine, stateSource);
+                                       m_part.ParentGroup.AddActiveScriptCount(1);
+                                       m_part.ScheduleFullUpdate();
+                                   }
+                               });
             }
         }
 
@@ -378,14 +380,17 @@ namespace OpenSim.Region.Framework.Scenes
         /// </param>
         public void CreateScriptInstance(UUID itemId, int startParam, bool postOnRez, string engine, int stateSource)
         {
-            lock (m_items)
+            m_items.LockItemsForRead(true);
+            if (m_items.ContainsKey(itemId))
             {
                 if (m_items.ContainsKey(itemId))
                 {
+                    m_items.LockItemsForRead(false);
                     CreateScriptInstance(m_items[itemId], startParam, postOnRez, engine, stateSource);
                 }
                 else
                 {
+                    m_items.LockItemsForRead(false);
                     m_log.ErrorFormat(
                         "[PRIM INVENTORY]: " +
                         "Couldn't start script with ID {0} since it couldn't be found for prim {1}, {2} at {3} in {4}",
@@ -393,6 +398,15 @@ namespace OpenSim.Region.Framework.Scenes
                         m_part.AbsolutePosition, m_part.ParentGroup.Scene.RegionInfo.RegionName);
                 }
             }
+            else
+            {
+                m_items.LockItemsForRead(false);
+                m_log.ErrorFormat(
+                    "[PRIM INVENTORY]: " +
+                    "Couldn't start script with ID {0} since it couldn't be found for prim {1}, {2}",
+                    itemId, m_part.Name, m_part.UUID);
+            }
+            
         }
 
         /// <summary>
@@ -405,15 +419,7 @@ namespace OpenSim.Region.Framework.Scenes
         /// </param>
         public void RemoveScriptInstance(UUID itemId, bool sceneObjectBeingDeleted)
         {
-            bool scriptPresent = false;
-
-            lock (m_items)
-            {
-                if (m_items.ContainsKey(itemId))
-                    scriptPresent = true;
-            }
-            
-            if (scriptPresent)
+            if (m_items.ContainsKey(itemId))
             {
                 if (!sceneObjectBeingDeleted)
                     m_part.RemoveScriptEvents(itemId);
@@ -439,11 +445,16 @@ namespace OpenSim.Region.Framework.Scenes
         /// <returns></returns>
         private bool InventoryContainsName(string name)
         {
-            foreach (TaskInventoryItem item in Items.Values)
+            m_items.LockItemsForRead(true);
+            foreach (TaskInventoryItem item in m_items.Values)
             {
                 if (item.Name == name)
+                {
+                    m_items.LockItemsForRead(false);
                     return true;
+                }
             }
+            m_items.LockItemsForRead(false);
             return false;
         }
 
@@ -485,13 +496,9 @@ namespace OpenSim.Region.Framework.Scenes
         /// <param name="item"></param>
         public void AddInventoryItemExclusive(TaskInventoryItem item, bool allowedDrop)
         {
-            List<TaskInventoryItem> il;
-
-            lock (m_items)
-            {
-                il = new List<TaskInventoryItem>(m_items.Values);
-            }
-            
+            m_items.LockItemsForRead(true);
+            List<TaskInventoryItem> il = new List<TaskInventoryItem>(m_items.Values);
+            m_items.LockItemsForRead(false);
             foreach (TaskInventoryItem i in il)
             {
                 if (i.Name == item.Name)
@@ -528,15 +535,14 @@ namespace OpenSim.Region.Framework.Scenes
             item.ParentPartID = m_part.UUID;
             item.Name = name;
 
-            lock (m_items)
-            {
-                m_items.Add(item.ItemID, item);
-
+            m_items.LockItemsForWrite(true);
+            m_items.Add(item.ItemID, item);
+            m_items.LockItemsForWrite(false);
                 if (allowedDrop) 
                     m_part.TriggerScriptChangedEvent(Changed.ALLOWED_DROP);
                 else
                     m_part.TriggerScriptChangedEvent(Changed.INVENTORY);
-            }
+            
 
             m_inventorySerial++;
             //m_inventorySerial += 2;
@@ -553,14 +559,13 @@ namespace OpenSim.Region.Framework.Scenes
         /// <param name="items"></param>
         public void RestoreInventoryItems(ICollection<TaskInventoryItem> items)
         {
-            lock (m_items)
+            m_items.LockItemsForWrite(true);
+            foreach (TaskInventoryItem item in items)
             {
-                foreach (TaskInventoryItem item in items)
-                {
-                    m_items.Add(item.ItemID, item);
-                    m_part.TriggerScriptChangedEvent(Changed.INVENTORY);
-                }
+                m_items.Add(item.ItemID, item);
+                m_part.TriggerScriptChangedEvent(Changed.INVENTORY);
             }
+            m_items.LockItemsForWrite(false);
 
             m_inventorySerial++;
         }
@@ -573,10 +578,9 @@ namespace OpenSim.Region.Framework.Scenes
         public TaskInventoryItem GetInventoryItem(UUID itemId)
         {
             TaskInventoryItem item;
-
-            lock (m_items)
-                m_items.TryGetValue(itemId, out item);
-
+            m_items.LockItemsForRead(true);
+            m_items.TryGetValue(itemId, out item);
+            m_items.LockItemsForRead(false);
             return item;
         }
 
@@ -612,45 +616,45 @@ namespace OpenSim.Region.Framework.Scenes
         /// <returns>false if the item did not exist, true if the update occurred successfully</returns>
         public bool UpdateInventoryItem(TaskInventoryItem item)
         {
-            lock (m_items)
+            m_items.LockItemsForWrite(true);
+
+            if (m_items.ContainsKey(item.ItemID))
             {
-                if (m_items.ContainsKey(item.ItemID))
+                item.ParentID = m_part.UUID;
+                item.ParentPartID = m_part.UUID;
+                item.Flags = m_items[item.ItemID].Flags;
+                if (item.AssetID == UUID.Zero)
                 {
-                    item.ParentID = m_part.UUID;
-                    item.ParentPartID = m_part.UUID;
-                    item.Flags = m_items[item.ItemID].Flags;
-                    if (item.AssetID == UUID.Zero)
-                    {
-                        item.AssetID = m_items[item.ItemID].AssetID;
-                    }
-                    else if ((InventoryType)item.Type == InventoryType.Notecard)
-                    {
-                        ScenePresence presence = m_part.ParentGroup.Scene.GetScenePresence(item.OwnerID);
-
-                        if (presence != null)
-                        {
-                            presence.ControllingClient.SendAgentAlertMessage(
-                                    "Notecard saved", false);
-                        }
-                    }
-
-                    m_items[item.ItemID] = item;
-                    m_inventorySerial++;
-                    m_part.TriggerScriptChangedEvent(Changed.INVENTORY);
-                    HasInventoryChanged = true;
-                    m_part.ParentGroup.HasGroupChanged = true;
-
-                    return true;
+                    item.AssetID = m_items[item.ItemID].AssetID;
                 }
-                else
+                else if ((InventoryType)item.Type == InventoryType.Notecard)
                 {
-                    m_log.ErrorFormat(
-                        "[PRIM INVENTORY]: " +
-                        "Tried to retrieve item ID {0} from prim {1}, {2} at {3} in {4} but the item does not exist in this inventory",
-                        item.ItemID, m_part.Name, m_part.UUID, 
-                        m_part.AbsolutePosition, m_part.ParentGroup.Scene.RegionInfo.RegionName);
+                    ScenePresence presence = m_part.ParentGroup.Scene.GetScenePresence(item.OwnerID);
+
+                    if (presence != null)
+                    {
+                        presence.ControllingClient.SendAgentAlertMessage(
+                                "Notecard saved", false);
+                    }
                 }
+
+                m_items[item.ItemID] = item;
+                m_inventorySerial++;
+                m_part.TriggerScriptChangedEvent(Changed.INVENTORY);
+                HasInventoryChanged = true;
+                m_part.ParentGroup.HasGroupChanged = true;
+                m_items.LockItemsForWrite(false);
+                return true;
+            }
+            else
+            {
+                m_log.ErrorFormat(
+                    "[PRIM INVENTORY]: " +
+                    "Tried to retrieve item ID {0} from prim {1}, {2} at {3} in {4} but the item does not exist in this inventory",
+                    item.ItemID, m_part.Name, m_part.UUID, 
+                    m_part.AbsolutePosition, m_part.ParentGroup.Scene.RegionInfo.RegionName);
             }
+            m_items.LockItemsForWrite(false);
 
             return false;
         }
@@ -663,53 +667,54 @@ namespace OpenSim.Region.Framework.Scenes
         /// in this prim's inventory.</returns>
         public int RemoveInventoryItem(UUID itemID)
         {
-            lock (m_items)
+            m_items.LockItemsForRead(true);
+
+            if (m_items.ContainsKey(itemID))
             {
-                if (m_items.ContainsKey(itemID))
+                int type = m_items[itemID].InvType;
+                m_items.LockItemsForRead(false);
+                if (type == 10) // Script
                 {
-                    int type = m_items[itemID].InvType;
-                    if (type == 10) // Script
-                    {
-                        m_part.RemoveScriptEvents(itemID);
-                        m_part.ParentGroup.Scene.EventManager.TriggerRemoveScript(m_part.LocalId, itemID);
-                    }
-                    m_items.Remove(itemID);
-                    m_inventorySerial++;
-                    m_part.TriggerScriptChangedEvent(Changed.INVENTORY);
-
-                    HasInventoryChanged = true;
-                    m_part.ParentGroup.HasGroupChanged = true;
+                    m_part.ParentGroup.Scene.EventManager.TriggerRemoveScript(m_part.LocalId, itemID);
+                }
+                m_items.LockItemsForWrite(true);
+                m_items.Remove(itemID);
+                m_items.LockItemsForWrite(false);
+                m_inventorySerial++;
+                m_part.TriggerScriptChangedEvent(Changed.INVENTORY);
 
-                    int scriptcount = 0;
-                    lock (m_items)
-                    {
-                        foreach (TaskInventoryItem item in m_items.Values)
-                        {
-                            if (item.Type == 10)
-                            {
-                                scriptcount++;
-                            }
-                        }
-                    }
+                HasInventoryChanged = true;
+                m_part.ParentGroup.HasGroupChanged = true;
 
-                    if (scriptcount <= 0)
+                int scriptcount = 0;
+                m_items.LockItemsForRead(true);
+                foreach (TaskInventoryItem item in m_items.Values)
+                {
+                    if (item.Type == 10)
                     {
-                        m_part.RemFlag(PrimFlags.Scripted);
+                        scriptcount++;
                     }
-
-                    m_part.ScheduleFullUpdate();
-
-                    return type;
                 }
-                else
+                m_items.LockItemsForRead(false);
+                
+
+                if (scriptcount <= 0)
                 {
-                    m_log.ErrorFormat(
-                        "[PRIM INVENTORY]: " +
-                        "Tried to remove item ID {0} from prim {1}, {2} at {3} in {4} but the item does not exist in this inventory",
-                        itemID, m_part.Name, m_part.UUID, 
-                        m_part.AbsolutePosition, m_part.ParentGroup.Scene.RegionInfo.RegionName);
+                    m_part.RemFlag(PrimFlags.Scripted);
                 }
+
+                m_part.ScheduleFullUpdate();
+
+                return type;
+            }
+            else
+            {
+                m_log.ErrorFormat(
+                    "[PRIM INVENTORY]: " +
+                    "Tried to remove item ID {0} from prim {1}, {2} but the item does not exist in this inventory",
+                    itemID, m_part.Name, m_part.UUID);
             }
+            m_items.LockItemsForWrite(false);
 
             return -1;
         }
@@ -762,52 +767,53 @@ namespace OpenSim.Region.Framework.Scenes
             // isn't available (such as drag from prim inventory to agent inventory)
             InventoryStringBuilder invString = new InventoryStringBuilder(m_part.UUID, UUID.Zero);
 
-            lock (m_items)
+            m_items.LockItemsForRead(true);
+
+            foreach (TaskInventoryItem item in m_items.Values)
             {
-                foreach (TaskInventoryItem item in m_items.Values)
-                {
-                    UUID ownerID = item.OwnerID;
-                    uint everyoneMask = 0;
-                    uint baseMask = item.BasePermissions;
-                    uint ownerMask = item.CurrentPermissions;
+                UUID ownerID = item.OwnerID;
+                uint everyoneMask = 0;
+                uint baseMask = item.BasePermissions;
+                uint ownerMask = item.CurrentPermissions;
 
-                    invString.AddItemStart();
-                    invString.AddNameValueLine("item_id", item.ItemID.ToString());
-                    invString.AddNameValueLine("parent_id", m_part.UUID.ToString());
+                invString.AddItemStart();
+                invString.AddNameValueLine("item_id", item.ItemID.ToString());
+                invString.AddNameValueLine("parent_id", m_part.UUID.ToString());
 
-                    invString.AddPermissionsStart();
+                invString.AddPermissionsStart();
 
-                    invString.AddNameValueLine("base_mask", Utils.UIntToHexString(baseMask));
-                    invString.AddNameValueLine("owner_mask", Utils.UIntToHexString(ownerMask));
-                    invString.AddNameValueLine("group_mask", Utils.UIntToHexString(0));
-                    invString.AddNameValueLine("everyone_mask", Utils.UIntToHexString(everyoneMask));
-                    invString.AddNameValueLine("next_owner_mask", Utils.UIntToHexString(item.NextPermissions));
+                invString.AddNameValueLine("base_mask", Utils.UIntToHexString(baseMask));
+                invString.AddNameValueLine("owner_mask", Utils.UIntToHexString(ownerMask));
+                invString.AddNameValueLine("group_mask", Utils.UIntToHexString(0));
+                invString.AddNameValueLine("everyone_mask", Utils.UIntToHexString(everyoneMask));
+                invString.AddNameValueLine("next_owner_mask", Utils.UIntToHexString(item.NextPermissions));
 
-                    invString.AddNameValueLine("creator_id", item.CreatorID.ToString());
-                    invString.AddNameValueLine("owner_id", ownerID.ToString());
+                invString.AddNameValueLine("creator_id", item.CreatorID.ToString());
+                invString.AddNameValueLine("owner_id", ownerID.ToString());
 
-                    invString.AddNameValueLine("last_owner_id", item.LastOwnerID.ToString());
+                invString.AddNameValueLine("last_owner_id", item.LastOwnerID.ToString());
 
-                    invString.AddNameValueLine("group_id", item.GroupID.ToString());
-                    invString.AddSectionEnd();
+                invString.AddNameValueLine("group_id", item.GroupID.ToString());
+                invString.AddSectionEnd();
 
-                    invString.AddNameValueLine("asset_id", item.AssetID.ToString());
-                    invString.AddNameValueLine("type", TaskInventoryItem.Types[item.Type]);
-                    invString.AddNameValueLine("inv_type", TaskInventoryItem.InvTypes[item.InvType]);
-                    invString.AddNameValueLine("flags", Utils.UIntToHexString(item.Flags));
+                invString.AddNameValueLine("asset_id", item.AssetID.ToString());
+                invString.AddNameValueLine("type", TaskInventoryItem.Types[item.Type]);
+                invString.AddNameValueLine("inv_type", TaskInventoryItem.InvTypes[item.InvType]);
+                invString.AddNameValueLine("flags", Utils.UIntToHexString(item.Flags));
 
-                    invString.AddSaleStart();
-                    invString.AddNameValueLine("sale_type", "not");
-                    invString.AddNameValueLine("sale_price", "0");
-                    invString.AddSectionEnd();
+                invString.AddSaleStart();
+                invString.AddNameValueLine("sale_type", "not");
+                invString.AddNameValueLine("sale_price", "0");
+                invString.AddSectionEnd();
 
-                    invString.AddNameValueLine("name", item.Name + "|");
-                    invString.AddNameValueLine("desc", item.Description + "|");
+                invString.AddNameValueLine("name", item.Name + "|");
+                invString.AddNameValueLine("desc", item.Description + "|");
 
-                    invString.AddNameValueLine("creation_date", item.CreationDate.ToString());
-                    invString.AddSectionEnd();
-                }
+                invString.AddNameValueLine("creation_date", item.CreationDate.ToString());
+                invString.AddSectionEnd();
             }
+            int count = m_items.Count;
+            m_items.LockItemsForRead(false);
 
             fileData = Utils.StringToBytes(invString.BuildString);
 
@@ -828,10 +834,9 @@ namespace OpenSim.Region.Framework.Scenes
         {
             if (HasInventoryChanged)
             {
-                lock (Items)
-                {
-                    datastore.StorePrimInventory(m_part.UUID, Items.Values);
-                }
+                Items.LockItemsForRead(true);
+                datastore.StorePrimInventory(m_part.UUID, Items.Values);
+                Items.LockItemsForRead(false);
 
                 HasInventoryChanged = false;
             }
@@ -900,61 +905,54 @@ namespace OpenSim.Region.Framework.Scenes
         {
             uint mask=0x7fffffff;
 
-            lock (m_items)
+            foreach (TaskInventoryItem item in m_items.Values)
             {
-                foreach (TaskInventoryItem item in m_items.Values)
+                if (item.InvType != (int)InventoryType.Object)
                 {
-                    if (item.InvType != (int)InventoryType.Object)
-                    {
-                        if ((item.CurrentPermissions & item.NextPermissions & (uint)PermissionMask.Copy) == 0)
-                            mask &= ~((uint)PermissionMask.Copy >> 13);
-                        if ((item.CurrentPermissions & item.NextPermissions & (uint)PermissionMask.Transfer) == 0)
-                            mask &= ~((uint)PermissionMask.Transfer >> 13);
-                        if ((item.CurrentPermissions & item.NextPermissions & (uint)PermissionMask.Modify) == 0)
-                            mask &= ~((uint)PermissionMask.Modify >> 13);
-                    }
-                    else
-                    {
-                        if ((item.CurrentPermissions & ((uint)PermissionMask.Copy >> 13)) == 0)
-                            mask &= ~((uint)PermissionMask.Copy >> 13);
-                        if ((item.CurrentPermissions & ((uint)PermissionMask.Transfer >> 13)) == 0)
-                            mask &= ~((uint)PermissionMask.Transfer >> 13);
-                        if ((item.CurrentPermissions & ((uint)PermissionMask.Modify >> 13)) == 0)
-                            mask &= ~((uint)PermissionMask.Modify >> 13);
-                    }
-    
-                    if ((item.CurrentPermissions & (uint)PermissionMask.Copy) == 0)
-                        mask &= ~(uint)PermissionMask.Copy;
-                    if ((item.CurrentPermissions & (uint)PermissionMask.Transfer) == 0)
-                        mask &= ~(uint)PermissionMask.Transfer;
-                    if ((item.CurrentPermissions & (uint)PermissionMask.Modify) == 0)
-                        mask &= ~(uint)PermissionMask.Modify;
+                    if ((item.CurrentPermissions & item.NextPermissions & (uint)PermissionMask.Copy) == 0)
+                        mask &= ~((uint)PermissionMask.Copy >> 13);
+                    if ((item.CurrentPermissions & item.NextPermissions & (uint)PermissionMask.Transfer) == 0)
+                        mask &= ~((uint)PermissionMask.Transfer >> 13);
+                    if ((item.CurrentPermissions & item.NextPermissions & (uint)PermissionMask.Modify) == 0)
+                        mask &= ~((uint)PermissionMask.Modify >> 13);
+                }
+                else
+                {
+                    if ((item.CurrentPermissions & ((uint)PermissionMask.Copy >> 13)) == 0)
+                        mask &= ~((uint)PermissionMask.Copy >> 13);
+                    if ((item.CurrentPermissions & ((uint)PermissionMask.Transfer >> 13)) == 0)
+                        mask &= ~((uint)PermissionMask.Transfer >> 13);
+                    if ((item.CurrentPermissions & ((uint)PermissionMask.Modify >> 13)) == 0)
+                        mask &= ~((uint)PermissionMask.Modify >> 13);
                 }
+
+                if ((item.CurrentPermissions & (uint)PermissionMask.Copy) == 0)
+                    mask &= ~(uint)PermissionMask.Copy;
+                if ((item.CurrentPermissions & (uint)PermissionMask.Transfer) == 0)
+                    mask &= ~(uint)PermissionMask.Transfer;
+                if ((item.CurrentPermissions & (uint)PermissionMask.Modify) == 0)
+                    mask &= ~(uint)PermissionMask.Modify;
             }
-                
             return mask;
         }
 
         public void ApplyNextOwnerPermissions()
         {
-            lock (m_items)
+            foreach (TaskInventoryItem item in m_items.Values)
             {
-                foreach (TaskInventoryItem item in m_items.Values)
+                if (item.InvType == (int)InventoryType.Object && (item.CurrentPermissions & 7) != 0)
                 {
-                    if (item.InvType == (int)InventoryType.Object && (item.CurrentPermissions & 7) != 0)
-                    {
-                        if ((item.CurrentPermissions & ((uint)PermissionMask.Copy >> 13)) == 0)
-                            item.CurrentPermissions &= ~(uint)PermissionMask.Copy;
-                        if ((item.CurrentPermissions & ((uint)PermissionMask.Transfer >> 13)) == 0)
-                            item.CurrentPermissions &= ~(uint)PermissionMask.Transfer;
-                        if ((item.CurrentPermissions & ((uint)PermissionMask.Modify >> 13)) == 0)
-                            item.CurrentPermissions &= ~(uint)PermissionMask.Modify;
-                        item.CurrentPermissions |= 8;
-                    }
-                    item.CurrentPermissions &= item.NextPermissions;
-                    item.BasePermissions &= item.NextPermissions;
-                    item.EveryonePermissions &= item.NextPermissions;
+                    if ((item.CurrentPermissions & ((uint)PermissionMask.Copy >> 13)) == 0)
+                        item.CurrentPermissions &= ~(uint)PermissionMask.Copy;
+                    if ((item.CurrentPermissions & ((uint)PermissionMask.Transfer >> 13)) == 0)
+                        item.CurrentPermissions &= ~(uint)PermissionMask.Transfer;
+                    if ((item.CurrentPermissions & ((uint)PermissionMask.Modify >> 13)) == 0)
+                        item.CurrentPermissions &= ~(uint)PermissionMask.Modify;
+                    item.CurrentPermissions |= 8;
                 }
+                item.CurrentPermissions &= item.NextPermissions;
+                item.BasePermissions &= item.NextPermissions;
+                item.EveryonePermissions &= item.NextPermissions;
             }
 
             m_part.TriggerScriptChangedEvent(Changed.OWNER);
@@ -962,29 +960,22 @@ namespace OpenSim.Region.Framework.Scenes
 
         public void ApplyGodPermissions(uint perms)
         {
-            lock (m_items)
+            foreach (TaskInventoryItem item in m_items.Values)
             {
-                foreach (TaskInventoryItem item in m_items.Values)
-                {
-                    item.CurrentPermissions = perms;
-                    item.BasePermissions = perms;
-                }
+                item.CurrentPermissions = perms;
+                item.BasePermissions = perms;
             }
         }
 
         public bool ContainsScripts()
         {
-            lock (m_items)
+            foreach (TaskInventoryItem item in m_items.Values)
             {
-                foreach (TaskInventoryItem item in m_items.Values)
+                if (item.InvType == (int)InventoryType.LSL)
                 {
-                    if (item.InvType == (int)InventoryType.LSL)
-                    {
-                        return true;
-                    }
+                    return true;
                 }
             }
-            
             return false;
         }
 
@@ -992,11 +983,8 @@ namespace OpenSim.Region.Framework.Scenes
         {
             List<UUID> ret = new List<UUID>();
 
-            lock (m_items)
-            {
-                foreach (TaskInventoryItem item in m_items.Values)
-                    ret.Add(item.ItemID);
-            }
+            foreach (TaskInventoryItem item in m_items.Values)
+                ret.Add(item.ItemID);
 
             return ret;
         }
@@ -1009,30 +997,26 @@ namespace OpenSim.Region.Framework.Scenes
             if (engines == null) // No engine at all
                 return ret;
 
-            lock (m_items)
+            foreach (TaskInventoryItem item in m_items.Values)
             {
-                foreach (TaskInventoryItem item in m_items.Values)
+                if (item.InvType == (int)InventoryType.LSL)
                 {
-                    if (item.InvType == (int)InventoryType.LSL)
+                    foreach (IScriptModule e in engines)
                     {
-                        foreach (IScriptModule e in engines)
+                        if (e != null)
                         {
-                            if (e != null)
+                            string n = e.GetXMLState(item.ItemID);
+                            if (n != String.Empty)
                             {
-                                string n = e.GetXMLState(item.ItemID);
-                                if (n != String.Empty)
-                                {
-                                    if (!ret.ContainsKey(item.ItemID))
-                                        ret[item.ItemID] = n;
-                                    break;
-                                }
+                                if (!ret.ContainsKey(item.ItemID))
+                                    ret[item.ItemID] = n;
+                                break;
                             }
                         }
                     }
                 }
             }
-            
             return ret;
         }
     }
-}
\ No newline at end of file
+}
diff --git a/OpenSim/Region/Framework/Scenes/ScenePresence.cs b/OpenSim/Region/Framework/Scenes/ScenePresence.cs
index b5f6217..5c54616 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;
 
@@ -123,8 +125,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;
@@ -134,7 +139,6 @@ namespace OpenSim.Region.Framework.Scenes
         private Vector3? m_forceToApply;
         private uint m_requestedSitTargetID;
         private UUID m_requestedSitTargetUUID;
-        public bool SitGround = false;
 
         private SendCourseLocationsMethod m_sendCourseLocationsMethod;
 
@@ -157,7 +161,6 @@ namespace OpenSim.Region.Framework.Scenes
         private int m_perfMonMS;
 
         private bool m_setAlwaysRun;
-        
         private bool m_forceFly;
         private bool m_flyDisabled;
 
@@ -183,7 +186,8 @@ namespace OpenSim.Region.Framework.Scenes
         protected RegionInfo m_regionInfo;
         protected ulong crossingFromRegion;
 
-        private readonly Vector3[] Dir_Vectors = new Vector3[9];
+        private readonly Vector3[] Dir_Vectors = new Vector3[11];
+        private bool m_isNudging = false;
 
         // Position of agent's camera in world (region cordinates)
         protected Vector3 m_CameraCenter;
@@ -208,6 +212,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;
 
@@ -218,6 +223,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;
 
@@ -246,7 +254,9 @@ namespace OpenSim.Region.Framework.Scenes
             DIR_CONTROL_FLAG_UP = AgentManager.ControlFlags.AGENT_CONTROL_UP_POS,
             DIR_CONTROL_FLAG_DOWN = AgentManager.ControlFlags.AGENT_CONTROL_UP_NEG,
             DIR_CONTROL_FLAG_FORWARD_NUDGE = AgentManager.ControlFlags.AGENT_CONTROL_NUDGE_AT_POS,
-            DIR_CONTROL_FLAG_BACKWARD_NUDGE = AgentManager.ControlFlags.AGENT_CONTROL_NUDGE_AT_NEG,
+            DIR_CONTROL_FLAG_BACK_NUDGE = AgentManager.ControlFlags.AGENT_CONTROL_NUDGE_AT_NEG,
+            DIR_CONTROL_FLAG_LEFT_NUDGE = AgentManager.ControlFlags.AGENT_CONTROL_NUDGE_LEFT_POS,
+            DIR_CONTROL_FLAG_RIGHT_NUDGE = AgentManager.ControlFlags.AGENT_CONTROL_NUDGE_LEFT_NEG,
             DIR_CONTROL_FLAG_DOWN_NUDGE = AgentManager.ControlFlags.AGENT_CONTROL_NUDGE_UP_NEG
         }
         
@@ -453,7 +463,8 @@ namespace OpenSim.Region.Framework.Scenes
             get
             {
                 PhysicsActor actor = m_physicsActor;
-                if (actor != null)
+//                if (actor != null)
+                if ((actor != null) && (m_parentID == 0))   // KF Do NOT update m_pos here if Av is sitting!
                     m_pos = actor.Position;
 
                 return m_parentPosition + m_pos;
@@ -474,7 +485,8 @@ namespace OpenSim.Region.Framework.Scenes
                     }
                 }
 
-                m_pos = value;
+                if (m_parentID == 0)   // KF Do NOT update m_pos here if Av is sitting!
+                    m_pos = value;
                 m_parentPosition = Vector3.Zero;
             }
         }
@@ -693,10 +705,7 @@ namespace OpenSim.Region.Framework.Scenes
             m_reprioritization_timer.AutoReset = false;
 
             AdjustKnownSeeds();
-
-            // TODO: I think, this won't send anything, as we are still a child here...
             Animator.TrySetMovementAnimation("STAND"); 
-
             // we created a new ScenePresence (a new child agent) in a fresh region.
             // Request info about all the (root) agents in this region
             // Note: This won't send data *to* other clients in that region (children don't send)
@@ -752,25 +761,47 @@ namespace OpenSim.Region.Framework.Scenes
             Dir_Vectors[3] = -Vector3.UnitY; //RIGHT
             Dir_Vectors[4] = Vector3.UnitZ; //UP
             Dir_Vectors[5] = -Vector3.UnitZ; //DOWN
-            Dir_Vectors[8] = new Vector3(0f, 0f, -0.5f); //DOWN_Nudge
-            Dir_Vectors[6] = Vector3.UnitX*2; //FORWARD
-            Dir_Vectors[7] = -Vector3.UnitX; //BACK
+            Dir_Vectors[6] = new Vector3(0.5f, 0f, 0f); //FORWARD_NUDGE
+            Dir_Vectors[7] = new Vector3(-0.5f, 0f, 0f);  //BACK_NUDGE
+            Dir_Vectors[8] = new Vector3(0f, 0.5f, 0f);  //LEFT_NUDGE
+            Dir_Vectors[9] = new Vector3(0f, -0.5f, 0f);  //RIGHT_NUDGE
+            Dir_Vectors[10] = new Vector3(0f, 0f, -0.5f); //DOWN_Nudge
         }
 
         private Vector3[] GetWalkDirectionVectors()
         {
-            Vector3[] vector = new Vector3[9];
+            Vector3[] vector = new Vector3[11];
             vector[0] = new Vector3(m_CameraUpAxis.Z, 0f, -m_CameraAtAxis.Z); //FORWARD
             vector[1] = new Vector3(-m_CameraUpAxis.Z, 0f, m_CameraAtAxis.Z); //BACK
             vector[2] = Vector3.UnitY; //LEFT
             vector[3] = -Vector3.UnitY; //RIGHT
             vector[4] = new Vector3(m_CameraAtAxis.Z, 0f, m_CameraUpAxis.Z); //UP
             vector[5] = new Vector3(-m_CameraAtAxis.Z, 0f, -m_CameraUpAxis.Z); //DOWN
-            vector[8] = new Vector3(-m_CameraAtAxis.Z, 0f, -m_CameraUpAxis.Z); //DOWN_Nudge
-            vector[6] = (new Vector3(m_CameraUpAxis.Z, 0f, -m_CameraAtAxis.Z) * 2); //FORWARD Nudge
-            vector[7] = new Vector3(-m_CameraUpAxis.Z, 0f, m_CameraAtAxis.Z); //BACK Nudge
+            vector[6] = new Vector3(m_CameraUpAxis.Z, 0f, -m_CameraAtAxis.Z); //FORWARD_NUDGE
+            vector[7] = new Vector3(-m_CameraUpAxis.Z, 0f, m_CameraAtAxis.Z); //BACK_NUDGE
+            vector[8] = Vector3.UnitY; //LEFT_NUDGE
+            vector[9] = -Vector3.UnitY; //RIGHT_NUDGE
+            vector[10] = new Vector3(-m_CameraAtAxis.Z, 0f, -m_CameraUpAxis.Z); //DOWN_NUDGE
             return vector;
         }
+        
+        private bool[] GetDirectionIsNudge()
+        {
+            bool[] isNudge = new bool[11];
+            isNudge[0] = false; //FORWARD
+            isNudge[1] = false; //BACK
+            isNudge[2] = false; //LEFT
+            isNudge[3] = false; //RIGHT
+            isNudge[4] = false; //UP
+            isNudge[5] = false; //DOWN
+            isNudge[6] = true; //FORWARD_NUDGE
+            isNudge[7] = true; //BACK_NUDGE
+            isNudge[8] = true; //LEFT_NUDGE
+            isNudge[9] = true; //RIGHT_NUDGE
+            isNudge[10] = true; //DOWN_Nudge
+            return isNudge;
+        }
+
 
         #endregion
 
@@ -839,6 +870,22 @@ namespace OpenSim.Region.Framework.Scenes
             {
                 Border crossedBorder = m_scene.GetCrossedBorder(pos, Cardinals.N);
                 pos.Y = crossedBorder.BorderLine.Z - 1;
+            }

+

+            //If they're TP'ing in or logging in, we haven't had time to add any known child regions yet.

+            //This has the unfortunate consequence that if somebody is TP'ing who is already a child agent,

+            //they'll bypass the landing point. But I can't think of any decent way of fixing this.

+            if (KnownChildRegionHandles.Count == 0)

+            {

+                ILandObject land = m_scene.LandChannel.GetLandObject(pos.X, pos.Y);

+                if (land != null)

+                {

+                    //Don't restrict gods, estate managers, or land owners to the TP point. This behaviour mimics agni.

+                    if (land.LandData.LandingType == (byte)1 && land.LandData.UserLocation != Vector3.Zero && m_userLevel < 200 && !m_scene.RegionInfo.EstateSettings.IsEstateManager(m_uuid) && land.LandData.OwnerID != m_uuid)

+                    {

+                        pos = land.LandData.UserLocation;

+                    }

+                }

             }
 
             if (pos.X < 0f || pos.Y < 0f || pos.Z < 0f)
@@ -972,9 +1019,10 @@ namespace OpenSim.Region.Framework.Scenes
         public void Teleport(Vector3 pos)
         {
             bool isFlying = false;
-            if (m_physicsActor != null)
-                isFlying = m_physicsActor.Flying;
             
+            if (m_physicsActor != null)
+                isFlying = m_physicsActor.Flying;

+

             RemoveFromPhysicalScene();
             Velocity = Vector3.Zero;
             AbsolutePosition = pos;
@@ -985,7 +1033,8 @@ namespace OpenSim.Region.Framework.Scenes
                     SetHeight(m_appearance.AvatarHeight);
             }
 
-            SendTerseUpdateToAllClients();
+            SendTerseUpdateToAllClients();

+            
         }
 
         public void TeleportWithMomentum(Vector3 pos)
@@ -1030,7 +1079,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)));
         }
@@ -1215,6 +1266,7 @@ namespace OpenSim.Region.Framework.Scenes
                 m_log.Error("[AVATAR]: NonFinite Avatar position detected... Reset Position. Mantis this please. Error #9999902");
 
                 m_pos = m_LastFinitePos;
+                
                 if (!m_pos.IsFinite())
                 {
                     m_pos.X = 127f;
@@ -1281,7 +1333,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)
@@ -1296,12 +1347,8 @@ 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.
-                //Animator.TrySetMovementAnimation("SIT_GROUND_CONSTRAINED");
-                SitGround = true;
-                
+                m_updateCount = 0;  // Kill animation update burst so that the SIT_G.. will stick.
+                Animator.TrySetMovementAnimation("SIT_GROUND_CONSTRAINED");
             }
 
             // In the future, these values might need to go global.
@@ -1351,6 +1398,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;
@@ -1365,9 +1417,12 @@ namespace OpenSim.Region.Framework.Scenes
                     else
                         dirVectors = Dir_Vectors;
 
-                    // The fact that m_movementflag is a byte needs to be fixed
-                    // it really should be a uint
-                    uint nudgehack = 250;
+                    bool[] isNudge = GetDirectionIsNudge();
+
+                    
+                    
+                    
+
                     foreach (Dir_ControlFlags DCF in DIR_CONTROL_FLAGS)
                     {
                         if (((uint)flags & (uint)DCF) != 0)
@@ -1377,40 +1432,28 @@ namespace OpenSim.Region.Framework.Scenes
                             try
                             {
                                 agent_control_v3 += dirVectors[i];
-                                //m_log.DebugFormat("[Motion]: {0}, {1}",i, dirVectors[i]);
+                                if (isNudge[i] == false)
+                                {
+                                    Nudging = false;
+                                }
                             }
                             catch (IndexOutOfRangeException)
                             {
                                 // Why did I get this?
                             }
 
-                            if ((m_movementflag & (byte)(uint)DCF) == 0)
+                            if ((m_movementflag & (uint)DCF) == 0)
                             {
-                                if (DCF == Dir_ControlFlags.DIR_CONTROL_FLAG_FORWARD_NUDGE || DCF == Dir_ControlFlags.DIR_CONTROL_FLAG_BACKWARD_NUDGE)
-                                {
-                                    m_movementflag |= (byte)nudgehack;
-                                }
                                 m_movementflag += (byte)(uint)DCF;
                                 update_movementflag = true;
                             }
                         }
                         else
                         {
-                            if ((m_movementflag & (byte)(uint)DCF) != 0 ||
-                                ((DCF == Dir_ControlFlags.DIR_CONTROL_FLAG_FORWARD_NUDGE || DCF == Dir_ControlFlags.DIR_CONTROL_FLAG_BACKWARD_NUDGE)
-                                && ((m_movementflag & (byte)nudgehack) == nudgehack))
-                                ) // This or is for Nudge forward
+                            if ((m_movementflag & (uint)DCF) != 0)
                             {
-                                m_movementflag -= ((byte)(uint)DCF);
-
+                                m_movementflag -= (byte)(uint)DCF;
                                 update_movementflag = true;
-                                /*
-                                    if ((DCF == Dir_ControlFlags.DIR_CONTROL_FLAG_FORWARD_NUDGE || DCF == Dir_ControlFlags.DIR_CONTROL_FLAG_BACKWARD_NUDGE)
-                                    && ((m_movementflag & (byte)nudgehack) == nudgehack))
-                                    {
-                                        m_log.Debug("Removed Hack flag");
-                                    }
-                                */
                             }
                             else
                             {
@@ -1454,6 +1497,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:
@@ -1542,13 +1588,13 @@ namespace OpenSim.Region.Framework.Scenes
                     //                    m_log.DebugFormat(
                     //                        "In {0} adding velocity to {1} of {2}", m_scene.RegionInfo.RegionName, Name, agent_control_v3);
 
-                    AddNewMovement(agent_control_v3, q);
+                    AddNewMovement(agent_control_v3, q, Nudging);
 
                     
                 }
             }
 
-            if (update_movementflag && ((flags & AgentManager.ControlFlags.AGENT_CONTROL_SIT_ON_GROUND) == 0) && (m_parentID == 0) && !SitGround)
+            if (update_movementflag)
                 Animator.UpdateMovementAnimations();
 
             m_scene.EventManager.TriggerOnClientMovement(this);
@@ -1563,7 +1609,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);
             
@@ -1605,7 +1650,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?
@@ -1627,7 +1672,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;
@@ -1660,55 +1705,84 @@ namespace OpenSim.Region.Framework.Scenes
         /// </summary>
         public void StandUp()
         {
-            if (SitGround)
-                SitGround = false;
-
             if (m_parentID != 0)
             {
-                m_log.Debug("StandupCode Executed");
                 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();

                 }
 
-                m_pos += m_parentPosition + new Vector3(0.0f, 0.0f, 2.0f*m_sitAvatarHeight);
-                m_parentPosition = Vector3.Zero;
+                    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
 
-                m_parentID = 0;
+                

+                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_parentPosition = Vector3.Zero;
+                                m_parentID = 0;
                 SendFullUpdateToAllClients();
                 m_requestedSitTargetID = 0;
+
                 if ((m_physicsActor != null) && (m_avHeight > 0))
                 {
                     SetHeight(m_avHeight);
                 }
             }
-
             Animator.TrySetMovementAnimation("STAND");
         }
 
@@ -1739,13 +1813,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;
@@ -1759,84 +1829,153 @@ 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;
+//Console.WriteLine("UsSmall autopilotTarget={0}", autopilotTarget);                                            
+                        }
+                        else return;            // occupied small
+                }       // end large/small
+            } // end Scripted/not
+            cameraAtOffset = part.GetCameraAtOffset();
+            cameraEyeOffset = part.GetCameraEyeOffset();
+            forceMouselook = part.GetForceMouselook();
+            if(cameraAtOffset == Vector3.Zero) cameraAtOffset =  new Vector3(0f, 0f, 0.1f); // 
+            if(cameraEyeOffset == Vector3.Zero) cameraEyeOffset =  new Vector3(0f, 0f, 0.1f); // 
 
-                            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);
         }
@@ -2131,31 +2270,66 @@ 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;
+Console.WriteLine("Scripted Sit ofset {0}", m_pos);                                             
                     }
                     else
                     {
-                        m_pos -= part.AbsolutePosition;
+                                                // if m_avUnscriptedSitPos is zero then Av sits above center
+                                                // Else Av sits at m_avUnscriptedSitPos                                         
+                                                
+                        // Non-scripted sit by Kitto Flora 21Nov09
+                        // Calculate angle of line from prim to Av
+                        Quaternion partIRot;
+//                      if (part.LinkNum == 1)
+//                      {                               // Root prim of linkset
+//                              partIRot = Quaternion.Inverse(part.ParentGroup.RootPart.RotationOffset);
+//                      }
+//                      else
+//                      {                               // single or child prim
+                                partIRot = Quaternion.Inverse(part.GetWorldRotation());
+//                      }
+                        Vector3 sitTargetPos= part.AbsolutePosition + m_avUnscriptedSitPos;
+                                float y_diff = (m_avInitialPos.Y - sitTargetPos.Y);
+                        float x_diff = ( m_avInitialPos.X - sitTargetPos.X);
+                        if(Math.Abs(x_diff) < 0.001f) x_diff = 0.001f;                  // avoid div by 0
+                        if(Math.Abs(y_diff) < 0.001f) y_diff = 0.001f;                  // avoid pol flip at 0
+                        float sit_angle = (float)Math.Atan2( (double)y_diff, (double)x_diff);
+                                                // NOTE: when sitting m_ pos and m_bodyRot are *relative* to the prim location/rotation, not 'World'.
+                        //  Av sits at world euler <0,0, z>, translated by part rotation 
+                        m_bodyRot = partIRot * Quaternion.CreateFromEulers(0f, 0f, sit_angle);          // sit at 0,0,inv-click
+                        
                         m_parentPosition = part.AbsolutePosition;
-                    }
+                        part.IsOccupied = true;
+                        m_pos = new Vector3(0f, 0f, 0.05f) +                                    // corrections to get Sit Animation
+                                        (new Vector3(0.0f, 0f, 0.61f) * partIRot) +     // located on center
+                                        (new Vector3(0.34f, 0f, 0.0f) * m_bodyRot) +
+                                        m_avUnscriptedSitPos;                                                   // adds click offset, if any
+                            //Set up raytrace to find top surface of prim
+                                        Vector3 size = part.Scale;
+                                                float mag = 2.0f; // 0.1f + (float)Math.Sqrt((size.X * size.X) + (size.Y * size.Y) + (size.Z * size.Z));
+                                                Vector3 start =  part.AbsolutePosition + new Vector3(0f, 0f, mag);
+                                                Vector3 down = new Vector3(0f, 0f, -1f);    
+//Console.WriteLine("st={0}  do={1}  ma={2}", start, down, mag);                                                
+                                m_scene.PhysicsScene.RaycastWorld(
+                                        start,          // Vector3 position,
+                                        down,           // Vector3 direction,
+                                        mag,            // float length,
+                                        SitAltitudeCallback);           // retMethod
+                        } // end scripted/not
                 }
-                else
+                else  // no Av
                 {
                     return;
                 }
@@ -2167,11 +2341,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
@@ -2201,7 +2400,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)
             {
@@ -2278,7 +2477,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));
         }
 
@@ -2293,7 +2492,7 @@ namespace OpenSim.Region.Framework.Scenes
             const float POSITION_TOLERANCE = 0.05f;
             //const int TIME_MS_TOLERANCE = 3000;
 
-            SendPrimUpdates();
+            
 
             if (m_newCoarseLocations)
             {
@@ -2329,6 +2528,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
@@ -3145,6 +3347,7 @@ namespace OpenSim.Region.Framework.Scenes
             m_callbackURI = cAgent.CallbackURI;
 
             m_pos = cAgent.Position;
+            
             m_velocity = cAgent.Velocity;
             m_CameraCenter = cAgent.Center;
             //m_avHeight = cAgent.Size.Z;
@@ -3233,14 +3436,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)
@@ -3291,18 +3505,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)
@@ -3909,5 +4134,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));
+            }
+
+
     }
 }
diff --git a/OpenSim/Region/Framework/Scenes/SceneViewer.cs b/OpenSim/Region/Framework/Scenes/SceneViewer.cs
index c6cf4cc..4ba4fab 100644
--- a/OpenSim/Region/Framework/Scenes/SceneViewer.cs
+++ b/OpenSim/Region/Framework/Scenes/SceneViewer.cs
@@ -75,7 +75,7 @@ namespace OpenSim.Region.Framework.Scenes
 
                     foreach (EntityBase e in m_presence.Scene.Entities)
                     {
-                        if (e is SceneObjectGroup)
+                        if (e != null && e is SceneObjectGroup)
                             m_pendingObjects.Enqueue((SceneObjectGroup)e);
                     }
                 }
diff --git a/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETCharacter.cs b/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETCharacter.cs
index ac4e2b9..f13c323 100644
--- a/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETCharacter.cs
+++ b/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETCharacter.cs
@@ -624,8 +624,7 @@ namespace OpenSim.Region.Physics.BulletDotNETPlugin
         {
             set { return; }
         }
-
-
+        
         public override Quaternion APIDTarget 
         { 
             set { return; }
diff --git a/OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs b/OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs
index e2a6a2e..f4245b6 100644
--- a/OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs
+++ b/OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs
@@ -996,7 +996,7 @@ namespace OpenSim.Region.Physics.BulletXPlugin
         {
 
         }
-        
+
         public override void VehicleFlags(int param, bool remove)
         {
 
diff --git a/OpenSim/Region/Physics/ChOdePlugin/AssemblyInfo.cs b/OpenSim/Region/Physics/ChOdePlugin/AssemblyInfo.cs
new file mode 100644
index 0000000..d65929a
--- /dev/null
+++ b/OpenSim/Region/Physics/ChOdePlugin/AssemblyInfo.cs
@@ -0,0 +1,58 @@
+/*
+ * Copyright (c) Contributors, http://opensimulator.org/
+ * See CONTRIBUTORS.TXT for a full list of copyright holders.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the OpenSimulator Project nor the
+ *       names of its contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+using System.Reflection;
+using System.Runtime.InteropServices;
+
+// Information about this assembly is defined by the following
+// attributes.
+//
+// change them to the information which is associated with the assembly
+// you compile.
+
+[assembly : AssemblyTitle("OdePlugin")]
+[assembly : AssemblyDescription("")]
+[assembly : AssemblyConfiguration("")]
+[assembly : AssemblyCompany("http://opensimulator.org")]
+[assembly : AssemblyProduct("OdePlugin")]
+[assembly : AssemblyCopyright("Copyright (c) OpenSimulator.org Developers 2007-2009")]
+[assembly : AssemblyTrademark("")]
+[assembly : AssemblyCulture("")]
+
+// This sets the default COM visibility of types in the assembly to invisible.
+// If you need to expose a type to COM, use [ComVisible(true)] on that type.
+
+[assembly : ComVisible(false)]
+
+// The assembly version has following format :
+//
+// Major.Minor.Build.Revision
+//
+// You can specify all values by your own or you can build default build and revision
+// numbers with the '*' character (the default):
+
+[assembly : AssemblyVersion("0.6.5.*")]
diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs
new file mode 100644
index 0000000..8e87ad9
--- /dev/null
+++ b/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs
@@ -0,0 +1,1357 @@
+/*
+ * Copyright (c) Contributors, http://opensimulator.org/
+ * See CONTRIBUTORS.TXT for a full list of copyright holders.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the OpenSimulator Project nor the
+ *       names of its contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+using System;
+using System.Collections.Generic;
+using System.Reflection;
+using OpenMetaverse;
+using Ode.NET;
+using OpenSim.Framework;
+using OpenSim.Region.Physics.Manager;
+using log4net;
+
+namespace OpenSim.Region.Physics.OdePlugin
+{
+    /// <summary>
+    /// Various properties that ODE uses for AMotors but isn't exposed in ODE.NET so we must define them ourselves.
+    /// </summary>
+
+    public enum dParam : int
+    {
+        LowStop = 0,
+        HiStop = 1,
+        Vel = 2,
+        FMax = 3,
+        FudgeFactor = 4,
+        Bounce = 5,
+        CFM = 6,
+        StopERP = 7,
+        StopCFM = 8,
+        LoStop2 = 256,
+        HiStop2 = 257,
+        Vel2 = 258,
+        FMax2 = 259,
+        StopERP2 = 7 + 256,
+        StopCFM2 = 8 + 256,
+        LoStop3 = 512,
+        HiStop3 = 513,
+        Vel3 = 514,
+        FMax3 = 515,
+        StopERP3 = 7 + 512,
+        StopCFM3 = 8 + 512
+    }
+    public class OdeCharacter : PhysicsActor
+    {
+        private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
+
+        private Vector3 _position;
+        private d.Vector3 _zeroPosition;
+        // private d.Matrix3 m_StandUpRotation;
+        private bool _zeroFlag = false;
+        private bool m_lastUpdateSent = false;
+        private Vector3 _velocity;
+        private Vector3 _target_velocity;
+        private Vector3 _acceleration;
+        private Vector3 m_rotationalVelocity;
+        private float m_mass = 80f;
+        public float m_density = 60f;
+        private bool m_pidControllerActive = true;
+        public float PID_D = 800.0f;
+        public float PID_P = 900.0f;
+        //private static float POSTURE_SERVO = 10000.0f;
+        public float CAPSULE_RADIUS = 0.37f;
+        public float CAPSULE_LENGTH = 2.140599f;
+        public float m_tensor = 3800000f;
+        public float heightFudgeFactor = 0.52f;
+        public float walkDivisor = 1.3f;
+        public float runDivisor = 0.8f;
+        private bool flying = false;
+        private bool m_iscolliding = false;
+        private bool m_iscollidingGround = false;
+        private bool m_wascolliding = false;
+        private bool m_wascollidingGround = false;
+        private bool m_iscollidingObj = false;
+        private bool m_alwaysRun = false;
+        private bool m_hackSentFall = false;
+        private bool m_hackSentFly = false;
+        private int m_requestedUpdateFrequency = 0;
+        private Vector3 m_taintPosition = Vector3.Zero;
+        public uint m_localID = 0;
+        public bool m_returnCollisions = false;
+        // taints and their non-tainted counterparts
+        public bool m_isPhysical = false; // the current physical status
+        public bool m_tainted_isPhysical = false; // set when the physical status is tainted (false=not existing in physics engine, true=existing)
+        public float MinimumGroundFlightOffset = 3f;
+
+        private float m_tainted_CAPSULE_LENGTH; // set when the capsule length changes. 
+        private float m_tiltMagnitudeWhenProjectedOnXYPlane = 0.1131371f; // used to introduce a fixed tilt because a straight-up capsule falls through terrain, probably a bug in terrain collider
+
+
+        private float m_buoyancy = 0f;
+
+        // private CollisionLocker ode;
+
+        private string m_name = String.Empty;
+
+        private bool[] m_colliderarr = new bool[11];
+        private bool[] m_colliderGroundarr = new bool[11];
+
+        // Default we're a Character
+        private CollisionCategories m_collisionCategories = (CollisionCategories.Character);
+
+        // Default, Collide with Other Geometries, spaces, bodies and characters.
+        private CollisionCategories m_collisionFlags = (CollisionCategories.Geom
+                                                        | CollisionCategories.Space
+                                                        | CollisionCategories.Body
+                                                        | CollisionCategories.Character
+                                                        | CollisionCategories.Land);
+        public IntPtr Body = IntPtr.Zero;
+        private OdeScene _parent_scene;
+        public IntPtr Shell = IntPtr.Zero;
+        public IntPtr Amotor = IntPtr.Zero;
+        public d.Mass ShellMass;
+        public bool collidelock = false;
+
+        public int m_eventsubscription = 0;
+        private CollisionEventUpdate CollisionEventsThisFrame = new CollisionEventUpdate();
+
+        // unique UUID of this character object
+        public UUID m_uuid;
+        public bool bad = false;
+
+        public OdeCharacter(String avName, OdeScene parent_scene, Vector3 pos, CollisionLocker dode, Vector3 size, float pid_d, float pid_p, float capsule_radius, float tensor, float density, float height_fudge_factor, float walk_divisor, float rundivisor)
+        {
+            m_uuid = UUID.Random();
+
+            if (pos.IsFinite())
+            {
+                if (pos.Z > 9999999f)
+                {
+                    pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
+                }
+                if (pos.Z < -90000f)
+                {
+                    pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
+                }
+                _position = pos;
+                m_taintPosition.X = pos.X;
+                m_taintPosition.Y = pos.Y;
+                m_taintPosition.Z = pos.Z;
+            }
+            else
+            {
+                _position = new Vector3(((float)_parent_scene.WorldExtents.X * 0.5f), ((float)_parent_scene.WorldExtents.Y * 0.5f), parent_scene.GetTerrainHeightAtXY(128f, 128f) + 10f);
+                m_taintPosition.X = _position.X;
+                m_taintPosition.Y = _position.Y;
+                m_taintPosition.Z = _position.Z;
+                m_log.Warn("[PHYSICS]: Got NaN Position on Character Create");
+            }
+
+            _parent_scene = parent_scene;
+
+            PID_D = pid_d;
+            PID_P = pid_p;
+            CAPSULE_RADIUS = capsule_radius;
+            m_tensor = tensor;
+            m_density = density;
+            heightFudgeFactor = height_fudge_factor;
+            walkDivisor = walk_divisor;
+            runDivisor = rundivisor;
+
+            // m_StandUpRotation =
+            //     new d.Matrix3(0.5f, 0.7071068f, 0.5f, -0.7071068f, 0f, 0.7071068f, 0.5f, -0.7071068f,
+            //                   0.5f);
+
+            for (int i = 0; i < 11; i++)
+            {
+                m_colliderarr[i] = false;
+            }
+            CAPSULE_LENGTH = (size.Z * 1.15f) - CAPSULE_RADIUS * 2.0f;
+            //m_log.Info("[SIZE]: " + CAPSULE_LENGTH.ToString());
+            m_tainted_CAPSULE_LENGTH = CAPSULE_LENGTH;
+
+            m_isPhysical = false; // current status: no ODE information exists
+            m_tainted_isPhysical = true; // new tainted status: need to create ODE information
+
+            _parent_scene.AddPhysicsActorTaint(this);
+            
+            m_name = avName;
+        }
+
+        public override int PhysicsActorType
+        {
+            get { return (int) ActorTypes.Agent; }
+            set { return; }
+        }
+
+        /// <summary>
+        /// If this is set, the avatar will move faster
+        /// </summary>
+        public override bool SetAlwaysRun
+        {
+            get { return m_alwaysRun; }
+            set { m_alwaysRun = value; }
+        }
+
+        public override uint LocalID
+        {
+            set { m_localID = value; }
+        }
+
+        public override bool Grabbed
+        {
+            set { return; }
+        }
+
+        public override bool Selected
+        {
+            set { return; }
+        }
+
+        public override float Buoyancy
+        {
+            get { return m_buoyancy; }
+            set { m_buoyancy = value; }
+        }
+
+        public override bool FloatOnWater
+        {
+            set { return; }
+        }
+
+        public override bool IsPhysical
+        {
+            get { return false; }
+            set { return; }
+        }
+
+        public override bool ThrottleUpdates
+        {
+            get { return false; }
+            set { return; }
+        }
+
+        public override bool Flying
+        {
+            get { return flying; }
+            set { flying = value; }
+        }
+
+        /// <summary>
+        /// Returns if the avatar is colliding in general.
+        /// This includes the ground and objects and avatar.
+        /// </summary>
+        public override bool IsColliding
+        {
+            get { return m_iscolliding; }
+            set
+            {
+                int i;
+                int truecount = 0;
+                int falsecount = 0;
+
+                if (m_colliderarr.Length >= 10)
+                {
+                    for (i = 0; i < 10; i++)
+                    {
+                        m_colliderarr[i] = m_colliderarr[i + 1];
+                    }
+                }
+                m_colliderarr[10] = value;
+
+                for (i = 0; i < 11; i++)
+                {
+                    if (m_colliderarr[i])
+                    {
+                        truecount++;
+                    }
+                    else
+                    {
+                        falsecount++;
+                    }
+                }
+
+                // Equal truecounts and false counts means we're colliding with something.
+
+                if (falsecount > 1.2*truecount)
+                {
+                    m_iscolliding = false;
+                }
+                else
+                {
+                    m_iscolliding = true;
+                }
+                if (m_wascolliding != m_iscolliding)
+                {
+                    //base.SendCollisionUpdate(new CollisionEventUpdate());
+                }
+                m_wascolliding = m_iscolliding;
+            }
+        }
+
+        /// <summary>
+        /// Returns if an avatar is colliding with the ground
+        /// </summary>
+        public override bool CollidingGround
+        {
+            get { return m_iscollidingGround; }
+            set
+            {
+                // Collisions against the ground are not really reliable
+                // So, to get a consistant value we have to average the current result over time
+                // Currently we use 1 second = 10 calls to this.
+                int i;
+                int truecount = 0;
+                int falsecount = 0;
+
+                if (m_colliderGroundarr.Length >= 10)
+                {
+                    for (i = 0; i < 10; i++)
+                    {
+                        m_colliderGroundarr[i] = m_colliderGroundarr[i + 1];
+                    }
+                }
+                m_colliderGroundarr[10] = value;
+
+                for (i = 0; i < 11; i++)
+                {
+                    if (m_colliderGroundarr[i])
+                    {
+                        truecount++;
+                    }
+                    else
+                    {
+                        falsecount++;
+                    }
+                }
+
+                // Equal truecounts and false counts means we're colliding with something.
+
+                if (falsecount > 1.2*truecount)
+                {
+                    m_iscollidingGround = false;
+                }
+                else
+                {
+                    m_iscollidingGround = true;
+                }
+                if (m_wascollidingGround != m_iscollidingGround)
+                {
+                    //base.SendCollisionUpdate(new CollisionEventUpdate());
+                }
+                m_wascollidingGround = m_iscollidingGround;
+            }
+        }
+
+        /// <summary>
+        /// Returns if the avatar is colliding with an object
+        /// </summary>
+        public override bool CollidingObj
+        {
+            get { return m_iscollidingObj; }
+            set
+            {
+                m_iscollidingObj = value;
+                if (value)
+                    m_pidControllerActive = false;
+                else
+                    m_pidControllerActive = true;
+            }
+        }
+
+        /// <summary>
+        /// turn the PID controller on or off.
+        /// The PID Controller will turn on all by itself in many situations
+        /// </summary>
+        /// <param name="status"></param>
+        public void SetPidStatus(bool status)
+        {
+            m_pidControllerActive = status;
+        }
+
+        public override bool Stopped
+        {
+            get { return _zeroFlag; }
+        }
+
+        /// <summary>
+        /// This 'puts' an avatar somewhere in the physics space.
+        /// Not really a good choice unless you 'know' it's a good
+        /// spot otherwise you're likely to orbit the avatar.
+        /// </summary>
+        public override Vector3 Position
+        {
+            get { return _position; }
+            set
+            {
+                if (Body == IntPtr.Zero || Shell == IntPtr.Zero)
+                {
+                    if (value.IsFinite())
+                    {
+                        if (value.Z > 9999999f)
+                        {
+                            value.Z = _parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
+                        }
+                        if (value.Z < -90000f)
+                        {
+                            value.Z = _parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
+                        }
+
+                        _position.X = value.X;
+                        _position.Y = value.Y;
+                        _position.Z = value.Z;
+
+                        m_taintPosition.X = value.X;
+                        m_taintPosition.Y = value.Y;
+                        m_taintPosition.Z = value.Z;
+                        _parent_scene.AddPhysicsActorTaint(this);
+                    }
+                    else
+                    {
+                        m_log.Warn("[PHYSICS]: Got a NaN Position from Scene on a Character");
+                    }
+                }
+            }
+        }
+
+        public override Vector3 RotationalVelocity
+        {
+            get { return m_rotationalVelocity; }
+            set { m_rotationalVelocity = value; }
+        }
+
+        /// <summary>
+        /// This property sets the height of the avatar only.  We use the height to make sure the avatar stands up straight
+        /// and use it to offset landings properly
+        /// </summary>
+        public override Vector3 Size
+        {
+            get { return new Vector3(CAPSULE_RADIUS * 2, CAPSULE_RADIUS * 2, CAPSULE_LENGTH); }
+            set
+            {
+                if (value.IsFinite())
+                {
+                    m_pidControllerActive = true;
+
+                    Vector3 SetSize = value;
+                    m_tainted_CAPSULE_LENGTH = (SetSize.Z*1.15f) - CAPSULE_RADIUS*2.0f;
+                    //m_log.Info("[SIZE]: " + CAPSULE_LENGTH.ToString());
+
+                    Velocity = Vector3.Zero;
+
+                    _parent_scene.AddPhysicsActorTaint(this);
+                }
+                else
+                {
+                    m_log.Warn("[PHYSICS]: Got a NaN Size from Scene on a Character");
+                }
+            }
+        }
+
+        private void AlignAvatarTiltWithCurrentDirectionOfMovement(Vector3 movementVector)
+        {
+            movementVector.Z = 0f;
+            float magnitude = (float)Math.Sqrt((double)(movementVector.X * movementVector.X + movementVector.Y * movementVector.Y));
+            if (magnitude < 0.1f) return;
+
+            // normalize the velocity vector
+            float invMagnitude = 1.0f / magnitude;
+            movementVector.X *= invMagnitude;
+            movementVector.Y *= invMagnitude;
+
+            // if we change the capsule heading too often, the capsule can fall down
+            // therefore we snap movement vector to just 1 of 4 predefined directions (ne, nw, se, sw),
+            // meaning only 4 possible capsule tilt orientations
+            if (movementVector.X > 0)
+            {
+                // east
+                if (movementVector.Y > 0)
+                {
+                    // northeast
+                    movementVector.X = (float)Math.Sqrt(2.0);
+                    movementVector.Y = (float)Math.Sqrt(2.0);
+                }
+                else
+                {
+                    // southeast
+                    movementVector.X = (float)Math.Sqrt(2.0);
+                    movementVector.Y = -(float)Math.Sqrt(2.0);
+                }
+            }
+            else
+            {
+                // west
+                if (movementVector.Y > 0)
+                {
+                    // northwest
+                    movementVector.X = -(float)Math.Sqrt(2.0);
+                    movementVector.Y = (float)Math.Sqrt(2.0);
+                }
+                else
+                {
+                    // southwest
+                    movementVector.X = -(float)Math.Sqrt(2.0);
+                    movementVector.Y = -(float)Math.Sqrt(2.0);
+                }
+            }
+
+
+            // movementVector.Z is zero
+
+            // calculate tilt components based on desired amount of tilt and current (snapped) heading.
+            // the "-" sign is to force the tilt to be OPPOSITE the direction of movement.
+            float xTiltComponent = -movementVector.X * m_tiltMagnitudeWhenProjectedOnXYPlane;
+            float yTiltComponent = -movementVector.Y * m_tiltMagnitudeWhenProjectedOnXYPlane;
+
+            //m_log.Debug("[PHYSICS] changing avatar tilt");
+            d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, xTiltComponent);
+            d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, xTiltComponent); // must be same as lowstop, else a different, spurious tilt is introduced
+            d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, yTiltComponent);
+            d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, yTiltComponent); // same as lowstop
+            d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, 0f);
+            d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0f); // same as lowstop
+        }
+
+        /// <summary>
+        /// This creates the Avatar's physical Surrogate at the position supplied
+        /// </summary>
+        /// <param name="npositionX"></param>
+        /// <param name="npositionY"></param>
+        /// <param name="npositionZ"></param>
+
+        // WARNING: This MUST NOT be called outside of ProcessTaints, else we can have unsynchronized access
+        // to ODE internals. ProcessTaints is called from within thread-locked Simulate(), so it is the only 
+        // place that is safe to call this routine AvatarGeomAndBodyCreation.
+        private void AvatarGeomAndBodyCreation(float npositionX, float npositionY, float npositionZ, float tensor)
+        {
+            //CAPSULE_LENGTH = -5;
+            //CAPSULE_RADIUS = -5;
+            int dAMotorEuler = 1;
+            _parent_scene.waitForSpaceUnlock(_parent_scene.space);
+            if (CAPSULE_LENGTH <= 0)
+            {
+                m_log.Warn("[PHYSICS]: The capsule size you specified in opensim.ini is invalid!  Setting it to the smallest possible size!");
+                CAPSULE_LENGTH = 0.01f;
+
+            }
+
+            if (CAPSULE_RADIUS <= 0)
+            {
+                m_log.Warn("[PHYSICS]: The capsule size you specified in opensim.ini is invalid!  Setting it to the smallest possible size!");
+                CAPSULE_RADIUS = 0.01f;
+
+            }
+            Shell = d.CreateCapsule(_parent_scene.space, CAPSULE_RADIUS, CAPSULE_LENGTH);
+
+            d.GeomSetCategoryBits(Shell, (int)m_collisionCategories);
+            d.GeomSetCollideBits(Shell, (int)m_collisionFlags);
+
+            d.MassSetCapsuleTotal(out ShellMass, m_mass, 2, CAPSULE_RADIUS, CAPSULE_LENGTH);
+            Body = d.BodyCreate(_parent_scene.world);
+            d.BodySetPosition(Body, npositionX, npositionY, npositionZ);
+            
+            _position.X = npositionX;
+            _position.Y = npositionY;
+            _position.Z = npositionZ;
+
+            
+            m_taintPosition.X = npositionX;
+            m_taintPosition.Y = npositionY;
+            m_taintPosition.Z = npositionZ;
+
+            d.BodySetMass(Body, ref ShellMass);
+            d.Matrix3 m_caprot;
+            // 90 Stand up on the cap of the capped cyllinder
+            if (_parent_scene.IsAvCapsuleTilted)
+            {
+                d.RFromAxisAndAngle(out m_caprot, 1, 0, 1, (float)(Math.PI / 2));
+            }
+            else
+            {
+                d.RFromAxisAndAngle(out m_caprot, 0, 0, 1, (float)(Math.PI / 2));
+            }
+
+
+            d.GeomSetRotation(Shell, ref m_caprot);
+            d.BodySetRotation(Body, ref m_caprot);
+
+            d.GeomSetBody(Shell, Body);
+
+
+            // The purpose of the AMotor here is to keep the avatar's physical
+            // surrogate from rotating while moving
+            Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero);
+            d.JointAttach(Amotor, Body, IntPtr.Zero);
+            d.JointSetAMotorMode(Amotor, dAMotorEuler);
+            d.JointSetAMotorNumAxes(Amotor, 3);
+            d.JointSetAMotorAxis(Amotor, 0, 0, 1, 0, 0);
+            d.JointSetAMotorAxis(Amotor, 1, 0, 0, 1, 0);
+            d.JointSetAMotorAxis(Amotor, 2, 0, 0, 0, 1);
+            d.JointSetAMotorAngle(Amotor, 0, 0);
+            d.JointSetAMotorAngle(Amotor, 1, 0);
+            d.JointSetAMotorAngle(Amotor, 2, 0);
+
+            // These lowstops and high stops are effectively (no wiggle room)
+            if (_parent_scene.IsAvCapsuleTilted)
+            {
+                d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, -0.000000000001f);
+                d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -0.000000000001f);
+                d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, -0.000000000001f);
+                d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0.000000000001f);
+                d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0.000000000001f);
+                d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0.000000000001f);
+            }
+            else
+            {
+                #region Documentation of capsule motor LowStop and HighStop parameters
+                // Intentionally introduce some tilt into the capsule by setting
+                // the motor stops to small epsilon values. This small tilt prevents
+                // the capsule from falling into the terrain; a straight-up capsule
+                // (with -0..0 motor stops) falls into the terrain for reasons yet
+                // to be comprehended in their entirety.
+                #endregion
+                AlignAvatarTiltWithCurrentDirectionOfMovement(Vector3.Zero);
+                d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, 0.08f);
+                d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -0f);
+                d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, 0.08f);
+                d.JointSetAMotorParam(Amotor, (int)dParam.HiStop,  0.08f); // must be same as lowstop, else a different, spurious tilt is introduced
+                d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0f); // same as lowstop
+                d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0.08f); // same as lowstop
+            }
+
+            // Fudge factor is 1f by default, we're setting it to 0.  We don't want it to Fudge or the
+            // capped cyllinder will fall over
+            d.JointSetAMotorParam(Amotor, (int)dParam.FudgeFactor, 0f);
+            d.JointSetAMotorParam(Amotor, (int)dParam.FMax, tensor);
+
+            //d.Matrix3 bodyrotation = d.BodyGetRotation(Body);
+            //d.QfromR(
+            //d.Matrix3 checkrotation = new d.Matrix3(0.7071068,0.5, -0.7071068,
+            //
+            //m_log.Info("[PHYSICSAV]: Rotation: " + bodyrotation.M00 + " : " + bodyrotation.M01 + " : " + bodyrotation.M02 + " : " + bodyrotation.M10 + " : " + bodyrotation.M11 + " : " + bodyrotation.M12 + " : " + bodyrotation.M20 + " : " + bodyrotation.M21 + " : " + bodyrotation.M22);
+            //standupStraight();
+        }
+
+        //
+        /// <summary>
+        /// Uses the capped cyllinder volume formula to calculate the avatar's mass.
+        /// This may be used in calculations in the scene/scenepresence
+        /// </summary>
+        public override float Mass
+        {
+            get
+            {
+                float AVvolume = (float) (Math.PI*Math.Pow(CAPSULE_RADIUS, 2)*CAPSULE_LENGTH);
+                return m_density*AVvolume;
+            }
+        }
+        public override void link(PhysicsActor obj)
+        {
+
+        }
+
+        public override void delink()
+        {
+
+        }
+
+        public override void LockAngularMotion(Vector3 axis)
+        {
+
+        }
+
+//      This code is very useful. Written by DanX0r. We're just not using it right now.
+//      Commented out to prevent a warning.
+//
+//         private void standupStraight()
+//         {
+//             // The purpose of this routine here is to quickly stabilize the Body while it's popped up in the air.
+//             // The amotor needs a few seconds to stabilize so without it, the avatar shoots up sky high when you
+//             // change appearance and when you enter the simulator
+//             // After this routine is done, the amotor stabilizes much quicker
+//             d.Vector3 feet;
+//             d.Vector3 head;
+//             d.BodyGetRelPointPos(Body, 0.0f, 0.0f, -1.0f, out feet);
+//             d.BodyGetRelPointPos(Body, 0.0f, 0.0f, 1.0f, out head);
+//             float posture = head.Z - feet.Z;
+
+//             // restoring force proportional to lack of posture:
+//             float servo = (2.5f - posture) * POSTURE_SERVO;
+//             d.BodyAddForceAtRelPos(Body, 0.0f, 0.0f, servo, 0.0f, 0.0f, 1.0f);
+//             d.BodyAddForceAtRelPos(Body, 0.0f, 0.0f, -servo, 0.0f, 0.0f, -1.0f);
+//             //d.Matrix3 bodyrotation = d.BodyGetRotation(Body);
+//             //m_log.Info("[PHYSICSAV]: Rotation: " + bodyrotation.M00 + " : " + bodyrotation.M01 + " : " + bodyrotation.M02 + " : " + bodyrotation.M10 + " : " + bodyrotation.M11 + " : " + bodyrotation.M12 + " : " + bodyrotation.M20 + " : " + bodyrotation.M21 + " : " + bodyrotation.M22);
+//         }
+
+        public override Vector3 Force
+        {
+            get { return _target_velocity; }
+            set { return; }
+        }
+
+        public override int VehicleType
+        {
+            get { return 0; }
+            set { return; }
+        }
+
+        public override void VehicleFloatParam(int param, float value)
+        {
+
+        }
+
+        public override void VehicleVectorParam(int param, Vector3 value)
+        {
+
+        }
+
+        public override void VehicleRotationParam(int param, Quaternion rotation)
+        {
+
+        }
+        
+        public override void VehicleFlags(int flags, bool remove)
+        {
+        }
+
+        public override void SetVolumeDetect(int param)
+        {
+
+        }
+
+        public override Vector3 CenterOfMass
+        {
+            get { return Vector3.Zero; }
+        }
+
+        public override Vector3 GeometricCenter
+        {
+            get { return Vector3.Zero; }
+        }
+
+        public override PrimitiveBaseShape Shape
+        {
+            set { return; }
+        }
+
+        public override Vector3 Velocity
+        {
+            get {
+                // There's a problem with Vector3.Zero! Don't Use it Here!
+                if (_zeroFlag)
+                    return Vector3.Zero;
+                m_lastUpdateSent = false;
+                return _velocity;
+            }
+            set
+            {
+                if (value.IsFinite())
+                {
+                    m_pidControllerActive = true;
+                    _target_velocity = value;
+                }
+                else
+                {
+                    m_log.Warn("[PHYSICS]: Got a NaN velocity from Scene in a Character");
+                }
+            }
+        }
+
+        public override Vector3 Torque
+        {
+            get { return Vector3.Zero; }
+            set { return; }
+        }
+
+        public override float CollisionScore
+        {
+            get { return 0f; }
+            set { }
+        }
+
+        public override bool Kinematic
+        {
+            get { return false; }
+            set { }
+        }
+
+        public override Quaternion Orientation
+        {
+            get { return Quaternion.Identity; }
+            set {
+                //Matrix3 or = Orientation.ToRotationMatrix();
+                //d.Matrix3 ord = new d.Matrix3(or.m00, or.m10, or.m20, or.m01, or.m11, or.m21, or.m02, or.m12, or.m22);
+                //d.BodySetRotation(Body, ref ord);
+            }
+        }
+
+        public override Vector3 Acceleration
+        {
+            get { return _acceleration; }
+        }
+
+        public void SetAcceleration(Vector3 accel)
+        {
+            m_pidControllerActive = true;
+            _acceleration = accel;
+        }
+
+        /// <summary>
+        /// Adds the force supplied to the Target Velocity
+        /// The PID controller takes this target velocity and tries to make it a reality
+        /// </summary>
+        /// <param name="force"></param>
+        public override void AddForce(Vector3 force, bool pushforce)
+        {
+            if (force.IsFinite())
+            {
+                if (pushforce)
+                {
+                    m_pidControllerActive = false;
+                    force *= 100f;
+                    doForce(force);
+                    // If uncommented, things get pushed off world
+                    //
+                    // m_log.Debug("Push!");
+                    // _target_velocity.X += force.X;
+                    // _target_velocity.Y += force.Y;
+                    // _target_velocity.Z += force.Z;
+                }
+                else
+                {
+                    m_pidControllerActive = true;
+                    _target_velocity.X += force.X;
+                    _target_velocity.Y += force.Y;
+                    _target_velocity.Z += force.Z;
+                }
+            }
+            else
+            {
+                m_log.Warn("[PHYSICS]: Got a NaN force applied to a Character");
+            }
+            //m_lastUpdateSent = false;
+        }
+
+        public override void AddAngularForce(Vector3 force, bool pushforce)
+        {
+
+        }
+
+        /// <summary>
+        /// After all of the forces add up with 'add force' we apply them with doForce
+        /// </summary>
+        /// <param name="force"></param>
+        public void doForce(Vector3 force)
+        {
+            if (!collidelock)
+            {
+                d.BodyAddForce(Body, force.X, force.Y, force.Z);
+                //d.BodySetRotation(Body, ref m_StandUpRotation);
+                //standupStraight();
+
+            }
+        }
+
+        public override void SetMomentum(Vector3 momentum)
+        {
+        }
+
+
+        /// <summary>
+        /// Called from Simulate
+        /// This is the avatar's movement control + PID Controller
+        /// </summary>
+        /// <param name="timeStep"></param>
+        public void Move(float timeStep, List<OdeCharacter> defects)
+        {
+            //  no lock; for now it's only called from within Simulate()
+
+            // If the PID Controller isn't active then we set our force
+            // calculating base velocity to the current position
+
+            if (Body == IntPtr.Zero)
+                return;
+
+            if (m_pidControllerActive == false)
+            {
+                _zeroPosition = d.BodyGetPosition(Body);
+            }
+            //PidStatus = true;
+
+            d.Vector3 localpos = d.BodyGetPosition(Body);
+            Vector3 localPos = new Vector3(localpos.X, localpos.Y, localpos.Z);
+            
+            if (!localPos.IsFinite())
+            {
+
+                m_log.Warn("[PHYSICS]: Avatar Position is non-finite!");
+                defects.Add(this);
+                // _parent_scene.RemoveCharacter(this);
+
+                // destroy avatar capsule and related ODE data
+                if (Amotor != IntPtr.Zero)
+                {
+                    // Kill the Amotor
+                    d.JointDestroy(Amotor);
+                    Amotor = IntPtr.Zero;
+                }
+
+                //kill the Geometry
+                _parent_scene.waitForSpaceUnlock(_parent_scene.space);
+
+                if (Body != IntPtr.Zero)
+                {
+                    //kill the body
+                    d.BodyDestroy(Body);
+
+                    Body = IntPtr.Zero;
+                }
+
+                if (Shell != IntPtr.Zero)
+                {
+                    d.GeomDestroy(Shell);
+                    _parent_scene.geom_name_map.Remove(Shell);
+                    Shell = IntPtr.Zero;
+                }
+
+                return;
+            }
+
+            Vector3 vec = Vector3.Zero;
+            d.Vector3 vel = d.BodyGetLinearVel(Body);
+
+            float movementdivisor = 1f;
+
+            if (!m_alwaysRun)
+            {
+                movementdivisor = walkDivisor;
+            }
+            else
+            {
+                movementdivisor = runDivisor;
+            }
+
+            //  if velocity is zero, use position control; otherwise, velocity control
+            if (_target_velocity.X == 0.0f && _target_velocity.Y == 0.0f && _target_velocity.Z == 0.0f && m_iscolliding)
+            {
+                //  keep track of where we stopped.  No more slippin' & slidin'
+                if (!_zeroFlag)
+                {
+                    _zeroFlag = true;
+                    _zeroPosition = d.BodyGetPosition(Body);
+                }
+                if (m_pidControllerActive)
+                {
+                    // We only want to deactivate the PID Controller if we think we want to have our surrogate
+                    // react to the physics scene by moving it's position.
+                    // Avatar to Avatar collisions
+                    // Prim to avatar collisions
+
+                    d.Vector3 pos = d.BodyGetPosition(Body);
+                    vec.X = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2);
+                    vec.Y = (_target_velocity.Y - vel.Y)*(PID_D) + (_zeroPosition.Y - pos.Y)* (PID_P * 2);
+                    if (flying)
+                    {
+                        vec.Z = (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P;
+                    }
+                }
+                //PidStatus = true;
+            }
+            else
+            {
+                m_pidControllerActive = true;
+                _zeroFlag = false;
+                if (m_iscolliding && !flying)
+                {
+                    // We're standing on something
+                    vec.X = ((_target_velocity.X / movementdivisor) - vel.X) * (PID_D);
+                    vec.Y = ((_target_velocity.Y / movementdivisor) - vel.Y) * (PID_D);
+                }
+                else if (m_iscolliding && flying)
+                {
+                    // We're flying and colliding with something
+                    vec.X = ((_target_velocity.X/movementdivisor) - vel.X)*(PID_D / 16);
+                    vec.Y = ((_target_velocity.Y/movementdivisor) - vel.Y)*(PID_D / 16);
+                }
+                else if (!m_iscolliding && flying)
+                {
+                    // we're in mid air suspended
+                    vec.X = ((_target_velocity.X / movementdivisor) - vel.X) * (PID_D/6);
+                    vec.Y = ((_target_velocity.Y / movementdivisor) - vel.Y) * (PID_D/6);
+                }
+
+                if (m_iscolliding && !flying && _target_velocity.Z > 0.0f)
+                {
+                    // We're colliding with something and we're not flying but we're moving
+                    // This means we're walking or running.
+                    d.Vector3 pos = d.BodyGetPosition(Body);
+                    vec.Z = (_target_velocity.Z - vel.Z)*PID_D + (_zeroPosition.Z - pos.Z)*PID_P;
+                    if (_target_velocity.X > 0)
+                    {
+                        vec.X = ((_target_velocity.X - vel.X)/1.2f)*PID_D;
+                    }
+                    if (_target_velocity.Y > 0)
+                    {
+                        vec.Y = ((_target_velocity.Y - vel.Y)/1.2f)*PID_D;
+                    }
+                }
+                else if (!m_iscolliding && !flying)
+                {
+                    // we're not colliding and we're not flying so that means we're falling!
+                    // m_iscolliding includes collisions with the ground.
+
+                    // d.Vector3 pos = d.BodyGetPosition(Body);
+                    if (_target_velocity.X > 0)
+                    {
+                        vec.X = ((_target_velocity.X - vel.X)/1.2f)*PID_D;
+                    }
+                    if (_target_velocity.Y > 0)
+                    {
+                        vec.Y = ((_target_velocity.Y - vel.Y)/1.2f)*PID_D;
+                    }
+                }
+
+                if (flying)
+                {
+                    vec.Z = (_target_velocity.Z - vel.Z) * (PID_D);
+                }
+            }
+            if (flying)
+            {
+                vec.Z += ((-1 * _parent_scene.gravityz)*m_mass);
+
+                //Added for auto fly height. Kitto Flora
+                //d.Vector3 pos = d.BodyGetPosition(Body);
+                float target_altitude = _parent_scene.GetTerrainHeightAtXY(_position.X, _position.Y) + MinimumGroundFlightOffset;
+                
+                if (_position.Z < target_altitude)
+                {
+                    vec.Z += (target_altitude - _position.Z) * PID_P * 5.0f;
+                }
+                // end add Kitto Flora
+            }
+            if (vec.IsFinite())
+            {
+                doForce(vec);
+                if (!_zeroFlag)
+                {
+                  AlignAvatarTiltWithCurrentDirectionOfMovement(vec);
+                }
+            }
+            else
+            {
+                m_log.Warn("[PHYSICS]: Got a NaN force vector in Move()");
+                m_log.Warn("[PHYSICS]: Avatar Position is non-finite!");
+                defects.Add(this);
+                // _parent_scene.RemoveCharacter(this);
+                // destroy avatar capsule and related ODE data
+                if (Amotor != IntPtr.Zero)
+                {
+                    // Kill the Amotor
+                    d.JointDestroy(Amotor);
+                    Amotor = IntPtr.Zero;
+                }
+                //kill the Geometry
+                _parent_scene.waitForSpaceUnlock(_parent_scene.space);
+
+                if (Body != IntPtr.Zero)
+                {
+                    //kill the body
+                    d.BodyDestroy(Body);
+
+                    Body = IntPtr.Zero;
+                }
+
+                if (Shell != IntPtr.Zero)
+                {
+                    d.GeomDestroy(Shell);
+                    _parent_scene.geom_name_map.Remove(Shell);
+                    Shell = IntPtr.Zero;
+                }
+            }
+        }
+
+        /// <summary>
+        /// Updates the reported position and velocity.  This essentially sends the data up to ScenePresence.
+        /// </summary>
+        public void UpdatePositionAndVelocity()
+        {
+            //  no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit!
+            d.Vector3 vec;
+            try
+            {
+                vec = d.BodyGetPosition(Body);
+            }
+            catch (NullReferenceException)
+            {
+                bad = true;
+                _parent_scene.BadCharacter(this);
+                vec = new d.Vector3(_position.X, _position.Y, _position.Z);
+                base.RaiseOutOfBounds(_position); // Tells ScenePresence that there's a problem!
+                m_log.WarnFormat("[ODEPLUGIN]: Avatar Null reference for Avatar {0}, physical actor {1}", m_name, m_uuid);
+            }
+            
+
+            //  kluge to keep things in bounds.  ODE lets dead avatars drift away (they should be removed!)
+            if (vec.X < 0.0f) vec.X = 0.0f;
+            if (vec.Y < 0.0f) vec.Y = 0.0f;
+            if (vec.X > (int)_parent_scene.WorldExtents.X - 0.05f) vec.X = (int)_parent_scene.WorldExtents.X - 0.05f;
+            if (vec.Y > (int)_parent_scene.WorldExtents.Y - 0.05f) vec.Y = (int)_parent_scene.WorldExtents.Y - 0.05f;
+
+            _position.X = vec.X;
+            _position.Y = vec.Y;
+            _position.Z = vec.Z;
+
+            // Did we move last? = zeroflag
+            // This helps keep us from sliding all over
+
+            if (_zeroFlag)
+            {
+                _velocity.X = 0.0f;
+                _velocity.Y = 0.0f;
+                _velocity.Z = 0.0f;
+
+                // Did we send out the 'stopped' message?
+                if (!m_lastUpdateSent)
+                {
+                    m_lastUpdateSent = true;
+                    //base.RequestPhysicsterseUpdate();
+
+                }
+            }
+            else
+            {
+                m_lastUpdateSent = false;
+                try
+                {
+                    vec = d.BodyGetLinearVel(Body);
+                }
+                catch (NullReferenceException)
+                {
+                    vec.X = _velocity.X;
+                    vec.Y = _velocity.Y;
+                    vec.Z = _velocity.Z;
+                }
+                _velocity.X = (vec.X);
+                _velocity.Y = (vec.Y);
+
+                _velocity.Z = (vec.Z);
+
+                if (_velocity.Z < -6 && !m_hackSentFall)
+                {
+                    m_hackSentFall = true;
+                    m_pidControllerActive = false;
+                }
+                else if (flying && !m_hackSentFly)
+                {
+                    //m_hackSentFly = true;
+                    //base.SendCollisionUpdate(new CollisionEventUpdate());
+                }
+                else
+                {
+                    m_hackSentFly = false;
+                    m_hackSentFall = false;
+                }
+            }
+        }
+
+        /// <summary>
+        /// Cleanup the things we use in the scene.
+        /// </summary>
+        public void Destroy()
+        {
+            m_tainted_isPhysical = false;
+            _parent_scene.AddPhysicsActorTaint(this);
+        }
+
+        public override void CrossingFailure()
+        {
+        }
+
+        public override Vector3 PIDTarget { set { return; } }
+        public override bool PIDActive { set { return; } }
+        public override float PIDTau { set { return; } }
+
+        public override float PIDHoverHeight { set { return; } }
+        public override bool PIDHoverActive { set { return; } }
+        public override PIDHoverType PIDHoverType { set { return; } }
+        public override float PIDHoverTau { set { return; } }
+
+        public override Quaternion APIDTarget{ set { return; } }
+
+        public override bool APIDActive{ set { return; } }
+
+        public override float APIDStrength{ set { return; } }
+
+        public override float APIDDamping{ set { return; } }
+
+                
+        public override void SubscribeEvents(int ms)
+        {
+            m_requestedUpdateFrequency = ms;
+            m_eventsubscription = ms;
+            _parent_scene.addCollisionEventReporting(this);
+        }
+        public override void UnSubscribeEvents()
+        {
+            _parent_scene.remCollisionEventReporting(this);
+            m_requestedUpdateFrequency = 0;
+            m_eventsubscription = 0;
+        }
+        public void AddCollisionEvent(uint CollidedWith, ContactPoint contact)
+        {
+            if (m_eventsubscription > 0)
+            {
+                CollisionEventsThisFrame.addCollider(CollidedWith, contact);
+            }
+        }
+
+        public void SendCollisions()
+        {
+            if (m_eventsubscription > m_requestedUpdateFrequency)
+            {
+                if (CollisionEventsThisFrame != null)
+                {
+                    base.SendCollisionUpdate(CollisionEventsThisFrame);
+                }
+                CollisionEventsThisFrame = new CollisionEventUpdate();
+                m_eventsubscription = 0;
+            }
+        }
+        public override bool SubscribedEvents()
+        {
+            if (m_eventsubscription > 0)
+                return true;
+            return false;
+        }
+
+        public void ProcessTaints(float timestep)
+        {
+
+            if (m_tainted_isPhysical != m_isPhysical)
+            {
+                if (m_tainted_isPhysical)
+                {
+                    // Create avatar capsule and related ODE data
+                    if (!(Shell == IntPtr.Zero && Body == IntPtr.Zero && Amotor == IntPtr.Zero))
+                    {
+                        m_log.Warn("[PHYSICS]: re-creating the following avatar ODE data, even though it already exists - "
+                            + (Shell!=IntPtr.Zero ? "Shell ":"")
+                            + (Body!=IntPtr.Zero ? "Body ":"")
+                            + (Amotor!=IntPtr.Zero ? "Amotor ":""));
+                    }
+                    AvatarGeomAndBodyCreation(_position.X, _position.Y, _position.Z, m_tensor);
+                    
+                    _parent_scene.geom_name_map[Shell] = m_name;
+                    _parent_scene.actor_name_map[Shell] = (PhysicsActor)this;
+                    _parent_scene.AddCharacter(this);
+                }
+                else
+                {
+                    _parent_scene.RemoveCharacter(this);
+                    // destroy avatar capsule and related ODE data
+                    if (Amotor != IntPtr.Zero)
+                    {
+                        // Kill the Amotor
+                        d.JointDestroy(Amotor);
+                        Amotor = IntPtr.Zero;
+                    }
+                    //kill the Geometry
+                    _parent_scene.waitForSpaceUnlock(_parent_scene.space);
+
+                    if (Body != IntPtr.Zero)
+                    {
+                        //kill the body
+                        d.BodyDestroy(Body);
+
+                        Body = IntPtr.Zero;
+                    }
+
+                    if (Shell != IntPtr.Zero)
+                    {
+                        d.GeomDestroy(Shell);
+                        _parent_scene.geom_name_map.Remove(Shell);
+                        Shell = IntPtr.Zero;
+                    }
+
+                }
+
+                m_isPhysical = m_tainted_isPhysical;
+            }
+
+            if (m_tainted_CAPSULE_LENGTH != CAPSULE_LENGTH)
+            {
+                if (Shell != IntPtr.Zero && Body != IntPtr.Zero && Amotor != IntPtr.Zero)
+                {
+
+                    m_pidControllerActive = true;
+                    // no lock needed on _parent_scene.OdeLock because we are called from within the thread lock in OdePlugin's simulate()
+                    d.JointDestroy(Amotor);
+                    float prevCapsule = CAPSULE_LENGTH;
+                    CAPSULE_LENGTH = m_tainted_CAPSULE_LENGTH;
+                    //m_log.Info("[SIZE]: " + CAPSULE_LENGTH.ToString());
+                    d.BodyDestroy(Body);
+                    d.GeomDestroy(Shell);
+                    AvatarGeomAndBodyCreation(_position.X, _position.Y,
+                                      _position.Z + (Math.Abs(CAPSULE_LENGTH - prevCapsule) * 2), m_tensor);
+                    Velocity = Vector3.Zero;
+
+                    _parent_scene.geom_name_map[Shell] = m_name;
+                    _parent_scene.actor_name_map[Shell] = (PhysicsActor)this;
+                }
+                else
+                {
+                    m_log.Warn("[PHYSICS]: trying to change capsule size, but the following ODE data is missing - " 
+                        + (Shell==IntPtr.Zero ? "Shell ":"")
+                        + (Body==IntPtr.Zero ? "Body ":"")
+                        + (Amotor==IntPtr.Zero ? "Amotor ":""));
+                }
+            }
+
+            if (!m_taintPosition.ApproxEquals(_position, 0.05f))
+            {
+                if (Body != IntPtr.Zero)
+                {
+                    d.BodySetPosition(Body, m_taintPosition.X, m_taintPosition.Y, m_taintPosition.Z);
+
+                    _position.X = m_taintPosition.X;
+                    _position.Y = m_taintPosition.Y;
+                    _position.Z = m_taintPosition.Z;
+                }
+            }
+
+        }
+
+        internal void AddCollisionFrameTime(int p)
+        {
+            // protect it from overflow crashing
+            if (m_eventsubscription + p >= int.MaxValue)
+                m_eventsubscription = 0;
+            m_eventsubscription += p;
+        }
+    }
+}
diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
new file mode 100644
index 0000000..5e6696e
--- /dev/null
+++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
@@ -0,0 +1,3750 @@
+/* Copyright (c) Contributors, http://opensimulator.org/
+ * See CONTRIBUTORS.TXT for a full list of copyright holders.
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the OpenSimulator Project nor the
+ *       names of its contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Revised March 5th 2010 by Kitto Flora. ODEDynamics.cs 
+ * rolled into ODEPrim.cs
+ */
+
+using System;
+using System.Collections.Generic;
+using System.Reflection;
+using System.Runtime.InteropServices;
+using System.Threading;
+using log4net;
+using OpenMetaverse;
+using Ode.NET;
+using OpenSim.Framework;
+using OpenSim.Region.Physics.Manager;
+
+
+namespace OpenSim.Region.Physics.OdePlugin
+{
+    /// <summary>
+    /// Various properties that ODE uses for AMotors but isn't exposed in ODE.NET so we must define them ourselves.
+    /// </summary>
+
+    public class OdePrim : PhysicsActor
+    {
+        private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
+
+        private Vector3 _position;
+        private Vector3 _velocity;
+        private Vector3 _torque;
+        private Vector3 m_lastVelocity;
+        private Vector3 m_lastposition;
+        private Quaternion m_lastorientation = new Quaternion();
+        private Vector3 m_rotationalVelocity;
+        private Vector3 _size;
+        private Vector3 _acceleration;
+        // private d.Vector3 _zeroPosition = new d.Vector3(0.0f, 0.0f, 0.0f);
+        private Quaternion _orientation;
+        private Vector3 m_taintposition;
+        private Vector3 m_taintsize;
+        private Vector3 m_taintVelocity;
+        private Vector3 m_taintTorque;
+        private Quaternion m_taintrot;
+        private Vector3 m_angularEnable = Vector3.One;                          // Current setting
+        private Vector3 m_taintAngularLock = Vector3.One;                       // Request from LSL
+        
+        
+        private IntPtr Amotor = IntPtr.Zero;
+
+        private Vector3 m_PIDTarget;
+        private float m_PIDTau;
+        private float PID_D = 35f;
+        private float PID_G = 25f;
+        private bool m_usePID = false;
+        
+        private Quaternion m_APIDTarget = new Quaternion();
+        private float m_APIDStrength = 0.5f;
+        private float m_APIDDamping = 0.5f;
+        private bool m_useAPID = false;
+
+        // These next 7 params apply to llSetHoverHeight(float height, integer water, float tau),
+        // do not confuse with VEHICLE HOVER
+
+        private float m_PIDHoverHeight;
+        private float m_PIDHoverTau;
+        private bool m_useHoverPID;
+        private PIDHoverType m_PIDHoverType = PIDHoverType.Ground;
+        private float m_targetHoverHeight;
+        private float m_groundHeight;
+        private float m_waterHeight;
+        private float m_buoyancy;                               //m_buoyancy set by llSetBuoyancy() 
+
+        // private float m_tensor = 5f;
+        private int body_autodisable_frames = 20;
+
+
+        private const CollisionCategories m_default_collisionFlags = (CollisionCategories.Geom
+                                                        | CollisionCategories.Space
+                                                        | CollisionCategories.Body
+                                                        | CollisionCategories.Character
+                                                        );
+        private bool m_taintshape;
+        private bool m_taintPhysics;
+        private bool m_collidesLand = true;
+        private bool m_collidesWater;
+        public bool m_returnCollisions;
+
+        // Default we're a Geometry
+        private CollisionCategories m_collisionCategories = (CollisionCategories.Geom);
+
+        // Default, Collide with Other Geometries, spaces and Bodies
+        private CollisionCategories m_collisionFlags = m_default_collisionFlags;
+
+        public bool m_taintremove;
+        public bool m_taintdisable;
+        public bool m_disabled;
+        public bool m_taintadd;
+        public bool m_taintselected;
+        public bool m_taintCollidesWater;
+
+        public uint m_localID;
+
+        //public GCHandle gc;
+        private CollisionLocker ode;
+
+        private bool m_taintforce = false;
+        private bool m_taintaddangularforce = false;
+        private Vector3 m_force;
+        private List<Vector3> m_forcelist = new List<Vector3>();
+        private List<Vector3> m_angularforcelist = new List<Vector3>();
+
+        private IMesh _mesh;
+        private PrimitiveBaseShape _pbs;
+        private OdeScene _parent_scene;
+        public IntPtr m_targetSpace = IntPtr.Zero;
+        public IntPtr prim_geom;
+        public IntPtr prev_geom;
+        public IntPtr _triMeshData;
+
+        private IntPtr _linkJointGroup = IntPtr.Zero;
+        private PhysicsActor _parent;
+        private PhysicsActor m_taintparent;
+
+        private List<OdePrim> childrenPrim = new List<OdePrim>();
+
+        private bool iscolliding;
+        private bool m_isphysical;
+        private bool m_isSelected;
+
+        internal bool m_isVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively
+
+        private bool m_throttleUpdates;
+        private int throttleCounter;
+        public int m_interpenetrationcount;
+        public float m_collisionscore;
+        public int m_roundsUnderMotionThreshold;
+        private int m_crossingfailures;
+
+        public bool outofBounds;
+        private float m_density = 10.000006836f; // Aluminum g/cm3;
+
+        public bool _zeroFlag;                                   // if body has been stopped
+        private bool m_lastUpdateSent;
+
+        public IntPtr Body = IntPtr.Zero;
+        public String m_primName;
+        private Vector3 _target_velocity;
+        public d.Mass pMass;
+
+        public int m_eventsubscription;
+        private CollisionEventUpdate CollisionEventsThisFrame;
+
+        private IntPtr m_linkJoint = IntPtr.Zero;
+
+        public volatile bool childPrim;
+
+        internal int m_material = (int)Material.Wood;
+        
+        private int frcount = 0;                                                                                // Used to limit dynamics debug output to 
+
+        private IntPtr m_body = IntPtr.Zero;
+        
+        // Vehicle properties ============================================================================================
+        private Vehicle m_type = Vehicle.TYPE_NONE;                                             // If a 'VEHICLE', and what kind
+        // private Quaternion m_referenceFrame = Quaternion.Identity;   // Axis modifier
+        private VehicleFlag m_flags = (VehicleFlag) 0;                                  // Bit settings:
+                                                                                                                                                // HOVER_TERRAIN_ONLY
+                                                                                                                                                // HOVER_GLOBAL_HEIGHT
+                                                                                                                                                // NO_DEFLECTION_UP
+                                                                                                                                                // HOVER_WATER_ONLY
+                                                                                                                                                // HOVER_UP_ONLY
+                                                                                                                                                // LIMIT_MOTOR_UP
+                                                                                                                                                // LIMIT_ROLL_ONLY
+        
+        // Linear properties
+        private Vector3 m_linearMotorDirection = Vector3.Zero;                  // (was m_linearMotorDirectionLASTSET) the (local) Velocity 
+                                                                                                                                                                //requested by LSL
+        private float   m_linearMotorTimescale = 0;                                             // Motor Attack rate set by LSL
+        private float   m_linearMotorDecayTimescale = 0;                                // Motor Decay rate set by LSL
+        private Vector3 m_linearFrictionTimescale = Vector3.Zero;               // General Friction set by LSL
+        
+                private Vector3 m_lLinMotorDVel = Vector3.Zero;                                 // decayed motor
+                private Vector3 m_lLinObjectVel = Vector3.Zero;                                 // local frame object velocity
+                private Vector3 m_wLinObjectVel = Vector3.Zero;                                 // world frame object velocity
+        
+        //Angular properties
+        private Vector3 m_angularMotorDirection = Vector3.Zero;                 // angular velocity requested by LSL motor 
+        
+        private float m_angularMotorTimescale = 0;                                              // motor angular Attack rate set by LSL
+        private float m_angularMotorDecayTimescale = 0;                                 // motor angular Decay rate set by LSL
+        private Vector3 m_angularFrictionTimescale = Vector3.Zero;              // body angular Friction set by LSL
+
+        private Vector3 m_angularMotorDVel = Vector3.Zero;                              // decayed angular motor
+//        private Vector3 m_angObjectVel = Vector3.Zero;                                        // current body angular velocity
+        private Vector3 m_lastAngularVelocity = Vector3.Zero;                   // what was last applied to body
+
+                //Deflection properties        
+        // private float m_angularDeflectionEfficiency = 0;
+        // private float m_angularDeflectionTimescale = 0;
+        // private float m_linearDeflectionEfficiency = 0;
+        // private float m_linearDeflectionTimescale = 0;
+        
+        //Banking properties
+        // private float m_bankingEfficiency = 0;
+        // private float m_bankingMix = 0;
+        // private float m_bankingTimescale = 0;
+        
+        //Hover and Buoyancy properties
+        private float m_VhoverHeight = 0f;
+//        private float m_VhoverEfficiency = 0f;
+        private float m_VhoverTimescale = 0f;
+        private float m_VhoverTargetHeight = -1.0f;     // if <0 then no hover, else its the current target height 
+        private float m_VehicleBuoyancy = 0f;                   // Set by VEHICLE_BUOYANCY, for a vehicle.
+                                // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity) 
+                                // KF: So far I have found no good method to combine a script-requested .Z velocity and gravity.
+                                // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity. 
+                                                                                                        
+                //Attractor properties                                                                                                  
+        private float m_verticalAttractionEfficiency = 1.0f;            // damped
+        private float m_verticalAttractionTimescale = 500f;                     // Timescale > 300  means no vert attractor.
+        
+        
+
+
+
+
+        public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size,
+                       Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical, CollisionLocker dode)
+        {
+            ode = dode;
+            if (!pos.IsFinite())
+            {
+                pos = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f),
+                    parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f);
+                m_log.Warn("[PHYSICS]: Got nonFinite Object create Position");
+            }
+            
+            _position = pos;
+            m_taintposition = pos;
+            PID_D = parent_scene.bodyPIDD;
+            PID_G = parent_scene.bodyPIDG;
+            m_density = parent_scene.geomDefaultDensity;
+            // m_tensor = parent_scene.bodyMotorJointMaxforceTensor;
+            body_autodisable_frames = parent_scene.bodyFramesAutoDisable;
+
+
+            prim_geom = IntPtr.Zero;
+            prev_geom = IntPtr.Zero;
+
+            if (!pos.IsFinite())
+            {
+                size = new Vector3(0.5f, 0.5f, 0.5f);
+                m_log.Warn("[PHYSICS]: Got nonFinite Object create Size");
+            }
+
+            if (size.X <= 0) size.X = 0.01f;
+            if (size.Y <= 0) size.Y = 0.01f;
+            if (size.Z <= 0) size.Z = 0.01f;
+
+            _size = size;
+            m_taintsize = _size;
+
+            if (!QuaternionIsFinite(rotation))
+            {
+                rotation = Quaternion.Identity;
+                m_log.Warn("[PHYSICS]: Got nonFinite Object create Rotation");
+            }
+
+            _orientation = rotation;
+            m_taintrot = _orientation;
+            _mesh = mesh;
+            _pbs = pbs;
+
+            _parent_scene = parent_scene;
+            m_targetSpace = (IntPtr)0;
+
+//            if (pos.Z < 0)
+            if (pos.Z < parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y))
+                m_isphysical = false;
+            else
+            {
+                m_isphysical = pisPhysical;
+                // If we're physical, we need to be in the master space for now.
+                // linksets *should* be in a space together..  but are not currently
+                if (m_isphysical)
+                    m_targetSpace = _parent_scene.space;
+            }
+            m_primName = primName;
+            m_taintadd = true;
+            _parent_scene.AddPhysicsActorTaint(this);
+            //  don't do .add() here; old geoms get recycled with the same hash
+        }
+
+        public override int PhysicsActorType
+        {
+            get { return (int) ActorTypes.Prim; }
+            set { return; }
+        }
+
+        public override bool SetAlwaysRun
+        {
+            get { return false; }
+            set { return; }
+        }
+
+        public override uint LocalID
+        {
+            set {
+                //m_log.Info("[PHYSICS]: Setting TrackerID: " + value);
+                m_localID = value; }
+        }
+
+        public override bool Grabbed
+        {
+            set { return; }
+        }
+
+        public override bool Selected
+        {
+            set {
+        
+//Console.WriteLine("Sel {0}  {1}  {2}", m_primName, value,   m_isphysical);        
+                // This only makes the object not collidable if the object
+                // is physical or the object is modified somehow *IN THE FUTURE*
+                // without this, if an avatar selects prim, they can walk right
+                // through it while it's selected
+                m_collisionscore = 0;
+                if ((m_isphysical && !_zeroFlag) || !value)
+                {
+                    m_taintselected = value;
+                    _parent_scene.AddPhysicsActorTaint(this);
+                }
+                else
+                {
+                    m_taintselected = value;
+                    m_isSelected = value;
+                }
+                if(m_isSelected) disableBodySoft();         
+            }
+        }
+
+        public override bool IsPhysical
+        {
+            get { return m_isphysical; }
+            set 
+            { 
+                  m_isphysical = value;
+                  if (!m_isphysical)
+                  {             // Zero the remembered last velocity
+                                m_lastVelocity = Vector3.Zero;
+                            if (m_type != Vehicle.TYPE_NONE) Halt();
+                  }
+            }
+        }
+
+        public void setPrimForRemoval()
+        {
+            m_taintremove = true;
+        }
+
+        public override bool Flying
+        {
+            // no flying prims for you
+            get { return false; }
+            set { }
+        }
+
+        public override bool IsColliding
+        {
+            get { return iscolliding; }
+            set { iscolliding = value; }
+        }
+
+        public override bool CollidingGround
+        {
+            get { return false; }
+            set { return; }
+        }
+
+        public override bool CollidingObj
+        {
+            get { return false; }
+            set { return; }
+        }
+
+        public override bool ThrottleUpdates
+        {
+            get { return m_throttleUpdates; }
+            set { m_throttleUpdates = value; }
+        }
+
+        public override bool Stopped
+        {
+            get { return _zeroFlag; }
+        }
+
+        public override Vector3 Position
+        {
+            get { return _position; }
+
+            set { _position = value;
+                //m_log.Info("[PHYSICS]: " + _position.ToString());
+            }
+        }
+
+        public override Vector3 Size
+        {
+            get { return _size; }
+            set
+            {
+                if (value.IsFinite())
+                {
+                    _size = value;
+                }
+                else
+                {
+                    m_log.Warn("[PHYSICS]: Got NaN Size on object");
+                }
+            }
+        }
+
+        public override float Mass
+        {
+            get { return CalculateMass(); }
+        }
+
+        public override Vector3 Force
+        {
+            //get { return Vector3.Zero; }
+            get { return m_force; }
+            set
+            {
+                if (value.IsFinite())
+                {
+                    m_force = value;
+                }
+                else
+                {
+                    m_log.Warn("[PHYSICS]: NaN in Force Applied to an Object");
+                }
+            }
+        }
+
+        public override int VehicleType
+        {
+            get { return (int)m_type; }
+            set { ProcessTypeChange((Vehicle)value); }
+        }
+
+        public override void VehicleFloatParam(int param, float value)
+        {
+            ProcessFloatVehicleParam((Vehicle) param, value);
+        }
+
+        public override void VehicleVectorParam(int param, Vector3 value)
+        {
+            ProcessVectorVehicleParam((Vehicle) param, value);
+        }
+
+        public override void VehicleRotationParam(int param, Quaternion rotation)
+        {
+            ProcessRotationVehicleParam((Vehicle) param, rotation);
+        }
+
+        public override void VehicleFlags(int param, bool remove)
+        {
+            ProcessVehicleFlags(param, remove);
+        }
+        
+        public override void SetVolumeDetect(int param)
+        {
+            lock (_parent_scene.OdeLock)
+            {
+                m_isVolumeDetect = (param!=0);
+            }
+        }
+
+        public override Vector3 CenterOfMass
+        {
+            get { return Vector3.Zero; }
+        }
+
+        public override Vector3 GeometricCenter
+        {
+            get { return Vector3.Zero; }
+        }
+
+        public override PrimitiveBaseShape Shape
+        {
+            set
+            {
+                _pbs = value;
+                m_taintshape = true;
+            }
+        }
+
+        public override Vector3 Velocity
+        {
+            get
+            {
+                // Averate previous velocity with the new one so
+                // client object interpolation works a 'little' better
+                if (_zeroFlag)
+                    return Vector3.Zero;
+
+                Vector3 returnVelocity = Vector3.Zero;
+                returnVelocity.X = (m_lastVelocity.X + _velocity.X)/2;
+                returnVelocity.Y = (m_lastVelocity.Y + _velocity.Y)/2;
+                returnVelocity.Z = (m_lastVelocity.Z + _velocity.Z)/2;
+                return returnVelocity;
+            }
+            set
+            {
+                if (value.IsFinite())
+                {
+                    _velocity = value;
+
+                    m_taintVelocity = value;
+                    _parent_scene.AddPhysicsActorTaint(this);
+                }
+                else
+                {
+                    m_log.Warn("[PHYSICS]: Got NaN Velocity in Object");
+                }
+
+            }
+        }
+
+        public override Vector3 Torque
+        {
+            get
+            {
+                if (!m_isphysical || Body == IntPtr.Zero)
+                    return Vector3.Zero;
+
+                return _torque;
+            }
+
+            set
+            {
+                if (value.IsFinite())
+                {
+                    m_taintTorque = value;
+                    _parent_scene.AddPhysicsActorTaint(this);
+                }
+                else
+                {
+                    m_log.Warn("[PHYSICS]: Got NaN Torque in Object");
+                }
+            }
+        }
+
+        public override float CollisionScore
+        {
+            get { return m_collisionscore; }
+            set { m_collisionscore = value; }
+        }
+
+        public override bool Kinematic
+        {
+            get { return false; }
+            set { }
+        }
+
+        public override Quaternion Orientation
+        {
+            get { return _orientation; }
+            set
+            {
+                if (QuaternionIsFinite(value))
+                {
+                    _orientation = value;
+                                }
+                else
+                    m_log.Warn("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object");
+
+            }
+        }
+        
+        
+                public override bool FloatOnWater
+        {
+            set {
+                m_taintCollidesWater = value;
+                _parent_scene.AddPhysicsActorTaint(this);
+            }
+        }
+
+        public override void SetMomentum(Vector3 momentum)
+        {
+        }
+
+        public override Vector3 PIDTarget 
+        { 
+            set
+            {
+                if (value.IsFinite())
+                {
+                    m_PIDTarget = value;
+                }
+                else
+                    m_log.Warn("[PHYSICS]: Got NaN PIDTarget from Scene on Object");
+            } 
+        }
+        public override bool PIDActive { set { m_usePID = value; } }
+        public override float PIDTau { set { m_PIDTau = value; } }
+        
+                // For RotLookAt        
+        public  override Quaternion APIDTarget { set { m_APIDTarget = value; } }
+        public  override bool APIDActive { set { m_useAPID = value; } }
+        public  override float APIDStrength { set { m_APIDStrength = value; } }
+        public  override float APIDDamping { set { m_APIDDamping = value; } }
+
+        public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } }
+        public override bool PIDHoverActive { set { m_useHoverPID = value; } }
+        public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } }
+        public override float PIDHoverTau { set { m_PIDHoverTau = value; } }
+
+        internal static bool QuaternionIsFinite(Quaternion q)
+        {
+            if (Single.IsNaN(q.X) || Single.IsInfinity(q.X))
+                return false;
+            if (Single.IsNaN(q.Y) || Single.IsInfinity(q.Y))
+                return false;
+            if (Single.IsNaN(q.Z) || Single.IsInfinity(q.Z))
+                return false;
+            if (Single.IsNaN(q.W) || Single.IsInfinity(q.W))
+                return false;
+            return true;
+        }
+
+        public override Vector3 Acceleration            // client updates read data via here
+        {
+            get { return _acceleration; }
+        }
+
+
+        public void SetAcceleration(Vector3 accel) // No one calls this, and it would not do anything.
+        {
+            _acceleration = accel;
+        }
+
+        public override void AddForce(Vector3 force, bool pushforce)
+        {
+            if (force.IsFinite())
+            {
+                lock (m_forcelist)
+                    m_forcelist.Add(force);
+
+                m_taintforce = true;
+            }
+            else
+            {
+                m_log.Warn("[PHYSICS]: Got Invalid linear force vector from Scene in Object");
+            }
+            //m_log.Info("[PHYSICS]: Added Force:" + force.ToString() +  " to prim at " + Position.ToString());
+        }
+
+        public override void AddAngularForce(Vector3 force, bool pushforce)
+        {
+            if (force.IsFinite())
+            {
+                m_angularforcelist.Add(force);
+                m_taintaddangularforce = true;
+            }
+            else
+            {
+                m_log.Warn("[PHYSICS]: Got Invalid Angular force vector from Scene in Object");
+            }
+        }
+
+        public override Vector3 RotationalVelocity
+        {
+            get
+            {
+                return m_rotationalVelocity;
+            }
+            set
+            {
+                if (value.IsFinite())
+                {
+                    m_rotationalVelocity = value;
+                }
+                else
+                {
+                    m_log.Warn("[PHYSICS]: Got NaN RotationalVelocity in Object");
+                }
+            }
+        }
+
+        public override void CrossingFailure()
+        {
+            m_crossingfailures++;
+            if (m_crossingfailures > _parent_scene.geomCrossingFailuresBeforeOutofbounds)
+            {
+                base.RaiseOutOfBounds(_position);
+                return;
+            }
+            else if (m_crossingfailures == _parent_scene.geomCrossingFailuresBeforeOutofbounds)
+            {
+                m_log.Warn("[PHYSICS]: Too many crossing failures for: " + m_primName);
+            }
+        }
+
+        public override float Buoyancy
+        {
+            get { return m_buoyancy; }
+            set { m_buoyancy = value; }
+        }
+
+        public override void link(PhysicsActor obj)
+        {
+            m_taintparent = obj;
+        }
+
+        public override void delink()
+        {
+            m_taintparent = null;
+        }
+
+        public override void LockAngularMotion(Vector3 axis)
+        {
+            // reverse the zero/non zero values for ODE.
+            if (axis.IsFinite())
+            {
+                axis.X = (axis.X > 0) ? 1f : 0f;
+                axis.Y = (axis.Y > 0) ? 1f : 0f;
+                axis.Z = (axis.Z > 0) ? 1f : 0f;
+                m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z);
+                m_taintAngularLock = axis;
+            }
+            else
+            {
+                m_log.Warn("[PHYSICS]: Got NaN locking axis from Scene on Object");
+            }
+        }
+
+
+        public void SetGeom(IntPtr geom)
+        {
+            prev_geom = prim_geom;
+            prim_geom = geom;
+//Console.WriteLine("SetGeom to " + prim_geom + " for " + m_primName);     
+            if (prim_geom != IntPtr.Zero)
+            {
+                d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
+                d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
+            }
+
+            if (childPrim)
+            {
+                if (_parent != null && _parent is OdePrim)
+                {
+                    OdePrim parent = (OdePrim)_parent;
+//Console.WriteLine("SetGeom calls ChildSetGeom");                    
+                    parent.ChildSetGeom(this);
+                }
+            }
+            //m_log.Warn("Setting Geom to: " + prim_geom);
+        }
+
+        public void enableBodySoft()
+        {
+            if (!childPrim)
+            {
+                if (m_isphysical && Body != IntPtr.Zero)
+                {
+                    d.BodyEnable(Body);
+                    if (m_type != Vehicle.TYPE_NONE)
+                            Enable(Body, _parent_scene);
+                }
+
+                m_disabled = false;
+            }
+        }
+
+        public void disableBodySoft()
+        {
+            m_disabled = true;
+
+            if (m_isphysical && Body != IntPtr.Zero)
+            {
+                d.BodyDisable(Body);
+            }
+        }
+
+        public void enableBody()
+        {
+            // Don't enable this body if we're a child prim
+            // this should be taken care of in the parent function not here
+            if (!childPrim)
+            {
+                // Sets the geom to a body
+                Body = d.BodyCreate(_parent_scene.world);
+
+                setMass();
+                d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
+                d.Quaternion myrot = new d.Quaternion();
+                myrot.X = _orientation.X;
+                myrot.Y = _orientation.Y;
+                myrot.Z = _orientation.Z;
+                myrot.W = _orientation.W;
+                d.BodySetQuaternion(Body, ref myrot);
+                d.GeomSetBody(prim_geom, Body);
+                m_collisionCategories |= CollisionCategories.Body;
+                m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
+
+                d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
+                d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
+
+                d.BodySetAutoDisableFlag(Body, true);
+                d.BodySetAutoDisableSteps(Body, body_autodisable_frames);
+                
+                // disconnect from world gravity so we can apply buoyancy
+                                d.BodySetGravityMode (Body, false);
+
+                m_interpenetrationcount = 0;
+                m_collisionscore = 0;
+                m_disabled = false;
+
+                if (m_type != Vehicle.TYPE_NONE)
+                {
+                    Enable(Body, _parent_scene);
+                }
+
+                _parent_scene.addActivePrim(this);
+            }
+        }
+
+        #region Mass Calculation
+
+        private float CalculateMass()
+        {
+            float volume = 0;
+
+            // No material is passed to the physics engines yet..  soo..
+            // we're using the m_density constant in the class definition
+
+            float returnMass = 0;
+
+            switch (_pbs.ProfileShape)
+            {
+                case ProfileShape.Square:
+                    // Profile Volume
+
+                    volume = _size.X*_size.Y*_size.Z;
+
+                    // If the user has 'hollowed out'
+                    // ProfileHollow is one of those 0 to 50000 values :P
+                    // we like percentages better..   so turning into a percentage
+
+                    if (((float) _pbs.ProfileHollow/50000f) > 0.0)
+                    {
+                        float hollowAmount = (float) _pbs.ProfileHollow/50000f;
+
+                        // calculate the hollow volume by it's shape compared to the prim shape
+                        float hollowVolume = 0;
+                        switch (_pbs.HollowShape)
+                        {
+                            case HollowShape.Square:
+                            case HollowShape.Same:
+                                // Cube Hollow volume calculation
+                                float hollowsizex = _size.X*hollowAmount;
+                                float hollowsizey = _size.Y*hollowAmount;
+                                float hollowsizez = _size.Z*hollowAmount;
+                                hollowVolume = hollowsizex*hollowsizey*hollowsizez;
+                                break;
+
+                            case HollowShape.Circle:
+                                // Hollow shape is a perfect cyllinder in respect to the cube's scale
+                                // Cyllinder hollow volume calculation
+                                float hRadius = _size.X/2;
+                                float hLength = _size.Z;
+
+                                // pi * r2 * h
+                                hollowVolume = ((float) (Math.PI*Math.Pow(hRadius, 2)*hLength)*hollowAmount);
+                                break;
+
+                            case HollowShape.Triangle:
+                                // Equilateral Triangular Prism volume hollow calculation
+                                // Triangle is an Equilateral Triangular Prism with aLength = to _size.Y
+
+                                float aLength = _size.Y;
+                                // 1/2 abh
+                                hollowVolume = (float) ((0.5*aLength*_size.X*_size.Z)*hollowAmount);
+                                break;
+
+                            default:
+                                hollowVolume = 0;
+                                break;
+                        }
+                        volume = volume - hollowVolume;
+                    }
+
+                    break;
+                case ProfileShape.Circle:
+                    if (_pbs.PathCurve == (byte)Extrusion.Straight)
+                    {
+                        // Cylinder
+                        float volume1 = (float)(Math.PI * Math.Pow(_size.X/2, 2) * _size.Z);
+                        float volume2 = (float)(Math.PI * Math.Pow(_size.Y/2, 2) * _size.Z);
+
+                        // Approximating the cylinder's irregularity.
+                        if (volume1 > volume2)
+                        {
+                            volume = (float)volume1 - (volume1 - volume2);
+                        }
+                        else if (volume2 > volume1)
+                        {
+                            volume = (float)volume2 - (volume2 - volume1);
+                        }
+                        else
+                        {
+                            // Regular cylinder
+                            volume = volume1;
+                        }
+                    }
+                    else
+                    {
+                        // We don't know what the shape is yet, so use default
+                        volume = _size.X * _size.Y * _size.Z;
+                    }
+                    // If the user has 'hollowed out'
+                    // ProfileHollow is one of those 0 to 50000 values :P
+                    // we like percentages better..   so turning into a percentage
+
+                    if (((float)_pbs.ProfileHollow / 50000f) > 0.0)
+                    {
+                        float hollowAmount = (float)_pbs.ProfileHollow / 50000f;
+
+                        // calculate the hollow volume by it's shape compared to the prim shape
+                        float hollowVolume = 0;
+                        switch (_pbs.HollowShape)
+                        {
+                            case HollowShape.Same:
+                            case HollowShape.Circle:
+                                // Hollow shape is a perfect cyllinder in respect to the cube's scale
+                                // Cyllinder hollow volume calculation
+                                float hRadius = _size.X / 2;
+                                float hLength = _size.Z;
+
+                                // pi * r2 * h
+                                hollowVolume = ((float)(Math.PI * Math.Pow(hRadius, 2) * hLength) * hollowAmount);
+                                break;
+
+                            case HollowShape.Square:
+                                // Cube Hollow volume calculation
+                                float hollowsizex = _size.X * hollowAmount;
+                                float hollowsizey = _size.Y * hollowAmount;
+                                float hollowsizez = _size.Z * hollowAmount;
+                                hollowVolume = hollowsizex * hollowsizey * hollowsizez;
+                                break;
+
+                            case HollowShape.Triangle:
+                                // Equilateral Triangular Prism volume hollow calculation
+                                // Triangle is an Equilateral Triangular Prism with aLength = to _size.Y
+
+                                float aLength = _size.Y;
+                                // 1/2 abh
+                                hollowVolume = (float)((0.5 * aLength * _size.X * _size.Z) * hollowAmount);
+                                break;
+
+                            default:
+                                hollowVolume = 0;
+                                break;
+                        }
+                        volume = volume - hollowVolume;
+                    }
+                    break;
+
+                case ProfileShape.HalfCircle:
+                    if (_pbs.PathCurve == (byte)Extrusion.Curve1)
+                    {
+                        if (_size.X == _size.Y && _size.Y == _size.Z)
+                        {
+                            // regular sphere
+                            // v = 4/3 * pi * r^3
+                            float sradius3 = (float)Math.Pow((_size.X / 2), 3);
+                            volume = (float)((4f / 3f) * Math.PI * sradius3);
+                        }
+                        else
+                        {
+                            // we treat this as a box currently
+                            volume = _size.X * _size.Y * _size.Z;
+                        }
+                    }
+                    else
+                    {
+                        // We don't know what the shape is yet, so use default
+                        volume = _size.X * _size.Y * _size.Z;
+                    }
+                    break;
+
+                case ProfileShape.EquilateralTriangle:
+                    /*
+                        v = (abs((xB*yA-xA*yB)+(xC*yB-xB*yC)+(xA*yC-xC*yA))/2) * h
+
+                        // seed mesh
+                        Vertex MM = new Vertex(-0.25f, -0.45f, 0.0f);
+                        Vertex PM = new Vertex(+0.5f, 0f, 0.0f);
+                        Vertex PP = new Vertex(-0.25f, +0.45f, 0.0f);
+                     */
+                    float xA = -0.25f * _size.X;
+                    float yA = -0.45f * _size.Y;
+
+                    float xB = 0.5f * _size.X;
+                    float yB = 0;
+
+                    float xC = -0.25f * _size.X;
+                    float yC = 0.45f * _size.Y;
+
+                    volume = (float)((Math.Abs((xB * yA - xA * yB) + (xC * yB - xB * yC) + (xA * yC - xC * yA)) / 2) * _size.Z);
+
+                    // If the user has 'hollowed out'
+                    // ProfileHollow is one of those 0 to 50000 values :P
+                    // we like percentages better..   so turning into a percentage
+                    float fhollowFactor = ((float)_pbs.ProfileHollow / 1.9f);
+                    if (((float)fhollowFactor / 50000f) > 0.0)
+                    {
+                        float hollowAmount = (float)fhollowFactor / 50000f;
+
+                        // calculate the hollow volume by it's shape compared to the prim shape
+                        float hollowVolume = 0;
+                        switch (_pbs.HollowShape)
+                        {
+                            case HollowShape.Same:
+                            case HollowShape.Triangle:
+                                // Equilateral Triangular Prism volume hollow calculation
+                                // Triangle is an Equilateral Triangular Prism with aLength = to _size.Y
+
+                                float aLength = _size.Y;
+                                // 1/2 abh
+                                hollowVolume = (float)((0.5 * aLength * _size.X * _size.Z) * hollowAmount);
+                                break;
+
+                            case HollowShape.Square:
+                                // Cube Hollow volume calculation
+                                float hollowsizex = _size.X * hollowAmount;
+                                float hollowsizey = _size.Y * hollowAmount;
+                                float hollowsizez = _size.Z * hollowAmount;
+                                hollowVolume = hollowsizex * hollowsizey * hollowsizez;
+                                break;
+
+                            case HollowShape.Circle:
+                                // Hollow shape is a perfect cyllinder in respect to the cube's scale
+                                // Cyllinder hollow volume calculation
+                                float hRadius = _size.X / 2;
+                                float hLength = _size.Z;
+
+                                // pi * r2 * h
+                                hollowVolume = ((float)((Math.PI * Math.Pow(hRadius, 2) * hLength)/2) * hollowAmount);
+                                break;
+
+                            default:
+                                hollowVolume = 0;
+                                break;
+                        }
+                        volume = volume - hollowVolume;
+                    }
+                    break;
+
+                default:
+                    // we don't have all of the volume formulas yet so
+                    // use the common volume formula for all
+                    volume = _size.X*_size.Y*_size.Z;
+                    break;
+            }
+
+            // Calculate Path cut effect on volume
+            // Not exact, in the triangle hollow example
+            // They should never be zero or less then zero..
+            // we'll ignore it if it's less then zero
+
+            // ProfileEnd and ProfileBegin are values
+            // from 0 to 50000
+
+            // Turning them back into percentages so that I can cut that percentage off the volume
+
+            float PathCutEndAmount = _pbs.ProfileEnd;
+            float PathCutStartAmount = _pbs.ProfileBegin;
+            if (((PathCutStartAmount + PathCutEndAmount)/50000f) > 0.0f)
+            {
+                float pathCutAmount = ((PathCutStartAmount + PathCutEndAmount)/50000f);
+
+                // Check the return amount for sanity
+                if (pathCutAmount >= 0.99f)
+                    pathCutAmount = 0.99f;
+
+                volume = volume - (volume*pathCutAmount);
+            }
+            UInt16 taperX = _pbs.PathScaleX;
+            UInt16 taperY = _pbs.PathScaleY;
+            float taperFactorX = 0;
+            float taperFactorY = 0;
+
+            // Mass = density * volume
+            if (taperX != 100)
+            {
+                if (taperX > 100)
+                {
+                    taperFactorX = 1.0f - ((float)taperX / 200);
+                    //m_log.Warn("taperTopFactorX: " + extr.taperTopFactorX.ToString());
+                }
+                else
+                {
+                    taperFactorX = 1.0f - ((100 - (float)taperX) / 100);
+                    //m_log.Warn("taperBotFactorX: " + extr.taperBotFactorX.ToString());
+                }
+                volume = (float)volume * ((taperFactorX / 3f) + 0.001f);
+            }
+
+            if (taperY != 100)
+            {
+                if (taperY > 100)
+                {
+                    taperFactorY = 1.0f - ((float)taperY / 200);
+                    //m_log.Warn("taperTopFactorY: " + extr.taperTopFactorY.ToString());
+                }
+                else
+                {
+                    taperFactorY = 1.0f - ((100 - (float)taperY) / 100);
+                    //m_log.Warn("taperBotFactorY: " + extr.taperBotFactorY.ToString());
+                }
+                volume = (float)volume * ((taperFactorY / 3f) + 0.001f);
+            }
+            returnMass = m_density*volume;
+            if (returnMass <= 0) returnMass = 0.0001f;//ckrinke: Mass must be greater then zero.
+
+
+
+            // Recursively calculate mass
+            bool HasChildPrim = false;
+            lock (childrenPrim)
+            {
+                if (childrenPrim.Count > 0)
+                {
+                    HasChildPrim = true;
+                }
+
+            }
+            if (HasChildPrim)
+            {
+                OdePrim[] childPrimArr = new OdePrim[0];
+
+                lock (childrenPrim)
+                    childPrimArr = childrenPrim.ToArray();
+
+                for (int i = 0; i < childPrimArr.Length; i++)
+                {
+                    if (childPrimArr[i] != null && !childPrimArr[i].m_taintremove)
+                        returnMass += childPrimArr[i].CalculateMass();
+                    // failsafe, this shouldn't happen but with OpenSim, you never know :)
+                    if (i > 256)
+                        break;
+                }
+            }
+            if (returnMass > _parent_scene.maximumMassObject)
+                returnMass = _parent_scene.maximumMassObject;
+            return returnMass;
+        }// end CalculateMass
+
+        #endregion
+
+        public void setMass()
+        {
+            if (Body != (IntPtr) 0)
+            {
+                float newmass = CalculateMass();
+
+                //m_log.Info("[PHYSICS]: New Mass: " + newmass.ToString());
+
+                d.MassSetBoxTotal(out pMass, newmass, _size.X, _size.Y, _size.Z);
+                d.BodySetMass(Body, ref pMass);
+            }
+        }
+
+        public void disableBody()
+        {
+            //this kills the body so things like 'mesh' can re-create it.
+            lock (this)
+            {
+                if (!childPrim)
+                {
+                    if (Body != IntPtr.Zero)
+                    {
+                        _parent_scene.remActivePrim(this);
+                        m_collisionCategories &= ~CollisionCategories.Body;
+                        m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land);
+
+                        if (prim_geom != IntPtr.Zero)
+                        {
+                            d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
+                            d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
+                        }
+
+                        
+                        d.BodyDestroy(Body);
+                        lock (childrenPrim)
+                        {
+                            if (childrenPrim.Count > 0)
+                            {
+                                foreach (OdePrim prm in childrenPrim)
+                                {
+                                    _parent_scene.remActivePrim(prm);
+                                    prm.Body = IntPtr.Zero;
+                                }
+                            }
+                        }
+                        Body = IntPtr.Zero;
+                    }
+                }
+                else
+                {
+                    _parent_scene.remActivePrim(this);
+                    
+                    m_collisionCategories &= ~CollisionCategories.Body;
+                    m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land);
+
+                    if (prim_geom != IntPtr.Zero)
+                    {
+                        d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
+                        d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
+                    }
+
+                    
+                    Body = IntPtr.Zero;
+                }
+            }
+            m_disabled = true;
+            m_collisionscore = 0;
+        }
+
+        private static Dictionary<IMesh, IntPtr> m_MeshToTriMeshMap = new Dictionary<IMesh, IntPtr>();
+
+        public void setMesh(OdeScene parent_scene, IMesh mesh)
+        {
+            // This sleeper is there to moderate how long it takes between
+            // setting up the mesh and pre-processing it when we get rapid fire mesh requests on a single object
+
+            //Thread.Sleep(10);
+
+            //Kill Body so that mesh can re-make the geom
+            if (IsPhysical && Body != IntPtr.Zero)
+            {
+                if (childPrim)
+                {
+                    if (_parent != null)
+                    {
+                        OdePrim parent = (OdePrim)_parent;
+                        parent.ChildDelink(this);
+                    }
+                }
+                else
+                {
+                    disableBody();
+                }
+            }
+
+            IntPtr vertices, indices;
+            int vertexCount, indexCount;
+            int vertexStride, triStride;
+            mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); // Note, that vertices are fixed in unmanaged heap
+            mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); // Also fixed, needs release after usage
+
+            mesh.releaseSourceMeshData(); // free up the original mesh data to save memory
+            if (m_MeshToTriMeshMap.ContainsKey(mesh))
+            {
+                _triMeshData = m_MeshToTriMeshMap[mesh];
+            }
+            else
+            {
+                _triMeshData = d.GeomTriMeshDataCreate();
+
+                d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride);
+                d.GeomTriMeshDataPreprocess(_triMeshData);
+                m_MeshToTriMeshMap[mesh] = _triMeshData;
+            }
+
+            _parent_scene.waitForSpaceUnlock(m_targetSpace);
+            try
+            {
+                if (prim_geom == IntPtr.Zero)
+                {
+                    SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, parent_scene.triCallback, null, null));
+                }
+            }
+            catch (AccessViolationException)
+            {
+                m_log.Error("[PHYSICS]: MESH LOCKED");
+                return;
+            }
+
+
+           // if (IsPhysical && Body == (IntPtr) 0)
+           // {
+                // Recreate the body
+          //     m_interpenetrationcount = 0;
+           //     m_collisionscore = 0;
+
+           //     enableBody();
+           // }
+        }
+
+        public void ProcessTaints(float timestep) //=============================================================================
+        {
+            if (m_taintadd)
+            {
+                changeadd(timestep);
+            }
+            
+            if (prim_geom != IntPtr.Zero)
+            {
+                         if (!_position.ApproxEquals(m_taintposition, 0f))
+                            changemove(timestep);
+
+                     if (m_taintrot != _orientation)
+                     {
+                        if(childPrim && IsPhysical)     // For physical child prim...
+                        {
+                            rotate(timestep);       
+                            // KF: ODE will also rotate the parent prim!
+                            // so rotate the root back to where it was
+                            OdePrim parent = (OdePrim)_parent;
+                            parent.rotate(timestep);
+                    }
+                    else
+                    {
+                            //Just rotate the prim
+                            rotate(timestep);
+                    }  
+                }
+                //
+            
+                if (m_taintPhysics != m_isphysical && !(m_taintparent != _parent))
+                    changePhysicsStatus(timestep);
+                //
+
+                if (!_size.ApproxEquals(m_taintsize,0f))
+                    changesize(timestep);
+                //
+
+                if (m_taintshape)
+                    changeshape(timestep);
+                //
+
+                if (m_taintforce)
+                    changeAddForce(timestep);
+
+                if (m_taintaddangularforce)
+                    changeAddAngularForce(timestep);
+
+                if (!m_taintTorque.ApproxEquals(Vector3.Zero, 0.001f))
+                    changeSetTorque(timestep);
+
+                if (m_taintdisable)
+                    changedisable(timestep);
+
+                if (m_taintselected != m_isSelected)
+                    changeSelectedStatus(timestep);
+
+                if (!m_taintVelocity.ApproxEquals(Vector3.Zero, 0.001f))
+                    changevelocity(timestep);
+
+                if (m_taintparent != _parent)
+                    changelink(timestep);
+
+                if (m_taintCollidesWater != m_collidesWater)
+                    changefloatonwater(timestep);
+                    
+                if (!m_angularEnable.ApproxEquals(m_taintAngularLock,0f))
+                    changeAngularLock(timestep);
+ 
+            }
+            else
+            {
+                m_log.Error("[PHYSICS]: The scene reused a disposed PhysActor! *waves finger*, Don't be evil.  A couple of things can cause this.   An improper prim breakdown(be sure to set prim_geom to zero after d.GeomDestroy!   An improper buildup (creating the geom failed).   Or, the Scene Reused a physics actor after disposing it.)");
+            }
+        }
+
+
+        private void changeAngularLock(float timestep) 
+        {
+            if (_parent == null)
+            {
+                m_angularEnable = m_taintAngularLock;
+            }
+        }
+
+        private void changelink(float timestep)
+        {
+            // If the newly set parent is not null
+            // create link
+            if (_parent == null && m_taintparent != null)
+            {
+                if (m_taintparent.PhysicsActorType == (int)ActorTypes.Prim)
+                {
+                    OdePrim obj = (OdePrim)m_taintparent;
+                    //obj.disableBody();
+                    obj.ParentPrim(this);
+
+                    /*
+                    if (obj.Body != (IntPtr)0 && Body != (IntPtr)0 && obj.Body != Body)
+                    {
+                        _linkJointGroup = d.JointGroupCreate(0);
+                        m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup);
+                        d.JointAttach(m_linkJoint, obj.Body, Body);
+                        d.JointSetFixed(m_linkJoint);
+                    }
+                     */
+                }
+            }
+            // If the newly set parent is null
+            // destroy link
+            else if (_parent != null && m_taintparent == null)
+            {
+                if (_parent is OdePrim)
+                {
+                    OdePrim obj = (OdePrim)_parent;
+                    obj.ChildDelink(this);
+                    childPrim = false;
+                    //_parent = null;
+                }
+                
+                /*
+                    if (Body != (IntPtr)0 && _linkJointGroup != (IntPtr)0)
+                    d.JointGroupDestroy(_linkJointGroup);
+                        
+                    _linkJointGroup = (IntPtr)0;
+                    m_linkJoint = (IntPtr)0;
+                */
+            }
+ 
+            _parent = m_taintparent;
+            m_taintPhysics = m_isphysical;
+        }
+
+        // I'm the parent
+        // prim is the child
+        public void ParentPrim(OdePrim prim)
+        {
+            if (this.m_localID != prim.m_localID)
+            {
+                if (Body == IntPtr.Zero)
+                {
+                    Body = d.BodyCreate(_parent_scene.world);
+                    setMass();
+                }
+                if (Body != IntPtr.Zero)
+                {
+                    lock (childrenPrim)
+                    {
+                        if (!childrenPrim.Contains(prim))
+                        {
+                            childrenPrim.Add(prim);
+                            
+                            foreach (OdePrim prm in childrenPrim)
+                            {
+                                d.Mass m2;
+                                d.MassSetZero(out m2);
+                                d.MassSetBoxTotal(out m2, prim.CalculateMass(), prm._size.X, prm._size.Y, prm._size.Z);
+
+
+                                d.Quaternion quat = new d.Quaternion();
+                                quat.W = prm._orientation.W;
+                                quat.X = prm._orientation.X;
+                                quat.Y = prm._orientation.Y;
+                                quat.Z = prm._orientation.Z;
+
+                                d.Matrix3 mat = new d.Matrix3();
+                                d.RfromQ(out mat, ref quat);
+                                d.MassRotate(ref m2, ref mat);
+                                d.MassTranslate(ref m2, Position.X - prm.Position.X, Position.Y - prm.Position.Y, Position.Z - prm.Position.Z);
+                                d.MassAdd(ref pMass, ref m2);
+                            }
+                            foreach (OdePrim prm in childrenPrim)
+                            {
+                       
+                                prm.m_collisionCategories |= CollisionCategories.Body;
+                                prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
+
+                                if (prm.prim_geom == IntPtr.Zero)
+                                {
+                                    m_log.Warn("[PHYSICS]: Unable to link one of the linkset elements.  No geom yet");
+                                    continue;
+                                }
+//Console.WriteLine(" GeomSetCategoryBits 1: " + prm.prim_geom + " - " + (int)prm.m_collisionCategories + " for " + m_primName);    
+                                d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories);
+                                d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags);
+
+
+                                d.Quaternion quat = new d.Quaternion();
+                                quat.W = prm._orientation.W;
+                                quat.X = prm._orientation.X;
+                                quat.Y = prm._orientation.Y;
+                                quat.Z = prm._orientation.Z;
+
+                                d.Matrix3 mat = new d.Matrix3();
+                                d.RfromQ(out mat, ref quat);
+                                if (Body != IntPtr.Zero)
+                                {
+                                    d.GeomSetBody(prm.prim_geom, Body);
+                                    prm.childPrim = true;
+                                    d.GeomSetOffsetWorldPosition(prm.prim_geom, prm.Position.X , prm.Position.Y, prm.Position.Z);
+                                    //d.GeomSetOffsetPosition(prim.prim_geom,
+                                    //    (Position.X - prm.Position.X) - pMass.c.X,
+                                    //    (Position.Y - prm.Position.Y) - pMass.c.Y,
+                                    //    (Position.Z - prm.Position.Z) - pMass.c.Z);
+                                    d.GeomSetOffsetWorldRotation(prm.prim_geom, ref mat);
+                                    //d.GeomSetOffsetRotation(prm.prim_geom, ref mat);
+                                    d.MassTranslate(ref pMass, -pMass.c.X, -pMass.c.Y, -pMass.c.Z);
+                                    d.BodySetMass(Body, ref pMass);
+                                }
+                                else
+                                {
+                                    m_log.Debug("[PHYSICS]:I ain't got no boooooooooddy, no body");
+                                }
+
+
+                                prm.m_interpenetrationcount = 0;
+                                prm.m_collisionscore = 0;
+                                prm.m_disabled = false;
+
+                                prm.Body = Body;
+                                _parent_scene.addActivePrim(prm);
+                            }
+                            m_collisionCategories |= CollisionCategories.Body;
+                            m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
+
+//Console.WriteLine("GeomSetCategoryBits 2: " + prim_geom + " - " + (int)m_collisionCategories + " for " + m_primName);  
+                            d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
+//Console.WriteLine(" Post GeomSetCategoryBits 2");
+                            d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
+
+
+                            d.Quaternion quat2 = new d.Quaternion();
+                            quat2.W = _orientation.W;
+                            quat2.X = _orientation.X;
+                            quat2.Y = _orientation.Y;
+                            quat2.Z = _orientation.Z;
+
+                            d.Matrix3 mat2 = new d.Matrix3();
+                            d.RfromQ(out mat2, ref quat2);
+                            d.GeomSetBody(prim_geom, Body);
+                            d.GeomSetOffsetWorldPosition(prim_geom, Position.X - pMass.c.X, Position.Y - pMass.c.Y, Position.Z - pMass.c.Z);
+                            //d.GeomSetOffsetPosition(prim.prim_geom,
+                            //    (Position.X - prm.Position.X) - pMass.c.X,
+                            //    (Position.Y - prm.Position.Y) - pMass.c.Y,
+                            //    (Position.Z - prm.Position.Z) - pMass.c.Z);
+                            //d.GeomSetOffsetRotation(prim_geom, ref mat2);
+                            d.MassTranslate(ref pMass, -pMass.c.X, -pMass.c.Y, -pMass.c.Z);
+                            d.BodySetMass(Body, ref pMass);
+
+                            d.BodySetAutoDisableFlag(Body, true);
+                            d.BodySetAutoDisableSteps(Body, body_autodisable_frames);
+
+
+                            m_interpenetrationcount = 0;
+                            m_collisionscore = 0;
+                            m_disabled = false;
+
+                            d.BodySetPosition(Body, Position.X, Position.Y, Position.Z);
+                            if (m_type != Vehicle.TYPE_NONE) Enable(Body, _parent_scene);
+                            _parent_scene.addActivePrim(this);
+                        }
+                    }
+                }
+            }
+
+        }
+
+        private void ChildSetGeom(OdePrim odePrim)
+        {
+            //if (m_isphysical && Body != IntPtr.Zero)
+            lock (childrenPrim)
+            {
+                foreach (OdePrim prm in childrenPrim)
+                {
+                    //prm.childPrim = true;
+                    prm.disableBody();
+                    //prm.m_taintparent = null;
+                    //prm._parent = null;
+                    //prm.m_taintPhysics = false;
+                    //prm.m_disabled = true;
+                    //prm.childPrim = false;
+                }
+            }
+            disableBody();
+
+
+            if (Body != IntPtr.Zero)
+            {
+                _parent_scene.remActivePrim(this);
+            }
+
+            lock (childrenPrim)
+            {
+                foreach (OdePrim prm in childrenPrim)
+                {
+                    ParentPrim(prm);
+                }
+            }
+            
+        }
+
+        private void ChildDelink(OdePrim odePrim)
+        {
+            // Okay, we have a delinked child..   need to rebuild the body.
+            lock (childrenPrim)
+            {
+                foreach (OdePrim prm in childrenPrim)
+                {
+                    prm.childPrim = true;
+                    prm.disableBody();
+                    //prm.m_taintparent = null;
+                    //prm._parent = null;
+                    //prm.m_taintPhysics = false;
+                    //prm.m_disabled = true;
+                    //prm.childPrim = false;
+                }
+            }
+            disableBody();
+
+            lock (childrenPrim)
+            {
+                childrenPrim.Remove(odePrim);
+            }
+
+            if (Body != IntPtr.Zero)
+            {
+                _parent_scene.remActivePrim(this);
+            }
+
+            lock (childrenPrim)
+            {
+                foreach (OdePrim prm in childrenPrim)
+                {
+                    ParentPrim(prm);
+                }
+            }
+        }
+
+        private void changeSelectedStatus(float timestep)
+        {
+            if (m_taintselected)
+            {
+                m_collisionCategories = CollisionCategories.Selected;
+                m_collisionFlags = (CollisionCategories.Sensor | CollisionCategories.Space);
+
+                // We do the body disable soft twice because 'in theory' a collision could have happened
+                // in between the disabling and the collision properties setting
+                // which would wake the physical body up from a soft disabling and potentially cause it to fall
+                // through the ground.
+                
+                // NOTE FOR JOINTS: this doesn't always work for jointed assemblies because if you select
+                // just one part of the assembly, the rest of the assembly is non-selected and still simulating,
+                // so that causes the selected part to wake up and continue moving.
+
+                // even if you select all parts of a jointed assembly, it is not guaranteed that the entire
+                // assembly will stop simulating during the selection, because of the lack of atomicity
+                // of select operations (their processing could be interrupted by a thread switch, causing
+                // simulation to continue before all of the selected object notifications trickle down to
+                // the physics engine).
+
+                // e.g. we select 100 prims that are connected by joints. non-atomically, the first 50 are
+                // selected and disabled. then, due to a thread switch, the selection processing is
+                // interrupted and the physics engine continues to simulate, so the last 50 items, whose
+                // selection was not yet processed, continues to simulate. this wakes up ALL of the 
+                // first 50 again. then the last 50 are disabled. then the first 50, which were just woken
+                // up, start simulating again, which in turn wakes up the last 50.
+
+                if (m_isphysical)
+                {
+                    disableBodySoft();
+                }
+
+                if (prim_geom != IntPtr.Zero)
+                {
+                    d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
+                    d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
+                }
+
+                if (m_isphysical)
+                {
+                    disableBodySoft();
+                }
+            }
+            else
+            {
+                m_collisionCategories = CollisionCategories.Geom;
+
+                if (m_isphysical)
+                    m_collisionCategories |= CollisionCategories.Body;
+
+                m_collisionFlags = m_default_collisionFlags;
+
+                if (m_collidesLand)
+                    m_collisionFlags |= CollisionCategories.Land;
+                if (m_collidesWater)
+                    m_collisionFlags |= CollisionCategories.Water;
+
+                if (prim_geom != IntPtr.Zero)
+                {
+                    d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
+                    d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
+                }
+                if (m_isphysical)
+                {
+                    if (Body != IntPtr.Zero)
+                    {
+                        d.BodySetLinearVel(Body, 0f, 0f, 0f);
+                        d.BodySetForce(Body, 0, 0, 0);
+                        enableBodySoft();
+                    }
+                }
+            }
+
+            resetCollisionAccounting();
+            m_isSelected = m_taintselected;
+        }//end changeSelectedStatus
+
+        public void ResetTaints()
+        {
+            m_taintposition = _position;
+            m_taintrot = _orientation;
+            m_taintPhysics = m_isphysical;
+            m_taintselected = m_isSelected;
+            m_taintsize = _size;
+            m_taintshape = false;
+            m_taintforce = false;
+            m_taintdisable = false;
+            m_taintVelocity = Vector3.Zero;
+        }
+
+        public void CreateGeom(IntPtr m_targetSpace, IMesh _mesh)
+        {
+//Console.WriteLine("CreateGeom:");         
+            if (_mesh != null)
+            {
+                setMesh(_parent_scene, _mesh);
+            }
+            else
+            {
+                if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1)
+                {
+                    if (_size.X == _size.Y && _size.Y == _size.Z && _size.X == _size.Z)
+                    {
+                        if (((_size.X / 2f) > 0f))
+                        {
+                            _parent_scene.waitForSpaceUnlock(m_targetSpace);
+                            try
+                            {
+//Console.WriteLine(" CreateGeom 1");
+                                SetGeom(d.CreateSphere(m_targetSpace, _size.X / 2));
+                            }
+                            catch (AccessViolationException)
+                            {
+                                m_log.Warn("[PHYSICS]: Unable to create physics proxy for object");
+                                ode.dunlock(_parent_scene.world);
+                                return;
+                            }
+                        }
+                        else
+                        {
+                            _parent_scene.waitForSpaceUnlock(m_targetSpace);
+                            try
+                            {
+//Console.WriteLine(" CreateGeom 2");                           
+                                SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
+                            }
+                            catch (AccessViolationException)
+                            {
+                                m_log.Warn("[PHYSICS]: Unable to create physics proxy for object");
+                                ode.dunlock(_parent_scene.world);
+                                return;
+                            }
+                        }
+                    }
+                    else
+                    {
+                        _parent_scene.waitForSpaceUnlock(m_targetSpace);
+                        try
+                        {
+//Console.WriteLine("  CreateGeom 3");                       
+                            SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
+                        }
+                        catch (AccessViolationException)
+                        {
+                            m_log.Warn("[PHYSICS]: Unable to create physics proxy for object");
+                            ode.dunlock(_parent_scene.world);
+                            return;
+                        }
+                    }
+                }
+
+                else
+                {
+                    _parent_scene.waitForSpaceUnlock(m_targetSpace);
+                    try
+                    {
+//Console.WriteLine("  CreateGeom 4");                  
+                        SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
+                    }
+                    catch (AccessViolationException)
+                    {
+                        m_log.Warn("[PHYSICS]: Unable to create physics proxy for object");
+                        ode.dunlock(_parent_scene.world);
+                        return;
+                    }
+                }
+            }
+        }
+
+        public void changeadd(float timestep)
+        {
+            int[] iprimspaceArrItem = _parent_scene.calculateSpaceArrayItemFromPos(_position);
+            IntPtr targetspace = _parent_scene.calculateSpaceForGeom(_position);
+
+            if (targetspace == IntPtr.Zero)
+                targetspace = _parent_scene.createprimspace(iprimspaceArrItem[0], iprimspaceArrItem[1]);
+
+            m_targetSpace = targetspace;
+
+            if (_mesh == null)
+            {
+                if (_parent_scene.needsMeshing(_pbs))
+                {
+                    // Don't need to re-enable body..   it's done in SetMesh
+                    _mesh = _parent_scene.mesher.CreateMesh(m_primName, _pbs, _size, _parent_scene.meshSculptLOD, IsPhysical);
+                    // createmesh returns null when it's a shape that isn't a cube.
+                   // m_log.Debug(m_localID);
+                }
+            }
+
+
+            lock (_parent_scene.OdeLock)
+            {
+//Console.WriteLine("changeadd 1");           
+                CreateGeom(m_targetSpace, _mesh);
+
+                if (prim_geom != IntPtr.Zero)
+                {
+                    d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
+                    d.Quaternion myrot = new d.Quaternion();
+                    myrot.X = _orientation.X;
+                    myrot.Y = _orientation.Y;
+                    myrot.Z = _orientation.Z;
+                    myrot.W = _orientation.W;
+                    d.GeomSetQuaternion(prim_geom, ref myrot);
+                }
+
+                if (m_isphysical && Body == IntPtr.Zero)
+                {
+                    enableBody();
+                }
+            }
+
+            _parent_scene.geom_name_map[prim_geom] = this.m_primName;
+            _parent_scene.actor_name_map[prim_geom] = (PhysicsActor)this;
+
+            changeSelectedStatus(timestep);
+
+            m_taintadd = false;
+        }
+
+        public void changemove(float timestep)
+        {
+//Console.WriteLine("changemove for {0}", m_primName );
+        
+            if (m_isphysical)
+            {
+//Console.WriteLine("phys  {0}   {1}   {2}", m_disabled, m_taintremove, childPrim);                
+//                if (!m_disabled && !m_taintremove && !childPrim)  After one edit m_disabled is sometimes set, disabling further edits!
+                if (!m_taintremove && !childPrim)
+                {
+//Console.WriteLine("physOK");                
+                    if (Body == IntPtr.Zero)
+                        enableBody();
+                    //Prim auto disable after 20 frames,
+                    //if you move it, re-enable the prim manually.
+                    if (_parent != null)
+                    {
+//Console.WriteLine("physChild");                
+                        if (m_linkJoint != IntPtr.Zero)
+                        {
+                            d.JointDestroy(m_linkJoint);
+                            m_linkJoint = IntPtr.Zero;
+                        }
+                    }
+                    if (Body != IntPtr.Zero)
+                    {
+//Console.WriteLine("physNotIPZ");                
+                        d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
+
+                        if (_parent != null)
+                        {
+                            OdePrim odParent = (OdePrim)_parent;
+                            if (Body != (IntPtr)0 && odParent.Body != (IntPtr)0 && Body != odParent.Body)
+                            {
+// KF: Fixed Joints were removed? Anyway - this Console.WriteLine does not show up, so routine is not used??
+Console.WriteLine(" JointCreateFixed");                           
+                                m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup);
+                                d.JointAttach(m_linkJoint, Body, odParent.Body);
+                                d.JointSetFixed(m_linkJoint);
+                            }
+                        }
+                        d.BodyEnable(Body);
+                        if (m_type != Vehicle.TYPE_NONE)
+                        {
+                            Enable(Body, _parent_scene);
+                        }
+                    }
+                    else
+                    {
+                        m_log.Warn("[PHYSICS]: Body Still null after enableBody().  This is a crash scenario.");
+                    }
+                }
+                //else
+               // {
+                    //m_log.Debug("[BUG]: race!");
+                //}
+            }
+            else
+            {
+//Console.WriteLine("NONphys");                
+                // string primScenAvatarIn = _parent_scene.whichspaceamIin(_position);
+                // int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position);
+                _parent_scene.waitForSpaceUnlock(m_targetSpace);
+
+                IntPtr tempspace = _parent_scene.recalculateSpaceForGeom(prim_geom, _position, m_targetSpace);
+                m_targetSpace = tempspace;
+
+                _parent_scene.waitForSpaceUnlock(m_targetSpace);
+                if (prim_geom != IntPtr.Zero)
+                {
+                    d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
+
+                    _parent_scene.waitForSpaceUnlock(m_targetSpace);
+                    d.SpaceAdd(m_targetSpace, prim_geom);
+                }
+            }
+
+            changeSelectedStatus(timestep);
+
+            resetCollisionAccounting();
+            m_taintposition = _position;
+        }
+
+
+
+        public void rotate(float timestep)
+        {
+            d.Quaternion myrot = new d.Quaternion();
+            myrot.X = _orientation.X;
+            myrot.Y = _orientation.Y;
+            myrot.Z = _orientation.Z;
+            myrot.W = _orientation.W;
+            if (Body != IntPtr.Zero)
+            {
+                    // KF: If this is a root prim do BodySet
+                d.BodySetQuaternion(Body, ref myrot);
+                }
+                else
+                {
+                        // daughter prim, do Geom set
+                    d.GeomSetQuaternion(prim_geom, ref myrot);
+            }
+            
+            resetCollisionAccounting();
+            m_taintrot = _orientation;
+        }
+
+        private void resetCollisionAccounting()
+        {
+            m_collisionscore = 0;
+            m_interpenetrationcount = 0;
+            m_disabled = false;
+        }
+
+        public void changedisable(float timestep)
+        {
+            m_disabled = true;
+            if (Body != IntPtr.Zero)
+            {
+                d.BodyDisable(Body);
+                Body = IntPtr.Zero;
+            }
+
+            m_taintdisable = false;
+        }
+
+        public void changePhysicsStatus(float timestep)
+        {
+            if (m_isphysical == true)
+            {
+                if (Body == IntPtr.Zero)
+                {
+                    if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim)
+                    {
+                        changeshape(2f);
+                    }
+                    else
+                    {
+                        enableBody();
+                    }
+                }
+            }
+            else
+            {
+                if (Body != IntPtr.Zero)
+                {
+                    if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim)
+                    {
+                        
+
+                        if (prim_geom != IntPtr.Zero)
+                        {
+                            try
+                            {
+                                d.GeomDestroy(prim_geom);
+                                prim_geom = IntPtr.Zero;
+                                _mesh = null;
+                            }
+                            catch (System.AccessViolationException)
+                            {
+                                prim_geom = IntPtr.Zero;
+                                m_log.Error("[PHYSICS]: PrimGeom dead");
+                            }
+                        }
+//Console.WriteLine("changePhysicsStatus for " + m_primName );
+                        changeadd(2f);
+                    }
+                    if (childPrim)
+                    {
+                        if (_parent != null)
+                        {
+                            OdePrim parent = (OdePrim)_parent;
+                            parent.ChildDelink(this);
+                        }
+                    }
+                    else
+                    {
+                        disableBody();
+                    }
+                }
+            }
+
+            changeSelectedStatus(timestep);
+
+            resetCollisionAccounting();
+            m_taintPhysics = m_isphysical;
+        }
+
+        public void changesize(float timestamp)
+        {
+            
+            string oldname = _parent_scene.geom_name_map[prim_geom];
+
+            if (_size.X <= 0) _size.X = 0.01f;
+            if (_size.Y <= 0) _size.Y = 0.01f;
+            if (_size.Z <= 0) _size.Z = 0.01f;
+
+            // Cleanup of old prim geometry
+            if (_mesh != null)
+            {
+                // Cleanup meshing here
+            }
+            //kill body to rebuild
+            if (IsPhysical && Body != IntPtr.Zero)
+            {
+                if (childPrim)
+                {
+                    if (_parent != null)
+                    {
+                        OdePrim parent = (OdePrim)_parent;
+                        parent.ChildDelink(this);
+                    }
+                }
+                else
+                {
+                    disableBody();
+                }
+            }
+            if (d.SpaceQuery(m_targetSpace, prim_geom))
+            {
+                _parent_scene.waitForSpaceUnlock(m_targetSpace);
+                d.SpaceRemove(m_targetSpace, prim_geom);
+            }
+            d.GeomDestroy(prim_geom);
+            prim_geom = IntPtr.Zero;
+            // we don't need to do space calculation because the client sends a position update also.
+
+            // Construction of new prim
+            if (_parent_scene.needsMeshing(_pbs))
+            {
+                float meshlod = _parent_scene.meshSculptLOD;
+
+                if (IsPhysical)
+                    meshlod = _parent_scene.MeshSculptphysicalLOD;
+                // Don't need to re-enable body..   it's done in SetMesh
+
+                IMesh mesh = null;
+
+                if (_parent_scene.needsMeshing(_pbs))
+                    mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical);
+
+                //IMesh mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical);
+//Console.WriteLine("changesize 1");
+                CreateGeom(m_targetSpace, mesh);
+
+               
+            }
+            else
+            {
+                _mesh = null;
+//Console.WriteLine("changesize 2");    
+                CreateGeom(m_targetSpace, _mesh);
+            }
+
+            d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
+            d.Quaternion myrot = new d.Quaternion();
+            myrot.X = _orientation.X;
+            myrot.Y = _orientation.Y;
+            myrot.Z = _orientation.Z;
+            myrot.W = _orientation.W;
+            d.GeomSetQuaternion(prim_geom, ref myrot);
+
+            //d.GeomBoxSetLengths(prim_geom, _size.X, _size.Y, _size.Z);
+            if (IsPhysical && Body == IntPtr.Zero && !childPrim)
+            {
+                // Re creates body on size.
+                // EnableBody also does setMass()
+                enableBody();
+                d.BodyEnable(Body);
+            }
+
+            _parent_scene.geom_name_map[prim_geom] = oldname;
+
+            changeSelectedStatus(timestamp);
+            if (childPrim)
+            {
+                if (_parent is OdePrim)
+                {
+                    OdePrim parent = (OdePrim)_parent;
+                    parent.ChildSetGeom(this);
+                }
+            }
+            resetCollisionAccounting();
+            m_taintsize = _size;
+        }
+
+       
+
+        public void changefloatonwater(float timestep)
+        {
+            m_collidesWater = m_taintCollidesWater;
+
+            if (prim_geom != IntPtr.Zero)
+            {
+                if (m_collidesWater)
+                {
+                    m_collisionFlags |= CollisionCategories.Water;
+                }
+                else
+                {
+                    m_collisionFlags &= ~CollisionCategories.Water;
+                }
+                d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
+            }
+        }
+
+        public void changeshape(float timestamp)
+        {
+            string oldname = _parent_scene.geom_name_map[prim_geom];
+
+            // Cleanup of old prim geometry and Bodies
+            if (IsPhysical && Body != IntPtr.Zero)
+            {
+                if (childPrim)
+                {
+                    if (_parent != null)
+                    {
+                        OdePrim parent = (OdePrim)_parent;
+                        parent.ChildDelink(this);
+                    }
+                }
+                else
+                {
+                    disableBody();
+                }
+            }
+            try
+            {
+                d.GeomDestroy(prim_geom);
+            }
+            catch (System.AccessViolationException)
+            {
+                prim_geom = IntPtr.Zero;
+                m_log.Error("[PHYSICS]: PrimGeom dead");
+            }
+            prim_geom = IntPtr.Zero;
+            // we don't need to do space calculation because the client sends a position update also.
+            if (_size.X <= 0) _size.X = 0.01f;
+            if (_size.Y <= 0) _size.Y = 0.01f;
+            if (_size.Z <= 0) _size.Z = 0.01f;
+            // Construction of new prim
+
+            if (_parent_scene.needsMeshing(_pbs))
+            {
+                // Don't need to re-enable body..   it's done in SetMesh
+                float meshlod = _parent_scene.meshSculptLOD;
+
+                if (IsPhysical)
+                    meshlod = _parent_scene.MeshSculptphysicalLOD;
+
+                IMesh mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical);
+                // createmesh returns null when it doesn't mesh.
+                CreateGeom(m_targetSpace, mesh);
+            }
+            else
+            {
+                _mesh = null;
+//Console.WriteLine("changeshape");              
+                CreateGeom(m_targetSpace, null);
+            }
+
+            d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
+            d.Quaternion myrot = new d.Quaternion();
+            //myrot.W = _orientation.w;
+            myrot.W = _orientation.W;
+            myrot.X = _orientation.X;
+            myrot.Y = _orientation.Y;
+            myrot.Z = _orientation.Z;
+            d.GeomSetQuaternion(prim_geom, ref myrot);
+
+            //d.GeomBoxSetLengths(prim_geom, _size.X, _size.Y, _size.Z);
+            if (IsPhysical && Body == IntPtr.Zero)
+            {
+                // Re creates body on size.
+                // EnableBody also does setMass()
+                enableBody();
+                if (Body != IntPtr.Zero)
+                {
+                    d.BodyEnable(Body);
+                }
+            }
+            _parent_scene.geom_name_map[prim_geom] = oldname;
+
+            changeSelectedStatus(timestamp);
+            if (childPrim)
+            {
+                if (_parent is OdePrim)
+                {
+                    OdePrim parent = (OdePrim)_parent;
+                    parent.ChildSetGeom(this);
+                }
+            }
+            resetCollisionAccounting();
+            m_taintshape = false;
+        }
+
+        public void changeAddForce(float timestamp)
+        {
+            if (!m_isSelected)
+            {
+                lock (m_forcelist)
+                {
+                    //m_log.Info("[PHYSICS]: dequeing forcelist");
+                    if (IsPhysical)
+                    {
+                        Vector3 iforce = Vector3.Zero;
+                        int i = 0;
+                        try
+                        {
+                            for (i = 0; i < m_forcelist.Count; i++)
+                            {
+
+                                iforce = iforce + (m_forcelist[i] * 100);
+                            }
+                        }
+                        catch (IndexOutOfRangeException)
+                        {
+                            m_forcelist = new List<Vector3>();
+                            m_collisionscore = 0;
+                            m_interpenetrationcount = 0;
+                            m_taintforce = false;
+                            return;
+                        }
+                        catch (ArgumentOutOfRangeException)
+                        {
+                            m_forcelist = new List<Vector3>();
+                            m_collisionscore = 0;
+                            m_interpenetrationcount = 0;
+                            m_taintforce = false;
+                            return;
+                        }
+                        d.BodyEnable(Body);
+                        d.BodyAddForce(Body, iforce.X, iforce.Y, iforce.Z);
+                    }
+                    m_forcelist.Clear();
+                }
+
+                m_collisionscore = 0;
+                m_interpenetrationcount = 0;
+            }
+
+            m_taintforce = false;
+
+        }
+
+
+
+        public void changeSetTorque(float timestamp)
+        {
+            if (!m_isSelected)
+            {
+                if (IsPhysical && Body != IntPtr.Zero)
+                {
+                    d.BodySetTorque(Body, m_taintTorque.X, m_taintTorque.Y, m_taintTorque.Z);
+                }
+            }
+
+            m_taintTorque = Vector3.Zero;
+        }
+
+        public void changeAddAngularForce(float timestamp)
+        {
+            if (!m_isSelected)
+            {
+                lock (m_angularforcelist)
+                {
+                    //m_log.Info("[PHYSICS]: dequeing forcelist");
+                    if (IsPhysical)
+                    {
+                        Vector3 iforce = Vector3.Zero;
+                        for (int i = 0; i < m_angularforcelist.Count; i++)
+                        {
+                            iforce = iforce + (m_angularforcelist[i] * 100);
+                        }
+                        d.BodyEnable(Body);
+                        d.BodyAddTorque(Body, iforce.X, iforce.Y, iforce.Z);
+                        
+                    }
+                    m_angularforcelist.Clear();
+                }
+
+                m_collisionscore = 0;
+                m_interpenetrationcount = 0;
+            }
+
+            m_taintaddangularforce = false;
+        }
+
+        private void changevelocity(float timestep)
+        {
+            if (!m_isSelected)
+            {
+                Thread.Sleep(20);
+                if (IsPhysical)
+                {
+                    if (Body != IntPtr.Zero)
+                    {
+                        d.BodySetLinearVel(Body, m_taintVelocity.X, m_taintVelocity.Y, m_taintVelocity.Z);
+                    }
+                }
+                
+                //resetCollisionAccounting();
+            }
+            m_taintVelocity = Vector3.Zero;
+        }
+
+        public void UpdatePositionAndVelocity()
+        {
+                return;   // moved to the Move() method
+        }
+/* No one uses this?        
+        public Matrix4 FromDMass(d.Mass pMass)
+        {
+            Matrix4 obj;
+            obj.M11 = pMass.I.M00;
+            obj.M12 = pMass.I.M01;
+            obj.M13 = pMass.I.M02;
+            obj.M14 = 0;
+            obj.M21 = pMass.I.M10;
+            obj.M22 = pMass.I.M11;
+            obj.M23 = pMass.I.M12;
+            obj.M24 = 0;
+            obj.M31 = pMass.I.M20;
+            obj.M32 = pMass.I.M21;
+            obj.M33 = pMass.I.M22;
+            obj.M34 = 0;
+            obj.M41 = 0;
+            obj.M42 = 0;
+            obj.M43 = 0;
+            obj.M44 = 1;
+            return obj;
+        }
+*/
+        public d.Mass FromMatrix4(Matrix4 pMat, ref d.Mass obj)
+        {
+            obj.I.M00 = pMat[0, 0];
+            obj.I.M01 = pMat[0, 1];
+            obj.I.M02 = pMat[0, 2];
+            obj.I.M10 = pMat[1, 0];
+            obj.I.M11 = pMat[1, 1];
+            obj.I.M12 = pMat[1, 2];
+            obj.I.M20 = pMat[2, 0];
+            obj.I.M21 = pMat[2, 1];
+            obj.I.M22 = pMat[2, 2];
+            return obj;
+        }
+
+        public override void SubscribeEvents(int ms)
+        {
+            m_eventsubscription = ms;
+            _parent_scene.addCollisionEventReporting(this);
+        }
+
+        public override void UnSubscribeEvents()
+        {
+            _parent_scene.remCollisionEventReporting(this);
+            m_eventsubscription = 0;
+        }        
+
+        public void AddCollisionEvent(uint CollidedWith, ContactPoint contact)
+        {
+            if (CollisionEventsThisFrame == null)
+                CollisionEventsThisFrame = new CollisionEventUpdate();
+            CollisionEventsThisFrame.addCollider(CollidedWith, contact);
+        }
+
+        public void SendCollisions()
+        {
+            if (CollisionEventsThisFrame == null)
+                return;
+
+            base.SendCollisionUpdate(CollisionEventsThisFrame);
+
+            if (CollisionEventsThisFrame.m_objCollisionList.Count == 0)
+                CollisionEventsThisFrame = null;
+            else
+                CollisionEventsThisFrame = new CollisionEventUpdate();
+        }
+
+        public override bool SubscribedEvents()
+        {
+            if (m_eventsubscription > 0)
+                return true;
+            return false;
+        }
+
+        public static Matrix4 Inverse(Matrix4 pMat)
+        {
+            if (determinant3x3(pMat) == 0)
+            {
+                return Matrix4.Identity; // should probably throw an error.  singluar matrix inverse not possible
+            }
+
+
+
+            return (Adjoint(pMat) / determinant3x3(pMat));
+        }
+
+        public static Matrix4 Adjoint(Matrix4 pMat)
+        {
+            Matrix4 adjointMatrix = new Matrix4();
+            for (int i=0; i<4; i++)
+            {
+                for (int j=0; j<4; j++)
+                {
+                    Matrix4SetValue(ref adjointMatrix, i, j, (float)(Math.Pow(-1, i + j) * (determinant3x3(Minor(pMat, i, j)))));
+                }
+            }
+
+            adjointMatrix = Transpose(adjointMatrix);
+            return adjointMatrix;
+        }
+
+        public static Matrix4 Minor(Matrix4 matrix, int iRow, int iCol)
+        {
+            Matrix4 minor = new Matrix4();
+            int m = 0, n = 0;
+            for (int i = 0; i < 4; i++)
+            {
+                if (i == iRow)
+                    continue;
+                n = 0;
+                for (int j = 0; j < 4; j++)
+                {
+                    if (j == iCol)
+                        continue;
+                    Matrix4SetValue(ref minor, m,n, matrix[i, j]);
+                    n++;
+                }
+                m++;
+            }
+            return minor;
+        }
+
+        public static Matrix4 Transpose(Matrix4 pMat)
+        {
+            Matrix4 transposeMatrix = new Matrix4();
+            for (int i = 0; i < 4; i++)
+                for (int j = 0; j < 4; j++)
+                    Matrix4SetValue(ref transposeMatrix, i, j, pMat[j, i]);
+            return transposeMatrix;
+        }
+
+        public static void Matrix4SetValue(ref Matrix4 pMat, int r, int c, float val)
+        {
+            switch (r)
+            {
+                case 0:
+                    switch (c)
+                    {
+                        case 0:
+                            pMat.M11 = val;
+                            break;
+                        case 1:
+                            pMat.M12 = val;
+                            break;
+                        case 2:
+                            pMat.M13 = val;
+                            break;
+                        case 3:
+                            pMat.M14 = val;
+                            break;
+                    }
+
+                    break;
+                case 1:
+                    switch (c)
+                    {
+                        case 0:
+                            pMat.M21 = val;
+                            break;
+                        case 1:
+                            pMat.M22 = val;
+                            break;
+                        case 2:
+                            pMat.M23 = val;
+                            break;
+                        case 3:
+                            pMat.M24 = val;
+                            break;
+                    }
+
+                    break;
+                case 2:
+                    switch (c)
+                    {
+                        case 0:
+                            pMat.M31 = val;
+                            break;
+                        case 1:
+                            pMat.M32 = val;
+                            break;
+                        case 2:
+                            pMat.M33 = val;
+                            break;
+                        case 3:
+                            pMat.M34 = val;
+                            break;
+                    }
+
+                    break;
+                case 3:
+                    switch (c)
+                    {
+                        case 0:
+                            pMat.M41 = val;
+                            break;
+                        case 1:
+                            pMat.M42 = val;
+                            break;
+                        case 2:
+                            pMat.M43 = val;
+                            break;
+                        case 3:
+                            pMat.M44 = val;
+                            break;
+                    }
+
+                    break;
+            }
+        }
+        private static float determinant3x3(Matrix4 pMat)
+        {
+            float det = 0;
+            float diag1 = pMat[0, 0]*pMat[1, 1]*pMat[2, 2];
+            float diag2 = pMat[0, 1]*pMat[2, 1]*pMat[2, 0];
+            float diag3 = pMat[0, 2]*pMat[1, 0]*pMat[2, 1];
+            float diag4 = pMat[2, 0]*pMat[1, 1]*pMat[0, 2];
+            float diag5 = pMat[2, 1]*pMat[1, 2]*pMat[0, 0];
+            float diag6 = pMat[2, 2]*pMat[1, 0]*pMat[0, 1];
+
+            det = diag1 + diag2 + diag3 - (diag4 + diag5 + diag6);
+            return det;
+
+        }
+        
+        private static void DMassCopy(ref d.Mass src, ref d.Mass dst)
+        {
+            dst.c.W = src.c.W;
+            dst.c.X = src.c.X;
+            dst.c.Y = src.c.Y;
+            dst.c.Z = src.c.Z;
+            dst.mass = src.mass;
+            dst.I.M00 = src.I.M00;
+            dst.I.M01 = src.I.M01;
+            dst.I.M02 = src.I.M02;
+            dst.I.M10 = src.I.M10;
+            dst.I.M11 = src.I.M11;
+            dst.I.M12 = src.I.M12;
+            dst.I.M20 = src.I.M20;
+            dst.I.M21 = src.I.M21;
+            dst.I.M22 = src.I.M22;
+        }
+
+        public override void SetMaterial(int pMaterial)
+        {
+            m_material = pMaterial;
+        }
+
+        internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue)
+        {
+            switch (pParam)
+            {
+                case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY:
+                    if (pValue < 0.01f) pValue = 0.01f;
+                    // m_angularDeflectionEfficiency = pValue;
+                    break;
+                case Vehicle.ANGULAR_DEFLECTION_TIMESCALE:
+                    if (pValue < 0.01f) pValue = 0.01f;
+                    // m_angularDeflectionTimescale = pValue;
+                    break;
+                case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE:
+                    if (pValue < 0.01f) pValue = 0.01f;
+                    m_angularMotorDecayTimescale = pValue;
+                    break;
+                case Vehicle.ANGULAR_MOTOR_TIMESCALE:
+                    if (pValue < 0.01f) pValue = 0.01f;
+                    m_angularMotorTimescale = pValue;
+                    break;
+                case Vehicle.BANKING_EFFICIENCY:
+                    if (pValue < 0.01f) pValue = 0.01f;
+                    // m_bankingEfficiency = pValue;
+                    break;
+                case Vehicle.BANKING_MIX:
+                    if (pValue < 0.01f) pValue = 0.01f;
+                    // m_bankingMix = pValue;
+                    break;
+                case Vehicle.BANKING_TIMESCALE:
+                    if (pValue < 0.01f) pValue = 0.01f;
+                    // m_bankingTimescale = pValue;
+                    break;
+                case Vehicle.BUOYANCY:
+                        if (pValue < -1f) pValue = -1f;
+                        if (pValue > 1f) pValue = 1f;
+                    m_VehicleBuoyancy = pValue;
+                    break;
+//                case Vehicle.HOVER_EFFICIENCY:
+//                      if (pValue < 0f) pValue = 0f;
+//                      if (pValue > 1f) pValue = 1f;
+//                    m_VhoverEfficiency = pValue;
+//                    break;
+                case Vehicle.HOVER_HEIGHT:
+                    m_VhoverHeight = pValue;
+                    break;
+                case Vehicle.HOVER_TIMESCALE:
+                    if (pValue < 0.01f) pValue = 0.01f;
+                    m_VhoverTimescale = pValue;
+                    break;
+                case Vehicle.LINEAR_DEFLECTION_EFFICIENCY:
+                    if (pValue < 0.01f) pValue = 0.01f;
+                    // m_linearDeflectionEfficiency = pValue;
+                    break;
+                case Vehicle.LINEAR_DEFLECTION_TIMESCALE:
+                    if (pValue < 0.01f) pValue = 0.01f;
+                    // m_linearDeflectionTimescale = pValue;
+                    break;
+                case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE:
+                    if (pValue < 0.01f) pValue = 0.01f;
+                    m_linearMotorDecayTimescale = pValue;
+                    break;
+                case Vehicle.LINEAR_MOTOR_TIMESCALE:
+                    if (pValue < 0.01f) pValue = 0.01f;
+                    m_linearMotorTimescale = pValue;
+                    break;
+                case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY:
+                    if (pValue < 0.1f) pValue = 0.1f;   // Less goes unstable
+                    if (pValue > 1.0f) pValue = 1.0f;
+                    m_verticalAttractionEfficiency = pValue;
+                    break;
+                case Vehicle.VERTICAL_ATTRACTION_TIMESCALE:
+                    if (pValue < 0.01f) pValue = 0.01f;
+                    m_verticalAttractionTimescale = pValue;
+                    break;
+                    
+                // These are vector properties but the engine lets you use a single float value to 
+                // set all of the components to the same value
+                case Vehicle.ANGULAR_FRICTION_TIMESCALE:
+                        if (pValue > 30f) pValue = 30f;
+                        if (pValue < 0.1f) pValue = 0.1f;
+                    m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue);
+                    break;
+                case Vehicle.ANGULAR_MOTOR_DIRECTION:
+                    m_angularMotorDirection = new Vector3(pValue, pValue, pValue);
+                    UpdateAngDecay();
+                    break;
+                case Vehicle.LINEAR_FRICTION_TIMESCALE:
+                    m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue);
+                    break;
+                case Vehicle.LINEAR_MOTOR_DIRECTION:
+                    m_linearMotorDirection = new Vector3(pValue, pValue, pValue);
+                    UpdateLinDecay();
+                    break;
+                case Vehicle.LINEAR_MOTOR_OFFSET:
+                    // m_linearMotorOffset = new Vector3(pValue, pValue, pValue);
+                    break;
+
+            }
+            
+        }//end ProcessFloatVehicleParam
+
+        internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue)
+        {
+            switch (pParam)
+            {
+                case Vehicle.ANGULAR_FRICTION_TIMESCALE:
+                        if (pValue.X > 30f) pValue.X = 30f;
+                        if (pValue.X < 0.1f) pValue.X = 0.1f;
+                        if (pValue.Y > 30f) pValue.Y = 30f;
+                        if (pValue.Y < 0.1f) pValue.Y = 0.1f;
+                        if (pValue.Z > 30f) pValue.Z = 30f;
+                        if (pValue.Z < 0.1f) pValue.Z = 0.1f;
+                    m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
+                    break;
+                case Vehicle.ANGULAR_MOTOR_DIRECTION:
+                    m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
+                    // Limit requested angular speed to 2 rps= 4 pi rads/sec
+                    if(m_angularMotorDirection.X > 12.56f) m_angularMotorDirection.X = 12.56f; 
+                    if(m_angularMotorDirection.X < - 12.56f) m_angularMotorDirection.X = - 12.56f; 
+                    if(m_angularMotorDirection.Y > 12.56f) m_angularMotorDirection.Y = 12.56f; 
+                    if(m_angularMotorDirection.Y < - 12.56f) m_angularMotorDirection.Y = - 12.56f; 
+                    if(m_angularMotorDirection.Z > 12.56f) m_angularMotorDirection.Z = 12.56f; 
+                    if(m_angularMotorDirection.Z < - 12.56f) m_angularMotorDirection.Z = - 12.56f; 
+                    UpdateAngDecay();
+                    break;
+                case Vehicle.LINEAR_FRICTION_TIMESCALE:
+                    m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
+                    break;
+                case Vehicle.LINEAR_MOTOR_DIRECTION:
+                    m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); // velocity requested by LSL, for max limiting
+                    UpdateLinDecay();
+                    break;
+                case Vehicle.LINEAR_MOTOR_OFFSET:
+                    // m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z);
+                    break;
+            }
+            
+        }//end ProcessVectorVehicleParam
+
+        internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue)
+        {
+            switch (pParam)
+            {
+                case Vehicle.REFERENCE_FRAME:
+                    // m_referenceFrame = pValue;
+                    break;
+            }
+            
+        }//end ProcessRotationVehicleParam
+        
+        internal void ProcessVehicleFlags(int pParam, bool remove)
+        {
+            if (remove)
+            {
+                        m_flags &= ~((VehicleFlag)pParam);         
+                        }
+                        else
+                        {
+                        m_flags |= (VehicleFlag)pParam;
+                }
+        }
+        
+        internal void ProcessTypeChange(Vehicle pType)
+        {
+                        // Set Defaults For Type
+            m_type = pType;
+            switch (pType)
+            {
+                case Vehicle.TYPE_SLED:
+                    m_linearFrictionTimescale = new Vector3(30, 1, 1000);
+                    m_angularFrictionTimescale = new Vector3(30, 30, 30);
+//                     m_lLinMotorVel = Vector3.Zero;
+                    m_linearMotorTimescale = 1000;
+                    m_linearMotorDecayTimescale = 120;
+                    m_angularMotorDirection = Vector3.Zero;
+                    m_angularMotorDVel = Vector3.Zero;
+                    m_angularMotorTimescale = 1000;
+                    m_angularMotorDecayTimescale = 120;
+                    m_VhoverHeight = 0;
+//                    m_VhoverEfficiency = 1;
+                    m_VhoverTimescale = 10;
+                    m_VehicleBuoyancy = 0;
+                    // m_linearDeflectionEfficiency = 1;
+                    // m_linearDeflectionTimescale = 1;
+                    // m_angularDeflectionEfficiency = 1;
+                    // m_angularDeflectionTimescale = 1000;
+                    // m_bankingEfficiency = 0;
+                    // m_bankingMix = 1;
+                    // m_bankingTimescale = 10;
+                    // m_referenceFrame = Quaternion.Identity;
+                    m_flags &=
+                        ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
+                          VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
+                    m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.LIMIT_MOTOR_UP);
+                    break;
+                case Vehicle.TYPE_CAR:
+                    m_linearFrictionTimescale = new Vector3(100, 2, 1000);
+                    m_angularFrictionTimescale = new Vector3(30, 30, 30);               // was 1000, but sl max frict time is 30.
+//                     m_lLinMotorVel = Vector3.Zero;
+                    m_linearMotorTimescale = 1;
+                    m_linearMotorDecayTimescale = 60;
+                    m_angularMotorDirection = Vector3.Zero;
+                    m_angularMotorDVel = Vector3.Zero;
+                    m_angularMotorTimescale = 1;
+                    m_angularMotorDecayTimescale = 0.8f;
+                    m_VhoverHeight = 0;
+//                    m_VhoverEfficiency = 0;
+                    m_VhoverTimescale = 1000;
+                    m_VehicleBuoyancy = 0;
+                    // // m_linearDeflectionEfficiency = 1;
+                    // // m_linearDeflectionTimescale = 2;
+                    // // m_angularDeflectionEfficiency = 0;
+                    // m_angularDeflectionTimescale = 10;
+                    m_verticalAttractionEfficiency = 1f;
+                    m_verticalAttractionTimescale = 10f;
+                    // m_bankingEfficiency = -0.2f;
+                    // m_bankingMix = 1;
+                    // m_bankingTimescale = 1;
+                    // m_referenceFrame = Quaternion.Identity;
+                    m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT);
+                    m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.HOVER_UP_ONLY |
+                                VehicleFlag.LIMIT_MOTOR_UP);
+                    break;
+                case Vehicle.TYPE_BOAT:
+                    m_linearFrictionTimescale = new Vector3(10, 3, 2);
+                    m_angularFrictionTimescale = new Vector3(10,10,10);
+//                     m_lLinMotorVel = Vector3.Zero;
+                    m_linearMotorTimescale = 5;
+                    m_linearMotorDecayTimescale = 60;
+                    m_angularMotorDirection = Vector3.Zero;
+                    m_angularMotorDVel = Vector3.Zero;
+                    m_angularMotorTimescale = 4;
+                    m_angularMotorDecayTimescale = 4;
+                    m_VhoverHeight = 0;
+//                    m_VhoverEfficiency = 0.5f;
+                    m_VhoverTimescale = 2;
+                    m_VehicleBuoyancy = 1;
+                    // m_linearDeflectionEfficiency = 0.5f;
+                    // m_linearDeflectionTimescale = 3;
+                    // m_angularDeflectionEfficiency = 0.5f;
+                    // m_angularDeflectionTimescale = 5;
+                    m_verticalAttractionEfficiency = 0.5f;
+                    m_verticalAttractionTimescale = 5f;
+                    // m_bankingEfficiency = -0.3f;
+                    // m_bankingMix = 0.8f;
+                    // m_bankingTimescale = 1;
+                    // m_referenceFrame = Quaternion.Identity;
+                    m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.LIMIT_ROLL_ONLY | 
+                                VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
+                    m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY |
+                                VehicleFlag.LIMIT_MOTOR_UP);
+                    break;
+                case Vehicle.TYPE_AIRPLANE:
+                    m_linearFrictionTimescale = new Vector3(200, 10, 5);
+                    m_angularFrictionTimescale = new Vector3(20, 20, 20);
+//                     m_lLinMotorVel = Vector3.Zero;
+                    m_linearMotorTimescale = 2;
+                    m_linearMotorDecayTimescale = 60;
+                    m_angularMotorDirection = Vector3.Zero;
+                    m_angularMotorDVel = Vector3.Zero;
+                    m_angularMotorTimescale = 4;
+                    m_angularMotorDecayTimescale = 4;
+                    m_VhoverHeight = 0;
+//                    m_VhoverEfficiency = 0.5f;
+                    m_VhoverTimescale = 1000;
+                    m_VehicleBuoyancy = 0;
+                    // m_linearDeflectionEfficiency = 0.5f;
+                    // m_linearDeflectionTimescale = 3;
+                    // m_angularDeflectionEfficiency = 1;
+                    // m_angularDeflectionTimescale = 2;
+                    m_verticalAttractionEfficiency = 0.9f;
+                    m_verticalAttractionTimescale = 2f;
+                    // m_bankingEfficiency = 1;
+                    // m_bankingMix = 0.7f;
+                    // m_bankingTimescale = 2;
+                    // m_referenceFrame = Quaternion.Identity;
+                    m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
+                        VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY | VehicleFlag.LIMIT_MOTOR_UP);
+                    m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY);
+                    break;
+                case Vehicle.TYPE_BALLOON:
+                    m_linearFrictionTimescale = new Vector3(5, 5, 5);
+                    m_angularFrictionTimescale = new Vector3(10, 10, 10);
+                    m_linearMotorTimescale = 5;
+                    m_linearMotorDecayTimescale = 60;
+                    m_angularMotorDirection = Vector3.Zero;
+                    m_angularMotorDVel = Vector3.Zero;
+                    m_angularMotorTimescale = 6;
+                    m_angularMotorDecayTimescale = 10;
+                    m_VhoverHeight = 5;
+//                    m_VhoverEfficiency = 0.8f;
+                    m_VhoverTimescale = 10;
+                    m_VehicleBuoyancy = 1;
+                    // m_linearDeflectionEfficiency = 0;
+                    // m_linearDeflectionTimescale = 5;
+                    // m_angularDeflectionEfficiency = 0;
+                    // m_angularDeflectionTimescale = 5;
+                    m_verticalAttractionEfficiency = 1f;
+                    m_verticalAttractionTimescale = 100f;
+                    // m_bankingEfficiency = 0;
+                    // m_bankingMix = 0.7f;
+                    // m_bankingTimescale = 5;
+                    // m_referenceFrame = Quaternion.Identity;
+                    m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
+                        VehicleFlag.HOVER_UP_ONLY | VehicleFlag.LIMIT_MOTOR_UP);
+                    m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT);
+                    break;
+
+            }
+        }//end SetDefaultsForType
+
+        internal void Enable(IntPtr pBody, OdeScene pParentScene)
+        {
+            if (m_type == Vehicle.TYPE_NONE)
+                return;
+
+            m_body = pBody;
+        }
+
+
+                internal void Halt()
+                {       // Kill all motions, when non-physical
+                        m_linearMotorDirection = Vector3.Zero;
+                        m_lLinMotorDVel = Vector3.Zero;
+                        m_lLinObjectVel = Vector3.Zero;                                         
+                        m_wLinObjectVel = Vector3.Zero;
+                        m_angularMotorDirection = Vector3.Zero;         
+                        m_lastAngularVelocity = Vector3.Zero;
+                        m_angularMotorDVel = Vector3.Zero;      
+                }
+                
+                private void UpdateLinDecay()
+                {
+                        if (Math.Abs(m_linearMotorDirection.X) > Math.Abs(m_lLinMotorDVel.X)) m_lLinMotorDVel.X = m_linearMotorDirection.X;
+                        if (Math.Abs(m_linearMotorDirection.Y) > Math.Abs(m_lLinMotorDVel.Y)) m_lLinMotorDVel.Y = m_linearMotorDirection.Y;
+                        if (Math.Abs(m_linearMotorDirection.Z) > Math.Abs(m_lLinMotorDVel.Z)) m_lLinMotorDVel.Z = m_linearMotorDirection.Z;
+                } // else let the motor decay on its own
+
+                private void UpdateAngDecay()
+                {
+                        if (Math.Abs(m_angularMotorDirection.X) > Math.Abs(m_angularMotorDVel.X)) m_angularMotorDVel.X = m_angularMotorDirection.X;
+                        if (Math.Abs(m_angularMotorDirection.Y) > Math.Abs(m_angularMotorDVel.Y)) m_angularMotorDVel.Y = m_angularMotorDirection.Y;
+                        if (Math.Abs(m_angularMotorDirection.Z) > Math.Abs(m_angularMotorDVel.Z)) m_angularMotorDVel.Z = m_angularMotorDirection.Z;
+                } // else let the motor decay on its own
+        
+        public void Move(float timestep)
+        {
+            float fx = 0;
+            float fy = 0;
+            float fz = 0;
+
+            frcount++;                                  // used to limit debug comment output
+            if (frcount > 100)
+                frcount = 0;
+                
+            if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim)             // KF: Only move root prims.
+            {
+                
+//  Old public void UpdatePositionAndVelocity(), more accuratley calculated here
+                bool lastZeroFlag = _zeroFlag;  // was it stopped
+                d.Vector3 vec = d.BodyGetPosition(Body);
+                d.Quaternion ori = d.BodyGetQuaternion(Body);
+                d.Vector3 vel = d.BodyGetLinearVel(Body);
+      //              d.Vector3 rotvel = d.BodyGetAngularVel(Body);
+                d.Vector3 torque = d.BodyGetTorque(Body);
+                _torque = new Vector3(torque.X, torque.Y, torque.Z);
+                Vector3 l_position = Vector3.Zero;
+                Quaternion l_orientation = Quaternion.Identity;
+
+                m_lastposition = _position;
+                m_lastorientation = _orientation;
+                    
+                l_position.X = vec.X;
+                l_position.Y = vec.Y;
+                l_position.Z = vec.Z;
+                l_orientation.X = ori.X;
+                l_orientation.Y = ori.Y;
+                l_orientation.Z = ori.Z;
+                l_orientation.W = ori.W;
+//Console.WriteLine("Move {0}  at  {1}", m_primName, l_position);        
+                    
+                                // Check if outside region horizontally
+                if (l_position.X > ((int)_parent_scene.WorldExtents.X - 0.05f) || 
+                        l_position.X < 0f || 
+                        l_position.Y > ((int)_parent_scene.WorldExtents.Y - 0.05f) || 
+                        l_position.Y < 0f)
+                {
+                    if (m_crossingfailures < _parent_scene.geomCrossingFailuresBeforeOutofbounds)
+                    {   // keep trying to cross?
+                        _position = l_position;
+                        //_parent_scene.remActivePrim(this);
+                        if (_parent == null) base.RequestPhysicsterseUpdate();
+                        return;         // Dont process any other motion?
+                    }
+                    else
+                    {   // Too many tries
+                        if (_parent == null) base.RaiseOutOfBounds(l_position);
+                        return;         // Dont process any other motion?
+                    }
+                }    // end outside region horizontally
+
+                if (l_position.Z < 0)
+                {
+                    // This is so prim that get lost underground don't fall forever and suck up
+                    //
+                    // Sim resources and memory.
+                    // Disables the prim's movement physics....
+                    // It's a hack and will generate a console message if it fails.
+
+                    //IsPhysical = false;
+                    if (_parent == null) base.RaiseOutOfBounds(_position);
+                    
+                    _acceleration.X = 0;                        // This stuff may stop client display but it has no
+                    _acceleration.Y = 0;                        // effect on the object in phys engine!
+                    _acceleration.Z = 0;
+
+                    _velocity.X = 0;
+                    _velocity.Y = 0;
+                    _velocity.Z = 0;
+                    m_rotationalVelocity.X = 0;
+                    m_rotationalVelocity.Y = 0;
+                    m_rotationalVelocity.Z = 0;
+
+                    if (_parent == null) base.RequestPhysicsterseUpdate();
+
+                    m_throttleUpdates = false;
+                    throttleCounter = 0;
+                    _zeroFlag = true;
+                    //outofBounds = true;
+                }  // end neg Z check
+
+                                // Is it moving?
+                if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02)
+                    && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02)
+                    && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02)
+                    && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.0001))  // KF 0.01 is far to large
+                {
+                    _zeroFlag = true;
+//Console.WriteLine("ZFT 2");                        
+                    m_throttleUpdates = false;
+                }
+                else
+                {
+                    //m_log.Debug(Math.Abs(m_lastposition.X - l_position.X).ToString());
+                    _zeroFlag = false;
+                    m_lastUpdateSent = false;
+                    //m_throttleUpdates = false;
+                }
+
+                if (_zeroFlag)
+                {               // Its stopped
+                    _velocity.X = 0.0f;
+                    _velocity.Y = 0.0f;
+                    _velocity.Z = 0.0f;
+
+                    _acceleration.X = 0;
+                    _acceleration.Y = 0;
+                    _acceleration.Z = 0;
+                    //_orientation.w = 0f;
+                    //_orientation.X = 0f;
+                    //_orientation.Y = 0f;
+                    //_orientation.Z = 0f;
+                    m_rotationalVelocity.X = 0;
+                    m_rotationalVelocity.Y = 0;
+                    m_rotationalVelocity.Z = 0;
+                    if (!m_lastUpdateSent)
+                    {
+                        m_throttleUpdates = false;
+                        throttleCounter = 0;
+                        if (_parent == null)
+                        {
+                            base.RequestPhysicsterseUpdate();
+                        }
+
+                        m_lastUpdateSent = true;
+                    }
+                }
+                else
+                {                       // Its moving
+                    if (lastZeroFlag != _zeroFlag)
+                    {
+                        if (_parent == null)
+                        {
+                            base.RequestPhysicsterseUpdate();
+                        }
+                    }
+
+                    m_lastVelocity = _velocity;
+
+                    _position = l_position;
+
+                    _velocity.X = vel.X;
+                    _velocity.Y = vel.Y;
+                    _velocity.Z = vel.Z;
+// Why 2 calcs???
+//                  _acceleration = ((_velocity - m_lastVelocity) / 0.1f);
+//                  _acceleration = new Vector3(_velocity.X - m_lastVelocity.X / 0.1f, 
+//                                                                              _velocity.Y - m_lastVelocity.Y / 0.1f, 
+//                                                                              _velocity.Z - m_lastVelocity.Z / 0.1f);
+                                                                                
+                    _acceleration = ((_velocity - m_lastVelocity) / timestep);
+                                                                                                        
+                    _orientation.X = ori.X;
+                    _orientation.Y = ori.Y;
+                    _orientation.Z = ori.Z;
+                    _orientation.W = ori.W;
+                    m_lastUpdateSent = false;
+                    if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate)
+                    {
+                        if (_parent == null)
+                        {
+                            base.RequestPhysicsterseUpdate();
+                        }
+                    }
+                    else
+                    {
+                        throttleCounter++;
+                    }
+                }
+                m_lastposition = l_position;
+             
+                                /// End of old   UpdatePositionAndVelocity insert         
+            
+//if (!Acceleration.ApproxEquals(Vector3.Zero, 0.01f)) Console.WriteLine("Move " +  m_primName + "  Accel=" + Acceleration);            
+// if(frcount == 0) Console.WriteLine("Move " +  m_primName + "  VTyp " + m_type +
+//                                              "    usePID=" + m_usePID  + "    seHover=" + m_useHoverPID  + "  useAPID=" + m_useAPID);                
+                if (m_type != Vehicle.TYPE_NONE)
+                {
+                        // get body attitude
+                            d.Quaternion rot = d.BodyGetQuaternion(Body);
+                                Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);   // rotq = rotation of object
+                                Quaternion irotq = Quaternion.Inverse(rotq);
+                                
+                                        // VEHICLE Linear Motion
+                                        d.Vector3 velnow = d.BodyGetLinearVel(Body);                                    // this is in world frame
+                                        Vector3 vel_now = new Vector3(velnow.X, velnow.Y, velnow.Z);
+                                m_lLinObjectVel = vel_now * irotq;
+                
+                            if (m_linearMotorDecayTimescale < 300.0f) //setting of 300 or more disables decay rate
+                            {
+                                if ( Vector3.Mag(m_lLinMotorDVel) < 1.0f)
+                                {
+                                                float decayfactor = m_linearMotorDecayTimescale/timestep;
+                                        Vector3 decayAmount = (m_lLinMotorDVel/decayfactor);
+                                        m_lLinMotorDVel -= decayAmount;
+                                                }
+                                                else
+                                                {
+                                                float decayfactor = 3.0f - (0.57f * (float)Math.Log((double)(m_linearMotorDecayTimescale)));
+                                                        Vector3 decel = Vector3.Normalize(m_lLinMotorDVel) * decayfactor * timestep;
+                                                        m_lLinMotorDVel -= decel;
+                                                }
+                                                if (m_lLinMotorDVel.ApproxEquals(Vector3.Zero, 0.01f))
+                                                {
+                                                        m_lLinMotorDVel = Vector3.Zero;
+                                                }
+                                                else
+                                {
+                                                if (Math.Abs(m_lLinMotorDVel.X) <  Math.Abs(m_lLinObjectVel.X)) m_lLinObjectVel.X = m_lLinMotorDVel.X;
+                                            if (Math.Abs(m_lLinMotorDVel.Y) <  Math.Abs(m_lLinObjectVel.Y)) m_lLinObjectVel.Y = m_lLinMotorDVel.Y;
+                                                if (Math.Abs(m_lLinMotorDVel.Z) <  Math.Abs(m_lLinObjectVel.Z)) m_lLinObjectVel.Z = m_lLinMotorDVel.Z;
+                                            }
+                                        }  // end linear motor decay
+        
+                            if ( (! m_lLinMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (! m_lLinObjectVel.ApproxEquals(Vector3.Zero, 0.01f)) )
+                            {
+                                if(!d.BodyIsEnabled (Body))  d.BodyEnable (Body);
+                                if (m_linearMotorTimescale < 300.0f)
+                                {       
+                                        Vector3 attack_error = m_lLinMotorDVel - m_lLinObjectVel;       
+                                        float linfactor = m_linearMotorTimescale/timestep;
+                                        Vector3 attackAmount = (attack_error/linfactor) * 1.3f;
+                                        m_lLinObjectVel += attackAmount;
+                                }
+                                        if (m_linearFrictionTimescale.X < 300.0f)
+                                        {
+                                                float fricfactor = m_linearFrictionTimescale.X / timestep;
+                                                float fricX = m_lLinObjectVel.X / fricfactor;
+                                                m_lLinObjectVel.X -= fricX;
+                                            }
+                                        if (m_linearFrictionTimescale.Y < 300.0f)
+                                        {
+                                                float fricfactor = m_linearFrictionTimescale.Y / timestep;
+                                                float fricY = m_lLinObjectVel.Y / fricfactor;
+                                                m_lLinObjectVel.Y -= fricY;
+                                            }
+                                        if (m_linearFrictionTimescale.Z < 300.0f)
+                                        {
+                                                float fricfactor = m_linearFrictionTimescale.Z / timestep;
+//if(frcount == 0) Console.WriteLine("Zfric={0}", fricfactor);
+                                                float fricZ = m_lLinObjectVel.Z / fricfactor;
+                                                m_lLinObjectVel.Z -= fricZ;
+                                            }
+                                        }
+                                    m_wLinObjectVel = m_lLinObjectVel * rotq;
+                                    
+                                        // Gravity and Buoyancy
+                            Vector3 grav = Vector3.Zero;
+                                        if(m_VehicleBuoyancy < 1.0f)
+                                        {
+                                                // There is some gravity, make a gravity force vector
+                                                // that is applied after object velocity.     
+                                    d.Mass objMass;
+                                    d.BodyGetMass(Body, out objMass);
+                                    // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g; 
+                                    grav.Z = _parent_scene.gravityz * objMass.mass * (1f - m_VehicleBuoyancy); // Applied later as a force
+                                } // else its 1.0, no gravity.
+                
+                                // Hovering
+                                if( (m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0)
+                                {       
+                                        // We should hover, get the target height
+                                        d.Vector3 pos = d.BodyGetPosition(Body);
+                                        if((m_flags & VehicleFlag.HOVER_WATER_ONLY) == VehicleFlag.HOVER_WATER_ONLY)
+                                        {
+                                                m_VhoverTargetHeight = _parent_scene.GetWaterLevel() + m_VhoverHeight;
+                                        }
+                                        else if((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) == VehicleFlag.HOVER_TERRAIN_ONLY)
+                                        {
+                                                m_VhoverTargetHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y) + m_VhoverHeight;
+                                        }
+                                        else if((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == VehicleFlag.HOVER_GLOBAL_HEIGHT)
+                                        {
+                                                m_VhoverTargetHeight = m_VhoverHeight;
+                                        }
+                        
+                                                if((m_flags & VehicleFlag.HOVER_UP_ONLY) == VehicleFlag.HOVER_UP_ONLY)
+                                                {
+                                                        // If body is aready heigher, use its height as target height
+                                                        if(pos.Z > m_VhoverTargetHeight) m_VhoverTargetHeight = pos.Z;
+                                                }
+                                
+//                  m_VhoverEfficiency = 0f;    // 0=boucy, 1=Crit.damped
+//                              m_VhoverTimescale = 0f;         // time to acheive height
+//                              timestep  is time since last frame,in secs 
+                                                float herr0 = pos.Z - m_VhoverTargetHeight;
+                                                // Replace Vertical speed with correction figure if significant
+                                                if(Math.Abs(herr0) > 0.01f )
+                                                {
+                                  //?          d.Mass objMass;
+                                  //?          d.BodyGetMass(Body, out objMass);
+                                                        m_wLinObjectVel.Z = - ( (herr0 * timestep * 50.0f) / m_VhoverTimescale);
+                                                        //KF: m_VhoverEfficiency is not yet implemented
+                                                }
+                                                else
+                                                {
+                                                        m_wLinObjectVel.Z = 0f;
+                                                }
+                                        }
+                                        else
+                                        {       // not hovering, Gravity rules
+                                                m_wLinObjectVel.Z = vel_now.Z;
+                            }   
+                            
+                            
+                            // Vehicle Linear  Motion done =======================================
+                                // Apply velocity
+                                d.BodySetLinearVel(Body, m_wLinObjectVel.X, m_wLinObjectVel.Y, m_wLinObjectVel.Z);        
+                            // apply gravity force
+                                        d.BodyAddForce(Body, grav.X, grav.Y, grav.Z);           
+//if(frcount == 0) Console.WriteLine("Grav {0}", grav);
+                                // end MoveLinear()
+
+                                        
+                                    // MoveAngular
+                                /*
+                                private Vector3 m_angularMotorDirection = Vector3.Zero;                 // angular velocity requested by LSL motor 
+        
+                                private float m_angularMotorTimescale = 0;                                              // motor angular Attack rate set by LSL
+                                private float m_angularMotorDecayTimescale = 0;                                 // motor angular Decay rate set by LSL
+                                private Vector3 m_angularFrictionTimescale = Vector3.Zero;              // body angular Friction set by LSL
+
+                                private Vector3 m_angularMotorDVel = Vector3.Zero;                              // decayed angular motor
+                                private Vector3 m_angObjectVel = Vector3.Zero;                                  // what was last applied to body
+                                        */
+//if(frcount == 0) Console.WriteLine("MoveAngular ");   
+        
+                                d.Vector3 angularObjectVel = d.BodyGetAngularVel(Body);
+                                Vector3 angObjectVel = new Vector3(angularObjectVel.X, angularObjectVel.Y, angularObjectVel.Z);
+                                angObjectVel = angObjectVel * irotq;    // ============ Converts to LOCAL rotation
+                
+//if(frcount == 0) Console.WriteLine("V0 = {0}", angObjectVel);         
+
+                                // Decay Angular Motor 1. In SL this also depends on attack rate! decay ~= 23/Attack.
+                                float atk_decayfactor = 23.0f  / (m_angularMotorTimescale * timestep); 
+                                m_angularMotorDVel -= m_angularMotorDVel / atk_decayfactor;
+                                // Decay Angular Motor 2.
+                                if (m_angularMotorDecayTimescale < 300.0f)
+                                {
+                                if ( Vector3.Mag(m_angularMotorDVel) < 1.0f)
+                                {
+                                                        float decayfactor = (m_angularMotorDecayTimescale)/timestep;
+                                            Vector3 decayAmount = (m_angularMotorDVel/decayfactor);     
+                                            m_angularMotorDVel -= decayAmount;                  
+                                                }
+                                                else
+                                                {
+                                                        Vector3 decel = Vector3.Normalize(m_angularMotorDVel) * timestep / m_angularMotorDecayTimescale;
+                                                        m_angularMotorDVel -= decel;
+                                                }
+                
+                                                if (m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f))
+                                                {
+                                                        m_angularMotorDVel = Vector3.Zero;
+                                                }
+                                                else
+                                        {
+                                                if (Math.Abs(m_angularMotorDVel.X) <  Math.Abs(angObjectVel.X)) angObjectVel.X = m_angularMotorDVel.X;
+                                            if (Math.Abs(m_angularMotorDVel.Y) <  Math.Abs(angObjectVel.Y)) angObjectVel.Y = m_angularMotorDVel.Y;
+                                                if (Math.Abs(m_angularMotorDVel.Z) <  Math.Abs(angObjectVel.Z)) angObjectVel.Z = m_angularMotorDVel.Z;
+                                            }           
+                                } // end decay angular motor
+//if(frcount == 0) Console.WriteLine("MotorDvel {0}    Obj {1}", m_angularMotorDVel, angObjectVel);
+
+//if(frcount == 0) Console.WriteLine("VA = {0}", angObjectVel);   
+
+                            // Vertical attractor section
+                                        Vector3 vertattr = Vector3.Zero;
+            
+                                        if(m_verticalAttractionTimescale < 300)
+                                        {
+                                    float VAservo = 1.0f / (m_verticalAttractionTimescale * timestep);
+                                    // make a vector pointing up
+                                                Vector3 verterr = Vector3.Zero;
+                                                verterr.Z = 1.0f;
+                                                // rotate it to Body Angle
+                                                verterr = verterr * rotq;
+                                                // verterr.X and .Y are the World error ammounts. They are 0 when there is no error (Vehicle Body is 'vertical'), and .Z will be 1.
+                                                // As the body leans to its side |.X| will increase to 1 and .Z fall to 0. As body inverts |.X| will fall and .Z will go
+                                                // negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body.
+                                
+                                                if (verterr.Z < 0.0f)
+                                                {       // Deflection from vertical exceeds 90-degrees. This method will ensure stable return to
+                                                        // vertical, BUT for some reason a z-rotation is imparted to the object. TBI.
+//Console.WriteLine("InvertFlip");      
+                                                        verterr.X = 2.0f - verterr.X;
+                                                        verterr.Y = 2.0f - verterr.Y;
+                                                }
+                                                verterr *= 0.5f;
+                                                // verterror is 0 (no error) to +/- 1 (max error at 180-deg tilt)
+        
+                                                if ((!angObjectVel.ApproxEquals(Vector3.Zero, 0.001f)) || (verterr.Z < 0.49f))
+                                                {
+                                                        // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so 
+                                                        // Change  Body angular velocity  X based on Y, and Y based on X. Z is not changed.
+                                                        vertattr.X =    verterr.Y;
+                                                        vertattr.Y =  - verterr.X;
+                                                        vertattr.Z = 0f;
+//if(frcount == 0) Console.WriteLine("VAerr=" + verterr);       
+                        
+                                                        // scaling appears better usingsquare-law
+                                                        float damped = m_verticalAttractionEfficiency * m_verticalAttractionEfficiency;
+                                                        float bounce = 1.0f - damped;  
+                                                        // 0 = crit damp, 1 = bouncy
+                                                        float oavz = angObjectVel.Z;   // retain z velocity
+                                                        // time-scaled correction, which sums, therefore is bouncy:
+                                                        angObjectVel = (angObjectVel + (vertattr * VAservo * 0.0333f)) * bounce; 
+                                                        // damped, good @ < 90:
+                                                        angObjectVel = angObjectVel + (vertattr * VAservo *  0.0667f * damped); 
+                                                        angObjectVel.Z = oavz;
+//if(frcount == 0) Console.WriteLine("VA+");                                    
+//Console.WriteLine("VAttr {0}         OAvel {1}", vertattr, angObjectVel);
+                                                }
+                                                else
+                                                {
+                                                        // else error is very small
+                                                        angObjectVel.X = 0f;
+                                                        angObjectVel.Y = 0f;
+//if(frcount == 0) Console.WriteLine("VA0");                                    
+                                                }
+                                        } // else vertical attractor is off
+//if(frcount == 0) Console.WriteLine("V1 = {0}", angObjectVel);         
+
+                            if ( (! m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (! angObjectVel.ApproxEquals(Vector3.Zero, 0.01f)) )
+                            {  // if motor or object have motion
+                                if(!d.BodyIsEnabled (Body))  d.BodyEnable (Body);
+                
+                                if (m_angularMotorTimescale < 300.0f)
+                                {       
+                                        Vector3 attack_error = m_angularMotorDVel - angObjectVel;       
+                                        float angfactor = m_angularMotorTimescale/timestep;
+                                        Vector3 attackAmount = (attack_error/angfactor);
+                                        angObjectVel += attackAmount;
+//if(frcount == 0) Console.WriteLine("Accel {0}      Attk {1}",FrAaccel, attackAmount);                 
+//if(frcount == 0) Console.WriteLine("V2+= {0}", angObjectVel);         
+                                }
+                
+                                        angObjectVel.X -= angObjectVel.X / (m_angularFrictionTimescale.X * 0.7f / timestep);
+                                        angObjectVel.Y -= angObjectVel.Y / (m_angularFrictionTimescale.Y * 0.7f / timestep);
+                                        angObjectVel.Z -= angObjectVel.Z / (m_angularFrictionTimescale.Z * 0.7f / timestep);
+                                        } // else no signif. motion
+                        
+//if(frcount == 0) Console.WriteLine("Dmotor {0}      Obj {1}", m_angularMotorDVel, angObjectVel);
+                                        // Bank section tba
+                                        // Deflection section tba
+//if(frcount == 0) Console.WriteLine("V3 = {0}", angObjectVel);         
+                        
+                                        m_lastAngularVelocity = angObjectVel;
+
+                                    if (!m_angularEnable.ApproxEquals(Vector3.One, 0.003f))
+                                {
+                                if (m_angularEnable.X == 0)
+                                        m_lastAngularVelocity.X = 0f;
+                                    if (m_angularEnable.Y == 0)
+                                        m_lastAngularVelocity.Y = 0f;
+                                    if (m_angularEnable.Z == 0)
+                                        m_lastAngularVelocity.Z = 0f;
+                                }        
+                                        // Apply to the body
+// Vector3 aInc = m_lastAngularVelocity - initavel;
+//if(frcount == 0) Console.WriteLine("Inc {0}", aInc);  
+                                        m_lastAngularVelocity = m_lastAngularVelocity * rotq; // ================ Converts to WORLD rotation
+
+                                        d.BodySetAngularVel (Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z);
+//if(frcount == 0) Console.WriteLine("V4 = {0}", m_lastAngularVelocity);                
+                            
+                }  // end VEHICLES
+                else
+                {
+                        if(!d.BodyIsEnabled (Body))  d.BodyEnable (Body); // KF add 161009
+                        // NON-'VEHICLES' are dealt with here
+                                        /// Dynamics Angular Lock ========================================================================
+                        if (d.BodyIsEnabled(Body) && !m_angularEnable.ApproxEquals(Vector3.One, 0.003f))
+                        {
+                            d.Vector3 avel2 = d.BodyGetAngularVel(Body);
+                            if (m_angularEnable.X == 0)
+                                avel2.X = 0;
+                            if (m_angularEnable.Y == 0)
+                                avel2.Y = 0;
+                            if (m_angularEnable.Z == 0)
+                                avel2.Z = 0;
+                            d.BodySetAngularVel(Body, avel2.X, avel2.Y, avel2.Z);
+                        }
+
+
+                                        /// Dynamics Buoyancy ===============================================================================                   
+                        //KF: m_buoyancy is set by llSetBuoyancy() and is for non-vehicle.
+                        // m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up 
+                        // NB Prims in ODE are no subject to global gravity
+                        float m_mass = CalculateMass();
+                        fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass; // force = acceleration * mass
+
+                        if (m_usePID)
+                        {
+//if(frcount == 0) Console.WriteLine("PID " +  m_primName);             
+                        // KF - this is for object MoveToTarget.
+                        
+                            //if (!d.BodyIsEnabled(Body))
+                            //d.BodySetForce(Body, 0f, 0f, 0f);
+
+                            //  no lock; for now it's only called from within Simulate()
+        
+                            // If the PID Controller isn't active then we set our force
+                            // calculating base velocity to the current position
+
+                            if ((m_PIDTau < 1) && (m_PIDTau != 0))
+                            {
+                                //PID_G = PID_G / m_PIDTau;
+                                m_PIDTau = 1;
+                            }
+        
+                            if ((PID_G - m_PIDTau) <= 0)
+                            {
+                                PID_G = m_PIDTau + 1;
+                            }
+                            //PidStatus = true;
+
+                            // PhysicsVector vec = new PhysicsVector();
+//                          d.Vector3 vel = d.BodyGetLinearVel(Body);
+
+                            d.Vector3 pos = d.BodyGetPosition(Body);
+                            _target_velocity =
+                            new Vector3(
+                                    (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep),
+                                    (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep),
+                                    (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep)
+                                    );
+
+                            //  if velocity is zero, use position control; otherwise, velocity control
+
+                            if (_target_velocity.ApproxEquals(Vector3.Zero,0.1f))
+                            {
+                                //  keep track of where we stopped.  No more slippin' & slidin'
+        
+                                // We only want to deactivate the PID Controller if we think we want to have our surrogate
+                                // react to the physics scene by moving it's position.
+                                // Avatar to Avatar collisions
+                                // Prim to avatar collisions
+
+                                //fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2);
+                                //fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2);
+                                //fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P;
+                                d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);
+                                d.BodySetLinearVel(Body, 0, 0, 0);
+                                d.BodyAddForce(Body, 0, 0, fz);
+                        //        return;
+                            }
+                            else
+                            {
+                                _zeroFlag = false;
+
+                                // We're flying and colliding with something
+                                fx = ((_target_velocity.X) - vel.X) * (PID_D);
+                                fy = ((_target_velocity.Y) - vel.Y) * (PID_D);
+        
+                                // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P;
+
+                                fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
+                            }
+                        }               // end if (m_usePID)
+                        
+                        /// Dynamics Hover ===================================================================================
+                        // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller
+                        if (m_useHoverPID && !m_usePID)
+                        {
+//Console.WriteLine("Hover " +  m_primName);            
+                        
+                            // If we're using the PID controller, then we have no gravity
+                            fz = (-1 * _parent_scene.gravityz) * m_mass;
+
+                            //  no lock; for now it's only called from within Simulate()
+
+                            // If the PID Controller isn't active then we set our force
+                            // calculating base velocity to the current position
+
+                            if ((m_PIDTau < 1))
+                            {
+                                PID_G = PID_G / m_PIDTau;
+                            }
+
+                            if ((PID_G - m_PIDTau) <= 0)
+                            {
+                                PID_G = m_PIDTau + 1;
+                            }
+                    
+
+                            // Where are we, and where are we headed?
+                            d.Vector3 pos = d.BodyGetPosition(Body);
+//                          d.Vector3 vel = d.BodyGetLinearVel(Body);
+
+
+                                                //    Non-Vehicles have a limited set of Hover options.
+                            // determine what our target height really is based on HoverType
+                            switch (m_PIDHoverType)
+                            {
+                                case PIDHoverType.Ground:
+                                    m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
+                                    m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
+                                    break;
+                                case PIDHoverType.GroundAndWater:
+                                    m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
+                                    m_waterHeight  = _parent_scene.GetWaterLevel();
+                                    if (m_groundHeight > m_waterHeight)
+                                    {
+                                        m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
+                                    }
+                                    else
+                                    {
+                                        m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
+                                    }
+                                    break;
+
+                            }   // end switch (m_PIDHoverType)
+
+
+                            _target_velocity =
+                            new Vector3(0.0f, 0.0f,
+                                (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep)
+                                );
+
+                        //  if velocity is zero, use position control; otherwise, velocity control
+
+                        if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f))
+                        {
+                            //  keep track of where we stopped.  No more slippin' & slidin'
+        
+                            // We only want to deactivate the PID Controller if we think we want to have our surrogate
+                            // react to the physics scene by moving it's position.
+                            // Avatar to Avatar collisions
+                            // Prim to avatar collisions
+
+                            d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight);
+                            d.BodySetLinearVel(Body, vel.X, vel.Y, 0);
+                            d.BodyAddForce(Body, 0, 0, fz);
+                    //KF this prevents furthur motions        return;
+                        }
+                        else
+                        {
+                            _zeroFlag = false;
+
+                            // We're flying and colliding with something
+                            fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
+                        }
+                    } // end m_useHoverPID && !m_usePID
+                    
+                    /// Dynamics RotLookAt =================================================================================
+                        if (m_useAPID)
+                        {
+                                // RotLookAt, apparently overrides all other rotation sources. Inputs:
+                                // Quaternion m_APIDTarget
+                                                // float m_APIDStrength         // From SL experiments, this is the time to get there
+                                                // float m_APIDDamping          // From SL experiments, this is damping, 1.0 = damped, 0.1 = wobbly
+                                                                                                        // Also in SL the mass of the object has no effect on time to get there.
+                                                // Factors:
+                                    // get present body rotation
+                                    float limit = 1.0f;
+                                    float scaler = 50f;         // adjusts damping time
+                                    float RLAservo = 0f;
+                                    
+                                    d.Quaternion rot = d.BodyGetQuaternion(Body);
+                                    Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);
+                                    Quaternion rot_diff = Quaternion.Inverse(rotq) * m_APIDTarget;
+                        float diff_angle;
+                        Vector3 diff_axis;
+                        rot_diff.GetAxisAngle(out diff_axis, out diff_angle);
+                        diff_axis.Normalize();
+                                                if(diff_angle > 0.01f)                  // diff_angle is always +ve
+                                                {
+//                              PhysicsVector rotforce = new PhysicsVector(diff_axis.X, diff_axis.Y, diff_axis.Z);
+                                Vector3 rotforce = new Vector3(diff_axis.X, diff_axis.Y, diff_axis.Z);
+                            rotforce = rotforce * rotq;
+                            if(diff_angle > limit) diff_angle = limit;          // cap the rotate rate
+//                          RLAservo = timestep / m_APIDStrength * m_mass * scaler;
+  //                        rotforce = rotforce * RLAservo * diff_angle ;
+    //                      d.BodyAddRelTorque(Body, rotforce.X, rotforce.Y, rotforce.Z);
+                            RLAservo = timestep / m_APIDStrength * scaler;
+                            rotforce = rotforce * RLAservo * diff_angle ;
+                            
+                                    if (m_angularEnable.X == 0)
+                                        rotforce.X = 0;
+                                    if (m_angularEnable.Y == 0)
+                                        rotforce.Y = 0;
+                                    if (m_angularEnable.Z == 0)
+                                        rotforce.Z = 0;
+                            
+                                                        d.BodySetAngularVel (Body,  rotforce.X, rotforce.Y, rotforce.Z);
+//Console.WriteLine("axis= " + diff_axis + "    angle= " + diff_angle + "servo= " + RLAservo);                                                  
+                                                }
+//if(frcount == 0) Console.WriteLine("mass= " + m_mass + "  servo= " + RLAservo + "   angle= " + diff_angle);                                                   
+                        } // end m_useAPID
+                        
+                        /// Dynamics Apply Forces ===================================================================================
+                    fx *= m_mass;
+                    fy *= m_mass;
+                    //fz *= m_mass;
+
+                    fx += m_force.X;
+                    fy += m_force.Y;
+                    fz += m_force.Z;
+
+                    //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString());
+                    if (fx != 0 || fy != 0 || fz != 0)
+                    {
+                        //m_taintdisable = true;
+                        //base.RaiseOutOfBounds(Position);
+                        //d.BodySetLinearVel(Body, fx, fy, 0f);
+                        if (!d.BodyIsEnabled(Body))
+                        {
+                                // A physical body at rest on a surface will auto-disable after a while,
+                                // this appears to re-enable it incase the surface it is upon vanishes,
+                                // and the body should fall again. 
+                            d.BodySetLinearVel(Body, 0f, 0f, 0f);
+                            d.BodySetForce(Body, 0, 0, 0);
+                            enableBodySoft();
+                        }
+
+                        // 35x10 = 350n times the mass per second applied maximum.
+                        float nmax = 35f * m_mass;
+                        float nmin = -35f * m_mass;
+
+                    
+                        if (fx > nmax)
+                            fx = nmax;
+                        if (fx < nmin)
+                            fx = nmin;
+                        if (fy > nmax)
+                            fy = nmax;
+                        if (fy < nmin)
+                            fy = nmin;
+                        d.BodyAddForce(Body, fx, fy, fz);
+//Console.WriteLine("AddForce " + fx + "," + fy + "," + fz);                            
+                    }
+                                }               
+            }
+            else
+            {   // is not physical, or is not a body or is selected
+                     // from old UpdatePositionAndVelocity, ... Not a body..   so Make sure the client isn't interpolating
+                    _velocity.X = 0;
+                    _velocity.Y = 0;
+                    _velocity.Z = 0;
+
+                    _acceleration.X = 0;
+                    _acceleration.Y = 0;
+                    _acceleration.Z = 0;
+
+                    m_rotationalVelocity.X = 0;
+                    m_rotationalVelocity.Y = 0;
+                    m_rotationalVelocity.Z = 0;
+                    _zeroFlag = true;
+               return;
+                        }   
+        }  // end Move()
+        } // end class
+}
diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODERayCastRequestManager.cs b/OpenSim/Region/Physics/ChOdePlugin/ODERayCastRequestManager.cs
new file mode 100644
index 0000000..7314107
--- /dev/null
+++ b/OpenSim/Region/Physics/ChOdePlugin/ODERayCastRequestManager.cs
@@ -0,0 +1,375 @@
+/*
+ * Copyright (c) Contributors, http://opensimulator.org/
+ * See CONTRIBUTORS.TXT for a full list of copyright holders.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the OpenSimulator Project nor the
+ *       names of its contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+using System;
+using System.Collections.Generic;
+using System.Reflection;
+using System.Runtime.InteropServices;
+using System.Text;
+using OpenMetaverse;
+using OpenSim.Region.Physics.Manager;
+using Ode.NET;
+using log4net;
+
+namespace OpenSim.Region.Physics.OdePlugin
+{
+    /// <summary>
+    /// Processes raycast requests as ODE is in a state to be able to do them.
+    /// This ensures that it's thread safe and there will be no conflicts.
+    /// Requests get returned by a different thread then they were requested by.
+    /// </summary>
+    public class ODERayCastRequestManager
+    {
+        /// <summary>
+        /// Pending Raycast Requests
+        /// </summary>
+        protected List<ODERayCastRequest> m_PendingRequests = new List<ODERayCastRequest>();
+
+        /// <summary>
+        /// Scene that created this object.
+        /// </summary>
+        private OdeScene m_scene;
+
+        /// <summary>
+        /// ODE contact array to be filled by the collision testing
+        /// </summary>
+        d.ContactGeom[] contacts = new d.ContactGeom[5];
+
+        /// <summary>
+        /// ODE near callback delegate
+        /// </summary>
+        private d.NearCallback nearCallback;
+        private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
+        private List<ContactResult> m_contactResults = new List<ContactResult>();
+
+
+        public ODERayCastRequestManager(OdeScene pScene)
+        {
+            m_scene = pScene;
+            nearCallback = near;
+            
+        }
+
+        /// <summary>
+        /// Queues a raycast
+        /// </summary>
+        /// <param name="position">Origin of Ray</param>
+        /// <param name="direction">Ray normal</param>
+        /// <param name="length">Ray length</param>
+        /// <param name="retMethod">Return method to send the results</param>
+        public void QueueRequest(Vector3 position, Vector3 direction, float length, RaycastCallback retMethod)
+        {
+            lock (m_PendingRequests)
+            {
+                ODERayCastRequest req = new ODERayCastRequest();
+                req.callbackMethod = retMethod;
+                req.length = length;
+                req.Normal = direction;
+                req.Origin = position;
+
+                m_PendingRequests.Add(req);
+            }
+        }
+
+        /// <summary>
+        /// Process all queued raycast requests
+        /// </summary>
+        /// <returns>Time in MS the raycasts took to process.</returns>
+        public int ProcessQueuedRequests()
+        {
+            int time = System.Environment.TickCount;
+            lock (m_PendingRequests)
+            {
+                if (m_PendingRequests.Count > 0)
+                {
+                    ODERayCastRequest[] reqs = m_PendingRequests.ToArray();
+                    for (int i = 0; i < reqs.Length; i++)
+                    {
+                        if (reqs[i].callbackMethod != null) // quick optimization here, don't raycast 
+                            RayCast(reqs[i]);               // if there isn't anyone to send results   
+                    }
+                    /*
+                    foreach (ODERayCastRequest req in m_PendingRequests)
+                    {
+                        if (req.callbackMethod != null) // quick optimization here, don't raycast 
+                            RayCast(req);               // if there isn't anyone to send results to
+                            
+                    }
+                    */
+                    m_PendingRequests.Clear();
+                }
+            }
+
+            lock (m_contactResults)
+                m_contactResults.Clear();
+
+            return System.Environment.TickCount - time;
+        }
+
+        /// <summary>
+        /// Method that actually initiates the raycast
+        /// </summary>
+        /// <param name="req"></param>
+        private void RayCast(ODERayCastRequest req)
+        {
+            // Create the ray
+            IntPtr ray = d.CreateRay(m_scene.space, req.length);
+            d.GeomRaySet(ray, req.Origin.X, req.Origin.Y, req.Origin.Z, req.Normal.X, req.Normal.Y, req.Normal.Z);
+
+            // Collide test
+            d.SpaceCollide2(m_scene.space, ray, IntPtr.Zero, nearCallback);
+
+            // Remove Ray
+            d.GeomDestroy(ray);
+
+
+            // Define default results
+            bool hitYN = false;
+            uint hitConsumerID = 0;
+            float distance = 999999999999f;
+            Vector3 closestcontact = new Vector3(99999f, 99999f, 99999f);
+            Vector3 snormal = Vector3.Zero;
+
+            // Find closest contact and object.
+            lock (m_contactResults)
+            {
+                foreach (ContactResult cResult in m_contactResults)
+                {
+                    if (Vector3.Distance(req.Origin, cResult.Pos) < Vector3.Distance(req.Origin, closestcontact))
+                    {
+                        closestcontact = cResult.Pos;
+                        hitConsumerID = cResult.ConsumerID;
+                        distance = cResult.Depth;
+                        hitYN = true;
+                        snormal = cResult.Normal;
+                    }
+                }
+
+                m_contactResults.Clear();
+            }
+
+            // Return results
+            if (req.callbackMethod != null)
+                req.callbackMethod(hitYN, closestcontact, hitConsumerID, distance, snormal);
+        }
+
+        // This is the standard Near.   Uses space AABBs to speed up detection.
+        private void near(IntPtr space, IntPtr g1, IntPtr g2)
+        {
+
+            //Don't test against heightfield Geom, or you'll be sorry!
+            
+            /*
+             terminate called after throwing an instance of 'std::bad_alloc'
+                  what():  std::bad_alloc
+                Stacktrace:
+                 
+                  at (wrapper managed-to-native) Ode.NET.d.Collide (intptr,intptr,int,Ode.NET.d/ContactGeom[],int) <0x00004>
+                  at (wrapper managed-to-native) Ode.NET.d.Collide (intptr,intptr,int,Ode.NET.d/ContactGeom[],int) <0xffffffff>
+                  at OpenSim.Region.Physics.OdePlugin.ODERayCastRequestManager.near (intptr,intptr,intptr) <0x00280>
+                  at (wrapper native-to-managed) OpenSim.Region.Physics.OdePlugin.ODERayCastRequestManager.near (intptr,intptr,intptr) <0xfff
+                fffff>
+                  at (wrapper managed-to-native) Ode.NET.d.SpaceCollide2 (intptr,intptr,intptr,Ode.NET.d/NearCallback) <0x00004>
+                  at (wrapper managed-to-native) Ode.NET.d.SpaceCollide2 (intptr,intptr,intptr,Ode.NET.d/NearCallback) <0xffffffff>
+                  at OpenSim.Region.Physics.OdePlugin.ODERayCastRequestManager.RayCast (OpenSim.Region.Physics.OdePlugin.ODERayCastRequest) <
+                0x00114>
+                  at OpenSim.Region.Physics.OdePlugin.ODERayCastRequestManager.ProcessQueuedRequests () <0x000eb>
+                  at OpenSim.Region.Physics.OdePlugin.OdeScene.Simulate (single) <0x017e6>
+                  at OpenSim.Region.Framework.Scenes.SceneGraph.UpdatePhysics (double) <0x00042>
+                  at OpenSim.Region.Framework.Scenes.Scene.Update () <0x0039e>
+                  at OpenSim.Region.Framework.Scenes.Scene.Heartbeat (object) <0x00019>
+                  at (wrapper runtime-invoke) object.runtime_invoke_void__this___object (object,intptr,intptr,intptr) <0xffffffff>
+                 
+                Native stacktrace:
+                 
+                        mono [0x80d2a42]
+                        [0xb7f5840c]
+                        /lib/i686/cmov/libc.so.6(abort+0x188) [0xb7d1a018]
+                        /usr/lib/libstdc++.so.6(_ZN9__gnu_cxx27__verbose_terminate_handlerEv+0x158) [0xb45fc988]
+                        /usr/lib/libstdc++.so.6 [0xb45fa865]
+                        /usr/lib/libstdc++.so.6 [0xb45fa8a2]
+                        /usr/lib/libstdc++.so.6 [0xb45fa9da]
+                        /usr/lib/libstdc++.so.6(_Znwj+0x83) [0xb45fb033]
+                        /usr/lib/libstdc++.so.6(_Znaj+0x1d) [0xb45fb11d]
+                        libode.so(_ZN13dxHeightfield23dCollideHeightfieldZoneEiiiiP6dxGeomiiP12dContactGeomi+0xd04) [0xb46678e4]
+                        libode.so(_Z19dCollideHeightfieldP6dxGeomS0_iP12dContactGeomi+0x54b) [0xb466832b]
+                        libode.so(dCollide+0x102) [0xb46571b2]
+                        [0x95cfdec9]
+                        [0x8ea07fe1]
+                        [0xab260146]
+                        libode.so [0xb465a5c4]
+                        libode.so(_ZN11dxHashSpace8collide2EPvP6dxGeomPFvS0_S2_S2_E+0x75) [0xb465bcf5]
+                        libode.so(dSpaceCollide2+0x177) [0xb465ac67]
+                        [0x95cf978e]
+                        [0x8ea07945]
+                        [0x95cf2bbc]
+                        [0xab2787e7]
+                        [0xab419fb3]
+                        [0xab416657]
+                        [0xab415bda]
+                        [0xb609b08e]
+                        mono(mono_runtime_delegate_invoke+0x34) [0x8192534]
+                        mono [0x81a2f0f]
+                        mono [0x81d28b6]
+                        mono [0x81ea2c6]
+                        /lib/i686/cmov/libpthread.so.0 [0xb7e744c0]
+                        /lib/i686/cmov/libc.so.6(clone+0x5e) [0xb7dcd6de]
+             */
+
+            // Exclude heightfield geom
+
+            if (g1 == IntPtr.Zero || g2 == IntPtr.Zero)
+                return;
+            if (d.GeomGetClass(g1) == d.GeomClassID.HeightfieldClass || d.GeomGetClass(g2) == d.GeomClassID.HeightfieldClass)
+                return;
+
+            // Raytest against AABBs of spaces first, then dig into the spaces it hits for actual geoms.
+            if (d.GeomIsSpace(g1) || d.GeomIsSpace(g2))
+            {
+                if (g1 == IntPtr.Zero || g2 == IntPtr.Zero)
+                    return;
+                
+                // Separating static prim geometry spaces.
+                // We'll be calling near recursivly if one
+                // of them is a space to find all of the
+                // contact points in the space
+                try
+                {
+                    d.SpaceCollide2(g1, g2, IntPtr.Zero, nearCallback);
+                }
+                catch (AccessViolationException)
+                {
+                    m_log.Warn("[PHYSICS]: Unable to collide test a space");
+                    return;
+                }
+                //Colliding a space or a geom with a space or a geom. so drill down
+
+                //Collide all geoms in each space..
+                //if (d.GeomIsSpace(g1)) d.SpaceCollide(g1, IntPtr.Zero, nearCallback);
+                //if (d.GeomIsSpace(g2)) d.SpaceCollide(g2, IntPtr.Zero, nearCallback);
+                return;
+            }
+
+            if (g1 == IntPtr.Zero || g2 == IntPtr.Zero)
+                return;
+
+            int count = 0;
+            try
+            {
+                
+                if (g1 == g2)
+                    return; // Can't collide with yourself
+
+                lock (contacts)
+                {
+                    count = d.Collide(g1, g2, contacts.GetLength(0), contacts, d.ContactGeom.SizeOf);
+                }
+            }
+            catch (SEHException)
+            {
+                m_log.Error("[PHYSICS]: The Operating system shut down ODE because of corrupt memory.  This could be a result of really irregular terrain.  If this repeats continuously, restart using Basic Physics and terrain fill your terrain.  Restarting the sim.");
+            }
+            catch (Exception e)
+            {
+                m_log.WarnFormat("[PHYSICS]: Unable to collide test an object: {0}", e.Message);
+                return;
+            }
+
+            PhysicsActor p1 = null;
+            PhysicsActor p2 = null;
+
+            if (g1 != IntPtr.Zero)
+                m_scene.actor_name_map.TryGetValue(g1, out p1);
+
+            if (g2 != IntPtr.Zero)
+                m_scene.actor_name_map.TryGetValue(g1, out p2);
+
+            // Loop over contacts, build results.
+            for (int i = 0; i < count; i++)
+            {
+                if (p1 != null) { 
+                    if (p1 is OdePrim)
+                    {
+                        ContactResult collisionresult = new ContactResult();
+                    
+                        collisionresult.ConsumerID = ((OdePrim)p1).m_localID;
+                        collisionresult.Pos = new Vector3(contacts[i].pos.X, contacts[i].pos.Y, contacts[i].pos.Z);
+                        collisionresult.Depth = contacts[i].depth;
+                        collisionresult.Normal = new Vector3(contacts[i].normal.X, contacts[i].normal.Y,
+                                                             contacts[i].normal.Z);
+                        lock (m_contactResults)
+                            m_contactResults.Add(collisionresult);
+                    }
+                }
+
+                if (p2 != null)
+                {
+                    if (p2 is OdePrim)
+                    {
+                        ContactResult collisionresult = new ContactResult();
+
+                        collisionresult.ConsumerID = ((OdePrim)p2).m_localID;
+                        collisionresult.Pos = new Vector3(contacts[i].pos.X, contacts[i].pos.Y, contacts[i].pos.Z);
+                        collisionresult.Depth = contacts[i].depth;
+                        collisionresult.Normal = new Vector3(contacts[i].normal.X, contacts[i].normal.Y,
+                                      contacts[i].normal.Z);
+
+                        lock (m_contactResults)
+                            m_contactResults.Add(collisionresult);
+                    }
+                }
+
+                
+            }
+
+        }
+
+        /// <summary>
+        /// Dereference the creator scene so that it can be garbage collected if needed.
+        /// </summary>
+        internal void Dispose()
+        {
+            m_scene = null;
+        }
+    }
+
+    public struct ODERayCastRequest
+    {
+        public Vector3 Origin;
+        public Vector3 Normal;
+        public float length;
+        public RaycastCallback callbackMethod;
+    }
+
+    public struct ContactResult
+    {
+        public Vector3 Pos;
+        public float Depth;
+        public uint ConsumerID;
+        public Vector3 Normal;
+    }
+}
diff --git a/OpenSim/Region/Physics/PhysXPlugin/PhysXPlugin.cs b/OpenSim/Region/Physics/ChOdePlugin/OdePhysicsJoint.cs
index ca7a4f8..b4a3c48 100644
--- a/OpenSim/Region/Physics/PhysXPlugin/PhysXPlugin.cs
+++ b/OpenSim/Region/Physics/ChOdePlugin/OdePhysicsJoint.cs
@@ -26,51 +26,23 @@
  */
 
 using System;
-using System.Collections.Generic;
-using Nini.Config;
+using OpenMetaverse;
+using Ode.NET;
 using OpenSim.Framework;
 using OpenSim.Region.Physics.Manager;
-using PhysXWrapper;
-using Quaternion=OpenMetaverse.Quaternion;
-using System.Reflection;
-using log4net;
-using OpenMetaverse;
+using OpenSim.Region.Physics.OdePlugin;
 
-namespace OpenSim.Region.Physics.PhysXPlugin
+namespace OpenSim.Region.Physics.OdePlugin
 {
-    /// <summary>
-    /// Will be the PhysX plugin but for now will be a very basic physics engine
-    /// </summary>
-    public class PhysXPlugin : IPhysicsPlugin
+    class OdePhysicsJoint : PhysicsJoint
     {
-        //private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); 
-        private PhysXScene _mScene;
-
-        public PhysXPlugin()
-        {
-        }
-
-        public bool Init()
+        public override bool IsInPhysicsEngine
         {
-            return true;
-        }
-
-        public PhysicsScene GetScene(string sceneIdentifier)
-        {
-            if (_mScene == null)
+            get
             {
-                _mScene = new PhysXScene(sceneIdentifier);
+                return (jointID != IntPtr.Zero);
             }
-            return (_mScene);
-        }
-
-        public string GetName()
-        {
-            return ("RealPhysX");
-        }
-
-        public void Dispose()
-        {
         }
+        public IntPtr jointID;
     }
 }
diff --git a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs
new file mode 100644
index 0000000..deb6164
--- /dev/null
+++ b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs
@@ -0,0 +1,3874 @@
+/*
+ * Copyright (c) Contributors, http://opensimulator.org/
+ * See CONTRIBUTORS.TXT for a full list of copyright holders.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the OpenSimulator Project nor the
+ *       names of its contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+//#define USE_DRAWSTUFF
+
+using System;
+using System.Collections.Generic;
+using System.Reflection;
+using System.Runtime.InteropServices;
+using System.Threading;
+using System.IO;
+using System.Diagnostics;
+using log4net;
+using Nini.Config;
+using Ode.NET;
+#if USE_DRAWSTUFF
+using Drawstuff.NET;
+#endif 
+using OpenSim.Framework;
+using OpenSim.Region.Physics.Manager;
+using OpenMetaverse;
+
+//using OpenSim.Region.Physics.OdePlugin.Meshing;
+
+namespace OpenSim.Region.Physics.OdePlugin
+{
+    /// <summary>
+    /// ODE plugin
+    /// </summary>
+    public class OdePlugin : IPhysicsPlugin
+    {
+        //private static readonly log4net.ILog m_log = log4net.LogManager.GetLogger(System.Reflection.MethodBase.GetCurrentMethod().DeclaringType);
+
+        private CollisionLocker ode;
+        private OdeScene _mScene;
+
+        public OdePlugin()
+        {
+            ode = new CollisionLocker();
+        }
+
+        public bool Init()
+        {
+            return true;
+        }
+
+        public PhysicsScene GetScene(String sceneIdentifier)
+        {
+            if (_mScene == null)
+            {
+                // Initializing ODE only when a scene is created allows alternative ODE plugins to co-habit (according to
+                // http://opensimulator.org/mantis/view.php?id=2750).
+                d.InitODE();
+                
+                _mScene = new OdeScene(ode, sceneIdentifier);
+            }
+            return (_mScene);
+        }
+
+        public string GetName()
+        {
+            return ("ChODE");
+        }
+
+        public void Dispose()
+        {
+        }
+    }
+
+    public enum StatusIndicators : int
+    {
+        Generic = 0,
+        Start = 1,
+        End = 2
+    }
+
+    public struct sCollisionData
+    {
+        public uint ColliderLocalId;
+        public uint CollidedWithLocalId;
+        public int NumberOfCollisions;
+        public int CollisionType;
+        public int StatusIndicator;
+        public int lastframe;
+    }
+
+    [Flags]
+    public enum CollisionCategories : int
+    {
+        Disabled = 0,
+        Geom = 0x00000001,
+        Body = 0x00000002,
+        Space = 0x00000004,
+        Character = 0x00000008,
+        Land = 0x00000010,
+        Water = 0x00000020,
+        Wind = 0x00000040,
+        Sensor = 0x00000080,
+        Selected = 0x00000100
+    }
+
+    /// <summary>
+    /// Material type for a primitive
+    /// </summary>
+    public enum Material : int
+    {
+        /// <summary></summary>
+        Stone = 0,
+        /// <summary></summary>
+        Metal = 1,
+        /// <summary></summary>
+        Glass = 2,
+        /// <summary></summary>
+        Wood = 3,
+        /// <summary></summary>
+        Flesh = 4,
+        /// <summary></summary>
+        Plastic = 5,
+        /// <summary></summary>
+        Rubber = 6
+
+    }
+
+    public sealed class OdeScene : PhysicsScene
+    {
+        private readonly ILog m_log;
+        // private Dictionary<string, sCollisionData> m_storedCollisions = new Dictionary<string, sCollisionData>();
+
+        CollisionLocker ode;
+
+        private Random fluidRandomizer = new Random(Environment.TickCount);
+
+        private const uint m_regionWidth = Constants.RegionSize;
+        private const uint m_regionHeight = Constants.RegionSize;
+
+        private float ODE_STEPSIZE = 0.020f;
+        private float metersInSpace = 29.9f;
+        private float m_timeDilation = 1.0f;
+
+        public float gravityx = 0f;
+        public float gravityy = 0f;
+        public float gravityz = -9.8f;
+
+        private float contactsurfacelayer = 0.001f;
+
+        private int worldHashspaceLow = -4;
+        private int worldHashspaceHigh = 128;
+
+        private int smallHashspaceLow = -4;
+        private int smallHashspaceHigh = 66;
+
+        private float waterlevel = 0f;
+        private int framecount = 0;
+        //private int m_returncollisions = 10;
+
+        private readonly IntPtr contactgroup;
+
+        internal IntPtr LandGeom;
+        internal IntPtr WaterGeom;
+
+        private float nmTerrainContactFriction = 255.0f;
+        private float nmTerrainContactBounce = 0.1f;
+        private float nmTerrainContactERP = 0.1025f;
+
+        private float mTerrainContactFriction = 75f;
+        private float mTerrainContactBounce = 0.1f;
+        private float mTerrainContactERP = 0.05025f;
+
+        private float nmAvatarObjectContactFriction = 250f;
+        private float nmAvatarObjectContactBounce = 0.1f;
+
+        private float mAvatarObjectContactFriction = 75f;
+        private float mAvatarObjectContactBounce = 0.1f;
+
+        private float avPIDD = 3200f;
+        private float avPIDP = 1400f;
+        private float avCapRadius = 0.37f;
+        private float avStandupTensor = 2000000f;
+        private bool avCapsuleTilted = true; // true = old compatibility mode with leaning capsule; false = new corrected mode
+        public bool IsAvCapsuleTilted { get { return avCapsuleTilted; } set { avCapsuleTilted = value; } }
+        private float avDensity = 80f;
+        private float avHeightFudgeFactor = 0.52f;
+        private float avMovementDivisorWalk = 1.3f;
+        private float avMovementDivisorRun = 0.8f;
+        private float minimumGroundFlightOffset = 3f;
+        public float maximumMassObject = 10000.01f;
+
+        public bool meshSculptedPrim = true;
+        public bool forceSimplePrimMeshing = false;
+
+        public float meshSculptLOD = 32;
+        public float MeshSculptphysicalLOD = 16;
+
+        public float geomDefaultDensity = 10.000006836f;
+
+        public int geomContactPointsStartthrottle = 3;
+        public int geomUpdatesPerThrottledUpdate = 15;
+
+        public float bodyPIDD = 35f;
+        public float bodyPIDG = 25;
+
+        public int geomCrossingFailuresBeforeOutofbounds = 5;
+
+        public float bodyMotorJointMaxforceTensor = 2;
+
+        public int bodyFramesAutoDisable = 20;
+
+        private DateTime m_lastframe = DateTime.UtcNow;
+
+        private float[] _watermap;
+        private bool m_filterCollisions = true;
+
+        private d.NearCallback nearCallback;
+        public d.TriCallback triCallback;
+        public d.TriArrayCallback triArrayCallback;
+        private readonly HashSet<OdeCharacter> _characters = new HashSet<OdeCharacter>();
+        private readonly HashSet<OdePrim> _prims = new HashSet<OdePrim>();
+        private readonly HashSet<OdePrim> _activeprims = new HashSet<OdePrim>();
+        private readonly HashSet<OdePrim> _taintedPrimH = new HashSet<OdePrim>();
+        private readonly Object _taintedPrimLock = new Object();
+        private readonly List<OdePrim> _taintedPrimL = new List<OdePrim>();
+        private readonly HashSet<OdeCharacter> _taintedActors = new HashSet<OdeCharacter>();
+        private readonly List<d.ContactGeom> _perloopContact = new List<d.ContactGeom>();
+        private readonly List<PhysicsActor> _collisionEventPrim = new List<PhysicsActor>();
+        private readonly HashSet<OdeCharacter> _badCharacter = new HashSet<OdeCharacter>();
+        public Dictionary<IntPtr, String> geom_name_map = new Dictionary<IntPtr, String>();
+        public Dictionary<IntPtr, PhysicsActor> actor_name_map = new Dictionary<IntPtr, PhysicsActor>();
+        private bool m_NINJA_physics_joints_enabled = false;
+        //private Dictionary<String, IntPtr> jointpart_name_map = new Dictionary<String,IntPtr>();
+        private readonly Dictionary<String, List<PhysicsJoint>> joints_connecting_actor = new Dictionary<String, List<PhysicsJoint>>();
+        private d.ContactGeom[] contacts;
+        private readonly List<PhysicsJoint> requestedJointsToBeCreated = new List<PhysicsJoint>(); // lock only briefly. accessed by external code (to request new joints) and by OdeScene.Simulate() to move those joints into pending/active
+        private readonly List<PhysicsJoint> pendingJoints = new List<PhysicsJoint>(); // can lock for longer. accessed only by OdeScene.
+        private readonly List<PhysicsJoint> activeJoints = new List<PhysicsJoint>(); // can lock for longer. accessed only by OdeScene.
+        private readonly List<string> requestedJointsToBeDeleted = new List<string>(); // lock only briefly. accessed by external code (to request deletion of joints) and by OdeScene.Simulate() to move those joints out of pending/active
+        private Object externalJointRequestsLock = new Object();
+        private readonly Dictionary<String, PhysicsJoint> SOPName_to_activeJoint = new Dictionary<String, PhysicsJoint>();
+        private readonly Dictionary<String, PhysicsJoint> SOPName_to_pendingJoint = new Dictionary<String, PhysicsJoint>();
+        private readonly DoubleDictionary<Vector3, IntPtr, IntPtr> RegionTerrain = new DoubleDictionary<Vector3, IntPtr, IntPtr>();
+        private readonly Dictionary<IntPtr,float[]> TerrainHeightFieldHeights = new Dictionary<IntPtr, float[]>();
+
+        private d.Contact contact;
+        private d.Contact TerrainContact;
+        private d.Contact AvatarMovementprimContact;
+        private d.Contact AvatarMovementTerrainContact;
+        private d.Contact WaterContact;
+        private d.Contact[,] m_materialContacts;
+
+//Ckrinke: Comment out until used. We declare it, initialize it, but do not use it
+//Ckrinke        private int m_randomizeWater = 200;
+        private int m_physicsiterations = 10;
+        private const float m_SkipFramesAtms = 0.40f; // Drop frames gracefully at a 400 ms lag
+        private readonly PhysicsActor PANull = new NullPhysicsActor();
+        private float step_time = 0.0f;
+//Ckrinke: Comment out until used. We declare it, initialize it, but do not use it
+//Ckrinke        private int ms = 0;
+        public IntPtr world;
+        //private bool returncollisions = false;
+        // private uint obj1LocalID = 0;
+        private uint obj2LocalID = 0;
+        //private int ctype = 0;
+        private OdeCharacter cc1;
+        private OdePrim cp1;
+        private OdeCharacter cc2;
+        private OdePrim cp2;
+        //private int cStartStop = 0;
+        //private string cDictKey = "";
+
+        public IntPtr space;
+
+        //private IntPtr tmpSpace;
+        // split static geometry collision handling into spaces of 30 meters
+        public IntPtr[,] staticPrimspace;
+
+        public Object OdeLock;
+
+        public IMesher mesher;
+
+        private IConfigSource m_config;
+
+        public bool physics_logging = false;
+        public int physics_logging_interval = 0;
+        public bool physics_logging_append_existing_logfile = false;
+
+        public d.Vector3 xyz = new d.Vector3(128.1640f, 128.3079f, 25.7600f);
+        public d.Vector3 hpr = new d.Vector3(125.5000f, -17.0000f, 0.0000f);
+
+        // TODO: unused: private uint heightmapWidth = m_regionWidth + 1;
+        // TODO: unused: private uint heightmapHeight = m_regionHeight + 1;
+        // TODO: unused: private uint heightmapWidthSamples;
+        // TODO: unused: private uint heightmapHeightSamples;
+
+        private volatile int m_global_contactcount = 0;
+
+        private Vector3 m_worldOffset = Vector3.Zero;
+        public Vector2 WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize);
+        private PhysicsScene m_parentScene = null;
+
+        private ODERayCastRequestManager m_rayCastManager;
+
+        /// <summary>
+        /// Initiailizes the scene
+        /// Sets many properties that ODE requires to be stable
+        /// These settings need to be tweaked 'exactly' right or weird stuff happens.
+        /// </summary>
+        public OdeScene(CollisionLocker dode, string sceneIdentifier)
+        {
+            m_log 
+                = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType.ToString() + "." + sceneIdentifier);
+
+            OdeLock = new Object();
+            ode = dode;
+            nearCallback = near;
+            triCallback = TriCallback;
+            triArrayCallback = TriArrayCallback;
+            m_rayCastManager = new ODERayCastRequestManager(this);
+            lock (OdeLock)
+            {
+                // Create the world and the first space
+                world = d.WorldCreate();
+                space = d.HashSpaceCreate(IntPtr.Zero);
+                
+
+                contactgroup = d.JointGroupCreate(0);
+                //contactgroup
+
+                d.WorldSetAutoDisableFlag(world, false);
+                #if USE_DRAWSTUFF
+                
+                Thread viewthread = new Thread(new ParameterizedThreadStart(startvisualization));
+                viewthread.Start();
+                #endif
+            }
+
+
+            _watermap = new float[258 * 258];
+
+            // Zero out the prim spaces array (we split our space into smaller spaces so
+            // we can hit test less.
+        }
+
+#if USE_DRAWSTUFF
+        public void startvisualization(object o)
+        {
+            ds.Functions fn;
+            fn.version = ds.VERSION;
+            fn.start = new ds.CallbackFunction(start);
+            fn.step = new ds.CallbackFunction(step);
+            fn.command = new ds.CallbackFunction(command);
+            fn.stop = null;
+            fn.path_to_textures = "./textures";
+            string[] args = new string[0];
+            ds.SimulationLoop(args.Length, args, 352, 288, ref fn);
+        }
+#endif
+
+        // Initialize the mesh plugin
+        public override void Initialise(IMesher meshmerizer, IConfigSource config)
+        {
+            mesher = meshmerizer;
+            m_config = config;
+            // Defaults
+
+            if (Environment.OSVersion.Platform == PlatformID.Unix)
+            {
+                avPIDD = 3200.0f;
+                avPIDP = 1400.0f;
+                avStandupTensor = 2000000f;
+            }
+            else
+            {
+                avPIDD = 2200.0f;
+                avPIDP = 900.0f;
+                avStandupTensor = 550000f;
+            }
+
+            int contactsPerCollision = 80;
+
+            if (m_config != null)
+            {
+                IConfig physicsconfig = m_config.Configs["ODEPhysicsSettings"];
+                if (physicsconfig != null)
+                {
+                    gravityx = physicsconfig.GetFloat("world_gravityx", 0f);
+                    gravityy = physicsconfig.GetFloat("world_gravityy", 0f);
+                    gravityz = physicsconfig.GetFloat("world_gravityz", -9.8f);
+
+                    worldHashspaceLow = physicsconfig.GetInt("world_hashspace_size_low", -4);
+                    worldHashspaceHigh = physicsconfig.GetInt("world_hashspace_size_high", 128);
+
+                    metersInSpace = physicsconfig.GetFloat("meters_in_small_space", 29.9f);
+                    smallHashspaceLow = physicsconfig.GetInt("small_hashspace_size_low", -4);
+                    smallHashspaceHigh = physicsconfig.GetInt("small_hashspace_size_high", 66);
+
+                    contactsurfacelayer = physicsconfig.GetFloat("world_contact_surface_layer", 0.001f);
+
+                    nmTerrainContactFriction = physicsconfig.GetFloat("nm_terraincontact_friction", 255.0f);
+                    nmTerrainContactBounce = physicsconfig.GetFloat("nm_terraincontact_bounce", 0.1f);
+                    nmTerrainContactERP = physicsconfig.GetFloat("nm_terraincontact_erp", 0.1025f);
+
+                    mTerrainContactFriction = physicsconfig.GetFloat("m_terraincontact_friction", 75f);
+                    mTerrainContactBounce = physicsconfig.GetFloat("m_terraincontact_bounce", 0.05f);
+                    mTerrainContactERP = physicsconfig.GetFloat("m_terraincontact_erp", 0.05025f);
+
+                    nmAvatarObjectContactFriction = physicsconfig.GetFloat("objectcontact_friction", 250f);
+                    nmAvatarObjectContactBounce = physicsconfig.GetFloat("objectcontact_bounce", 0.2f);
+
+                    mAvatarObjectContactFriction = physicsconfig.GetFloat("m_avatarobjectcontact_friction", 75f);
+                    mAvatarObjectContactBounce = physicsconfig.GetFloat("m_avatarobjectcontact_bounce", 0.1f);
+
+                    ODE_STEPSIZE = physicsconfig.GetFloat("world_stepsize", 0.020f);
+                    m_physicsiterations = physicsconfig.GetInt("world_internal_steps_without_collisions", 10);
+
+                    avDensity = physicsconfig.GetFloat("av_density", 80f);
+                    avHeightFudgeFactor = physicsconfig.GetFloat("av_height_fudge_factor", 0.52f);
+                    avMovementDivisorWalk = physicsconfig.GetFloat("av_movement_divisor_walk", 1.3f);
+                    avMovementDivisorRun = physicsconfig.GetFloat("av_movement_divisor_run", 0.8f);
+                    avCapRadius = physicsconfig.GetFloat("av_capsule_radius", 0.37f);
+                    avCapsuleTilted = physicsconfig.GetBoolean("av_capsule_tilted", false);
+
+                    contactsPerCollision = physicsconfig.GetInt("contacts_per_collision", 80);
+
+                    geomContactPointsStartthrottle = physicsconfig.GetInt("geom_contactpoints_start_throttling", 3);
+                    geomUpdatesPerThrottledUpdate = physicsconfig.GetInt("geom_updates_before_throttled_update", 15);
+                    geomCrossingFailuresBeforeOutofbounds = physicsconfig.GetInt("geom_crossing_failures_before_outofbounds", 5);
+
+                    geomDefaultDensity = physicsconfig.GetFloat("geometry_default_density", 10.000006836f);
+                    bodyFramesAutoDisable = physicsconfig.GetInt("body_frames_auto_disable", 20);
+
+                    bodyPIDD = physicsconfig.GetFloat("body_pid_derivative", 35f);
+                    bodyPIDG = physicsconfig.GetFloat("body_pid_gain", 25f);
+
+                    forceSimplePrimMeshing = physicsconfig.GetBoolean("force_simple_prim_meshing", forceSimplePrimMeshing);
+                    meshSculptedPrim = physicsconfig.GetBoolean("mesh_sculpted_prim", true);
+                    meshSculptLOD = physicsconfig.GetFloat("mesh_lod", 32f);
+                    MeshSculptphysicalLOD = physicsconfig.GetFloat("mesh_physical_lod", 16f);
+                    m_filterCollisions = physicsconfig.GetBoolean("filter_collisions", false);
+
+                    if (Environment.OSVersion.Platform == PlatformID.Unix)
+                    {
+                        avPIDD = physicsconfig.GetFloat("av_pid_derivative_linux", 2200.0f);
+                        avPIDP = physicsconfig.GetFloat("av_pid_proportional_linux", 900.0f);
+                        avStandupTensor = physicsconfig.GetFloat("av_capsule_standup_tensor_linux", 550000f);
+                        bodyMotorJointMaxforceTensor = physicsconfig.GetFloat("body_motor_joint_maxforce_tensor_linux", 5f);
+                    }
+                    else
+                    {
+                        avPIDD = physicsconfig.GetFloat("av_pid_derivative_win", 2200.0f);
+                        avPIDP = physicsconfig.GetFloat("av_pid_proportional_win", 900.0f);
+                        avStandupTensor = physicsconfig.GetFloat("av_capsule_standup_tensor_win", 550000f);
+                        bodyMotorJointMaxforceTensor = physicsconfig.GetFloat("body_motor_joint_maxforce_tensor_win", 5f);
+                    }
+
+                    physics_logging = physicsconfig.GetBoolean("physics_logging", false);
+                    physics_logging_interval = physicsconfig.GetInt("physics_logging_interval", 0);
+                    physics_logging_append_existing_logfile = physicsconfig.GetBoolean("physics_logging_append_existing_logfile", false);
+
+                    m_NINJA_physics_joints_enabled = physicsconfig.GetBoolean("use_NINJA_physics_joints", false);
+                    minimumGroundFlightOffset = physicsconfig.GetFloat("minimum_ground_flight_offset", 3f);
+                    maximumMassObject = physicsconfig.GetFloat("maximum_mass_object", 10000.01f);
+                }
+            }
+
+            contacts = new d.ContactGeom[contactsPerCollision];
+
+            staticPrimspace = new IntPtr[(int)(300 / metersInSpace), (int)(300 / metersInSpace)];
+
+            // Centeral contact friction and bounce
+            // ckrinke 11/10/08 Enabling soft_erp but not soft_cfm until I figure out why
+            // an avatar falls through in Z but not in X or Y when walking on a prim.
+            contact.surface.mode |= d.ContactFlags.SoftERP;
+            contact.surface.mu = nmAvatarObjectContactFriction;
+            contact.surface.bounce = nmAvatarObjectContactBounce;
+            contact.surface.soft_cfm = 0.010f;
+            contact.surface.soft_erp = 0.010f;
+
+            // Terrain contact friction and Bounce
+            // This is the *non* moving version.   Use this when an avatar
+            // isn't moving to keep it in place better
+            TerrainContact.surface.mode |= d.ContactFlags.SoftERP;
+            TerrainContact.surface.mu = nmTerrainContactFriction;
+            TerrainContact.surface.bounce = nmTerrainContactBounce;
+            TerrainContact.surface.soft_erp = nmTerrainContactERP;
+
+            WaterContact.surface.mode |= (d.ContactFlags.SoftERP | d.ContactFlags.SoftCFM);
+            WaterContact.surface.mu = 0f; // No friction
+            WaterContact.surface.bounce = 0.0f; // No bounce
+            WaterContact.surface.soft_cfm = 0.010f;
+            WaterContact.surface.soft_erp = 0.010f;
+
+            // Prim contact friction and bounce
+            // THis is the *non* moving version of friction and bounce
+            // Use this when an avatar comes in contact with a prim
+            // and is moving
+            AvatarMovementprimContact.surface.mu = mAvatarObjectContactFriction;
+            AvatarMovementprimContact.surface.bounce = mAvatarObjectContactBounce;
+
+            // Terrain contact friction bounce and various error correcting calculations
+            // Use this when an avatar is in contact with the terrain and moving.
+            AvatarMovementTerrainContact.surface.mode |= d.ContactFlags.SoftERP;
+            AvatarMovementTerrainContact.surface.mu = mTerrainContactFriction;
+            AvatarMovementTerrainContact.surface.bounce = mTerrainContactBounce;
+            AvatarMovementTerrainContact.surface.soft_erp = mTerrainContactERP;
+
+
+            /*
+                <summary></summary>
+                Stone = 0,
+                /// <summary></summary>
+                Metal = 1,
+                /// <summary></summary>
+                Glass = 2,
+                /// <summary></summary>
+                Wood = 3,
+                /// <summary></summary>
+                Flesh = 4,
+                /// <summary></summary>
+                Plastic = 5,
+                /// <summary></summary>
+                Rubber = 6
+             */
+
+            m_materialContacts = new d.Contact[7,2];
+
+            m_materialContacts[(int)Material.Stone, 0] = new d.Contact();
+            m_materialContacts[(int)Material.Stone, 0].surface.mode |= d.ContactFlags.SoftERP;
+            m_materialContacts[(int)Material.Stone, 0].surface.mu = nmAvatarObjectContactFriction;
+            m_materialContacts[(int)Material.Stone, 0].surface.bounce = nmAvatarObjectContactBounce;
+            m_materialContacts[(int)Material.Stone, 0].surface.soft_cfm = 0.010f;
+            m_materialContacts[(int)Material.Stone, 0].surface.soft_erp = 0.010f;
+
+            m_materialContacts[(int)Material.Stone, 1] = new d.Contact();
+            m_materialContacts[(int)Material.Stone, 1].surface.mode |= d.ContactFlags.SoftERP;
+            m_materialContacts[(int)Material.Stone, 1].surface.mu = mAvatarObjectContactFriction;
+            m_materialContacts[(int)Material.Stone, 1].surface.bounce = mAvatarObjectContactBounce;
+            m_materialContacts[(int)Material.Stone, 1].surface.soft_cfm = 0.010f;
+            m_materialContacts[(int)Material.Stone, 1].surface.soft_erp = 0.010f;
+
+            m_materialContacts[(int)Material.Metal, 0] = new d.Contact();
+            m_materialContacts[(int)Material.Metal, 0].surface.mode |= d.ContactFlags.SoftERP;
+            m_materialContacts[(int)Material.Metal, 0].surface.mu = nmAvatarObjectContactFriction;
+            m_materialContacts[(int)Material.Metal, 0].surface.bounce = nmAvatarObjectContactBounce;
+            m_materialContacts[(int)Material.Metal, 0].surface.soft_cfm = 0.010f;
+            m_materialContacts[(int)Material.Metal, 0].surface.soft_erp = 0.010f;
+
+            m_materialContacts[(int)Material.Metal, 1] = new d.Contact();
+            m_materialContacts[(int)Material.Metal, 1].surface.mode |= d.ContactFlags.SoftERP;
+            m_materialContacts[(int)Material.Metal, 1].surface.mu = mAvatarObjectContactFriction;
+            m_materialContacts[(int)Material.Metal, 1].surface.bounce = mAvatarObjectContactBounce;
+            m_materialContacts[(int)Material.Metal, 1].surface.soft_cfm = 0.010f;
+            m_materialContacts[(int)Material.Metal, 1].surface.soft_erp = 0.010f;
+
+            m_materialContacts[(int)Material.Glass, 0] = new d.Contact();
+            m_materialContacts[(int)Material.Glass, 0].surface.mode |= d.ContactFlags.SoftERP;
+            m_materialContacts[(int)Material.Glass, 0].surface.mu = 1f;
+            m_materialContacts[(int)Material.Glass, 0].surface.bounce = 0.5f;
+            m_materialContacts[(int)Material.Glass, 0].surface.soft_cfm = 0.010f;
+            m_materialContacts[(int)Material.Glass, 0].surface.soft_erp = 0.010f;
+
+            /*
+                private float nmAvatarObjectContactFriction = 250f;
+                private float nmAvatarObjectContactBounce = 0.1f;
+
+                private float mAvatarObjectContactFriction = 75f;
+                private float mAvatarObjectContactBounce = 0.1f;
+            */
+            m_materialContacts[(int)Material.Glass, 1] = new d.Contact();
+            m_materialContacts[(int)Material.Glass, 1].surface.mode |= d.ContactFlags.SoftERP;
+            m_materialContacts[(int)Material.Glass, 1].surface.mu = 1f;
+            m_materialContacts[(int)Material.Glass, 1].surface.bounce = 0.5f;
+            m_materialContacts[(int)Material.Glass, 1].surface.soft_cfm = 0.010f;
+            m_materialContacts[(int)Material.Glass, 1].surface.soft_erp = 0.010f;
+
+            m_materialContacts[(int)Material.Wood, 0] = new d.Contact();
+            m_materialContacts[(int)Material.Wood, 0].surface.mode |= d.ContactFlags.SoftERP;
+            m_materialContacts[(int)Material.Wood, 0].surface.mu = nmAvatarObjectContactFriction;
+            m_materialContacts[(int)Material.Wood, 0].surface.bounce = nmAvatarObjectContactBounce;
+            m_materialContacts[(int)Material.Wood, 0].surface.soft_cfm = 0.010f;
+            m_materialContacts[(int)Material.Wood, 0].surface.soft_erp = 0.010f;
+
+            m_materialContacts[(int)Material.Wood, 1] = new d.Contact();
+            m_materialContacts[(int)Material.Wood, 1].surface.mode |= d.ContactFlags.SoftERP;
+            m_materialContacts[(int)Material.Wood, 1].surface.mu = mAvatarObjectContactFriction;
+            m_materialContacts[(int)Material.Wood, 1].surface.bounce = mAvatarObjectContactBounce;
+            m_materialContacts[(int)Material.Wood, 1].surface.soft_cfm = 0.010f;
+            m_materialContacts[(int)Material.Wood, 1].surface.soft_erp = 0.010f;
+
+            m_materialContacts[(int)Material.Flesh, 0] = new d.Contact();
+            m_materialContacts[(int)Material.Flesh, 0].surface.mode |= d.ContactFlags.SoftERP;
+            m_materialContacts[(int)Material.Flesh, 0].surface.mu = nmAvatarObjectContactFriction;
+            m_materialContacts[(int)Material.Flesh, 0].surface.bounce = nmAvatarObjectContactBounce;
+            m_materialContacts[(int)Material.Flesh, 0].surface.soft_cfm = 0.010f;
+            m_materialContacts[(int)Material.Flesh, 0].surface.soft_erp = 0.010f;
+
+            m_materialContacts[(int)Material.Flesh, 1] = new d.Contact();
+            m_materialContacts[(int)Material.Flesh, 1].surface.mode |= d.ContactFlags.SoftERP;
+            m_materialContacts[(int)Material.Flesh, 1].surface.mu = mAvatarObjectContactFriction;
+            m_materialContacts[(int)Material.Flesh, 1].surface.bounce = mAvatarObjectContactBounce;
+            m_materialContacts[(int)Material.Flesh, 1].surface.soft_cfm = 0.010f;
+            m_materialContacts[(int)Material.Flesh, 1].surface.soft_erp = 0.010f;
+
+            m_materialContacts[(int)Material.Plastic, 0] = new d.Contact();
+            m_materialContacts[(int)Material.Plastic, 0].surface.mode |= d.ContactFlags.SoftERP;
+            m_materialContacts[(int)Material.Plastic, 0].surface.mu = nmAvatarObjectContactFriction;
+            m_materialContacts[(int)Material.Plastic, 0].surface.bounce = nmAvatarObjectContactBounce;
+            m_materialContacts[(int)Material.Plastic, 0].surface.soft_cfm = 0.010f;
+            m_materialContacts[(int)Material.Plastic, 0].surface.soft_erp = 0.010f;
+
+            m_materialContacts[(int)Material.Plastic, 1] = new d.Contact();
+            m_materialContacts[(int)Material.Plastic, 1].surface.mode |= d.ContactFlags.SoftERP;
+            m_materialContacts[(int)Material.Plastic, 1].surface.mu = mAvatarObjectContactFriction;
+            m_materialContacts[(int)Material.Plastic, 1].surface.bounce = mAvatarObjectContactBounce;
+            m_materialContacts[(int)Material.Plastic, 1].surface.soft_cfm = 0.010f;
+            m_materialContacts[(int)Material.Plastic, 1].surface.soft_erp = 0.010f;
+
+            m_materialContacts[(int)Material.Rubber, 0] = new d.Contact();
+            m_materialContacts[(int)Material.Rubber, 0].surface.mode |= d.ContactFlags.SoftERP;
+            m_materialContacts[(int)Material.Rubber, 0].surface.mu = nmAvatarObjectContactFriction;
+            m_materialContacts[(int)Material.Rubber, 0].surface.bounce = nmAvatarObjectContactBounce;
+            m_materialContacts[(int)Material.Rubber, 0].surface.soft_cfm = 0.010f;
+            m_materialContacts[(int)Material.Rubber, 0].surface.soft_erp = 0.010f;
+
+            m_materialContacts[(int)Material.Rubber, 1] = new d.Contact();
+            m_materialContacts[(int)Material.Rubber, 1].surface.mode |= d.ContactFlags.SoftERP;
+            m_materialContacts[(int)Material.Rubber, 1].surface.mu = mAvatarObjectContactFriction;
+            m_materialContacts[(int)Material.Rubber, 1].surface.bounce = mAvatarObjectContactBounce;
+            m_materialContacts[(int)Material.Rubber, 1].surface.soft_cfm = 0.010f;
+            m_materialContacts[(int)Material.Rubber, 1].surface.soft_erp = 0.010f;
+
+            d.HashSpaceSetLevels(space, worldHashspaceLow, worldHashspaceHigh);
+
+            // Set the gravity,, don't disable things automatically (we set it explicitly on some things)
+
+            d.WorldSetGravity(world, gravityx, gravityy, gravityz);
+            d.WorldSetContactSurfaceLayer(world, contactsurfacelayer);
+
+            d.WorldSetLinearDamping(world, 256f);
+            d.WorldSetAngularDamping(world, 256f);
+            d.WorldSetAngularDampingThreshold(world, 256f);
+            d.WorldSetLinearDampingThreshold(world, 256f);
+            d.WorldSetMaxAngularSpeed(world, 256f);
+
+            // Set how many steps we go without running collision testing
+            // This is in addition to the step size.
+            // Essentially Steps * m_physicsiterations
+            d.WorldSetQuickStepNumIterations(world, m_physicsiterations);
+            //d.WorldSetContactMaxCorrectingVel(world, 1000.0f);
+
+            
+
+            for (int i = 0; i < staticPrimspace.GetLength(0); i++)
+            {
+                for (int j = 0; j < staticPrimspace.GetLength(1); j++)
+                {
+                    staticPrimspace[i, j] = IntPtr.Zero;
+                }
+            }
+        }
+
+        internal void waitForSpaceUnlock(IntPtr space)
+        {
+            //if (space != IntPtr.Zero)
+                //while (d.SpaceLockQuery(space)) { } // Wait and do nothing
+        }
+
+        /// <summary>
+        /// Debug space message for printing the space that a prim/avatar is in.
+        /// </summary>
+        /// <param name="pos"></param>
+        /// <returns>Returns which split up space the given position is in.</returns>
+        public string whichspaceamIin(Vector3 pos)
+        {
+            return calculateSpaceForGeom(pos).ToString();
+        }
+
+        #region Collision Detection
+
+        /// <summary>
+        /// This is our near callback.  A geometry is near a body
+        /// </summary>
+        /// <param name="space">The space that contains the geoms.  Remember, spaces are also geoms</param>
+        /// <param name="g1">a geometry or space</param>
+        /// <param name="g2">another geometry or space</param>
+        private void near(IntPtr space, IntPtr g1, IntPtr g2)
+        {
+            //  no lock here!  It's invoked from within Simulate(), which is thread-locked
+
+            // Test if we're colliding a geom with a space.
+            // If so we have to drill down into the space recursively
+
+            if (d.GeomIsSpace(g1) || d.GeomIsSpace(g2))
+            {
+                if (g1 == IntPtr.Zero || g2 == IntPtr.Zero)
+                    return;
+                
+                // Separating static prim geometry spaces.
+                // We'll be calling near recursivly if one
+                // of them is a space to find all of the
+                // contact points in the space
+                try
+                {
+                    d.SpaceCollide2(g1, g2, IntPtr.Zero, nearCallback);
+                }
+                catch (AccessViolationException)
+                {
+                    m_log.Warn("[PHYSICS]: Unable to collide test a space");
+                    return;
+                }
+                //Colliding a space or a geom with a space or a geom. so drill down
+
+                //Collide all geoms in each space..
+                //if (d.GeomIsSpace(g1)) d.SpaceCollide(g1, IntPtr.Zero, nearCallback);
+                //if (d.GeomIsSpace(g2)) d.SpaceCollide(g2, IntPtr.Zero, nearCallback);
+                return;
+            }
+
+            if (g1 == IntPtr.Zero || g2 == IntPtr.Zero)
+                return;
+
+            IntPtr b1 = d.GeomGetBody(g1);
+            IntPtr b2 = d.GeomGetBody(g2);
+
+            // d.GeomClassID id = d.GeomGetClass(g1);
+
+            String name1 = null;
+            String name2 = null;
+
+            if (!geom_name_map.TryGetValue(g1, out name1))
+            {
+                name1 = "null";
+            }
+            if (!geom_name_map.TryGetValue(g2, out name2))
+            {
+                name2 = "null";
+            }
+
+            //if (id == d.GeomClassId.TriMeshClass)
+            //{
+                //               m_log.InfoFormat("near: A collision was detected between {1} and {2}", 0, name1, name2);
+                //m_log.Debug("near: A collision was detected between {1} and {2}", 0, name1, name2);
+            //}
+
+            // Figure out how many contact points we have
+            int count = 0;
+            try
+            {
+                // Colliding Geom To Geom
+                // This portion of the function 'was' blatantly ripped off from BoxStack.cs
+
+                if (g1 == g2)
+                    return; // Can't collide with yourself
+
+                if (b1 != IntPtr.Zero && b2 != IntPtr.Zero && d.AreConnectedExcluding(b1, b2, d.JointType.Contact))
+                    return;
+
+                lock (contacts)
+                {
+                    count = d.Collide(g1, g2, contacts.Length, contacts, d.ContactGeom.SizeOf);
+                    if (count > contacts.Length)
+                        m_log.Error("[PHYSICS]: Got " + count + " contacts when we asked for a maximum of " + contacts.Length);
+                }
+            }
+            catch (SEHException)
+            {
+                m_log.Error("[PHYSICS]: The Operating system shut down ODE because of corrupt memory.  This could be a result of really irregular terrain.  If this repeats continuously, restart using Basic Physics and terrain fill your terrain.  Restarting the sim.");
+                ode.drelease(world);
+                base.TriggerPhysicsBasedRestart();
+            }
+            catch (Exception e)
+            {
+                m_log.WarnFormat("[PHYSICS]: Unable to collide test an object: {0}", e.Message);
+                return;
+            }
+
+            PhysicsActor p1;
+            PhysicsActor p2;
+
+            if (!actor_name_map.TryGetValue(g1, out p1))
+            {
+                p1 = PANull;
+            }
+
+            if (!actor_name_map.TryGetValue(g2, out p2))
+            {
+                p2 = PANull;
+            }
+
+            ContactPoint maxDepthContact = new ContactPoint();
+            if (p1.CollisionScore + count >= float.MaxValue)
+                p1.CollisionScore = 0;
+            p1.CollisionScore += count;
+
+            if (p2.CollisionScore + count >= float.MaxValue)
+                p2.CollisionScore = 0;
+            p2.CollisionScore += count;
+
+            for (int i = 0; i < count; i++)
+            {
+                d.ContactGeom curContact = contacts[i];
+
+                if (curContact.depth > maxDepthContact.PenetrationDepth)
+                {
+                    maxDepthContact = new ContactPoint(
+                        new Vector3(curContact.pos.X, curContact.pos.Y, curContact.pos.Z),
+                        new Vector3(curContact.normal.X, curContact.normal.Y, curContact.normal.Z),
+                        curContact.depth
+                    );
+                }
+
+                //m_log.Warn("[CCOUNT]: " + count);
+                IntPtr joint;
+                // If we're colliding with terrain, use 'TerrainContact' instead of contact.
+                // allows us to have different settings
+                
+                // We only need to test p2 for 'jump crouch purposes'
+                if (p2 is OdeCharacter && p1.PhysicsActorType == (int)ActorTypes.Prim)
+                {
+                    // Testing if the collision is at the feet of the avatar
+
+                    //m_log.DebugFormat("[PHYSICS]: {0} - {1} - {2} - {3}", curContact.pos.Z, p2.Position.Z, (p2.Position.Z - curContact.pos.Z), (p2.Size.Z * 0.6f));
+                    if ((p2.Position.Z - curContact.pos.Z) > (p2.Size.Z * 0.6f))
+                        p2.IsColliding = true;
+                }
+                else
+                {
+                    p2.IsColliding = true;
+                }
+                
+                //if ((framecount % m_returncollisions) == 0)
+
+                switch (p1.PhysicsActorType)
+                {
+                    case (int)ActorTypes.Agent:
+                        p2.CollidingObj = true;
+                        break;
+                    case (int)ActorTypes.Prim:
+                        if (p2.Velocity.LengthSquared() > 0.0f)
+                            p2.CollidingObj = true;
+                        break;
+                    case (int)ActorTypes.Unknown:
+                        p2.CollidingGround = true;
+                        break;
+                    default:
+                        p2.CollidingGround = true;
+                        break;
+                }
+
+                // we don't want prim or avatar to explode
+
+                #region InterPenetration Handling - Unintended physics explosions
+# region disabled code1
+
+                if (curContact.depth >= 0.08f)
+                {
+                    //This is disabled at the moment only because it needs more tweaking
+                    //It will eventually be uncommented
+                    /*
+                    if (contact.depth >= 1.00f)
+                    {
+                        //m_log.Debug("[PHYSICS]: " + contact.depth.ToString());
+                    }
+
+                    //If you interpenetrate a prim with an agent
+                    if ((p2.PhysicsActorType == (int) ActorTypes.Agent &&
+                         p1.PhysicsActorType == (int) ActorTypes.Prim) ||
+                        (p1.PhysicsActorType == (int) ActorTypes.Agent &&
+                         p2.PhysicsActorType == (int) ActorTypes.Prim))
+                    {
+                        
+                        //contact.depth = contact.depth * 4.15f;
+                        /*
+                        if (p2.PhysicsActorType == (int) ActorTypes.Agent)
+                        {
+                            p2.CollidingObj = true;
+                            contact.depth = 0.003f;
+                            p2.Velocity = p2.Velocity + new PhysicsVector(0, 0, 2.5f);
+                            OdeCharacter character = (OdeCharacter) p2;
+                            character.SetPidStatus(true);
+                            contact.pos = new d.Vector3(contact.pos.X + (p1.Size.X / 2), contact.pos.Y + (p1.Size.Y / 2), contact.pos.Z + (p1.Size.Z / 2));
+
+                        }
+                        else
+                        {
+
+                            //contact.depth = 0.0000000f;
+                        }
+                        if (p1.PhysicsActorType == (int) ActorTypes.Agent)
+                        {
+
+                            p1.CollidingObj = true;
+                            contact.depth = 0.003f;
+                            p1.Velocity = p1.Velocity + new PhysicsVector(0, 0, 2.5f);
+                            contact.pos = new d.Vector3(contact.pos.X + (p2.Size.X / 2), contact.pos.Y + (p2.Size.Y / 2), contact.pos.Z + (p2.Size.Z / 2));
+                            OdeCharacter character = (OdeCharacter)p1;
+                            character.SetPidStatus(true);
+                        }
+                        else
+                        {
+
+                            //contact.depth = 0.0000000f;
+                        }
+                          
+                        
+                     
+                    }
+*/
+                    // If you interpenetrate a prim with another prim
+                /*
+                    if (p1.PhysicsActorType == (int) ActorTypes.Prim && p2.PhysicsActorType == (int) ActorTypes.Prim)
+                    {
+                        #region disabledcode2
+                        //OdePrim op1 = (OdePrim)p1;
+                        //OdePrim op2 = (OdePrim)p2;
+                        //op1.m_collisionscore++;
+                        //op2.m_collisionscore++;
+
+                        //if (op1.m_collisionscore > 8000 || op2.m_collisionscore > 8000)
+                        //{
+                            //op1.m_taintdisable = true;
+                            //AddPhysicsActorTaint(p1);
+                            //op2.m_taintdisable = true;
+                            //AddPhysicsActorTaint(p2);
+                        //}
+
+                        //if (contact.depth >= 0.25f)
+                        //{
+                            // Don't collide, one or both prim will expld.
+
+                            //op1.m_interpenetrationcount++;
+                            //op2.m_interpenetrationcount++;
+                            //interpenetrations_before_disable = 200;
+                            //if (op1.m_interpenetrationcount >= interpenetrations_before_disable)
+                            //{
+                                //op1.m_taintdisable = true;
+                                //AddPhysicsActorTaint(p1);
+                            //}
+                            //if (op2.m_interpenetrationcount >= interpenetrations_before_disable)
+                            //{
+                               // op2.m_taintdisable = true;
+                                //AddPhysicsActorTaint(p2);
+                            //}
+
+                            //contact.depth = contact.depth / 8f;
+                            //contact.normal = new d.Vector3(0, 0, 1);
+                        //}
+                        //if (op1.m_disabled || op2.m_disabled)
+                        //{
+                            //Manually disabled objects stay disabled
+                            //contact.depth = 0f;
+                        //}
+                        #endregion
+                    }
+                    */
+#endregion
+                    if (curContact.depth >= 1.00f)
+                    {
+                        //m_log.Info("[P]: " + contact.depth.ToString());
+                        if ((p2.PhysicsActorType == (int) ActorTypes.Agent &&
+                             p1.PhysicsActorType == (int) ActorTypes.Unknown) ||
+                            (p1.PhysicsActorType == (int) ActorTypes.Agent &&
+                             p2.PhysicsActorType == (int) ActorTypes.Unknown))
+                        {
+                            if (p2.PhysicsActorType == (int) ActorTypes.Agent)
+                            {
+                                if (p2 is OdeCharacter)
+                                {
+                                    OdeCharacter character = (OdeCharacter) p2;
+
+                                    //p2.CollidingObj = true;
+                                    curContact.depth = 0.00000003f;
+                                    p2.Velocity = p2.Velocity + new Vector3(0f, 0f, 0.5f);
+                                    curContact.pos =
+                                        new d.Vector3(curContact.pos.X + (p1.Size.X/2),
+                                                      curContact.pos.Y + (p1.Size.Y/2),
+                                                      curContact.pos.Z + (p1.Size.Z/2));
+                                    character.SetPidStatus(true);
+                                }
+                            }
+                            
+
+                            if (p1.PhysicsActorType == (int) ActorTypes.Agent)
+                            {
+                                if (p1 is OdeCharacter)
+                                {
+                                    OdeCharacter character = (OdeCharacter) p1;
+
+                                    //p2.CollidingObj = true;
+                                    curContact.depth = 0.00000003f;
+                                    p1.Velocity = p1.Velocity + new Vector3(0f, 0f, 0.5f);
+                                    curContact.pos =
+                                        new d.Vector3(curContact.pos.X + (p1.Size.X/2),
+                                                      curContact.pos.Y + (p1.Size.Y/2),
+                                                      curContact.pos.Z + (p1.Size.Z/2));
+                                    character.SetPidStatus(true);
+                                }
+                            }
+                        }
+                    }
+                }
+
+                #endregion
+
+                // Logic for collision handling
+                // Note, that if *all* contacts are skipped (VolumeDetect)
+                // The prim still detects (and forwards) collision events but 
+                // appears to be phantom for the world
+                Boolean skipThisContact = false;
+
+                if ((p1 is OdePrim) && (((OdePrim)p1).m_isVolumeDetect))
+                    skipThisContact = true;   // No collision on volume detect prims
+
+                if (!skipThisContact && (p2 is OdePrim) && (((OdePrim)p2).m_isVolumeDetect))
+                    skipThisContact = true;   // No collision on volume detect prims
+
+                if (!skipThisContact && curContact.depth < 0f)
+                    skipThisContact = true;
+
+                if (!skipThisContact && checkDupe(curContact, p2.PhysicsActorType))
+                    skipThisContact = true;
+
+                const int maxContactsbeforedeath = 4000;
+                joint = IntPtr.Zero;
+
+                if (!skipThisContact)
+                {
+                    // If we're colliding against terrain
+                    if (name1 == "Terrain" || name2 == "Terrain")
+                    {
+                        // If we're moving
+                        if ((p2.PhysicsActorType == (int) ActorTypes.Agent) &&
+                            (Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f))
+                        {
+                            // Use the movement terrain contact
+                            AvatarMovementTerrainContact.geom = curContact;
+                            _perloopContact.Add(curContact);
+                            if (m_global_contactcount < maxContactsbeforedeath)
+                            {
+                                joint = d.JointCreateContact(world, contactgroup, ref AvatarMovementTerrainContact);
+                                m_global_contactcount++;
+                            }
+                        }
+                        else
+                        {
+                            if (p2.PhysicsActorType == (int)ActorTypes.Agent)
+                            {
+                                // Use the non moving terrain contact
+                                TerrainContact.geom = curContact;
+                                _perloopContact.Add(curContact);
+                                if (m_global_contactcount < maxContactsbeforedeath)
+                                {
+                                    joint = d.JointCreateContact(world, contactgroup, ref TerrainContact);
+                                    m_global_contactcount++;
+                                }
+                            }
+                            else
+                            {
+                                if (p2.PhysicsActorType == (int)ActorTypes.Prim && p1.PhysicsActorType == (int)ActorTypes.Prim)
+                                {
+                                    // prim prim contact
+                                    // int pj294950 = 0;
+                                    int movintYN = 0;
+                                    int material = (int) Material.Wood;
+                                    // prim terrain contact
+                                    if (Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f)
+                                    {
+                                        movintYN = 1;
+                                    }
+
+                                    if (p2 is OdePrim)
+                                        material = ((OdePrim)p2).m_material;
+
+                                    //m_log.DebugFormat("Material: {0}", material);
+                                    m_materialContacts[material, movintYN].geom = curContact;
+                                    _perloopContact.Add(curContact);
+
+                                    if (m_global_contactcount < maxContactsbeforedeath)
+                                    {
+                                        joint = d.JointCreateContact(world, contactgroup, ref m_materialContacts[material, movintYN]);
+                                        m_global_contactcount++;
+
+                                    }
+                                    
+                                }
+                                else
+                                {
+
+                                    int movintYN = 0;
+                                    // prim terrain contact
+                                    if (Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f)
+                                    {
+                                        movintYN = 1;
+                                    }
+
+                                    int material = (int)Material.Wood;
+
+                                    if (p2 is OdePrim)
+                                        material = ((OdePrim)p2).m_material;
+                                    //m_log.DebugFormat("Material: {0}", material);
+                                    m_materialContacts[material, movintYN].geom = curContact;
+                                    _perloopContact.Add(curContact);
+
+                                    if (m_global_contactcount < maxContactsbeforedeath)
+                                    {
+                                        joint = d.JointCreateContact(world, contactgroup, ref m_materialContacts[material, movintYN]);
+                                        m_global_contactcount++;
+
+                                    }
+                                }
+                            }
+                        }
+                        //if (p2.PhysicsActorType == (int)ActorTypes.Prim)
+                        //{
+                        //m_log.Debug("[PHYSICS]: prim contacting with ground");
+                        //}
+                    }
+                    else if (name1 == "Water" || name2 == "Water")
+                    {
+                        /*
+                        if ((p2.PhysicsActorType == (int) ActorTypes.Prim))
+                        {
+                        }
+                        else
+                        {
+                        }
+                        */
+                        //WaterContact.surface.soft_cfm = 0.0000f;
+                        //WaterContact.surface.soft_erp = 0.00000f;
+                        if (curContact.depth > 0.1f)
+                        {
+                            curContact.depth *= 52;
+                            //contact.normal = new d.Vector3(0, 0, 1);
+                            //contact.pos = new d.Vector3(0, 0, contact.pos.Z - 5f);
+                        }
+                        WaterContact.geom = curContact;
+                        _perloopContact.Add(curContact);
+                        if (m_global_contactcount < maxContactsbeforedeath)
+                        {
+                            joint = d.JointCreateContact(world, contactgroup, ref WaterContact);
+                            m_global_contactcount++;
+                        }
+                        //m_log.Info("[PHYSICS]: Prim Water Contact" + contact.depth);
+                    }
+                    else
+                    {
+                        // we're colliding with prim or avatar
+                        // check if we're moving
+                        if ((p2.PhysicsActorType == (int)ActorTypes.Agent))
+                        {
+                            if ((Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f))
+                            {
+                                // Use the Movement prim contact
+                                AvatarMovementprimContact.geom = curContact;
+                                _perloopContact.Add(curContact);
+                                if (m_global_contactcount < maxContactsbeforedeath)
+                                {
+                                    joint = d.JointCreateContact(world, contactgroup, ref AvatarMovementprimContact);
+                                    m_global_contactcount++;
+                                }
+                            }
+                            else
+                            {
+                                // Use the non movement contact
+                                contact.geom = curContact;
+                                _perloopContact.Add(curContact);
+
+                                if (m_global_contactcount < maxContactsbeforedeath)
+                                {
+                                    joint = d.JointCreateContact(world, contactgroup, ref contact);
+                                    m_global_contactcount++;
+                                }
+                            }
+                        }
+                        else if (p2.PhysicsActorType == (int)ActorTypes.Prim)
+                        {
+                            //p1.PhysicsActorType
+                            int material = (int)Material.Wood;
+
+                            if (p2 is OdePrim)
+                                material = ((OdePrim)p2).m_material;
+                            
+                            //m_log.DebugFormat("Material: {0}", material);
+                            m_materialContacts[material, 0].geom = curContact;
+                            _perloopContact.Add(curContact);
+
+                            if (m_global_contactcount < maxContactsbeforedeath)
+                            {
+                                joint = d.JointCreateContact(world, contactgroup, ref m_materialContacts[material, 0]);
+                                m_global_contactcount++;
+
+                            }
+                        }
+                    }
+
+                    if (m_global_contactcount < maxContactsbeforedeath && joint != IntPtr.Zero) // stack collide!
+                    {
+                        d.JointAttach(joint, b1, b2);
+                        m_global_contactcount++;
+                    }
+
+                }
+                collision_accounting_events(p1, p2, maxDepthContact);
+                if (count > geomContactPointsStartthrottle)
+                {
+                    // If there are more then 3 contact points, it's likely
+                    // that we've got a pile of objects, so ...
+                    // We don't want to send out hundreds of terse updates over and over again
+                    // so lets throttle them and send them again after it's somewhat sorted out.
+                    p2.ThrottleUpdates = true;
+                }
+                //m_log.Debug(count.ToString());
+                //m_log.Debug("near: A collision was detected between {1} and {2}", 0, name1, name2);
+            }
+        }
+
+        private bool checkDupe(d.ContactGeom contactGeom, int atype)
+        {
+            bool result = false;
+            //return result;
+            if (!m_filterCollisions)
+                return false;
+
+            ActorTypes at = (ActorTypes)atype;
+            lock (_perloopContact)
+            {
+                foreach (d.ContactGeom contact in _perloopContact)
+                {
+                    //if ((contact.g1 == contactGeom.g1 && contact.g2 == contactGeom.g2))
+                    //{
+                        // || (contact.g2 == contactGeom.g1 && contact.g1 == contactGeom.g2)
+                    if (at == ActorTypes.Agent)
+                    {
+                            if (((Math.Abs(contactGeom.normal.X - contact.normal.X) < 1.026f) && (Math.Abs(contactGeom.normal.Y - contact.normal.Y) < 0.303f) && (Math.Abs(contactGeom.normal.Z - contact.normal.Z) < 0.065f)) && contactGeom.g1 != LandGeom && contactGeom.g2 != LandGeom)
+                            {
+                                
+                                if (Math.Abs(contact.depth - contactGeom.depth) < 0.052f)
+                                {
+                                    //contactGeom.depth *= .00005f;
+                                   //m_log.DebugFormat("[Collsion]: Depth {0}", Math.Abs(contact.depth - contactGeom.depth));
+                                    // m_log.DebugFormat("[Collision]: <{0},{1},{2}>", Math.Abs(contactGeom.normal.X - contact.normal.X), Math.Abs(contactGeom.normal.Y - contact.normal.Y), Math.Abs(contactGeom.normal.Z - contact.normal.Z));
+                                    result = true;
+                                    break;
+                                }
+                                else
+                                {
+                                    //m_log.DebugFormat("[Collsion]: Depth {0}", Math.Abs(contact.depth - contactGeom.depth));
+                                }
+                            }
+                            else
+                            {
+                                //m_log.DebugFormat("[Collision]: <{0},{1},{2}>", Math.Abs(contactGeom.normal.X - contact.normal.X), Math.Abs(contactGeom.normal.Y - contact.normal.Y), Math.Abs(contactGeom.normal.Z - contact.normal.Z));
+                                //int i = 0;
+                            }
+                    } 
+                    else if (at == ActorTypes.Prim)
+                    {
+                            //d.AABB aabb1 = new d.AABB();
+                            //d.AABB aabb2 = new d.AABB();
+
+                            //d.GeomGetAABB(contactGeom.g2, out aabb2);
+                            //d.GeomGetAABB(contactGeom.g1, out aabb1);
+                            //aabb1.
+                            if (((Math.Abs(contactGeom.normal.X - contact.normal.X) < 1.026f) && (Math.Abs(contactGeom.normal.Y - contact.normal.Y) < 0.303f) && (Math.Abs(contactGeom.normal.Z - contact.normal.Z) < 0.065f)) && contactGeom.g1 != LandGeom && contactGeom.g2 != LandGeom)
+                            {
+                                if (contactGeom.normal.X == contact.normal.X && contactGeom.normal.Y == contact.normal.Y && contactGeom.normal.Z == contact.normal.Z)
+                                {
+                                    if (Math.Abs(contact.depth - contactGeom.depth) < 0.272f)
+                                    {
+                                        result = true;
+                                        break;
+                                    }
+                                }
+                                //m_log.DebugFormat("[Collsion]: Depth {0}", Math.Abs(contact.depth - contactGeom.depth));
+                                //m_log.DebugFormat("[Collision]: <{0},{1},{2}>", Math.Abs(contactGeom.normal.X - contact.normal.X), Math.Abs(contactGeom.normal.Y - contact.normal.Y), Math.Abs(contactGeom.normal.Z - contact.normal.Z));
+                            }
+                            
+                    }
+                    
+                    //}
+
+                }
+            }
+            return result;
+        }
+
+        private void collision_accounting_events(PhysicsActor p1, PhysicsActor p2, ContactPoint contact)
+        {
+            // obj1LocalID = 0;
+            //returncollisions = false;
+            obj2LocalID = 0;
+            //ctype = 0;
+            //cStartStop = 0;
+            if (!p2.SubscribedEvents() && !p1.SubscribedEvents())
+                return;
+
+            switch ((ActorTypes)p2.PhysicsActorType)
+            {
+                case ActorTypes.Agent:
+                    cc2 = (OdeCharacter)p2;
+
+                    // obj1LocalID = cc2.m_localID;
+                    switch ((ActorTypes)p1.PhysicsActorType)
+                    {
+                        case ActorTypes.Agent:
+                            cc1 = (OdeCharacter)p1;
+                            obj2LocalID = cc1.m_localID;
+                            cc1.AddCollisionEvent(cc2.m_localID, contact);
+                            //ctype = (int)CollisionCategories.Character;
+
+                            //if (cc1.CollidingObj)
+                            //cStartStop = (int)StatusIndicators.Generic;
+                            //else
+                            //cStartStop = (int)StatusIndicators.Start;
+
+                            //returncollisions = true;
+                            break;
+                        case ActorTypes.Prim:
+                            if (p1 is OdePrim)
+                            {
+                                cp1 = (OdePrim) p1;
+                                obj2LocalID = cp1.m_localID;
+                                cp1.AddCollisionEvent(cc2.m_localID, contact);
+                            }
+                            //ctype = (int)CollisionCategories.Geom;
+
+                            //if (cp1.CollidingObj)
+                            //cStartStop = (int)StatusIndicators.Generic;
+                            //else
+                            //cStartStop = (int)StatusIndicators.Start;
+
+                            //returncollisions = true;
+                            break;
+
+                        case ActorTypes.Ground:
+                        case ActorTypes.Unknown:
+                            obj2LocalID = 0;
+                            //ctype = (int)CollisionCategories.Land;
+                            //returncollisions = true;
+                            break;
+                    }
+
+                    cc2.AddCollisionEvent(obj2LocalID, contact);
+                    break;
+                case ActorTypes.Prim:
+
+                    if (p2 is OdePrim)
+                    {
+                        cp2 = (OdePrim) p2;
+
+                        // obj1LocalID = cp2.m_localID;
+                        switch ((ActorTypes) p1.PhysicsActorType)
+                        {
+                            case ActorTypes.Agent:
+                                if (p1 is OdeCharacter)
+                                {
+                                    cc1 = (OdeCharacter) p1;
+                                    obj2LocalID = cc1.m_localID;
+                                    cc1.AddCollisionEvent(cp2.m_localID, contact);
+                                    //ctype = (int)CollisionCategories.Character;
+
+                                    //if (cc1.CollidingObj)
+                                    //cStartStop = (int)StatusIndicators.Generic;
+                                    //else
+                                    //cStartStop = (int)StatusIndicators.Start;
+                                    //returncollisions = true;
+                                }
+                                break;
+                            case ActorTypes.Prim:
+
+                                if (p1 is OdePrim)
+                                {
+                                    cp1 = (OdePrim) p1;
+                                    obj2LocalID = cp1.m_localID;
+                                    cp1.AddCollisionEvent(cp2.m_localID, contact);
+                                    //ctype = (int)CollisionCategories.Geom;
+
+                                    //if (cp1.CollidingObj)
+                                    //cStartStop = (int)StatusIndicators.Generic;
+                                    //else
+                                    //cStartStop = (int)StatusIndicators.Start;
+
+                                    //returncollisions = true;
+                                }
+                                break;
+
+                            case ActorTypes.Ground:
+                            case ActorTypes.Unknown:
+                                obj2LocalID = 0;
+                                //ctype = (int)CollisionCategories.Land;
+
+                                //returncollisions = true;
+                                break;
+                        }
+
+                        cp2.AddCollisionEvent(obj2LocalID, contact);
+                    }
+                    break;
+            }
+            //if (returncollisions)
+            //{
+
+                //lock (m_storedCollisions)
+                //{
+                    //cDictKey = obj1LocalID.ToString() + obj2LocalID.ToString() + cStartStop.ToString() + ctype.ToString();
+                    //if (m_storedCollisions.ContainsKey(cDictKey))
+                    //{
+                        //sCollisionData objd = m_storedCollisions[cDictKey];
+                        //objd.NumberOfCollisions += 1;
+                        //objd.lastframe = framecount;
+                        //m_storedCollisions[cDictKey] = objd;
+                    //}
+                    //else
+                    //{
+                        //sCollisionData objd = new sCollisionData();
+                        //objd.ColliderLocalId = obj1LocalID;
+                        //objd.CollidedWithLocalId = obj2LocalID;
+                        //objd.CollisionType = ctype;
+                        //objd.NumberOfCollisions = 1;
+                        //objd.lastframe = framecount;
+                        //objd.StatusIndicator = cStartStop;
+                        //m_storedCollisions.Add(cDictKey, objd);
+                    //}
+                //}
+           // }
+        }
+
+        public int TriArrayCallback(IntPtr trimesh, IntPtr refObject, int[] triangleIndex, int triCount)
+        {
+            /*            String name1 = null;
+                        String name2 = null;
+
+                        if (!geom_name_map.TryGetValue(trimesh, out name1))
+                        {
+                            name1 = "null";
+                        }
+                        if (!geom_name_map.TryGetValue(refObject, out name2))
+                        {
+                            name2 = "null";
+                        }
+
+                        m_log.InfoFormat("TriArrayCallback: A collision was detected between {1} and {2}", 0, name1, name2);
+            */
+            return 1;
+        }
+
+        public int TriCallback(IntPtr trimesh, IntPtr refObject, int triangleIndex)
+        {
+            String name1 = null;
+            String name2 = null;
+
+            if (!geom_name_map.TryGetValue(trimesh, out name1))
+            {
+                name1 = "null";
+            }
+
+            if (!geom_name_map.TryGetValue(refObject, out name2))
+            {
+                name2 = "null";
+            }
+
+            //            m_log.InfoFormat("TriCallback: A collision was detected between {1} and {2}. Index was {3}", 0, name1, name2, triangleIndex);
+
+            d.Vector3 v0 = new d.Vector3();
+            d.Vector3 v1 = new d.Vector3();
+            d.Vector3 v2 = new d.Vector3();
+
+            d.GeomTriMeshGetTriangle(trimesh, 0, ref v0, ref v1, ref v2);
+            //            m_log.DebugFormat("Triangle {0} is <{1},{2},{3}>, <{4},{5},{6}>, <{7},{8},{9}>", triangleIndex, v0.X, v0.Y, v0.Z, v1.X, v1.Y, v1.Z, v2.X, v2.Y, v2.Z);
+
+            return 1;
+        }
+
+        /// <summary>
+        /// This is our collision testing routine in ODE
+        /// </summary>
+        /// <param name="timeStep"></param>
+        private void collision_optimized(float timeStep)
+        {
+            _perloopContact.Clear();
+
+            lock (_characters)
+            {
+                foreach (OdeCharacter chr in _characters)
+                {
+                    // Reset the collision values to false
+                    // since we don't know if we're colliding yet
+                    
+                    // For some reason this can happen. Don't ask...
+                    //
+                    if (chr == null)
+                        continue;
+                    
+                    if (chr.Shell == IntPtr.Zero || chr.Body == IntPtr.Zero)
+                        continue;
+                    
+                    chr.IsColliding = false;
+                    chr.CollidingGround = false;
+                    chr.CollidingObj = false;
+                    
+                    // test the avatar's geometry for collision with the space
+                    // This will return near and the space that they are the closest to
+                    // And we'll run this again against the avatar and the space segment
+                    // This will return with a bunch of possible objects in the space segment
+                    // and we'll run it again on all of them.
+                    try
+                    {
+                        d.SpaceCollide2(space, chr.Shell, IntPtr.Zero, nearCallback);
+                    }
+                    catch (AccessViolationException)
+                    {
+                        m_log.Warn("[PHYSICS]: Unable to space collide");
+                    }
+                    //float terrainheight = GetTerrainHeightAtXY(chr.Position.X, chr.Position.Y);
+                    //if (chr.Position.Z + (chr.Velocity.Z * timeStep) < terrainheight + 10)
+                    //{
+                    //chr.Position.Z = terrainheight + 10.0f;
+                    //forcedZ = true;
+                    //}
+                }
+            }
+
+            lock (_activeprims)
+            {
+                List<OdePrim> removeprims = null;
+                foreach (OdePrim chr in _activeprims)
+                {
+                    if (chr.Body != IntPtr.Zero && d.BodyIsEnabled(chr.Body) && (!chr.m_disabled))
+                    {
+                        try
+                        {
+                            lock (chr)
+                            {
+                                if (space != IntPtr.Zero && chr.prim_geom != IntPtr.Zero && chr.m_taintremove == false)
+                                {
+                                    d.SpaceCollide2(space, chr.prim_geom, IntPtr.Zero, nearCallback);
+                                }
+                                else
+                                {
+                                    if (removeprims == null)
+                                    {
+                                        removeprims = new List<OdePrim>();
+                                    }
+                                    removeprims.Add(chr);
+                                    m_log.Debug("[PHYSICS]: unable to collide test active prim against space.  The space was zero, the geom was zero or it was in the process of being removed.  Removed it from the active prim list.  This needs to be fixed!");
+                                }
+                            }
+                        }
+                        catch (AccessViolationException)
+                        {
+                            m_log.Warn("[PHYSICS]: Unable to space collide");
+                        }
+                    }
+                }
+                if (removeprims != null)
+                {
+                    foreach (OdePrim chr in removeprims)
+                    {
+                        _activeprims.Remove(chr);
+                    }
+                }
+            }
+
+            _perloopContact.Clear();
+        }
+
+        #endregion
+
+        public override void Combine(PhysicsScene pScene, Vector3 offset, Vector3 extents)
+        {
+            m_worldOffset = offset;
+            WorldExtents = new Vector2(extents.X, extents.Y);
+            m_parentScene = pScene;
+            
+        }
+        
+        // Recovered for use by fly height. Kitto Flora
+        public float GetTerrainHeightAtXY(float x, float y)
+        {
+
+            int offsetX = ((int)(x / (int)Constants.RegionSize)) * (int)Constants.RegionSize;
+            int offsetY = ((int)(y / (int)Constants.RegionSize)) * (int)Constants.RegionSize;
+
+            IntPtr heightFieldGeom = IntPtr.Zero;
+
+            if (RegionTerrain.TryGetValue(new Vector3(offsetX,offsetY,0), out heightFieldGeom))
+            {
+                if (heightFieldGeom != IntPtr.Zero)
+                {
+                    if (TerrainHeightFieldHeights.ContainsKey(heightFieldGeom))
+                    {
+                        
+                        int index;
+
+
+                        if ((int)x > WorldExtents.X || (int)y > WorldExtents.Y ||
+                            (int)x < 0.001f || (int)y < 0.001f)
+                            return 0;
+
+                        x = x - offsetX;
+                        y = y - offsetY;
+
+                        index = (int)((int)x * ((int)Constants.RegionSize + 2) + (int)y);
+
+                        if (index < TerrainHeightFieldHeights[heightFieldGeom].Length)
+                        {
+                            //m_log.DebugFormat("x{0} y{1} = {2}", x, y, (float)TerrainHeightFieldHeights[heightFieldGeom][index]);
+                            return (float)TerrainHeightFieldHeights[heightFieldGeom][index];
+                        }
+                            
+                        else
+                            return 0f;
+                    }
+                    else
+                    {
+                        return 0f;
+                    }
+
+                }
+                else
+                {
+                    return 0f;
+                }
+
+            }
+            else
+            {
+                return 0f;
+            }
+
+
+        } 
+// End recovered. Kitto Flora
+
+        public void addCollisionEventReporting(PhysicsActor obj)
+        {
+            lock (_collisionEventPrim)
+            {
+                if (!_collisionEventPrim.Contains(obj))
+                    _collisionEventPrim.Add(obj);
+            }
+        }
+
+        public void remCollisionEventReporting(PhysicsActor obj)
+        {
+            lock (_collisionEventPrim)
+            {
+                if (!_collisionEventPrim.Contains(obj))
+                    _collisionEventPrim.Remove(obj);
+            }
+        }
+
+        #region Add/Remove Entities
+
+        public override PhysicsActor AddAvatar(string avName, Vector3 position, Vector3 size, bool isFlying)
+        {
+            Vector3 pos;
+            pos.X = position.X;
+            pos.Y = position.Y;
+            pos.Z = position.Z;
+            OdeCharacter newAv = new OdeCharacter(avName, this, pos, ode, size, avPIDD, avPIDP, avCapRadius, avStandupTensor, avDensity, avHeightFudgeFactor, avMovementDivisorWalk, avMovementDivisorRun);
+            newAv.Flying = isFlying;
+            newAv.MinimumGroundFlightOffset = minimumGroundFlightOffset;
+            
+            return newAv;
+        }
+
+        public void AddCharacter(OdeCharacter chr)
+        {
+            lock (_characters)
+            {
+                if (!_characters.Contains(chr))
+                {
+                    _characters.Add(chr);
+                    if (chr.bad)
+                        m_log.DebugFormat("[PHYSICS] Added BAD actor {0} to characters list", chr.m_uuid);
+                }
+            }
+        }
+
+        public void RemoveCharacter(OdeCharacter chr)
+        {
+            lock (_characters)
+            {
+                if (_characters.Contains(chr))
+                {
+                    _characters.Remove(chr);
+                }
+            }
+        }
+        public void BadCharacter(OdeCharacter chr)
+        {
+            lock (_badCharacter)
+            {
+                if (!_badCharacter.Contains(chr))
+                    _badCharacter.Add(chr);
+            }
+        }
+
+        public override void RemoveAvatar(PhysicsActor actor)
+        {
+            //m_log.Debug("[PHYSICS]:ODELOCK");
+            ((OdeCharacter) actor).Destroy();
+            
+        }
+
+        private PhysicsActor AddPrim(String name, Vector3 position, Vector3 size, Quaternion rotation,
+                                     IMesh mesh, PrimitiveBaseShape pbs, bool isphysical)
+        {
+
+            Vector3 pos = position;
+            Vector3 siz = size;
+            Quaternion rot = rotation;
+
+            OdePrim newPrim;
+            lock (OdeLock)
+            {
+                newPrim = new OdePrim(name, this, pos, siz, rot, mesh, pbs, isphysical, ode);
+
+                lock (_prims)
+                    _prims.Add(newPrim);
+            }
+
+            return newPrim;
+        }
+
+        public void addActivePrim(OdePrim activatePrim)
+        {
+            // adds active prim..   (ones that should be iterated over in collisions_optimized
+            lock (_activeprims)
+            {
+                if (!_activeprims.Contains(activatePrim))
+                    _activeprims.Add(activatePrim);
+                //else
+                  //  m_log.Warn("[PHYSICS]: Double Entry in _activeprims detected, potential crash immenent");
+            }
+        }
+
+        public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
+                                                  Vector3 size, Quaternion rotation) //To be removed
+        {
+            return AddPrimShape(primName, pbs, position, size, rotation, false);
+        }
+
+        public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
+                                                  Vector3 size, Quaternion rotation, bool isPhysical)
+        {
+            PhysicsActor result;
+            IMesh mesh = null;
+
+            if (needsMeshing(pbs))
+                mesh = mesher.CreateMesh(primName, pbs, size, 32f, isPhysical);
+
+            result = AddPrim(primName, position, size, rotation, mesh, pbs, isPhysical);
+
+            return result;
+        }
+
+        public override float TimeDilation
+        {
+            get { return m_timeDilation; }
+        }
+
+        public override bool SupportsNINJAJoints
+        {
+            get { return m_NINJA_physics_joints_enabled; }
+        }
+
+        // internal utility function: must be called within a lock (OdeLock)
+        private void InternalAddActiveJoint(PhysicsJoint joint)
+        {
+            activeJoints.Add(joint);
+            SOPName_to_activeJoint.Add(joint.ObjectNameInScene, joint);
+        }
+
+        // internal utility function: must be called within a lock (OdeLock)
+        private void InternalAddPendingJoint(OdePhysicsJoint joint)
+        {
+            pendingJoints.Add(joint);
+            SOPName_to_pendingJoint.Add(joint.ObjectNameInScene, joint);
+        }
+
+        // internal utility function: must be called within a lock (OdeLock)
+        private void InternalRemovePendingJoint(PhysicsJoint joint)
+        {
+            pendingJoints.Remove(joint);
+            SOPName_to_pendingJoint.Remove(joint.ObjectNameInScene);
+        }
+
+        // internal utility function: must be called within a lock (OdeLock)
+        private void InternalRemoveActiveJoint(PhysicsJoint joint)
+        {
+            activeJoints.Remove(joint);
+            SOPName_to_activeJoint.Remove(joint.ObjectNameInScene);
+        }
+
+        public override void DumpJointInfo()
+        {
+            string hdr = "[NINJA] JOINTINFO: ";
+            foreach (PhysicsJoint j in pendingJoints)
+            {
+                m_log.Debug(hdr + " pending joint, Name: " + j.ObjectNameInScene + " raw parms:" + j.RawParams);
+            }
+            m_log.Debug(hdr + pendingJoints.Count + " total pending joints");
+            foreach (string jointName in SOPName_to_pendingJoint.Keys)
+            {
+                m_log.Debug(hdr + " pending joints dict contains Name: " + jointName);
+            }
+            m_log.Debug(hdr + SOPName_to_pendingJoint.Keys.Count + " total pending joints dict entries");
+            foreach (PhysicsJoint j in activeJoints)
+            {
+                m_log.Debug(hdr + " active joint, Name: " + j.ObjectNameInScene + " raw parms:" + j.RawParams);
+            }
+            m_log.Debug(hdr + activeJoints.Count + " total active joints");
+            foreach (string jointName in SOPName_to_activeJoint.Keys)
+            {
+                m_log.Debug(hdr + " active joints dict contains Name: " + jointName);
+            }
+            m_log.Debug(hdr + SOPName_to_activeJoint.Keys.Count + " total active joints dict entries");
+
+            m_log.Debug(hdr + " Per-body joint connectivity information follows.");
+            m_log.Debug(hdr + joints_connecting_actor.Keys.Count + " bodies are connected by joints.");
+            foreach (string actorName in joints_connecting_actor.Keys)
+            {
+                m_log.Debug(hdr + " Actor " + actorName + " has the following joints connecting it");
+                foreach (PhysicsJoint j in joints_connecting_actor[actorName])
+                {
+                    m_log.Debug(hdr + " * joint Name: " + j.ObjectNameInScene + " raw parms:" + j.RawParams);
+                }
+                m_log.Debug(hdr + joints_connecting_actor[actorName].Count + " connecting joints total for this actor");
+            }
+        }
+
+        public override void RequestJointDeletion(string ObjectNameInScene)
+        {
+            lock (externalJointRequestsLock)
+            {
+                if (!requestedJointsToBeDeleted.Contains(ObjectNameInScene)) // forbid same deletion request from entering twice to prevent spurious deletions processed asynchronously
+                {
+                    requestedJointsToBeDeleted.Add(ObjectNameInScene);
+                }
+            }
+        }
+
+        private void DeleteRequestedJoints()
+        {
+            List<string> myRequestedJointsToBeDeleted;
+            lock (externalJointRequestsLock)
+            {
+                // make a local copy of the shared list for processing (threading issues)
+                myRequestedJointsToBeDeleted = new List<string>(requestedJointsToBeDeleted);
+            }
+
+            foreach (string jointName in myRequestedJointsToBeDeleted)
+            {
+                lock (OdeLock)
+                {
+                    //m_log.Debug("[NINJA] trying to deleting requested joint " + jointName);
+                    if (SOPName_to_activeJoint.ContainsKey(jointName) || SOPName_to_pendingJoint.ContainsKey(jointName))
+                    {
+                        OdePhysicsJoint joint = null;
+                        if (SOPName_to_activeJoint.ContainsKey(jointName))
+                        {
+                            joint = SOPName_to_activeJoint[jointName] as OdePhysicsJoint;
+                            InternalRemoveActiveJoint(joint);
+                        }
+                        else if (SOPName_to_pendingJoint.ContainsKey(jointName))
+                        {
+                            joint = SOPName_to_pendingJoint[jointName] as OdePhysicsJoint;
+                            InternalRemovePendingJoint(joint);
+                        }
+
+                        if (joint != null)
+                        {
+                            //m_log.Debug("joint.BodyNames.Count is " + joint.BodyNames.Count + " and contents " + joint.BodyNames);
+                            for (int iBodyName = 0; iBodyName < 2; iBodyName++)
+                            {
+                                string bodyName = joint.BodyNames[iBodyName];
+                                if (bodyName != "NULL")
+                                {
+                                    joints_connecting_actor[bodyName].Remove(joint);
+                                    if (joints_connecting_actor[bodyName].Count == 0)
+                                    {
+                                        joints_connecting_actor.Remove(bodyName);
+                                    }
+                                }
+                            }
+
+                            DoJointDeactivated(joint);
+                            if (joint.jointID != IntPtr.Zero)
+                            {
+                                d.JointDestroy(joint.jointID);
+                                joint.jointID = IntPtr.Zero;
+                                //DoJointErrorMessage(joint, "successfully destroyed joint " + jointName);
+                            }
+                            else
+                            {
+                                //m_log.Warn("[NINJA] Ignoring re-request to destroy joint " + jointName);
+                            }
+                        }
+                        else
+                        {
+                            // DoJointErrorMessage(joint, "coult not find joint to destroy based on name " + jointName);
+                        }
+                    }
+                    else
+                    {
+                        // DoJointErrorMessage(joint, "WARNING - joint removal failed, joint " + jointName);
+                    }
+                }
+            }
+
+            // remove processed joints from the shared list
+            lock (externalJointRequestsLock)
+            {
+                foreach (string jointName in myRequestedJointsToBeDeleted)
+                {
+                    requestedJointsToBeDeleted.Remove(jointName);
+                }
+            }
+        }
+
+        // for pending joints we don't know if their associated bodies exist yet or not.
+        // the joint is actually created during processing of the taints
+        private void CreateRequestedJoints()
+        {
+            List<PhysicsJoint> myRequestedJointsToBeCreated;
+            lock (externalJointRequestsLock)
+            {
+                // make a local copy of the shared list for processing (threading issues)
+                myRequestedJointsToBeCreated = new List<PhysicsJoint>(requestedJointsToBeCreated);
+            }
+
+            foreach (PhysicsJoint joint in myRequestedJointsToBeCreated)
+            {
+                lock (OdeLock)
+                {
+                    if (SOPName_to_pendingJoint.ContainsKey(joint.ObjectNameInScene) && SOPName_to_pendingJoint[joint.ObjectNameInScene] != null)
+                    {
+                        DoJointErrorMessage(joint, "WARNING: ignoring request to re-add already pending joint Name:" + joint.ObjectNameInScene + " type:" + joint.Type + " parms: " + joint.RawParams + " pos: " + joint.Position + " rot:" + joint.Rotation);
+                        continue;
+                    }
+                    if (SOPName_to_activeJoint.ContainsKey(joint.ObjectNameInScene) && SOPName_to_activeJoint[joint.ObjectNameInScene] != null)
+                    {
+                        DoJointErrorMessage(joint, "WARNING: ignoring request to re-add already active joint Name:" + joint.ObjectNameInScene + " type:" + joint.Type + " parms: " + joint.RawParams + " pos: " + joint.Position + " rot:" + joint.Rotation);
+                        continue;
+                    }
+
+                    InternalAddPendingJoint(joint as OdePhysicsJoint);
+
+                    if (joint.BodyNames.Count >= 2)
+                    {
+                        for (int iBodyName = 0; iBodyName < 2; iBodyName++)
+                        {
+                            string bodyName = joint.BodyNames[iBodyName];
+                            if (bodyName != "NULL")
+                            {
+                                if (!joints_connecting_actor.ContainsKey(bodyName))
+                                {
+                                    joints_connecting_actor.Add(bodyName, new List<PhysicsJoint>());
+                                }
+                                joints_connecting_actor[bodyName].Add(joint);
+                            }
+                        }
+                    }
+                }
+            }
+
+            // remove processed joints from shared list
+            lock (externalJointRequestsLock)
+            {
+                foreach (PhysicsJoint joint in myRequestedJointsToBeCreated)
+                {
+                    requestedJointsToBeCreated.Remove(joint);
+                }
+            }
+
+        }
+
+        // public function to add an request for joint creation
+        // this joint will just be added to a waiting list that is NOT processed during the main
+        // Simulate() loop (to avoid deadlocks). After Simulate() is finished, we handle unprocessed joint requests.
+
+        public override PhysicsJoint RequestJointCreation(string objectNameInScene, PhysicsJointType jointType, Vector3 position,
+                                            Quaternion rotation, string parms, List<string> bodyNames, string trackedBodyName, Quaternion localRotation)
+
+        {
+
+            OdePhysicsJoint joint = new OdePhysicsJoint();
+            joint.ObjectNameInScene = objectNameInScene;
+            joint.Type = jointType;
+            joint.Position = position;
+            joint.Rotation = rotation;
+            joint.RawParams = parms;
+            joint.BodyNames = new List<string>(bodyNames);
+            joint.TrackedBodyName = trackedBodyName; 
+            joint.LocalRotation = localRotation;
+            joint.jointID = IntPtr.Zero;
+            joint.ErrorMessageCount = 0;
+
+            lock (externalJointRequestsLock)
+            {
+                if (!requestedJointsToBeCreated.Contains(joint)) // forbid same creation request from entering twice 
+                {
+                    requestedJointsToBeCreated.Add(joint);
+                }
+            }
+            return joint;
+        }
+
+        private void RemoveAllJointsConnectedToActor(PhysicsActor actor)
+        {
+            //m_log.Debug("RemoveAllJointsConnectedToActor: start");
+            if (actor.SOPName != null && joints_connecting_actor.ContainsKey(actor.SOPName) && joints_connecting_actor[actor.SOPName] != null)
+            {
+
+                List<PhysicsJoint> jointsToRemove = new List<PhysicsJoint>();
+                //TODO: merge these 2 loops (originally it was needed to avoid altering a list being iterated over, but it is no longer needed due to the joint request queue mechanism)
+                foreach (PhysicsJoint j in joints_connecting_actor[actor.SOPName])
+                {
+                    jointsToRemove.Add(j);
+                }
+                foreach (PhysicsJoint j in jointsToRemove)
+                {
+                    //m_log.Debug("RemoveAllJointsConnectedToActor: about to request deletion of " + j.ObjectNameInScene);
+                    RequestJointDeletion(j.ObjectNameInScene);
+                    //m_log.Debug("RemoveAllJointsConnectedToActor: done request deletion of " + j.ObjectNameInScene);
+                    j.TrackedBodyName = null; // *IMMEDIATELY* prevent any further movement of this joint (else a deleted actor might cause spurious tracking motion of the joint for a few frames, leading to the joint proxy object disappearing)
+                }
+            }
+        }
+
+        public override void RemoveAllJointsConnectedToActorThreadLocked(PhysicsActor actor)
+        {
+            //m_log.Debug("RemoveAllJointsConnectedToActorThreadLocked: start");
+            lock (OdeLock)
+            {
+                //m_log.Debug("RemoveAllJointsConnectedToActorThreadLocked: got lock");
+                RemoveAllJointsConnectedToActor(actor);
+            }
+        }
+
+        // normally called from within OnJointMoved, which is called from within a lock (OdeLock)
+        public override Vector3 GetJointAnchor(PhysicsJoint joint)
+        {
+            Debug.Assert(joint.IsInPhysicsEngine);
+            d.Vector3 pos = new d.Vector3();
+
+            if (!(joint is OdePhysicsJoint))
+            {
+                DoJointErrorMessage(joint, "warning: non-ODE joint requesting anchor: " + joint.ObjectNameInScene);
+            }
+            else
+            {
+                OdePhysicsJoint odeJoint = (OdePhysicsJoint)joint;
+                switch (odeJoint.Type)
+                {
+                    case PhysicsJointType.Ball:
+                        d.JointGetBallAnchor(odeJoint.jointID, out pos);
+                        break;
+                    case PhysicsJointType.Hinge:
+                        d.JointGetHingeAnchor(odeJoint.jointID, out pos);
+                        break;
+                }
+            }
+            return new Vector3(pos.X, pos.Y, pos.Z);
+        }
+
+        // normally called from within OnJointMoved, which is called from within a lock (OdeLock)
+        // WARNING: ODE sometimes returns <0,0,0> as the joint axis! Therefore this function
+        // appears to be unreliable. Fortunately we can compute the joint axis ourselves by
+        // keeping track of the joint's original orientation relative to one of the involved bodies.
+        public override Vector3 GetJointAxis(PhysicsJoint joint)
+        {
+            Debug.Assert(joint.IsInPhysicsEngine);
+            d.Vector3 axis = new d.Vector3();
+
+            if (!(joint is OdePhysicsJoint))
+            {
+                DoJointErrorMessage(joint, "warning: non-ODE joint requesting anchor: " + joint.ObjectNameInScene);
+            }
+            else
+            {
+                OdePhysicsJoint odeJoint = (OdePhysicsJoint)joint;
+                switch (odeJoint.Type)
+                {
+                    case PhysicsJointType.Ball:
+                        DoJointErrorMessage(joint, "warning - axis requested for ball joint: " + joint.ObjectNameInScene);
+                        break;
+                    case PhysicsJointType.Hinge:
+                        d.JointGetHingeAxis(odeJoint.jointID, out axis);
+                        break;
+                }
+            }
+            return new Vector3(axis.X, axis.Y, axis.Z);
+        }
+
+
+        public void remActivePrim(OdePrim deactivatePrim)
+        {
+            lock (_activeprims)
+            {
+                _activeprims.Remove(deactivatePrim);
+            }
+        }
+
+        public override void RemovePrim(PhysicsActor prim)
+        {
+            if (prim is OdePrim)
+            {
+                lock (OdeLock)
+                {
+                    OdePrim p = (OdePrim) prim;
+
+                    p.setPrimForRemoval();
+                    AddPhysicsActorTaint(prim);
+                    //RemovePrimThreadLocked(p);
+                }
+            }
+        }
+
+        /// <summary>
+        /// This is called from within simulate but outside the locked portion
+        /// We need to do our own locking here
+        /// Essentially, we need to remove the prim from our space segment, whatever segment it's in.
+        ///
+        /// If there are no more prim in the segment, we need to empty (spacedestroy)the segment and reclaim memory
+        /// that the space was using.
+        /// </summary>
+        /// <param name="prim"></param>
+        public void RemovePrimThreadLocked(OdePrim prim)
+        {
+//Console.WriteLine("RemovePrimThreadLocked " +  prim.m_primName);            
+            lock (prim)
+            {
+                remCollisionEventReporting(prim);
+                lock (ode)
+                {
+                    if (prim.prim_geom != IntPtr.Zero)
+                    {
+                        prim.ResetTaints();
+
+                        if (prim.IsPhysical)
+                        {
+                            prim.disableBody();
+                            if (prim.childPrim)
+                            {
+                                prim.childPrim = false;
+                                prim.Body = IntPtr.Zero;
+                                prim.m_disabled = true;
+                                prim.IsPhysical = false;
+                            }
+
+
+                        }
+                        // we don't want to remove the main space
+
+                        // If the geometry is in the targetspace, remove it from the target space
+                        //m_log.Warn(prim.m_targetSpace);
+
+                        //if (prim.m_targetSpace != IntPtr.Zero)
+                        //{
+                        //if (d.SpaceQuery(prim.m_targetSpace, prim.prim_geom))
+                        //{
+
+                        //if (d.GeomIsSpace(prim.m_targetSpace))
+                        //{
+                        //waitForSpaceUnlock(prim.m_targetSpace);
+                        //d.SpaceRemove(prim.m_targetSpace, prim.prim_geom);
+                        prim.m_targetSpace = IntPtr.Zero;
+                        //}
+                        //else
+                        //{
+                        // m_log.Info("[Physics]: Invalid Scene passed to 'removeprim from scene':" +
+                        //((OdePrim)prim).m_targetSpace.ToString());
+                        //}
+
+                        //}
+                        //}
+                        //m_log.Warn(prim.prim_geom);
+                        try
+                        {
+                            if (prim.prim_geom != IntPtr.Zero)
+                            {
+                                d.GeomDestroy(prim.prim_geom);
+                                prim.prim_geom = IntPtr.Zero;
+                            }
+                            else
+                            {
+                                m_log.Warn("[PHYSICS]: Unable to remove prim from physics scene");
+                            }
+                        }
+                        catch (AccessViolationException)
+                        {
+                            m_log.Info("[PHYSICS]: Couldn't remove prim from physics scene, it was already be removed.");
+                        }
+                        lock (_prims)
+                        _prims.Remove(prim);
+
+                        //If there are no more geometries in the sub-space, we don't need it in the main space anymore
+                        //if (d.SpaceGetNumGeoms(prim.m_targetSpace) == 0)
+                        //{
+                        //if (prim.m_targetSpace != null)
+                        //{
+                        //if (d.GeomIsSpace(prim.m_targetSpace))
+                        //{
+                        //waitForSpaceUnlock(prim.m_targetSpace);
+                        //d.SpaceRemove(space, prim.m_targetSpace);
+                        // free up memory used by the space.
+                        //d.SpaceDestroy(prim.m_targetSpace);
+                        //int[] xyspace = calculateSpaceArrayItemFromPos(prim.Position);
+                        //resetSpaceArrayItemToZero(xyspace[0], xyspace[1]);
+                        //}
+                        //else
+                        //{
+                        //m_log.Info("[Physics]: Invalid Scene passed to 'removeprim from scene':" +
+                        //((OdePrim) prim).m_targetSpace.ToString());
+                        //}
+                        //}
+                        //}
+
+                        if (SupportsNINJAJoints)
+                        {
+                            RemoveAllJointsConnectedToActorThreadLocked(prim);
+                        }
+                    }
+                }
+            }
+        }
+
+        #endregion
+
+        #region Space Separation Calculation
+
+        /// <summary>
+        /// Takes a space pointer and zeros out the array we're using to hold the spaces
+        /// </summary>
+        /// <param name="pSpace"></param>
+        public void resetSpaceArrayItemToZero(IntPtr pSpace)
+        {
+            for (int x = 0; x < staticPrimspace.GetLength(0); x++)
+            {
+                for (int y = 0; y < staticPrimspace.GetLength(1); y++)
+                {
+                    if (staticPrimspace[x, y] == pSpace)
+                        staticPrimspace[x, y] = IntPtr.Zero;
+                }
+            }
+        }
+
+        public void resetSpaceArrayItemToZero(int arrayitemX, int arrayitemY)
+        {
+            staticPrimspace[arrayitemX, arrayitemY] = IntPtr.Zero;
+        }
+
+        /// <summary>
+        /// Called when a static prim moves.  Allocates a space for the prim based on its position
+        /// </summary>
+        /// <param name="geom">the pointer to the geom that moved</param>
+        /// <param name="pos">the position that the geom moved to</param>
+        /// <param name="currentspace">a pointer to the space it was in before it was moved.</param>
+        /// <returns>a pointer to the new space it's in</returns>
+        public IntPtr recalculateSpaceForGeom(IntPtr geom, Vector3 pos, IntPtr currentspace)
+        {
+            // Called from setting the Position and Size of an ODEPrim so
+            // it's already in locked space.
+
+            // we don't want to remove the main space
+            // we don't need to test physical here because this function should
+            // never be called if the prim is physical(active)
+
+            // All physical prim end up in the root space
+            //Thread.Sleep(20);
+            if (currentspace != space)
+            {
+                //m_log.Info("[SPACE]: C:" + currentspace.ToString() + " g:" + geom.ToString());
+                //if (currentspace == IntPtr.Zero)
+               //{
+                    //int adfadf = 0;
+                //}
+                if (d.SpaceQuery(currentspace, geom) && currentspace != IntPtr.Zero)
+                {
+                    if (d.GeomIsSpace(currentspace))
+                    {
+                        waitForSpaceUnlock(currentspace);
+                        d.SpaceRemove(currentspace, geom);
+                    }
+                    else
+                    {
+                        m_log.Info("[Physics]: Invalid Scene passed to 'recalculatespace':" + currentspace +
+                                   " Geom:" + geom);
+                    }
+                }
+                else
+                {
+                    IntPtr sGeomIsIn = d.GeomGetSpace(geom);
+                    if (sGeomIsIn != IntPtr.Zero)
+                    {
+                        if (d.GeomIsSpace(currentspace))
+                        {
+                            waitForSpaceUnlock(sGeomIsIn);
+                            d.SpaceRemove(sGeomIsIn, geom);
+                        }
+                        else
+                        {
+                            m_log.Info("[Physics]: Invalid Scene passed to 'recalculatespace':" +
+                                       sGeomIsIn + " Geom:" + geom);
+                        }
+                    }
+                }
+
+                //If there are no more geometries in the sub-space, we don't need it in the main space anymore
+                if (d.SpaceGetNumGeoms(currentspace) == 0)
+                {
+                    if (currentspace != IntPtr.Zero)
+                    {
+                        if (d.GeomIsSpace(currentspace))
+                        {
+                            waitForSpaceUnlock(currentspace);
+                            waitForSpaceUnlock(space);
+                            d.SpaceRemove(space, currentspace);
+                            // free up memory used by the space.
+
+                            //d.SpaceDestroy(currentspace);
+                            resetSpaceArrayItemToZero(currentspace);
+                        }
+                        else
+                        {
+                            m_log.Info("[Physics]: Invalid Scene passed to 'recalculatespace':" +
+                                       currentspace + " Geom:" + geom);
+                        }
+                    }
+                }
+            }
+            else
+            {
+                // this is a physical object that got disabled. ;.;
+                if (currentspace != IntPtr.Zero && geom != IntPtr.Zero)
+                {
+                    if (d.SpaceQuery(currentspace, geom))
+                    {
+                        if (d.GeomIsSpace(currentspace))
+                        {
+                            waitForSpaceUnlock(currentspace);
+                            d.SpaceRemove(currentspace, geom);
+                        }
+                        else
+                        {
+                            m_log.Info("[Physics]: Invalid Scene passed to 'recalculatespace':" +
+                                       currentspace + " Geom:" + geom);
+                        }
+                    }
+                    else
+                    {
+                        IntPtr sGeomIsIn = d.GeomGetSpace(geom);
+                        if (sGeomIsIn != IntPtr.Zero)
+                        {
+                            if (d.GeomIsSpace(sGeomIsIn))
+                            {
+                                waitForSpaceUnlock(sGeomIsIn);
+                                d.SpaceRemove(sGeomIsIn, geom);
+                            }
+                            else
+                            {
+                                m_log.Info("[Physics]: Invalid Scene passed to 'recalculatespace':" +
+                                           sGeomIsIn + " Geom:" + geom);
+                            }
+                        }
+                    }
+                }
+            }
+
+            // The routines in the Position and Size sections do the 'inserting' into the space,
+            // so all we have to do is make sure that the space that we're putting the prim into
+            // is in the 'main' space.
+            int[] iprimspaceArrItem = calculateSpaceArrayItemFromPos(pos);
+            IntPtr newspace = calculateSpaceForGeom(pos);
+
+            if (newspace == IntPtr.Zero)
+            {
+                newspace = createprimspace(iprimspaceArrItem[0], iprimspaceArrItem[1]);
+                d.HashSpaceSetLevels(newspace, smallHashspaceLow, smallHashspaceHigh);
+            }
+
+            return newspace;
+        }
+
+        /// <summary>
+        /// Creates a new space at X Y
+        /// </summary>
+        /// <param name="iprimspaceArrItemX"></param>
+        /// <param name="iprimspaceArrItemY"></param>
+        /// <returns>A pointer to the created space</returns>
+        public IntPtr createprimspace(int iprimspaceArrItemX, int iprimspaceArrItemY)
+        {
+            // creating a new space for prim and inserting it into main space.
+            staticPrimspace[iprimspaceArrItemX, iprimspaceArrItemY] = d.HashSpaceCreate(IntPtr.Zero);
+            d.GeomSetCategoryBits(staticPrimspace[iprimspaceArrItemX, iprimspaceArrItemY], (int)CollisionCategories.Space);
+            waitForSpaceUnlock(space);
+            d.SpaceSetSublevel(space, 1);
+            d.SpaceAdd(space, staticPrimspace[iprimspaceArrItemX, iprimspaceArrItemY]);
+            return staticPrimspace[iprimspaceArrItemX, iprimspaceArrItemY];
+        }
+
+        /// <summary>
+        /// Calculates the space the prim should be in by its position
+        /// </summary>
+        /// <param name="pos"></param>
+        /// <returns>a pointer to the space. This could be a new space or reused space.</returns>
+        public IntPtr calculateSpaceForGeom(Vector3 pos)
+        {
+            int[] xyspace = calculateSpaceArrayItemFromPos(pos);
+            //m_log.Info("[Physics]: Attempting to use arrayItem: " + xyspace[0].ToString() + "," + xyspace[1].ToString());
+            return staticPrimspace[xyspace[0], xyspace[1]];
+        }
+
+        /// <summary>
+        /// Holds the space allocation logic
+        /// </summary>
+        /// <param name="pos"></param>
+        /// <returns>an array item based on the position</returns>
+        public int[] calculateSpaceArrayItemFromPos(Vector3 pos)
+        {
+            int[] returnint = new int[2];
+
+            returnint[0] = (int) (pos.X/metersInSpace);
+
+            if (returnint[0] > ((int) (259f/metersInSpace)))
+                returnint[0] = ((int) (259f/metersInSpace));
+            if (returnint[0] < 0)
+                returnint[0] = 0;
+
+            returnint[1] = (int) (pos.Y/metersInSpace);
+            if (returnint[1] > ((int) (259f/metersInSpace)))
+                returnint[1] = ((int) (259f/metersInSpace));
+            if (returnint[1] < 0)
+                returnint[1] = 0;
+
+            return returnint;
+        }
+
+        #endregion
+
+        /// <summary>
+        /// Routine to figure out if we need to mesh this prim with our mesher
+        /// </summary>
+        /// <param name="pbs"></param>
+        /// <returns></returns>
+        public bool needsMeshing(PrimitiveBaseShape pbs)
+        {
+            // most of this is redundant now as the mesher will return null if it cant mesh a prim
+            // but we still need to check for sculptie meshing being enabled so this is the most
+            // convenient place to do it for now...
+
+        //    //if (pbs.PathCurve == (byte)Primitive.PathCurve.Circle && pbs.ProfileCurve == (byte)Primitive.ProfileCurve.Circle && pbs.PathScaleY <= 0.75f)
+        //    //m_log.Debug("needsMeshing: " + " pathCurve: " + pbs.PathCurve.ToString() + " profileCurve: " + pbs.ProfileCurve.ToString() + " pathScaleY: " + Primitive.UnpackPathScale(pbs.PathScaleY).ToString());
+            int iPropertiesNotSupportedDefault = 0;
+
+            if (pbs.SculptEntry && !meshSculptedPrim)
+            {
+#if SPAM
+                m_log.Warn("NonMesh");
+#endif
+                return false;
+            }
+
+            // if it's a standard box or sphere with no cuts, hollows, twist or top shear, return false since ODE can use an internal representation for the prim
+            if (!forceSimplePrimMeshing)
+            {
+                if ((pbs.ProfileShape == ProfileShape.Square && pbs.PathCurve == (byte)Extrusion.Straight)
+                    || (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1
+                    && pbs.Scale.X == pbs.Scale.Y && pbs.Scale.Y == pbs.Scale.Z))
+                {
+
+                    if (pbs.ProfileBegin == 0 && pbs.ProfileEnd == 0
+                        && pbs.ProfileHollow == 0
+                        && pbs.PathTwist == 0 && pbs.PathTwistBegin == 0
+                        && pbs.PathBegin == 0 && pbs.PathEnd == 0
+                        && pbs.PathTaperX == 0 && pbs.PathTaperY == 0
+                        && pbs.PathScaleX == 100 && pbs.PathScaleY == 100
+                        && pbs.PathShearX == 0 && pbs.PathShearY == 0)
+                    {
+#if SPAM
+                    m_log.Warn("NonMesh");
+#endif
+                        return false;
+                    }
+                }
+            }
+
+            if (pbs.ProfileHollow != 0)
+                iPropertiesNotSupportedDefault++;
+
+            if ((pbs.PathTwistBegin != 0) || (pbs.PathTwist != 0))
+                iPropertiesNotSupportedDefault++; 
+
+            if ((pbs.ProfileBegin != 0) || pbs.ProfileEnd != 0)
+                iPropertiesNotSupportedDefault++;
+
+            if ((pbs.PathScaleX != 100) || (pbs.PathScaleY != 100))
+                iPropertiesNotSupportedDefault++;
+
+            if ((pbs.PathShearX != 0) || (pbs.PathShearY != 0))
+                iPropertiesNotSupportedDefault++;
+
+            if (pbs.ProfileShape == ProfileShape.Circle && pbs.PathCurve == (byte)Extrusion.Straight)
+                iPropertiesNotSupportedDefault++;
+
+            if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1 && (pbs.Scale.X != pbs.Scale.Y || pbs.Scale.Y != pbs.Scale.Z || pbs.Scale.Z != pbs.Scale.X))
+                iPropertiesNotSupportedDefault++;
+
+            if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte) Extrusion.Curve1)
+                iPropertiesNotSupportedDefault++;
+
+            // test for torus
+            if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Square)
+            {
+                if (pbs.PathCurve == (byte)Extrusion.Curve1)
+                {
+                    iPropertiesNotSupportedDefault++;
+                }
+            }
+            else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Circle)
+            {
+                if (pbs.PathCurve == (byte)Extrusion.Straight)
+                {
+                    iPropertiesNotSupportedDefault++;
+                }
+
+                // ProfileCurve seems to combine hole shape and profile curve so we need to only compare against the lower 3 bits
+                else if (pbs.PathCurve == (byte)Extrusion.Curve1)
+                {
+                    iPropertiesNotSupportedDefault++;
+                }
+            }
+            else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.HalfCircle)
+            {
+                if (pbs.PathCurve == (byte)Extrusion.Curve1 || pbs.PathCurve == (byte)Extrusion.Curve2)
+                {
+                    iPropertiesNotSupportedDefault++;
+                }
+            }
+            else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.EquilateralTriangle)
+            {
+                if (pbs.PathCurve == (byte)Extrusion.Straight)
+                {
+                    iPropertiesNotSupportedDefault++;
+                }
+                else if (pbs.PathCurve == (byte)Extrusion.Curve1)
+                {
+                    iPropertiesNotSupportedDefault++;
+                }
+            }
+
+
+            if (iPropertiesNotSupportedDefault == 0)
+            {
+#if SPAM
+                m_log.Warn("NonMesh");
+#endif
+                return false;
+            }
+#if SPAM
+            m_log.Debug("Mesh");
+#endif
+            return true; 
+        }
+
+        /// <summary>
+        /// Called after our prim properties are set Scale, position etc.
+        /// We use this event queue like method to keep changes to the physical scene occuring in the threadlocked mutex
+        /// This assures us that we have no race conditions
+        /// </summary>
+        /// <param name="prim"></param>
+        public override void AddPhysicsActorTaint(PhysicsActor prim)
+        {
+            
+            if (prim is OdePrim)
+            {
+                OdePrim taintedprim = ((OdePrim) prim);
+                lock (_taintedPrimLock)
+                {
+                    if (!(_taintedPrimH.Contains(taintedprim))) 
+                                {
+//Console.WriteLine("AddPhysicsActorTaint to " +  taintedprim.m_primName);                              
+                        _taintedPrimH.Add(taintedprim);                                 // HashSet for searching
+                        _taintedPrimL.Add(taintedprim);                                 // List for ordered readout
+                                        }
+                                }
+                return;
+            }
+            else if (prim is OdeCharacter)
+            {
+                OdeCharacter taintedchar = ((OdeCharacter)prim);
+                lock (_taintedActors)
+                {
+                    if (!(_taintedActors.Contains(taintedchar)))
+                    {
+                        _taintedActors.Add(taintedchar);
+                        if (taintedchar.bad)
+                            m_log.DebugFormat("[PHYSICS]: Added BAD actor {0} to tainted actors", taintedchar.m_uuid);
+                    }
+                }
+            }
+        }
+
+        /// <summary>
+        /// This is our main simulate loop
+        /// It's thread locked by a Mutex in the scene.
+        /// It holds Collisions, it instructs ODE to step through the physical reactions
+        /// It moves the objects around in memory
+        /// It calls the methods that report back to the object owners.. (scenepresence, SceneObjectGroup)
+        /// </summary>
+        /// <param name="timeStep"></param>
+        /// <returns></returns>
+        public override float Simulate(float timeStep)
+        {
+            if (framecount >= int.MaxValue)
+                framecount = 0;
+            //if (m_worldOffset != Vector3.Zero)
+            //    return 0;
+
+            framecount++;
+            
+            DateTime now = DateTime.UtcNow;
+                        TimeSpan SinceLastFrame = now - m_lastframe;
+                        m_lastframe = now;
+                        float realtime = (float)SinceLastFrame.TotalSeconds;
+// Console.WriteLine("ts={0}    rt={1}",        timeStep,       realtime);      
+                        timeStep = realtime;
+                        
+  //          float fps = 1.0f / realtime;
+            float fps = 0.0f;           // number of ODE steps in this Simulate step
+            //m_log.Info(timeStep.ToString());
+            step_time += timeStep;
+            
+            // If We're loaded down by something else,
+            // or debugging with the Visual Studio project on pause
+            // skip a few frames to catch up gracefully.
+            // without shooting the physicsactors all over the place
+
+            if (step_time >= m_SkipFramesAtms)
+            {
+                // Instead of trying to catch up, it'll do 5 physics frames only
+                step_time = ODE_STEPSIZE;
+                m_physicsiterations = 5;
+            }
+            else
+            {
+                m_physicsiterations = 10;
+            }
+
+            if (SupportsNINJAJoints)
+            {
+                DeleteRequestedJoints(); // this must be outside of the lock (OdeLock) to avoid deadlocks
+                CreateRequestedJoints(); // this must be outside of the lock (OdeLock) to avoid deadlocks
+            }
+
+            lock (OdeLock)
+            {
+                // Process 10 frames if the sim is running normal..
+                // process 5 frames if the sim is running slow
+                //try
+                //{
+                    //d.WorldSetQuickStepNumIterations(world, m_physicsiterations);
+                //}
+                //catch (StackOverflowException)
+                //{
+                   // m_log.Error("[PHYSICS]: The operating system wasn't able to allocate enough memory for the simulation.  Restarting the sim.");
+                   // ode.drelease(world);
+                    //base.TriggerPhysicsBasedRestart();
+                //}
+
+                int i = 0;
+
+                // Figure out the Frames Per Second we're going at.
+                //(step_time == 0.004f, there's 250 of those per second.   Times the step time/step size
+
+ //               fps = (step_time / ODE_STEPSIZE) * 1000;
+                // HACK: Using a time dilation of 1.0 to debug rubberbanding issues
+                //m_timeDilation = Math.Min((step_time / ODE_STEPSIZE) / (0.09375f / ODE_STEPSIZE), 1.0f);
+
+ //               step_time = 0.09375f;
+
+                while (step_time > 0.0f)
+                {
+                    //lock (ode)
+                    //{
+                        //if (!ode.lockquery())
+                        //{
+                           // ode.dlock(world);
+                            try
+                            { 
+                                // Insert, remove Characters
+                                bool processedtaints = false;
+
+                                lock (_taintedActors)
+                                {
+                                    if (_taintedActors.Count > 0)
+                                    {
+                                        foreach (OdeCharacter character in _taintedActors)
+                                        {
+
+                                            character.ProcessTaints(ODE_STEPSIZE);
+
+                                            processedtaints = true;
+                                            //character.m_collisionscore = 0;
+                                        }
+
+                                        if (processedtaints)
+                                            _taintedActors.Clear();
+                                    }
+                                } // end lock _taintedActors
+
+                                // Modify other objects in the scene.
+                                processedtaints = false;
+
+                                lock (_taintedPrimLock)
+                                {
+                                    foreach (OdePrim prim in _taintedPrimL)
+                                    {
+                                        if (prim.m_taintremove)
+                                        {
+                                            //Console.WriteLine("Simulate calls RemovePrimThreadLocked");                                          
+                                            RemovePrimThreadLocked(prim);
+                                        }
+                                        else
+                                        {
+                                            //Console.WriteLine("Simulate calls ProcessTaints");                                        
+                                            prim.ProcessTaints(ODE_STEPSIZE);
+                                        }
+                                        processedtaints = true;
+                                        prim.m_collisionscore = 0;
+
+                                        // This loop can block up the Heartbeat for a very long time on large regions.
+                                        // We need to let the Watchdog know that the Heartbeat is not dead
+                                        // NOTE: This is currently commented out, but if things like OAR loading are 
+                                        // timing the heartbeat out we will need to uncomment it
+                                        //Watchdog.UpdateThread();
+                                    }
+
+                                    if (SupportsNINJAJoints)
+                                    {
+                                        // Create pending joints, if possible
+
+                                        // joints can only be processed after ALL bodies are processed (and exist in ODE), since creating
+                                        // a joint requires specifying the body id of both involved bodies
+                                        if (pendingJoints.Count > 0)
+                                        {
+                                            List<PhysicsJoint> successfullyProcessedPendingJoints = new List<PhysicsJoint>();
+                                            //DoJointErrorMessage(joints_connecting_actor, "taint: " + pendingJoints.Count + " pending joints");
+                                            foreach (PhysicsJoint joint in pendingJoints)
+                                            {
+                                                //DoJointErrorMessage(joint, "taint: time to create joint with parms: " + joint.RawParams);
+                                                string[] jointParams = joint.RawParams.Split(" ".ToCharArray(), 
+                                                                                                System.StringSplitOptions.RemoveEmptyEntries);
+                                                List<IntPtr> jointBodies = new List<IntPtr>();
+                                                bool allJointBodiesAreReady = true;
+                                                foreach (string jointParam in jointParams)
+                                                {
+                                                    if (jointParam == "NULL")
+                                                    {
+                                                        //DoJointErrorMessage(joint, "attaching NULL joint to world");
+                                                        jointBodies.Add(IntPtr.Zero);
+                                                    }
+                                                    else
+                                                    {
+                                                        //DoJointErrorMessage(joint, "looking for prim name: " + jointParam);
+                                                        bool foundPrim = false;
+                                                        lock (_prims)
+                                                        {
+                                                            foreach (OdePrim prim in _prims) // FIXME: inefficient
+                                                            {
+                                                                if (prim.SOPName == jointParam)
+                                                                {
+                                                                    //DoJointErrorMessage(joint, "found for prim name: " + jointParam);
+                                                                    if (prim.IsPhysical && prim.Body != IntPtr.Zero)
+                                                                    {
+                                                                        jointBodies.Add(prim.Body);
+                                                                        foundPrim = true;
+                                                                        break;
+                                                                    }
+                                                                    else
+                                                                    {
+                                                                        DoJointErrorMessage(joint, "prim name " + jointParam +
+                                                                            " exists but is not (yet) physical; deferring joint creation. " +
+                                                                            "IsPhysical property is " + prim.IsPhysical +
+                                                                            " and body is " + prim.Body);
+                                                                        foundPrim = false;
+                                                                        break;
+                                                                    }
+                                                                }
+                                                            }
+                                                        }
+                                                        if (foundPrim)
+                                                        {
+                                                            // all is fine
+                                                        }
+                                                        else
+                                                        {
+                                                            allJointBodiesAreReady = false;
+                                                            break;
+                                                        }
+                                                    }
+                                                }
+                                                if (allJointBodiesAreReady)
+                                                {
+                                                    //DoJointErrorMessage(joint, "allJointBodiesAreReady for " + joint.ObjectNameInScene + " with parms " + joint.RawParams);
+                                                    if (jointBodies[0] == jointBodies[1])
+                                                    {
+                                                        DoJointErrorMessage(joint, "ERROR: joint cannot be created; the joint bodies are the same, body1==body2. Raw body is " + jointBodies[0] + ". raw parms: " + joint.RawParams);
+                                                    }
+                                                    else
+                                                    {
+                                                        switch (joint.Type)
+                                                        {
+                                                            case PhysicsJointType.Ball:
+                                                                {
+                                                                    IntPtr odeJoint;
+                                                                    //DoJointErrorMessage(joint, "ODE creating ball joint ");
+                                                                    odeJoint = d.JointCreateBall(world, IntPtr.Zero);
+                                                                    //DoJointErrorMessage(joint, "ODE attaching ball joint: " + odeJoint + " with b1:" + jointBodies[0] + " b2:" + jointBodies[1]);
+                                                                    d.JointAttach(odeJoint, jointBodies[0], jointBodies[1]);
+                                                                    //DoJointErrorMessage(joint, "ODE setting ball anchor: " + odeJoint + " to vec:" + joint.Position);
+                                                                    d.JointSetBallAnchor(odeJoint,
+                                                                                        joint.Position.X,
+                                                                                        joint.Position.Y,
+                                                                                        joint.Position.Z);
+                                                                    //DoJointErrorMessage(joint, "ODE joint setting OK");
+                                                                    //DoJointErrorMessage(joint, "The ball joint's bodies are here: b0: ");
+                                                                    //DoJointErrorMessage(joint, "" + (jointBodies[0] != IntPtr.Zero ? "" + d.BodyGetPosition(jointBodies[0]) : "fixed environment"));
+                                                                    //DoJointErrorMessage(joint, "The ball joint's bodies are here: b1: ");
+                                                                    //DoJointErrorMessage(joint, "" + (jointBodies[1] != IntPtr.Zero ? "" + d.BodyGetPosition(jointBodies[1]) : "fixed environment"));
+
+                                                                    if (joint is OdePhysicsJoint)
+                                                                    {
+                                                                        ((OdePhysicsJoint)joint).jointID = odeJoint;
+                                                                    }
+                                                                    else
+                                                                    {
+                                                                        DoJointErrorMessage(joint, "WARNING: non-ode joint in ODE!");
+                                                                    }
+                                                                }
+                                                                break;
+                                                            case PhysicsJointType.Hinge:
+                                                                {
+                                                                    IntPtr odeJoint;
+                                                                    //DoJointErrorMessage(joint, "ODE creating hinge joint ");
+                                                                    odeJoint = d.JointCreateHinge(world, IntPtr.Zero);
+                                                                    //DoJointErrorMessage(joint, "ODE attaching hinge joint: " + odeJoint + " with b1:" + jointBodies[0] + " b2:" + jointBodies[1]);
+                                                                    d.JointAttach(odeJoint, jointBodies[0], jointBodies[1]);
+                                                                    //DoJointErrorMessage(joint, "ODE setting hinge anchor: " + odeJoint + " to vec:" + joint.Position);
+                                                                    d.JointSetHingeAnchor(odeJoint,
+                                                                                          joint.Position.X,
+                                                                                          joint.Position.Y,
+                                                                                          joint.Position.Z);
+                                                                    // We use the orientation of the x-axis of the joint's coordinate frame
+                                                                    // as the axis for the hinge. 
+
+                                                                    // Therefore, we must get the joint's coordinate frame based on the
+                                                                    // joint.Rotation field, which originates from the orientation of the 
+                                                                    // joint's proxy object in the scene.
+
+                                                                    // The joint's coordinate frame is defined as the transformation matrix
+                                                                    // that converts a vector from joint-local coordinates into world coordinates.
+                                                                    // World coordinates are defined as the XYZ coordinate system of the sim,
+                                                                    // as shown in the top status-bar of the viewer.
+
+                                                                    // Once we have the joint's coordinate frame, we extract its X axis (AtAxis)
+                                                                    // and use that as the hinge axis.
+
+                                                                    //joint.Rotation.Normalize();
+                                                                    Matrix4 proxyFrame = Matrix4.CreateFromQuaternion(joint.Rotation);
+
+                                                                    // Now extract the X axis of the joint's coordinate frame.
+
+                                                                    // Do not try to use proxyFrame.AtAxis or you will become mired in the
+                                                                    // tar pit of transposed, inverted, and generally messed-up orientations.
+                                                                    // (In other words, Matrix4.AtAxis() is borked.)
+                                                                    // Vector3 jointAxis = proxyFrame.AtAxis; <--- this path leadeth to madness
+
+                                                                    // Instead, compute the X axis of the coordinate frame by transforming
+                                                                    // the (1,0,0) vector. At least that works.
+
+                                                                    //m_log.Debug("PHY: making axis: complete matrix is " + proxyFrame);
+                                                                    Vector3 jointAxis = Vector3.Transform(Vector3.UnitX, proxyFrame);
+                                                                    //m_log.Debug("PHY: making axis: hinge joint axis is " + jointAxis); 
+                                                                    //DoJointErrorMessage(joint, "ODE setting hinge axis: " + odeJoint + " to vec:" + jointAxis);
+                                                                    d.JointSetHingeAxis(odeJoint,
+                                                                                        jointAxis.X,
+                                                                                        jointAxis.Y,
+                                                                                        jointAxis.Z);
+                                                                    //d.JointSetHingeParam(odeJoint, (int)dParam.CFM, 0.1f);
+                                                                    if (joint is OdePhysicsJoint)
+                                                                    {
+                                                                        ((OdePhysicsJoint)joint).jointID = odeJoint;
+                                                                    }
+                                                                    else
+                                                                    {
+                                                                        DoJointErrorMessage(joint, "WARNING: non-ode joint in ODE!");
+                                                                    }
+                                                                }
+                                                                break;
+                                                        }
+                                                        successfullyProcessedPendingJoints.Add(joint);
+                                                    }
+                                                }
+                                                else
+                                                {
+                                                    DoJointErrorMessage(joint, "joint could not yet be created; still pending");
+                                                }
+                                            }
+                                            foreach (PhysicsJoint successfullyProcessedJoint in successfullyProcessedPendingJoints)
+                                            {
+                                                //DoJointErrorMessage(successfullyProcessedJoint, "finalizing succesfully procsssed joint " + successfullyProcessedJoint.ObjectNameInScene + " parms " + successfullyProcessedJoint.RawParams);
+                                                //DoJointErrorMessage(successfullyProcessedJoint, "removing from pending");
+                                                InternalRemovePendingJoint(successfullyProcessedJoint);
+                                                //DoJointErrorMessage(successfullyProcessedJoint, "adding to active");
+                                                InternalAddActiveJoint(successfullyProcessedJoint);
+                                                //DoJointErrorMessage(successfullyProcessedJoint, "done");
+                                            }
+                                        }
+                                    } // end SupportsNINJAJoints 
+
+                                    if (processedtaints)
+//Console.WriteLine("Simulate calls Clear of _taintedPrim list");                                      
+                                        _taintedPrimH.Clear();                          // ??? if this only ???
+                                        _taintedPrimL.Clear();
+                                } // end lock _taintedPrimLock
+
+                                // Move characters
+                                lock (_characters)
+                                {
+                                    List<OdeCharacter> defects = new List<OdeCharacter>();
+                                    foreach (OdeCharacter actor in _characters)
+                                    {
+                                        if (actor != null)
+                                            actor.Move(ODE_STEPSIZE, defects);
+                                    }
+                                    if (0 != defects.Count)
+                                    {
+                                        foreach (OdeCharacter defect in defects)
+                                        {
+                                            RemoveCharacter(defect);
+                                        }
+                                    }
+                                } // end lock _characters
+
+                                // Move other active objects
+                                lock (_activeprims)
+                                {
+                                    foreach (OdePrim prim in _activeprims)
+                                    {
+                                        prim.m_collisionscore = 0;
+                                        prim.Move(ODE_STEPSIZE);
+                                    }
+                                } // end lock _activeprims
+
+                                //if ((framecount % m_randomizeWater) == 0)
+                                   // randomizeWater(waterlevel);
+
+                                //int RayCastTimeMS = m_rayCastManager.ProcessQueuedRequests();
+                                m_rayCastManager.ProcessQueuedRequests();
+
+                                collision_optimized(ODE_STEPSIZE);
+
+                                lock (_collisionEventPrim)
+                                {
+                                    foreach (PhysicsActor obj in _collisionEventPrim)
+                                    {
+                                        if (obj == null)
+                                            continue;
+
+                                        switch ((ActorTypes)obj.PhysicsActorType)
+                                        {
+                                            case ActorTypes.Agent:
+                                                OdeCharacter cobj = (OdeCharacter)obj;
+                                                cobj.AddCollisionFrameTime(100);
+                                                cobj.SendCollisions();
+                                                break;
+                                            case ActorTypes.Prim:
+                                                OdePrim pobj = (OdePrim)obj;
+                                                pobj.SendCollisions();
+                                                break;
+                                        }
+                                    }
+                                } // end lock _collisionEventPrim
+
+                                //if (m_global_contactcount > 5)
+                                //{
+                                //    m_log.DebugFormat("[PHYSICS]: Contacts:{0}", m_global_contactcount);
+                                //}
+
+                                m_global_contactcount = 0;
+                                
+                                d.WorldQuickStep(world, ODE_STEPSIZE);
+                                d.JointGroupEmpty(contactgroup);
+                                fps++;
+                                //ode.dunlock(world);
+                            } // end try
+                            catch (Exception e)
+                            {
+                                m_log.ErrorFormat("[PHYSICS]: {0}, {1}, {2}", e.Message, e.TargetSite, e);
+                                ode.dunlock(world);
+                            }
+
+                            step_time -= ODE_STEPSIZE;
+                            i++;
+                        //}
+                        //else
+                        //{
+                            //fps = 0;
+                        //}
+                    //}
+                }  // end while (step_time > 0.0f)
+
+                lock (_characters)
+                {
+                    foreach (OdeCharacter actor in _characters)
+                    {
+                        if (actor != null)
+                        {
+                            if (actor.bad)
+                                m_log.WarnFormat("[PHYSICS]: BAD Actor {0} in _characters list was not removed?", actor.m_uuid);
+                            actor.UpdatePositionAndVelocity();
+                        }
+                    }
+                }
+
+                lock (_badCharacter)
+                {
+                    if (_badCharacter.Count > 0)
+                    {
+                        foreach (OdeCharacter chr in _badCharacter)
+                        {
+                            RemoveCharacter(chr);
+                        }
+                        _badCharacter.Clear();
+                    }
+                }
+
+                lock (_activeprims)
+                {
+                    //if (timeStep < 0.2f)
+                    {
+                        foreach (OdePrim actor in _activeprims)
+                        {
+                            if (actor.IsPhysical && (d.BodyIsEnabled(actor.Body) || !actor._zeroFlag))
+                            {
+                                actor.UpdatePositionAndVelocity();
+
+                                if (SupportsNINJAJoints)
+                                {
+                                    // If an actor moved, move its joint proxy objects as well.
+                                    // There seems to be an event PhysicsActor.OnPositionUpdate that could be used
+                                    // for this purpose but it is never called! So we just do the joint
+                                    // movement code here.
+
+                                    if (actor.SOPName != null &&
+                                        joints_connecting_actor.ContainsKey(actor.SOPName) &&
+                                        joints_connecting_actor[actor.SOPName] != null &&
+                                        joints_connecting_actor[actor.SOPName].Count > 0)
+                                    {
+                                        foreach (PhysicsJoint affectedJoint in joints_connecting_actor[actor.SOPName])
+                                        {
+                                            if (affectedJoint.IsInPhysicsEngine)
+                                            {
+                                                DoJointMoved(affectedJoint);
+                                            }
+                                            else
+                                            {
+                                                DoJointErrorMessage(affectedJoint, "a body connected to a joint was moved, but the joint doesn't exist yet! this will lead to joint error. joint was: " + affectedJoint.ObjectNameInScene + " parms:" + affectedJoint.RawParams);
+                                            }
+                                        }
+                                    }
+                                }
+                            }
+                        }
+                    }
+                } // end lock _activeprims
+
+                //DumpJointInfo();
+
+                // Finished with all sim stepping. If requested, dump world state to file for debugging.
+                // TODO: This call to the export function is already inside lock (OdeLock) - but is an extra lock needed?
+                // TODO: This overwrites all dump files in-place. Should this be a growing logfile, or separate snapshots?
+                if (physics_logging && (physics_logging_interval>0) && (framecount % physics_logging_interval == 0))
+                {
+                    string fname = "state-" + world.ToString() + ".DIF"; // give each physics world a separate filename
+                    string prefix = "world" + world.ToString(); // prefix for variable names in exported .DIF file
+
+                    if (physics_logging_append_existing_logfile)
+                    {
+                        string header = "-------------- START OF PHYSICS FRAME " + framecount.ToString() + " --------------";
+                        TextWriter fwriter = File.AppendText(fname);
+                        fwriter.WriteLine(header);
+                        fwriter.Close();
+                    }
+                    d.WorldExportDIF(world, fname, physics_logging_append_existing_logfile, prefix);
+                }
+            } // end lock OdeLock
+            
+            return fps * 1000.0f;               //NB This is a FRAME COUNT, not a time! AND is divide by 1000 in SimStatusReporter!
+        } // end Simulate
+
+        public override void GetResults()
+        {
+        }
+
+        public override bool IsThreaded
+        {
+            // for now we won't be multithreaded
+            get { return (false); }
+        }
+
+        #region ODE Specific Terrain Fixes
+        public float[] ResizeTerrain512NearestNeighbour(float[] heightMap)
+        {
+            float[] returnarr = new float[262144];
+            float[,] resultarr = new float[(int)WorldExtents.X, (int)WorldExtents.Y];
+
+            // Filling out the array into its multi-dimensional components
+            for (int y = 0; y < WorldExtents.Y; y++)
+            {
+                for (int x = 0; x < WorldExtents.X; x++)
+                {
+                    resultarr[y, x] = heightMap[y * (int)WorldExtents.Y + x];
+                }
+            }
+
+            // Resize using Nearest Neighbour
+
+            // This particular way is quick but it only works on a multiple of the original
+
+            // The idea behind this method can be described with the following diagrams
+            // second pass and third pass happen in the same loop really..  just separated
+            // them to show what this does.
+
+            // First Pass
+            // ResultArr:
+            // 1,1,1,1,1,1
+            // 1,1,1,1,1,1
+            // 1,1,1,1,1,1
+            // 1,1,1,1,1,1
+            // 1,1,1,1,1,1
+            // 1,1,1,1,1,1
+
+            // Second Pass
+            // ResultArr2:
+            // 1,,1,,1,,1,,1,,1,
+            // ,,,,,,,,,,
+            // 1,,1,,1,,1,,1,,1,
+            // ,,,,,,,,,,
+            // 1,,1,,1,,1,,1,,1,
+            // ,,,,,,,,,,
+            // 1,,1,,1,,1,,1,,1,
+            // ,,,,,,,,,,
+            // 1,,1,,1,,1,,1,,1,
+            // ,,,,,,,,,,
+            // 1,,1,,1,,1,,1,,1,
+
+            // Third pass fills in the blanks
+            // ResultArr2:
+            // 1,1,1,1,1,1,1,1,1,1,1,1
+            // 1,1,1,1,1,1,1,1,1,1,1,1
+            // 1,1,1,1,1,1,1,1,1,1,1,1
+            // 1,1,1,1,1,1,1,1,1,1,1,1
+            // 1,1,1,1,1,1,1,1,1,1,1,1
+            // 1,1,1,1,1,1,1,1,1,1,1,1
+            // 1,1,1,1,1,1,1,1,1,1,1,1
+            // 1,1,1,1,1,1,1,1,1,1,1,1
+            // 1,1,1,1,1,1,1,1,1,1,1,1
+            // 1,1,1,1,1,1,1,1,1,1,1,1
+            // 1,1,1,1,1,1,1,1,1,1,1,1
+
+            // X,Y = .
+            // X+1,y = ^
+            // X,Y+1 = *
+            // X+1,Y+1 = #
+
+            // Filling in like this;
+            // .*
+            // ^#
+            // 1st .
+            // 2nd *
+            // 3rd ^
+            // 4th #
+            // on single loop.
+
+            float[,] resultarr2 = new float[512, 512];
+            for (int y = 0; y < WorldExtents.Y; y++)
+            {
+                for (int x = 0; x < WorldExtents.X; x++)
+                {
+                    resultarr2[y * 2, x * 2] = resultarr[y, x];
+
+                    if (y < WorldExtents.Y)
+                    {
+                        resultarr2[(y * 2) + 1, x * 2] = resultarr[y, x];
+                    }
+                    if (x < WorldExtents.X)
+                    {
+                        resultarr2[y * 2, (x * 2) + 1] = resultarr[y, x];
+                    }
+                    if (x < WorldExtents.X && y < WorldExtents.Y)
+                    {
+                        resultarr2[(y * 2) + 1, (x * 2) + 1] = resultarr[y, x];
+                    }
+                }
+            }
+
+            //Flatten out the array
+            int i = 0;
+            for (int y = 0; y < 512; y++)
+            {
+                for (int x = 0; x < 512; x++)
+                {
+                    if (resultarr2[y, x] <= 0)
+                        returnarr[i] = 0.0000001f;
+                    else
+                        returnarr[i] = resultarr2[y, x];
+
+                    i++;
+                }
+            }
+
+            return returnarr;
+        }
+
+        public float[] ResizeTerrain512Interpolation(float[] heightMap)
+        {
+            float[] returnarr = new float[262144];
+            float[,] resultarr = new float[512,512];
+
+            // Filling out the array into its multi-dimensional components
+            for (int y = 0; y < 256; y++)
+            {
+                for (int x = 0; x < 256; x++)
+                {
+                    resultarr[y, x] = heightMap[y * 256 + x];
+                }
+            }
+
+            // Resize using interpolation
+
+            // This particular way is quick but it only works on a multiple of the original
+
+            // The idea behind this method can be described with the following diagrams
+            // second pass and third pass happen in the same loop really..  just separated
+            // them to show what this does.
+
+            // First Pass
+            // ResultArr:
+            // 1,1,1,1,1,1
+            // 1,1,1,1,1,1
+            // 1,1,1,1,1,1
+            // 1,1,1,1,1,1
+            // 1,1,1,1,1,1
+            // 1,1,1,1,1,1
+
+            // Second Pass
+            // ResultArr2:
+            // 1,,1,,1,,1,,1,,1,
+            // ,,,,,,,,,,
+            // 1,,1,,1,,1,,1,,1,
+            // ,,,,,,,,,,
+            // 1,,1,,1,,1,,1,,1,
+            // ,,,,,,,,,,
+            // 1,,1,,1,,1,,1,,1,
+            // ,,,,,,,,,,
+            // 1,,1,,1,,1,,1,,1,
+            // ,,,,,,,,,,
+            // 1,,1,,1,,1,,1,,1,
+
+            // Third pass fills in the blanks
+            // ResultArr2:
+            // 1,1,1,1,1,1,1,1,1,1,1,1
+            // 1,1,1,1,1,1,1,1,1,1,1,1
+            // 1,1,1,1,1,1,1,1,1,1,1,1
+            // 1,1,1,1,1,1,1,1,1,1,1,1
+            // 1,1,1,1,1,1,1,1,1,1,1,1
+            // 1,1,1,1,1,1,1,1,1,1,1,1
+            // 1,1,1,1,1,1,1,1,1,1,1,1
+            // 1,1,1,1,1,1,1,1,1,1,1,1
+            // 1,1,1,1,1,1,1,1,1,1,1,1
+            // 1,1,1,1,1,1,1,1,1,1,1,1
+            // 1,1,1,1,1,1,1,1,1,1,1,1
+
+            // X,Y = .
+            // X+1,y = ^
+            // X,Y+1 = *
+            // X+1,Y+1 = #
+
+            // Filling in like this;
+            // .*
+            // ^#
+            // 1st .
+            // 2nd *
+            // 3rd ^
+            // 4th #
+            // on single loop.
+
+            float[,] resultarr2 = new float[512,512];
+            for (int y = 0; y < (int)Constants.RegionSize; y++)
+            {
+                for (int x = 0; x < (int)Constants.RegionSize; x++)
+                {
+                    resultarr2[y*2, x*2] = resultarr[y, x];
+
+                    if (y < (int)Constants.RegionSize)
+                    {
+                        if (y + 1 < (int)Constants.RegionSize)
+                        {
+                            if (x + 1 < (int)Constants.RegionSize)
+                            {
+                                resultarr2[(y*2) + 1, x*2] = ((resultarr[y, x] + resultarr[y + 1, x] +
+                                                               resultarr[y, x + 1] + resultarr[y + 1, x + 1])/4);
+                            }
+                            else
+                            {
+                                resultarr2[(y*2) + 1, x*2] = ((resultarr[y, x] + resultarr[y + 1, x])/2);
+                            }
+                        }
+                        else
+                        {
+                            resultarr2[(y*2) + 1, x*2] = resultarr[y, x];
+                        }
+                    }
+                    if (x < (int)Constants.RegionSize)
+                    {
+                        if (x + 1 < (int)Constants.RegionSize)
+                        {
+                            if (y + 1 < (int)Constants.RegionSize)
+                            {
+                                resultarr2[y*2, (x*2) + 1] = ((resultarr[y, x] + resultarr[y + 1, x] +
+                                                               resultarr[y, x + 1] + resultarr[y + 1, x + 1])/4);
+                            }
+                            else
+                            {
+                                resultarr2[y*2, (x*2) + 1] = ((resultarr[y, x] + resultarr[y, x + 1])/2);
+                            }
+                        }
+                        else
+                        {
+                            resultarr2[y*2, (x*2) + 1] = resultarr[y, x];
+                        }
+                    }
+                    if (x < (int)Constants.RegionSize && y < (int)Constants.RegionSize)
+                    {
+                        if ((x + 1 < (int)Constants.RegionSize) && (y + 1 < (int)Constants.RegionSize))
+                        {
+                            resultarr2[(y*2) + 1, (x*2) + 1] = ((resultarr[y, x] + resultarr[y + 1, x] +
+                                                                 resultarr[y, x + 1] + resultarr[y + 1, x + 1])/4);
+                        }
+                        else
+                        {
+                            resultarr2[(y*2) + 1, (x*2) + 1] = resultarr[y, x];
+                        }
+                    }
+                }
+            }
+            //Flatten out the array
+            int i = 0;
+            for (int y = 0; y < 512; y++)
+            {
+                for (int x = 0; x < 512; x++)
+                {
+                    if (Single.IsNaN(resultarr2[y, x]) || Single.IsInfinity(resultarr2[y, x]))
+                    {
+                        m_log.Warn("[PHYSICS]: Non finite heightfield element detected.  Setting it to 0");
+                        resultarr2[y, x] = 0;
+                    }
+                    returnarr[i] = resultarr2[y, x];
+                    i++;
+                }
+            }
+
+            return returnarr;
+        }
+
+        #endregion
+
+        public override void SetTerrain(float[] heightMap)
+        {
+            if (m_worldOffset != Vector3.Zero && m_parentScene != null)
+            {
+                if (m_parentScene is OdeScene)
+                {
+                    ((OdeScene)m_parentScene).SetTerrain(heightMap, m_worldOffset);
+                }
+            }
+            else
+            {
+                SetTerrain(heightMap, m_worldOffset);
+            }
+        }
+
+        public void SetTerrain(float[] heightMap, Vector3 pOffset)
+        {
+            // this._heightmap[i] = (double)heightMap[i];
+            // dbm (danx0r) -- creating a buffer zone of one extra sample all around
+            //_origheightmap = heightMap;
+           
+            float[] _heightmap;
+
+            // zero out a heightmap array float array (single dimension [flattened]))
+            //if ((int)Constants.RegionSize == 256)
+            //    _heightmap = new float[514 * 514];
+            //else
+
+            _heightmap = new float[(((int)Constants.RegionSize + 2) * ((int)Constants.RegionSize + 2))];
+
+            uint heightmapWidth = Constants.RegionSize + 1;
+            uint heightmapHeight = Constants.RegionSize + 1;
+
+            uint heightmapWidthSamples;
+
+            uint heightmapHeightSamples;
+
+            //if (((int)Constants.RegionSize) == 256)
+            //{
+            //    heightmapWidthSamples = 2 * (uint)Constants.RegionSize + 2;
+            //    heightmapHeightSamples = 2 * (uint)Constants.RegionSize + 2;
+            //    heightmapWidth++;
+            //    heightmapHeight++;
+            //}
+            //else
+            //{
+
+                heightmapWidthSamples = (uint)Constants.RegionSize + 1;
+                heightmapHeightSamples = (uint)Constants.RegionSize + 1;
+            //}
+
+            const float scale = 1.0f;
+            const float offset = 0.0f;
+            const float thickness = 0.2f;
+            const int wrap = 0;
+
+            int regionsize = (int) Constants.RegionSize + 2;
+            //Double resolution
+            //if (((int)Constants.RegionSize) == 256)
+            //    heightMap = ResizeTerrain512Interpolation(heightMap);
+
+
+           // if (((int)Constants.RegionSize) == 256 && (int)Constants.RegionSize == 256)
+           //     regionsize = 512;
+
+            float hfmin = 2000;
+            float hfmax = -2000;
+            
+                for (int x = 0; x < heightmapWidthSamples; x++)
+                {
+                    for (int y = 0; y < heightmapHeightSamples; y++)
+                    {
+                        int xx = Util.Clip(x - 1, 0, regionsize - 1);
+                        int yy = Util.Clip(y - 1, 0, regionsize - 1);
+                        
+                        
+                        float val= heightMap[yy * (int)Constants.RegionSize + xx];
+                         _heightmap[x * ((int)Constants.RegionSize + 2) + y] = val;
+                        
+                        hfmin = (val < hfmin) ? val : hfmin;
+                        hfmax = (val > hfmax) ? val : hfmax;
+                    }
+                }
+                
+            
+            
+
+            lock (OdeLock)
+            {
+                IntPtr GroundGeom = IntPtr.Zero;
+                if (RegionTerrain.TryGetValue(pOffset, out GroundGeom))
+                {
+                    RegionTerrain.Remove(pOffset);
+                    if (GroundGeom != IntPtr.Zero)
+                    {
+                        if (TerrainHeightFieldHeights.ContainsKey(GroundGeom))
+                        {
+                            TerrainHeightFieldHeights.Remove(GroundGeom);
+                        }
+                        d.SpaceRemove(space, GroundGeom);
+                        d.GeomDestroy(GroundGeom);
+                    }
+
+                }
+                IntPtr HeightmapData = d.GeomHeightfieldDataCreate();
+                d.GeomHeightfieldDataBuildSingle(HeightmapData, _heightmap, 0, heightmapWidth + 1, heightmapHeight + 1,
+                                                 (int)heightmapWidthSamples + 1, (int)heightmapHeightSamples + 1, scale,
+                                                 offset, thickness, wrap);
+                d.GeomHeightfieldDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1);
+                GroundGeom = d.CreateHeightfield(space, HeightmapData, 1);
+                if (GroundGeom != IntPtr.Zero)
+                {
+                    d.GeomSetCategoryBits(GroundGeom, (int)(CollisionCategories.Land));
+                    d.GeomSetCollideBits(GroundGeom, (int)(CollisionCategories.Space));
+
+                }
+                geom_name_map[GroundGeom] = "Terrain";
+
+                d.Matrix3 R = new d.Matrix3();
+
+                Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f);
+                Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f);
+                //Axiom.Math.Quaternion q3 = Axiom.Math.Quaternion.FromAngleAxis(3.14f, new Axiom.Math.Vector3(0, 0, 1));
+
+                q1 = q1 * q2;
+                //q1 = q1 * q3;
+                Vector3 v3;
+                float angle;
+                q1.GetAxisAngle(out v3, out angle);
+
+                d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle);
+                d.GeomSetRotation(GroundGeom, ref R);
+                d.GeomSetPosition(GroundGeom, (pOffset.X + ((int)Constants.RegionSize * 0.5f)) - 1, (pOffset.Y + ((int)Constants.RegionSize * 0.5f)) - 1, 0);
+                IntPtr testGround = IntPtr.Zero;
+                if (RegionTerrain.TryGetValue(pOffset, out testGround))
+                {
+                    RegionTerrain.Remove(pOffset);
+                }
+                RegionTerrain.Add(pOffset, GroundGeom, GroundGeom);
+                TerrainHeightFieldHeights.Add(GroundGeom,_heightmap);
+                
+            }
+        }
+
+        public override void DeleteTerrain()
+        {
+        }
+
+        public float GetWaterLevel()
+        {
+            return waterlevel;
+        }
+
+        public override bool SupportsCombining()
+        {
+            return true;
+        }
+
+        public override void UnCombine(PhysicsScene pScene)
+        {
+            IntPtr localGround = IntPtr.Zero;
+//            float[] localHeightfield;
+            bool proceed = false;
+            List<IntPtr> geomDestroyList = new List<IntPtr>();
+
+            lock (OdeLock)
+            {
+                if (RegionTerrain.TryGetValue(Vector3.Zero, out localGround))
+                {
+                    foreach (IntPtr geom in TerrainHeightFieldHeights.Keys)
+                    {
+                        if (geom == localGround)
+                        {
+//                            localHeightfield = TerrainHeightFieldHeights[geom];
+                            proceed = true;
+                        }
+                        else
+                        {
+                            geomDestroyList.Add(geom);
+                        }
+                    }
+
+                    if (proceed)
+                    {
+                        m_worldOffset = Vector3.Zero;
+                        WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize);
+                        m_parentScene = null;
+
+                        foreach (IntPtr g in geomDestroyList)
+                        {
+                            // removingHeightField needs to be done or the garbage collector will
+                            // collect the terrain data before we tell ODE to destroy it causing 
+                            // memory corruption
+                            if (TerrainHeightFieldHeights.ContainsKey(g))
+                            {
+//                                float[] removingHeightField = TerrainHeightFieldHeights[g];
+                                TerrainHeightFieldHeights.Remove(g);
+
+                                if (RegionTerrain.ContainsKey(g))
+                                {
+                                    RegionTerrain.Remove(g);
+                                }
+
+                                d.GeomDestroy(g);
+                                //removingHeightField = new float[0];
+                            }
+                        }
+
+                    }
+                    else
+                    {
+                        m_log.Warn("[PHYSICS]: Couldn't proceed with UnCombine.  Region has inconsistant data.");
+
+                    }
+                }
+            }
+        }
+
+        public override void SetWaterLevel(float baseheight)
+        {
+            waterlevel = baseheight;
+            randomizeWater(waterlevel);
+        }
+
+        public void randomizeWater(float baseheight)
+        {
+            const uint heightmapWidth = m_regionWidth + 2;
+            const uint heightmapHeight = m_regionHeight + 2;
+            const uint heightmapWidthSamples = m_regionWidth + 2;
+            const uint heightmapHeightSamples = m_regionHeight + 2;
+            const float scale = 1.0f;
+            const float offset = 0.0f;
+            const float thickness = 2.9f;
+            const int wrap = 0;
+
+            for (int i = 0; i < (258 * 258); i++)
+            {
+                _watermap[i] = (baseheight-0.1f) + ((float)fluidRandomizer.Next(1,9) / 10f);
+               // m_log.Info((baseheight - 0.1f) + ((float)fluidRandomizer.Next(1, 9) / 10f));
+            }
+
+            lock (OdeLock)
+            {
+                if (WaterGeom != IntPtr.Zero)
+                {
+                    d.SpaceRemove(space, WaterGeom);
+                }
+                IntPtr HeightmapData = d.GeomHeightfieldDataCreate();
+                d.GeomHeightfieldDataBuildSingle(HeightmapData, _watermap, 0, heightmapWidth, heightmapHeight,
+                                                 (int)heightmapWidthSamples, (int)heightmapHeightSamples, scale,
+                                                 offset, thickness, wrap);
+                d.GeomHeightfieldDataSetBounds(HeightmapData, m_regionWidth, m_regionHeight);
+                WaterGeom = d.CreateHeightfield(space, HeightmapData, 1);
+                if (WaterGeom != IntPtr.Zero)
+                {
+                    d.GeomSetCategoryBits(WaterGeom, (int)(CollisionCategories.Water));
+                    d.GeomSetCollideBits(WaterGeom, (int)(CollisionCategories.Space));
+
+                }
+                geom_name_map[WaterGeom] = "Water";
+
+                d.Matrix3 R = new d.Matrix3();
+
+                Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f);
+                Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f);
+                //Axiom.Math.Quaternion q3 = Axiom.Math.Quaternion.FromAngleAxis(3.14f, new Axiom.Math.Vector3(0, 0, 1));
+
+                q1 = q1 * q2;
+                //q1 = q1 * q3;
+                Vector3 v3;
+                float angle;
+                q1.GetAxisAngle(out v3, out angle);
+
+                d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle);
+                d.GeomSetRotation(WaterGeom, ref R);
+                d.GeomSetPosition(WaterGeom, 128, 128, 0);
+                
+            }
+
+        }
+
+        public override void Dispose()
+        {
+            m_rayCastManager.Dispose();
+            m_rayCastManager = null;
+
+            lock (OdeLock)
+            {
+                lock (_prims)
+                {
+                    foreach (OdePrim prm in _prims)
+                    {
+                        RemovePrim(prm);
+                    }
+                }
+
+                //foreach (OdeCharacter act in _characters)
+                //{
+                    //RemoveAvatar(act);
+                //}
+                d.WorldDestroy(world);
+                //d.CloseODE();
+            }
+        }
+        public override Dictionary<uint, float> GetTopColliders()
+        {
+            Dictionary<uint, float> returncolliders = new Dictionary<uint, float>();
+            int cnt = 0;
+            lock (_prims)
+            {
+                foreach (OdePrim prm in _prims)
+                {
+                    if (prm.CollisionScore > 0)
+                    {
+                        returncolliders.Add(prm.m_localID, prm.CollisionScore);
+                        cnt++;
+                        prm.CollisionScore = 0f;
+                        if (cnt > 25)
+                        {
+                            break;
+                        }
+                    }
+                }
+            }
+            return returncolliders;
+        }
+
+        public override bool SupportsRayCast()
+        {
+            return true;
+        }
+
+        public override void RaycastWorld(Vector3 position, Vector3 direction, float length, RaycastCallback retMethod)
+        {
+            if (retMethod != null)
+            {
+                m_rayCastManager.QueueRequest(position, direction, length, retMethod);
+            }
+        }
+
+#if USE_DRAWSTUFF
+        // Keyboard callback
+        public void command(int cmd)
+        {
+            IntPtr geom;
+            d.Mass mass;
+            d.Vector3 sides = new d.Vector3(d.RandReal() * 0.5f + 0.1f, d.RandReal() * 0.5f + 0.1f, d.RandReal() * 0.5f + 0.1f);
+
+            
+
+            Char ch = Char.ToLower((Char)cmd);
+            switch ((Char)ch)
+            {
+                case 'w':
+                    try
+                    {
+                        Vector3 rotate = (new Vector3(1, 0, 0) * Quaternion.CreateFromEulers(hpr.Z * Utils.DEG_TO_RAD, hpr.Y * Utils.DEG_TO_RAD, hpr.X * Utils.DEG_TO_RAD));
+
+                        xyz.X += rotate.X; xyz.Y += rotate.Y; xyz.Z += rotate.Z;
+                        ds.SetViewpoint(ref xyz, ref hpr);
+                    }
+                    catch (ArgumentException)
+                    { hpr.X = 0; }
+                    break;
+
+                case 'a':
+                    hpr.X++;
+                    ds.SetViewpoint(ref xyz, ref hpr);
+                    break;
+
+                case 's':
+                    try
+                    {
+                        Vector3 rotate2 = (new Vector3(-1, 0, 0) * Quaternion.CreateFromEulers(hpr.Z * Utils.DEG_TO_RAD, hpr.Y * Utils.DEG_TO_RAD, hpr.X * Utils.DEG_TO_RAD));
+
+                        xyz.X += rotate2.X; xyz.Y += rotate2.Y; xyz.Z += rotate2.Z;
+                        ds.SetViewpoint(ref xyz, ref hpr);
+                    }
+                    catch (ArgumentException)
+                    { hpr.X = 0; }
+                    break;
+                case 'd':
+                    hpr.X--;
+                    ds.SetViewpoint(ref xyz, ref hpr);
+                    break;
+                case 'r':
+                    xyz.Z++;
+                    ds.SetViewpoint(ref xyz, ref hpr);
+                    break;
+                case 'f':
+                    xyz.Z--;
+                    ds.SetViewpoint(ref xyz, ref hpr);
+                    break;
+                case 'e':
+                    xyz.Y++;
+                    ds.SetViewpoint(ref xyz, ref hpr);
+                    break;
+                case 'q':
+                    xyz.Y--;
+                    ds.SetViewpoint(ref xyz, ref hpr);
+                    break;
+            }
+        }
+
+        public void step(int pause)
+        {
+            
+            ds.SetColor(1.0f, 1.0f, 0.0f);
+            ds.SetTexture(ds.Texture.Wood);
+            lock (_prims)
+            {
+                foreach (OdePrim prm in _prims)
+                {
+                    //IntPtr body = d.GeomGetBody(prm.prim_geom);
+                    if (prm.prim_geom != IntPtr.Zero)
+                    {
+                        d.Vector3 pos;
+                        d.GeomCopyPosition(prm.prim_geom, out pos);
+                        //d.BodyCopyPosition(body, out pos);
+
+                        d.Matrix3 R;
+                        d.GeomCopyRotation(prm.prim_geom, out R);
+                        //d.BodyCopyRotation(body, out R);
+
+
+                        d.Vector3 sides = new d.Vector3();
+                        sides.X = prm.Size.X;
+                        sides.Y = prm.Size.Y;
+                        sides.Z = prm.Size.Z;
+
+                        ds.DrawBox(ref pos, ref R, ref sides);
+                    }
+                }
+            }
+            ds.SetColor(1.0f, 0.0f, 0.0f);
+            lock (_characters)
+            {
+                foreach (OdeCharacter chr in _characters)
+                {
+                    if (chr.Shell != IntPtr.Zero)
+                    {
+                        IntPtr body = d.GeomGetBody(chr.Shell);
+
+                        d.Vector3 pos;
+                        d.GeomCopyPosition(chr.Shell, out pos);
+                        //d.BodyCopyPosition(body, out pos);
+
+                        d.Matrix3 R;
+                        d.GeomCopyRotation(chr.Shell, out R);
+                        //d.BodyCopyRotation(body, out R);
+
+                        ds.DrawCapsule(ref pos, ref R, chr.Size.Z, 0.35f);
+                        d.Vector3 sides = new d.Vector3();
+                        sides.X = 0.5f;
+                        sides.Y = 0.5f;
+                        sides.Z = 0.5f;
+
+                        ds.DrawBox(ref pos, ref R, ref sides);
+                    }
+                }
+            }
+        }
+
+        public void start(int unused)
+        {
+            ds.SetViewpoint(ref xyz, ref hpr);
+        }
+#endif
+    }
+}
diff --git a/OpenSim/Region/Physics/ChOdePlugin/Tests/ODETestClass.cs b/OpenSim/Region/Physics/ChOdePlugin/Tests/ODETestClass.cs
new file mode 100644
index 0000000..69e2d03
--- /dev/null
+++ b/OpenSim/Region/Physics/ChOdePlugin/Tests/ODETestClass.cs
@@ -0,0 +1,122 @@
+/*
+ * Copyright (c) Contributors, http://opensimulator.org/
+ * See CONTRIBUTORS.TXT for a full list of copyright holders.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the OpenSimulator Project nor the
+ *       names of its contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+using System;
+using Nini.Config;
+using NUnit.Framework;
+using OpenMetaverse;
+using OpenSim.Framework;
+using OpenSim.Region.Physics.Manager;
+using log4net;
+using System.Reflection;
+
+namespace OpenSim.Region.Physics.OdePlugin
+{
+    [TestFixture]
+    public class ODETestClass
+    {
+        private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
+
+        private OdePlugin cbt;
+        private PhysicsScene ps;
+        private IMeshingPlugin imp;
+
+        [SetUp]
+        public void Initialize()
+        {
+            // Loading ODEPlugin
+            cbt = new OdePlugin();
+            // Loading Zero Mesher
+            imp = new ZeroMesherPlugin();
+            // Getting Physics Scene
+            ps = cbt.GetScene("test");
+            // Initializing Physics Scene.
+            ps.Initialise(imp.GetMesher(),null);
+            float[] _heightmap = new float[(int)Constants.RegionSize * (int)Constants.RegionSize];
+            for (int i = 0; i < ((int)Constants.RegionSize * (int)Constants.RegionSize); i++)
+            {
+                _heightmap[i] = 21f;
+            }
+            ps.SetTerrain(_heightmap);
+        }
+
+        [TearDown]
+        public void Terminate()
+        {
+            ps.DeleteTerrain();
+            ps.Dispose();
+
+        }
+
+        [Test]
+        public void CreateAndDropPhysicalCube()
+        {
+            PrimitiveBaseShape newcube = PrimitiveBaseShape.CreateBox();
+            Vector3 position = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f), 128f);
+            Vector3 size = new Vector3(0.5f, 0.5f, 0.5f);
+            Quaternion rot = Quaternion.Identity;
+            PhysicsActor prim = ps.AddPrimShape("CoolShape", newcube, position, size, rot, true);
+            OdePrim oprim = (OdePrim)prim;
+            OdeScene pscene = (OdeScene) ps;
+
+            Assert.That(oprim.m_taintadd);
+
+            prim.LocalID = 5;
+
+            for (int i = 0; i < 58; i++)
+            {
+                ps.Simulate(0.133f);
+
+                Assert.That(oprim.prim_geom != (IntPtr)0);
+
+                Assert.That(oprim.m_targetSpace != (IntPtr)0);
+
+                //Assert.That(oprim.m_targetSpace == pscene.space);
+                m_log.Info("TargetSpace: " + oprim.m_targetSpace + " - SceneMainSpace: " + pscene.space);
+
+                Assert.That(!oprim.m_taintadd);
+                m_log.Info("Prim Position (" + oprim.m_localID + "): " + prim.Position.ToString());
+
+                // Make sure we're above the ground
+                //Assert.That(prim.Position.Z > 20f);
+                //m_log.Info("PrimCollisionScore (" + oprim.m_localID + "): " + oprim.m_collisionscore);
+
+                // Make sure we've got a Body
+                Assert.That(oprim.Body != (IntPtr)0);
+                //m_log.Info(
+            }
+
+            // Make sure we're not somewhere above the ground
+            Assert.That(prim.Position.Z < 21.5f);
+
+            ps.RemovePrim(prim);
+            Assert.That(oprim.m_taintremove);
+            ps.Simulate(0.133f);
+            Assert.That(oprim.Body == (IntPtr)0);
+        }
+    }
+}
diff --git a/OpenSim/Region/Physics/ChOdePlugin/drawstuff.cs b/OpenSim/Region/Physics/ChOdePlugin/drawstuff.cs
new file mode 100644
index 0000000..87ca446
--- /dev/null
+++ b/OpenSim/Region/Physics/ChOdePlugin/drawstuff.cs
@@ -0,0 +1,98 @@
+/*
+ * Copyright ODE
+ * Ode.NET - .NET bindings for ODE
+ *  Jason Perkins (starkos@industriousone.com)
+ *  Licensed under the New BSD
+ *  Part of the OpenDynamicsEngine
+Open Dynamics Engine
+Copyright (c) 2001-2007, Russell L. Smith.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions
+are met:
+
+Redistributions of source code must retain the above copyright notice,
+this list of conditions and the following disclaimer.
+
+Redistributions in binary form must reproduce the above copyright notice,
+this list of conditions and the following disclaimer in the documentation
+and/or other materials provided with the distribution.
+
+Neither the names of ODE's copyright owner nor the names of its
+contributors may be used to endorse or promote products derived from
+this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ * 
+ * 
+ */
+
+using System;
+using System.Runtime.InteropServices;
+using Ode.NET;
+
+namespace Drawstuff.NET
+{
+#if dDOUBLE
+    using dReal = System.Double;
+#else
+    using dReal = System.Single;
+#endif
+
+    public static class ds
+    {
+        public const int VERSION = 2;
+
+        public enum Texture
+        {
+            None,
+            Wood
+        }
+
+        [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
+        public delegate void CallbackFunction(int arg);
+
+        [StructLayout(LayoutKind.Sequential)]
+        public struct Functions
+        {
+            public int version;
+            public CallbackFunction start;
+            public CallbackFunction step;
+            public CallbackFunction command;
+            public CallbackFunction stop;
+            public string path_to_textures;
+        }
+
+        [DllImport("drawstuff", EntryPoint = "dsDrawBox")]
+        public static extern void DrawBox(ref d.Vector3 pos, ref d.Matrix3 R, ref d.Vector3 sides);
+
+        [DllImport("drawstuff", EntryPoint = "dsDrawCapsule")]
+        public static extern void DrawCapsule(ref d.Vector3 pos, ref d.Matrix3 R, dReal length, dReal radius);
+
+        [DllImport("drawstuff", EntryPoint = "dsDrawConvex")]
+        public static extern void DrawConvex(ref d.Vector3 pos, ref d.Matrix3 R, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons);
+
+        [DllImport("drawstuff", EntryPoint = "dsSetColor")]
+        public static extern void SetColor(float red, float green, float blue);
+
+        [DllImport("drawstuff", EntryPoint = "dsSetTexture")]
+        public static extern void SetTexture(Texture texture);
+
+        [DllImport("drawstuff", EntryPoint = "dsSetViewpoint")]
+        public static extern void SetViewpoint(ref d.Vector3 xyz, ref d.Vector3 hpr);
+
+        [DllImport("drawstuff", EntryPoint = "dsSimulationLoop")]
+        public static extern void SimulationLoop(int argc, string[] argv, int window_width, int window_height, ref Functions fn);
+    }
+}
diff --git a/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs
index a2229e8..2a66060 100644
--- a/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs
+++ b/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs
@@ -1210,7 +1210,6 @@ namespace OpenSim.Region.Physics.OdePlugin
 
         public override float APIDDamping{ set { return; } }
 
-
         public override void SubscribeEvents(int ms)
         {
             m_requestedUpdateFrequency = ms;
diff --git a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
index 0720b5e..be7c348 100644
--- a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
+++ b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
@@ -2831,7 +2831,7 @@ Console.WriteLine(" JointCreateFixed");
         }
         public override bool PIDActive { set { m_usePID = value; } }
         public override float PIDTau { set { m_PIDTau = value; } }
-
+        
         public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } }
         public override bool PIDHoverActive { set { m_useHoverPID = value; } }
         public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } }
diff --git a/OpenSim/Region/Physics/POSPlugin/POSPrim.cs b/OpenSim/Region/Physics/POSPlugin/POSPrim.cs
index 91ec3df..9ba5ebb 100644
--- a/OpenSim/Region/Physics/POSPlugin/POSPrim.cs
+++ b/OpenSim/Region/Physics/POSPlugin/POSPrim.cs
@@ -301,7 +301,7 @@ namespace OpenSim.Region.Physics.POSPlugin
         {
             set { return; }
         }
-        
+
         public override Quaternion APIDTarget
         {
             set { return; }
diff --git a/OpenSim/Region/RegionCombinerModule/RegionCombinerModule.cs b/OpenSim/Region/RegionCombinerModule/RegionCombinerModule.cs
index 9cb349a..b42d3bf 100644
--- a/OpenSim/Region/RegionCombinerModule/RegionCombinerModule.cs
+++ b/OpenSim/Region/RegionCombinerModule/RegionCombinerModule.cs
@@ -88,89 +88,7 @@ namespace OpenSim.Region.RegionCombinerModule
         public void RegionLoaded(Scene scene)
         {
             if (enabledYN)
-            {
                 RegionLoadedDoWork(scene);
-
-                scene.EventManager.OnNewPresence += NewPresence;
-            }
-        }
-
-        private void NewPresence(ScenePresence presence)
-        {
-            if (presence.IsChildAgent)
-            {
-                byte[] throttleData;
-
-                try
-                {
-                    throttleData = presence.ControllingClient.GetThrottlesPacked(1);
-                } 
-                catch (NotImplementedException)
-                {
-                    return;
-                }
-
-                if (throttleData == null)
-                    return;
-
-                if (throttleData.Length == 0)
-                    return;
-
-                if (throttleData.Length != 28)
-                    return;
-
-                byte[] adjData;
-                int pos = 0;
-
-                if (!BitConverter.IsLittleEndian)
-                {
-                    byte[] newData = new byte[7 * 4];
-                    Buffer.BlockCopy(throttleData, 0, newData, 0, 7 * 4);
-
-                    for (int i = 0; i < 7; i++)
-                        Array.Reverse(newData, i * 4, 4);
-
-                    adjData = newData;
-                }
-                else
-                {
-                    adjData = throttleData;
-                }
-
-                // 0.125f converts from bits to bytes
-                int resend = (int)(BitConverter.ToSingle(adjData, pos) * 0.125f); pos += 4;
-                int land = (int)(BitConverter.ToSingle(adjData, pos) * 0.125f); pos += 4;
-                int wind = (int)(BitConverter.ToSingle(adjData, pos) * 0.125f); pos += 4;
-                int cloud = (int)(BitConverter.ToSingle(adjData, pos) * 0.125f); pos += 4;
-                int task = (int)(BitConverter.ToSingle(adjData, pos) * 0.125f); pos += 4;
-                int texture = (int)(BitConverter.ToSingle(adjData, pos) * 0.125f); pos += 4;
-                int asset = (int)(BitConverter.ToSingle(adjData, pos) * 0.125f);
-                // State is a subcategory of task that we allocate a percentage to
-
-
-                //int total = resend + land + wind + cloud + task + texture + asset;
-
-                byte[] data = new byte[7 * 4];
-                int ii = 0;
-
-                Buffer.BlockCopy(Utils.FloatToBytes(resend), 0, data, ii, 4); ii += 4;
-                Buffer.BlockCopy(Utils.FloatToBytes(land * 50), 0, data, ii, 4); ii += 4;
-                Buffer.BlockCopy(Utils.FloatToBytes(wind), 0, data, ii, 4); ii += 4;
-                Buffer.BlockCopy(Utils.FloatToBytes(cloud), 0, data, ii, 4); ii += 4;
-                Buffer.BlockCopy(Utils.FloatToBytes(task), 0, data, ii, 4); ii += 4;
-                Buffer.BlockCopy(Utils.FloatToBytes(texture), 0, data, ii, 4); ii += 4;
-                Buffer.BlockCopy(Utils.FloatToBytes(asset), 0, data, ii, 4);
-
-                try
-                {
-                    presence.ControllingClient.SetChildAgentThrottle(data);
-                }
-                catch (NotImplementedException)
-                {
-                    return;
-                }
-
-            }
         }
 
         private void RegionLoadedDoWork(Scene scene)
diff --git a/OpenSim/Region/ScriptEngine/Shared/Api/Implementation/LSL_Api.cs b/OpenSim/Region/ScriptEngine/Shared/Api/Implementation/LSL_Api.cs
index 3ccbb3a..3f630f4 100644
--- a/OpenSim/Region/ScriptEngine/Shared/Api/Implementation/LSL_Api.cs
+++ b/OpenSim/Region/ScriptEngine/Shared/Api/Implementation/LSL_Api.cs
@@ -28,6 +28,7 @@
 using System;
 using System.Collections;
 using System.Collections.Generic;
+using System.Diagnostics; //for [DebuggerNonUserCode]
 using System.Runtime.Remoting.Lifetime;
 using System.Text;
 using System.Threading;
@@ -151,6 +152,7 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
             get { return m_ScriptEngine.World; }
         }
 
+        [DebuggerNonUserCode]
         public void state(string newState)
         {
             m_ScriptEngine.SetState(m_itemID, newState);
@@ -160,6 +162,7 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
         /// Reset the named script. The script must be present
         /// in the same prim.
         /// </summary>
+        [DebuggerNonUserCode]
         public void llResetScript()
         {
             m_host.AddScriptLPS(1);
@@ -219,6 +222,8 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
         public List<SceneObjectPart> GetLinkParts(int linkType)
         {
             List<SceneObjectPart> ret = new List<SceneObjectPart>();
+            if (m_host == null || m_host.ParentGroup == null || m_host.ParentGroup.IsDeleted)
+                return ret;
             ret.Add(m_host);
 
             switch (linkType)
@@ -272,40 +277,48 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
         protected UUID InventorySelf()
         {
             UUID invItemID = new UUID();
-
-            lock (m_host.TaskInventory)
+            bool unlock = false;
+            if (!m_host.TaskInventory.IsReadLockedByMe())
             {
-                foreach (KeyValuePair<UUID, TaskInventoryItem> inv in m_host.TaskInventory)
+                m_host.TaskInventory.LockItemsForRead(true);
+                unlock = true;
+            }
+            foreach (KeyValuePair<UUID, TaskInventoryItem> inv in m_host.TaskInventory)
+            {
+                if (inv.Value.Type == 10 && inv.Value.ItemID == m_itemID)
                 {
-                    if (inv.Value.Type == 10 && inv.Value.ItemID == m_itemID)
-                    {
-                        invItemID = inv.Key;
-                        break;
-                    }
+                    invItemID = inv.Key;
+                    break;
                 }
             }
-
+            if (unlock)
+            {
+                m_host.TaskInventory.LockItemsForRead(false);
+            }
             return invItemID;
         }
 
         protected UUID InventoryKey(string name, int type)
         {
             m_host.AddScriptLPS(1);
-
-            lock (m_host.TaskInventory)
+            m_host.TaskInventory.LockItemsForRead(true);
+            
+            foreach (KeyValuePair<UUID, TaskInventoryItem> inv in m_host.TaskInventory)
             {
-                foreach (KeyValuePair<UUID, TaskInventoryItem> inv in m_host.TaskInventory)
+                if (inv.Value.Name == name)
                 {
-                    if (inv.Value.Name == name)
+                    m_host.TaskInventory.LockItemsForRead(false);
+                    
+                    if (inv.Value.Type != type)
                     {
-                        if (inv.Value.Type != type)
-                            return UUID.Zero;
-
-                        return inv.Value.AssetID;
+                        return UUID.Zero;
                     }
+
+                    return inv.Value.AssetID;
                 }
             }
 
+            m_host.TaskInventory.LockItemsForRead(false);
             return UUID.Zero;
         }
 
@@ -313,17 +326,21 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
         {
             m_host.AddScriptLPS(1);
 
-            lock (m_host.TaskInventory)
+            
+            m_host.TaskInventory.LockItemsForRead(true);
+
+            foreach (KeyValuePair<UUID, TaskInventoryItem> inv in m_host.TaskInventory)
             {
-                foreach (KeyValuePair<UUID, TaskInventoryItem> inv in m_host.TaskInventory)
+                if (inv.Value.Name == name)
                 {
-                    if (inv.Value.Name == name)
-                    {
-                        return inv.Value.AssetID;
-                    }
+                    m_host.TaskInventory.LockItemsForRead(false);
+                    return inv.Value.AssetID;
                 }
             }
 
+            m_host.TaskInventory.LockItemsForRead(false);
+
+
             return UUID.Zero;
         }
 
@@ -705,6 +722,9 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
         {
             //A and B should both be normalized
             m_host.AddScriptLPS(1);
+            /*  This method is more accurate than the SL one, and thus causes problems
+                for scripts that deal with the SL inaccuracy around 180-degrees -.- .._.
+                
             double dotProduct = LSL_Vector.Dot(a, b);
             LSL_Vector crossProduct = LSL_Vector.Cross(a, b);
             double magProduct = LSL_Vector.Mag(a) * LSL_Vector.Mag(b);
@@ -721,8 +741,57 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
                 return new LSL_Rotation(0.0f, 0.0f, 0.0f, 1.0f);
 
             return new LSL_Rotation((float)x, (float)y, (float)z, (float)w);
-        }
-
+            */
+            
+            // This method mimics the 180 errors found in SL
+            // See www.euclideanspace.com... angleBetween
+                        LSL_Vector vec_a = a;
+                        LSL_Vector vec_b = b;
+                        
+                        // Eliminate zero length
+                        LSL_Float vec_a_mag = LSL_Vector.Mag(vec_a);
+                        LSL_Float vec_b_mag = LSL_Vector.Mag(vec_b);
+                        if (vec_a_mag < 0.00001 ||
+                            vec_b_mag < 0.00001)
+                        {
+                                return new LSL_Rotation(0.0f, 0.0f, 0.0f, 1.0f);
+                        }
+                        
+                        // Normalize
+                        vec_a = llVecNorm(vec_a);
+                        vec_b = llVecNorm(vec_b);
+
+                        // Calculate axis and rotation angle
+                        LSL_Vector axis = vec_a % vec_b;
+                        LSL_Float cos_theta  = vec_a * vec_b;
+        
+                        // Check if parallel
+                        if (cos_theta > 0.99999)
+                        {
+                                return new LSL_Rotation(0.0f, 0.0f, 0.0f, 1.0f);
+                        }
+                        
+                        // Check if anti-parallel
+                        else if (cos_theta < -0.99999)
+                        {
+                                LSL_Vector orthog_axis = new LSL_Vector(1.0, 0.0, 0.0) - (vec_a.x / (vec_a * vec_a) * vec_a);
+                                if (LSL_Vector.Mag(orthog_axis)  < 0.000001)  orthog_axis = new LSL_Vector(0.0, 0.0, 1.0);
+                    return new LSL_Rotation((float)orthog_axis.x, (float)orthog_axis.y, (float)orthog_axis.z, 0.0);
+                        }
+                        else // other rotation
+                        {
+                                LSL_Float theta = (LSL_Float)Math.Acos(cos_theta) * 0.5f;
+                                axis = llVecNorm(axis);
+                    double x, y, z, s, t;
+                    s = Math.Cos(theta);
+                    t = Math.Sin(theta);
+                    x = axis.x * t;
+                    y = axis.y * t;
+                    z = axis.z * t;
+                    return new LSL_Rotation(x,y,z,s);
+                }
+        }
+                                
         public void llWhisper(int channelID, string text)
         {
             m_host.AddScriptLPS(1);
@@ -1046,10 +1115,11 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
             return detectedParams.TouchUV;
         }
 
+        [DebuggerNonUserCode]
         public virtual void llDie()
         {
             m_host.AddScriptLPS(1);
-            throw new SelfDeleteException();
+            if (!m_host.IsAttachment) throw new SelfDeleteException();
         }
 
         public LSL_Float llGround(LSL_Vector offset)
@@ -1122,6 +1192,8 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
 
         public void llSetStatus(int status, int value)
         {
+            if (m_host == null || m_host.ParentGroup == null || m_host.ParentGroup.IsDeleted)
+                return;
             m_host.AddScriptLPS(1);
 
             int statusrotationaxis = 0;
@@ -1351,6 +1423,48 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
         {
             m_host.AddScriptLPS(1);
 
+            SetColor(m_host, color, face);
+        }
+
+        protected void SetColor(SceneObjectPart part, LSL_Vector color, int face)
+        {
+            if (part == null || part.ParentGroup == null || part.ParentGroup.IsDeleted)
+                return;
+
+            Primitive.TextureEntry tex = part.Shape.Textures;
+            Color4 texcolor;
+            if (face >= 0 && face < GetNumberOfSides(part))
+            {
+                texcolor = tex.CreateFace((uint)face).RGBA;
+                texcolor.R = Util.Clip((float)color.x, 0.0f, 1.0f);
+                texcolor.G = Util.Clip((float)color.y, 0.0f, 1.0f);
+                texcolor.B = Util.Clip((float)color.z, 0.0f, 1.0f);
+                tex.FaceTextures[face].RGBA = texcolor;
+                part.UpdateTexture(tex);
+                return;
+            }
+            else if (face == ScriptBaseClass.ALL_SIDES)
+            {
+                for (uint i = 0; i < GetNumberOfSides(part); i++)
+                {
+                    if (tex.FaceTextures[i] != null)
+                    {
+                        texcolor = tex.FaceTextures[i].RGBA;
+                        texcolor.R = Util.Clip((float)color.x, 0.0f, 1.0f);
+                        texcolor.G = Util.Clip((float)color.y, 0.0f, 1.0f);
+                        texcolor.B = Util.Clip((float)color.z, 0.0f, 1.0f);
+                        tex.FaceTextures[i].RGBA = texcolor;
+                    }
+                    texcolor = tex.DefaultTexture.RGBA;
+                    texcolor.R = Util.Clip((float)color.x, 0.0f, 1.0f);
+                    texcolor.G = Util.Clip((float)color.y, 0.0f, 1.0f);
+                    texcolor.B = Util.Clip((float)color.z, 0.0f, 1.0f);
+                    tex.DefaultTexture.RGBA = texcolor;
+                }
+                part.UpdateTexture(tex);
+                return;
+            }
+
             if (face == ScriptBaseClass.ALL_SIDES)
                 face = SceneObjectPart.ALL_SIDES;
             
@@ -1359,6 +1473,9 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
 
         public void SetTexGen(SceneObjectPart part, int face,int style)
         {
+            if (part == null || part.ParentGroup == null || part.ParentGroup.IsDeleted)
+                return;
+
             Primitive.TextureEntry tex = part.Shape.Textures;
             MappingType textype;
             textype = MappingType.Default;
@@ -1389,6 +1506,9 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
 
         public void SetGlow(SceneObjectPart part, int face, float glow)
         {
+            if (part == null || part.ParentGroup == null || part.ParentGroup.IsDeleted)
+                return;
+
             Primitive.TextureEntry tex = part.Shape.Textures;
             if (face >= 0 && face < GetNumberOfSides(part))
             {
@@ -1414,6 +1534,8 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
 
         public void SetShiny(SceneObjectPart part, int face, int shiny, Bumpiness bump)
         {
+            if (part == null || part.ParentGroup == null || part.ParentGroup.IsDeleted)
+                return;
 
             Shininess sval = new Shininess();
 
@@ -1464,6 +1586,9 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
 
         public void SetFullBright(SceneObjectPart part, int face, bool bright)
         {
+            if (part == null || part.ParentGroup == null || part.ParentGroup.IsDeleted)
+                return;
+
              Primitive.TextureEntry tex = part.Shape.Textures;
              if (face >= 0 && face < GetNumberOfSides(part))
              {
@@ -1531,6 +1656,9 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
 
         protected void SetAlpha(SceneObjectPart part, double alpha, int face)
         {
+            if (part == null || part.ParentGroup == null || part.ParentGroup.IsDeleted)
+                return;
+
             Primitive.TextureEntry tex = part.Shape.Textures;
             Color4 texcolor;
             if (face >= 0 && face < GetNumberOfSides(part))
@@ -1576,7 +1704,7 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
         protected void SetFlexi(SceneObjectPart part, bool flexi, int softness, float gravity, float friction,
             float wind, float tension, LSL_Vector Force)
         {
-            if (part == null)
+            if (part == null || part.ParentGroup == null || part.ParentGroup.IsDeleted)
                 return;
 
             if (flexi)
@@ -1611,7 +1739,7 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
         /// <param name="falloff"></param>
         protected void SetPointLight(SceneObjectPart part, bool light, LSL_Vector color, float intensity, float radius, float falloff)
         {
-            if (part == null)
+            if (part == null || part.ParentGroup == null || part.ParentGroup.IsDeleted)
                 return;
 
             if (light)
@@ -1697,6 +1825,9 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
 
         protected void SetTexture(SceneObjectPart part, string texture, int face)
         {
+            if (part == null || part.ParentGroup == null || part.ParentGroup.IsDeleted)
+                return;
+
             UUID textureID=new UUID();
 
             if (!UUID.TryParse(texture, out textureID))
@@ -1742,6 +1873,9 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
 
         protected void ScaleTexture(SceneObjectPart part, double u, double v, int face)
         {
+            if (part == null || part.ParentGroup == null || part.ParentGroup.IsDeleted)
+                return;
+
             Primitive.TextureEntry tex = part.Shape.Textures;
             if (face >= 0 && face < GetNumberOfSides(part))
             {
@@ -1778,6 +1912,9 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
 
         protected void OffsetTexture(SceneObjectPart part, double u, double v, int face)
         {
+            if (part == null || part.ParentGroup == null || part.ParentGroup.IsDeleted)
+                return;
+
             Primitive.TextureEntry tex = part.Shape.Textures;
             if (face >= 0 && face < GetNumberOfSides(part))
             {
@@ -1814,6 +1951,9 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
 
         protected void RotateTexture(SceneObjectPart part, double rotation, int face)
         {
+            if (part == null || part.ParentGroup == null || part.ParentGroup.IsDeleted)
+                return;
+
             Primitive.TextureEntry tex = part.Shape.Textures;
             if (face >= 0 && face < GetNumberOfSides(part))
             {
@@ -1884,6 +2024,9 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
 
         protected void SetPos(SceneObjectPart part, LSL_Vector targetPos)
         {
+            if (part == null || part.ParentGroup == null || part.ParentGroup.IsDeleted)
+                return;
+
             // Capped movemment if distance > 10m (http://wiki.secondlife.com/wiki/LlSetPos)
             LSL_Vector currentPos = llGetLocalPos();
 
@@ -1978,6 +2121,9 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
 
         protected void SetRot(SceneObjectPart part, Quaternion rot)
         {
+            if (part == null || part.ParentGroup == null || part.ParentGroup.IsDeleted)
+                return;
+
             part.UpdateRotation(rot);
             // Update rotation does not move the object in the physics scene if it's a linkset.
 
@@ -2597,12 +2743,9 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
 
             m_host.AddScriptLPS(1);
 
+            m_host.TaskInventory.LockItemsForRead(true);
             TaskInventoryItem item = m_host.TaskInventory[invItemID];
-
-            lock (m_host.TaskInventory)
-            {
-                item = m_host.TaskInventory[invItemID];
-            }
+            m_host.TaskInventory.LockItemsForRead(false);
 
             if (item.PermsGranter == UUID.Zero)
                 return 0;
@@ -2677,6 +2820,7 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
             if (dist > m_ScriptDistanceFactor * 10.0f)
                 return;
 
+            //Clone is thread-safe
             TaskInventoryDictionary partInventory = (TaskInventoryDictionary)m_host.TaskInventory.Clone();
 
             foreach (KeyValuePair<UUID, TaskInventoryItem> inv in partInventory)
@@ -2756,10 +2900,20 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
             // the angles of rotation in radians into rotation value
 
             LSL_Types.Quaternion rot = llEuler2Rot(angle);
+            /*
             Quaternion rotation = new Quaternion((float)rot.x, (float)rot.y, (float)rot.z, (float)rot.s);
             m_host.startLookAt(rotation, (float)damping, (float)strength);
+                This would only work if your physics system contains an APID controller */
             // Orient the object to the angle calculated
-            //llSetRot(rot);
+            llSetRot(rot);
+        }
+        
+        public void llRotLookAt(LSL_Rotation target, double strength, double damping)
+        {
+            m_host.AddScriptLPS(1);
+//            NotImplemented("llRotLookAt");
+            m_host.RotLookAt(Rot2Quaternion(target), (float)strength, (float)damping);
+            
         }
 
         public void llStopLookAt()
@@ -2808,13 +2962,17 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
         {
             TaskInventoryItem item;
 
-            lock (m_host.TaskInventory)
+            m_host.TaskInventory.LockItemsForRead(true);
+            if (!m_host.TaskInventory.ContainsKey(InventorySelf()))
             {
-                if (!m_host.TaskInventory.ContainsKey(InventorySelf()))
-                    return;
-                else
-                    item = m_host.TaskInventory[InventorySelf()];
+                m_host.TaskInventory.LockItemsForRead(false);
+                return;
+            }
+            else
+            {
+                item = m_host.TaskInventory[InventorySelf()];
             }
+            m_host.TaskInventory.LockItemsForRead(false);
 
             if (item.PermsGranter != UUID.Zero)
             {
@@ -2836,13 +2994,21 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
         {
             TaskInventoryItem item;
 
+            m_host.TaskInventory.LockItemsForRead(true);
             lock (m_host.TaskInventory)
             {
+                
                 if (!m_host.TaskInventory.ContainsKey(InventorySelf()))
+                {
+                    m_host.TaskInventory.LockItemsForRead(false);
                     return;
+                }
                 else
+                {
                     item = m_host.TaskInventory[InventorySelf()];
+                }
             }
+            m_host.TaskInventory.LockItemsForRead(false);
 
             m_host.AddScriptLPS(1);
 
@@ -2879,14 +3045,20 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
 
             TaskInventoryItem item;
 
-            lock (m_host.TaskInventory)
+            m_host.TaskInventory.LockItemsForRead(true);
+
+            if (!m_host.TaskInventory.ContainsKey(InventorySelf()))
             {
-                if (!m_host.TaskInventory.ContainsKey(InventorySelf()))
-                    return;
-                else
-                    item = m_host.TaskInventory[InventorySelf()];
+                m_host.TaskInventory.LockItemsForRead(false);
+                return;
+            }
+            else
+            {
+                item = m_host.TaskInventory[InventorySelf()];
             }
 
+            m_host.TaskInventory.LockItemsForRead(false);
+
             if (item.PermsGranter != m_host.OwnerID)
                 return;
 
@@ -2913,13 +3085,19 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
 
             TaskInventoryItem item;
 
-            lock (m_host.TaskInventory)
+            m_host.TaskInventory.LockItemsForRead(true);
+
+            if (!m_host.TaskInventory.ContainsKey(InventorySelf()))
             {
-                if (!m_host.TaskInventory.ContainsKey(InventorySelf()))
-                    return;
-                else
-                    item = m_host.TaskInventory[InventorySelf()];
+                m_host.TaskInventory.LockItemsForRead(false);
+                return;
+            }
+            else
+            {
+                item = m_host.TaskInventory[InventorySelf()];
             }
+            m_host.TaskInventory.LockItemsForRead(false);
+
 
             if (item.PermsGranter != m_host.OwnerID)
                 return;
@@ -2956,8 +3134,17 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
             return m_host.OwnerID.ToString();
         }
 
+        [DebuggerNonUserCode]
         public void llInstantMessage(string user, string message)
         {
+            UUID result;
+            if (!UUID.TryParse(user, out result))
+            {
+                throw new Exception(String.Format("An invalid key of '{0} was passed to llInstantMessage", user));
+                return;
+            }
+            
+
             m_host.AddScriptLPS(1);
 
             // We may be able to use ClientView.SendInstantMessage here, but we need a client instance.
@@ -2972,7 +3159,7 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
             UUID friendTransactionID = UUID.Random();
 
             //m_pendingFriendRequests.Add(friendTransactionID, fromAgentID);
-
+            
             GridInstantMessage msg = new GridInstantMessage();
             msg.fromAgentID = new Guid(m_host.UUID.ToString()); // fromAgentID.Guid;
             msg.toAgentID = new Guid(user); // toAgentID.Guid;
@@ -3121,13 +3308,6 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
             m_host.AddScriptLPS(1);
         }
 
-        public void llRotLookAt(LSL_Rotation target, double strength, double damping)
-        {
-            m_host.AddScriptLPS(1);
-            Quaternion rot = new Quaternion((float)target.x, (float)target.y, (float)target.z, (float)target.s);
-            m_host.RotLookAt(rot, (float)strength, (float)damping);
-        }
-
         public LSL_Integer llStringLength(string str)
         {
             m_host.AddScriptLPS(1);
@@ -3151,14 +3331,17 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
 
             TaskInventoryItem item;
 
-            lock (m_host.TaskInventory)
+            m_host.TaskInventory.LockItemsForRead(true);
+            if (!m_host.TaskInventory.ContainsKey(InventorySelf()))
             {
-                if (!m_host.TaskInventory.ContainsKey(InventorySelf()))
-                    return;
-                else
-                    item = m_host.TaskInventory[InventorySelf()];
+                m_host.TaskInventory.LockItemsForRead(false);
+                return;
             }
-
+            else
+            {
+                item = m_host.TaskInventory[InventorySelf()];
+            }
+            m_host.TaskInventory.LockItemsForRead(false);
             if (item.PermsGranter == UUID.Zero)
                 return;
 
@@ -3188,13 +3371,18 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
 
             TaskInventoryItem item;
 
-            lock (m_host.TaskInventory)
+            m_host.TaskInventory.LockItemsForRead(true);
+            if (!m_host.TaskInventory.ContainsKey(InventorySelf()))
             {
-                if (!m_host.TaskInventory.ContainsKey(InventorySelf()))
-                    return;
-                else
-                    item = m_host.TaskInventory[InventorySelf()];
+                m_host.TaskInventory.LockItemsForRead(false);
+                return;
+            }
+            else
+            {
+                item = m_host.TaskInventory[InventorySelf()];
             }
+            m_host.TaskInventory.LockItemsForRead(false);
+            
 
             if (item.PermsGranter == UUID.Zero)
                 return;
@@ -3271,10 +3459,18 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
 
             TaskInventoryItem item;
 
-            lock (m_host.TaskInventory)
+
+            m_host.TaskInventory.LockItemsForRead(true);
+            if (!m_host.TaskInventory.ContainsKey(invItemID))
+            {
+                m_host.TaskInventory.LockItemsForRead(false);
+                return;
+            }
+            else
             {
                 item = m_host.TaskInventory[invItemID];
             }
+            m_host.TaskInventory.LockItemsForRead(false);
 
             if (agentID == UUID.Zero || perm == 0) // Releasing permissions
             {
@@ -3306,11 +3502,10 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
 
                 if ((perm & (~implicitPerms)) == 0) // Requested only implicit perms
                 {
-                    lock (m_host.TaskInventory)
-                    {
-                        m_host.TaskInventory[invItemID].PermsGranter = agentID;
-                        m_host.TaskInventory[invItemID].PermsMask = perm;
-                    }
+                    m_host.TaskInventory.LockItemsForWrite(true);
+                    m_host.TaskInventory[invItemID].PermsGranter = agentID;
+                    m_host.TaskInventory[invItemID].PermsMask = perm;
+                    m_host.TaskInventory.LockItemsForWrite(false);
 
                     m_ScriptEngine.PostScriptEvent(m_itemID, new EventParams(
                             "run_time_permissions", new Object[] {
@@ -3330,11 +3525,10 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
 
                 if ((perm & (~implicitPerms)) == 0) // Requested only implicit perms
                 {
-                    lock (m_host.TaskInventory)
-                    {
-                        m_host.TaskInventory[invItemID].PermsGranter = agentID;
-                        m_host.TaskInventory[invItemID].PermsMask = perm;
-                    }
+                    m_host.TaskInventory.LockItemsForWrite(true);
+                    m_host.TaskInventory[invItemID].PermsGranter = agentID;
+                    m_host.TaskInventory[invItemID].PermsMask = perm;
+                    m_host.TaskInventory.LockItemsForWrite(false);
 
                     m_ScriptEngine.PostScriptEvent(m_itemID, new EventParams(
                             "run_time_permissions", new Object[] {
@@ -3355,11 +3549,10 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
 
                 if (!m_waitingForScriptAnswer)
                 {
-                    lock (m_host.TaskInventory)
-                    {
-                        m_host.TaskInventory[invItemID].PermsGranter = agentID;
-                        m_host.TaskInventory[invItemID].PermsMask = 0;
-                    }
+                    m_host.TaskInventory.LockItemsForWrite(true);
+                    m_host.TaskInventory[invItemID].PermsGranter = agentID;
+                    m_host.TaskInventory[invItemID].PermsMask = 0;
+                    m_host.TaskInventory.LockItemsForWrite(false);
 
                     presence.ControllingClient.OnScriptAnswer += handleScriptAnswer;
                     m_waitingForScriptAnswer=true;
@@ -3394,10 +3587,11 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
             if ((answer & ScriptBaseClass.PERMISSION_TAKE_CONTROLS) == 0)
                 llReleaseControls();
 
-            lock (m_host.TaskInventory)
-            {
-                m_host.TaskInventory[invItemID].PermsMask = answer;
-            }
+            
+            m_host.TaskInventory.LockItemsForWrite(true);
+            m_host.TaskInventory[invItemID].PermsMask = answer;
+            m_host.TaskInventory.LockItemsForWrite(false);
+            
 
             m_ScriptEngine.PostScriptEvent(m_itemID, new EventParams(
                     "run_time_permissions", new Object[] {
@@ -3409,16 +3603,17 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
         {
             m_host.AddScriptLPS(1);
 
-            lock (m_host.TaskInventory)
+            m_host.TaskInventory.LockItemsForRead(true);
+            
+            foreach (TaskInventoryItem item in m_host.TaskInventory.Values)
             {
-                foreach (TaskInventoryItem item in m_host.TaskInventory.Values)
+                if (item.Type == 10 && item.ItemID == m_itemID)
                 {
-                    if (item.Type == 10 && item.ItemID == m_itemID)
-                    {
-                        return item.PermsGranter.ToString();
-                    }
+                    m_host.TaskInventory.LockItemsForRead(false);
+                    return item.PermsGranter.ToString();
                 }
             }
+            m_host.TaskInventory.LockItemsForRead(false);
 
             return UUID.Zero.ToString();
         }
@@ -3427,19 +3622,20 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
         {
             m_host.AddScriptLPS(1);
 
-            lock (m_host.TaskInventory)
+            m_host.TaskInventory.LockItemsForRead(true);
+
+            foreach (TaskInventoryItem item in m_host.TaskInventory.Values)
             {
-                foreach (TaskInventoryItem item in m_host.TaskInventory.Values)
+                if (item.Type == 10 && item.ItemID == m_itemID)
                 {
-                    if (item.Type == 10 && item.ItemID == m_itemID)
-                    {
-                        int perms = item.PermsMask;
-                        if (m_automaticLinkPermission)
-                            perms |= ScriptBaseClass.PERMISSION_CHANGE_LINKS;
-                        return perms;
-                    }
+                    int perms = item.PermsMask;
+                    if (m_automaticLinkPermission)
+                        perms |= ScriptBaseClass.PERMISSION_CHANGE_LINKS;
+                    m_host.TaskInventory.LockItemsForRead(false);
+                    return perms;
                 }
             }
+            m_host.TaskInventory.LockItemsForRead(false);
 
             return 0;
         }
@@ -3472,11 +3668,10 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
             UUID invItemID = InventorySelf();
 
             TaskInventoryItem item;
-            lock (m_host.TaskInventory)
-            {
-                item = m_host.TaskInventory[invItemID];
-            }
-
+            m_host.TaskInventory.LockItemsForRead(true);
+            item = m_host.TaskInventory[invItemID];
+            m_host.TaskInventory.LockItemsForRead(false);
+             
             if ((item.PermsMask & ScriptBaseClass.PERMISSION_CHANGE_LINKS) == 0
                 && !m_automaticLinkPermission)
             {
@@ -3529,16 +3724,16 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
             m_host.AddScriptLPS(1);
             UUID invItemID = InventorySelf();
 
-            lock (m_host.TaskInventory)
-            {
+            m_host.TaskInventory.LockItemsForRead(true);
                 if ((m_host.TaskInventory[invItemID].PermsMask & ScriptBaseClass.PERMISSION_CHANGE_LINKS) == 0
                     && !m_automaticLinkPermission)
                 {
                     ShoutError("Script trying to link but PERMISSION_CHANGE_LINKS permission not set!");
+                    m_host.TaskInventory.LockItemsForRead(false);
                     return;
                 }
-            }
-
+            m_host.TaskInventory.LockItemsForRead(false);
+            
             if (linknum < ScriptBaseClass.LINK_THIS)
                 return;
 
@@ -3715,17 +3910,16 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
             m_host.AddScriptLPS(1);
             int count = 0;
 
-            lock (m_host.TaskInventory)
+            m_host.TaskInventory.LockItemsForRead(true);
+            foreach (KeyValuePair<UUID, TaskInventoryItem> inv in m_host.TaskInventory)
             {
-                foreach (KeyValuePair<UUID, TaskInventoryItem> inv in m_host.TaskInventory)
+                if (inv.Value.Type == type || type == -1)
                 {
-                    if (inv.Value.Type == type || type == -1)
-                    {
-                        count = count + 1;
-                    }
+                    count = count + 1;
                 }
             }
-
+            
+            m_host.TaskInventory.LockItemsForRead(false);
             return count;
         }
 
@@ -3734,16 +3928,15 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
             m_host.AddScriptLPS(1);
             ArrayList keys = new ArrayList();
 
-            lock (m_host.TaskInventory)
+            m_host.TaskInventory.LockItemsForRead(true);
+            foreach (KeyValuePair<UUID, TaskInventoryItem> inv in m_host.TaskInventory)
             {
-                foreach (KeyValuePair<UUID, TaskInventoryItem> inv in m_host.TaskInventory)
+                if (inv.Value.Type == type || type == -1)
                 {
-                    if (inv.Value.Type == type || type == -1)
-                    {
-                        keys.Add(inv.Value.Name);
-                    }
+                    keys.Add(inv.Value.Name);
                 }
             }
+            m_host.TaskInventory.LockItemsForRead(false);
 
             if (keys.Count == 0)
             {
@@ -3780,20 +3973,19 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
             }
 
             // move the first object found with this inventory name
-            lock (m_host.TaskInventory)
+            m_host.TaskInventory.LockItemsForRead(true);
+            foreach (KeyValuePair<UUID, TaskInventoryItem> inv in m_host.TaskInventory)
             {
-                foreach (KeyValuePair<UUID, TaskInventoryItem> inv in m_host.TaskInventory)
+                if (inv.Value.Name == inventory)
                 {
-                    if (inv.Value.Name == inventory)
-                    {
-                        found = true;
-                        objId = inv.Key;
-                        assetType = inv.Value.Type;
-                        objName = inv.Value.Name;
-                        break;
-                    }
+                    found = true;
+                    objId = inv.Key;
+                    assetType = inv.Value.Type;
+                    objName = inv.Value.Name;
+                    break;
                 }
             }
+            m_host.TaskInventory.LockItemsForRead(false);
 
             if (!found)
             {
@@ -3829,33 +4021,38 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
 
                 if (m_TransferModule != null)
                     m_TransferModule.SendInstantMessage(msg, delegate(bool success) {});
+                
+                //This delay should only occur when giving inventory to avatars.
+                ScriptSleep(3000);
             }
             else
             {
                 // destination is an object
                 World.MoveTaskInventoryItem(destId, m_host, objId);
             }
-            ScriptSleep(3000);
+            
         }
 
+        [DebuggerNonUserCode]
         public void llRemoveInventory(string name)
         {
             m_host.AddScriptLPS(1);
 
-            lock (m_host.TaskInventory)
+            m_host.TaskInventory.LockItemsForRead(true);
+            foreach (TaskInventoryItem item in m_host.TaskInventory.Values)
             {
-                foreach (TaskInventoryItem item in m_host.TaskInventory.Values)
+                if (item.Name == name)
                 {
-                    if (item.Name == name)
-                    {
-                        if (item.ItemID == m_itemID)
-                            throw new ScriptDeleteException();
-                        else
-                            m_host.Inventory.RemoveInventoryItem(item.ItemID);
-                        return;
-                    }
+                    if (item.ItemID == m_itemID)
+                        throw new ScriptDeleteException();
+                    else
+                        m_host.Inventory.RemoveInventoryItem(item.ItemID);
+
+                    m_host.TaskInventory.LockItemsForRead(false);
+                    return;
                 }
             }
+            m_host.TaskInventory.LockItemsForRead(false);
         }
 
         public void llSetText(string text, LSL_Vector color, double alpha)
@@ -3944,6 +4141,7 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
         {
             m_host.AddScriptLPS(1);
 
+            //Clone is thread safe
             TaskInventoryDictionary itemDictionary = (TaskInventoryDictionary)m_host.TaskInventory.Clone();
 
             foreach (TaskInventoryItem item in itemDictionary.Values)
@@ -3997,6 +4195,9 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
                 ScenePresence presence = World.GetScenePresence(agentId);
                 if (presence != null)
                 {
+                    // agent must not be a god
+                    if (presence.GodLevel >= 200) return;
+
                     // agent must be over the owners land
                     if (m_host.OwnerID == World.LandChannel.GetLandObject(
                             presence.AbsolutePosition.X, presence.AbsolutePosition.Y).LandData.OwnerID)
@@ -4057,17 +4258,16 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
             UUID soundId = UUID.Zero;
             if (!UUID.TryParse(impact_sound, out soundId))
             {
-                lock (m_host.TaskInventory)
+                m_host.TaskInventory.LockItemsForRead(true);
+                foreach (TaskInventoryItem item in m_host.TaskInventory.Values)
                 {
-                    foreach (TaskInventoryItem item in m_host.TaskInventory.Values)
+                    if (item.Type == (int)AssetType.Sound && item.Name == impact_sound)
                     {
-                        if (item.Type == (int)AssetType.Sound && item.Name == impact_sound)
-                        {
-                            soundId = item.AssetID;
-                            break;
-                        }
+                        soundId = item.AssetID;
+                        break;
                     }
                 }
+                m_host.TaskInventory.LockItemsForRead(false);
             }
             m_host.CollisionSound = soundId;
             m_host.CollisionSoundVolume = (float)impact_volume;
@@ -4113,6 +4313,7 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
             UUID partItemID;
             foreach (SceneObjectPart part in parts)
             {
+                //Clone is thread safe
                 TaskInventoryDictionary itemsDictionary = (TaskInventoryDictionary)part.TaskInventory.Clone();
 
                 foreach (TaskInventoryItem item in itemsDictionary.Values)
@@ -4327,17 +4528,16 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
 
             m_host.AddScriptLPS(1);
 
-            lock (m_host.TaskInventory)
+            m_host.TaskInventory.LockItemsForRead(true);
+            foreach (TaskInventoryItem item in m_host.TaskInventory.Values)
             {
-                foreach (TaskInventoryItem item in m_host.TaskInventory.Values)
+                if (item.Type == 10 && item.ItemID == m_itemID)
                 {
-                    if (item.Type == 10 && item.ItemID == m_itemID)
-                    {
-                        result = item.Name != null ? item.Name : String.Empty;
-                        break;
-                    }
+                    result =  item.Name!=null?item.Name:String.Empty;
+                    break;
                 }
             }
+            m_host.TaskInventory.LockItemsForRead(false);
 
             return result;
         }
@@ -4490,23 +4690,24 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
         {
             m_host.AddScriptLPS(1);
 
-            lock (m_host.TaskInventory)
+            m_host.TaskInventory.LockItemsForRead(true);
+            foreach (KeyValuePair<UUID, TaskInventoryItem> inv in m_host.TaskInventory)
             {
-                foreach (KeyValuePair<UUID, TaskInventoryItem> inv in m_host.TaskInventory)
+                if (inv.Value.Name == name)
                 {
-                    if (inv.Value.Name == name)
+                    if ((inv.Value.CurrentPermissions & (uint)(PermissionMask.Copy | PermissionMask.Transfer | PermissionMask.Modify)) == (uint)(PermissionMask.Copy | PermissionMask.Transfer | PermissionMask.Modify))
                     {
-                        if ((inv.Value.CurrentPermissions & (uint)(PermissionMask.Copy | PermissionMask.Transfer | PermissionMask.Modify)) == (uint)(PermissionMask.Copy | PermissionMask.Transfer | PermissionMask.Modify))
-                        {
-                            return inv.Value.AssetID.ToString();
-                        }
-                        else
-                        {
-                            return UUID.Zero.ToString();
-                        }
+                        m_host.TaskInventory.LockItemsForRead(false);
+                        return inv.Value.AssetID.ToString();
+                    }
+                    else
+                    {
+                        m_host.TaskInventory.LockItemsForRead(false);
+                        return UUID.Zero.ToString();
                     }
                 }
             }
+            m_host.TaskInventory.LockItemsForRead(false);
 
             return UUID.Zero.ToString();
         }
@@ -6059,14 +6260,16 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
 
         protected UUID GetTaskInventoryItem(string name)
         {
-            lock (m_host.TaskInventory)
+            m_host.TaskInventory.LockItemsForRead(true);
+            foreach (KeyValuePair<UUID, TaskInventoryItem> inv in m_host.TaskInventory)
             {
-                foreach (KeyValuePair<UUID, TaskInventoryItem> inv in m_host.TaskInventory)
+                if (inv.Value.Name == name)
                 {
-                    if (inv.Value.Name == name)
-                        return inv.Key;
+                    m_host.TaskInventory.LockItemsForRead(false);
+                    return inv.Key;
                 }
             }
+            m_host.TaskInventory.LockItemsForRead(false);
 
             return UUID.Zero;
         }
@@ -6394,22 +6597,21 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
             }
 
             // copy the first script found with this inventory name
-            lock (m_host.TaskInventory)
+            m_host.TaskInventory.LockItemsForRead(true);
+            foreach (KeyValuePair<UUID, TaskInventoryItem> inv in m_host.TaskInventory)
             {
-                foreach (KeyValuePair<UUID, TaskInventoryItem> inv in m_host.TaskInventory)
+                if (inv.Value.Name == name)
                 {
-                    if (inv.Value.Name == name)
+                    // make sure the object is a script
+                    if (10 == inv.Value.Type)
                     {
-                        // make sure the object is a script
-                        if (10 == inv.Value.Type)
-                        {
-                            found = true;
-                            srcId = inv.Key;
-                            break;
-                        }
+                        found = true;
+                        srcId = inv.Key;
+                        break;
                     }
                 }
             }
+            m_host.TaskInventory.LockItemsForRead(false);
 
             if (!found)
             {
@@ -6493,6 +6695,8 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
         protected ObjectShapePacket.ObjectDataBlock SetPrimitiveBlockShapeParams(SceneObjectPart part, int holeshape, LSL_Vector cut, float hollow, LSL_Vector twist)
         {
             ObjectShapePacket.ObjectDataBlock shapeBlock = new ObjectShapePacket.ObjectDataBlock();
+            if (part == null || part.ParentGroup == null || part.ParentGroup.IsDeleted)
+                return shapeBlock;
 
             if (holeshape != (int)ScriptBaseClass.PRIM_HOLE_DEFAULT &&
                 holeshape != (int)ScriptBaseClass.PRIM_HOLE_CIRCLE &&
@@ -6563,6 +6767,9 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
 
         protected void SetPrimitiveShapeParams(SceneObjectPart part, int holeshape, LSL_Vector cut, float hollow, LSL_Vector twist, LSL_Vector taper_b, LSL_Vector topshear, byte fudge)
         {
+            if (part == null || part.ParentGroup == null || part.ParentGroup.IsDeleted)
+                return;
+
             ObjectShapePacket.ObjectDataBlock shapeBlock;
 
             shapeBlock = SetPrimitiveBlockShapeParams(part, holeshape, cut, hollow, twist);
@@ -6612,6 +6819,9 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
 
         protected void SetPrimitiveShapeParams(SceneObjectPart part, int holeshape, LSL_Vector cut, float hollow, LSL_Vector twist, LSL_Vector dimple, byte fudge)
         {
+            if (part == null || part.ParentGroup == null || part.ParentGroup.IsDeleted)
+                return;
+
             ObjectShapePacket.ObjectDataBlock shapeBlock;
 
             shapeBlock = SetPrimitiveBlockShapeParams(part, holeshape, cut, hollow, twist);
@@ -6654,6 +6864,9 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
 
         protected void SetPrimitiveShapeParams(SceneObjectPart part, int holeshape, LSL_Vector cut, float hollow, LSL_Vector twist, LSL_Vector holesize, LSL_Vector topshear, LSL_Vector profilecut, LSL_Vector taper_a, float revolutions, float radiusoffset, float skew, byte fudge)
         {
+            if (part == null || part.ParentGroup == null || part.ParentGroup.IsDeleted)
+                return;
+
             ObjectShapePacket.ObjectDataBlock shapeBlock;
 
             shapeBlock = SetPrimitiveBlockShapeParams(part, holeshape, cut, hollow, twist);
@@ -6775,6 +6988,9 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
 
         protected void SetPrimitiveShapeParams(SceneObjectPart part, string map, int type)
         {
+            if (part == null || part.ParentGroup == null || part.ParentGroup.IsDeleted)
+                return;
+
             ObjectShapePacket.ObjectDataBlock shapeBlock = new ObjectShapePacket.ObjectDataBlock();
             UUID sculptId;
 
@@ -6815,7 +7031,7 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
 
         public void llSetLinkPrimitiveParams(int linknumber, LSL_List rules)
         {
-            m_host.AddScriptLPS(1);
+             m_host.AddScriptLPS(1);
 
             List<SceneObjectPart> parts = GetLinkParts(linknumber);
 
@@ -6830,6 +7046,9 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
 
         protected void SetPrimParams(SceneObjectPart part, LSL_List rules)
         {
+            if (part == null || part.ParentGroup == null || part.ParentGroup.IsDeleted)
+                return;
+
             int idx = 0;
 
             while (idx < rules.Length)
@@ -7661,24 +7880,95 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
                         break;
 
                     case (int)ScriptBaseClass.PRIM_BUMP_SHINY:
-                        // TODO--------------
                         if (remain < 1)
                             return res;
+                        face = (int)rules.GetLSLIntegerItem(idx++);
 
-                        face=(int)rules.GetLSLIntegerItem(idx++);
-
-                        res.Add(new LSL_Integer(0));
-                        res.Add(new LSL_Integer(0));
+                        tex = part.Shape.Textures;
+                        int shiny;
+                        if (face == ScriptBaseClass.ALL_SIDES)
+                        {
+                            for (face = 0; face < GetNumberOfSides(part); face++)
+                            {
+                                Shininess shinyness = tex.GetFace((uint)face).Shiny;
+                                if (shinyness == Shininess.High)
+                                {
+                                    shiny = ScriptBaseClass.PRIM_SHINY_HIGH;
+                                }
+                                else if (shinyness == Shininess.Medium)
+                                {
+                                    shiny = ScriptBaseClass.PRIM_SHINY_MEDIUM;
+                                }
+                                else if (shinyness == Shininess.Low)
+                                {
+                                    shiny = ScriptBaseClass.PRIM_SHINY_LOW;
+                                }
+                                else
+                                {
+                                    shiny = ScriptBaseClass.PRIM_SHINY_NONE;
+                                }
+                                res.Add(new LSL_Integer(shiny));
+                                res.Add(new LSL_Integer((int)tex.GetFace((uint)face).Bump));
+                            }
+                        }
+                        else
+                        {
+                            Shininess shinyness = tex.GetFace((uint)face).Shiny;
+                            if (shinyness == Shininess.High)
+                            {
+                                shiny = ScriptBaseClass.PRIM_SHINY_HIGH; 
+                            }
+                            else if (shinyness == Shininess.Medium)
+                            {
+                                shiny = ScriptBaseClass.PRIM_SHINY_MEDIUM;
+                            }
+                            else if (shinyness == Shininess.Low)
+                            {
+                                shiny = ScriptBaseClass.PRIM_SHINY_LOW;
+                            }
+                            else
+                            {
+                                shiny = ScriptBaseClass.PRIM_SHINY_NONE;
+                            }
+                            res.Add(new LSL_Integer(shiny));
+                            res.Add(new LSL_Integer((int)tex.GetFace((uint)face).Bump));
+                        }
                         break;
 
                     case (int)ScriptBaseClass.PRIM_FULLBRIGHT:
-                        // TODO--------------
                         if (remain < 1)
                             return res;
+                        face = (int)rules.GetLSLIntegerItem(idx++);
 
-                        face=(int)rules.GetLSLIntegerItem(idx++);
-
-                        res.Add(new LSL_Integer(0));
+                        tex = part.Shape.Textures;
+                        int fullbright;
+                        if (face == ScriptBaseClass.ALL_SIDES)
+                        {
+                            for (face = 0; face < GetNumberOfSides(part); face++)
+                            {
+                                if (tex.GetFace((uint)face).Fullbright == true)
+                                {
+                                    fullbright = ScriptBaseClass.TRUE;
+                                }
+                                else
+                                {
+                                    fullbright = ScriptBaseClass.FALSE;
+                                }
+                                res.Add(new LSL_Integer(fullbright));
+                            }
+                        }
+                        else
+                        {
+                            if (tex.GetFace((uint)face).Fullbright == true)
+                            {
+                                fullbright = ScriptBaseClass.TRUE;
+                            }
+                            else
+                            {
+                                fullbright = ScriptBaseClass.FALSE;
+                            }
+                            res.Add(new LSL_Integer(fullbright));
+                        }
                         break;
 
                     case (int)ScriptBaseClass.PRIM_FLEXIBLE:
@@ -7699,14 +7989,37 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
                         break;
 
                     case (int)ScriptBaseClass.PRIM_TEXGEN:
-                        // TODO--------------
                         // (PRIM_TEXGEN_DEFAULT, PRIM_TEXGEN_PLANAR)
                         if (remain < 1)
                             return res;
+                        face = (int)rules.GetLSLIntegerItem(idx++);
 
-                        face=(int)rules.GetLSLIntegerItem(idx++);
-
-                        res.Add(new LSL_Integer(0));
+                        tex = part.Shape.Textures;
+                        if (face == ScriptBaseClass.ALL_SIDES)
+                        {
+                            for (face = 0; face < GetNumberOfSides(part); face++)
+                            {
+                                if (tex.GetFace((uint)face).TexMapType == MappingType.Planar)
+                                {
+                                    res.Add(new LSL_Integer(ScriptBaseClass.PRIM_TEXGEN_PLANAR));
+                                }
+                                else
+                                {
+                                    res.Add(new LSL_Integer(ScriptBaseClass.PRIM_TEXGEN_DEFAULT));
+                                }
+                            }
+                        }
+                        else
+                        {
+                            if (tex.GetFace((uint)face).TexMapType == MappingType.Planar)
+                            {
+                                res.Add(new LSL_Integer(ScriptBaseClass.PRIM_TEXGEN_PLANAR));
+                            }
+                            else
+                            {
+                                res.Add(new LSL_Integer(ScriptBaseClass.PRIM_TEXGEN_DEFAULT));
+                            }
+                        }
                         break;
 
                     case (int)ScriptBaseClass.PRIM_POINT_LIGHT:
@@ -7725,13 +8038,25 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
                         break;
 
                     case (int)ScriptBaseClass.PRIM_GLOW:
-                        // TODO--------------
                         if (remain < 1)
                             return res;
+                        face = (int)rules.GetLSLIntegerItem(idx++);
 
-                        face=(int)rules.GetLSLIntegerItem(idx++);
-
-                        res.Add(new LSL_Float(0));
+                        tex = part.Shape.Textures;
+                        float primglow;
+                        if (face == ScriptBaseClass.ALL_SIDES)
+                        {
+                            for (face = 0; face < GetNumberOfSides(part); face++)
+                            {
+                                primglow = tex.GetFace((uint)face).Glow;
+                                res.Add(new LSL_Float(primglow));
+                            }
+                        }
+                        else
+                        {
+                            primglow = tex.GetFace((uint)face).Glow;
+                            res.Add(new LSL_Float(primglow));
+                        }
                         break;
                     case (int)ScriptBaseClass.PRIM_TEXT:
                         Color4 textColor = part.GetTextColor();
@@ -8268,28 +8593,28 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
         {
             m_host.AddScriptLPS(1);
 
-            lock (m_host.TaskInventory)
+            m_host.TaskInventory.LockItemsForRead(true);
+            foreach (KeyValuePair<UUID, TaskInventoryItem> inv in m_host.TaskInventory)
             {
-                foreach (KeyValuePair<UUID, TaskInventoryItem> inv in m_host.TaskInventory)
+                if (inv.Value.Name == item)
                 {
-                    if (inv.Value.Name == item)
+                    m_host.TaskInventory.LockItemsForRead(false);
+                    switch (mask)
                     {
-                        switch (mask)
-                        {
-                            case 0:
-                                return (int)inv.Value.BasePermissions;
-                            case 1:
-                                return (int)inv.Value.CurrentPermissions;
-                            case 2:
-                                return (int)inv.Value.GroupPermissions;
-                            case 3:
-                                return (int)inv.Value.EveryonePermissions;
-                            case 4:
-                                return (int)inv.Value.NextPermissions;
-                        }
+                        case 0:
+                            return (int)inv.Value.BasePermissions;
+                        case 1:
+                            return (int)inv.Value.CurrentPermissions;
+                        case 2:
+                            return (int)inv.Value.GroupPermissions;
+                        case 3:
+                            return (int)inv.Value.EveryonePermissions;
+                        case 4:
+                            return (int)inv.Value.NextPermissions;
                     }
                 }
             }
+            m_host.TaskInventory.LockItemsForRead(false);
 
             return -1;
         }
@@ -8336,16 +8661,16 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
         {
             m_host.AddScriptLPS(1);
 
-            lock (m_host.TaskInventory)
+            m_host.TaskInventory.LockItemsForRead(true);
+            foreach (KeyValuePair<UUID, TaskInventoryItem> inv in m_host.TaskInventory)
             {
-                foreach (KeyValuePair<UUID, TaskInventoryItem> inv in m_host.TaskInventory)
+                if (inv.Value.Name == item)
                 {
-                    if (inv.Value.Name == item)
-                    {
-                        return inv.Value.CreatorID.ToString();
-                    }
+                    m_host.TaskInventory.LockItemsForRead(false);
+                    return inv.Value.CreatorID.ToString();
                 }
             }
+            m_host.TaskInventory.LockItemsForRead(false);
 
             llSay(0, "No item name '" + item + "'");
 
@@ -8878,16 +9203,16 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
         {
             m_host.AddScriptLPS(1);
 
-            lock (m_host.TaskInventory)
+            m_host.TaskInventory.LockItemsForRead(true);
+            foreach (KeyValuePair<UUID, TaskInventoryItem> inv in m_host.TaskInventory)
             {
-                foreach (KeyValuePair<UUID, TaskInventoryItem> inv in m_host.TaskInventory)
+                if (inv.Value.Name == name)
                 {
-                    if (inv.Value.Name == name)
-                    {
-                        return inv.Value.Type;
-                    }
+                    m_host.TaskInventory.LockItemsForRead(false);
+                    return inv.Value.Type;
                 }
             }
+            m_host.TaskInventory.LockItemsForRead(false);
 
             return -1;
         }
@@ -8898,15 +9223,19 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
 
             if (quick_pay_buttons.Data.Length < 4)
             {
-                LSLError("List must have at least 4 elements");
-                return;
+                int x;
+                for (x=quick_pay_buttons.Data.Length; x<= 4; x++)
+                {
+                    quick_pay_buttons.Add(ScriptBaseClass.PAY_HIDE);
+                }
             }
-            m_host.ParentGroup.RootPart.PayPrice[0]=price;
-
-            m_host.ParentGroup.RootPart.PayPrice[1]=(LSL_Integer)quick_pay_buttons.Data[0];
-            m_host.ParentGroup.RootPart.PayPrice[2]=(LSL_Integer)quick_pay_buttons.Data[1];
-            m_host.ParentGroup.RootPart.PayPrice[3]=(LSL_Integer)quick_pay_buttons.Data[2];
-            m_host.ParentGroup.RootPart.PayPrice[4]=(LSL_Integer)quick_pay_buttons.Data[3];
+            int[] nPrice = new int[5];
+            nPrice[0]=price;
+            nPrice[1] = (LSL_Integer)quick_pay_buttons.Data[0];
+            nPrice[2] = (LSL_Integer)quick_pay_buttons.Data[1];
+            nPrice[3] = (LSL_Integer)quick_pay_buttons.Data[2];
+            nPrice[4] = (LSL_Integer)quick_pay_buttons.Data[3];
+            m_host.ParentGroup.RootPart.PayPrice = nPrice;
             m_host.ParentGroup.HasGroupChanged = true;
         }
 
@@ -8918,17 +9247,20 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
             if (invItemID == UUID.Zero)
                 return new LSL_Vector();
 
-            lock (m_host.TaskInventory)
+            m_host.TaskInventory.LockItemsForRead(true);
+            if (m_host.TaskInventory[invItemID].PermsGranter == UUID.Zero)
             {
-                if (m_host.TaskInventory[invItemID].PermsGranter == UUID.Zero)
-                   return new LSL_Vector();
+                m_host.TaskInventory.LockItemsForRead(false);
+                return new LSL_Vector();
+            }
 
-                if ((m_host.TaskInventory[invItemID].PermsMask & ScriptBaseClass.PERMISSION_TRACK_CAMERA) == 0)
-                {
-                    ShoutError("No permissions to track the camera");
-                    return new LSL_Vector();
-                }
+            if ((m_host.TaskInventory[invItemID].PermsMask & ScriptBaseClass.PERMISSION_TRACK_CAMERA) == 0)
+            {
+                ShoutError("No permissions to track the camera");
+                m_host.TaskInventory.LockItemsForRead(false);
+                return new LSL_Vector();
             }
+            m_host.TaskInventory.LockItemsForRead(false);
 
             ScenePresence presence = World.GetScenePresence(m_host.OwnerID);
             if (presence != null)
@@ -8946,17 +9278,19 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
             if (invItemID == UUID.Zero)
                 return new LSL_Rotation();
 
-            lock (m_host.TaskInventory)
+            m_host.TaskInventory.LockItemsForRead(true);
+            if (m_host.TaskInventory[invItemID].PermsGranter == UUID.Zero)
             {
-                if (m_host.TaskInventory[invItemID].PermsGranter == UUID.Zero)
-                   return new LSL_Rotation();
-
-                if ((m_host.TaskInventory[invItemID].PermsMask & ScriptBaseClass.PERMISSION_TRACK_CAMERA) == 0)
-                {
-                    ShoutError("No permissions to track the camera");
-                    return new LSL_Rotation();
-                }
+                m_host.TaskInventory.LockItemsForRead(false);
+                return new LSL_Rotation();
             }
+            if ((m_host.TaskInventory[invItemID].PermsMask & ScriptBaseClass.PERMISSION_TRACK_CAMERA) == 0)
+            {
+                ShoutError("No permissions to track the camera");
+                m_host.TaskInventory.LockItemsForRead(false);
+                return new LSL_Rotation();
+            }
+            m_host.TaskInventory.LockItemsForRead(false);
 
             ScenePresence presence = World.GetScenePresence(m_host.OwnerID);
             if (presence != null)
@@ -9106,14 +9440,21 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
             if (objectID == UUID.Zero) return;
 
             UUID agentID;
-            lock (m_host.TaskInventory)
-            {
-                // we need the permission first, to know which avatar we want to set the camera for
-                agentID = m_host.TaskInventory[invItemID].PermsGranter;
+            m_host.TaskInventory.LockItemsForRead(true);
+            // we need the permission first, to know which avatar we want to set the camera for
+            agentID = m_host.TaskInventory[invItemID].PermsGranter;
 
-                if (agentID == UUID.Zero) return;
-                if ((m_host.TaskInventory[invItemID].PermsMask & ScriptBaseClass.PERMISSION_CONTROL_CAMERA) == 0) return;
+            if (agentID == UUID.Zero)
+            {
+                m_host.TaskInventory.LockItemsForRead(false);
+                return;
             }
+            if ((m_host.TaskInventory[invItemID].PermsMask & ScriptBaseClass.PERMISSION_CONTROL_CAMERA) == 0)
+            {
+                m_host.TaskInventory.LockItemsForRead(false);
+                return;
+            }
+            m_host.TaskInventory.LockItemsForRead(false);
 
             ScenePresence presence = World.GetScenePresence(agentID);
 
@@ -9163,12 +9504,19 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
 
             // we need the permission first, to know which avatar we want to clear the camera for
             UUID agentID;
-            lock (m_host.TaskInventory)
+            m_host.TaskInventory.LockItemsForRead(true);
+            agentID = m_host.TaskInventory[invItemID].PermsGranter;
+            if (agentID == UUID.Zero)
+            {
+                m_host.TaskInventory.LockItemsForRead(false);
+                return;
+            }
+            if ((m_host.TaskInventory[invItemID].PermsMask & ScriptBaseClass.PERMISSION_CONTROL_CAMERA) == 0)
             {
-                agentID = m_host.TaskInventory[invItemID].PermsGranter;
-                if (agentID == UUID.Zero) return;
-                if ((m_host.TaskInventory[invItemID].PermsMask & ScriptBaseClass.PERMISSION_CONTROL_CAMERA) == 0) return;
+                m_host.TaskInventory.LockItemsForRead(false);
+                return;
             }
+            m_host.TaskInventory.LockItemsForRead(false);
 
             ScenePresence presence = World.GetScenePresence(agentID);
 
@@ -9625,15 +9973,19 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
 
         internal UUID ScriptByName(string name)
         {
-            lock (m_host.TaskInventory)
+            m_host.TaskInventory.LockItemsForRead(true);
+
+            foreach (TaskInventoryItem item in m_host.TaskInventory.Values)
             {
-                foreach (TaskInventoryItem item in m_host.TaskInventory.Values)
+                if (item.Type == 10 && item.Name == name)
                 {
-                    if (item.Type == 10 && item.Name == name)
-                        return item.ItemID;
+                    m_host.TaskInventory.LockItemsForRead(false);
+                    return item.ItemID;
                 }
             }
 
+            m_host.TaskInventory.LockItemsForRead(false);
+
             return UUID.Zero;
         }
 
@@ -9674,6 +10026,7 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
         {
             m_host.AddScriptLPS(1);
 
+            //Clone is thread safe
             TaskInventoryDictionary itemsDictionary = (TaskInventoryDictionary)m_host.TaskInventory.Clone();
 
             UUID assetID = UUID.Zero;
@@ -9736,6 +10089,7 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
         {
             m_host.AddScriptLPS(1);
 
+            //Clone is thread safe
             TaskInventoryDictionary itemsDictionary = (TaskInventoryDictionary)m_host.TaskInventory.Clone();
 
             UUID assetID = UUID.Zero;
diff --git a/OpenSim/Region/ScriptEngine/Shared/Api/Implementation/OSSL_Api.cs b/OpenSim/Region/ScriptEngine/Shared/Api/Implementation/OSSL_Api.cs
index 7e68cc7..9474bab 100644
--- a/OpenSim/Region/ScriptEngine/Shared/Api/Implementation/OSSL_Api.cs
+++ b/OpenSim/Region/ScriptEngine/Shared/Api/Implementation/OSSL_Api.cs
@@ -719,18 +719,17 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
                 if (target != null)
                 {
                     UUID animID=UUID.Zero;
-                    lock (m_host.TaskInventory)
+                    m_host.TaskInventory.LockItemsForRead(true);
+                    foreach (KeyValuePair<UUID, TaskInventoryItem> inv in m_host.TaskInventory)
                     {
-                        foreach (KeyValuePair<UUID, TaskInventoryItem> inv in m_host.TaskInventory)
+                        if (inv.Value.Name == animation)
                         {
-                            if (inv.Value.Name == animation)
-                            {
-                                if (inv.Value.Type == (int)AssetType.Animation)
-                                    animID = inv.Value.AssetID;
-                                continue;
-                            }
+                            if (inv.Value.Type == (int)AssetType.Animation)
+                                animID = inv.Value.AssetID;
+                            continue;
                         }
                     }
+                    m_host.TaskInventory.LockItemsForRead(false);
                     if (animID == UUID.Zero)
                         target.Animator.AddAnimation(animation, m_host.UUID);
                     else
@@ -752,18 +751,17 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
                 if (target != null)
                 {
                     UUID animID=UUID.Zero;
-                    lock (m_host.TaskInventory)
+                    m_host.TaskInventory.LockItemsForRead(true);
+                    foreach (KeyValuePair<UUID, TaskInventoryItem> inv in m_host.TaskInventory)
                     {
-                        foreach (KeyValuePair<UUID, TaskInventoryItem> inv in m_host.TaskInventory)
+                        if (inv.Value.Name == animation)
                         {
-                            if (inv.Value.Name == animation)
-                            {
-                                if (inv.Value.Type == (int)AssetType.Animation)
-                                    animID = inv.Value.AssetID;
-                                continue;
-                            }
+                            if (inv.Value.Type == (int)AssetType.Animation)
+                                animID = inv.Value.AssetID;
+                            continue;
                         }
                     }
+                    m_host.TaskInventory.LockItemsForRead(false);
                     
                     if (animID == UUID.Zero)
                         target.Animator.RemoveAnimation(animation);
@@ -1532,6 +1530,7 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
 
             if (!UUID.TryParse(name, out assetID))
             {
+                m_host.TaskInventory.LockItemsForRead(true);
                 foreach (TaskInventoryItem item in m_host.TaskInventory.Values)
                 {
                     if (item.Type == 7 && item.Name == name)
@@ -1539,6 +1538,7 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
                         assetID = item.AssetID;
                     }
                 }
+                m_host.TaskInventory.LockItemsForRead(false);
             }
 
             if (assetID == UUID.Zero)
@@ -1585,6 +1585,7 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
 
             if (!UUID.TryParse(name, out assetID))
             {
+                m_host.TaskInventory.LockItemsForRead(true);
                 foreach (TaskInventoryItem item in m_host.TaskInventory.Values)
                 {
                     if (item.Type == 7 && item.Name == name)
@@ -1592,6 +1593,7 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
                         assetID = item.AssetID;
                     }
                 }
+                m_host.TaskInventory.LockItemsForRead(false);
             }
 
             if (assetID == UUID.Zero)
@@ -1642,6 +1644,7 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
 
             if (!UUID.TryParse(name, out assetID))
             {
+                m_host.TaskInventory.LockItemsForRead(true);
                 foreach (TaskInventoryItem item in m_host.TaskInventory.Values)
                 {
                     if (item.Type == 7 && item.Name == name)
@@ -1649,6 +1652,7 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
                         assetID = item.AssetID;
                     }
                 }
+                m_host.TaskInventory.LockItemsForRead(false);
             }
 
             if (assetID == UUID.Zero)
diff --git a/OpenSim/Region/ScriptEngine/Shared/Api/Implementation/Plugins/Timer.cs b/OpenSim/Region/ScriptEngine/Shared/Api/Implementation/Plugins/Timer.cs
index eeb59d9..2fd33fe 100644
--- a/OpenSim/Region/ScriptEngine/Shared/Api/Implementation/Plugins/Timer.cs
+++ b/OpenSim/Region/ScriptEngine/Shared/Api/Implementation/Plugins/Timer.cs
@@ -109,25 +109,27 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api.Plugins
             if (Timers.Count == 0)
                 return;
 
+            Dictionary<string, TimerClass>.ValueCollection tvals;
             lock (TimerListLock)
             {
                 // Go through all timers
-                Dictionary<string, TimerClass>.ValueCollection tvals = Timers.Values;
-                foreach (TimerClass ts in tvals)
+                tvals = Timers.Values;
+            }
+
+            foreach (TimerClass ts in tvals)
+            {
+                // Time has passed?
+                if (ts.next < DateTime.Now.Ticks)
                 {
-                    // Time has passed?
-                    if (ts.next < DateTime.Now.Ticks)
-                    {
-                        //m_log.Debug("Time has passed: Now: " + DateTime.Now.Ticks + ", Passed: " + ts.next);
-                        // Add it to queue
-                        m_CmdManager.m_ScriptEngine.PostScriptEvent(ts.itemID,
-                                new EventParams("timer", new Object[0],
-                                new DetectParams[0]));
-                        // set next interval
-
-                        //ts.next = DateTime.Now.ToUniversalTime().AddSeconds(ts.interval);
-                        ts.next = DateTime.Now.Ticks + ts.interval;
-                    }
+                    //m_log.Debug("Time has passed: Now: " + DateTime.Now.Ticks + ", Passed: " + ts.next);
+                    // Add it to queue
+                    m_CmdManager.m_ScriptEngine.PostScriptEvent(ts.itemID,
+                            new EventParams("timer", new Object[0],
+                            new DetectParams[0]));
+                    // set next interval
+
+                    //ts.next = DateTime.Now.ToUniversalTime().AddSeconds(ts.interval);
+                    ts.next = DateTime.Now.Ticks + ts.interval;
                 }
             }
         }
diff --git a/OpenSim/Region/ScriptEngine/Shared/Api/Interface/IOSSL_Api.cs b/OpenSim/Region/ScriptEngine/Shared/Api/Interface/IOSSL_Api.cs
index 60b8050..f5921e1 100644
--- a/OpenSim/Region/ScriptEngine/Shared/Api/Interface/IOSSL_Api.cs
+++ b/OpenSim/Region/ScriptEngine/Shared/Api/Interface/IOSSL_Api.cs
@@ -80,7 +80,7 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api.Interfaces
         // Avatar Info Commands
         string osGetAgentIP(string agent);
         LSL_List osGetAgents();
-
+        
         // Teleport commands
         void osTeleportAgent(string agent, string regionName, LSL_Types.Vector3 position, LSL_Types.Vector3 lookat);
         void osTeleportAgent(string agent, int regionX, int regionY, LSL_Types.Vector3 position, LSL_Types.Vector3 lookat);
diff --git a/OpenSim/Region/ScriptEngine/Shared/Api/Runtime/Executor.cs b/OpenSim/Region/ScriptEngine/Shared/Api/Runtime/Executor.cs
index 9615315..943d7a2 100644
--- a/OpenSim/Region/ScriptEngine/Shared/Api/Runtime/Executor.cs
+++ b/OpenSim/Region/ScriptEngine/Shared/Api/Runtime/Executor.cs
@@ -27,6 +27,7 @@
 
 using System;
 using System.Collections.Generic;
+using System.Diagnostics; //for [DebuggerNonUserCode]
 using System.Reflection;
 using System.Runtime.Remoting.Lifetime;
 using OpenSim.Region.ScriptEngine.Shared;
@@ -132,6 +133,7 @@ namespace OpenSim.Region.ScriptEngine.Shared.ScriptBase
             return (eventFlags);
         }
 
+        [DebuggerNonUserCode]
         public void ExecuteEvent(string state, string FunctionName, object[] args)
         {
             // IMPORTANT: Types and MemberInfo-derived objects require a LOT of memory.
diff --git a/OpenSim/Region/ScriptEngine/Shared/Api/Runtime/LSL_Stub.cs b/OpenSim/Region/ScriptEngine/Shared/Api/Runtime/LSL_Stub.cs
index 3339995..e86d08c 100644
--- a/OpenSim/Region/ScriptEngine/Shared/Api/Runtime/LSL_Stub.cs
+++ b/OpenSim/Region/ScriptEngine/Shared/Api/Runtime/LSL_Stub.cs
@@ -26,6 +26,7 @@
  */
 
 using System;
+using System.Diagnostics; //for [DebuggerNonUserCode]
 using System.Runtime.Remoting.Lifetime;
 using System.Threading;
 using System.Reflection;
@@ -309,6 +310,7 @@ namespace OpenSim.Region.ScriptEngine.Shared.ScriptBase
             m_LSL_Functions.llDialog(avatar, message, buttons, chat_channel);
         }
 
+        [DebuggerNonUserCode]
         public void llDie()
         {
             m_LSL_Functions.llDie();
diff --git a/OpenSim/Region/ScriptEngine/Shared/Api/Runtime/ScriptBase.cs b/OpenSim/Region/ScriptEngine/Shared/Api/Runtime/ScriptBase.cs
index edbbc2a..b138da3 100644
--- a/OpenSim/Region/ScriptEngine/Shared/Api/Runtime/ScriptBase.cs
+++ b/OpenSim/Region/ScriptEngine/Shared/Api/Runtime/ScriptBase.cs
@@ -33,6 +33,7 @@ using System.Threading;
 using System.Reflection;
 using System.Collections;
 using System.Collections.Generic;
+using System.Diagnostics; //for [DebuggerNonUserCode]
 using OpenSim.Region.ScriptEngine.Interfaces;
 using OpenSim.Region.ScriptEngine.Shared;
 using OpenSim.Region.ScriptEngine.Shared.Api.Runtime;
@@ -90,6 +91,7 @@ namespace OpenSim.Region.ScriptEngine.Shared.ScriptBase
             return (int)m_Executor.GetStateEventFlags(state);
         }
 
+        [DebuggerNonUserCode]
         public void ExecuteEvent(string state, string FunctionName, object[] args)
         {
             m_Executor.ExecuteEvent(state, FunctionName, args);
diff --git a/OpenSim/Region/ScriptEngine/Shared/Instance/ScriptInstance.cs b/OpenSim/Region/ScriptEngine/Shared/Instance/ScriptInstance.cs
index d30d2dc..6ecafd4 100644
--- a/OpenSim/Region/ScriptEngine/Shared/Instance/ScriptInstance.cs
+++ b/OpenSim/Region/ScriptEngine/Shared/Instance/ScriptInstance.cs
@@ -27,6 +27,7 @@
 
 using System;
 using System.IO;
+using System.Diagnostics; //for [DebuggerNonUserCode]
 using System.Runtime.Remoting;
 using System.Runtime.Remoting.Lifetime;
 using System.Threading;
@@ -237,13 +238,12 @@ namespace OpenSim.Region.ScriptEngine.Shared.Instance
 
             if (part != null)
             {
-                lock (part.TaskInventory)
+                part.TaskInventory.LockItemsForRead(true);
+                if (part.TaskInventory.ContainsKey(m_ItemID))
                 {
-                    if (part.TaskInventory.ContainsKey(m_ItemID))
-                    {
-                        m_thisScriptTask = part.TaskInventory[m_ItemID];
-                    }
+                    m_thisScriptTask = part.TaskInventory[m_ItemID];
                 }
+                part.TaskInventory.LockItemsForRead(false);
             }
 
             ApiManager am = new ApiManager();
@@ -428,14 +428,15 @@ namespace OpenSim.Region.ScriptEngine.Shared.Instance
             {
                 int permsMask;
                 UUID permsGranter;
-                lock (part.TaskInventory)
+                part.TaskInventory.LockItemsForRead(true);
+                if (!part.TaskInventory.ContainsKey(m_ItemID))
                 {
-                    if (!part.TaskInventory.ContainsKey(m_ItemID))
-                        return;
-
-                    permsGranter = part.TaskInventory[m_ItemID].PermsGranter;
-                    permsMask = part.TaskInventory[m_ItemID].PermsMask;
+                    part.TaskInventory.LockItemsForRead(false);
+                    return;
                 }
+                permsGranter = part.TaskInventory[m_ItemID].PermsGranter;
+                permsMask = part.TaskInventory[m_ItemID].PermsMask;
+                part.TaskInventory.LockItemsForRead(false);
 
                 if ((permsMask & ScriptBaseClass.PERMISSION_TAKE_CONTROLS) != 0)
                 {
@@ -544,6 +545,7 @@ namespace OpenSim.Region.ScriptEngine.Shared.Instance
             return true;
         }
 
+        [DebuggerNonUserCode] //Prevents the debugger from farting in this function
         public void SetState(string state)
         {
             if (state == State)
@@ -555,7 +557,7 @@ namespace OpenSim.Region.ScriptEngine.Shared.Instance
                                        new DetectParams[0]));
             PostEvent(new EventParams("state_entry", new Object[0],
                                        new DetectParams[0]));
-
+            
             throw new EventAbortException();
         }
 
@@ -638,154 +640,158 @@ namespace OpenSim.Region.ScriptEngine.Shared.Instance
         /// <returns></returns>
         public object EventProcessor()
         {
+
+        EventParams data = null;
+
+        lock (m_EventQueue)
+        {
             lock (m_Script)
             {
-                EventParams data = null;
-
-                lock (m_EventQueue)
+                data = (EventParams) m_EventQueue.Dequeue();
+                if (data == null) // Shouldn't happen
                 {
-                    data = (EventParams) m_EventQueue.Dequeue();
-                    if (data == null) // Shouldn't happen
+                    if ((m_EventQueue.Count > 0) && m_RunEvents && (!m_ShuttingDown))
                     {
-                        if ((m_EventQueue.Count > 0) && m_RunEvents && (!m_ShuttingDown))
-                        {
-                            m_CurrentResult = m_Engine.QueueEventHandler(this);
-                        }
-                        else
-                        {
-                            m_CurrentResult = null;
-                        }
-                        return 0;
+                        m_CurrentResult = m_Engine.QueueEventHandler(this);
                     }
-
-                    if (data.EventName == "timer")
-                        m_TimerQueued = false;
-                    if (data.EventName == "control")
+                    else
                     {
-                        if (m_ControlEventsInQueue > 0)
-                            m_ControlEventsInQueue--;
+                        m_CurrentResult = null;
                     }
-                    if (data.EventName == "collision")
-                        m_CollisionInQueue = false;
+                    return 0;
                 }
-                
-                //m_log.DebugFormat("[XENGINE]: Processing event {0} for {1}", data.EventName, this);
 
-                m_DetectParams = data.DetectParams;
-
-                if (data.EventName == "state") // Hardcoded state change
+                if (data.EventName == "timer")
+                    m_TimerQueued = false;
+                if (data.EventName == "control")
                 {
-    //                m_log.DebugFormat("[Script] Script {0}.{1} state set to {2}",
-    //                        m_PrimName, m_ScriptName, data.Params[0].ToString());
-                    m_State=data.Params[0].ToString();
-                    AsyncCommandManager.RemoveScript(m_Engine,
-                        m_LocalID, m_ItemID);
+                    if (m_ControlEventsInQueue > 0)
+                        m_ControlEventsInQueue--;
+                }
+                if (data.EventName == "collision")
+                    m_CollisionInQueue = false;
+            }
+        }
+        lock(m_Script)
+        {
+            
+            //m_log.DebugFormat("[XENGINE]: Processing event {0} for {1}", data.EventName, this);
 
-                    SceneObjectPart part = m_Engine.World.GetSceneObjectPart(
-                        m_LocalID);
-                    if (part != null)
-                    {
-                        part.SetScriptEvents(m_ItemID,
-                                             (int)m_Script.GetStateEventFlags(State));
-                    }
+            m_DetectParams = data.DetectParams;
+
+            if (data.EventName == "state") // Hardcoded state change
+            {
+//                m_log.DebugFormat("[Script] Script {0}.{1} state set to {2}",
+//                        m_PrimName, m_ScriptName, data.Params[0].ToString());
+                m_State=data.Params[0].ToString();
+                AsyncCommandManager.RemoveScript(m_Engine,
+                    m_LocalID, m_ItemID);
+
+                SceneObjectPart part = m_Engine.World.GetSceneObjectPart(
+                    m_LocalID);
+                if (part != null)
+                {
+                    part.SetScriptEvents(m_ItemID,
+                                         (int)m_Script.GetStateEventFlags(State));
                 }
-                else
+            }
+            else
+            {
+                if (m_Engine.World.PipeEventsForScript(m_LocalID) ||
+                    data.EventName == "control") // Don't freeze avies!
                 {
-                    if (m_Engine.World.PipeEventsForScript(m_LocalID) ||
-                        data.EventName == "control") // Don't freeze avies!
-                    {
-                        SceneObjectPart part = m_Engine.World.GetSceneObjectPart(
-                            m_LocalID);
-        //                m_log.DebugFormat("[Script] Delivered event {2} in state {3} to {0}.{1}",
-        //                        m_PrimName, m_ScriptName, data.EventName, m_State);
+                    SceneObjectPart part = m_Engine.World.GetSceneObjectPart(
+                        m_LocalID);
+    //                m_log.DebugFormat("[Script] Delivered event {2} in state {3} to {0}.{1}",
+    //                        m_PrimName, m_ScriptName, data.EventName, m_State);
 
-                        try
-                        {
-                            m_CurrentEvent = data.EventName;
-                            m_EventStart = DateTime.Now;
-                            m_InEvent = true;
+                    try
+                    {
+                        m_CurrentEvent = data.EventName;
+                        m_EventStart = DateTime.Now;
+                        m_InEvent = true;
 
-                            m_Script.ExecuteEvent(State, data.EventName, data.Params);
+                        m_Script.ExecuteEvent(State, data.EventName, data.Params);
 
-                            m_InEvent = false;
-                            m_CurrentEvent = String.Empty;
+                        m_InEvent = false;
+                        m_CurrentEvent = String.Empty;
 
-                            if (m_SaveState)
-                            {
-                                // This will be the very first event we deliver
-                                // (state_entry) in default state
-                                //
+                        if (m_SaveState)
+                        {
+                            // This will be the very first event we deliver
+                            // (state_entry) in default state
+                            //
 
-                                SaveState(m_Assembly);
+                            SaveState(m_Assembly);
 
-                                m_SaveState = false;
-                            }
+                            m_SaveState = false;
                         }
-                        catch (Exception e)
-                        {
-                            // m_log.DebugFormat("[SCRIPT] Exception: {0}", e.Message);
-                            m_InEvent = false;
-                            m_CurrentEvent = String.Empty;
+                    }
+                    catch (Exception e)
+                    {
+                        // m_log.DebugFormat("[SCRIPT] Exception: {0}", e.Message);
+                        m_InEvent = false;
+                        m_CurrentEvent = String.Empty;
 
-                            if ((!(e is TargetInvocationException) || (!(e.InnerException is SelfDeleteException) && !(e.InnerException is ScriptDeleteException))) && !(e is ThreadAbortException))
-                            {
-                                try
-                                {
-                                    // DISPLAY ERROR INWORLD
-                                    string text = FormatException(e);
-
-                                    if (text.Length > 1000)
-                                        text = text.Substring(0, 1000);
-                                    m_Engine.World.SimChat(Utils.StringToBytes(text),
-                                                           ChatTypeEnum.DebugChannel, 2147483647,
-                                                           part.AbsolutePosition,
-                                                           part.Name, part.UUID, false);
-                                }
-                                catch (Exception)
-                                {
-                                }
-                                // catch (Exception e2) // LEGIT: User Scripting
-                                // {
-                                    // m_log.Error("[SCRIPT]: "+
-                                      //           "Error displaying error in-world: " +
-                                        //         e2.ToString());
-                                 //    m_log.Error("[SCRIPT]: " +
-                                   //              "Errormessage: Error compiling script:\r\n" +
-                                     //            e.ToString());
-                                // }
-                            }
-                            else if ((e is TargetInvocationException) && (e.InnerException is SelfDeleteException))
+                        if ((!(e is TargetInvocationException) || (!(e.InnerException is SelfDeleteException) && !(e.InnerException is ScriptDeleteException))) && !(e is ThreadAbortException))
+                        {
+                            try
                             {
-                                m_InSelfDelete = true;
-                                if (part != null && part.ParentGroup != null)
-                                    m_Engine.World.DeleteSceneObject(part.ParentGroup, false);
+                                // DISPLAY ERROR INWORLD
+                                string text = FormatException(e);
+
+                                if (text.Length > 1000)
+                                    text = text.Substring(0, 1000);
+                                m_Engine.World.SimChat(Utils.StringToBytes(text),
+                                                       ChatTypeEnum.DebugChannel, 2147483647,
+                                                       part.AbsolutePosition,
+                                                       part.Name, part.UUID, false);
                             }
-                            else if ((e is TargetInvocationException) && (e.InnerException is ScriptDeleteException))
+                            catch (Exception)
                             {
-                                m_InSelfDelete = true;
-                                if (part != null && part.ParentGroup != null)
-                                    part.Inventory.RemoveInventoryItem(m_ItemID);
                             }
+                            // catch (Exception e2) // LEGIT: User Scripting
+                            // {
+                                // m_log.Error("[SCRIPT]: "+
+                                  //           "Error displaying error in-world: " +
+                                    //         e2.ToString());
+                             //    m_log.Error("[SCRIPT]: " +
+                               //              "Errormessage: Error compiling script:\r\n" +
+                                 //            e.ToString());
+                            // }
+                        }
+                        else if ((e is TargetInvocationException) && (e.InnerException is SelfDeleteException))
+                        {
+                            m_InSelfDelete = true;
+                            if (part != null && part.ParentGroup != null)
+                                m_Engine.World.DeleteSceneObject(part.ParentGroup, false);
+                        }
+                        else if ((e is TargetInvocationException) && (e.InnerException is ScriptDeleteException))
+                        {
+                            m_InSelfDelete = true;
+                            if (part != null && part.ParentGroup != null)
+                                part.Inventory.RemoveInventoryItem(m_ItemID);
                         }
                     }
                 }
+            }
 
-                lock (m_EventQueue)
+            lock (m_EventQueue)
+            {
+                if ((m_EventQueue.Count > 0) && m_RunEvents && (!m_ShuttingDown))
                 {
-                    if ((m_EventQueue.Count > 0) && m_RunEvents && (!m_ShuttingDown))
-                    {
-                        m_CurrentResult = m_Engine.QueueEventHandler(this);
-                    }
-                    else
-                    {
-                        m_CurrentResult = null;
-                    }
+                    m_CurrentResult = m_Engine.QueueEventHandler(this);
+                }
+                else
+                {
+                    m_CurrentResult = null;
                 }
+            }
 
-                m_DetectParams = null;
+            m_DetectParams = null;
 
-                return 0;
-            }
+            return 0;
+        }
         }
 
         public int EventTime()
@@ -824,6 +830,7 @@ namespace OpenSim.Region.ScriptEngine.Shared.Instance
                     new Object[0], new DetectParams[0]));
         }
 
+        [DebuggerNonUserCode] //Stops the VS debugger from farting in this function
         public void ApiResetScript()
         {
             // bool running = Running;
diff --git a/OpenSim/Region/ScriptEngine/XEngine/XEngine.cs b/OpenSim/Region/ScriptEngine/XEngine/XEngine.cs
index 98e77c0..35d57d8 100644
--- a/OpenSim/Region/ScriptEngine/XEngine/XEngine.cs
+++ b/OpenSim/Region/ScriptEngine/XEngine/XEngine.cs
@@ -30,6 +30,7 @@ using System.IO;
 using System.Threading;
 using System.Collections;
 using System.Collections.Generic;
+using System.Diagnostics; //for [DebuggerNonUserCode]
 using System.Security;
 using System.Security.Policy;
 using System.Reflection;
@@ -102,6 +103,8 @@ namespace OpenSim.Region.ScriptEngine.XEngine
         private Dictionary<UUID, IScriptInstance> m_Scripts =
                 new Dictionary<UUID, IScriptInstance>();
 
+        private OpenMetaverse.ReaderWriterLockSlim m_scriptsLock = new OpenMetaverse.ReaderWriterLockSlim();
+
         // Maps the asset ID to the assembly
 
         private Dictionary<UUID, string> m_Assemblies =
@@ -123,6 +126,71 @@ namespace OpenSim.Region.ScriptEngine.XEngine
         private ScriptCompileQueue m_CompileQueue = new ScriptCompileQueue();
         IWorkItemResult m_CurrentCompile = null;
 
+        private void lockScriptsForRead(bool locked)
+        {
+            if (locked)
+            {
+                if (m_scriptsLock.RecursiveReadCount > 0)
+                {
+                    m_log.Error("[XEngine.m_Scripts] Recursive read lock requested. This should not happen and means something needs to be fixed. For now though, it's safe to continue.");
+                    m_scriptsLock.ExitReadLock();
+                }
+                if (m_scriptsLock.RecursiveWriteCount > 0)
+                {
+                    m_log.Error("[XEngine.m_Scripts] Recursive write lock requested. This should not happen and means something needs to be fixed.");
+                    m_scriptsLock.ExitWriteLock();
+                }
+
+                while (!m_scriptsLock.TryEnterReadLock(60000))
+                {
+                    m_log.Error("[XEngine.m_Scripts] Thread lock detected while trying to aquire READ lock of m_scripts in XEngine. I'm going to try to solve the thread lock automatically to preserve region stability, but this needs to be fixed.");
+                    if (m_scriptsLock.IsWriteLockHeld)
+                    {
+                        m_scriptsLock = new OpenMetaverse.ReaderWriterLockSlim();
+                    }
+                }
+            }
+            else
+            {
+                if (m_scriptsLock.RecursiveReadCount > 0)
+                {
+                    m_scriptsLock.ExitReadLock();
+                }
+            }
+        }
+        private void lockScriptsForWrite(bool locked)
+        {
+            if (locked)
+            {
+                if (m_scriptsLock.RecursiveReadCount > 0)
+                {
+                    m_log.Error("[XEngine.m_Scripts] Recursive read lock requested. This should not happen and means something needs to be fixed. For now though, it's safe to continue.");
+                    m_scriptsLock.ExitReadLock();
+                }
+                if (m_scriptsLock.RecursiveWriteCount > 0)
+                {
+                    m_log.Error("[XEngine.m_Scripts] Recursive write lock requested. This should not happen and means something needs to be fixed.");
+                    m_scriptsLock.ExitWriteLock();
+                }
+
+                while (!m_scriptsLock.TryEnterWriteLock(60000))
+                {
+                    m_log.Error("[XEngine.m_Scripts] Thread lock detected while trying to aquire WRITE lock of m_scripts in XEngine. I'm going to try to solve the thread lock automatically to preserve region stability, but this needs to be fixed.");
+                    if (m_scriptsLock.IsWriteLockHeld)
+                    {
+                        m_scriptsLock = new OpenMetaverse.ReaderWriterLockSlim();
+                    }
+                }
+            }
+            else
+            {
+                if (m_scriptsLock.RecursiveWriteCount > 0)
+                {
+                    m_scriptsLock.ExitWriteLock();
+                }
+            }
+        }
+
         public string ScriptEngineName
         {
             get { return "XEngine"; }
@@ -262,43 +330,45 @@ namespace OpenSim.Region.ScriptEngine.XEngine
 
         public void RemoveRegion(Scene scene)
         {
-            lock (m_Scripts)
+            lockScriptsForRead(true);
+            foreach (IScriptInstance instance in m_Scripts.Values)
             {
-                foreach (IScriptInstance instance in m_Scripts.Values)
+                // Force a final state save
+                //
+                if (m_Assemblies.ContainsKey(instance.AssetID))
                 {
-                    // Force a final state save
-                    //
-                    if (m_Assemblies.ContainsKey(instance.AssetID))
-                    {
-                        string assembly = m_Assemblies[instance.AssetID];
-                        instance.SaveState(assembly);
-                    }
+                    string assembly = m_Assemblies[instance.AssetID];
+                    instance.SaveState(assembly);
+                }
 
-                    // Clear the event queue and abort the instance thread
-                    //
-                    instance.ClearQueue();
-                    instance.Stop(0);
+                // Clear the event queue and abort the instance thread
+                //
+                instance.ClearQueue();
+                instance.Stop(0);
 
-                    // Release events, timer, etc
-                    //
-                    instance.DestroyScriptInstance();
+                // Release events, timer, etc
+                //
+                instance.DestroyScriptInstance();
 
-                    // Unload scripts and app domains
-                    // Must be done explicitly because they have infinite
-                    // lifetime
-                    //
-                    m_DomainScripts[instance.AppDomain].Remove(instance.ItemID);
-                    if (m_DomainScripts[instance.AppDomain].Count == 0)
-                    {
-                        m_DomainScripts.Remove(instance.AppDomain);
-                        UnloadAppDomain(instance.AppDomain);
-                    }
+                // Unload scripts and app domains
+                // Must be done explicitly because they have infinite
+                // lifetime
+                //
+                m_DomainScripts[instance.AppDomain].Remove(instance.ItemID);
+                if (m_DomainScripts[instance.AppDomain].Count == 0)
+                {
+                    m_DomainScripts.Remove(instance.AppDomain);
+                    UnloadAppDomain(instance.AppDomain);
                 }
-                m_Scripts.Clear();
-                m_PrimObjects.Clear();
-                m_Assemblies.Clear();
-                m_DomainScripts.Clear();
             }
+            lockScriptsForRead(false);
+            lockScriptsForWrite(true);
+            m_Scripts.Clear();
+            lockScriptsForWrite(false);
+            m_PrimObjects.Clear();
+            m_Assemblies.Clear();
+            m_DomainScripts.Clear();
+           
             lock (m_ScriptEngines)
             {
                 m_ScriptEngines.Remove(this);
@@ -357,22 +427,20 @@ namespace OpenSim.Region.ScriptEngine.XEngine
 
             List<IScriptInstance> instances = new List<IScriptInstance>();
 
-            lock (m_Scripts)
-            {
-                foreach (IScriptInstance instance in m_Scripts.Values)
+            lockScriptsForRead(true);
+                 foreach (IScriptInstance instance in m_Scripts.Values)
                     instances.Add(instance);
-            }
+            lockScriptsForRead(false);
 
             foreach (IScriptInstance i in instances)
             {
                 string assembly = String.Empty;
 
-                lock (m_Scripts)
-                {
+                
                     if (!m_Assemblies.ContainsKey(i.AssetID))
                         continue;
                     assembly = m_Assemblies[i.AssetID];
-                }
+                
 
                 i.SaveState(assembly);
             }
@@ -684,170 +752,181 @@ namespace OpenSim.Region.ScriptEngine.XEngine
                 }
             }
 
-            lock (m_Scripts)
+            
+
+            ScriptInstance instance = null;
+            // Create the object record
+            lockScriptsForRead(true);
+            if ((!m_Scripts.ContainsKey(itemID)) ||
+                (m_Scripts[itemID].AssetID != assetID))
             {
-                ScriptInstance instance = null;
-                // Create the object record
+                lockScriptsForRead(false);
 
-                if ((!m_Scripts.ContainsKey(itemID)) ||
-                    (m_Scripts[itemID].AssetID != assetID))
-                {
-                    UUID appDomain = assetID;
+                UUID appDomain = assetID;
 
-                    if (part.ParentGroup.IsAttachment)
-                        appDomain = part.ParentGroup.RootPart.UUID;
+                if (part.ParentGroup.IsAttachment)
+                    appDomain = part.ParentGroup.RootPart.UUID;
 
-                    if (!m_AppDomains.ContainsKey(appDomain))
+                if (!m_AppDomains.ContainsKey(appDomain))
+                {
+                    try
                     {
-                        try
-                        {
-                            AppDomainSetup appSetup = new AppDomainSetup();
-//                            appSetup.ApplicationBase = Path.Combine(
-//                                    "ScriptEngines",
-//                                    m_Scene.RegionInfo.RegionID.ToString());
-
-                            Evidence baseEvidence = AppDomain.CurrentDomain.Evidence;
-                            Evidence evidence = new Evidence(baseEvidence);
-
-                            AppDomain sandbox;
-                            if (m_AppDomainLoading)
-                                sandbox = AppDomain.CreateDomain(
-                                                m_Scene.RegionInfo.RegionID.ToString(),
-                                                evidence, appSetup);
-                            else
-                                sandbox = AppDomain.CurrentDomain;
-                            
-                            //PolicyLevel sandboxPolicy = PolicyLevel.CreateAppDomainLevel();
-                            //AllMembershipCondition sandboxMembershipCondition = new AllMembershipCondition();
-                            //PermissionSet sandboxPermissionSet = sandboxPolicy.GetNamedPermissionSet("Internet");
-                            //PolicyStatement sandboxPolicyStatement = new PolicyStatement(sandboxPermissionSet);
-                            //CodeGroup sandboxCodeGroup = new UnionCodeGroup(sandboxMembershipCondition, sandboxPolicyStatement);
-                            //sandboxPolicy.RootCodeGroup = sandboxCodeGroup;
-                            //sandbox.SetAppDomainPolicy(sandboxPolicy);
-                            
-                            m_AppDomains[appDomain] = sandbox;
-
-                            m_AppDomains[appDomain].AssemblyResolve +=
-                                new ResolveEventHandler(
-                                    AssemblyResolver.OnAssemblyResolve);
-                            m_DomainScripts[appDomain] = new List<UUID>();
-                        }
-                        catch (Exception e)
-                        {
-                            m_log.ErrorFormat("[XEngine] Exception creating app domain:\n {0}", e.ToString());
-                            m_ScriptErrorMessage += "Exception creating app domain:\n";
-                            m_ScriptFailCount++;
-                            lock (m_AddingAssemblies) 
-                            {
-                                m_AddingAssemblies[assembly]--;
-                            }
-                            return false;
-                        }
+                        AppDomainSetup appSetup = new AppDomainSetup();
+                        //                            appSetup.ApplicationBase = Path.Combine(
+                        //                                    "ScriptEngines",
+                        //                                    m_Scene.RegionInfo.RegionID.ToString());
+
+                        Evidence baseEvidence = AppDomain.CurrentDomain.Evidence;
+                        Evidence evidence = new Evidence(baseEvidence);
+
+                        AppDomain sandbox;
+                        if (m_AppDomainLoading)
+                            sandbox = AppDomain.CreateDomain(
+                                            m_Scene.RegionInfo.RegionID.ToString(),
+                                            evidence, appSetup);
+                        else
+                            sandbox = AppDomain.CurrentDomain;
+
+                        //PolicyLevel sandboxPolicy = PolicyLevel.CreateAppDomainLevel();
+                        //AllMembershipCondition sandboxMembershipCondition = new AllMembershipCondition();
+                        //PermissionSet sandboxPermissionSet = sandboxPolicy.GetNamedPermissionSet("Internet");
+                        //PolicyStatement sandboxPolicyStatement = new PolicyStatement(sandboxPermissionSet);
+                        //CodeGroup sandboxCodeGroup = new UnionCodeGroup(sandboxMembershipCondition, sandboxPolicyStatement);
+                        //sandboxPolicy.RootCodeGroup = sandboxCodeGroup;
+                        //sandbox.SetAppDomainPolicy(sandboxPolicy);
+
+                        m_AppDomains[appDomain] = sandbox;
+
+                        m_AppDomains[appDomain].AssemblyResolve +=
+                            new ResolveEventHandler(
+                                AssemblyResolver.OnAssemblyResolve);
+                        m_DomainScripts[appDomain] = new List<UUID>();
                     }
-                    m_DomainScripts[appDomain].Add(itemID);
-
-                    instance = new ScriptInstance(this, part,
-                                                  itemID, assetID, assembly,
-                                                  m_AppDomains[appDomain],
-                                                  part.ParentGroup.RootPart.Name,
-                                                  item.Name, startParam, postOnRez,
-                                                  stateSource, m_MaxScriptQueue);
-                    
-                    m_log.DebugFormat("[XEngine] Loaded script {0}.{1}, script UUID {2}, prim UUID {3} @ {4}",
-                            part.ParentGroup.RootPart.Name, item.Name, assetID, part.UUID, part.ParentGroup.RootPart.AbsolutePosition.ToString());
-
-                    if (presence != null)
+                    catch (Exception e)
                     {
-                        ShowScriptSaveResponse(item.OwnerID,
-                                assetID, "Compile successful", true);
+                        m_log.ErrorFormat("[XEngine] Exception creating app domain:\n {0}", e.ToString());
+                        m_ScriptErrorMessage += "Exception creating app domain:\n";
+                        m_ScriptFailCount++;
+                        lock (m_AddingAssemblies)
+                        {
+                            m_AddingAssemblies[assembly]--;
+                        }
+                        return false;
                     }
+                }
+                m_DomainScripts[appDomain].Add(itemID);
 
-                    instance.AppDomain = appDomain;
-                    instance.LineMap = linemap;
+                instance = new ScriptInstance(this, part,
+                                              itemID, assetID, assembly,
+                                              m_AppDomains[appDomain],
+                                              part.ParentGroup.RootPart.Name,
+                                              item.Name, startParam, postOnRez,
+                                              stateSource, m_MaxScriptQueue);
 
-                    m_Scripts[itemID] = instance;
-                }
+                m_log.DebugFormat("[XEngine] Loaded script {0}.{1}, script UUID {2}, prim UUID {3} @ {4}",
+                        part.ParentGroup.RootPart.Name, item.Name, assetID, part.UUID, part.ParentGroup.RootPart.AbsolutePosition.ToString());
 
-                lock (m_PrimObjects)
+                if (presence != null)
                 {
-                    if (!m_PrimObjects.ContainsKey(localID))
-                        m_PrimObjects[localID] = new List<UUID>();
+                    ShowScriptSaveResponse(item.OwnerID,
+                            assetID, "Compile successful", true);
+                }
 
-                    if (!m_PrimObjects[localID].Contains(itemID))
-                        m_PrimObjects[localID].Add(itemID);
+                instance.AppDomain = appDomain;
+                instance.LineMap = linemap;
+                lockScriptsForWrite(true);
+                m_Scripts[itemID] = instance;
+                lockScriptsForWrite(false);
+            }
+            else
+            {
+                lockScriptsForRead(false);
+            }
+            lock (m_PrimObjects)
+            {
+                if (!m_PrimObjects.ContainsKey(localID))
+                    m_PrimObjects[localID] = new List<UUID>();
 
-                }
+                if (!m_PrimObjects[localID].Contains(itemID))
+                    m_PrimObjects[localID].Add(itemID);
 
-                if (!m_Assemblies.ContainsKey(assetID))
-                    m_Assemblies[assetID] = assembly;
+            }
 
-                lock (m_AddingAssemblies) 
-                {
-                    m_AddingAssemblies[assembly]--;
-                }
+            if (!m_Assemblies.ContainsKey(assetID))
+                m_Assemblies[assetID] = assembly;
 
-                if (instance!=null) 
-                    instance.Init();
+            lock (m_AddingAssemblies) 
+            {
+                m_AddingAssemblies[assembly]--;
             }
+
+            if (instance!=null) 
+                instance.Init();
+            
             return true;
         }
 
         public void OnRemoveScript(uint localID, UUID itemID)
         {
-            lock (m_Scripts)
+            lockScriptsForRead(true);
+            // Do we even have it?
+            if (!m_Scripts.ContainsKey(itemID))
             {
-                // Do we even have it?
-                if (!m_Scripts.ContainsKey(itemID))
-                    return;
-
-                IScriptInstance instance=m_Scripts[itemID];
-                m_Scripts.Remove(itemID);
+                lockScriptsForRead(false);
+                return;
+            }
+             
 
-                instance.ClearQueue();
-                instance.Stop(0);
+            IScriptInstance instance=m_Scripts[itemID];
+            lockScriptsForRead(false);
+            lockScriptsForWrite(true);
+            m_Scripts.Remove(itemID);
+            lockScriptsForWrite(false);
+            instance.ClearQueue();
+            instance.Stop(0);
 
 //                bool objectRemoved = false;
 
-                lock (m_PrimObjects)
+            lock (m_PrimObjects)
+            {
+                // Remove the script from it's prim
+                if (m_PrimObjects.ContainsKey(localID))
                 {
-                    // Remove the script from it's prim
-                    if (m_PrimObjects.ContainsKey(localID))
-                    {
-                        // Remove inventory item record
-                        if (m_PrimObjects[localID].Contains(itemID))
-                            m_PrimObjects[localID].Remove(itemID);
+                    // Remove inventory item record
+                    if (m_PrimObjects[localID].Contains(itemID))
+                        m_PrimObjects[localID].Remove(itemID);
 
-                        // If there are no more scripts, remove prim
-                        if (m_PrimObjects[localID].Count == 0)
-                        {
-                            m_PrimObjects.Remove(localID);
+                    // If there are no more scripts, remove prim
+                    if (m_PrimObjects[localID].Count == 0)
+                    {
+                        m_PrimObjects.Remove(localID);
 //                            objectRemoved = true;
-                        }
                     }
                 }
+            }
 
-                instance.RemoveState();
-                instance.DestroyScriptInstance();
-
-                m_DomainScripts[instance.AppDomain].Remove(instance.ItemID);
-                if (m_DomainScripts[instance.AppDomain].Count == 0)
-                {
-                    m_DomainScripts.Remove(instance.AppDomain);
-                    UnloadAppDomain(instance.AppDomain);
-                }
+            instance.RemoveState();
+            instance.DestroyScriptInstance();
 
-                instance = null;
+            m_DomainScripts[instance.AppDomain].Remove(instance.ItemID);
+            if (m_DomainScripts[instance.AppDomain].Count == 0)
+            {
+                m_DomainScripts.Remove(instance.AppDomain);
+                UnloadAppDomain(instance.AppDomain);
+            }
 
-                ObjectRemoved handlerObjectRemoved = OnObjectRemoved;
-                if (handlerObjectRemoved != null)
-                {
-                    SceneObjectPart part = m_Scene.GetSceneObjectPart(localID);
-                    handlerObjectRemoved(part.UUID);
-                }
+            instance = null;
 
-                CleanAssemblies();
+            ObjectRemoved handlerObjectRemoved = OnObjectRemoved;
+            if (handlerObjectRemoved != null)
+            {
+                SceneObjectPart part = m_Scene.GetSceneObjectPart(localID);                    
+                handlerObjectRemoved(part.UUID);
             }
 
+            CleanAssemblies();
+            
+
             ScriptRemoved handlerScriptRemoved = OnScriptRemoved;
             if (handlerScriptRemoved != null)
                 handlerScriptRemoved(itemID);
@@ -1099,12 +1178,14 @@ namespace OpenSim.Region.ScriptEngine.XEngine
         private IScriptInstance GetInstance(UUID itemID)
         {
             IScriptInstance instance;
-            lock (m_Scripts)
+            lockScriptsForRead(true);
+            if (!m_Scripts.ContainsKey(itemID))
             {
-                if (!m_Scripts.ContainsKey(itemID))
-                    return null;
-                instance = m_Scripts[itemID];
+                lockScriptsForRead(false);
+                return null;
             }
+            instance = m_Scripts[itemID];
+            lockScriptsForRead(false);
             return instance;
         }
 
@@ -1128,6 +1209,7 @@ namespace OpenSim.Region.ScriptEngine.XEngine
             return false;
         }
 
+        [DebuggerNonUserCode]
         public void ApiResetScript(UUID itemID)
         {
             IScriptInstance instance = GetInstance(itemID);
@@ -1179,6 +1261,7 @@ namespace OpenSim.Region.ScriptEngine.XEngine
             return UUID.Zero;
         }
 
+        [DebuggerNonUserCode]
         public void SetState(UUID itemID, string newState)
         {
             IScriptInstance instance = GetInstance(itemID);
@@ -1199,11 +1282,10 @@ namespace OpenSim.Region.ScriptEngine.XEngine
         {
             List<IScriptInstance> instances = new List<IScriptInstance>();
 
-            lock (m_Scripts)
-            {
-                foreach (IScriptInstance instance in m_Scripts.Values)
+            lockScriptsForRead(true);
+            foreach (IScriptInstance instance in m_Scripts.Values)
                     instances.Add(instance);
-            }
+            lockScriptsForRead(false);
 
             foreach (IScriptInstance i in instances)
             {
diff --git a/bin/OpenSim.ini.example b/bin/OpenSim.ini.example
index 2457318..9b0c09b 100644
--- a/bin/OpenSim.ini.example
+++ b/bin/OpenSim.ini.example
@@ -397,6 +397,9 @@
 
     ; Distance in meters that shouts should travel.  Default is 100m
     shout_distance = 100
+    
+    ; Append a prefix to the god avatar names appearing in chat whilst in god mode
+    ; admin_prefix = "@"
 
 
 [Messaging]
@@ -928,6 +931,9 @@
 
     ; Allow the use of os* functions (some are dangerous)
     AllowOSFunctions = false
+    
+    ; Allow the user of Careminster functions
+    AllowCareminsterFunctions = false
 
     ; Threat level to allow, one of None, VeryLow, Low, Moderate, High, VeryHigh, Severe
     OSFunctionThreatLevel = VeryLow
@@ -1214,6 +1220,9 @@
 ; For example- http://127.0.0.1:9000/SStats/
 ; enabled=false
 
+[LandManagement]
+; DefaultAdministratorGroupUUID = "";
+; DefaultAdministratorParcelName = "";
 
 ;;
 ;; These are defaults that are overwritten below in [Architecture].
diff --git a/prebuild.xml b/prebuild.xml
index 5cc742c..bfd6bbd 100644
--- a/prebuild.xml
+++ b/prebuild.xml
@@ -544,6 +544,36 @@
       </Files>
     </Project>
 
+ <Project frameworkVersion="v3_5" name="OpenSim.Region.Physics.ChOdePlugin" path="OpenSim/Region/Physics/ChOdePlugin" type="Library">
+      <Configuration name="Debug">
+        <Options>
+          <OutputPath>../../../../bin/Physics/</OutputPath>
+        </Options>
+      </Configuration>
+      <Configuration name="Release">
+        <Options>
+          <OutputPath>../../../../bin/Physics/</OutputPath>
+        </Options>
+      </Configuration>
+
+      <ReferencePath>../../../../bin/</ReferencePath>
+      <Reference name="System"/>
+      <Reference name="System.Core"/>
+      <Reference name="OpenMetaverseTypes.dll"/>
+      <Reference name="Nini.dll" />
+      <Reference name="OpenSim.Framework"/>
+      <Reference name="OpenSim.Framework.Console"/>
+      <Reference name="OpenSim.Region.Physics.Manager"/>
+      <Reference name="Ode.NET.dll" />    
+      <Reference name="log4net.dll"/>
+
+      <Files>
+        <Match pattern="*.cs" recurse="true">
+          <Exclude name="Tests" pattern="Tests"/>
+        </Match>
+      </Files>
+    </Project>
+
     <Project frameworkVersion="v3_5" name="OpenSim.Region.Physics.BulletXPlugin" path="OpenSim/Region/Physics/BulletXPlugin" type="Library">
       <Configuration name="Debug">
         <Options>