13 files changed, 9891 insertions, 45 deletions

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..505d455
--- /dev/null
+++ b/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs
@@ -0,0 +1,1437 @@
+/*
+ * 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 jumping = false;       // add for jumping
+        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;
+        private Object m_syncRoot = new Object();
+        
+        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; }
+            set { jumping = value; }    // add for jumping flag
+        }
+
+        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; }
+            get {  //##
+//Console.WriteLine(">>>>>>>>>>>> IC get = {0}", m_iscolliding);  //##
+                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;
+                }
+// ## Console.WriteLine("IC SET = {0}   t{1}  f{2}  i {3}", value, truecount, falsecount, m_iscolliding);
+                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;
+
+            }
+
+            if(Shell != IntPtr.Zero)
+            {
+                try
+                {
+                    d.GeomDestroy(Shell);
+                }
+                catch (System.AccessViolationException)
+                {
+                    m_log.Error("[PHYSICS]: PrimGeom dead");
+                }
+                // Remove any old entries
+//string tShell;
+//_parent_scene.geom_name_map.TryGetValue(Shell, out tShell);
+//Console.WriteLine("**** Remove {0}", tShell);
+                if(_parent_scene.geom_name_map.ContainsKey(Shell)) _parent_scene.geom_name_map.Remove(Shell);
+                if(_parent_scene.actor_name_map.ContainsKey(Shell)) _parent_scene.actor_name_map.Remove(Shell);
+            }
+
+            Shell = d.CreateCapsule(_parent_scene.space, CAPSULE_RADIUS, CAPSULE_LENGTH);
+            _parent_scene.geom_name_map[Shell] = m_name;
+            _parent_scene.actor_name_map[Shell] = (PhysicsActor)this;
+Console.WriteLine("**** Create {2}    Dicts: actor={0}   name={1}  height={3} rad={4}", _parent_scene.actor_name_map.Count, _parent_scene.geom_name_map.Count, m_name, CAPSULE_LENGTH, CAPSULE_RADIUS);
+
+            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;
+//Console.WriteLine("DF 1"); // ##
+                    if (!force.ApproxEquals(Vector3.Zero, 0.01f))
+                        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();
+           d.Vector3 vel = d.BodyGetLinearVel(Body);  //##
+//Console.WriteLine("AvVel <{0},{1},{2}>", vel.X, vel.Y, vel.Z); //##
+            }
+        }
+
+        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)
+                {
+                    try
+                    {
+                        d.GeomDestroy(Shell);
+                    }
+                    catch (System.AccessViolationException)
+                    {
+                        m_log.Error("[PHYSICS]: PrimGeom dead");
+                    }
+                    // Remove any old entries
+//string tShell;
+//_parent_scene.geom_name_map.TryGetValue(Shell, out tShell);
+//Console.WriteLine("**** Remove {0}", tShell);
+
+                    if(_parent_scene.geom_name_map.ContainsKey(Shell)) _parent_scene.geom_name_map.Remove(Shell);
+                    if(_parent_scene.actor_name_map.ContainsKey(Shell)) _parent_scene.actor_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);
+                    float errX = _zeroPosition.X - pos.X;
+                    float errY = _zeroPosition.Y - pos.Y;
+                    if( (Math.Abs(errX) > 0.1f) || (Math.Abs(errY) > 0.1f) )
+                    {
+                        vec.X = (_target_velocity.X - vel.X) * (PID_D) + (errX) * (PID_P * 2);
+                        vec.Y = (_target_velocity.Y - vel.Y) * (PID_D) + (errY) * (PID_P * 2);
+                    }
+                    else
+                    {   // close, jump to lateral destination
+                        d.BodySetPosition(Body, _zeroPosition.X, _zeroPosition.Y, pos.Z);
+                    }
+//                    if (flying)
+                    if (flying || jumping)  // add for jumping
+                    {
+                        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 (Math.Abs(_target_velocity.X) > 0)
+                    {
+                        vec.X = ((_target_velocity.X - vel.X)/1.2f)*PID_D;
+                    }
+                    if (Math.Abs(_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())
+            {
+                if (!vec.ApproxEquals(Vector3.Zero, 0.02f))     // 0.01 allows 0.002 !!
+                {
+//Console.WriteLine("DF 2"); // ##
+
+                        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)
+                {
+                    try
+                    {
+                        d.GeomDestroy(Shell);
+                    }
+                    catch (System.AccessViolationException)
+                    {
+                        m_log.Error("[PHYSICS]: PrimGeom dead");
+                    }
+                    // Remove any old entries
+//string tShell;
+//_parent_scene.geom_name_map.TryGetValue(Shell, out tShell);
+//Console.WriteLine("**** Remove {0}", tShell);
+
+                    if(_parent_scene.geom_name_map.ContainsKey(Shell)) _parent_scene.geom_name_map.Remove(Shell);
+                    if(_parent_scene.actor_name_map.ContainsKey(Shell)) _parent_scene.actor_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)
+        {
+            lock (m_syncRoot)
+            {
+
+                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.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)
+                        {
+                            try
+                            {
+                                d.GeomDestroy(Shell);
+                            }
+                            catch (System.AccessViolationException)
+                            {
+                                m_log.Error("[PHYSICS]: PrimGeom dead");
+                            }
+                            // Remove any old entries
+    //string tShell;
+    //_parent_scene.geom_name_map.TryGetValue(Shell, out tShell);
+    //Console.WriteLine("**** Remove {0}", tShell);
+
+                            if(_parent_scene.geom_name_map.ContainsKey(Shell)) _parent_scene.geom_name_map.Remove(Shell);
+                            if(_parent_scene.actor_name_map.ContainsKey(Shell)) _parent_scene.actor_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);
+                        AvatarGeomAndBodyCreation(_position.X, _position.Y,
+                                          _position.Z + (Math.Abs(CAPSULE_LENGTH - prevCapsule) * 2), m_tensor);
+                        Velocity = Vector3.Zero;
+                    }
+                    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..793774a
--- /dev/null
+++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
@@ -0,0 +1,3917 @@
+/* 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_rotateEnable = Vector3.One;                           // Current setting
+        private Vector3 m_rotateEnableRequest = Vector3.One;                    // Request from LSL
+        private bool m_rotateEnableUpdate = false;
+        private Vector3 m_lockX;
+        private Vector3 m_lockY;
+        private Vector3 m_lockZ;
+        private IntPtr Amotor = IntPtr.Zero;
+        private IntPtr AmotorX = IntPtr.Zero;
+        private IntPtr AmotorY = IntPtr.Zero;
+        private IntPtr AmotorZ = 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_meshfailed = false;
+        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 int revcount = 0;                                                                               // Reverse motion while > 0
+
+        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; }
+  set { 
+    m_buoyancy = value; 
+            
+    Console.WriteLine("m_buoyancy={0}", m_buoyancy);       
+  }
+        }
+
+        public override void link(PhysicsActor obj)
+        {
+            m_taintparent = obj;
+        }
+
+        public override void delink()
+        {
+            m_taintparent = null;
+        }
+
+        public override void LockAngularMotion(Vector3 axis)
+        {
+            // This is actually ROTATION ENABLE, not a lock.
+            // default is <1,1,1> which is all enabled. 
+            // The lock value is updated inside Move(), no point in using the taint system.
+            // OS 'm_taintAngularLock' etc change to m_rotateEnable.
+            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_rotateEnableRequest = axis;
+                m_rotateEnableUpdate = true;
+            }
+            else
+            {
+                m_log.Warn("[PHYSICS]: Got NaN locking axis from Scene on Object");
+            }
+        }
+
+
+        public void SetGeom(IntPtr geom)
+        {
+            if(prim_geom != IntPtr.Zero)
+            {
+                // Remove any old entries
+//string tPA;
+//_parent_scene.geom_name_map.TryGetValue(prim_geom, out tPA);
+//Console.WriteLine("**** Remove {0}", tPA);
+                if(_parent_scene.geom_name_map.ContainsKey(prim_geom)) _parent_scene.geom_name_map.Remove(prim_geom);
+                if(_parent_scene.actor_name_map.ContainsKey(prim_geom)) _parent_scene.actor_name_map.Remove(prim_geom);
+                d.GeomDestroy(prim_geom);
+                }
+
+            prim_geom = geom;
+//Console.WriteLine("SetGeom to " + prim_geom + " for " + m_primName);     
+            if (prim_geom != IntPtr.Zero)
+            {
+                _parent_scene.geom_name_map[prim_geom] = this.m_primName;
+                _parent_scene.actor_name_map[prim_geom] = (PhysicsActor)this;
+                d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
+                d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
+//Console.WriteLine("**** Create {2}    Dicts: actor={0}   name={1}", _parent_scene.actor_name_map.Count, _parent_scene.geom_name_map.Count, this.m_primName);
+            }
+
+            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);
+                Halt();
+            }
+        }
+
+        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 = _size.X * _size.Y * _size.Z; // default
+            float tmp;
+
+            float returnMass = 0;
+            float hollowAmount = (float)_pbs.ProfileHollow * 2.0e-5f;
+            float hollowVolume = hollowAmount * hollowAmount; 
+            
+            switch (_pbs.ProfileShape)
+            {
+                case ProfileShape.Square:
+                    // default box
+
+                    if (_pbs.PathCurve == (byte)Extrusion.Straight)
+                        {
+                        if (hollowAmount > 0.0)
+                            {
+                            switch (_pbs.HollowShape)
+                                {
+                                case HollowShape.Square:
+                                case HollowShape.Same:
+                                    break;
+
+                                case HollowShape.Circle:
+
+                                    hollowVolume *= 0.78539816339f;
+                                    break;
+
+                                case HollowShape.Triangle:
+
+                                    hollowVolume *= (0.5f * .5f);
+                                    break;
+
+                                default:
+                                    hollowVolume = 0;
+                                    break;
+                                }
+                            volume *= (1.0f - hollowVolume);
+                            }
+                        }
+
+                    else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
+                        {
+                        //a tube 
+
+                        volume *= 0.78539816339e-2f * (float)(200 - _pbs.PathScaleX);
+                        tmp= 1.0f -2.0e-2f * (float)(200 - _pbs.PathScaleY);
+                        volume -= volume*tmp*tmp;
+                        
+                        if (hollowAmount > 0.0)
+                            {
+                            hollowVolume *= hollowAmount;
+                            
+                            switch (_pbs.HollowShape)
+                                {
+                                case HollowShape.Square:
+                                case HollowShape.Same:
+                                    break;
+
+                                case HollowShape.Circle:
+                                    hollowVolume *= 0.78539816339f;;
+                                    break;
+
+                                case HollowShape.Triangle:
+                                    hollowVolume *= 0.5f * 0.5f;
+                                    break;
+                                default:
+                                    hollowVolume = 0;
+                                    break;
+                                }
+                            volume *= (1.0f - hollowVolume);
+                            }
+                        }
+
+                    break;
+
+                case ProfileShape.Circle:
+
+                    if (_pbs.PathCurve == (byte)Extrusion.Straight)
+                        {
+                        volume *= 0.78539816339f; // elipse base
+
+                        if (hollowAmount > 0.0)
+                            {
+                            switch (_pbs.HollowShape)
+                                {
+                                case HollowShape.Same:
+                                case HollowShape.Circle:
+                                    break;
+
+                                case HollowShape.Square:
+                                    hollowVolume *= 0.5f * 2.5984480504799f;
+                                    break;
+
+                                case HollowShape.Triangle:
+                                    hollowVolume *= .5f * 1.27323954473516f;
+                                    break;
+
+                                default:
+                                    hollowVolume = 0;
+                                    break;
+                                }
+                            volume *= (1.0f - hollowVolume);
+                            }
+                        }
+
+                    else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
+                        {
+                        volume *= 0.61685027506808491367715568749226e-2f * (float)(200 - _pbs.PathScaleX);
+                        tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY);
+                        volume *= (1.0f - tmp * tmp);
+                        
+                        if (hollowAmount > 0.0)
+                            {
+
+                            // calculate the hollow volume by it's shape compared to the prim shape
+                            hollowVolume *= hollowAmount;
+
+                            switch (_pbs.HollowShape)
+                                {
+                                case HollowShape.Same:
+                                case HollowShape.Circle:
+                                    break;
+
+                                case HollowShape.Square:
+                                    hollowVolume *= 0.5f * 2.5984480504799f;
+                                    break;
+
+                                case HollowShape.Triangle:
+                                    hollowVolume *= .5f * 1.27323954473516f;
+                                    break;
+
+                                default:
+                                    hollowVolume = 0;
+                                    break;
+                                }
+                            volume *= (1.0f - hollowVolume);
+                            }
+                        }
+                    break;
+
+                case ProfileShape.HalfCircle:
+                    if (_pbs.PathCurve == (byte)Extrusion.Curve1)
+                    {
+                    volume *= 0.52359877559829887307710723054658f;
+                    }
+                    break;
+
+                case ProfileShape.EquilateralTriangle:
+
+                    if (_pbs.PathCurve == (byte)Extrusion.Straight)
+                        {
+                        volume *= 0.32475953f;
+
+                        if (hollowAmount > 0.0)
+                            {
+
+                            // calculate the hollow volume by it's shape compared to the prim shape
+                            switch (_pbs.HollowShape)
+                                {
+                                case HollowShape.Same:
+                                case HollowShape.Triangle:
+                                    hollowVolume *= .25f;
+                                    break;
+
+                                case HollowShape.Square:
+                                    hollowVolume *= 0.499849f * 3.07920140172638f;
+                                    break;
+
+                                case HollowShape.Circle:
+                                    // Hollow shape is a perfect cyllinder in respect to the cube's scale
+                                    // Cyllinder hollow volume calculation
+
+                                    hollowVolume *= 0.1963495f * 3.07920140172638f;
+                                    break;
+
+                                default:
+                                    hollowVolume = 0;
+                                    break;
+                                }
+                            volume *= (1.0f - hollowVolume);
+                            }
+                        }
+                    else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
+                        {
+                        volume *= 0.32475953f;
+                        volume *= 0.01f * (float)(200 - _pbs.PathScaleX);
+                        tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY);
+                        volume *= (1.0f - tmp * tmp);
+
+                        if (hollowAmount > 0.0)
+                            {
+
+                            hollowVolume *= hollowAmount;
+
+                            switch (_pbs.HollowShape)
+                                {
+                                case HollowShape.Same:
+                                case HollowShape.Triangle:
+                                    hollowVolume *= .25f;
+                                    break;
+
+                                case HollowShape.Square:
+                                    hollowVolume *= 0.499849f * 3.07920140172638f;
+                                    break;
+
+                                case HollowShape.Circle:
+
+                                    hollowVolume *= 0.1963495f * 3.07920140172638f;
+                                    break;
+
+                                default:
+                                    hollowVolume = 0;
+                                    break;
+                                }
+                            volume *= (1.0f - hollowVolume);
+                            }
+                        }
+                        break;                  
+
+                default:
+                    break;
+                }
+
+
+
+            float taperX1;
+            float taperY1;
+            float taperX;
+            float taperY;
+            float pathBegin;
+            float pathEnd;
+            float profileBegin;
+            float profileEnd;
+
+            if (_pbs.PathCurve == (byte)Extrusion.Straight || _pbs.PathCurve == (byte)Extrusion.Flexible)
+                {
+                taperX1 = _pbs.PathScaleX * 0.01f;
+                if (taperX1 > 1.0f)
+                    taperX1 = 2.0f - taperX1;
+                taperX = 1.0f - taperX1;
+
+                taperY1 = _pbs.PathScaleY * 0.01f;
+                if (taperY1 > 1.0f)
+                    taperY1 = 2.0f - taperY1;
+                taperY = 1.0f - taperY1;
+                }
+            else
+                {
+                taperX = _pbs.PathTaperX * 0.01f;
+                if (taperX < 0.0f)
+                    taperX = -taperX;
+                taperX1 = 1.0f - taperX;
+
+                taperY = _pbs.PathTaperY * 0.01f;
+                if (taperY < 0.0f)
+                    taperY = -taperY;               
+                taperY1 = 1.0f - taperY;
+
+                }
+
+
+            volume *= (taperX1 * taperY1 + 0.5f * (taperX1 * taperY + taperX * taperY1) + 0.3333333333f * taperX * taperY);
+
+            pathBegin = (float)_pbs.PathBegin * 2.0e-5f;
+            pathEnd = 1.0f - (float)_pbs.PathEnd * 2.0e-5f;
+            volume *= (pathEnd - pathBegin);
+
+// this is crude aproximation
+            profileBegin = (float)_pbs.ProfileBegin * 2.0e-5f;
+            profileEnd = 1.0f - (float)_pbs.ProfileEnd * 2.0e-5f;
+            volume *= (profileEnd - profileBegin);
+
+            returnMass = m_density * volume;
+
+            if (returnMass <= 0)
+                returnMass = 0.0001f;//ckrinke: Mass must be greater then zero.
+//            else if (returnMass > _parent_scene.maximumMassObject)
+//                returnMass = _parent_scene.maximumMassObject;
+
+
+
+
+            // 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 takes care of phys engine recreate and prim_geom pointer 
+            //    {
+                    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);
+/* obsolete                    
+                if (!m_angularLock.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.)");
+            }
+        }
+
+/* obsolete
+        private void changeAngularLock(float timestep) 
+        {
+            if (_parent == null)
+            {
+                m_angularLock = m_taintAngularLock;
+                m_angularLockSet = true;
+            }
+        }
+ */
+        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();
+
+                    if (Body != IntPtr.Zero)
+                    {
+                        d.BodySetLinearVel(Body, 0f, 0f, 0f);
+                        d.BodySetForce(Body, 0, 0, 0);
+                        enableBodySoft();
+                    }
+                }
+            }
+            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);
+                }
+/* Uhhh - stop the motion if the object is _selected_!!
+                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)                                  // Special - make mesh
+            {
+                setMesh(_parent_scene, _mesh);
+            }
+            else                                                // not a mesh
+            {
+                if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1)           // special profile??
+                {
+                    if (_size.X == _size.Y && _size.Y == _size.Z && _size.X == _size.Z)                                 // Equi-size
+                    {
+                        if (((_size.X / 2f) > 0f))                                                                      // Has size
+                        {
+                            _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                                                                                                // not equi-size
+                    {
+                        _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                                                                                    // not special profile
+                {
+                    _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 && m_meshfailed == false)
+            {
+                if (_parent_scene.needsMeshing(_pbs))
+                {
+                    // Don't need to re-enable body..   it's done in SetMesh
+                    try
+                    {
+                        _mesh = _parent_scene.mesher.CreateMesh(m_primName, _pbs, _size, _parent_scene.meshSculptLOD, IsPhysical);
+                    }
+                    catch
+                    {
+                        //Don't continuously try to mesh prims when meshing has failed
+                        m_meshfailed = true;
+                    }
+                    // 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();
+                }
+            }
+
+            changeSelectedStatus(timestep);
+
+            m_taintadd = false;
+        }
+
+        public void changemove(float timestep)
+        {
+//Console.WriteLine("changemove sing/root {0} to {1}", m_primName, _position );
+            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)
+                {
+                    if (Body == IntPtr.Zero)
+                        enableBody();
+                    //Prim auto disable after 20 frames,
+                    //if you move it, re-enable the prim manually.
+                    if (_parent != null)
+                    {
+                        if (m_linkJoint != IntPtr.Zero)
+                        {
+                            d.JointDestroy(m_linkJoint);
+                            m_linkJoint = IntPtr.Zero;
+                        }
+                    }
+                    if (Body != IntPtr.Zero)
+                    {
+                        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
+            {
+                // 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)
+                    {
+                        _mesh = null;
+//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);
+            }
+            // 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) && m_meshfailed == false)
+            {
+                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;
+
+                try
+                {
+                    if (_parent_scene.needsMeshing(_pbs))
+                        mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical);
+                }
+                catch
+                {
+                    m_meshfailed = true;
+                }
+
+                //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();
+                }
+            }
+
+
+            // 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) && m_meshfailed == false)
+            {
+                // Don't need to re-enable body..   it's done in SetMesh
+                float meshlod = _parent_scene.meshSculptLOD;
+
+                if (IsPhysical)
+                    meshlod = _parent_scene.MeshSculptphysicalLOD;
+                try
+                {
+                    IMesh mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical);
+                    CreateGeom(m_targetSpace, mesh);
+                }
+                catch
+                {
+                    m_meshfailed = true;
+                }
+                // createmesh returns null when it doesn't 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
+        }
+        
+        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.1f) pValue = 0.1f;
+                    // m_angularDeflectionTimescale = pValue;
+                    break;
+                case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE:
+                    if (pValue < 0.3f) pValue = 0.3f;
+                    m_angularMotorDecayTimescale = pValue;
+                    break;
+                case Vehicle.ANGULAR_MOTOR_TIMESCALE:
+                    if (pValue < 0.3f) pValue = 0.3f;
+                    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.1f) pValue = 0.1f;
+                    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.3f) pValue = 0.3f;
+                    m_linearMotorDecayTimescale = pValue;
+                    break;
+                case Vehicle.LINEAR_MOTOR_TIMESCALE:
+                    if (pValue < 0.1f) pValue = 0.1f;
+                    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.1f) pValue = 0.1f;
+                    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:
+                        if (pValue < 0.1f) pValue = 0.1f;
+                    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:
+                        if (pValue.X < 0.1f) pValue.X = 0.1f;
+                        if (pValue.Y < 0.1f) pValue.Y = 0.1f;
+                        if (pValue.Z < 0.1f) pValue.Z = 0.1f;
+                    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;
+                        _acceleration = 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;
+                        m_lLinMotorDVel.X = m_linearMotorDirection.X;
+                        m_lLinMotorDVel.Y = m_linearMotorDirection.Y;
+                        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;
+                        m_angularMotorDVel.X = m_angularMotorDirection.X;
+                        m_angularMotorDVel.Y = m_angularMotorDirection.Y;
+                        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;
+            Vector3 linvel;                             // velocity applied, including any reversal
+            int outside = 0;
+            
+            // If geomCrossingFailuresBeforeOutofbounds is set to 0 in OpenSim.ini then phys objects bounce off region borders.
+            // This is a temp patch until proper region crossing is developed. 
+            
+            int failureLimit = _parent_scene.geomCrossingFailuresBeforeOutofbounds;
+            int fence = _parent_scene.geomRegionFence;
+            
+                        float border_limit = 0.05f;     // original limit
+                        if (fence == 1) border_limit = 0.5f;            // bounce point
+
+            frcount++;                                  // used to limit debug comment output
+            if (frcount > 50)
+                frcount = 0;
+                
+            if(revcount > 0) revcount--;
+                     
+            if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim)             // Only move root prims.
+            {
+                                //  Old public void UpdatePositionAndVelocity(), more accuratley calculated here
+                bool lastZeroFlag = _zeroFlag;  // was it stopped
+                
+                d.Vector3 vec = d.BodyGetPosition(Body);
+                Vector3 l_position = Vector3.Zero;
+                l_position.X = vec.X;
+                l_position.Y = vec.Y;
+                l_position.Z = vec.Z;
+                m_lastposition = _position;
+                _position = l_position;
+                
+                d.Quaternion ori = d.BodyGetQuaternion(Body);
+         //       Quaternion l_orientation = Quaternion.Identity;
+                _orientation.X = ori.X;
+                _orientation.Y = ori.Y;
+                _orientation.Z = ori.Z;
+                _orientation.W = ori.W;
+                m_lastorientation = _orientation;
+                
+                d.Vector3 vel = d.BodyGetLinearVel(Body);
+                m_lastVelocity = _velocity;
+                _velocity.X = vel.X;
+                _velocity.Y = vel.Y;
+                _velocity.Z = vel.Z;
+                _acceleration = ((_velocity - m_lastVelocity) / timestep);
+                
+                d.Vector3 torque = d.BodyGetTorque(Body);
+                _torque = new Vector3(torque.X, torque.Y, torque.Z);
+                
+                base.RequestPhysicsterseUpdate();
+                
+//Console.WriteLine("Move {0}  at  {1}", m_primName, l_position);        
+                    
+                                // Check if outside region
+                                // In Scene.cs/CrossPrimGroupIntoNewRegion the object is checked for 0.1M from border!
+                if (l_position.X > ((float)_parent_scene.WorldExtents.X - border_limit))
+                {
+                    l_position.X = ((float)_parent_scene.WorldExtents.X - border_limit);
+                        outside = 1;
+                }
+                
+                if (l_position.X < border_limit)
+                {
+                                        l_position.X = border_limit;
+                                        outside = 2;
+                                }
+                                if (l_position.Y > ((float)_parent_scene.WorldExtents.Y - border_limit))
+                                {
+                    l_position.Y = ((float)_parent_scene.WorldExtents.Y - border_limit);
+                        outside = 3;
+                }
+                                
+                if (l_position.Y < border_limit)
+                {
+                                        l_position.Y = border_limit;
+                                        outside = 4;
+                                }
+                
+                if (outside > 0)
+                {
+//Console.WriteLine("   fence = {0}",fence);     
+        
+//Console.WriteLine("Border {0}",  l_position);               
+                    if (fence == 1)             // bounce object off boundary 
+                    {
+                        if (revcount == 0)
+                            {
+                                if (outside < 3)
+                                        {
+                                                _velocity.X = -_velocity.X;
+                                        }
+                                        else
+                                        {
+                                                _velocity.Y = -_velocity.Y;
+                                        }
+                                        if (m_type != Vehicle.TYPE_NONE) Halt();
+                                        _position = l_position;
+                                    m_taintposition = _position;
+                                                m_lastVelocity = _velocity;     
+                                                        _acceleration = Vector3.Zero;   
+                            d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
+                                                d.BodySetLinearVel(Body, _velocity.X, _velocity.Y, _velocity.Z);
+                            base.RequestPhysicsterseUpdate();
+
+                                        revcount = 25;          // wait for object to move away from border
+                                    }
+                    } // else old crossing mode
+                    else if (m_crossingfailures < failureLimit)
+                    {   // 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);
+//Console.WriteLine("ROOB 2");            
+                        
+                        return;         // Dont process any other motion?
+                    }  // end various methods
+                }    // 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);
+//Console.WriteLine("ROOB 3");            
+                    
+                    
+                    _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, _orientation)) < 0.0001))  // KF 0.01 is far to large
+                {
+                    _zeroFlag = true;
+                    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;
+                    
+                    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_lastUpdateSent = false;
+                    if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate)
+                    {
+                        if (_parent == null)
+                        {
+                            base.RequestPhysicsterseUpdate();
+                        }
+                    }
+                    else
+                    {
+                        throttleCounter++;
+                    }
+                }
+                m_lastposition = l_position;
+             
+                                /// End UpdatePositionAndVelocity insert     
+                                
+                                
+                // Rotation lock  =====================================
+                if(m_rotateEnableUpdate)
+                {   
+                    // Snapshot current angles, set up Amotor(s)
+                    m_rotateEnableUpdate = false;
+                    m_rotateEnable = m_rotateEnableRequest;
+Console.WriteLine("RotEnable {0} = {1}",m_primName,  m_rotateEnable);
+
+                    if (Amotor != IntPtr.Zero)
+                    {
+                        d.JointDestroy(Amotor);
+                        Amotor = IntPtr.Zero;
+Console.WriteLine("Old Amotor Destroyed");                        
+                    }
+                    
+                    if (!m_rotateEnable.ApproxEquals(Vector3.One, 0.003f))
+                    {   // not all are enabled
+                        d.Quaternion r = d.BodyGetQuaternion(Body);
+                        Quaternion locrot = new Quaternion(r.X, r.Y, r.Z, r.W);
+                        // extract the axes vectors
+                        Vector3 vX = new Vector3(1f,0f,0f);
+                        Vector3 vY = new Vector3(0f,1f,0f);
+                        Vector3 vZ = new Vector3(0f,0f,1f);
+                        vX = vX * locrot;    
+                        vY = vY * locrot;
+                        vZ = vZ * locrot;
+                        // snapshot the current angle vectors
+                        m_lockX = vX;
+                        m_lockY = vY;
+                        m_lockZ = vZ;
+             //           m_lockRot = locrot;
+                        Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero);
+                        d.JointAttach(Amotor, Body, IntPtr.Zero);
+                        d.JointSetAMotorMode(Amotor, 0);  // User mode??
+Console.WriteLine("New Amotor Created for {0}", m_primName);                   
+                    
+                        float axisnum = 3;  // how many to lock
+                        axisnum = (axisnum - (m_rotateEnable.X + m_rotateEnable.Y + m_rotateEnable.Z));
+                        d.JointSetAMotorNumAxes(Amotor,(int)axisnum);
+Console.WriteLine("AxisNum={0}",(int)axisnum);                       
+
+                        int i = 0;
+
+                        if (m_rotateEnable.X == 0)
+                        {
+                            d.JointSetAMotorAxis(Amotor, i, 0, m_lockX.X, m_lockX.Y, m_lockX.Z);
+Console.WriteLine("AxisX {0} set to {1}", i,  m_lockX);                           
+                            i++;
+                        }
+
+                        if (m_rotateEnable.Y == 0)
+                        {
+                            d.JointSetAMotorAxis(Amotor, i, 0, m_lockY.X, m_lockY.Y, m_lockY.Z);
+Console.WriteLine("AxisY {0} set to {1}", i,  m_lockY);                           
+                            i++;
+                        }
+
+                        if (m_rotateEnable.Z == 0)
+                        {
+                            d.JointSetAMotorAxis(Amotor, i, 0, m_lockZ.X, m_lockZ.Y, m_lockZ.Z);
+Console.WriteLine("AxisZ {0} set to {1}", i,  m_lockZ);                           
+                            i++;
+                        }
+
+                        // These lowstops and high stops are effectively (no wiggle room)
+                        d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, 0f);
+                        d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, 0f);
+                        d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, 0f);
+                        d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0f);
+                        d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0f);
+                        d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0f);
+                        d.JointSetAMotorParam(Amotor, (int) dParam.Vel, 0f);
+                        d.JointSetAMotorParam(Amotor, (int) dParam.Vel3, 0f);
+                        d.JointSetAMotorParam(Amotor, (int) dParam.Vel2, 0f);
+                        d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM, 0f);
+                        d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM3, 0f);
+                        d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM2, 0f);
+                    } // else none are locked
+                } // end Rotation Update  
+                
+                
+                // VEHICLE processing ==========================================            
+                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;
+                                                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
+                                                if (m_wLinObjectVel.Z == 0f)
+                                                {               // Gravity rules
+                                                        m_wLinObjectVel.Z = vel_now.Z;
+                                                }  // else the motor has it
+                            }   
+                            linvel = m_wLinObjectVel;
+
+                            // Vehicle Linear  Motion done =======================================
+                                // Apply velocity
+                                        d.BodySetLinearVel(Body, linvel.X, linvel.Y, linvel.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);   
+
+                            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);         
+                        
+        
+                /*                      // Rotation Axis Disables:
+                                    if (!m_angularEnable.ApproxEquals(Vector3.One, 0.003f))
+                                {
+                                if (m_angularEnable.X == 0)
+                                        angObjectVel.X = 0f;
+                                    if (m_angularEnable.Y == 0)
+                                        angObjectVel.Y = 0f;
+                                    if (m_angularEnable.Z == 0)
+                                        angObjectVel.Z = 0f;
+                                }        
+                        */        
+                                        angObjectVel = angObjectVel * rotq; // ================ Converts to WORLD rotation
+                                        
+                            // 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)
+                                                Vector3 xyav = angObjectVel;
+                                                xyav.Z = 0.0f;
+                                                if ((!xyav.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);         
+
+                                        
+                                        m_lastAngularVelocity = angObjectVel;
+                                        // apply Angular Velocity to body
+                                        d.BodySetAngularVel (Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z);
+//if(frcount == 0) Console.WriteLine("V4 = {0}", m_lastAngularVelocity);                
+                            
+                }  // end VEHICLES
+                else
+                {
+                        // Dyamics (NON-'VEHICLES') are dealt with here ================================================================                        
+                        
+                        if(!d.BodyIsEnabled (Body))  d.BodyEnable (Body); // KF add 161009
+                        
+                                        /// 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
+                        // This should only affect gravity operations 
+                        
+                        float m_mass = CalculateMass();
+                        // calculate z-force due togravity on object.
+                        fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass; // force = acceleration * mass
+
+                        if ((m_usePID) && (m_PIDTau > 0.0f))  // Dynamics  llMoveToTarget.
+                        {
+                            fz = 0;     // llMoveToTarget ignores gravity.
+                                        // it also ignores mass of object, and any physical resting on it.
+                                        // Vector3 m_PIDTarget is where we are going
+                                    // float m_PIDTau is time to get there
+                        fx = 0;
+                        fy = 0;                                    
+                            d.Vector3 pos = d.BodyGetPosition(Body);
+                            Vector3 error = new Vector3(
+                                    (m_PIDTarget.X - pos.X),
+                                    (m_PIDTarget.Y - pos.Y),
+                                    (m_PIDTarget.Z - pos.Z));
+                            if (error.ApproxEquals(Vector3.Zero,0.01f))
+                            {   // Very close, Jump there and quit move
+                                d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);
+                                _target_velocity = Vector3.Zero;
+                            d.BodySetLinearVel(Body, _target_velocity.X, _target_velocity.Y, _target_velocity.Z);
+                            }
+                            else 
+                            {
+                            float scale =  50.0f * timestep / m_PIDTau;
+                            if ((error.ApproxEquals(Vector3.Zero,0.5f)) && (_target_velocity != Vector3.Zero))
+                            {
+                                    // Nearby, quit update of velocity
+                                }
+                                else
+                                {  // Far, calc damped velocity
+                                    _target_velocity = error * scale;
+                                }
+                            d.BodySetLinearVel(Body, _target_velocity.X, _target_velocity.Y, _target_velocity.Z);                                   
+                        }
+                        } // end PID MoveToTarget
+                        
+    /* Original OS implementation: Does not work correctly as another phys object resting on THIS object purturbs its position.
+       This is incorrect behavior. llMoveToTarget must move the Body no matter what phys object is resting on it.                       
+       
+                        //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(frcount == 0) Console.WriteLine("PID {0}  b={1}  fz={2}  vel={3}", m_primName, m_buoyancy, fz, _target_velocity);            
+                            //  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)
+ End of old PID  system */
+ 
+                        
+                        /// Dynamics Hover ===================================================================================
+                        // Hover PID Controller can only run if the PIDcontroller is not in use.
+                        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.Vector3 dlinvel = vel;
+
+                            d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight);
+                                                d.BodySetLinearVel(Body, dlinvel.X, dlinvel.Y, dlinvel.Z);       
+                            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);                            
+                    }  // end apply forces
+                                } // end Dynamics
+                                
+/* obsolete?                            
+                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 root prims
+            
+       }  // 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..86f9893
--- /dev/null
+++ b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs
@@ -0,0 +1,3869 @@
+/*
+ * 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 int geomRegionFence = 0;
+
+        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);
+                    geomRegionFence = physicsconfig.GetInt("region_border_fence", 0);
+
+                    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
+//Console.WriteLine("near -----------");  //##
+            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;
+                    if ((p2.Position.Z - curContact.pos.Z) > (p2.Size.Z * 0.6f)){       //##
+//Console.WriteLine("AvColl 1  {0} - {1} - {2} - {3}",        //##
+//                    curContact.pos.Z, p2.Position.Z, (p2.Position.Z - curContact.pos.Z), (p2.Size.Z * 0.6f));                              //##
+                        p2.IsColliding = true;       //##
+                    }else{
+//Console.WriteLine("AvColl 2  {0} - {1} - {2} - {3}",        //##
+//                    curContact.pos.Z, p2.Position.Z, (p2.Position.Z - curContact.pos.Z), (p2.Size.Z * 0.6f));                              //##
+
+                    }  //##   
+                 }
+                else
+                {
+//Console.WriteLine("AvColl 3 {0}", p2.PhysicsActorType); //##
+                    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)
+                            {
+
+//string tPA;
+//geom_name_map.TryGetValue(prim.prim_geom, out tPA);
+//Console.WriteLine("**** Remove {0}", tPA);
+                                if(geom_name_map.ContainsKey(prim.prim_geom)) geom_name_map.Remove(prim.prim_geom);
+                                if(actor_name_map.ContainsKey(prim.prim_geom)) actor_name_map.Remove(prim.prim_geom);
+                                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)
+        {
+           
+            uint regionsize = (uint) Constants.RegionSize;              // visible region size eg. 256(M)
+            
+            uint heightmapWidth = regionsize + 1;                               // ODE map size 257 x 257 (Meters) (1 extra 
+            uint heightmapHeight = regionsize + 1;
+            
+            uint heightmapWidthSamples = (uint)regionsize + 2;          // Sample file size, 258 x 258 samples
+            uint heightmapHeightSamples = (uint)regionsize + 2;
+            
+            // Array of height samples for ODE 
+            float[] _heightmap;
+            _heightmap = new float[(heightmapWidthSamples * heightmapHeightSamples)];   // loaded samples 258 x 258
+
+                        // Other ODE parameters
+            const float scale = 1.0f;
+            const float offset = 0.0f;
+            const float thickness = 2.0f; // Was 0.2f, Larger appears to prevent Av fall-through
+            const int wrap = 0;
+
+            float hfmin = 2000f;
+            float hfmax = -2000f;
+            float minele = 0.0f;        // Dont allow -ve heights
+            
+            uint x = 0;
+            uint y = 0;
+            uint xx = 0;
+            uint yy = 0;
+            
+            // load the height samples array from the heightMap    
+            for ( x = 0; x < heightmapWidthSamples; x++)                                // 0 to 257
+            {
+                for ( y = 0; y < heightmapHeightSamples; y++)                   // 0 to 257
+                {
+                        xx = x - 1;
+                        if (xx < 0) xx = 0;                                                     
+                        if (xx > (regionsize - 1)) xx = regionsize - 1;                                  
+                    
+                        yy = y - 1;
+                        if (yy < 0) yy = 0;                                                     
+                        if (yy > (regionsize - 1)) yy = regionsize - 1;
+                                        // Input xx = 0  0  1  2  ..... 254  255  255      256 total in
+                                // Output x = 0  1  2  3  ..... 255  256  257      258 total out
+                    float val= heightMap[(yy * regionsize) + xx];  // input from heightMap,  <0-255 * 256> <0-255>
+                    if (val < minele) val = minele;
+                    _heightmap[x * (regionsize + 2) + y] = val; // samples output to _heightmap,  <0-257 * 258> <0-257>
+                    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, heightmapHeight, (int)heightmapWidthSamples, 
+                    (int)heightmapHeightSamples, 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 + (regionsize * 0.5f)) - 0.5f, (pOffset.Y + (regionsize * 0.5f)) - 0.5f, 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 c37c9bd..f33b5b9 100644
--- a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
+++ b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
@@ -572,8 +572,8 @@ namespace OpenSim.Region.Physics.OdePlugin
                                     break;
 
                                 case HollowShape.Circle:
-                                    // Hollow shape is a perfect cyllinder in respect to the cube's scale
-                                    // Cyllinder hollow volume calculation
+                                    // Hollow shape is a perfect cylinder in respect to the cube's scale
+                                    // Cylinder hollow volume calculation
 
                                     hollowVolume *= 0.1963495f * 3.07920140172638f;
                                     break;
@@ -2819,7 +2819,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; }