Merge branch 'mbworknew1' into ubitworkvarnew

10 files changed, 11634 insertions, 0 deletions

diff --git a/OpenSim/Region/PhysicsModules/Ode/AssemblyInfo.cs b/OpenSim/Region/PhysicsModules/Ode/AssemblyInfo.cs
new file mode 100644
index 0000000..7869739
--- /dev/null
+++ b/OpenSim/Region/PhysicsModules/Ode/AssemblyInfo.cs
@@ -0,0 +1,62 @@
+/*
+ * 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;
+using Mono.Addins;
+
+// 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")]
+[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.8.2.*")]
+
+[assembly: Addin("OpenSim.Region.PhysicsModule.ODE", OpenSim.VersionInfo.VersionNumber)]
+[assembly: AddinDependency("OpenSim.Region.Framework", OpenSim.VersionInfo.VersionNumber)]
diff --git a/OpenSim/Region/PhysicsModules/Ode/ODECharacter.cs b/OpenSim/Region/PhysicsModules/Ode/ODECharacter.cs
new file mode 100644
index 0000000..b35c299
--- /dev/null
+++ b/OpenSim/Region/PhysicsModules/Ode/ODECharacter.cs
@@ -0,0 +1,1409 @@
+/*
+ * 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.PhysicsModules.SharedBase;
+using log4net;
+
+namespace OpenSim.Region.PhysicsModule.ODE
+{
+    /// <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 bool _zeroFlag = false;
+        private bool m_lastUpdateSent = false;
+        private Vector3 _velocity;
+        private Vector3 m_taintTargetVelocity;
+        private Vector3 _target_velocity;
+        private Vector3 _acceleration;
+        private Vector3 m_rotationalVelocity;
+        private float m_mass = 80f;
+        private float m_density = 60f;
+        private bool m_pidControllerActive = true;
+        private float PID_D = 800.0f;
+        private float PID_P = 900.0f;
+        //private static float POSTURE_SERVO = 10000.0f;
+        private float CAPSULE_RADIUS = 0.37f;
+        private float CAPSULE_LENGTH = 2.140599f;
+        private float m_tensor = 3800000f;
+//        private float heightFudgeFactor = 0.52f;
+        private float walkDivisor = 1.3f;
+        private float runDivisor = 0.8f;
+        private bool flying = false;
+        private bool m_iscolliding = false;
+        private bool m_iscollidingGround = false;
+        private bool m_wascolliding = false;
+        private bool m_wascollidingGround = false;
+        private bool m_iscollidingObj = false;
+        private bool m_alwaysRun = false;
+        private bool m_hackSentFall = false;
+        private bool m_hackSentFly = false;
+        private int m_requestedUpdateFrequency = 0;
+        private Vector3 m_taintPosition;
+        internal bool m_avatarplanted = false;
+        /// <summary>
+        /// Hold set forces so we can process them outside physics calculations.  This prevents race conditions if we set force
+        /// while calculatios are going on
+        /// </summary>
+        private Vector3 m_taintForce;
+
+        // taints and their non-tainted counterparts
+        private bool m_isPhysical = false; // the current physical status
+        private bool m_tainted_isPhysical = false; // set when the physical status is tainted (false=not existing in physics engine, true=existing)
+        internal float MinimumGroundFlightOffset = 3f;
+
+        private float m_tainted_CAPSULE_LENGTH; // set when the capsule length changes.
+
+        /// <summary>
+        /// Base movement for calculating tilt.
+        /// </summary>
+        private float m_tiltBaseMovement = (float)Math.Sqrt(2);
+
+        /// <summary>
+        /// Used to introduce a fixed tilt because a straight-up capsule falls through terrain, probably a bug in terrain collider
+        /// </summary>
+        private float m_tiltMagnitudeWhenProjectedOnXYPlane = 0.1131371f;
+
+        private float m_buoyancy = 0f;
+
+        // private CollisionLocker ode;
+        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);
+        /// <summary>
+        /// Body for dynamics simulation
+        /// </summary>
+        internal IntPtr Body { get; private set; }
+
+        private OdeScene _parent_scene;
+
+        /// <summary>
+        /// Collision geometry
+        /// </summary>
+        internal IntPtr Shell { get; private set; }
+        
+        private IntPtr Amotor = IntPtr.Zero;
+        private d.Mass ShellMass;
+
+        private int m_eventsubscription = 0;
+        private CollisionEventUpdate CollisionEventsThisFrame = new CollisionEventUpdate();
+
+        // unique UUID of this character object
+        internal UUID m_uuid { get; private set; }
+        internal bool bad = false;
+
+        /// <summary>
+        /// ODE Avatar.
+        /// </summary>
+        /// <param name="avName"></param>
+        /// <param name="parent_scene"></param>
+        /// <param name="pos"></param>
+        /// <param name="vel"></param>
+        /// <param name="size"></param>
+        /// <param name="pid_d"></param>
+        /// <param name="pid_p"></param>
+        /// <param name="capsule_radius"></param>
+        /// <param name="tensor"></param>
+        /// <param name="density">
+        /// Only used right now to return information to LSL.  Not actually used to set mass in ODE!
+        /// </param>
+        /// <param name="walk_divisor"></param>
+        /// <param name="rundivisor"></param>
+        public OdeCharacter(
+            String avName, OdeScene parent_scene, Vector3 pos, Vector3 vel, Vector3 size, float pid_d, float pid_p,
+            float capsule_radius, float tensor, float density,
+            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 = pos;
+            }
+            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 = _position;
+
+                m_log.WarnFormat("[ODE CHARACTER]: Got NaN Position on Character Create for {0}", avName);
+            }
+
+            _velocity = vel;
+            m_taintTargetVelocity = vel;
+
+            _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);
+
+            // We can set taint and actual to be the same here, since the entire character will be set up when the
+            // m_tainted_isPhysical is processed.
+            SetTaintedCapsuleLength(size);
+            CAPSULE_LENGTH = m_tainted_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);
+            
+            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 bool Grabbed
+        {
+            set { return; }
+        }
+
+        public override bool Selected
+        {
+            set { return; }
+        }
+
+        public override float Buoyancy
+        {
+            get { return m_buoyancy; }
+            set { m_buoyancy = value; }
+        }
+
+        public override bool FloatOnWater
+        {
+            set { return; }
+        }
+
+        public override bool IsPhysical
+        {
+            get { return false; }
+            set { return; }
+        }
+
+        public override bool ThrottleUpdates
+        {
+            get { return false; }
+            set { return; }
+        }
+
+        public override bool Flying
+        {
+            get { return flying; }
+            set
+            {
+                flying = value;
+//                m_log.DebugFormat("[ODE CHARACTER]: Set OdeCharacter Flying to {0}", flying);
+            }
+        }
+
+        /// <summary>
+        /// Returns if the avatar is colliding in general.
+        /// This includes the ground and objects and avatar.
+        /// </summary>
+        public override bool IsColliding
+        {
+            get { return m_iscolliding; }
+            set
+            {
+                int i;
+                int truecount = 0;
+                int falsecount = 0;
+
+                if (m_colliderarr.Length >= 10)
+                {
+                    for (i = 0; i < 10; i++)
+                    {
+                        m_colliderarr[i] = m_colliderarr[i + 1];
+                    }
+                }
+                m_colliderarr[10] = value;
+
+                for (i = 0; i < 11; i++)
+                {
+                    if (m_colliderarr[i])
+                    {
+                        truecount++;
+                    }
+                    else
+                    {
+                        falsecount++;
+                    }
+                }
+
+                // Equal truecounts and false counts means we're colliding with something.
+
+                if (falsecount > 1.2*truecount)
+                {
+                    m_iscolliding = false;
+                }
+                else
+                {
+                    m_iscolliding = true;
+                }
+
+                if (m_wascolliding != m_iscolliding)
+                {
+                    //base.SendCollisionUpdate(new CollisionEventUpdate());
+                }
+
+                m_wascolliding = m_iscolliding;
+            }
+        }
+
+        /// <summary>
+        /// Returns if an avatar is colliding with the ground
+        /// </summary>
+        public override bool CollidingGround
+        {
+            get { return m_iscollidingGround; }
+            set
+            {
+                // Collisions against the ground are not really reliable
+                // So, to get a consistant value we have to average the current result over time
+                // Currently we use 1 second = 10 calls to this.
+                int i;
+                int truecount = 0;
+                int falsecount = 0;
+
+                if (m_colliderGroundarr.Length >= 10)
+                {
+                    for (i = 0; i < 10; i++)
+                    {
+                        m_colliderGroundarr[i] = m_colliderGroundarr[i + 1];
+                    }
+                }
+                m_colliderGroundarr[10] = value;
+
+                for (i = 0; i < 11; i++)
+                {
+                    if (m_colliderGroundarr[i])
+                    {
+                        truecount++;
+                    }
+                    else
+                    {
+                        falsecount++;
+                    }
+                }
+
+                // Equal truecounts and false counts means we're colliding with something.
+
+                if (falsecount > 1.2*truecount)
+                {
+                    m_iscollidingGround = false;
+                }
+                else
+                {
+                    m_iscollidingGround = true;
+                }
+                if (m_wascollidingGround != m_iscollidingGround)
+                {
+                    //base.SendCollisionUpdate(new CollisionEventUpdate());
+                }
+                m_wascollidingGround = m_iscollidingGround;
+            }
+        }
+
+        /// <summary>
+        /// Returns if the avatar is colliding with an object
+        /// </summary>
+        public override bool CollidingObj
+        {
+            get { return m_iscollidingObj; }
+            set
+            {
+                m_iscollidingObj = value;
+                if (value && !m_avatarplanted)
+                    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;
+                        }
+
+                        m_taintPosition = value;                        
+                        _parent_scene.AddPhysicsActorTaint(this);
+                    }
+                    else
+                    {
+                        m_log.WarnFormat("[ODE CHARACTER]: Got a NaN Position from Scene on character {0}", Name);
+                    }
+                }
+            }
+        }
+
+        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
+            {
+                SetTaintedCapsuleLength(value);
+
+                    // If we reset velocity here, then an avatar stalls when it crosses a border for the first time
+                    // (as the height of the new root agent is set).
+//                    Velocity = Vector3.Zero;
+
+                _parent_scene.AddPhysicsActorTaint(this);
+            }
+        }
+
+        private void SetTaintedCapsuleLength(Vector3 size)
+        {
+            if (size.IsFinite())
+            {
+                m_pidControllerActive = true;
+
+                m_tainted_CAPSULE_LENGTH = size.Z - CAPSULE_RADIUS * 2.0f;
+
+                // m_log.InfoFormat("[ODE CHARACTER]: Size = {0}, Capsule Length = {1} (Capsule Radius = {2})",
+                //     size, m_tainted_CAPSULE_LENGTH, CAPSULE_RADIUS);
+            }
+            else
+            {
+                m_log.WarnFormat("[ODE CHARACTER]: Got a NaN Size for {0} in {1}", Name, _parent_scene.PhysicsSceneName);
+            }
+        }
+
+        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 = m_tiltBaseMovement;
+                    movementVector.Y = m_tiltBaseMovement;
+                }
+                else
+                {
+                    // southeast
+                    movementVector.X = m_tiltBaseMovement;
+                    movementVector.Y = -m_tiltBaseMovement;
+                }
+            }
+            else
+            {
+                // west
+                if (movementVector.Y > 0)
+                {
+                    // northwest
+                    movementVector.X = -m_tiltBaseMovement;
+                    movementVector.Y = m_tiltBaseMovement;
+                }
+                else
+                {
+                    // southwest
+                    movementVector.X = -m_tiltBaseMovement;
+                    movementVector.Y = -m_tiltBaseMovement;
+                }
+            }
+
+            // 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("[ODE CHARACTER]: 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>
+        /// 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 + " : " + bodyFArotation.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 param, 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 TargetVelocity
+        {
+            get
+            {
+                return m_taintTargetVelocity;
+            }
+
+            set
+            {
+                Velocity = value;
+            }
+        }
+
+
+        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;
+                    m_taintTargetVelocity = value;
+                    _parent_scene.AddPhysicsActorTaint(this);
+                }
+                else
+                {
+                    m_log.WarnFormat("[ODE CHARACTER]: Got a NaN velocity from Scene for {0}", Name);
+                }
+
+//                m_log.DebugFormat("[PHYSICS]: Set target velocity of {0}", m_taintTargetVelocity);
+            }
+        }
+
+        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; }
+            set { _acceleration = value; }
+        }
+
+        /// <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;
+                    m_taintForce += force;
+                    _parent_scene.AddPhysicsActorTaint(this);
+
+                    // If uncommented, things get pushed off world
+                    //
+                    // m_log.Debug("Push!");
+                    // m_taintTargetVelocity.X += force.X;
+                    // m_taintTargetVelocity.Y += force.Y;
+                    // m_taintTargetVelocity.Z += force.Z;
+                }
+                else
+                {
+                    m_pidControllerActive = true;
+                    m_taintTargetVelocity += force;
+                }
+            }
+            else
+            {
+                m_log.WarnFormat("[ODE CHARACTER]: Got a NaN force applied to {0}", Name);
+            }
+            //m_lastUpdateSent = false;
+        }
+
+        public override void AddAngularForce(Vector3 force, bool pushforce)
+        {
+        }
+
+        public override void SetMomentum(Vector3 momentum)
+        {
+        }
+
+        /// <summary>
+        /// Called from Simulate
+        /// This is the avatar's movement control + PID Controller
+        /// </summary>
+        /// <param name="defects">The character will be added to this list if there is something wrong (non-finite
+        /// position or velocity).
+        /// </param>
+        internal void Move(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.WarnFormat(
+                    "[ODE CHARACTER]: Avatar position of {0} for {1} is non-finite!  Removing from physics scene.",
+                    localPos, Name);
+
+                defects.Add(this);
+
+                return;
+            }
+
+            Vector3 vec = Vector3.Zero;
+            d.Vector3 vel = d.BodyGetLinearVel(Body);
+
+//            m_log.DebugFormat(
+//                "[ODE CHARACTER]: Current velocity in Move() is <{0},{1},{2}>, target {3} for {4}",
+//                vel.X, vel.Y, vel.Z, _target_velocity, Name);
+
+            float movementdivisor = 1f;
+
+            if (!m_alwaysRun)
+            {
+                movementdivisor = walkDivisor;
+            }
+            else
+            {
+                movementdivisor = runDivisor;
+            }
+
+            //  if velocity is zero, use position control; otherwise, velocity control
+            if (_target_velocity.X == 0.0f && _target_velocity.Y == 0.0f && _target_velocity.Z == 0.0f && m_iscolliding)
+            {
+                //  keep track of where we stopped.  No more slippin' & slidin'
+                if (!_zeroFlag)
+                {
+                    _zeroFlag = true;
+                    _zeroPosition = d.BodyGetPosition(Body);
+                }
+
+                if (m_pidControllerActive)
+                {
+                    // We only want to deactivate the PID Controller if we think we want to have our surrogate
+                    // react to the physics scene by moving it's position.
+                    // Avatar to Avatar collisions
+                    // Prim to avatar collisions
+
+                    d.Vector3 pos = d.BodyGetPosition(Body);
+                    vec.X = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2);
+                    vec.Y = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y)* (PID_P * 2);
+                    if (flying)
+                    {
+                        vec.Z = (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P;
+                    }
+                }
+                //PidStatus = true;
+            }
+            else
+            {
+                m_pidControllerActive = true;
+                _zeroFlag = false;
+                if (m_iscolliding && !flying)
+                {
+                    // We're standing on something
+                    vec.X = ((_target_velocity.X / movementdivisor) - vel.X) * (PID_D);
+                    vec.Y = ((_target_velocity.Y / movementdivisor) - vel.Y) * (PID_D);
+                }
+                else if (m_iscolliding && flying)
+                {
+                    // We're flying and colliding with something
+                    vec.X = ((_target_velocity.X / movementdivisor) - vel.X) * (PID_D / 16);
+                    vec.Y = ((_target_velocity.Y / movementdivisor) - vel.Y) * (PID_D / 16);
+                }
+                else if (!m_iscolliding && flying)
+                {
+                    // we're in mid air suspended
+                    vec.X = ((_target_velocity.X / movementdivisor) - vel.X) * (PID_D / 6);
+                    vec.Y = ((_target_velocity.Y / movementdivisor) - vel.Y) * (PID_D / 6);
+
+//                    m_log.DebugFormat(
+//                        "[ODE CHARACTER]: !m_iscolliding && flying, vec {0}, _target_velocity {1}, movementdivisor {2}, vel {3}",
+//                        vec, _target_velocity, movementdivisor, vel);
+                }
+
+                if (flying)
+                {
+                    // This also acts as anti-gravity so that we hover when flying rather than fall.
+                    vec.Z = (_target_velocity.Z - vel.Z) * (PID_D);
+                }
+                else
+                {
+                    if (m_iscolliding && _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;
+                        vec.X = ((_target_velocity.X - vel.X) / 1.2f) * PID_D;
+                        vec.Y = ((_target_velocity.Y - vel.Y) / 1.2f) * PID_D;
+                    }
+                    else if (!m_iscolliding)
+                    {
+                        // we're not colliding and we're not flying so that means we're falling!
+                        // m_iscolliding includes collisions with the ground.
+                        vec.X = ((_target_velocity.X - vel.X) / 1.2f) * PID_D;
+                        vec.Y = ((_target_velocity.Y - vel.Y) / 1.2f) * PID_D;
+                    }
+                }
+            }
+
+            if (flying)
+            {
+                // Anti-gravity so that we hover when flying rather than fall.
+                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())
+            {
+                // Apply the total force acting on this avatar
+                d.BodyAddForce(Body, vec.X, vec.Y, vec.Z);
+
+                if (!_zeroFlag)
+                    AlignAvatarTiltWithCurrentDirectionOfMovement(vec);
+            }
+            else
+            {
+                m_log.WarnFormat(
+                    "[ODE CHARACTER]: Got a NaN force vector {0} in Move() for {1}.  Removing character from physics scene.",
+                    vec, Name);
+
+                defects.Add(this);
+
+                return;
+            }
+
+            d.Vector3 newVel = d.BodyGetLinearVel(Body);
+            if (newVel.X >= 256 || newVel.X <= 256 || newVel.Y >= 256 || newVel.Y <= 256 || newVel.Z >= 256 || newVel.Z <= 256)
+            {
+//                m_log.DebugFormat(
+//                    "[ODE CHARACTER]: Limiting falling velocity from {0} to {1} for {2}", newVel.Z, -9.8, Name);
+
+                newVel.X = Util.Clamp<float>(newVel.X, -255f, 255f);
+                newVel.Y = Util.Clamp<float>(newVel.Y, -255f, 255f);
+
+                if (!flying)
+                    newVel.Z
+                        = Util.Clamp<float>(
+                            newVel.Z, -_parent_scene.AvatarTerminalVelocity, _parent_scene.AvatarTerminalVelocity);
+                else
+                    newVel.Z = Util.Clamp<float>(newVel.Z, -255f, 255f);
+
+                d.BodySetLinearVel(Body, newVel.X, newVel.Y, newVel.Z);
+            }
+        }
+
+        /// <summary>
+        /// Updates the reported position and velocity.  This essentially sends the data up to ScenePresence.
+        /// </summary>
+        /// <param name="defects">The character will be added to this list if there is something wrong (non-finite
+        /// position or velocity).
+        /// </param>
+        internal void UpdatePositionAndVelocity(List<OdeCharacter> defects)
+        {
+            //  no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit!
+            d.Vector3 newPos;
+            try
+            {
+                newPos = d.BodyGetPosition(Body);
+            }
+            catch (NullReferenceException)
+            {
+                bad = true;
+                defects.Add(this);
+                newPos = new d.Vector3(_position.X, _position.Y, _position.Z);
+                base.RaiseOutOfBounds(_position); // Tells ScenePresence that there's a problem!
+                m_log.WarnFormat("[ODE CHARACTER]: Avatar Null reference for Avatar {0}, physical actor {1}", Name, m_uuid);
+
+                return;
+            }
+
+            //  kluge to keep things in bounds.  ODE lets dead avatars drift away (they should be removed!)
+            if (newPos.X < 0.0f) newPos.X = 0.0f;
+            if (newPos.Y < 0.0f) newPos.Y = 0.0f;
+            if (newPos.X > (int)_parent_scene.WorldExtents.X - 0.05f) newPos.X = (int)_parent_scene.WorldExtents.X - 0.05f;
+            if (newPos.Y > (int)_parent_scene.WorldExtents.Y - 0.05f) newPos.Y = (int)_parent_scene.WorldExtents.Y - 0.05f;
+
+            _position.X = newPos.X;
+            _position.Y = newPos.Y;
+            _position.Z = newPos.Z;
+
+            // I think we need to update the taintPosition too -- Diva 12/24/10
+            m_taintPosition = _position;
+
+            // Did we move last? = zeroflag
+            // This helps keep us from sliding all over
+
+            if (_zeroFlag)
+            {
+                _velocity = Vector3.Zero;
+
+                // Did we send out the 'stopped' message?
+                if (!m_lastUpdateSent)
+                {
+                    m_lastUpdateSent = true;
+                    //base.RequestPhysicsterseUpdate();
+                }
+            }
+            else
+            {
+                m_lastUpdateSent = false;
+                d.Vector3 newVelocity;
+
+                try
+                {
+                    newVelocity = d.BodyGetLinearVel(Body);
+                }
+                catch (NullReferenceException)
+                {
+                    newVelocity.X = _velocity.X;
+                    newVelocity.Y = _velocity.Y;
+                    newVelocity.Z = _velocity.Z;
+                }
+
+                _velocity.X = newVelocity.X;
+                _velocity.Y = newVelocity.Y;
+                _velocity.Z = newVelocity.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>
+        /// This creates the Avatar's physical Surrogate in ODE at the position supplied
+        /// </summary>
+        /// <remarks>
+        /// 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.
+        /// </remarks>
+        /// <param name="npositionX"></param>
+        /// <param name="npositionY"></param>
+        /// <param name="npositionZ"></param>
+        /// <param name="tensor"></param>
+        private void CreateOdeStructures(float npositionX, float npositionY, float npositionZ, float tensor)
+        {
+            if (!(Shell == IntPtr.Zero && Body == IntPtr.Zero && Amotor == IntPtr.Zero))
+            {
+                m_log.ErrorFormat(
+                    "[ODE CHARACTER]: Creating ODE structures for {0} even though some already exist.  Shell = {1}, Body = {2}, Amotor = {3}",
+                    Name, Shell, Body, Amotor);
+            }
+
+            int dAMotorEuler = 1;
+//            _parent_scene.waitForSpaceUnlock(_parent_scene.space);
+            if (CAPSULE_LENGTH <= 0)
+            {
+                m_log.Warn("[ODE CHARACTER]: 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("[ODE CHARACTER]: The capsule size you specified in opensim.ini is invalid!  Setting it to the smallest possible size!");
+                CAPSULE_RADIUS = 0.01f;
+            }
+
+//          lock (OdeScene.UniversalColliderSyncObject)
+            Shell = d.CreateCapsule(_parent_scene.space, CAPSULE_RADIUS, CAPSULE_LENGTH);
+
+            d.GeomSetCategoryBits(Shell, (int)m_collisionCategories);
+            d.GeomSetCollideBits(Shell, (int)m_collisionFlags);
+
+            d.MassSetCapsuleTotal(out ShellMass, m_mass, 2, CAPSULE_RADIUS, CAPSULE_LENGTH);
+            Body = d.BodyCreate(_parent_scene.world);
+            d.BodySetPosition(Body, npositionX, npositionY, npositionZ);
+
+            _position.X = npositionX;
+            _position.Y = npositionY;
+            _position.Z = npositionZ;
+
+            m_taintPosition = _position;
+
+            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();
+
+            _parent_scene.geom_name_map[Shell] = Name;
+            _parent_scene.actor_name_map[Shell] = this;
+        }
+
+        /// <summary>
+        /// Cleanup the things we use in the scene.
+        /// </summary>
+        internal void Destroy()
+        {
+            m_tainted_isPhysical = false;
+            _parent_scene.AddPhysicsActorTaint(this);
+        }
+
+        /// <summary>
+        /// Used internally to destroy the ODE structures associated with this character.
+        /// </summary>
+        internal void DestroyOdeStructures()
+        {
+            // Create avatar capsule and related ODE data
+            if (Shell == IntPtr.Zero || Body == IntPtr.Zero || Amotor == IntPtr.Zero)
+            {
+                m_log.ErrorFormat(
+                    "[ODE CHARACTER]: Destroying ODE structures for {0} even though some are already null.  Shell = {1}, Body = {2}, Amotor = {3}",
+                    Name, Shell, Body, Amotor);
+            }
+
+            // 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)
+            {
+//              lock (OdeScene.UniversalColliderSyncObject)
+                d.GeomDestroy(Shell);
+
+                _parent_scene.geom_name_map.Remove(Shell);
+                _parent_scene.actor_name_map.Remove(Shell);
+
+                Shell = IntPtr.Zero;
+            }
+        }
+
+        public override void CrossingFailure()
+        {
+        }
+
+        public override Vector3 PIDTarget { set { return; } }
+        public override bool PIDActive 
+        { 
+            get { return false; }
+            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;
+
+            // Don't clear collision event reporting here.  This is called directly from scene code and so can lead
+            // to a race condition with the simulate loop
+
+            _parent_scene.AddCollisionEventReporting(this);
+        }
+
+        public override void UnSubscribeEvents()
+        {
+            _parent_scene.RemoveCollisionEventReporting(this);
+
+            // Don't clear collision event reporting here.  This is called directly from scene code and so can lead
+            // to a race condition with the simulate loop
+
+            m_requestedUpdateFrequency = 0;
+            m_eventsubscription = 0;
+        }
+
+        internal void AddCollisionEvent(uint CollidedWith, ContactPoint contact)
+        {
+            if (m_eventsubscription > 0)
+            {
+//                m_log.DebugFormat(
+//                    "[PHYSICS]: Adding collision event for {0}, collidedWith {1}, contact {2}", "", CollidedWith, contact);
+
+                CollisionEventsThisFrame.AddCollider(CollidedWith, contact);
+            }
+        }
+
+        internal void SendCollisions()
+        {
+            if (m_eventsubscription > m_requestedUpdateFrequency)
+            {
+                base.SendCollisionUpdate(CollisionEventsThisFrame);
+
+                CollisionEventsThisFrame.Clear();
+                m_eventsubscription = 0;
+            }
+        }
+
+        public override bool SubscribedEvents()
+        {
+            if (m_eventsubscription > 0)
+                return true;
+            return false;
+        }
+
+        internal void ProcessTaints()
+        {
+            if (m_taintPosition != _position)
+            {
+                if (Body != IntPtr.Zero)
+                {
+                    d.BodySetPosition(Body, m_taintPosition.X, m_taintPosition.Y, m_taintPosition.Z);
+                    _position = m_taintPosition;
+                }
+            }
+
+            if (m_taintForce != Vector3.Zero)
+            {
+                if (Body != IntPtr.Zero)
+                {
+                    // FIXME: This is not a good solution since it's subject to a race condition if a force is another
+                    // thread sets a new force while we're in this loop (since it could be obliterated by
+                    // m_taintForce = Vector3.Zero.  Need to lock ProcessTaints() when we set a new tainted force.
+                    d.BodyAddForce(Body, m_taintForce.X, m_taintForce.Y, m_taintForce.Z);
+                }
+
+                m_taintForce = Vector3.Zero;
+            }
+
+            if (m_taintTargetVelocity != _target_velocity)
+                _target_velocity = m_taintTargetVelocity;
+
+            if (m_tainted_isPhysical != m_isPhysical)
+            {
+                if (m_tainted_isPhysical)
+                {
+                    CreateOdeStructures(_position.X, _position.Y, _position.Z, m_tensor);
+                    _parent_scene.AddCharacter(this);
+                }
+                else
+                {
+                    _parent_scene.RemoveCharacter(this);
+                    DestroyOdeStructures();
+                }
+
+                m_isPhysical = m_tainted_isPhysical;
+            }
+
+            if (m_tainted_CAPSULE_LENGTH != CAPSULE_LENGTH)
+            {
+                if (Shell != IntPtr.Zero && Body != IntPtr.Zero && Amotor != IntPtr.Zero)
+                {
+//                    m_log.DebugFormat(
+//                        "[ODE CHARACTER]: Changing capsule size from {0} to {1} for {2}",
+//                        CAPSULE_LENGTH, m_tainted_CAPSULE_LENGTH, Name);
+                    
+                    m_pidControllerActive = true;
+
+                    // no lock needed on _parent_scene.OdeLock because we are called from within the thread lock in OdePlugin's simulate()
+                    DestroyOdeStructures();
+
+                    float prevCapsule = CAPSULE_LENGTH;
+                    CAPSULE_LENGTH = m_tainted_CAPSULE_LENGTH;
+
+                    CreateOdeStructures(
+                        _position.X,
+                        _position.Y,
+                        _position.Z + (Math.Abs(CAPSULE_LENGTH - prevCapsule) * 2), m_tensor);
+
+                    // As with Size, we reset velocity.  However, this isn't strictly necessary since it doesn't
+                    // appear to stall initial region crossings when done here.  Being done for consistency.
+//                    Velocity = Vector3.Zero;
+                }
+                else
+                {
+                    m_log.Warn("[ODE CHARACTER]: trying to change capsule size for " + Name + ", but the following ODE data is missing - "
+                        + (Shell==IntPtr.Zero ? "Shell ":"")
+                        + (Body==IntPtr.Zero ? "Body ":"")
+                        + (Amotor==IntPtr.Zero ? "Amotor ":""));
+                }
+            }
+        }
+
+        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/PhysicsModules/Ode/ODEDynamics.c_comments b/OpenSim/Region/PhysicsModules/Ode/ODEDynamics.c_comments
new file mode 100644
index 0000000..1060aa6
--- /dev/null
+++ b/OpenSim/Region/PhysicsModules/Ode/ODEDynamics.c_comments
@@ -0,0 +1,630 @@
+/*
+ * Revised August 26 2009 by Kitto Flora. ODEDynamics.cs replaces
+ * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised:
+ * ODEPrim.cs contains methods dealing with Prim editing, Prim
+ * characteristics and Kinetic motion.
+ * ODEDynamics.cs contains methods dealing with Prim Physical motion
+ * (dynamics) and the associated settings. Old Linear and angular
+ * motors for dynamic motion have been replace with  MoveLinear()
+ * and MoveAngular(); 'Physical' is used only to switch ODE dynamic 
+ * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_<other> is to
+ * switch between 'VEHICLE' parameter use and general dynamics
+ * settings use.
+ * 
+ * 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 log4net;
+using OpenMetaverse;
+using Ode.NET;
+using OpenSim.Framework;
+using OpenSim.Region.Physics.Manager;
+
+namespace OpenSim.Region.Physics.OdePlugin
+{
+    public class ODEDynamics
+    {
+        public Vehicle Type
+        {        
+            get { return m_type; }
+        }
+
+        public IntPtr Body
+        {
+            get { return m_body; }
+        }
+
+        private int frcount = 0;                                                                                // Used to limit dynamics debug output to 
+                                                                                                                                        // every 100th frame
+
+        // private OdeScene m_parentScene = null;
+        private IntPtr m_body = IntPtr.Zero;
+        private IntPtr m_jointGroup = IntPtr.Zero;
+        private IntPtr m_aMotor = 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;                                  // Boolean 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;                  // velocity requested by LSL, decayed by time
+        private Vector3 m_linearMotorDirectionLASTSET = Vector3.Zero;   // velocity requested by LSL
+        private Vector3 m_dir = Vector3.Zero;                                                   // velocity applied to body
+        private Vector3 m_linearFrictionTimescale = Vector3.Zero;
+        private float m_linearMotorDecayTimescale = 0;
+        private float m_linearMotorTimescale = 0;
+        private Vector3 m_lastLinearVelocityVector = Vector3.Zero;
+                // private bool m_LinearMotorSetLastFrame = false;
+        // private Vector3 m_linearMotorOffset = Vector3.Zero;
+        
+        //Angular properties
+        private Vector3 m_angularMotorDirection = Vector3.Zero;
+        private Vector3 m_angularMotorDirectionLASTSET = Vector3.Zero;
+        private Vector3 m_angularFrictionTimescale = Vector3.Zero;
+        private float m_angularMotorDecayTimescale = 0;
+        private float m_angularMotorTimescale = 0;
+        private Vector3 m_lastAngularVelocityVector = Vector3.Zero;
+
+                //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;                   //KF: m_VehicleBuoyancy is 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 = 0;
+        private float m_verticalAttractionTimescale = 0;
+        
+        
+
+
+
+        internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue)
+        {
+            switch (pParam)
+            {
+                case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY:
+                    if (pValue < 0.01f) pValue = 0.01f;
+                    // m_angularDeflectionEfficiency = pValue;
+                    break;
+                case Vehicle.ANGULAR_DEFLECTION_TIMESCALE:
+                    if (pValue < 0.01f) pValue = 0.01f;
+                    // m_angularDeflectionTimescale = pValue;
+                    break;
+                case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE:
+                    if (pValue < 0.01f) pValue = 0.01f;
+                    m_angularMotorDecayTimescale = pValue;
+                    break;
+                case Vehicle.ANGULAR_MOTOR_TIMESCALE:
+                    if (pValue < 0.01f) pValue = 0.01f;
+                    m_angularMotorTimescale = pValue;
+                    break;
+                case Vehicle.BANKING_EFFICIENCY:
+                    if (pValue < 0.01f) pValue = 0.01f;
+                    // m_bankingEfficiency = pValue;
+                    break;
+                case Vehicle.BANKING_MIX:
+                    if (pValue < 0.01f) pValue = 0.01f;
+                    // m_bankingMix = pValue;
+                    break;
+                case Vehicle.BANKING_TIMESCALE:
+                    if (pValue < 0.01f) pValue = 0.01f;
+                    // m_bankingTimescale = pValue;
+                    break;
+                case Vehicle.BUOYANCY:
+                        if (pValue < -1f) pValue = -1f;
+                        if (pValue > 1f) pValue = 1f;
+                    m_VehicleBuoyancy = pValue;
+                    break;
+                case Vehicle.HOVER_EFFICIENCY:
+                        if (pValue < 0f) pValue = 0f;
+                        if (pValue > 1f) pValue = 1f;
+                    m_VhoverEfficiency = pValue;
+                    break;
+                case Vehicle.HOVER_HEIGHT:
+                    m_VhoverHeight = pValue;
+                    break;
+                case Vehicle.HOVER_TIMESCALE:
+                    if (pValue < 0.01f) pValue = 0.01f;
+                    m_VhoverTimescale = pValue;
+                    break;
+                case Vehicle.LINEAR_DEFLECTION_EFFICIENCY:
+                    if (pValue < 0.01f) pValue = 0.01f;
+                    // m_linearDeflectionEfficiency = pValue;
+                    break;
+                case Vehicle.LINEAR_DEFLECTION_TIMESCALE:
+                    if (pValue < 0.01f) pValue = 0.01f;
+                    // m_linearDeflectionTimescale = pValue;
+                    break;
+                case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE:
+                    if (pValue < 0.01f) pValue = 0.01f;
+                    m_linearMotorDecayTimescale = pValue;
+                    break;
+                case Vehicle.LINEAR_MOTOR_TIMESCALE:
+                    if (pValue < 0.01f) pValue = 0.01f;
+                    m_linearMotorTimescale = pValue;
+                    break;
+                case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY:
+                    if (pValue < 0.0f) pValue = 0.0f;
+                    if (pValue > 1.0f) pValue = 1.0f;
+                    m_verticalAttractionEfficiency = pValue;
+                    break;
+                case Vehicle.VERTICAL_ATTRACTION_TIMESCALE:
+                    if (pValue < 0.01f) pValue = 0.01f;
+                    m_verticalAttractionTimescale = pValue;
+                    break;
+                    
+                // These are vector properties but the engine lets you use a single float value to 
+                // set all of the components to the same value
+                case Vehicle.ANGULAR_FRICTION_TIMESCALE:
+                    m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue);
+                    break;
+                case Vehicle.ANGULAR_MOTOR_DIRECTION:
+                    m_angularMotorDirection = new Vector3(pValue, pValue, pValue);
+                    m_angularMotorDirectionLASTSET = new Vector3(pValue, pValue, pValue);
+                    break;
+                case Vehicle.LINEAR_FRICTION_TIMESCALE:
+                    m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue);
+                    break;
+                case Vehicle.LINEAR_MOTOR_DIRECTION:
+                    m_linearMotorDirection = new Vector3(pValue, pValue, pValue);
+                    m_linearMotorDirectionLASTSET = new Vector3(pValue, pValue, pValue);
+                    break;
+                case Vehicle.LINEAR_MOTOR_OFFSET:
+                    // m_linearMotorOffset = new Vector3(pValue, pValue, pValue);
+                    break;
+
+            }
+            
+        }//end ProcessFloatVehicleParam
+
+        internal void ProcessVectorVehicleParam(Vehicle pParam, PhysicsVector pValue)
+        {
+            switch (pParam)
+            {
+                case Vehicle.ANGULAR_FRICTION_TIMESCALE:
+                    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);
+                    m_angularMotorDirectionLASTSET = new Vector3(pValue.X, pValue.Y, pValue.Z);
+                    break;
+                case Vehicle.LINEAR_FRICTION_TIMESCALE:
+                    m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
+                    break;
+                case Vehicle.LINEAR_MOTOR_DIRECTION:
+                    m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
+                    m_linearMotorDirectionLASTSET = new Vector3(pValue.X, pValue.Y, pValue.Z);
+                    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 ProcessTypeChange(Vehicle pType)
+        {
+Console.WriteLine("ProcessTypeChange to " + 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(1000, 1000, 1000);
+                    m_linearMotorDirection = Vector3.Zero;
+                    m_linearMotorTimescale = 1000;
+                    m_linearMotorDecayTimescale = 120;
+                    m_angularMotorDirection = 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(1000, 1000, 1000);
+                    m_linearMotorDirection = Vector3.Zero;
+                    m_linearMotorTimescale = 1;
+                    m_linearMotorDecayTimescale = 60;
+                    m_angularMotorDirection = 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 = 1;
+                    m_verticalAttractionTimescale = 10;
+                    // 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_linearMotorDirection = Vector3.Zero;
+                    m_linearMotorTimescale = 5;
+                    m_linearMotorDecayTimescale = 60;
+                    m_angularMotorDirection = 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 = 5;
+                    // 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_linearMotorDirection = Vector3.Zero;
+                    m_linearMotorTimescale = 2;
+                    m_linearMotorDecayTimescale = 60;
+                    m_angularMotorDirection = 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 = 2;
+                    // 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_linearMotorDirection = Vector3.Zero;
+                    m_linearMotorTimescale = 5;
+                    m_linearMotorDecayTimescale = 60;
+                    m_angularMotorDirection = 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 = 1;
+                    m_verticalAttractionTimescale = 1000;
+                    // 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)
+        {
+//Console.WriteLine("Enable  m_type=" + m_type + "   m_VehicleBuoyancy=" + m_VehicleBuoyancy);
+            if (m_type == Vehicle.TYPE_NONE)
+                return;
+
+            m_body = pBody;
+            //KF: This  used to set up the linear and angular joints
+        }
+
+        internal void Step(float pTimestep,  OdeScene pParentScene)
+        {
+            if (m_body == IntPtr.Zero || m_type == Vehicle.TYPE_NONE)
+                return;
+            frcount++;                                  // used to limit debug comment output
+            if (frcount > 100)
+                frcount = 0;
+
+                        MoveLinear(pTimestep, pParentScene);
+            MoveAngular(pTimestep);
+        }// end Step
+
+        private void MoveLinear(float pTimestep, OdeScene _pParentScene)
+        {
+            if (!m_linearMotorDirection.ApproxEquals(Vector3.Zero, 0.01f))              // requested m_linearMotorDirection is significant
+            {
+                 if(!d.BodyIsEnabled (Body))  d.BodyEnable (Body);
+
+                // add drive to body
+                Vector3 addAmount = m_linearMotorDirection/(m_linearMotorTimescale/pTimestep);
+                m_lastLinearVelocityVector += (addAmount*10);                                   // lastLinearVelocityVector is the current body velocity vector?
+        
+                // This will work temporarily, but we really need to compare speed on an axis
+                // KF: Limit body velocity to applied velocity?
+                if (Math.Abs(m_lastLinearVelocityVector.X) > Math.Abs(m_linearMotorDirectionLASTSET.X))
+                    m_lastLinearVelocityVector.X = m_linearMotorDirectionLASTSET.X;
+                if (Math.Abs(m_lastLinearVelocityVector.Y) > Math.Abs(m_linearMotorDirectionLASTSET.Y))
+                    m_lastLinearVelocityVector.Y = m_linearMotorDirectionLASTSET.Y;
+                if (Math.Abs(m_lastLinearVelocityVector.Z) > Math.Abs(m_linearMotorDirectionLASTSET.Z))
+                    m_lastLinearVelocityVector.Z = m_linearMotorDirectionLASTSET.Z;
+                
+                // decay applied velocity
+                Vector3 decayfraction = ((Vector3.One/(m_linearMotorDecayTimescale/pTimestep)));
+                //Console.WriteLine("decay: " + decayfraction);
+                m_linearMotorDirection -= m_linearMotorDirection * decayfraction;
+                //Console.WriteLine("actual: " + m_linearMotorDirection);
+            }
+            else
+            {           // requested is not significant
+                                        // if what remains of applied is small, zero it.
+                                if (m_lastLinearVelocityVector.ApproxEquals(Vector3.Zero, 0.01f))
+                                        m_lastLinearVelocityVector = Vector3.Zero;
+                        }
+                                    
+                        
+                        // convert requested object velocity to world-referenced vector
+            m_dir = m_lastLinearVelocityVector;
+            d.Quaternion rot = d.BodyGetQuaternion(Body);
+                Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);   // rotq = rotation of object
+                m_dir *= rotq;                                                  // apply obj rotation to velocity vector
+
+                        // add Gravity andBuoyancy
+                        // KF: So far I have found no good method to combine a script-requested
+                        // .Z velocity and gravity. Therefore only 0g will used script-requested
+                        // .Z velocity. >0g (m_VehicleBuoyancy < 1) will used modified gravity only.
+            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 = _pParentScene.gravityz * objMass.mass * (1f - m_VehicleBuoyancy);
+                    // Preserve the current Z velocity
+                                d.Vector3 vel_now = d.BodyGetLinearVel(Body);
+                    m_dir.Z = vel_now.Z;                // Preserve the accumulated falling velocity
+                } // else its 1.0, no gravity.
+                
+                // Check if 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 = _pParentScene.GetWaterLevel() + m_VhoverHeight;
+                        }
+                        else if((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) == VehicleFlag.HOVER_TERRAIN_ONLY)
+                        {
+                                m_VhoverTargetHeight = _pParentScene.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
+//                              pTimestep  is time since last frame,in secs 
+                                float herr0 = pos.Z - m_VhoverTargetHeight;
+//if(frcount == 0) Console.WriteLine("herr0=" + herr0);                 
+                                // Replace Vertical speed with correction figure if significant
+                                if(Math.Abs(herr0) > 0.01f )
+                                {
+                            d.Mass objMass;
+                            d.BodyGetMass(Body, out objMass);
+                                        m_dir.Z = - ( (herr0 * pTimestep * 50.0f) / m_VhoverTimescale);
+                                        // m_VhoverEfficiency is not yet implemented
+                                }
+                                else
+                                {
+                                        m_dir.Z = 0f;
+                                }
+                        }
+                        
+                // Apply velocity
+                d.BodySetLinearVel(Body, m_dir.X, m_dir.Y, m_dir.Z);        
+//if(frcount == 0) Console.WriteLine("Move  " + Body + ":"+ m_dir.X + " " + m_dir.Y + " " + m_dir.Z);
+            // apply gravity force
+                        d.BodyAddForce(Body, grav.X, grav.Y, grav.Z);           
+//if(frcount == 0) Console.WriteLine("Force " + Body + ":" + grav.X + " " + grav.Y + " " + grav.Z);     
+
+
+                        // apply friction
+            Vector3 decayamount = Vector3.One / (m_linearFrictionTimescale / pTimestep);
+            m_lastLinearVelocityVector -= m_lastLinearVelocityVector * decayamount;
+        } // end MoveLinear()
+        
+        private void MoveAngular(float pTimestep)
+        {
+        
+                // m_angularMotorDirection                      is the latest value from the script, and is decayed here
+                // m_angularMotorDirectionLASTSET               is the latest value from the script
+                // m_lastAngularVelocityVector                  is what is being applied to the Body, varied up and down here
+                
+            if (!m_angularMotorDirection.ApproxEquals(Vector3.Zero, 0.01f))
+            {
+                                if(!d.BodyIsEnabled (Body))  d.BodyEnable (Body);
+                                // ramp up to new value
+                Vector3 addAmount = m_angularMotorDirection / (m_angularMotorTimescale / pTimestep);
+                m_lastAngularVelocityVector += (addAmount * 10f);
+//if(frcount == 0) Console.WriteLine("add: " + addAmount);
+
+                                // limit applied value to what was set by script
+                // This will work temporarily, but we really need to compare speed on an axis
+                if (Math.Abs(m_lastAngularVelocityVector.X) > Math.Abs(m_angularMotorDirectionLASTSET.X))
+                    m_lastAngularVelocityVector.X = m_angularMotorDirectionLASTSET.X;
+                if (Math.Abs(m_lastAngularVelocityVector.Y) > Math.Abs(m_angularMotorDirectionLASTSET.Y))
+                    m_lastAngularVelocityVector.Y = m_angularMotorDirectionLASTSET.Y;
+                if (Math.Abs(m_lastAngularVelocityVector.Z) > Math.Abs(m_angularMotorDirectionLASTSET.Z))
+                    m_lastAngularVelocityVector.Z = m_angularMotorDirectionLASTSET.Z;
+
+                                // decay the requested value
+                Vector3 decayfraction = ((Vector3.One / (m_angularMotorDecayTimescale / pTimestep)));
+                //Console.WriteLine("decay: " + decayfraction);
+                m_angularMotorDirection -= m_angularMotorDirection * decayfraction;
+                //Console.WriteLine("actual: " + m_linearMotorDirection);
+            }
+            // KF: m_lastAngularVelocityVector is rotational speed in rad/sec ?
+            
+            // Vertical attractor section
+            
+//            d.Mass objMass;
+//            d.BodyGetMass(Body, out objMass);
+//            float servo = 100f * objMass.mass * m_verticalAttractionEfficiency / (m_verticalAttractionTimescale * pTimestep);
+            float servo = 0.1f * m_verticalAttractionEfficiency / (m_verticalAttractionTimescale * pTimestep);
+            // get present body rotation
+            d.Quaternion rot = d.BodyGetQuaternion(Body);
+            Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);
+            // 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)
+                        {
+                                verterr.X = 2.0f - verterr.X;
+                                verterr.Y = 2.0f - verterr.Y;
+                        }
+                        // Error is 0 (no error) to +/- 2 (max error)
+                        // scale it by servo
+                        verterr = verterr * servo;
+                        
+                        // rotate to object frame
+        //              verterr = verterr * rotq;
+                        
+                        // 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.
+                        m_lastAngularVelocityVector.X += verterr.Y;
+                        m_lastAngularVelocityVector.Y -= verterr.X;
+/*
+if(frcount == 0) 
+        {
+//                      Console.WriteLine("AngleMotor " + m_lastAngularVelocityVector);
+                        Console.WriteLine(String.Format("VA Body:{0} servo:{1} err:<{2},{3},{4}>  VAE:{5}", 
+                                        Body, servo, verterr.X, verterr.Y, verterr.Z, m_verticalAttractionEfficiency)); 
+        }
+        */
+                        d.BodySetAngularVel (Body, m_lastAngularVelocityVector.X, m_lastAngularVelocityVector.Y, m_lastAngularVelocityVector.Z);
+                        // apply friction
+            Vector3 decayamount = Vector3.One / (m_angularFrictionTimescale / pTimestep);
+            m_lastAngularVelocityVector -= m_lastAngularVelocityVector * decayamount;
+
+        } //end MoveAngular
+    }
+}
diff --git a/OpenSim/Region/PhysicsModules/Ode/ODEDynamics.cs b/OpenSim/Region/PhysicsModules/Ode/ODEDynamics.cs
new file mode 100644
index 0000000..8f8e2bd
--- /dev/null
+++ b/OpenSim/Region/PhysicsModules/Ode/ODEDynamics.cs
@@ -0,0 +1,974 @@
+/*
+ * 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 Aug, Sept 2009 by Kitto Flora. ODEDynamics.cs replaces
+ * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised:
+ * ODEPrim.cs contains methods dealing with Prim editing, Prim
+ * characteristics and Kinetic motion.
+ * ODEDynamics.cs contains methods dealing with Prim Physical motion
+ * (dynamics) and the associated settings. Old Linear and angular
+ * motors for dynamic motion have been replace with  MoveLinear()
+ * and MoveAngular(); 'Physical' is used only to switch ODE dynamic
+ * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_<other> is to
+ * switch between 'VEHICLE' parameter use and general dynamics
+ * settings use.
+ */
+
+using System;
+using System.Collections.Generic;
+using System.Reflection;
+using System.Runtime.InteropServices;
+using log4net;
+using OpenMetaverse;
+using Ode.NET;
+using OpenSim.Framework;
+using OpenSim.Region.PhysicsModules.SharedBase;
+
+namespace OpenSim.Region.PhysicsModule.ODE
+{
+    public class ODEDynamics
+    {
+        public Vehicle Type
+        {
+            get { return m_type; }
+        }
+
+        public IntPtr Body
+        {
+            get { return m_body; }
+        }
+
+        private int frcount = 0;                                        // Used to limit dynamics debug output to
+                                                                        // every 100th frame
+
+        // private OdeScene m_parentScene = null;
+        private IntPtr m_body = IntPtr.Zero;
+//        private IntPtr m_jointGroup = IntPtr.Zero;
+//        private IntPtr m_aMotor = 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;                  // Boolean settings:
+                                                                        // HOVER_TERRAIN_ONLY
+                                                                        // HOVER_GLOBAL_HEIGHT
+                                                                        // NO_DEFLECTION_UP
+                                                                        // HOVER_WATER_ONLY
+                                                                        // HOVER_UP_ONLY
+                                                                        // LIMIT_MOTOR_UP
+                                                                        // LIMIT_ROLL_ONLY
+        private VehicleFlag m_Hoverflags = (VehicleFlag)0;
+        private Vector3 m_BlockingEndPoint = Vector3.Zero;
+        private Quaternion m_RollreferenceFrame = Quaternion.Identity;
+        // Linear properties
+        private Vector3 m_linearMotorDirection = Vector3.Zero;          // velocity requested by LSL, decayed by time
+        private Vector3 m_linearMotorDirectionLASTSET = Vector3.Zero;   // velocity requested by LSL
+        private Vector3 m_dir = Vector3.Zero;                           // velocity applied to body
+        private Vector3 m_linearFrictionTimescale = Vector3.Zero;
+        private float m_linearMotorDecayTimescale = 0;
+        private float m_linearMotorTimescale = 0;
+        private Vector3 m_lastLinearVelocityVector = Vector3.Zero;
+        private d.Vector3 m_lastPositionVector = new d.Vector3();
+        // private bool m_LinearMotorSetLastFrame = false;
+        // private Vector3 m_linearMotorOffset = Vector3.Zero;
+
+        //Angular properties
+        private Vector3 m_angularMotorDirection = Vector3.Zero;         // angular velocity requested by LSL motor
+        private int m_angularMotorApply = 0;                            // application frame counter
+        private Vector3 m_angularMotorVelocity = Vector3.Zero;          // current angular motor velocity
+        private float m_angularMotorTimescale = 0;                      // motor angular velocity ramp up rate
+        private float m_angularMotorDecayTimescale = 0;                 // motor angular velocity decay rate
+        private Vector3 m_angularFrictionTimescale = Vector3.Zero;      // body angular velocity  decay rate
+        private Vector3 m_lastAngularVelocity = Vector3.Zero;           // what was last applied to body
+ //       private Vector3 m_lastVertAttractor = Vector3.Zero;             // what VA 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;           //KF: m_VehicleBuoyancy is 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.
+
+        internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue)
+        {
+            switch (pParam)
+            {
+                case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY:
+                    if (pValue < 0.01f) pValue = 0.01f;
+                    // m_angularDeflectionEfficiency = pValue;
+                    break;
+                case Vehicle.ANGULAR_DEFLECTION_TIMESCALE:
+                    if (pValue < 0.01f) pValue = 0.01f;
+                    // m_angularDeflectionTimescale = pValue;
+                    break;
+                case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE:
+                    if (pValue < 0.01f) pValue = 0.01f;
+                    m_angularMotorDecayTimescale = pValue;
+                    break;
+                case Vehicle.ANGULAR_MOTOR_TIMESCALE:
+                    if (pValue < 0.01f) pValue = 0.01f;
+                    m_angularMotorTimescale = pValue;
+                    break;
+                case Vehicle.BANKING_EFFICIENCY:
+                    if (pValue < 0.01f) pValue = 0.01f;
+                    // m_bankingEfficiency = pValue;
+                    break;
+                case Vehicle.BANKING_MIX:
+                    if (pValue < 0.01f) pValue = 0.01f;
+                    // m_bankingMix = pValue;
+                    break;
+                case Vehicle.BANKING_TIMESCALE:
+                    if (pValue < 0.01f) pValue = 0.01f;
+                    // m_bankingTimescale = pValue;
+                    break;
+                case Vehicle.BUOYANCY:
+                    if (pValue < -1f) pValue = -1f;
+                    if (pValue > 1f) pValue = 1f;
+                    m_VehicleBuoyancy = pValue;
+                    break;
+//                case Vehicle.HOVER_EFFICIENCY:
+//                    if (pValue < 0f) pValue = 0f;
+//                    if (pValue > 1f) pValue = 1f;
+//                    m_VhoverEfficiency = pValue;
+//                    break;
+                case Vehicle.HOVER_HEIGHT:
+                    m_VhoverHeight = pValue;
+                    break;
+                case Vehicle.HOVER_TIMESCALE:
+                    if (pValue < 0.01f) pValue = 0.01f;
+                    m_VhoverTimescale = pValue;
+                    break;
+                case Vehicle.LINEAR_DEFLECTION_EFFICIENCY:
+                    if (pValue < 0.01f) pValue = 0.01f;
+                    // m_linearDeflectionEfficiency = pValue;
+                    break;
+                case Vehicle.LINEAR_DEFLECTION_TIMESCALE:
+                    if (pValue < 0.01f) pValue = 0.01f;
+                    // m_linearDeflectionTimescale = pValue;
+                    break;
+                case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE:
+                    if (pValue < 0.01f) pValue = 0.01f;
+                    m_linearMotorDecayTimescale = pValue;
+                    break;
+                case Vehicle.LINEAR_MOTOR_TIMESCALE:
+                    if (pValue < 0.01f) pValue = 0.01f;
+                    m_linearMotorTimescale = pValue;
+                    break;
+                case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY:
+                    if (pValue < 0.1f) pValue = 0.1f;    // Less goes unstable
+                    if (pValue > 1.0f) pValue = 1.0f;
+                    m_verticalAttractionEfficiency = pValue;
+                    break;
+                case Vehicle.VERTICAL_ATTRACTION_TIMESCALE:
+                    if (pValue < 0.01f) pValue = 0.01f;
+                    m_verticalAttractionTimescale = pValue;
+                    break;
+
+                // These are vector properties but the engine lets you use a single float value to
+                // set all of the components to the same value
+                case Vehicle.ANGULAR_FRICTION_TIMESCALE:
+                    m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue);
+                    break;
+                case Vehicle.ANGULAR_MOTOR_DIRECTION:
+                    m_angularMotorDirection = new Vector3(pValue, pValue, pValue);
+                    m_angularMotorApply = 10;
+                    break;
+                case Vehicle.LINEAR_FRICTION_TIMESCALE:
+                    m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue);
+                    break;
+                case Vehicle.LINEAR_MOTOR_DIRECTION:
+                    m_linearMotorDirection = new Vector3(pValue, pValue, pValue);
+                    m_linearMotorDirectionLASTSET = new Vector3(pValue, pValue, pValue);
+                    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:
+                    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;
+                    m_angularMotorApply = 10;
+                    break;
+                case Vehicle.LINEAR_FRICTION_TIMESCALE:
+                    m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
+                    break;
+                case Vehicle.LINEAR_MOTOR_DIRECTION:
+                    m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
+                    m_linearMotorDirectionLASTSET = new Vector3(pValue.X, pValue.Y, pValue.Z);
+                    break;
+                case Vehicle.LINEAR_MOTOR_OFFSET:
+                    // m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z);
+                    break;
+                case Vehicle.BLOCK_EXIT:
+                    m_BlockingEndPoint = 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;
+                case Vehicle.ROLL_FRAME:
+                    m_RollreferenceFrame = pValue;
+                    break;
+            }
+        }//end ProcessRotationVehicleParam
+
+        internal void ProcessVehicleFlags(int pParam, bool remove)
+        {
+            if (remove)
+            {
+                if (pParam == -1)
+                {
+                    m_flags = (VehicleFlag)0;
+                    m_Hoverflags = (VehicleFlag)0;
+                    return;
+                }
+                if ((pParam & (int)VehicleFlag.HOVER_GLOBAL_HEIGHT) == (int)VehicleFlag.HOVER_GLOBAL_HEIGHT)
+                {
+                    if ((m_Hoverflags & VehicleFlag.HOVER_GLOBAL_HEIGHT) != (VehicleFlag)0)
+                        m_Hoverflags &= ~(VehicleFlag.HOVER_GLOBAL_HEIGHT);
+                }
+                if ((pParam & (int)VehicleFlag.HOVER_TERRAIN_ONLY) == (int)VehicleFlag.HOVER_TERRAIN_ONLY)
+                {
+                    if ((m_Hoverflags & VehicleFlag.HOVER_TERRAIN_ONLY) != (VehicleFlag)0)
+                        m_Hoverflags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY);
+                }
+                if ((pParam & (int)VehicleFlag.HOVER_UP_ONLY) == (int)VehicleFlag.HOVER_UP_ONLY)
+                {
+                    if ((m_Hoverflags & VehicleFlag.HOVER_UP_ONLY) != (VehicleFlag)0)
+                        m_Hoverflags &= ~(VehicleFlag.HOVER_UP_ONLY);
+                }
+                if ((pParam & (int)VehicleFlag.HOVER_WATER_ONLY) == (int)VehicleFlag.HOVER_WATER_ONLY)
+                {
+                    if ((m_Hoverflags & VehicleFlag.HOVER_WATER_ONLY) != (VehicleFlag)0)
+                        m_Hoverflags &= ~(VehicleFlag.HOVER_WATER_ONLY);
+                }
+                if ((pParam & (int)VehicleFlag.LIMIT_MOTOR_UP) == (int)VehicleFlag.LIMIT_MOTOR_UP)
+                {
+                    if ((m_flags & VehicleFlag.LIMIT_MOTOR_UP) != (VehicleFlag)0)
+                        m_flags &= ~(VehicleFlag.LIMIT_MOTOR_UP);
+                }
+                if ((pParam & (int)VehicleFlag.LIMIT_ROLL_ONLY) == (int)VehicleFlag.LIMIT_ROLL_ONLY)
+                {
+                    if ((m_flags & VehicleFlag.LIMIT_ROLL_ONLY) != (VehicleFlag)0)
+                        m_flags &= ~(VehicleFlag.LIMIT_ROLL_ONLY);
+                }
+                if ((pParam & (int)VehicleFlag.MOUSELOOK_BANK) == (int)VehicleFlag.MOUSELOOK_BANK)
+                {
+                    if ((m_flags & VehicleFlag.MOUSELOOK_BANK) != (VehicleFlag)0)
+                        m_flags &= ~(VehicleFlag.MOUSELOOK_BANK);
+                }
+                if ((pParam & (int)VehicleFlag.MOUSELOOK_STEER) == (int)VehicleFlag.MOUSELOOK_STEER)
+                {
+                    if ((m_flags & VehicleFlag.MOUSELOOK_STEER) != (VehicleFlag)0)
+                        m_flags &= ~(VehicleFlag.MOUSELOOK_STEER);
+                }
+                if ((pParam & (int)VehicleFlag.NO_DEFLECTION_UP) == (int)VehicleFlag.NO_DEFLECTION_UP)
+                {
+                    if ((m_flags & VehicleFlag.NO_DEFLECTION_UP) != (VehicleFlag)0)
+                        m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP);
+                }
+                if ((pParam & (int)VehicleFlag.CAMERA_DECOUPLED) == (int)VehicleFlag.CAMERA_DECOUPLED)
+                {
+                    if ((m_flags & VehicleFlag.CAMERA_DECOUPLED) != (VehicleFlag)0)
+                        m_flags &= ~(VehicleFlag.CAMERA_DECOUPLED);
+                }
+                if ((pParam & (int)VehicleFlag.NO_X) == (int)VehicleFlag.NO_X)
+                {
+                    if ((m_flags & VehicleFlag.NO_X) != (VehicleFlag)0)
+                        m_flags &= ~(VehicleFlag.NO_X);
+                }
+                if ((pParam & (int)VehicleFlag.NO_Y) == (int)VehicleFlag.NO_Y)
+                {
+                    if ((m_flags & VehicleFlag.NO_Y) != (VehicleFlag)0)
+                        m_flags &= ~(VehicleFlag.NO_Y);
+                }
+                if ((pParam & (int)VehicleFlag.NO_Z) == (int)VehicleFlag.NO_Z)
+                {
+                    if ((m_flags & VehicleFlag.NO_Z) != (VehicleFlag)0)
+                        m_flags &= ~(VehicleFlag.NO_Z);
+                }
+                if ((pParam & (int)VehicleFlag.LOCK_HOVER_HEIGHT) == (int)VehicleFlag.LOCK_HOVER_HEIGHT)
+                {
+                    if ((m_Hoverflags & VehicleFlag.LOCK_HOVER_HEIGHT) != (VehicleFlag)0)
+                        m_Hoverflags &= ~(VehicleFlag.LOCK_HOVER_HEIGHT);
+                }
+                if ((pParam & (int)VehicleFlag.NO_DEFLECTION) == (int)VehicleFlag.NO_DEFLECTION)
+                {
+                    if ((m_flags & VehicleFlag.NO_DEFLECTION) != (VehicleFlag)0)
+                        m_flags &= ~(VehicleFlag.NO_DEFLECTION);
+                }
+                if ((pParam & (int)VehicleFlag.LOCK_ROTATION) == (int)VehicleFlag.LOCK_ROTATION)
+                {
+                    if ((m_flags & VehicleFlag.LOCK_ROTATION) != (VehicleFlag)0)
+                        m_flags &= ~(VehicleFlag.LOCK_ROTATION);
+                }
+            }
+            else
+            {
+                if ((pParam & (int)VehicleFlag.HOVER_GLOBAL_HEIGHT) == (int)VehicleFlag.HOVER_GLOBAL_HEIGHT)
+                {
+                    m_Hoverflags |= (VehicleFlag.HOVER_GLOBAL_HEIGHT | m_flags);
+                }
+                if ((pParam & (int)VehicleFlag.HOVER_TERRAIN_ONLY) == (int)VehicleFlag.HOVER_TERRAIN_ONLY)
+                {
+                    m_Hoverflags |= (VehicleFlag.HOVER_TERRAIN_ONLY | m_flags);
+                }
+                if ((pParam & (int)VehicleFlag.HOVER_UP_ONLY) == (int)VehicleFlag.HOVER_UP_ONLY)
+                {
+                    m_Hoverflags |= (VehicleFlag.HOVER_UP_ONLY | m_flags);
+                }
+                if ((pParam & (int)VehicleFlag.HOVER_WATER_ONLY) == (int)VehicleFlag.HOVER_WATER_ONLY)
+                {
+                    m_Hoverflags |= (VehicleFlag.HOVER_WATER_ONLY | m_flags);
+                }
+                if ((pParam & (int)VehicleFlag.LIMIT_MOTOR_UP) == (int)VehicleFlag.LIMIT_MOTOR_UP)
+                {
+                    m_flags |= (VehicleFlag.LIMIT_MOTOR_UP | m_flags);
+                }
+                if ((pParam & (int)VehicleFlag.MOUSELOOK_BANK) == (int)VehicleFlag.MOUSELOOK_BANK)
+                {
+                    m_flags |= (VehicleFlag.MOUSELOOK_BANK | m_flags);
+                }
+                if ((pParam & (int)VehicleFlag.MOUSELOOK_STEER) == (int)VehicleFlag.MOUSELOOK_STEER)
+                {
+                    m_flags |= (VehicleFlag.MOUSELOOK_STEER | m_flags);
+                }
+                if ((pParam & (int)VehicleFlag.NO_DEFLECTION_UP) == (int)VehicleFlag.NO_DEFLECTION_UP)
+                {
+                    m_flags |= (VehicleFlag.NO_DEFLECTION_UP | m_flags);
+                }
+                if ((pParam & (int)VehicleFlag.CAMERA_DECOUPLED) == (int)VehicleFlag.CAMERA_DECOUPLED)
+                {
+                    m_flags |= (VehicleFlag.CAMERA_DECOUPLED | m_flags);
+                }
+                if ((pParam & (int)VehicleFlag.NO_X) == (int)VehicleFlag.NO_X)
+                {
+                    m_flags |= (VehicleFlag.NO_X);
+                }
+                if ((pParam & (int)VehicleFlag.NO_Y) == (int)VehicleFlag.NO_Y)
+                {
+                    m_flags |= (VehicleFlag.NO_Y);
+                }
+                if ((pParam & (int)VehicleFlag.NO_Z) == (int)VehicleFlag.NO_Z)
+                {
+                    m_flags |= (VehicleFlag.NO_Z);
+                }
+                if ((pParam & (int)VehicleFlag.LOCK_HOVER_HEIGHT) == (int)VehicleFlag.LOCK_HOVER_HEIGHT)
+                {
+                    m_Hoverflags |= (VehicleFlag.LOCK_HOVER_HEIGHT);
+                }
+                if ((pParam & (int)VehicleFlag.NO_DEFLECTION) == (int)VehicleFlag.NO_DEFLECTION)
+                {
+                    m_flags |= (VehicleFlag.NO_DEFLECTION);
+                }
+                if ((pParam & (int)VehicleFlag.LOCK_ROTATION) == (int)VehicleFlag.LOCK_ROTATION)
+                {
+                    m_flags |= (VehicleFlag.LOCK_ROTATION);
+                }
+            }
+        }//end ProcessVehicleFlags
+
+        internal void ProcessTypeChange(Vehicle pType)
+        {
+            // Set Defaults For Type
+            m_type = pType;
+            switch (pType)
+            {
+                    case Vehicle.TYPE_NONE:
+                    m_linearFrictionTimescale = new Vector3(0, 0, 0);
+                    m_angularFrictionTimescale = new Vector3(0, 0, 0);
+                    m_linearMotorDirection = Vector3.Zero;
+                    m_linearMotorTimescale = 0;
+                    m_linearMotorDecayTimescale = 0;
+                    m_angularMotorDirection = Vector3.Zero;
+                    m_angularMotorTimescale = 0;
+                    m_angularMotorDecayTimescale = 0;
+                    m_VhoverHeight = 0;
+                    m_VhoverTimescale = 0;
+                    m_VehicleBuoyancy = 0;
+                    m_flags = (VehicleFlag)0;
+                    break;
+
+                case Vehicle.TYPE_SLED:
+                    m_linearFrictionTimescale = new Vector3(30, 1, 1000);
+                    m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
+                    m_linearMotorDirection = Vector3.Zero;
+                    m_linearMotorTimescale = 1000;
+                    m_linearMotorDecayTimescale = 120;
+                    m_angularMotorDirection = 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_Hoverflags &=
+                         ~(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(1000, 1000, 1000);
+                    m_linearMotorDirection = Vector3.Zero;
+                    m_linearMotorTimescale = 1;
+                    m_linearMotorDecayTimescale = 60;
+                    m_angularMotorDirection = 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_Hoverflags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT);
+                    m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY |
+                                VehicleFlag.LIMIT_MOTOR_UP);
+                    m_Hoverflags |= (VehicleFlag.HOVER_UP_ONLY);
+                    break;
+                case Vehicle.TYPE_BOAT:
+                    m_linearFrictionTimescale = new Vector3(10, 3, 2);
+                    m_angularFrictionTimescale = new Vector3(10,10,10);
+                    m_linearMotorDirection = Vector3.Zero;
+                    m_linearMotorTimescale = 5;
+                    m_linearMotorDecayTimescale = 60;
+                    m_angularMotorDirection = 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_Hoverflags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY |
+                            VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
+                    m_flags &= ~(VehicleFlag.LIMIT_ROLL_ONLY);
+                    m_flags |= (VehicleFlag.NO_DEFLECTION_UP |
+                                VehicleFlag.LIMIT_MOTOR_UP);
+                    m_Hoverflags |= (VehicleFlag.HOVER_WATER_ONLY);
+                    break;
+                case Vehicle.TYPE_AIRPLANE:
+                    m_linearFrictionTimescale = new Vector3(200, 10, 5);
+                    m_angularFrictionTimescale = new Vector3(20, 20, 20);
+                    m_linearMotorDirection = Vector3.Zero;
+                    m_linearMotorTimescale = 2;
+                    m_linearMotorDecayTimescale = 60;
+                    m_angularMotorDirection = 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_Hoverflags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
+                        VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
+                    m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | 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_linearMotorDirection = Vector3.Zero;
+                    m_linearMotorTimescale = 5;
+                    m_linearMotorDecayTimescale = 60;
+                    m_angularMotorDirection = 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_Hoverflags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
+                        VehicleFlag.HOVER_UP_ONLY);
+                    m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_MOTOR_UP);
+                    m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY);
+                    m_Hoverflags |= (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 Step(float pTimestep,  OdeScene pParentScene)
+        {
+            if (m_body == IntPtr.Zero || m_type == Vehicle.TYPE_NONE)
+                return;
+            frcount++;  // used to limit debug comment output
+            if (frcount > 100)
+                frcount = 0;
+
+            MoveLinear(pTimestep, pParentScene);
+            MoveAngular(pTimestep);
+            LimitRotation(pTimestep);
+        }// end Step
+
+        private void MoveLinear(float pTimestep, OdeScene _pParentScene)
+        {
+            if (!m_linearMotorDirection.ApproxEquals(Vector3.Zero, 0.01f))  // requested m_linearMotorDirection is significant
+            {
+                 if (!d.BodyIsEnabled(Body))
+                     d.BodyEnable(Body);
+
+                // add drive to body
+                Vector3 addAmount = m_linearMotorDirection/(m_linearMotorTimescale/pTimestep);
+                m_lastLinearVelocityVector += (addAmount*10);  // lastLinearVelocityVector is the current body velocity vector?
+
+                // This will work temporarily, but we really need to compare speed on an axis
+                // KF: Limit body velocity to applied velocity?
+                if (Math.Abs(m_lastLinearVelocityVector.X) > Math.Abs(m_linearMotorDirectionLASTSET.X))
+                    m_lastLinearVelocityVector.X = m_linearMotorDirectionLASTSET.X;
+                if (Math.Abs(m_lastLinearVelocityVector.Y) > Math.Abs(m_linearMotorDirectionLASTSET.Y))
+                    m_lastLinearVelocityVector.Y = m_linearMotorDirectionLASTSET.Y;
+                if (Math.Abs(m_lastLinearVelocityVector.Z) > Math.Abs(m_linearMotorDirectionLASTSET.Z))
+                    m_lastLinearVelocityVector.Z = m_linearMotorDirectionLASTSET.Z;
+
+                // decay applied velocity
+                Vector3 decayfraction = ((Vector3.One/(m_linearMotorDecayTimescale/pTimestep)));
+                //Console.WriteLine("decay: " + decayfraction);
+                m_linearMotorDirection -= m_linearMotorDirection * decayfraction * 0.5f;
+                //Console.WriteLine("actual: " + m_linearMotorDirection);
+            }
+            else
+            {        // requested is not significant
+                    // if what remains of applied is small, zero it.
+                if (m_lastLinearVelocityVector.ApproxEquals(Vector3.Zero, 0.01f))
+                    m_lastLinearVelocityVector = Vector3.Zero;
+            }
+
+            // convert requested object velocity to world-referenced vector
+            m_dir = m_lastLinearVelocityVector;
+            d.Quaternion rot = d.BodyGetQuaternion(Body);
+            Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);    // rotq = rotation of object
+            m_dir *= rotq;                            // apply obj rotation to velocity vector
+
+            // add Gravity andBuoyancy
+            // KF: So far I have found no good method to combine a script-requested
+            // .Z velocity and gravity. Therefore only 0g will used script-requested
+            // .Z velocity. >0g (m_VehicleBuoyancy < 1) will used modified gravity only.
+            Vector3 grav = Vector3.Zero;
+            // 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 = _pParentScene.gravityz * objMass.mass * (1f - m_VehicleBuoyancy);
+            // Preserve the current Z velocity
+            d.Vector3 vel_now = d.BodyGetLinearVel(Body);
+            m_dir.Z = vel_now.Z;        // Preserve the accumulated falling velocity
+
+            d.Vector3 pos = d.BodyGetPosition(Body);
+//            Vector3 accel = new Vector3(-(m_dir.X - m_lastLinearVelocityVector.X / 0.1f), -(m_dir.Y - m_lastLinearVelocityVector.Y / 0.1f), m_dir.Z - m_lastLinearVelocityVector.Z / 0.1f);
+            Vector3 posChange = new Vector3();
+            posChange.X = pos.X - m_lastPositionVector.X;
+            posChange.Y = pos.Y - m_lastPositionVector.Y;
+            posChange.Z = pos.Z - m_lastPositionVector.Z;
+            double Zchange = Math.Abs(posChange.Z);
+            if (m_BlockingEndPoint != Vector3.Zero)
+            {
+                if (pos.X >= (m_BlockingEndPoint.X - (float)1))
+                {
+                    pos.X -= posChange.X + 1;
+                    d.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
+                }
+                if (pos.Y >= (m_BlockingEndPoint.Y - (float)1))
+                {
+                    pos.Y -= posChange.Y + 1;
+                    d.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
+                }
+                if (pos.Z >= (m_BlockingEndPoint.Z - (float)1))
+                {
+                    pos.Z -= posChange.Z + 1;
+                    d.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
+                }
+                if (pos.X <= 0)
+                {
+                    pos.X += posChange.X + 1;
+                    d.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
+                }
+                if (pos.Y <= 0)
+                {
+                    pos.Y += posChange.Y + 1;
+                    d.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
+                }
+            }
+            if (pos.Z < _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y))
+            {
+                pos.Z = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y) + 2;
+                d.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
+            }
+
+            // Check if hovering
+            if ((m_Hoverflags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0)
+            {
+                // We should hover, get the target height
+                if ((m_Hoverflags & VehicleFlag.HOVER_WATER_ONLY) != 0)
+                {
+                    m_VhoverTargetHeight = _pParentScene.GetWaterLevel() + m_VhoverHeight;
+                }
+                if ((m_Hoverflags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0)
+                {
+                    m_VhoverTargetHeight = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y) + m_VhoverHeight;
+                }
+                if ((m_Hoverflags & VehicleFlag.HOVER_GLOBAL_HEIGHT) != 0)
+                {
+                    m_VhoverTargetHeight = m_VhoverHeight;
+                }
+
+                if ((m_Hoverflags & VehicleFlag.HOVER_UP_ONLY) != 0)
+                {
+                    // If body is aready heigher, use its height as target height
+                    if (pos.Z > m_VhoverTargetHeight) m_VhoverTargetHeight = pos.Z;
+                }
+                if ((m_Hoverflags & VehicleFlag.LOCK_HOVER_HEIGHT) != 0)
+                {
+                    if ((pos.Z - m_VhoverTargetHeight) > .2 || (pos.Z - m_VhoverTargetHeight) < -.2)
+                    {
+                        d.BodySetPosition(Body, pos.X, pos.Y, m_VhoverTargetHeight);
+                    }
+                }
+                else
+                {
+                    float herr0 = pos.Z - m_VhoverTargetHeight;
+                    // Replace Vertical speed with correction figure if significant
+                    if (Math.Abs(herr0) > 0.01f)
+                    {
+                        m_dir.Z = -((herr0 * pTimestep * 50.0f) / m_VhoverTimescale);
+                        //KF: m_VhoverEfficiency is not yet implemented
+                    }
+                    else
+                    {
+                        m_dir.Z = 0f;
+                    }
+                }
+
+//                m_VhoverEfficiency = 0f;    // 0=boucy, 1=Crit.damped
+//                m_VhoverTimescale = 0f;        // time to acheive height
+//                pTimestep  is time since last frame,in secs
+            }
+
+            if ((m_flags & (VehicleFlag.LIMIT_MOTOR_UP)) != 0)
+            {
+                //Start Experimental Values
+                if (Zchange > .3)
+                {
+                    grav.Z = (float)(grav.Z * 3);
+                }
+                if (Zchange > .15)
+                {
+                    grav.Z = (float)(grav.Z * 2);
+                }
+                if (Zchange > .75)
+                {
+                    grav.Z = (float)(grav.Z * 1.5);
+                }
+                if (Zchange > .05)
+                {
+                    grav.Z = (float)(grav.Z * 1.25);
+                }
+                if (Zchange > .025)
+                {
+                    grav.Z = (float)(grav.Z * 1.125);
+                }
+                float terraintemp = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y);
+                float postemp = (pos.Z - terraintemp);
+                if (postemp > 2.5f)
+                {
+                    grav.Z = (float)(grav.Z * 1.037125);
+                }
+                //End Experimental Values
+            }
+            if ((m_flags & (VehicleFlag.NO_X)) != 0)
+            {
+                m_dir.X = 0;
+            }
+            if ((m_flags & (VehicleFlag.NO_Y)) != 0)
+            {
+                m_dir.Y = 0;
+            }
+            if ((m_flags & (VehicleFlag.NO_Z)) != 0)
+            {
+                m_dir.Z = 0;
+            }
+
+            m_lastPositionVector = d.BodyGetPosition(Body);
+
+            // Apply velocity
+            d.BodySetLinearVel(Body, m_dir.X, m_dir.Y, m_dir.Z);
+            // apply gravity force
+            d.BodyAddForce(Body, grav.X, grav.Y, grav.Z);
+
+
+            // apply friction
+            Vector3 decayamount = Vector3.One / (m_linearFrictionTimescale / pTimestep);
+            m_lastLinearVelocityVector -= m_lastLinearVelocityVector * decayamount;
+        } // end MoveLinear()
+
+        private void MoveAngular(float pTimestep)
+        {
+            /*
+            private Vector3 m_angularMotorDirection = Vector3.Zero;            // angular velocity requested by LSL motor
+            private int m_angularMotorApply = 0;                            // application frame counter
+             private float m_angularMotorVelocity = 0;                        // current angular motor velocity (ramps up and down)
+            private float m_angularMotorTimescale = 0;                        // motor angular velocity ramp up rate
+            private float m_angularMotorDecayTimescale = 0;                    // motor angular velocity decay rate
+            private Vector3 m_angularFrictionTimescale = Vector3.Zero;        // body angular velocity  decay rate
+            private Vector3 m_lastAngularVelocity = Vector3.Zero;            // what was last applied to body
+            */
+
+            // Get what the body is doing, this includes 'external' influences
+            d.Vector3 angularVelocity = d.BodyGetAngularVel(Body);
+   //         Vector3 angularVelocity = Vector3.Zero;
+
+            if (m_angularMotorApply > 0)
+            {
+                // ramp up to new value
+                //   current velocity  +=                         error                       /    (time to get there / step interval)
+                //                               requested speed            -  last motor speed
+                m_angularMotorVelocity.X += (m_angularMotorDirection.X - m_angularMotorVelocity.X) /  (m_angularMotorTimescale / pTimestep);
+                m_angularMotorVelocity.Y += (m_angularMotorDirection.Y - m_angularMotorVelocity.Y) /  (m_angularMotorTimescale / pTimestep);
+                m_angularMotorVelocity.Z += (m_angularMotorDirection.Z - m_angularMotorVelocity.Z) /  (m_angularMotorTimescale / pTimestep);
+
+                m_angularMotorApply--;        // This is done so that if script request rate is less than phys frame rate the expected
+                                            // velocity may still be acheived.
+            }
+            else
+            {
+                // no motor recently applied, keep the body velocity
+        /*        m_angularMotorVelocity.X = angularVelocity.X;
+                m_angularMotorVelocity.Y = angularVelocity.Y;
+                m_angularMotorVelocity.Z = angularVelocity.Z; */
+
+                // and decay the velocity
+                m_angularMotorVelocity -= m_angularMotorVelocity /  (m_angularMotorDecayTimescale / pTimestep);
+            } // end motor section
+
+            // Vertical attractor section
+            Vector3 vertattr = Vector3.Zero;
+
+            if (m_verticalAttractionTimescale < 300)
+            {
+                float VAservo = 0.2f / (m_verticalAttractionTimescale * pTimestep);
+                // get present body rotation
+                d.Quaternion rot = d.BodyGetQuaternion(Body);
+                Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);
+                // 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)
+                {
+                    verterr.X = 2.0f - verterr.X;
+                    verterr.Y = 2.0f - verterr.Y;
+                }
+                // Error is 0 (no error) to +/- 2 (max error)
+                // scale it by VAservo
+                verterr = verterr * VAservo;
+//if (frcount == 0) Console.WriteLine("VAerr=" + verterr);
+
+                // 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;
+
+                // scaling appears better usingsquare-law
+                float bounce = 1.0f - (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency);
+                vertattr.X += bounce * angularVelocity.X;
+                vertattr.Y += bounce * angularVelocity.Y;
+
+            } // else vertical attractor is off
+
+    //        m_lastVertAttractor = vertattr;
+
+            // Bank section tba
+            // Deflection section tba
+
+            // Sum velocities
+            m_lastAngularVelocity = m_angularMotorVelocity + vertattr; // + bank + deflection
+            
+            if ((m_flags & (VehicleFlag.NO_DEFLECTION_UP)) != 0)
+            {
+                m_lastAngularVelocity.X = 0;
+                m_lastAngularVelocity.Y = 0;
+            }
+
+            if (!m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f))
+            {
+                if (!d.BodyIsEnabled (Body))  d.BodyEnable (Body);
+            }
+            else
+            {
+                m_lastAngularVelocity = Vector3.Zero; // Reduce small value to zero.
+            }
+
+             // apply friction
+            Vector3 decayamount = Vector3.One / (m_angularFrictionTimescale / pTimestep);
+            m_lastAngularVelocity -= m_lastAngularVelocity * decayamount;
+
+            // Apply to the body
+            d.BodySetAngularVel (Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z);
+
+        } //end MoveAngular
+        internal void LimitRotation(float timestep)
+        {
+            d.Quaternion rot = d.BodyGetQuaternion(Body);
+            Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);    // rotq = rotation of object
+            d.Quaternion m_rot = new d.Quaternion();
+            bool changed = false;
+            m_rot.X = rotq.X;
+            m_rot.Y = rotq.Y;
+            m_rot.Z = rotq.Z;
+            m_rot.W = rotq.W;
+            if (m_RollreferenceFrame != Quaternion.Identity)
+            {
+                if (rotq.X >= m_RollreferenceFrame.X)
+                {
+                    m_rot.X = rotq.X - (m_RollreferenceFrame.X / 2);
+                }
+                if (rotq.Y >= m_RollreferenceFrame.Y)
+                {
+                    m_rot.Y = rotq.Y - (m_RollreferenceFrame.Y / 2);
+                }
+                if (rotq.X <= -m_RollreferenceFrame.X)
+                {
+                    m_rot.X = rotq.X + (m_RollreferenceFrame.X / 2);
+                }
+                if (rotq.Y <= -m_RollreferenceFrame.Y)
+                {
+                    m_rot.Y = rotq.Y + (m_RollreferenceFrame.Y / 2);
+                }
+                changed = true;
+            }
+            if ((m_flags & VehicleFlag.LOCK_ROTATION) != 0)
+            {
+                m_rot.X = 0;
+                m_rot.Y = 0;
+                changed = true;
+            }
+            if (changed)
+                d.BodySetQuaternion(Body, ref m_rot);
+        }
+    }
+}
diff --git a/OpenSim/Region/PhysicsModules/Ode/ODEPrim.cs b/OpenSim/Region/PhysicsModules/Ode/ODEPrim.cs
new file mode 100644
index 0000000..54a2033
--- /dev/null
+++ b/OpenSim/Region/PhysicsModules/Ode/ODEPrim.cs
@@ -0,0 +1,3435 @@
+/*
+ * 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 August 26 2009 by Kitto Flora. ODEDynamics.cs replaces
+ * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised:
+ * ODEPrim.cs contains methods dealing with Prim editing, Prim
+ * characteristics and Kinetic motion.
+ * ODEDynamics.cs contains methods dealing with Prim Physical motion
+ * (dynamics) and the associated settings. Old Linear and angular
+ * motors for dynamic motion have been replace with  MoveLinear()
+ * and MoveAngular(); 'Physical' is used only to switch ODE dynamic 
+ * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_<other> is to
+ * switch between 'VEHICLE' parameter use and general dynamics
+ * settings use.
+ */
+
+//#define SPAM
+
+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.PhysicsModules.SharedBase;
+
+namespace OpenSim.Region.PhysicsModule.ODE
+{
+    /// <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 bool m_isphysical;
+
+        public int ExpectedCollisionContacts { get { return m_expectedCollisionContacts; } }
+        private int m_expectedCollisionContacts = 0;
+
+        /// <summary>
+        /// Gets collide bits so that we can still perform land collisions if a mesh fails to load.
+        /// </summary>
+        private int BadMeshAssetCollideBits
+        {
+            get { return m_isphysical ? (int)CollisionCategories.Land : 0; }
+        }
+
+        /// <summary>
+        /// Is this prim subject to physics?  Even if not, it's still solid for collision purposes.
+        /// </summary>
+        public override bool IsPhysical
+        {
+            get { return m_isphysical; }
+            set
+            {
+                m_isphysical = value;
+                if (!m_isphysical) // Zero the remembered last velocity
+                    m_lastVelocity = Vector3.Zero;
+            }
+        }
+
+        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_angularlock = Vector3.One;
+        private Vector3 m_taintAngularLock = Vector3.One;
+        private IntPtr Amotor = IntPtr.Zero;
+
+        private bool m_assetFailed = false;
+
+        private Vector3 m_PIDTarget;
+        private float m_PIDTau;
+        private float PID_D = 35f;
+        private float PID_G = 25f;
+
+        // KF: These next 7 params apply to llSetHoverHeight(float height, integer water, float tau),
+        // and are for non-VEHICLES only.
+
+        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;                //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle. 
+
+        // 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;
+
+        // 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 { get; private set; }
+        public bool m_taintdisable { get; private set; }
+        internal bool m_disabled;
+        public bool m_taintadd { get; private set; }
+        public bool m_taintselected { get; private set; }
+        public bool m_taintCollidesWater { get; private set; }
+
+        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 PrimitiveBaseShape _pbs;
+        private OdeScene _parent_scene;
+        
+        /// <summary>
+        /// The physics space which contains prim geometries
+        /// </summary>
+        public IntPtr m_targetSpace = IntPtr.Zero;
+
+        /// <summary>
+        /// The prim geometry, used for collision detection.
+        /// </summary>
+        /// <remarks>
+        /// This is never null except for a brief period when the geometry needs to be replaced (due to resizing or
+        /// mesh change) or when the physical prim is being removed from the scene.
+        /// </remarks>
+        public IntPtr prim_geom { get; private set; }
+
+        public IntPtr _triMeshData { get; private set; }
+
+        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_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 { get; private set; }
+        internal float m_collisionscore;
+        public int m_roundsUnderMotionThreshold { get; private set; }
+        private int m_crossingfailures;
+
+        public bool outofBounds { get; private set; }
+        private float m_density = 10.000006836f; // Aluminum g/cm3;
+
+        public bool _zeroFlag { get; private set; }
+        private bool m_lastUpdateSent;
+
+        public IntPtr Body = IntPtr.Zero;
+        private Vector3 _target_velocity;
+        private d.Mass pMass;
+
+        private int m_eventsubscription;
+        private CollisionEventUpdate CollisionEventsThisFrame = new CollisionEventUpdate();
+
+        /// <summary>
+        /// Signal whether there were collisions on the previous frame, so we know if we need to send the
+        /// empty CollisionEventsThisFrame to the prim so that it can detect the end of a collision.
+        /// </summary>
+        /// <remarks>
+        /// This is probably a temporary measure, pending storing this information consistently in CollisionEventUpdate itself.
+        /// </remarks>
+        private bool m_collisionsOnPreviousFrame;
+
+        private IntPtr m_linkJoint = IntPtr.Zero;
+
+        internal volatile bool childPrim;
+
+        private ODEDynamics m_vehicle;
+
+        internal int m_material = (int)Material.Wood;
+
+        public OdePrim(
+            String primName, OdeScene parent_scene, Vector3 pos, Vector3 size,
+            Quaternion rotation, PrimitiveBaseShape pbs, bool pisPhysical)
+        {
+            Name = primName;
+            m_vehicle = new ODEDynamics();
+            //gc = GCHandle.Alloc(prim_geom, GCHandleType.Pinned);
+
+            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.WarnFormat("[PHYSICS]: Got nonFinite Object create Position for {0}", Name);
+            }
+            _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;
+
+            if (!pos.IsFinite())
+            {
+                size = new Vector3(0.5f, 0.5f, 0.5f);
+                m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Size for {0}", Name);
+            }
+
+            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.WarnFormat("[PHYSICS]: Got nonFinite Object create Rotation for {0}", Name);
+            }
+
+            _orientation = rotation;
+            m_taintrot = _orientation;
+            _pbs = pbs;
+
+            _parent_scene = parent_scene;
+            m_targetSpace = (IntPtr)0;
+
+            if (pos.Z < 0)
+            {
+                IsPhysical = false;
+            }
+            else
+            {
+                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 (IsPhysical)
+                    m_targetSpace = _parent_scene.space;
+            }
+
+            m_taintadd = true;
+            m_assetFailed = false;
+            _parent_scene.AddPhysicsActorTaint(this);
+        }
+
+        public override int PhysicsActorType
+        {
+            get { return (int) ActorTypes.Prim; }
+            set { return; }
+        }
+
+        public override bool SetAlwaysRun
+        {
+            get { return false; }
+            set { return; }
+        }
+
+        public override bool Grabbed
+        {
+            set { return; }
+        }
+
+        public override bool Selected
+        {
+            set
+            {
+                // 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 ((IsPhysical && !_zeroFlag) || !value)
+                {
+                    m_taintselected = value;
+                    _parent_scene.AddPhysicsActorTaint(this);
+                }
+                else
+                {
+                    m_taintselected = value;
+                    m_isSelected = value;
+                }
+
+                if (m_isSelected)
+                    disableBodySoft();
+            }
+        }
+
+        /// <summary>
+        /// Set a new geometry for this prim.
+        /// </summary>
+        /// <param name="geom"></param>
+        private void SetGeom(IntPtr geom)
+        {
+            prim_geom = geom;
+//Console.WriteLine("SetGeom to " + prim_geom + " for " + Name);
+
+            if (m_assetFailed)
+            {
+                d.GeomSetCategoryBits(prim_geom, 0);
+                d.GeomSetCollideBits(prim_geom, BadMeshAssetCollideBits);
+            }
+            else
+            {
+                d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
+                d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
+            }
+
+            _parent_scene.geom_name_map[prim_geom] = Name;
+            _parent_scene.actor_name_map[prim_geom] = this;
+
+            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);
+        }
+
+        private void enableBodySoft()
+        {
+            if (!childPrim)
+            {
+                if (IsPhysical && Body != IntPtr.Zero)
+                {
+                    d.BodyEnable(Body);
+                    if (m_vehicle.Type != Vehicle.TYPE_NONE)
+                        m_vehicle.Enable(Body, _parent_scene);
+                }
+
+                m_disabled = false;
+            }
+        }
+
+        private void disableBodySoft()
+        {
+            m_disabled = true;
+
+            if (IsPhysical && Body != IntPtr.Zero)
+            {
+                d.BodyDisable(Body);
+            }
+        }
+
+        /// <summary>
+        /// Make a prim subject to physics.
+        /// </summary>
+        private 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);
+
+                if (m_assetFailed)
+                {
+                    d.GeomSetCategoryBits(prim_geom, 0);
+                    d.GeomSetCollideBits(prim_geom, BadMeshAssetCollideBits);
+                }
+                else
+                {
+                    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;
+
+                // The body doesn't already have a finite rotation mode set here
+                if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0.0f)) && _parent == null)
+                {
+                    createAMotor(m_angularlock);
+                }
+                if (m_vehicle.Type != Vehicle.TYPE_NONE)
+                {
+                    m_vehicle.Enable(Body, _parent_scene);
+                }
+
+                _parent_scene.ActivatePrim(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;
+        }
+
+        #endregion
+
+        private 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);
+            }
+        }
+
+        private void setAngularVelocity(float x, float y, float z)
+        {
+            if (Body != (IntPtr)0)
+            {
+                d.BodySetAngularVel(Body, x, y, z);
+            }
+        }
+
+        /// <summary>
+        /// Stop a prim from being subject to physics.
+        /// </summary>
+        internal void disableBody()
+        {
+            //this kills the body so things like 'mesh' can re-create it.
+            lock (this)
+            {
+                if (!childPrim)
+                {
+                    if (Body != IntPtr.Zero)
+                    {
+                        _parent_scene.DeactivatePrim(this);
+                        m_collisionCategories &= ~CollisionCategories.Body;
+                        m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land);
+
+                        if (m_assetFailed)
+                        {
+                            d.GeomSetCategoryBits(prim_geom, 0);
+                            d.GeomSetCollideBits(prim_geom, 0);
+                        }
+                        else
+                        {
+                            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.DeactivatePrim(prm);
+                                    prm.Body = IntPtr.Zero;
+                                }
+                            }
+                        }
+                        Body = IntPtr.Zero;
+                    }
+                }
+                else
+                {
+                    _parent_scene.DeactivatePrim(this);
+                    
+                    m_collisionCategories &= ~CollisionCategories.Body;
+                    m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land);
+
+                    if (m_assetFailed)
+                    {
+                        d.GeomSetCategoryBits(prim_geom, 0);
+                        d.GeomSetCollideBits(prim_geom, 0);
+                    }
+                    else
+                    {
+
+                        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>();
+
+        private void setMesh(OdeScene parent_scene, IMesh mesh)
+        {
+//            m_log.DebugFormat("[ODE PRIM]: Setting mesh on {0} to {1}", Name, 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
+            m_expectedCollisionContacts = indexCount;
+            mesh.releaseSourceMeshData(); // free up the original mesh data to save memory
+
+            // We must lock here since m_MeshToTriMeshMap is static and multiple scene threads may call this method at
+            // the same time.
+            lock (m_MeshToTriMeshMap)
+            {
+                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
+            {
+                SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, parent_scene.triCallback, null, null));
+            }
+            catch (AccessViolationException)
+            {
+                m_log.ErrorFormat("[PHYSICS]: MESH LOCKED FOR {0}", Name);
+                return;
+            }
+
+           // if (IsPhysical && Body == (IntPtr) 0)
+           // {
+                // Recreate the body
+          //     m_interpenetrationcount = 0;
+           //     m_collisionscore = 0;
+
+           //     enableBody();
+           // }
+        }
+
+        internal void ProcessTaints()
+        {
+#if SPAM
+Console.WriteLine("ZProcessTaints for " + Name);
+#endif
+
+            // This must be processed as the very first taint so that later operations have a prim_geom to work with
+            // if this is a new prim.
+            if (m_taintadd)
+                changeadd();
+
+            if (!_position.ApproxEquals(m_taintposition, 0f))
+                 changemove();
+
+            if (m_taintrot != _orientation)
+            {
+                if (childPrim && IsPhysical)    // For physical child prim...
+                {
+                    rotate();
+                    // KF: ODE will also rotate the parent prim!
+                    // so rotate the root back to where it was
+                    OdePrim parent = (OdePrim)_parent;
+                    parent.rotate();
+                }
+                else
+                {
+                    //Just rotate the prim
+                    rotate();
+                }
+            }
+        
+            if (m_taintPhysics != IsPhysical && !(m_taintparent != _parent))
+                changePhysicsStatus();
+
+            if (!_size.ApproxEquals(m_taintsize, 0f))
+                changesize();
+
+            if (m_taintshape)
+                changeshape();
+
+            if (m_taintforce)
+                changeAddForce();
+
+            if (m_taintaddangularforce)
+                changeAddAngularForce();
+
+            if (!m_taintTorque.ApproxEquals(Vector3.Zero, 0.001f))
+                changeSetTorque();
+
+            if (m_taintdisable)
+                changedisable();
+
+            if (m_taintselected != m_isSelected)
+                changeSelectedStatus();
+
+            if (!m_taintVelocity.ApproxEquals(Vector3.Zero, 0.001f))
+                changevelocity();
+
+            if (m_taintparent != _parent)
+                changelink();
+
+            if (m_taintCollidesWater != m_collidesWater)
+                changefloatonwater();
+
+            if (!m_angularlock.ApproxEquals(m_taintAngularLock,0f))
+                changeAngularLock();
+        }
+
+        /// <summary>
+        /// Change prim in response to an angular lock taint.
+        /// </summary>
+        private void changeAngularLock()
+        {
+            // do we have a Physical object?
+            if (Body != IntPtr.Zero)
+            {
+                //Check that we have a Parent
+                //If we have a parent then we're not authorative here
+                if (_parent == null)
+                {
+                    if (!m_taintAngularLock.ApproxEquals(Vector3.One, 0f))
+                    {
+                        //d.BodySetFiniteRotationMode(Body, 0);
+                        //d.BodySetFiniteRotationAxis(Body,m_taintAngularLock.X,m_taintAngularLock.Y,m_taintAngularLock.Z);
+                        createAMotor(m_taintAngularLock);
+                    }
+                    else
+                    {
+                        if (Amotor != IntPtr.Zero)
+                        {
+                            d.JointDestroy(Amotor);
+                            Amotor = IntPtr.Zero;
+                        }
+                    }
+                }
+            }
+
+            // Store this for later in case we get turned into a separate body
+            m_angularlock = m_taintAngularLock;
+        }
+
+        /// <summary>
+        /// Change prim in response to a link taint.
+        /// </summary>
+        private void changelink()
+        {
+            // 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();
+//Console.WriteLine("changelink calls ParentPrim");
+                    obj.AddChildPrim(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)
+            {
+//Console.WriteLine("  changelink B");
+            
+                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 = IsPhysical;
+        }
+
+        /// <summary>
+        /// Add a child prim to this parent prim.
+        /// </summary>
+        /// <param name="prim">Child prim</param>
+        private void AddChildPrim(OdePrim prim)
+        {
+            if (LocalID == prim.LocalID)
+                return;
+
+            if (Body == IntPtr.Zero)
+            {
+                Body = d.BodyCreate(_parent_scene.world);
+                setMass();
+            }
+
+            lock (childrenPrim)
+            {
+                if (childrenPrim.Contains(prim))
+                    return;
+
+//                m_log.DebugFormat(
+//                    "[ODE PRIM]: Linking prim {0} {1} to {2} {3}", prim.Name, prim.LocalID, Name, LocalID);
+
+                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);
+
+//Console.WriteLine(" GeomSetCategoryBits 1: " + prm.prim_geom + " - " + (int)prm.m_collisionCategories + " for " + Name);
+                    if (prm.m_assetFailed)
+                    {
+                        d.GeomSetCategoryBits(prm.prim_geom, 0);
+                        d.GeomSetCollideBits(prm.prim_geom, prm.BadMeshAssetCollideBits);
+                    }
+                    else
+                    {
+                        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.DebugFormat("[PHYSICS]: {0} ain't got no boooooooooddy, no body", Name);
+                    }
+
+                    prm.m_interpenetrationcount = 0;
+                    prm.m_collisionscore = 0;
+                    prm.m_disabled = false;
+
+                    // The body doesn't already have a finite rotation mode set here
+                    if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0f)) && _parent == null)
+                    {
+                        prm.createAMotor(m_angularlock);
+                    }
+                    prm.Body = Body;
+                    _parent_scene.ActivatePrim(prm);
+                }
+
+                m_collisionCategories |= CollisionCategories.Body;
+                m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
+
+                if (m_assetFailed)
+                {
+                    d.GeomSetCategoryBits(prim_geom, 0);
+                    d.GeomSetCollideBits(prim_geom, BadMeshAssetCollideBits);
+                }
+                else
+                {
+                    //Console.WriteLine("GeomSetCategoryBits 2: " + prim_geom + " - " + (int)m_collisionCategories + " for " + Name);
+                    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;
+
+                // The body doesn't already have a finite rotation mode set here
+                if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0f)) && _parent == null)
+                {
+                    createAMotor(m_angularlock);
+                }
+
+                d.BodySetPosition(Body, Position.X, Position.Y, Position.Z);
+
+                if (m_vehicle.Type != Vehicle.TYPE_NONE)
+                    m_vehicle.Enable(Body, _parent_scene);
+
+                _parent_scene.ActivatePrim(this);
+            }
+        }
+
+        private void ChildSetGeom(OdePrim odePrim)
+        {
+//            m_log.DebugFormat(
+//                "[ODE PRIM]: ChildSetGeom {0} {1} for {2} {3}", odePrim.Name, odePrim.LocalID, Name, LocalID);
+
+            //if (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();
+
+            // Spurious - Body == IntPtr.Zero after disableBody()
+//            if (Body != IntPtr.Zero)
+//            {
+//                _parent_scene.DeactivatePrim(this);
+//            }
+
+            lock (childrenPrim)
+            {
+                foreach (OdePrim prm in childrenPrim)
+                {
+//Console.WriteLine("ChildSetGeom calls ParentPrim");
+                    AddChildPrim(prm);
+                }
+            }
+        }
+
+        private void ChildDelink(OdePrim odePrim)
+        {
+//            m_log.DebugFormat(
+//                "[ODE PRIM]: Delinking prim {0} {1} from {2} {3}", odePrim.Name, odePrim.LocalID, Name, LocalID);
+
+            // 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)
+            {
+ //Console.WriteLine("childrenPrim.Remove " + odePrim);
+                childrenPrim.Remove(odePrim);
+            }
+
+            // Spurious - Body == IntPtr.Zero after disableBody()
+//            if (Body != IntPtr.Zero)
+//            {
+//                _parent_scene.DeactivatePrim(this);
+//            }
+
+            lock (childrenPrim)
+            {
+                foreach (OdePrim prm in childrenPrim)
+                {
+//Console.WriteLine("ChildDelink calls ParentPrim");
+                    AddChildPrim(prm);
+                }
+            }
+        }
+
+        /// <summary>
+        /// Change prim in response to a selection taint.
+        /// </summary>
+        private void changeSelectedStatus()
+        {
+            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 (IsPhysical)
+                {
+                    disableBodySoft();
+                }
+
+                if (m_assetFailed)
+                {
+                    d.GeomSetCategoryBits(prim_geom, 0);
+                    d.GeomSetCollideBits(prim_geom, 0);
+                }
+                else
+                {
+                    d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
+                    d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
+                }
+
+                if (IsPhysical)
+                {
+                    disableBodySoft();
+                }
+            }
+            else
+            {
+                m_collisionCategories = CollisionCategories.Geom;
+
+                if (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 (m_assetFailed)
+                {
+                    d.GeomSetCategoryBits(prim_geom, 0);
+                    d.GeomSetCollideBits(prim_geom, BadMeshAssetCollideBits);
+                }
+                else
+                {
+                    d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
+                    d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
+                }
+
+                if (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
+
+        internal void ResetTaints()
+        {
+            m_taintposition = _position;
+            m_taintrot = _orientation;
+            m_taintPhysics = IsPhysical;
+            m_taintselected = m_isSelected;
+            m_taintsize = _size;
+            m_taintshape = false;
+            m_taintforce = false;
+            m_taintdisable = false;
+            m_taintVelocity = Vector3.Zero;
+        }
+
+        /// <summary>
+        /// Create a geometry for the given mesh in the given target space.
+        /// </summary>
+        /// <param name="m_targetSpace"></param>
+        /// <param name="mesh">If null, then a mesh is used that is based on the profile shape data.</param>
+        private void CreateGeom(IntPtr m_targetSpace, IMesh mesh)
+        {
+#if SPAM
+Console.WriteLine("CreateGeom:");
+#endif
+            if (mesh != null)
+            {
+                setMesh(_parent_scene, mesh);
+            }
+            else
+            {
+                if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1)
+                {
+                    if (_size.X == _size.Y && _size.Y == _size.Z && _size.X == _size.Z)
+                    {
+                        if (((_size.X / 2f) > 0f))
+                        {
+//                            _parent_scene.waitForSpaceUnlock(m_targetSpace);
+                            try
+                            {
+//Console.WriteLine(" CreateGeom 1");
+                                SetGeom(d.CreateSphere(m_targetSpace, _size.X / 2));
+                                m_expectedCollisionContacts = 3;
+                            }
+                            catch (AccessViolationException)
+                            {
+                                m_log.WarnFormat("[PHYSICS]: Unable to create physics proxy for object {0}", Name);
+                                return;
+                            }
+                        }
+                        else
+                        {
+//                            _parent_scene.waitForSpaceUnlock(m_targetSpace);
+                            try
+                            {
+//Console.WriteLine(" CreateGeom 2");
+                                SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
+                                m_expectedCollisionContacts = 4;
+                            }
+                            catch (AccessViolationException)
+                            {
+                                m_log.WarnFormat("[PHYSICS]: Unable to create physics proxy for object {0}", Name);
+                                return;
+                            }
+                        }
+                    }
+                    else
+                    {
+//                        _parent_scene.waitForSpaceUnlock(m_targetSpace);
+                        try
+                        {
+//Console.WriteLine("  CreateGeom 3");
+                            SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
+                            m_expectedCollisionContacts = 4;
+                        }
+                        catch (AccessViolationException)
+                        {
+                            m_log.WarnFormat("[PHYSICS]: Unable to create physics proxy for object {0}", Name);
+                            return;
+                        }
+                    }
+                }
+                else
+                {
+//                    _parent_scene.waitForSpaceUnlock(m_targetSpace);
+                    try
+                    {
+//Console.WriteLine("  CreateGeom 4");
+                        SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
+                        m_expectedCollisionContacts = 4;
+                    }
+                    catch (AccessViolationException)
+                    {
+                        m_log.WarnFormat("[PHYSICS]: Unable to create physics proxy for object {0}", Name);
+                        return;
+                    }
+                }
+            }
+        }
+
+        /// <summary>
+        /// Remove the existing geom from this prim.
+        /// </summary>
+        /// <param name="m_targetSpace"></param>
+        /// <param name="mesh">If null, then a mesh is used that is based on the profile shape data.</param>
+        /// <returns>true if the geom was successfully removed, false if it was already gone or the remove failed.</returns>
+        internal bool RemoveGeom()
+        {
+            if (prim_geom != IntPtr.Zero)
+            {
+                try
+                {
+                    _parent_scene.geom_name_map.Remove(prim_geom);
+                    _parent_scene.actor_name_map.Remove(prim_geom);
+                    d.GeomDestroy(prim_geom);
+                    m_expectedCollisionContacts = 0;
+                    prim_geom = IntPtr.Zero;
+                }
+                catch (System.AccessViolationException)
+                {
+                    prim_geom = IntPtr.Zero;
+                    m_expectedCollisionContacts = 0;
+                    m_log.ErrorFormat("[PHYSICS]: PrimGeom dead for {0}", Name);
+
+                    return false;
+                }
+
+                return true;
+            }
+            else
+            {
+                m_log.WarnFormat(
+                    "[ODE PRIM]: Called RemoveGeom() on {0} {1} where geometry was already null.", Name, LocalID);
+
+                return false;
+            }
+        }
+        /// <summary>
+        /// Add prim in response to an add taint.
+        /// </summary>
+        private void changeadd()
+        {
+//            m_log.DebugFormat("[ODE PRIM]: Adding prim {0}", Name);
+            
+            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;
+
+            IMesh mesh = null;
+
+            if (_parent_scene.needsMeshing(_pbs))
+            {
+                // Don't need to re-enable body..   it's done in SetMesh
+                mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, _parent_scene.meshSculptLOD, IsPhysical);
+                // createmesh returns null when it's a shape that isn't a cube.
+               // m_log.Debug(m_localID);
+                if (mesh == null)
+                    CheckMeshAsset();
+                else
+                    m_assetFailed = false;
+            }
+
+#if SPAM
+Console.WriteLine("changeadd 1");
+#endif
+            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);
+
+            if (IsPhysical && Body == IntPtr.Zero)
+                enableBody();
+
+            changeSelectedStatus();
+
+            m_taintadd = false;
+        }
+
+        /// <summary>
+        /// Move prim in response to a move taint.
+        /// </summary>
+        private void changemove()
+        {
+            if (IsPhysical)
+            {
+                if (!m_disabled && !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_vehicle.Type != Vehicle.TYPE_NONE)
+                        {
+                            m_vehicle.Enable(Body, _parent_scene);
+                        }
+                    }
+                    else
+                    {
+                        m_log.WarnFormat("[PHYSICS]: Body for {0} still null after enableBody().  This is a crash scenario.", Name);
+                    }
+                }
+                //else
+               // {
+                    //m_log.Debug("[BUG]: race!");
+                //}
+            }
+
+            // 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);
+
+            d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
+
+//                    _parent_scene.waitForSpaceUnlock(m_targetSpace);
+            d.SpaceAdd(m_targetSpace, prim_geom);
+
+            changeSelectedStatus();
+
+            resetCollisionAccounting();
+            m_taintposition = _position;
+        }
+
+        internal void Move(float timestep)
+        {
+            float fx = 0;
+            float fy = 0;
+            float fz = 0;
+
+            if (outofBounds)
+                return;
+
+            if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim)        // KF: Only move root prims.
+            {
+                if (m_vehicle.Type != Vehicle.TYPE_NONE)
+                {
+                    // 'VEHICLES' are dealt with in ODEDynamics.cs
+                    m_vehicle.Step(timestep, _parent_scene);
+                }
+                else
+                {
+//Console.WriteLine("Move " +  Name);
+                    if (!d.BodyIsEnabled (Body))  d.BodyEnable (Body); // KF add 161009
+                    // NON-'VEHICLES' are dealt with here
+//                    if (d.BodyIsEnabled(Body) && !m_angularlock.ApproxEquals(Vector3.Zero, 0.003f))
+//                    {
+//                        d.Vector3 avel2 = d.BodyGetAngularVel(Body);
+//                        /*
+//                        if (m_angularlock.X == 1)
+//                            avel2.X = 0;
+//                        if (m_angularlock.Y == 1)
+//                            avel2.Y = 0;
+//                        if (m_angularlock.Z == 1)
+//                            avel2.Z = 0;
+//                        d.BodySetAngularVel(Body, avel2.X, avel2.Y, avel2.Z);
+//                         */
+//                    }
+                    //float PID_P = 900.0f;
+
+                    float m_mass = CalculateMass();
+
+//                    fz = 0f;
+                    //m_log.Info(m_collisionFlags.ToString());
+
+                    
+                    //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle.
+                    // would come from SceneObjectPart.cs, public void SetBuoyancy(float fvalue) , PhysActor.Buoyancy = fvalue; ??
+                    // m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up 
+                    // gravityz multiplier = 1 - m_buoyancy
+                    fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass;
+
+                    if (PIDActive)
+                    {
+//Console.WriteLine("PID " +  Name);
+                        // KF - this is for object move? eg. llSetPos() ?
+                        //if (!d.BodyIsEnabled(Body))
+                        //d.BodySetForce(Body, 0f, 0f, 0f);
+                        // If we're using the PID controller, then we have no gravity
+                        //fz = (-1 * _parent_scene.gravityz) * m_mass;     //KF: ?? Prims have no global gravity,so simply...
+                        fz = 0f;
+
+                        //  no lock; for now it's only called from within Simulate()
+    
+                        // If the PID Controller isn't active then we set our force
+                        // calculating base velocity to the current position
+
+                        if ((m_PIDTau < 1) && (m_PIDTau != 0))
+                        {
+                            //PID_G = PID_G / m_PIDTau;
+                            m_PIDTau = 1;
+                        }
+    
+                        if ((PID_G - m_PIDTau) <= 0)
+                        {
+                            PID_G = m_PIDTau + 1;
+                        }
+                        //PidStatus = true;
+
+                        // PhysicsVector vec = new PhysicsVector();
+                        d.Vector3 vel = d.BodyGetLinearVel(Body);
+
+                        d.Vector3 pos = d.BodyGetPosition(Body);
+                        _target_velocity =
+                            new Vector3(
+                                (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep),
+                                (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep),
+                                (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep)
+                                );
+
+                        //  if velocity is zero, use position control; otherwise, velocity control
+
+                        if (_target_velocity.ApproxEquals(Vector3.Zero,0.1f))
+                        {
+                            //  keep track of where we stopped.  No more slippin' & slidin'
+    
+                            // We only want to deactivate the PID Controller if we think we want to have our surrogate
+                            // react to the physics scene by moving it's position.
+                            // Avatar to Avatar collisions
+                            // Prim to avatar collisions
+
+                            //fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2);
+                            //fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2);
+                            //fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P;
+                            d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);
+                            d.BodySetLinearVel(Body, 0, 0, 0);
+                            d.BodyAddForce(Body, 0, 0, fz);
+                            return;
+                        }
+                        else
+                        {
+                            _zeroFlag = false;
+
+                            // We're flying and colliding with something
+                            fx = ((_target_velocity.X) - vel.X) * (PID_D);
+                            fy = ((_target_velocity.Y) - vel.Y) * (PID_D);
+    
+                            // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P;
+
+                            fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
+                        }
+                    }        // end if (PIDActive)
+
+                    // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller
+                    if (m_useHoverPID && !PIDActive)
+                    {
+//Console.WriteLine("Hover " +  Name);
+                    
+                        // If we're using the PID controller, then we have no gravity
+                        fz = (-1 * _parent_scene.gravityz) * m_mass;
+
+                        //  no lock; for now it's only called from within Simulate()
+
+                        // If the PID Controller isn't active then we set our force
+                        // calculating base velocity to the current position
+
+                        if ((m_PIDTau < 1))
+                        {
+                            PID_G = PID_G / m_PIDTau;
+                        }
+
+                        if ((PID_G - m_PIDTau) <= 0)
+                        {
+                            PID_G = m_PIDTau + 1;
+                        }
+
+                        // Where are we, and where are we headed?
+                        d.Vector3 pos = d.BodyGetPosition(Body);
+                        d.Vector3 vel = d.BodyGetLinearVel(Body);
+
+                        //    Non-Vehicles have a limited set of Hover options.
+                        // determine what our target height really is based on HoverType
+                        switch (m_PIDHoverType)
+                        {
+                            case PIDHoverType.Ground:
+                                m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
+                                m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
+                                break;
+                            case PIDHoverType.GroundAndWater:
+                                m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
+                                m_waterHeight  = _parent_scene.GetWaterLevel();
+                                if (m_groundHeight > m_waterHeight)
+                                {
+                                    m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
+                                }
+                                else
+                                {
+                                    m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
+                                }
+                                break;
+
+                        }     // end switch (m_PIDHoverType)
+
+
+                        _target_velocity =
+                            new Vector3(0.0f, 0.0f,
+                                (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep)
+                                );
+
+                        //  if velocity is zero, use position control; otherwise, velocity control
+
+                        if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f))
+                        {
+                            //  keep track of where we stopped.  No more slippin' & slidin'
+    
+                            // We only want to deactivate the PID Controller if we think we want to have our surrogate
+                            // react to the physics scene by moving it's position.
+                            // Avatar to Avatar collisions
+                            // Prim to avatar collisions
+
+                            d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight);
+                            d.BodySetLinearVel(Body, vel.X, vel.Y, 0);
+                            d.BodyAddForce(Body, 0, 0, fz);
+                            return;
+                        }
+                        else
+                        {
+                            _zeroFlag = false;
+
+                            // We're flying and colliding with something
+                            fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
+                        }
+                    }
+
+                    fx *= m_mass;
+                    fy *= m_mass;
+                    //fz *= m_mass;
+
+                    fx += m_force.X;
+                    fy += m_force.Y;
+                    fz += m_force.Z;
+
+                    //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString());
+                    if (fx != 0 || fy != 0 || fz != 0)
+                    {
+                        //m_taintdisable = true;
+                        //base.RaiseOutOfBounds(Position);
+                        //d.BodySetLinearVel(Body, fx, fy, 0f);
+                        if (!d.BodyIsEnabled(Body))
+                        {
+                            // A physical body at rest on a surface will auto-disable after a while,
+                            // this appears to re-enable it incase the surface it is upon vanishes,
+                            // and the body should fall again. 
+                            d.BodySetLinearVel(Body, 0f, 0f, 0f);
+                            d.BodySetForce(Body, 0, 0, 0);
+                            enableBodySoft();
+                        }
+
+                        // 35x10 = 350n times the mass per second applied maximum.
+                        float nmax = 35f * m_mass;
+                        float nmin = -35f * m_mass;
+                    
+                        if (fx > nmax)
+                            fx = nmax;
+                        if (fx < nmin)
+                            fx = nmin;
+                        if (fy > nmax)
+                            fy = nmax;
+                        if (fy < nmin)
+                            fy = nmin;
+                        d.BodyAddForce(Body, fx, fy, fz);
+//Console.WriteLine("AddForce " + fx + "," + fy + "," + fz);
+                    }
+                }
+            }
+            else
+            {    // is not physical, or is not a body or is selected
+              //  _zeroPosition = d.BodyGetPosition(Body);
+                return;
+//Console.WriteLine("Nothing " +  Name);
+               
+            }
+        }
+
+        private void rotate()
+        {
+            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);
+                if (IsPhysical)
+                {
+                    if (!m_angularlock.ApproxEquals(Vector3.One, 0f))
+                        createAMotor(m_angularlock);
+                }
+            }
+            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;
+        }
+
+        /// <summary>
+        /// Change prim in response to a disable taint.
+        /// </summary>
+        private void changedisable()
+        {
+            m_disabled = true;
+            if (Body != IntPtr.Zero)
+            {
+                d.BodyDisable(Body);
+                Body = IntPtr.Zero;
+            }
+
+            m_taintdisable = false;
+        }
+
+        /// <summary>
+        /// Change prim in response to a physics status taint
+        /// </summary>
+        private void changePhysicsStatus()
+        {
+            if (IsPhysical)
+            {
+                if (Body == IntPtr.Zero)
+                {
+                    if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim)
+                    {
+                        changeshape();
+                    }
+                    else
+                    {
+                        enableBody();
+                    }
+                }
+            }
+            else
+            {
+                if (Body != IntPtr.Zero)
+                {
+                    if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim)
+                    {
+                        RemoveGeom();
+
+//Console.WriteLine("changePhysicsStatus for " + Name);
+                        changeadd();
+                    }
+
+                    if (childPrim)
+                    {
+                        if (_parent != null)
+                        {
+                            OdePrim parent = (OdePrim)_parent;
+                            parent.ChildDelink(this);
+                        }
+                    }
+                    else
+                    {
+                        disableBody();
+                    }
+                }
+            }
+
+            changeSelectedStatus();
+
+            resetCollisionAccounting();
+            m_taintPhysics = IsPhysical;
+        }
+
+        /// <summary>
+        /// Change prim in response to a size taint.
+        /// </summary>
+        private void changesize()
+        {
+#if SPAM
+            m_log.DebugFormat("[ODE PRIM]: Called changesize");
+#endif
+
+            if (_size.X <= 0) _size.X = 0.01f;
+            if (_size.Y <= 0) _size.Y = 0.01f;
+            if (_size.Z <= 0) _size.Z = 0.01f;
+
+            //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);
+            }
+
+            RemoveGeom();
+
+            // we don't need to do space calculation because the client sends a position update also.
+
+            IMesh mesh = null;
+
+            // Construction of new prim
+            if (_parent_scene.needsMeshing(_pbs))
+            {
+                float meshlod = _parent_scene.meshSculptLOD;
+
+                if (IsPhysical)
+                    meshlod = _parent_scene.MeshSculptphysicalLOD;
+                // Don't need to re-enable body..   it's done in SetMesh
+
+                if (_parent_scene.needsMeshing(_pbs))
+                {
+                    mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, meshlod, IsPhysical);
+                    if (mesh == null)
+                        CheckMeshAsset();
+                    else
+                        m_assetFailed = false;
+                }
+                    
+            }
+
+            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);
+            }
+
+            changeSelectedStatus();
+
+            if (childPrim)
+            {
+                if (_parent is OdePrim)
+                {
+                    OdePrim parent = (OdePrim)_parent;
+                    parent.ChildSetGeom(this);
+                }
+            }
+            resetCollisionAccounting();
+            m_taintsize = _size;
+        }
+
+        /// <summary>
+        /// Change prim in response to a float on water taint.
+        /// </summary>
+        /// <param name="timestep"></param>
+        private void changefloatonwater()
+        {
+            m_collidesWater = m_taintCollidesWater;
+
+            if (m_collidesWater)
+            {
+                m_collisionFlags |= CollisionCategories.Water;
+            }
+            else
+            {
+                m_collisionFlags &= ~CollisionCategories.Water;
+            }
+
+            if (m_assetFailed)
+                d.GeomSetCollideBits(prim_geom, BadMeshAssetCollideBits);
+            else
+
+                d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
+        }
+        /// <summary>
+        /// Change prim in response to a shape taint.
+        /// </summary>
+        private void changeshape()
+        {
+            m_taintshape = false;
+
+            // 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();
+                }
+            }
+
+            RemoveGeom();
+
+            // 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
+
+            IMesh mesh = null;
+
+
+            if (_parent_scene.needsMeshing(_pbs))
+            {
+                // Don't need to re-enable body..   it's done in CreateMesh
+                float meshlod = _parent_scene.meshSculptLOD;
+
+                if (IsPhysical)
+                    meshlod = _parent_scene.MeshSculptphysicalLOD;
+
+                // createmesh returns null when it doesn't mesh.
+                mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, meshlod, IsPhysical);
+                if (mesh == null)
+                    CheckMeshAsset();
+                else
+                    m_assetFailed = false;
+            }
+
+            CreateGeom(m_targetSpace, mesh);
+            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);
+                }
+            }
+
+            changeSelectedStatus();
+
+            if (childPrim)
+            {
+                if (_parent is OdePrim)
+                {
+                    OdePrim parent = (OdePrim)_parent;
+                    parent.ChildSetGeom(this);
+                }
+            }
+
+            resetCollisionAccounting();
+//            m_taintshape = false;
+        }
+
+        /// <summary>
+        /// Change prim in response to an add force taint.
+        /// </summary>
+        private void changeAddForce()
+        {
+            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;
+        }
+
+        /// <summary>
+        /// Change prim in response to a torque taint.
+        /// </summary>
+        private void changeSetTorque()
+        {
+            if (!m_isSelected)
+            {
+                if (IsPhysical && Body != IntPtr.Zero)
+                {
+                    d.BodySetTorque(Body, m_taintTorque.X, m_taintTorque.Y, m_taintTorque.Z);
+                }
+            }
+
+            m_taintTorque = Vector3.Zero;
+        }
+
+        /// <summary>
+        /// Change prim in response to an angular force taint.
+        /// </summary>
+        private void changeAddAngularForce()
+        {
+            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;
+        }
+
+        /// <summary>
+        /// Change prim in response to a velocity taint.
+        /// </summary>
+        private void changevelocity()
+        {
+            if (!m_isSelected)
+            {
+                // Not sure exactly why this sleep is here, but from experimentation it appears to stop an avatar
+                // walking through a default rez size prim if it keeps kicking it around - justincc.
+                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;
+        }
+
+        internal 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;
+//                    m_log.DebugFormat("[PHYSICS]: Set size on {0} to {1}", Name, value);
+                }
+                else
+                {
+                    m_log.WarnFormat("[PHYSICS]: Got NaN Size on object {0}", Name);
+                }
+            }
+        }
+
+        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.WarnFormat("[PHYSICS]: NaN in Force Applied to an Object {0}", Name);
+                }
+            }
+        }
+
+        public override int VehicleType
+        {
+            get { return (int)m_vehicle.Type; }
+            set { m_vehicle.ProcessTypeChange((Vehicle)value); }
+        }
+
+        public override void VehicleFloatParam(int param, float value)
+        {
+            m_vehicle.ProcessFloatVehicleParam((Vehicle) param, value);
+        }
+
+        public override void VehicleVectorParam(int param, Vector3 value)
+        {
+            m_vehicle.ProcessVectorVehicleParam((Vehicle) param, value);
+        }
+
+        public override void VehicleRotationParam(int param, Quaternion rotation)
+        {
+            m_vehicle.ProcessRotationVehicleParam((Vehicle) param, rotation);
+        }
+
+        public override void VehicleFlags(int param, bool remove)
+        {
+            m_vehicle.ProcessVehicleFlags(param, remove);
+        }
+
+        public override void SetVolumeDetect(int param)
+        {
+            // We have to lock the scene here so that an entire simulate loop either uses volume detect for all
+            // possible collisions with this prim or for none of them.
+            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_assetFailed = false;
+                m_taintshape = true;
+            }
+        }
+
+        public override Vector3 Velocity
+        {
+            get
+            {
+                // Average 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) * 0.5f; // 0.5f is mathematically equiv to '/ 2'
+                returnVelocity.Y = (m_lastVelocity.Y + _velocity.Y) * 0.5f;
+                returnVelocity.Z = (m_lastVelocity.Z + _velocity.Z) * 0.5f;
+                return returnVelocity;
+            }
+            set
+            {
+                if (value.IsFinite())
+                {
+                    _velocity = value;
+
+                    m_taintVelocity = value;
+                    _parent_scene.AddPhysicsActorTaint(this);
+                }
+                else
+                {
+                    m_log.WarnFormat("[PHYSICS]: Got NaN Velocity in Object {0}", Name);
+                }
+
+            }
+        }
+
+        public override Vector3 Torque
+        {
+            get
+            {
+                if (!IsPhysical || Body == IntPtr.Zero)
+                    return Vector3.Zero;
+
+                return _torque;
+            }
+
+            set
+            {
+                if (value.IsFinite())
+                {
+                    m_taintTorque = value;
+                    _parent_scene.AddPhysicsActorTaint(this);
+                }
+                else
+                {
+                    m_log.WarnFormat("[PHYSICS]: Got NaN Torque in Object {0}", Name);
+                }
+            }
+        }
+
+        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.WarnFormat("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object {0}", Name);
+            }
+        }
+
+        private 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
+        {
+            get { return _acceleration; }
+            set { _acceleration = value; }
+        }
+
+        public override void AddForce(Vector3 force, bool pushforce)
+        {
+            if (force.IsFinite())
+            {
+                lock (m_forcelist)
+                    m_forcelist.Add(force);
+
+                m_taintforce = true;
+            }
+            else
+            {
+                m_log.WarnFormat("[PHYSICS]: Got Invalid linear force vector from Scene in Object {0}", Name);
+            }
+            //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.WarnFormat("[PHYSICS]: Got Invalid Angular force vector from Scene in Object {0}", Name);
+            }
+        }
+
+        public override Vector3 RotationalVelocity
+        {
+            get
+            {
+                Vector3 pv = Vector3.Zero;
+                if (_zeroFlag)
+                    return pv;
+                m_lastUpdateSent = false;
+
+                if (m_rotationalVelocity.ApproxEquals(pv, 0.2f))
+                    return pv;
+
+                return m_rotationalVelocity;
+            }
+            set
+            {
+                if (value.IsFinite())
+                {
+                    m_rotationalVelocity = value;
+                    setAngularVelocity(value.X, value.Y, value.Z);
+                }
+                else
+                {
+                    m_log.WarnFormat("[PHYSICS]: Got NaN RotationalVelocity in Object {0}", Name);
+                }
+            }
+        }
+
+        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: " + Name);
+                        }
+            */
+            _position.X = Util.Clip(_position.X, 0.5f, _parent_scene.WorldExtents.X - 0.5f);
+            _position.Y = Util.Clip(_position.Y, 0.5f, _parent_scene.WorldExtents.Y - 0.5f);
+            _position.Z = Util.Clip(_position.Z + 0.2f, -100f, 50000f);
+
+            m_lastposition = _position;
+            _velocity.X = 0;
+            _velocity.Y = 0;
+            _velocity.Z = 0;
+
+            m_lastVelocity = _velocity;
+
+            if (Body != IntPtr.Zero)
+            {
+                d.BodySetLinearVel(Body, 0, 0, 0); // stop it
+                d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
+            }
+
+            outofBounds = false;
+            base.RequestPhysicsterseUpdate();
+
+        }
+
+        public override float Buoyancy
+        {
+            get { return m_buoyancy; }
+            set { m_buoyancy = value; }
+        }
+
+        public override void link(PhysicsActor obj)
+        {
+            m_taintparent = obj;
+        }
+
+        public override void delink()
+        {
+            m_taintparent = null;
+        }
+
+        public override void LockAngularMotion(Vector3 axis)
+        {
+            // reverse the zero/non zero values for ODE.
+            if (axis.IsFinite())
+            {
+                axis.X = (axis.X > 0) ? 1f : 0f;
+                axis.Y = (axis.Y > 0) ? 1f : 0f;
+                axis.Z = (axis.Z > 0) ? 1f : 0f;
+                m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z);
+                m_taintAngularLock = axis;
+            }
+            else
+            {
+                m_log.WarnFormat("[PHYSICS]: Got NaN locking axis from Scene on Object {0}", Name);
+            }
+        }
+
+        internal void UpdatePositionAndVelocity()
+        {
+            //  no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit!
+            if (outofBounds)
+                return;
+            if (_parent == null)
+            {
+                Vector3 pv = Vector3.Zero;
+                bool lastZeroFlag = _zeroFlag;
+                float m_minvelocity = 0;
+                if (Body != (IntPtr)0) // FIXME -> or if it is a joint
+                {
+                    d.Vector3 vec = d.BodyGetPosition(Body);
+                    d.Quaternion ori = d.BodyGetQuaternion(Body);
+                    d.Vector3 vel = d.BodyGetLinearVel(Body);
+                    d.Vector3 rotvel = d.BodyGetAngularVel(Body);
+                    d.Vector3 torque = d.BodyGetTorque(Body);
+                    _torque = new Vector3(torque.X, torque.Y, torque.Z);
+                    Vector3 l_position = Vector3.Zero;
+                    Quaternion l_orientation = Quaternion.Identity;
+
+                    m_lastposition = _position;
+                    m_lastorientation = _orientation;
+
+                    l_position.X = vec.X;
+                    l_position.Y = vec.Y;
+                    l_position.Z = vec.Z;
+                    l_orientation.X = ori.X;
+                    l_orientation.Y = ori.Y;
+                    l_orientation.Z = ori.Z;
+                    l_orientation.W = ori.W;
+
+                    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;
+
+                        _acceleration.X = 0;
+                        _acceleration.Y = 0;
+                        _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.RaiseOutOfBounds(_position);
+
+                        if (_parent == null)
+                            base.RequestPhysicsterseUpdate();
+
+                        m_throttleUpdates = false;
+                        throttleCounter = 0;
+                        _zeroFlag = true;
+                        //outofBounds = true;
+                        return;
+                    }
+
+                    if (l_position.X > ((int)_parent_scene.WorldExtents.X - 0.05f) || l_position.X < 0f || l_position.Y > ((int)_parent_scene.WorldExtents.Y - 0.05f) || l_position.Y < 0f)
+                    {
+                        //base.RaiseOutOfBounds(l_position);
+                        /*
+                                                if (m_crossingfailures < _parent_scene.geomCrossingFailuresBeforeOutofbounds)
+                                                {
+                                                    _position = l_position;
+                                                    //_parent_scene.remActivePrim(this);
+                                                    if (_parent == null)
+                                                        base.RequestPhysicsterseUpdate();
+                                                    return;
+                                                }
+                                                else
+                                                {
+                                                    if (_parent == null)
+                                                        base.RaiseOutOfBounds(l_position);
+                                                    return;
+                                                }
+                        */
+                        outofBounds = true;
+                        // part near the border on outside
+                        if (l_position.X < 0)
+                            Util.Clamp(l_position.X, -0.1f, -2f);
+                        else
+                            Util.Clamp(l_position.X, _parent_scene.WorldExtents.X + 0.1f, _parent_scene.WorldExtents.X + 2f);
+                        if (l_position.Y < 0)
+                            Util.Clamp(l_position.Y, -0.1f, -2f);
+                        else
+                            Util.Clamp(l_position.Y, _parent_scene.WorldExtents.Y + 0.1f, _parent_scene.WorldExtents.Y + 2f);
+
+                        d.BodySetPosition(Body, l_position.X, l_position.Y, l_position.Z);
+
+                        // stop it
+                        d.BodySetAngularVel(Body, 0, 0, 0);
+                        d.BodySetLinearVel(Body, 0, 0, 0);
+                        disableBodySoft();
+
+                        // tell framework to fix it
+                        if (_parent == null)
+                            base.RequestPhysicsterseUpdate();
+                        return;
+                    }
+
+
+                    //float Adiff = 1.0f - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation));
+                    //Console.WriteLine("Adiff " + Name + " = " + Adiff);
+                    if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02)
+                        && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02)
+                        && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02)
+//                        && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.01))
+                        && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.0001))  // KF 0.01 is far to large
+                    {
+                        _zeroFlag = true;
+//Console.WriteLine("ZFT 2");
+                        m_throttleUpdates = false;
+                    }
+                    else
+                    {
+                        //m_log.Debug(Math.Abs(m_lastposition.X - l_position.X).ToString());
+                        _zeroFlag = false;
+                        m_lastUpdateSent = false;
+                        //m_throttleUpdates = false;
+                    }
+
+                    if (_zeroFlag)
+                    {
+                        _velocity.X = 0.0f;
+                        _velocity.Y = 0.0f;
+                        _velocity.Z = 0.0f;
+
+                        _acceleration.X = 0;
+                        _acceleration.Y = 0;
+                        _acceleration.Z = 0;
+
+                        //_orientation.w = 0f;
+                        //_orientation.X = 0f;
+                        //_orientation.Y = 0f;
+                        //_orientation.Z = 0f;
+                        m_rotationalVelocity.X = 0;
+                        m_rotationalVelocity.Y = 0;
+                        m_rotationalVelocity.Z = 0;
+                        if (!m_lastUpdateSent)
+                        {
+                            m_throttleUpdates = false;
+                            throttleCounter = 0;
+                            m_rotationalVelocity = pv;
+
+                            if (_parent == null)
+                            {
+                                base.RequestPhysicsterseUpdate();
+                            }
+
+                            m_lastUpdateSent = true;
+                        }
+                    }
+                    else
+                    {
+                        if (lastZeroFlag != _zeroFlag)
+                        {
+                            if (_parent == null)
+                            {
+                                base.RequestPhysicsterseUpdate();
+                            }
+                        }
+
+                        m_lastVelocity = _velocity;
+
+                        _position = l_position;
+
+                        _velocity.X = vel.X;
+                        _velocity.Y = vel.Y;
+                        _velocity.Z = vel.Z;
+
+                        _acceleration = ((_velocity - m_lastVelocity) / 0.1f);
+                        _acceleration = new Vector3(_velocity.X - m_lastVelocity.X / 0.1f, _velocity.Y - m_lastVelocity.Y / 0.1f, _velocity.Z - m_lastVelocity.Z / 0.1f);
+                        //m_log.Info("[PHYSICS]: V1: " + _velocity + " V2: " + m_lastVelocity + " Acceleration: " + _acceleration.ToString());
+                       
+                        // Note here that linearvelocity is affecting angular velocity...  so I'm guessing this is a vehicle specific thing... 
+                        // it does make sense to do this for tiny little instabilities with physical prim, however 0.5m/frame is fairly large. 
+                        // reducing this to 0.02m/frame seems to help the angular rubberbanding quite a bit, however, to make sure it doesn't affect elevators and vehicles
+                        // adding these logical exclusion situations to maintain this where I think it was intended to be.
+                        if (m_throttleUpdates || PIDActive || (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) || (Amotor != IntPtr.Zero)) 
+                        {
+                            m_minvelocity = 0.5f;
+                        }
+                        else
+                        {
+                            m_minvelocity = 0.02f;
+                        }
+
+                        if (_velocity.ApproxEquals(pv, m_minvelocity))
+                        {
+                            m_rotationalVelocity = pv;
+                        }
+                        else
+                        {
+                            m_rotationalVelocity = new Vector3(rotvel.X, rotvel.Y, rotvel.Z);
+                        }
+
+                        //m_log.Debug("ODE: " + m_rotationalVelocity.ToString());
+                        _orientation.X = ori.X;
+                        _orientation.Y = ori.Y;
+                        _orientation.Z = ori.Z;
+                        _orientation.W = ori.W;
+                        m_lastUpdateSent = false;
+                        if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate)
+                        {
+                            if (_parent == null)
+                            {
+                                base.RequestPhysicsterseUpdate();
+                            }
+                        }
+                        else
+                        {
+                            throttleCounter++;
+                        }
+                    }
+                    m_lastposition = l_position;
+                }
+                else
+                {
+                    // 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;
+                }
+            }
+        }
+
+        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.WarnFormat("[PHYSICS]: Got NaN PIDTarget from Scene on Object {0}", Name);
+            } 
+        }
+
+        public override bool PIDActive { get; set; }
+        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; } }
+        public override float PIDHoverTau { set { m_PIDHoverTau = value; } }
+        
+        public override Quaternion APIDTarget{ set { return; } }
+
+        public override bool APIDActive{ set { return; } }
+
+        public override float APIDStrength{ set { return; } }
+
+        public override float APIDDamping{ set { return; } }
+
+        private void createAMotor(Vector3 axis)
+        {
+            if (Body == IntPtr.Zero)
+                return;
+
+            if (Amotor != IntPtr.Zero)
+            {
+                d.JointDestroy(Amotor);
+                Amotor = IntPtr.Zero;
+            }
+
+            float axisnum = 3;
+
+            axisnum = (axisnum - (axis.X + axis.Y + axis.Z));
+
+            // PhysicsVector totalSize = new PhysicsVector(_size.X, _size.Y, _size.Z);
+
+            
+            // Inverse Inertia Matrix, set the X, Y, and/r Z inertia to 0 then invert it again.
+            d.Mass objMass;
+            d.MassSetZero(out objMass);
+            DMassCopy(ref pMass, ref objMass);
+
+            //m_log.DebugFormat("1-{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, ", objMass.I.M00, objMass.I.M01, objMass.I.M02, objMass.I.M10, objMass.I.M11, objMass.I.M12, objMass.I.M20, objMass.I.M21, objMass.I.M22);
+
+            Matrix4 dMassMat = FromDMass(objMass);
+
+            Matrix4 mathmat = Inverse(dMassMat);
+
+            /*
+            //m_log.DebugFormat("2-{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, ", mathmat[0, 0], mathmat[0, 1], mathmat[0, 2], mathmat[1, 0], mathmat[1, 1], mathmat[1, 2], mathmat[2, 0], mathmat[2, 1], mathmat[2, 2]);
+
+            mathmat = Inverse(mathmat);
+
+
+            objMass = FromMatrix4(mathmat, ref objMass);
+            //m_log.DebugFormat("3-{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, ", objMass.I.M00, objMass.I.M01, objMass.I.M02, objMass.I.M10, objMass.I.M11, objMass.I.M12, objMass.I.M20, objMass.I.M21, objMass.I.M22);
+
+            mathmat = Inverse(mathmat);
+            */
+            if (axis.X == 0)
+            {
+                mathmat.M33 = 50.0000001f;
+                //objMass.I.M22 = 0;
+            }
+            if (axis.Y == 0)
+            {
+                mathmat.M22 = 50.0000001f;
+                //objMass.I.M11 = 0;
+            }
+            if (axis.Z == 0)
+            {
+                mathmat.M11 = 50.0000001f;
+                //objMass.I.M00 = 0;
+            }
+            
+            
+
+            mathmat = Inverse(mathmat);
+            objMass = FromMatrix4(mathmat, ref objMass);
+            //m_log.DebugFormat("4-{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, ", objMass.I.M00, objMass.I.M01, objMass.I.M02, objMass.I.M10, objMass.I.M11, objMass.I.M12, objMass.I.M20, objMass.I.M21, objMass.I.M22);
+           
+            //return;
+            if (d.MassCheck(ref objMass))
+            {
+                d.BodySetMass(Body, ref objMass);
+            }
+            else
+            {
+                //m_log.Debug("[PHYSICS]: Mass invalid, ignoring");
+            }
+
+            if (axisnum <= 0)
+                return;
+            // int dAMotorEuler = 1;
+
+            Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero);
+            d.JointAttach(Amotor, Body, IntPtr.Zero);
+            d.JointSetAMotorMode(Amotor, 0);
+
+            d.JointSetAMotorNumAxes(Amotor,(int)axisnum);
+            int i = 0;
+
+            if (axis.X == 0)
+            {
+                d.JointSetAMotorAxis(Amotor, i, 0, 1, 0, 0);
+                i++;
+            }
+
+            if (axis.Y == 0)
+            {
+                d.JointSetAMotorAxis(Amotor, i, 0, 0, 1, 0);
+                i++;
+            }
+
+            if (axis.Z == 0)
+            {
+                d.JointSetAMotorAxis(Amotor, i, 0, 0, 0, 1);
+                i++;
+            }
+
+            for (int j = 0; j < (int)axisnum; j++)
+            {
+                //d.JointSetAMotorAngle(Amotor, j, 0);
+            }
+
+            //d.JointSetAMotorAngle(Amotor, 1, 0);
+            //d.JointSetAMotorAngle(Amotor, 2, 0);
+
+            // 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, 9000f);
+            d.JointSetAMotorParam(Amotor, (int)dParam.FudgeFactor, 0f);
+            d.JointSetAMotorParam(Amotor, (int)dParam.FMax, Mass * 50f);//
+        }
+
+        private Matrix4 FromDMass(d.Mass pMass)
+        {
+            Matrix4 obj;
+            obj.M11 = pMass.I.M00;
+            obj.M12 = pMass.I.M01;
+            obj.M13 = pMass.I.M02;
+            obj.M14 = 0;
+            obj.M21 = pMass.I.M10;
+            obj.M22 = pMass.I.M11;
+            obj.M23 = pMass.I.M12;
+            obj.M24 = 0;
+            obj.M31 = pMass.I.M20;
+            obj.M32 = pMass.I.M21;
+            obj.M33 = pMass.I.M22;
+            obj.M34 = 0;
+            obj.M41 = 0;
+            obj.M42 = 0;
+            obj.M43 = 0;
+            obj.M44 = 1;
+            return obj;
+        }
+
+        private 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.RemoveCollisionEventReporting(this);
+            m_eventsubscription = 0;
+        }
+
+        public void AddCollisionEvent(uint CollidedWith, ContactPoint contact)
+        {
+            CollisionEventsThisFrame.AddCollider(CollidedWith, contact);
+        }
+
+        public void SendCollisions()
+        {
+            if (m_collisionsOnPreviousFrame || CollisionEventsThisFrame.Count > 0)
+            {
+                base.SendCollisionUpdate(CollisionEventsThisFrame);
+
+                if (CollisionEventsThisFrame.Count > 0)
+                {
+                    m_collisionsOnPreviousFrame = true;
+                    CollisionEventsThisFrame.Clear();
+                }
+                else
+                {
+                    m_collisionsOnPreviousFrame = false;
+                }
+            }
+        }
+
+        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;
+        }
+
+        private void CheckMeshAsset()
+        {
+            if (_pbs.SculptEntry && !m_assetFailed && _pbs.SculptTexture != UUID.Zero)
+            {
+                m_assetFailed = true;
+                Util.FireAndForget(delegate
+                    {
+                        RequestAssetDelegate assetProvider = _parent_scene.RequestAssetMethod;
+                        if (assetProvider != null)
+                            assetProvider(_pbs.SculptTexture, MeshAssetReceived);
+                    }, null, "ODEPrim.CheckMeshAsset");
+            }
+        }
+
+        private void MeshAssetReceived(AssetBase asset)
+        {
+            if (asset != null && asset.Data != null && asset.Data.Length > 0)
+            {
+                if (!_pbs.SculptEntry)
+                    return;
+                if (_pbs.SculptTexture.ToString() != asset.ID)
+                    return;
+
+                _pbs.SculptData = new byte[asset.Data.Length];
+                asset.Data.CopyTo(_pbs.SculptData, 0);
+//                m_assetFailed = false;
+
+//                m_log.DebugFormat(
+//                    "[ODE PRIM]: Received mesh/sculpt data asset {0} with {1} bytes for {2} at {3} in {4}", 
+//                    _pbs.SculptTexture, _pbs.SculptData.Length, Name, _position, _parent_scene.Name);
+
+                m_taintshape = true;
+               _parent_scene.AddPhysicsActorTaint(this);
+            }
+            else
+            {
+                m_log.WarnFormat(
+                    "[ODE PRIM]: Could not get mesh/sculpt asset {0} for {1} at {2} in {3}",
+                    _pbs.SculptTexture, Name, _position, _parent_scene.PhysicsSceneName);
+            }
+        }          
+    }
+}
\ No newline at end of file
diff --git a/OpenSim/Region/PhysicsModules/Ode/ODERayCastRequestManager.cs b/OpenSim/Region/PhysicsModules/Ode/ODERayCastRequestManager.cs
new file mode 100644
index 0000000..80f0fcf
--- /dev/null
+++ b/OpenSim/Region/PhysicsModules/Ode/ODERayCastRequestManager.cs
@@ -0,0 +1,446 @@
+/*
+ * 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.PhysicsModules.SharedBase;
+using Ode.NET;
+using log4net;
+
+namespace OpenSim.Region.PhysicsModule.ODE
+{
+    /// <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>
+        /// Pending ray requests
+        /// </summary>
+        protected List<ODERayRequest> m_PendingRayRequests = new List<ODERayRequest>();
+
+        /// <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>
+        /// 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="count"></param>
+        /// <param name="retMethod">Return method to send the results</param>
+        public void QueueRequest(Vector3 position, Vector3 direction, float length, int count, RayCallback retMethod)
+        {
+            lock (m_PendingRequests)
+            {
+                ODERayRequest req = new ODERayRequest();
+                req.callbackMethod = retMethod;
+                req.length = length;
+                req.Normal = direction;
+                req.Origin = position;
+                req.Count = count;
+
+                m_PendingRayRequests.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
+                    }
+
+                    m_PendingRequests.Clear();
+                }
+            }
+
+            lock (m_PendingRayRequests)
+            {
+                if (m_PendingRayRequests.Count > 0)
+                {
+                    ODERayRequest[] reqs = m_PendingRayRequests.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
+                    }
+
+                    m_PendingRayRequests.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)
+        {
+            // NOTE: limit ray lenght or collisions will take all avaiable stack space
+            // this value may still be too large, depending on machine configuration
+            // of maximum stack
+            float len = req.length;
+            if (len > 250f)
+                len = 250f;
+
+            // Create the ray
+            IntPtr ray = d.CreateRay(m_scene.space, len);
+            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);
+        }
+
+        /// <summary>
+        /// Method that actually initiates the raycast
+        /// </summary>
+        /// <param name="req"></param>
+        private void RayCast(ODERayRequest req)
+        {
+            // limit ray lenght or collisions will take all avaiable stack space
+            float len = req.length;
+            if (len > 250f)
+                len = 250f;
+
+            // Create the ray
+            IntPtr ray = d.CreateRay(m_scene.space, len);
+            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);
+
+            // Find closest contact and object.
+            lock (m_contactResults)
+            {
+                // Return results
+                if (req.callbackMethod != null)
+                    req.callbackMethod(m_contactResults);
+            }
+        }
+        
+        // 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 = p1.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 = p2.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 ODERayRequest
+    {
+        public Vector3 Origin;
+        public Vector3 Normal;
+        public int Count;
+        public float length;
+        public RayCallback callbackMethod;
+    }
+}
diff --git a/OpenSim/Region/PhysicsModules/Ode/OdePhysicsJoint.cs b/OpenSim/Region/PhysicsModules/Ode/OdePhysicsJoint.cs
new file mode 100644
index 0000000..2eb7ba6
--- /dev/null
+++ b/OpenSim/Region/PhysicsModules/Ode/OdePhysicsJoint.cs
@@ -0,0 +1,48 @@
+/*
+ * 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 OpenMetaverse;
+using Ode.NET;
+using OpenSim.Framework;
+using OpenSim.Region.PhysicsModules.SharedBase;
+using OpenSim.Region.PhysicsModule.ODE;
+
+namespace OpenSim.Region.PhysicsModule.ODE
+{
+    class OdePhysicsJoint : PhysicsJoint
+    {
+        public override bool IsInPhysicsEngine
+        {
+            get
+            {
+                return (jointID != IntPtr.Zero);
+            }
+        }
+        public IntPtr jointID;
+    }
+}
diff --git a/OpenSim/Region/PhysicsModules/Ode/OdeScene.cs b/OpenSim/Region/PhysicsModules/Ode/OdeScene.cs
new file mode 100644
index 0000000..b00a5ab
--- /dev/null
+++ b/OpenSim/Region/PhysicsModules/Ode/OdeScene.cs
@@ -0,0 +1,4381 @@
+/*
+ * 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.
+ */
+
+// changes for varsize regions
+// note that raycasts need to have limited range
+// (even in normal regions)
+// or aplication thread stack may just blowup
+// see RayCast(ODERayCastRequest req)
+
+//#define USE_DRAWSTUFF
+//#define SPAM
+
+using System;
+using System.Collections.Generic;
+using System.Diagnostics;
+using System.IO;
+using System.Linq;
+using System.Reflection;
+using System.Runtime.InteropServices;
+using System.Threading;
+using log4net;
+using Nini.Config;
+using Mono.Addins;
+using Ode.NET;
+using OpenMetaverse;
+#if USE_DRAWSTUFF
+using Drawstuff.NET;
+#endif 
+using OpenSim.Framework;
+using OpenSim.Region.PhysicsModules.SharedBase;
+using OpenSim.Region.Framework.Scenes;
+using OpenSim.Region.Framework.Interfaces;
+
+
+namespace OpenSim.Region.PhysicsModule.ODE
+{
+    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
+    }
+
+    [Extension(Path = "/OpenSim/RegionModules", NodeName = "RegionModule", Id = "ODEPhysicsScene")]
+    public class OdeScene : PhysicsScene, INonSharedRegionModule
+    {
+        private readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType.ToString());
+        private bool m_Enabled = false;
+
+        // private Dictionary<string, sCollisionData> m_storedCollisions = new Dictionary<string, sCollisionData>();
+
+        /// <summary>
+        /// Provide a sync object so that only one thread calls d.Collide() at a time across all OdeScene instances.
+        /// </summary>
+        /// <remarks>
+        /// With ODE as of r1755 (though also tested on r1860), only one thread can call d.Collide() at a
+        /// time, even where physics objects are in entirely different ODE worlds.  This is because generating contacts
+        /// uses a static cache at the ODE level.
+        ///
+        /// Without locking, simulators running multiple regions will eventually crash with a native stack trace similar
+        /// to
+        ///
+        /// mono() [0x489171]
+        /// mono() [0x4d154f]
+        /// /lib/x86_64-linux-gnu/libpthread.so.0(+0xfc60) [0x7f6ded592c60]
+        /// .../opensim/bin/libode-x86_64.so(_ZN6Opcode11OBBCollider8_CollideEPKNS_14AABBNoLeafNodeE+0xd7a) [0x7f6dd822628a]
+        ///
+        /// ODE provides an experimental option to cache in thread local storage but compiling ODE with this option
+        /// causes OpenSimulator to immediately crash with a native stack trace similar to
+        ///
+        /// mono() [0x489171]
+        /// mono() [0x4d154f]
+        /// /lib/x86_64-linux-gnu/libpthread.so.0(+0xfc60) [0x7f03c9849c60]
+        /// .../opensim/bin/libode-x86_64.so(_Z12dCollideCCTLP6dxGeomS0_iP12dContactGeomi+0x92) [0x7f03b44bcf82]
+        /// </remarks>
+        internal static Object UniversalColliderSyncObject = new Object();
+
+        /// <summary>
+        /// Is stats collecting enabled for this ODE scene?
+        /// </summary>
+        public bool CollectStats { get; set; }
+
+        /// <summary>
+        /// Statistics for this scene.
+        /// </summary>
+        private Dictionary<string, float> m_stats = new Dictionary<string, float>();
+
+        /// <summary>
+        /// Stat name for total number of avatars in this ODE scene.
+        /// </summary>
+        public const string ODETotalAvatarsStatName = "ODETotalAvatars";
+
+        /// <summary>
+        /// Stat name for total number of prims in this ODE scene.
+        /// </summary>
+        public const string ODETotalPrimsStatName = "ODETotalPrims";
+
+        /// <summary>
+        /// Stat name for total number of prims with active physics in this ODE scene.
+        /// </summary>
+        public const string ODEActivePrimsStatName = "ODEActivePrims";
+
+        /// <summary>
+        /// Stat name for the total time spent in ODE frame processing.
+        /// </summary>
+        /// <remarks>
+        /// A sanity check for the main scene loop physics time.
+        /// </remarks>
+        public const string ODETotalFrameMsStatName = "ODETotalFrameMS";
+
+        /// <summary>
+        /// Stat name for time spent processing avatar taints per frame
+        /// </summary>
+        public const string ODEAvatarTaintMsStatName = "ODEAvatarTaintFrameMS";
+
+        /// <summary>
+        /// Stat name for time spent processing prim taints per frame
+        /// </summary>
+        public const string ODEPrimTaintMsStatName = "ODEPrimTaintFrameMS";
+
+        /// <summary>
+        /// Stat name for time spent calculating avatar forces per frame.
+        /// </summary>
+        public const string ODEAvatarForcesFrameMsStatName = "ODEAvatarForcesFrameMS";
+
+        /// <summary>
+        /// Stat name for time spent calculating prim forces per frame
+        /// </summary>
+        public const string ODEPrimForcesFrameMsStatName = "ODEPrimForcesFrameMS";
+
+        /// <summary>
+        /// Stat name for time spent fulfilling raycasting requests per frame
+        /// </summary>
+        public const string ODERaycastingFrameMsStatName = "ODERaycastingFrameMS";
+
+        /// <summary>
+        /// Stat name for time spent in native code that actually steps through the simulation.
+        /// </summary>
+        public const string ODENativeStepFrameMsStatName = "ODENativeStepFrameMS";
+
+        /// <summary>
+        /// Stat name for the number of milliseconds that ODE spends in native space collision code.
+        /// </summary>
+        public const string ODENativeSpaceCollisionFrameMsStatName = "ODENativeSpaceCollisionFrameMS";
+
+        /// <summary>
+        /// Stat name for milliseconds that ODE spends in native geom collision code.
+        /// </summary>
+        public const string ODENativeGeomCollisionFrameMsStatName = "ODENativeGeomCollisionFrameMS";
+
+        /// <summary>
+        /// Time spent in collision processing that is not spent in native space or geom collision code.
+        /// </summary>
+        public const string ODEOtherCollisionFrameMsStatName = "ODEOtherCollisionFrameMS";
+
+        /// <summary>
+        /// Stat name for time spent notifying listeners of collisions
+        /// </summary>
+        public const string ODECollisionNotificationFrameMsStatName = "ODECollisionNotificationFrameMS";
+
+        /// <summary>
+        /// Stat name for milliseconds spent updating avatar position and velocity
+        /// </summary>
+        public const string ODEAvatarUpdateFrameMsStatName = "ODEAvatarUpdateFrameMS";
+
+        /// <summary>
+        /// Stat name for the milliseconds spent updating prim position and velocity
+        /// </summary>
+        public const string ODEPrimUpdateFrameMsStatName = "ODEPrimUpdateFrameMS";
+
+        /// <summary>
+        /// Stat name for avatar collisions with another entity.
+        /// </summary>
+        public const string ODEAvatarContactsStatsName = "ODEAvatarContacts";
+
+        /// <summary>
+        /// Stat name for prim collisions with another entity.
+        /// </summary>
+        public const string ODEPrimContactsStatName = "ODEPrimContacts";
+
+        /// <summary>
+        /// Used to hold tick numbers for stat collection purposes.
+        /// </summary>
+        private int m_nativeCollisionStartTick;
+
+        /// <summary>
+        /// A messy way to tell if we need to avoid adding a collision time because this was already done in the callback.
+        /// </summary>
+        private bool m_inCollisionTiming;
+
+        /// <summary>
+        /// A temporary holder for the number of avatar collisions in a frame, so we can work out how many object
+        /// collisions occured using the _perloopcontact if stats collection is enabled.
+        /// </summary>
+        private int m_tempAvatarCollisionsThisFrame;
+
+        /// <summary>
+        /// Used in calculating physics frame time dilation
+        /// </summary>
+        private int tickCountFrameRun;
+
+        /// <summary>
+        /// Used in calculating physics frame time dilation
+        /// </summary>
+        private int latertickcount;
+
+        private Random fluidRandomizer = new Random(Environment.TickCount);
+
+        public bool m_suportCombine = true;
+
+        private uint m_regionWidth = Constants.RegionSize;
+        private uint m_regionHeight = Constants.RegionSize;
+
+        private float ODE_STEPSIZE = 0.0178f;
+        private float metersInSpace = 29.9f;
+        private float m_timeDilation = 1.0f;
+
+        public float gravityx = 0f;
+        public float gravityy = 0f;
+        public float gravityz = -9.8f;
+
+        public float AvatarTerminalVelocity { get; set; }
+
+        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 IntPtr contactgroup;
+
+//        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;
+
+        /// <summary>
+        /// true = old compatibility mode with leaning capsule; false = new corrected mode
+        /// </summary>
+        /// <remarks>
+        /// Even when set to false, the capsule still tilts but this is done in a different way.
+        /// </remarks>
+        public bool IsAvCapsuleTilted { get; private set; }
+
+        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;
+        private const int avatarExpectedContacts = 3;
+
+        public float bodyPIDD = 35f;
+        public float bodyPIDG = 25;
+
+        public int geomCrossingFailuresBeforeOutofbounds = 5;
+
+        public float bodyMotorJointMaxforceTensor = 2;
+
+        public int bodyFramesAutoDisable = 20;
+
+        private float[] _watermap;
+        private bool m_filterCollisions = true;
+
+        private d.NearCallback nearCallback;
+        public d.TriCallback triCallback;
+        public d.TriArrayCallback triArrayCallback;
+
+        /// <summary>
+        /// Avatars in the physics scene.
+        /// </summary>
+        private readonly HashSet<OdeCharacter> _characters = new HashSet<OdeCharacter>();
+
+        /// <summary>
+        /// Prims in the physics scene.
+        /// </summary>
+        private readonly HashSet<OdePrim> _prims = new HashSet<OdePrim>();
+
+        /// <summary>
+        /// Prims in the physics scene that are subject to physics, not just collisions.
+        /// </summary>
+        private readonly HashSet<OdePrim> _activeprims = new HashSet<OdePrim>();
+
+        /// <summary>
+        /// Prims that the simulator has created/deleted/updated and so need updating in ODE.
+        /// </summary>
+        private readonly HashSet<OdePrim> _taintedPrims = new HashSet<OdePrim>();
+
+        /// <summary>
+        /// Record a character that has taints to be processed.
+        /// </summary>
+        private readonly HashSet<OdeCharacter> _taintedActors = new HashSet<OdeCharacter>();
+
+        /// <summary>
+        /// Keep record of contacts in the physics loop so that we can remove duplicates.
+        /// </summary>
+        private readonly List<d.ContactGeom> _perloopContact = new List<d.ContactGeom>();
+
+        /// <summary>
+        /// A dictionary of actors that should receive collision events.
+        /// </summary>
+        private readonly Dictionary<uint, PhysicsActor> m_collisionEventActors = new Dictionary<uint, PhysicsActor>();
+
+        /// <summary>
+        /// A dictionary of collision event changes that are waiting to be processed.
+        /// </summary>
+        private readonly Dictionary<uint, PhysicsActor> m_collisionEventActorsChanges = new Dictionary<uint, PhysicsActor>();
+
+        /// <summary>
+        /// Maps a unique geometry id (a memory location) to a physics actor name.
+        /// </summary>
+        /// <remarks>
+        /// Only actors participating in collisions have geometries.  This has to be maintained separately from
+        /// actor_name_map because terrain and water currently don't conceptually have a physics actor of their own
+        /// apart from the singleton PANull
+        /// </remarks>
+        public Dictionary<IntPtr, String> geom_name_map = new Dictionary<IntPtr, String>();
+
+        /// <summary>
+        /// Maps a unique geometry id (a memory location) to a physics actor.
+        /// </summary>
+        /// <remarks>
+        /// Only actors participating in collisions have geometries.
+        /// </remarks>
+        public Dictionary<IntPtr, PhysicsActor> actor_name_map = new Dictionary<IntPtr, PhysicsActor>();
+
+        /// <summary>
+        /// Defects list to remove characters that no longer have finite positions due to some other bug.
+        /// </summary>
+        /// <remarks>
+        /// Used repeatedly in Simulate() but initialized once here.
+        /// </remarks>
+        private readonly List<OdeCharacter> defects = new List<OdeCharacter>();
+
+        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;
+
+        /// <summary>
+        /// Lock only briefly. accessed by external code (to request new joints) and by OdeScene.Simulate() to move those joints into pending/active
+        /// </summary>
+        private readonly List<PhysicsJoint> requestedJointsToBeCreated = new List<PhysicsJoint>();
+
+        /// <summary>
+        /// can lock for longer. accessed only by OdeScene.
+        /// </summary>
+        private readonly List<PhysicsJoint> pendingJoints = new List<PhysicsJoint>();
+
+        /// <summary>
+        /// can lock for longer. accessed only by OdeScene.
+        /// </summary>
+        private readonly List<PhysicsJoint> activeJoints = new List<PhysicsJoint>();
+
+        /// <summary>
+        /// lock only briefly. accessed by external code (to request deletion of joints) and by OdeScene.Simulate() to move those joints out of pending/active
+        /// </summary>
+        private readonly List<string> requestedJointsToBeDeleted = new List<string>();
+
+        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 p1ExpectedPoints = 0;
+        private int p2ExpectedPoints = 0;
+        //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;
+
+        /// <summary>
+        /// Used to lock the entire physics scene.  Locked during the main part of Simulate()
+        /// </summary>
+        internal Object OdeLock = new Object();
+
+        private bool _worldInitialized = false;
+
+        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;
+
+        private bool avplanted = false;
+        private bool av_av_collisions_off = 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);
+
+        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;
+
+        float spacesPerMeterX;
+        float spacesPerMeterY;
+        int spaceGridMaxX;
+        int spaceGridMaxY;
+
+        private ODERayCastRequestManager m_rayCastManager;
+
+
+        #region INonSharedRegionModule
+        public string Name
+        {
+            get { return "OpenDynamicsEngine"; }
+        }
+
+        public Type ReplaceableInterface
+        {
+            get { return null; }
+        }
+
+        public void Initialise(IConfigSource source)
+        {
+            // TODO: Move this out of Startup
+            IConfig config = source.Configs["Startup"];
+            if (config != null)
+            {
+                string physics = config.GetString("physics", string.Empty);
+                if (physics == Name)
+                {
+                    m_Enabled = true;
+                    m_config = source;
+
+                    // We do this so that OpenSimulator on Windows loads the correct native ODE library depending on whether
+                    // it's running as a 32-bit process or a 64-bit one.  By invoking LoadLibary here, later DLLImports
+                    // will find it already loaded later on.
+                    //
+                    // This isn't necessary for other platforms (e.g. Mac OSX and Linux) since the DLL used can be
+                    // controlled in Ode.NET.dll.config
+                    if (Util.IsWindows())
+                        Util.LoadArchSpecificWindowsDll("ode.dll");
+
+                    // 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();
+
+                }
+            }
+
+        }
+
+        public void Close()
+        {
+        }
+
+        public void AddRegion(Scene scene)
+        {
+            if (!m_Enabled)
+                return;
+
+            EngineType = Name;
+            PhysicsSceneName = EngineType + "/" + scene.RegionInfo.RegionName;
+
+            scene.RegisterModuleInterface<PhysicsScene>(this);
+            Vector3 extent = new Vector3(scene.RegionInfo.RegionSizeX, scene.RegionInfo.RegionSizeY, scene.RegionInfo.RegionSizeZ);
+            Initialise(extent);
+            InitialiseFromConfig(m_config);
+
+            // This may not be that good since terrain may not be avaiable at this point
+            base.Initialise(scene.PhysicsRequestAsset, 
+                (scene.Heightmap != null ? scene.Heightmap.GetFloatsSerialised() : new float[(int)(extent.X * extent.Y)]), 
+                (float)scene.RegionInfo.RegionSettings.WaterHeight);
+
+        }
+
+        public void RemoveRegion(Scene scene)
+        {
+            if (!m_Enabled)
+                return;
+        }
+
+        public void RegionLoaded(Scene scene)
+        {
+            if (!m_Enabled)
+                return;
+
+            mesher = scene.RequestModuleInterface<IMesher>();
+            if (mesher == null)
+                m_log.WarnFormat("[ODE SCENE]: No mesher in {0}. Things will not work well.", PhysicsSceneName);
+        }
+        #endregion
+
+        /// <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>
+        private void Initialise(Vector3 regionExtent)
+        {
+                        WorldExtents.X = regionExtent.X;
+            m_regionWidth = (uint)regionExtent.X;
+            WorldExtents.Y = regionExtent.Y;
+            m_regionHeight = (uint)regionExtent.Y;
+                        
+            m_suportCombine = false;
+                        
+            nearCallback = near;
+            triCallback = TriCallback;
+            triArrayCallback = TriArrayCallback;
+            m_rayCastManager = new ODERayCastRequestManager(this);
+
+            // Create the world and the first space
+            world = d.WorldCreate();
+            space = d.HashSpaceCreate(IntPtr.Zero);
+
+            contactgroup = d.JointGroupCreate(0);
+
+            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
+
+        public void Initialise(IMesher meshmerizer, IConfigSource config, Vector3 regionExtent)
+        {
+            WorldExtents.X = regionExtent.X;
+            m_regionWidth = (uint)regionExtent.X;
+            WorldExtents.Y = regionExtent.Y;
+            m_regionHeight = (uint)regionExtent.Y;
+            m_suportCombine = false;
+            Initialise(meshmerizer, config);
+        }
+
+        // Initialize the mesh plugin
+        public void Initialise(IMesher meshmerizer, IConfigSource config)
+        {
+            InitializeExtraStats();
+
+            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)
+                {
+                    CollectStats = physicsconfig.GetBoolean("collect_stats", false);
+
+                    gravityx = physicsconfig.GetFloat("world_gravityx", 0f);
+                    gravityy = physicsconfig.GetFloat("world_gravityy", 0f);
+                    gravityz = physicsconfig.GetFloat("world_gravityz", -9.8f);
+
+                    float avatarTerminalVelocity = physicsconfig.GetFloat("avatar_terminal_velocity", 54f);
+                    AvatarTerminalVelocity = Util.Clamp<float>(avatarTerminalVelocity, 0, 255f);
+                    if (AvatarTerminalVelocity != avatarTerminalVelocity)
+                    {
+                        m_log.WarnFormat(
+                            "[ODE SCENE]: avatar_terminal_velocity of {0} is invalid.  Clamping to {1}",
+                            avatarTerminalVelocity, AvatarTerminalVelocity);
+                    }
+
+                    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", ODE_STEPSIZE);
+                    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);
+                    avplanted = physicsconfig.GetBoolean("av_planted", false);
+                    av_av_collisions_off = physicsconfig.GetBoolean("av_av_collisions_off", false);
+
+                    IsAvCapsuleTilted = physicsconfig.GetBoolean("av_capsule_tilted", false);
+
+                    contactsPerCollision = physicsconfig.GetInt("contacts_per_collision", 80);
+
+                    geomContactPointsStartthrottle = physicsconfig.GetInt("geom_contactpoints_start_throttling", 5);
+                    geomUpdatesPerThrottledUpdate = physicsconfig.GetInt("geom_updates_before_throttled_update", 15);
+                    geomCrossingFailuresBeforeOutofbounds = physicsconfig.GetInt("geom_crossing_failures_before_outofbounds", 5);
+
+                    geomDefaultDensity = physicsconfig.GetFloat("geometry_default_density", 10.000006836f);
+                    bodyFramesAutoDisable = physicsconfig.GetInt("body_frames_auto_disable", 20);
+
+                    bodyPIDD = physicsconfig.GetFloat("body_pid_derivative", 35f);
+                    bodyPIDG = physicsconfig.GetFloat("body_pid_gain", 25f);
+
+                    forceSimplePrimMeshing = physicsconfig.GetBoolean("force_simple_prim_meshing", forceSimplePrimMeshing);
+                    meshSculptedPrim = physicsconfig.GetBoolean("mesh_sculpted_prim", true);
+                    meshSculptLOD = physicsconfig.GetFloat("mesh_lod", 32f);
+                    MeshSculptphysicalLOD = physicsconfig.GetFloat("mesh_physical_lod", 16f);
+                    m_filterCollisions = physicsconfig.GetBoolean("filter_collisions", false);
+                    
+                    
+
+                    if (Environment.OSVersion.Platform == PlatformID.Unix)
+                    {
+                        avPIDD = physicsconfig.GetFloat("av_pid_derivative_linux", 2200.0f);
+                        avPIDP = physicsconfig.GetFloat("av_pid_proportional_linux", 900.0f);
+                        avStandupTensor = physicsconfig.GetFloat("av_capsule_standup_tensor_linux", 550000f);
+                        bodyMotorJointMaxforceTensor = physicsconfig.GetFloat("body_motor_joint_maxforce_tensor_linux", 5f);
+                    }
+                    else
+                    {
+                        avPIDD = physicsconfig.GetFloat("av_pid_derivative_win", 2200.0f);
+                        avPIDP = physicsconfig.GetFloat("av_pid_proportional_win", 900.0f);
+                        avStandupTensor = physicsconfig.GetFloat("av_capsule_standup_tensor_win", 550000f);
+                        bodyMotorJointMaxforceTensor = physicsconfig.GetFloat("body_motor_joint_maxforce_tensor_win", 5f);
+                    }
+
+                    physics_logging = physicsconfig.GetBoolean("physics_logging", false);
+                    physics_logging_interval = physicsconfig.GetInt("physics_logging_interval", 0);
+                    physics_logging_append_existing_logfile = physicsconfig.GetBoolean("physics_logging_append_existing_logfile", false);
+
+                    m_NINJA_physics_joints_enabled = physicsconfig.GetBoolean("use_NINJA_physics_joints", false);
+                    minimumGroundFlightOffset = physicsconfig.GetFloat("minimum_ground_flight_offset", 3f);
+                    maximumMassObject = physicsconfig.GetFloat("maximum_mass_object", 10000.01f);
+                }
+            }
+
+            contacts = new d.ContactGeom[contactsPerCollision];
+
+            spacesPerMeterX = 1.0f / metersInSpace;
+            spacesPerMeterY = 1.0f / metersInSpace;
+
+            spaceGridMaxX = (int)(WorldExtents.X * spacesPerMeterX);
+            spaceGridMaxY = (int)(WorldExtents.Y * spacesPerMeterY);
+
+            // note: limit number of spaces
+            if (spaceGridMaxX > 24)
+            {
+                spaceGridMaxX = 24;
+                spacesPerMeterX =  spaceGridMaxX / WorldExtents.X;
+            }
+            if (spaceGridMaxY > 24)
+            {
+                spaceGridMaxY = 24;
+                spacesPerMeterY = spaceGridMaxY / WorldExtents.Y;
+            }
+
+            staticPrimspace = new IntPtr[spaceGridMaxX, spaceGridMaxY]; 
+
+            // make this index limits
+            spaceGridMaxX--;
+            spaceGridMaxY--;
+
+
+
+            // 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;
+                }
+            }
+
+            _worldInitialized = true;
+        }
+
+//        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>
+        /// Collides two geometries.
+        /// </summary>
+        /// <returns></returns>
+        /// <param name='geom1'></param>
+        /// <param name='geom2'>/param>
+        /// <param name='maxContacts'></param>
+        /// <param name='contactsArray'></param>
+        /// <param name='contactGeomSize'></param>
+        private int CollideGeoms(
+            IntPtr geom1, IntPtr geom2, int maxContacts, Ode.NET.d.ContactGeom[] contactsArray, int contactGeomSize)
+        {
+            int count;
+
+            lock (OdeScene.UniversalColliderSyncObject)
+            {
+                // We do this inside the lock so that we don't count any delay in acquiring it
+                if (CollectStats)
+                    m_nativeCollisionStartTick = Util.EnvironmentTickCount();
+
+                count = d.Collide(geom1, geom2, maxContacts, contactsArray, contactGeomSize);
+            }
+
+            // We do this outside the lock so that any waiting threads aren't held up, though the effect is probably
+            // negligable
+            if (CollectStats)
+                m_stats[ODENativeGeomCollisionFrameMsStatName]
+                    += Util.EnvironmentTickCountSubtract(m_nativeCollisionStartTick);
+
+            return count;
+        }
+
+        /// <summary>
+        /// Collide two spaces or a space and a geometry.
+        /// </summary>
+        /// <param name='space1'></param>
+        /// <param name='space2'>/param>
+        /// <param name='data'></param>
+        private void CollideSpaces(IntPtr space1, IntPtr space2, IntPtr data)
+        {
+            if (CollectStats)
+            {
+                m_inCollisionTiming = true;
+                m_nativeCollisionStartTick = Util.EnvironmentTickCount();
+            }
+
+            d.SpaceCollide2(space1, space2, data, nearCallback);
+
+            if (CollectStats && m_inCollisionTiming)
+            {
+                m_stats[ODENativeSpaceCollisionFrameMsStatName]
+                    += Util.EnvironmentTickCountSubtract(m_nativeCollisionStartTick);
+                m_inCollisionTiming = false;
+            }
+        }
+
+        /// <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)
+        {
+            if (CollectStats && m_inCollisionTiming)
+            {
+                m_stats[ODENativeSpaceCollisionFrameMsStatName]
+                    += Util.EnvironmentTickCountSubtract(m_nativeCollisionStartTick);
+                m_inCollisionTiming = false;
+            }
+
+//            m_log.DebugFormat("[PHYSICS]: Colliding {0} and {1} in {2}", g1, g2, space);
+            //  no lock here!  It's invoked from within Simulate(), which is thread-locked
+
+            // Test if we're colliding a geom with a space.
+            // If so we have to drill down into the space recursively
+
+            if (d.GeomIsSpace(g1) || d.GeomIsSpace(g2))
+            {
+                if (g1 == IntPtr.Zero || g2 == IntPtr.Zero)
+                    return;
+                
+                // Separating static prim geometry spaces.
+                // We'll be calling near recursivly if one
+                // of them is a space to find all of the
+                // contact points in the space
+                try
+                {
+                    CollideSpaces(g1, g2, IntPtr.Zero);
+                }
+                catch (AccessViolationException)
+                {
+                    m_log.Error("[ODE SCENE]: 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;
+
+                count = CollideGeoms(g1, g2, contacts.Length, contacts, d.ContactGeom.SizeOf);
+
+                // All code after this is only relevant if we have any collisions
+                if (count <= 0)
+                    return;
+
+                if (count > contacts.Length)
+                    m_log.Error("[ODE SCENE]: Got " + count + " contacts when we asked for a maximum of " + contacts.Length);
+            }
+            catch (SEHException)
+            {
+                m_log.Error(
+                    "[ODE SCENE]: 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.");
+                base.TriggerPhysicsBasedRestart();
+            }
+            catch (Exception e)
+            {
+                m_log.ErrorFormat("[ODE SCENE]: Unable to collide test an object: {0}", e.Message);
+                return;
+            }
+
+            PhysicsActor p1;
+            PhysicsActor p2;
+            
+            p1ExpectedPoints = 0;
+            p2ExpectedPoints = 0;
+            
+            if (!actor_name_map.TryGetValue(g1, out p1))
+            {
+                p1 = PANull;
+            }
+
+            if (!actor_name_map.TryGetValue(g2, out p2))
+            {
+                p2 = PANull;
+            }
+
+            ContactPoint maxDepthContact = new ContactPoint();
+            if (p1.CollisionScore + count >= float.MaxValue)
+                p1.CollisionScore = 0;
+            p1.CollisionScore += count;
+
+            if (p2.CollisionScore + count >= float.MaxValue)
+                p2.CollisionScore = 0;
+            p2.CollisionScore += count;
+
+            for (int i = 0; i < count; i++)
+            {
+                d.ContactGeom curContact = contacts[i];
+
+                if (curContact.depth > maxDepthContact.PenetrationDepth)
+                {
+                    maxDepthContact = new ContactPoint(
+                        new Vector3(curContact.pos.X, curContact.pos.Y, curContact.pos.Z),
+                        new Vector3(curContact.normal.X, curContact.normal.Y, curContact.normal.Z),
+                        curContact.depth
+                    );
+                }
+
+                //m_log.Warn("[CCOUNT]: " + count);
+                IntPtr joint;
+                // If we're colliding with terrain, use 'TerrainContact' instead of contact.
+                // allows us to have different settings
+                
+                // We only need to test p2 for 'jump crouch purposes'
+                if (p2 is OdeCharacter && p1.PhysicsActorType == (int)ActorTypes.Prim)
+                {
+                    // Testing if the collision is at the feet of the avatar
+
+                    //m_log.DebugFormat("[PHYSICS]: {0} - {1} - {2} - {3}", curContact.pos.Z, p2.Position.Z, (p2.Position.Z - curContact.pos.Z), (p2.Size.Z * 0.6f));
+                    if ((p2.Position.Z - curContact.pos.Z) > (p2.Size.Z * 0.6f))
+                        p2.IsColliding = true;
+                }
+                else
+                {
+                    p2.IsColliding = true;
+                }
+                
+                //if ((framecount % m_returncollisions) == 0)
+
+                switch (p1.PhysicsActorType)
+                {
+                    case (int)ActorTypes.Agent:
+                        p1ExpectedPoints = avatarExpectedContacts;
+                        p2.CollidingObj = true;
+                        break;
+                    case (int)ActorTypes.Prim:
+                        if (p1 != null && p1 is OdePrim)
+                            p1ExpectedPoints = ((OdePrim) p1).ExpectedCollisionContacts;
+
+                        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 (av_av_collisions_off)
+                    if ((p1 is OdeCharacter) && (p2 is OdeCharacter))
+                        skipThisContact = true;
+
+                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)
+                {
+                    _perloopContact.Add(curContact);
+
+                    if (name1 == "Terrain" || name2 == "Terrain")
+                    {
+                        if ((p2.PhysicsActorType == (int) ActorTypes.Agent) &&
+                            (Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f))
+                        {
+                            p2ExpectedPoints = avatarExpectedContacts;
+                            // Avatar is moving on terrain, use the movement terrain contact
+                            AvatarMovementTerrainContact.geom = curContact;
+
+                            if (m_global_contactcount < maxContactsbeforedeath)
+                            {
+                                joint = d.JointCreateContact(world, contactgroup, ref AvatarMovementTerrainContact);
+                                m_global_contactcount++;
+                            }
+                        }
+                        else
+                        {
+                            if (p2.PhysicsActorType == (int)ActorTypes.Agent)
+                            {
+                                p2ExpectedPoints = avatarExpectedContacts;
+                                // Avatar is standing on terrain, use the non moving terrain contact
+                                TerrainContact.geom = 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;
+                                        p2ExpectedPoints = ((OdePrim)p2).ExpectedCollisionContacts;
+                                    }
+                                   
+                                    // Unnessesary because p1 is defined above
+                                    //if (p1 is OdePrim)
+                                    // { 
+                                    //     p1ExpectedPoints = ((OdePrim)p1).ExpectedCollisionContacts;
+                                    // }
+                                    //m_log.DebugFormat("Material: {0}", material);
+
+                                    m_materialContacts[material, movintYN].geom = 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;
+                                        p2ExpectedPoints = ((OdePrim)p2).ExpectedCollisionContacts;
+                                    }
+
+                                    //m_log.DebugFormat("Material: {0}", material);
+                                    m_materialContacts[material, movintYN].geom = 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;
+
+                        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
+                    {
+                        if ((p2.PhysicsActorType == (int)ActorTypes.Agent))
+                        {
+                            p2ExpectedPoints = avatarExpectedContacts;
+                            if ((Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f))
+                            {
+                                // Avatar is moving on a prim, use the Movement prim contact
+                                AvatarMovementprimContact.geom = curContact;
+
+                                if (m_global_contactcount < maxContactsbeforedeath)
+                                {
+                                    joint = d.JointCreateContact(world, contactgroup, ref AvatarMovementprimContact);
+                                    m_global_contactcount++;
+                                }
+                            }
+                            else
+                            {
+                                // Avatar is standing still on a prim, use the non movement contact
+                                contact.geom = 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;
+                                p2ExpectedPoints = ((OdePrim)p2).ExpectedCollisionContacts;
+                            }
+                            
+                            //m_log.DebugFormat("Material: {0}", material);
+                            m_materialContacts[material, 0].geom = 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 > ((p1ExpectedPoints + p2ExpectedPoints) * 0.25) + (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)
+        {
+            if (!m_filterCollisions)
+                return false;
+
+            bool result = false;
+
+            ActorTypes at = (ActorTypes)atype;
+
+            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)))
+                    {
+                        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)))
+                    {
+                        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("[Collision]: 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.LocalID;
+                            cc1.AddCollisionEvent(cc2.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.LocalID;
+                                cp1.AddCollisionEvent(cc2.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.LocalID;
+                                    cc1.AddCollisionEvent(cp2.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.LocalID;
+                                    cp1.AddCollisionEvent(cp2.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);
+                    //}
+                //}
+           // }
+        }
+
+        private 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;
+        }
+
+        private 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>
+        private void collision_optimized()
+        {
+            _perloopContact.Clear();
+
+            foreach (OdeCharacter chr in _characters)
+            {
+                // Reset the collision values to false
+                // since we don't know if we're colliding yet
+                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
+                {
+                    CollideSpaces(space, chr.Shell, IntPtr.Zero);
+                }
+                catch (AccessViolationException)
+                {
+                    m_log.ErrorFormat("[ODE SCENE]: Unable to space collide {0}", PhysicsSceneName);
+                }
+                
+                //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;
+                //}
+            }
+
+            if (CollectStats)
+            {
+                m_tempAvatarCollisionsThisFrame = _perloopContact.Count;
+                m_stats[ODEAvatarContactsStatsName] += m_tempAvatarCollisionsThisFrame;
+            }
+
+            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)
+                            {
+                                CollideSpaces(space, chr.prim_geom, IntPtr.Zero);
+                            }
+                            else
+                            {
+                                if (removeprims == null)
+                                {
+                                    removeprims = new List<OdePrim>();
+                                }
+                                removeprims.Add(chr);
+                                m_log.Error(
+                                    "[ODE SCENE]: 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.Error("[ODE SCENE]: Unable to space collide");
+                    }
+                }
+            }
+
+            if (CollectStats)
+                m_stats[ODEPrimContactsStatName] += _perloopContact.Count - m_tempAvatarCollisionsThisFrame;
+
+            if (removeprims != null)
+            {
+                foreach (OdePrim chr in removeprims)
+                {
+                    _activeprims.Remove(chr);
+                }
+            }
+        }
+
+        #endregion
+
+        public override void Combine(PhysicsScene pScene, Vector3 offset, Vector3 extents)
+        {
+            if (!m_suportCombine)
+                return;
+            m_worldOffset = offset;
+            WorldExtents = new Vector2(extents.X, extents.Y);
+            m_parentScene = pScene;
+        }
+
+        // Recovered for use by fly height. Kitto Flora
+        internal float GetTerrainHeightAtXY(float x, float y)
+        {
+            IntPtr heightFieldGeom = IntPtr.Zero;
+            int offsetX = 0;
+            int offsetY = 0;
+
+            if (m_suportCombine)
+            {
+                offsetX = ((int)(x / (int)Constants.RegionSize)) * (int)Constants.RegionSize;
+                offsetY = ((int)(y / (int)Constants.RegionSize)) * (int)Constants.RegionSize;
+            }
+
+            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 + 1f;
+                        y = y - offsetY + 1f;
+
+                        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
+
+        /// <summary>
+        /// Add actor to the list that should receive collision events in the simulate loop.
+        /// </summary>
+        /// <param name="obj"></param>
+        internal void AddCollisionEventReporting(PhysicsActor obj)
+        {
+//            m_log.DebugFormat("[PHYSICS]: Adding {0} {1} to collision event reporting", obj.SOPName, obj.LocalID);
+            
+            lock (m_collisionEventActorsChanges)
+                m_collisionEventActorsChanges[obj.LocalID] = obj;
+        }
+
+        /// <summary>
+        /// Remove actor from the list that should receive collision events in the simulate loop.
+        /// </summary>
+        /// <param name="obj"></param>
+        internal void RemoveCollisionEventReporting(PhysicsActor obj)
+        {
+//            m_log.DebugFormat("[PHYSICS]: Removing {0} {1} from collision event reporting", obj.SOPName, obj.LocalID);
+
+            lock (m_collisionEventActorsChanges)
+                m_collisionEventActorsChanges[obj.LocalID] = null;
+        }
+
+        #region Add/Remove Entities
+
+        public override PhysicsActor AddAvatar(string avName, Vector3 position, Vector3 velocity, Vector3 size, bool isFlying)
+        {                       
+            OdeCharacter newAv
+                = new OdeCharacter(
+                    avName, this, position, velocity, size, avPIDD, avPIDP,
+                    avCapRadius, avStandupTensor, avDensity,
+                    avMovementDivisorWalk, avMovementDivisorRun);
+
+            newAv.Flying = isFlying;
+            newAv.MinimumGroundFlightOffset = minimumGroundFlightOffset;
+            newAv.m_avatarplanted = avplanted;
+
+            return newAv;
+        }
+
+        public override void RemoveAvatar(PhysicsActor actor)
+        {
+//            m_log.DebugFormat(
+//                "[ODE SCENE]: Removing physics character {0} {1} from physics scene {2}",
+//                actor.Name, actor.LocalID, Name);
+
+            ((OdeCharacter) actor).Destroy();
+        }
+
+        internal void AddCharacter(OdeCharacter chr)
+        {
+            chr.m_avatarplanted = avplanted;
+            if (!_characters.Contains(chr))
+            {
+                _characters.Add(chr);
+
+//                m_log.DebugFormat(
+//                    "[ODE SCENE]: Adding physics character {0} {1} to physics scene {2}.  Count now {3}",
+//                    chr.Name, chr.LocalID, Name, _characters.Count);
+
+                if (chr.bad)
+                    m_log.ErrorFormat("[ODE SCENE]: Added BAD actor {0} to characters list", chr.m_uuid);
+            }
+            else
+            {
+                m_log.ErrorFormat(
+                    "[ODE SCENE]: Tried to add character {0} {1} but they are already in the set!",
+                    chr.Name, chr.LocalID);
+            }
+        }
+
+        internal void RemoveCharacter(OdeCharacter chr)
+        {
+            if (_characters.Contains(chr))
+            {
+                _characters.Remove(chr);
+
+//                m_log.DebugFormat(
+//                    "[ODE SCENE]: Removing physics character {0} {1} from physics scene {2}.  Count now {3}",
+//                    chr.Name, chr.LocalID, Name, _characters.Count);
+            }
+            else
+            {
+                m_log.ErrorFormat(
+                    "[ODE SCENE]: Tried to remove character {0} {1} but they are not in the list!",
+                    chr.Name, chr.LocalID);
+            }
+        }
+
+        private PhysicsActor AddPrim(String name, Vector3 position, Vector3 size, Quaternion rotation,
+                                     PrimitiveBaseShape pbs, bool isphysical, uint localID)
+        {
+            Vector3 pos = position;
+            Vector3 siz = size;
+            Quaternion rot = rotation;
+
+            OdePrim newPrim;
+            lock (OdeLock)
+            {
+                newPrim = new OdePrim(name, this, pos, siz, rot, pbs, isphysical);
+
+                lock (_prims)
+                    _prims.Add(newPrim);
+            }
+            newPrim.LocalID = localID;
+            return newPrim;
+        }
+
+        /// <summary>
+        /// Make this prim subject to physics.
+        /// </summary>
+        /// <param name="prim"></param>
+        internal void ActivatePrim(OdePrim prim)
+        {
+            // adds active prim..   (ones that should be iterated over in collisions_optimized
+            if (!_activeprims.Contains(prim))
+                _activeprims.Add(prim);
+            //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, bool isPhysical, uint localid)
+        {
+//            m_log.DebugFormat("[ODE SCENE]: Adding physics prim {0} {1} to physics scene {2}", primName, localid, Name);
+
+            return AddPrim(primName, position, size, rotation, pbs, isPhysical, localid);
+        }
+
+        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);
+                }
+            }
+        }
+
+        /// <summary>
+        /// Add a request for joint creation.
+        /// </summary>
+        /// <remarks>
+        /// 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.
+        /// </remarks>
+        /// <param name="objectNameInScene"></param>
+        /// <param name="jointType"></param>
+        /// <param name="position"></param>
+        /// <param name="rotation"></param>
+        /// <param name="parms"></param>
+        /// <param name="bodyNames"></param>
+        /// <param name="trackedBodyName"></param>
+        /// <param name="localRotation"></param>
+        /// <returns></returns>
+        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);
+        }
+
+        /// <summary>
+        /// Get joint axis.
+        /// </summary>
+        /// <remarks>
+        /// 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.
+        /// </remarks>
+        /// <param name="joint"></param>
+        /// <returns></returns>
+        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);
+        }
+
+        /// <summary>
+        /// Stop this prim being subject to physics
+        /// </summary>
+        /// <param name="prim"></param>
+        internal void DeactivatePrim(OdePrim prim)
+        {
+            _activeprims.Remove(prim);
+        }
+
+        public override void RemovePrim(PhysicsActor prim)
+        {
+            // As with all ODE physics operations, we don't remove the prim immediately but signal that it should be
+            // removed in the next physics simulate pass.
+            if (prim is OdePrim)
+            {
+                lock (OdeLock)
+                {
+                    OdePrim p = (OdePrim) prim;
+
+                    p.setPrimForRemoval();
+                    AddPhysicsActorTaint(prim);
+                }
+            }
+        }
+
+        /// <summary>
+        /// This is called from within simulate but outside the locked portion
+        /// We need to do our own locking here
+        /// (Note: As of 20110801 this no longer appears to be true - this is being called within lock (odeLock) in
+        /// Simulate() -- justincc).
+        ///
+        /// 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>
+        internal void RemovePrimThreadLocked(OdePrim prim)
+        {
+//            m_log.DebugFormat("[ODE SCENE]: Removing physical prim {0} {1}", prim.Name, prim.LocalID);
+
+            lock (prim)
+            {
+                RemoveCollisionEventReporting(prim);
+
+                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);
+
+                    if (!prim.RemoveGeom())
+                        m_log.Warn("[ODE SCENE]: Unable to remove prim from physics scene");
+
+                    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>
+        private 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;
+                }
+            }
+        }
+
+//        private 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>
+        internal 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("[ODE SCENE]: 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("[ODE SCENE]: 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("[ODE SCENE]: 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("[ODE SCENE]: 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("[ODE SCENE]: 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>
+        internal 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>
+        internal 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>
+        internal int[] calculateSpaceArrayItemFromPos(Vector3 pos)
+        {
+            int[] returnint = new int[2];
+
+            returnint[0] = (int) (pos.X * spacesPerMeterX);
+
+            if (returnint[0] > spaceGridMaxX)
+                returnint[0] = spaceGridMaxX;
+            if (returnint[0] < 0)
+                returnint[0] = 0;
+
+            returnint[1] = (int)(pos.Y * spacesPerMeterY);
+            if (returnint[1] > spaceGridMaxY)
+                returnint[1] = spaceGridMaxY;
+            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>
+        internal 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 && !pbs.SculptEntry)
+            {
+                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.PathBegin != 0) || pbs.PathEnd != 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 (pbs.SculptEntry && meshSculptedPrim)
+                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.
+        /// </summary>
+        /// <remarks>
+        /// 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
+        /// </remarks>
+        /// <param name="actor"></param>
+        public override void AddPhysicsActorTaint(PhysicsActor actor)
+        {
+            if (actor is OdePrim)
+            {
+                OdePrim taintedprim = ((OdePrim)actor);
+                lock (_taintedPrims)
+                    _taintedPrims.Add(taintedprim);
+            }
+            else if (actor is OdeCharacter)
+            {
+                OdeCharacter taintedchar = ((OdeCharacter)actor);
+                lock (_taintedActors)
+                {
+                    _taintedActors.Add(taintedchar);
+                    if (taintedchar.bad)
+                        m_log.ErrorFormat("[ODE SCENE]: Added BAD actor {0} to tainted actors", taintedchar.m_uuid);
+                }
+            }
+        }
+
+        /// <summary>
+        /// This is our main simulate loop
+        /// </summary>
+        /// <remarks>
+        /// 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)
+        /// </remarks>
+        /// <param name="timeStep"></param>
+        /// <returns>The number of frames simulated over that period.</returns>
+        public override float Simulate(float timeStep)
+        {
+            if (!_worldInitialized) return 11f;
+
+            int startFrameTick = CollectStats ? Util.EnvironmentTickCount() : 0;
+            int tempTick = 0, tempTick2 = 0;
+
+            if (framecount >= int.MaxValue)
+                framecount = 0;
+
+            framecount++;
+
+            float fps = 0;
+
+            step_time += timeStep;
+
+            float HalfOdeStep = ODE_STEPSIZE * 0.5f;
+            if (step_time < HalfOdeStep)
+                return 0;
+
+
+            // We change _collisionEventPrimChanges to avoid locking _collisionEventPrim itself and causing potential
+            // deadlock if the collision event tries to lock something else later on which is already locked by a
+            // caller that is adding or removing the collision event.
+            lock (m_collisionEventActorsChanges)
+            {
+                foreach (KeyValuePair<uint, PhysicsActor> kvp in m_collisionEventActorsChanges)
+                {
+                    if (kvp.Value == null)
+                        m_collisionEventActors.Remove(kvp.Key);
+                    else
+                        m_collisionEventActors[kvp.Key] = kvp.Value;
+                }
+
+                m_collisionEventActorsChanges.Clear();
+            }
+
+            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();
+                //}
+
+                // 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
+
+
+                // 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);
+
+                while (step_time > HalfOdeStep)
+                {
+                    try
+                    {
+                        if (CollectStats)
+                            tempTick = Util.EnvironmentTickCount();
+
+                        lock (_taintedActors)
+                        {
+                            foreach (OdeCharacter character in _taintedActors)
+                                character.ProcessTaints();
+
+                            _taintedActors.Clear();
+                        }
+
+                        if (CollectStats)
+                        {
+                            tempTick2 = Util.EnvironmentTickCount();
+                            m_stats[ODEAvatarTaintMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick);
+                            tempTick = tempTick2;
+                        }
+
+                        lock (_taintedPrims)
+                        {
+                            foreach (OdePrim prim in _taintedPrims)
+                            {
+                                if (prim.m_taintremove)
+                                {
+//                                    Console.WriteLine("Simulate calls RemovePrimThreadLocked for {0}", prim.Name);
+                                    RemovePrimThreadLocked(prim);
+                                }
+                                else
+                                {
+//                                    Console.WriteLine("Simulate calls ProcessTaints for {0}", prim.Name);
+                                    prim.ProcessTaints();
+                                }
+
+                                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)
+                                SimulatePendingNINJAJoints();
+
+                            _taintedPrims.Clear();
+                        }
+
+                        if (CollectStats)
+                        {
+                            tempTick2 = Util.EnvironmentTickCount();
+                            m_stats[ODEPrimTaintMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick);
+                            tempTick = tempTick2;
+                        }
+
+                        // Move characters
+                        foreach (OdeCharacter actor in _characters)
+                            actor.Move(defects);
+
+                        if (defects.Count != 0)
+                        {
+                            foreach (OdeCharacter actor in defects)
+                            {
+                                m_log.ErrorFormat(
+                                    "[ODE SCENE]: Removing physics character {0} {1} from physics scene {2} due to defect found when moving",
+                                    actor.Name, actor.LocalID, PhysicsSceneName);
+
+                                RemoveCharacter(actor);
+                                actor.DestroyOdeStructures();
+                            }
+
+                            defects.Clear();
+                        }
+
+                        if (CollectStats)
+                        {
+                            tempTick2 = Util.EnvironmentTickCount();
+                            m_stats[ODEAvatarForcesFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick);
+                            tempTick = tempTick2;
+                        }
+
+                        // Move other active objects
+                        foreach (OdePrim prim in _activeprims)
+                        {
+                            prim.m_collisionscore = 0;
+                            prim.Move(timeStep);
+                        }
+
+                        if (CollectStats)
+                        {
+                            tempTick2 = Util.EnvironmentTickCount();
+                            m_stats[ODEPrimForcesFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick);
+                            tempTick = tempTick2;
+                        }
+
+                        //if ((framecount % m_randomizeWater) == 0)
+                           // randomizeWater(waterlevel);
+
+                        //int RayCastTimeMS = m_rayCastManager.ProcessQueuedRequests();
+                        m_rayCastManager.ProcessQueuedRequests();
+
+                        if (CollectStats)
+                        {
+                            tempTick2 = Util.EnvironmentTickCount();
+                            m_stats[ODERaycastingFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick);
+                            tempTick = tempTick2;
+                        }
+
+                        collision_optimized();
+
+                        if (CollectStats)
+                        {
+                            tempTick2 = Util.EnvironmentTickCount();
+                            m_stats[ODEOtherCollisionFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick);
+                            tempTick = tempTick2;
+                        }
+
+                        foreach (PhysicsActor obj in m_collisionEventActors.Values)
+                        {
+//                                m_log.DebugFormat("[PHYSICS]: Assessing {0} {1} for collision events", obj.SOPName, obj.LocalID);
+
+                            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;
+                            }
+                        }
+
+//                        if (m_global_contactcount > 0)
+//                            m_log.DebugFormat(
+//                                "[PHYSICS]: Collision contacts to process this frame = {0}", m_global_contactcount);
+
+                        m_global_contactcount = 0;
+
+                        if (CollectStats)
+                        {
+                            tempTick2 = Util.EnvironmentTickCount();
+                            m_stats[ODECollisionNotificationFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick);
+                            tempTick = tempTick2;
+                        }
+
+                        d.WorldQuickStep(world, ODE_STEPSIZE);
+
+                        if (CollectStats)
+                            m_stats[ODENativeStepFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick);
+
+                        d.JointGroupEmpty(contactgroup);
+                    }
+                    catch (Exception e)
+                    {
+                        m_log.ErrorFormat("[ODE SCENE]: {0}, {1}, {2}", e.Message, e.TargetSite, e);
+                    }
+
+                    step_time -= ODE_STEPSIZE;
+                    fps += ODE_STEPSIZE;
+                }
+
+                if (CollectStats)
+                    tempTick = Util.EnvironmentTickCount();
+
+                foreach (OdeCharacter actor in _characters)
+                {
+                    if (actor.bad)
+                        m_log.ErrorFormat("[ODE SCENE]: BAD Actor {0} in _characters list was not removed?", actor.m_uuid);
+
+                    actor.UpdatePositionAndVelocity(defects);
+                }
+
+                if (defects.Count != 0)
+                {
+                    foreach (OdeCharacter actor in defects)
+                    {
+                        m_log.ErrorFormat(
+                            "[ODE SCENE]: Removing physics character {0} {1} from physics scene {2} due to defect found when updating position and velocity",
+                            actor.Name, actor.LocalID, PhysicsSceneName);
+
+                        RemoveCharacter(actor);
+                        actor.DestroyOdeStructures();
+                    }
+
+                    defects.Clear();
+                }
+
+                if (CollectStats)
+                {
+                    tempTick2 = Util.EnvironmentTickCount();
+                    m_stats[ODEAvatarUpdateFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick);
+                    tempTick = tempTick2;
+                }
+
+                //if (timeStep < 0.2f)
+
+                foreach (OdePrim prim in _activeprims)
+                {
+                    if (prim.IsPhysical && (d.BodyIsEnabled(prim.Body) || !prim._zeroFlag))
+                    {
+                        prim.UpdatePositionAndVelocity();
+
+                        if (SupportsNINJAJoints)
+                            SimulateActorPendingJoints(prim);
+                    }
+                }
+
+                if (CollectStats)
+                    m_stats[ODEPrimUpdateFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick);
+
+                //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);
+                }
+
+                latertickcount = Util.EnvironmentTickCountSubtract(tickCountFrameRun);
+
+                // OpenSimulator above does 10 fps.  10 fps = means that the main thread loop and physics
+                // has a max of 100 ms to run theoretically.
+                // If the main loop stalls, it calls Simulate later which makes the tick count ms larger.
+                // If Physics stalls, it takes longer which makes the tick count ms larger.
+
+                if (latertickcount < 100)
+                {
+                    m_timeDilation = 1.0f;
+                }
+                else
+                {
+                    m_timeDilation = 100f / latertickcount;
+                    //m_timeDilation = Math.Min((Math.Max(100 - (Util.EnvironmentTickCount() - tickCountFrameRun), 1) / 100f), 1.0f);
+                }
+
+                tickCountFrameRun = Util.EnvironmentTickCount();
+
+                if (CollectStats)
+                    m_stats[ODETotalFrameMsStatName] += Util.EnvironmentTickCountSubtract(startFrameTick);
+            }
+
+            fps *= 1.0f/timeStep;
+            return fps;
+        }
+
+        /// <summary>
+        /// Simulate pending NINJA joints.
+        /// </summary>
+        /// <remarks>
+        /// Called by the main Simulate() loop if NINJA joints are active.  Should not be called from anywhere else.
+        /// </remarks>
+        private void SimulatePendingNINJAJoints()
+        {
+            // 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");
+                }
+            }
+        }
+
+        /// <summary>
+        /// Simulate the joint proxies of a NINJA actor.
+        /// </summary>
+        /// <remarks>
+        /// Called as part of the Simulate() loop if NINJA physics is active.  Must only be called from there.
+        /// </remarks>
+        /// <param name="actor"></param>
+        private void SimulateActorPendingJoints(OdePrim actor)
+        {
+            // 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);
+                    }
+                }
+            }
+        }
+
+        public override void GetResults()
+        {
+        }
+
+        public override bool IsThreaded
+        {
+            // for now we won't be multithreaded
+            get { return false; }
+        }
+
+/* godd try.. but not a fix
+        #region ODE Specific Terrain Fixes
+        private 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;
+        }
+
+        private 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("[ODE SCENE]: 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);
+            }
+        }
+
+        private void SetTerrain(float[] heightMap, Vector3 pOffset)
+        {
+            int startTime = Util.EnvironmentTickCount();
+            m_log.DebugFormat("[ODE SCENE]: Setting terrain for {0} with offset {1}", PhysicsSceneName, pOffset);
+
+            
+            float[] _heightmap;
+
+            // ok im lasy this are just a aliases
+            uint regionsizeX = m_regionWidth;
+            uint regionsizeY = m_regionHeight;
+
+            // map is rotated
+            uint heightmapWidth = regionsizeY + 2;
+            uint heightmapHeight = regionsizeX + 2;
+
+            uint heightmapWidthSamples = heightmapWidth + 1;
+            uint heightmapHeightSamples = heightmapHeight + 1;
+
+            _heightmap = new float[heightmapWidthSamples * heightmapHeightSamples];
+
+            const float scale = 1.0f;
+            const float offset = 0.0f;
+            const float thickness = 10f;
+            const int wrap = 0;
+
+
+            float hfmin = float.MaxValue;
+            float hfmax = float.MinValue;
+            float val;
+            uint xx;
+            uint yy;
+
+            uint maxXX = regionsizeX - 1;
+            uint maxYY = regionsizeY - 1;
+
+            // flipping map adding one margin all around so things don't fall in edges
+
+            uint xt = 0;
+            xx = 0;
+
+
+            for (uint x = 0; x < heightmapWidthSamples; x++)
+            {
+                if (x > 1 && xx < maxXX)
+                    xx++;
+                yy = 0;
+                for (uint y = 0; y < heightmapHeightSamples; y++)
+                {
+                    if (y > 1 && y < maxYY)
+                        yy += regionsizeX;
+
+                    val = heightMap[yy + xx];
+                    if (val < 0.0f)
+                        val = 0.0f;
+                    _heightmap[xt + y] = val;
+
+                    if (hfmin > val)
+                        hfmin = val;
+                    if (hfmax < val)
+                        hfmax = val;
+                }
+                xt += heightmapHeightSamples;
+            }
+
+            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);
+
+                q1 = q1 * q2;
+                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 + regionsizeX * 0.5f, pOffset.Y + regionsizeY * 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);
+            }
+
+            m_log.DebugFormat(
+                "[ODE SCENE]: Setting terrain for {0} took {1}ms", PhysicsSceneName, Util.EnvironmentTickCountSubtract(startTime));
+        }
+
+        public override void DeleteTerrain()
+        {
+        }
+
+        internal float GetWaterLevel()
+        {
+            return waterlevel;
+        }
+
+        public override bool SupportsCombining()
+        {
+            return m_suportCombine;
+        }
+
+        public override void SetWaterLevel(float baseheight)
+        {
+            waterlevel = baseheight;
+//            randomizeWater(waterlevel);
+        }
+
+/*
+        private void randomizeWater(float baseheight)
+        {
+            uint heightmapWidth = m_regionWidth + 2;
+            uint heightmapHeight = m_regionHeight + 2;
+            uint heightmapWidthSamples = m_regionWidth + 2;
+            uint heightmapHeightSamples = m_regionHeight + 2;
+            float scale = 1.0f;
+            float offset = 0.0f;
+            float thickness = 2.9f;
+            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()
+        {
+            _worldInitialized = false;
+
+            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> topColliders;
+
+            lock (_prims)
+            {
+                List<OdePrim> orderedPrims = new List<OdePrim>(_prims);
+                orderedPrims.OrderByDescending(p => p.CollisionScore);
+                topColliders = orderedPrims.Take(25).ToDictionary(p => p.LocalID, p => p.CollisionScore);
+
+                foreach (OdePrim p in _prims)
+                    p.CollisionScore = 0;
+            }
+
+            return topColliders;
+        }
+
+        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);
+            }
+        }
+
+        public override void RaycastWorld(Vector3 position, Vector3 direction, float length, int Count, RayCallback retMethod)
+        {
+            if (retMethod != null)
+            {
+                m_rayCastManager.QueueRequest(position, direction, length, Count, retMethod);
+            }
+        }
+
+        public override List<ContactResult> RaycastWorld(Vector3 position, Vector3 direction, float length, int Count)
+        {
+            ContactResult[] ourResults = null;
+            RayCallback retMethod = delegate(List<ContactResult> results)
+            {
+                ourResults = new ContactResult[results.Count];
+                results.CopyTo(ourResults, 0);
+            };
+            int waitTime = 0;
+            m_rayCastManager.QueueRequest(position, direction, length, Count, retMethod);
+            while (ourResults == null && waitTime < 1000)
+            {
+                Thread.Sleep(1);
+                waitTime++;
+            }
+            if (ourResults == null)
+                return new List<ContactResult> ();
+            return new List<ContactResult>(ourResults);
+        }
+
+#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);
+
+            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
+
+        public override Dictionary<string, float> GetStats()
+        {
+            if (!CollectStats)
+                return null;
+
+            Dictionary<string, float> returnStats;
+
+            lock (OdeLock)
+            {
+                returnStats = new Dictionary<string, float>(m_stats);
+
+                // FIXME: This is a SUPER DUMB HACK until we can establish stats that aren't subject to a division by
+                // 3 from the SimStatsReporter.
+                returnStats[ODETotalAvatarsStatName] = _characters.Count * 3;
+                returnStats[ODETotalPrimsStatName] = _prims.Count * 3;
+                returnStats[ODEActivePrimsStatName] = _activeprims.Count * 3;
+
+                InitializeExtraStats();
+            }
+
+            returnStats[ODEOtherCollisionFrameMsStatName]
+                = returnStats[ODEOtherCollisionFrameMsStatName]
+                    - returnStats[ODENativeSpaceCollisionFrameMsStatName]
+                    - returnStats[ODENativeGeomCollisionFrameMsStatName];
+
+            return returnStats;
+        }
+
+        private void InitializeExtraStats()
+        {
+            m_stats[ODETotalFrameMsStatName] = 0;
+            m_stats[ODEAvatarTaintMsStatName] = 0;
+            m_stats[ODEPrimTaintMsStatName] = 0;
+            m_stats[ODEAvatarForcesFrameMsStatName] = 0;
+            m_stats[ODEPrimForcesFrameMsStatName] = 0;
+            m_stats[ODERaycastingFrameMsStatName] = 0;
+            m_stats[ODENativeStepFrameMsStatName] = 0;
+            m_stats[ODENativeSpaceCollisionFrameMsStatName] = 0;
+            m_stats[ODENativeGeomCollisionFrameMsStatName] = 0;
+            m_stats[ODEOtherCollisionFrameMsStatName] = 0;
+            m_stats[ODECollisionNotificationFrameMsStatName] = 0;
+            m_stats[ODEAvatarContactsStatsName] = 0;
+            m_stats[ODEPrimContactsStatName] = 0;
+            m_stats[ODEAvatarUpdateFrameMsStatName] = 0;
+            m_stats[ODEPrimUpdateFrameMsStatName] = 0;
+        }
+    }
+}
diff --git a/OpenSim/Region/PhysicsModules/Ode/Tests/ODETestClass.cs b/OpenSim/Region/PhysicsModules/Ode/Tests/ODETestClass.cs
new file mode 100644
index 0000000..6dc22bd
--- /dev/null
+++ b/OpenSim/Region/PhysicsModules/Ode/Tests/ODETestClass.cs
@@ -0,0 +1,151 @@
+/*
+ * 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.PhysicsModules.SharedBase;
+using OpenSim.Region.PhysicsModule.ODE;
+using OpenSim.Region.Framework.Scenes;
+using OpenSim.Region.Framework.Interfaces;
+using OpenSim.Tests.Common;
+using log4net;
+using System.Reflection;
+
+namespace OpenSim.Region.PhysicsModule.ODE.Tests
+{
+    [TestFixture]
+    public class ODETestClass : OpenSimTestCase
+    {
+        private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
+
+        //private OpenSim.Region.PhysicsModule.ODE.OdePlugin cbt;
+        private PhysicsScene pScene;
+
+        [SetUp]
+        public void Initialize()
+        {
+            IConfigSource openSimINI = new IniConfigSource();
+            IConfig startupConfig = openSimINI.AddConfig("Startup");
+            startupConfig.Set("physics", "OpenDynamicsEngine");
+            startupConfig.Set("DecodedSculptMapPath", "j2kDecodeCache");
+
+            Vector3 regionExtent = new Vector3(Constants.RegionSize, Constants.RegionSize, Constants.RegionHeight);
+
+            //PhysicsScene pScene = physicsPluginManager.GetPhysicsScene(
+            //                "BulletSim", "Meshmerizer", openSimINI, "BSTestRegion", regionExtent);
+            RegionInfo info = new RegionInfo();
+            info.RegionName = "ODETestRegion";
+            info.RegionSizeX = info.RegionSizeY = info.RegionSizeZ = Constants.RegionSize;
+            OpenSim.Region.Framework.Scenes.Scene scene = new OpenSim.Region.Framework.Scenes.Scene(info);
+
+            //IMesher mesher = new OpenSim.Region.PhysicsModules.Meshing.Meshmerizer();
+            //INonSharedRegionModule mod = mesher as INonSharedRegionModule;
+            //mod.Initialise(openSimINI);
+            //mod.AddRegion(scene);
+            //mod.RegionLoaded(scene);
+
+            pScene = new OdeScene();
+            Console.WriteLine("HERE " + (pScene == null ? "Null" : "Not null"));
+            INonSharedRegionModule mod = (pScene as INonSharedRegionModule);
+            Console.WriteLine("HERE " + (mod == null ? "Null" : "Not null"));
+            mod.Initialise(openSimINI);
+            mod.AddRegion(scene);
+            mod.RegionLoaded(scene);
+
+            // Loading ODEPlugin
+            //cbt = new OdePlugin();
+            // Getting Physics Scene
+            //ps = cbt.GetScene("test");
+            // Initializing Physics Scene.
+            //ps.Initialise(imp.GetMesher(TopConfig), null, Vector3.Zero);
+            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;
+            }
+            pScene.SetTerrain(_heightmap);
+        }
+
+        [TearDown]
+        public void Terminate()
+        {
+            pScene.DeleteTerrain();
+            pScene.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 = pScene.AddPrimShape("CoolShape", newcube, position, size, rot, true, 0);
+            OdePrim oprim = (OdePrim)prim;
+            OdeScene pscene = (OdeScene)pScene;
+
+            Assert.That(oprim.m_taintadd);
+
+            prim.LocalID = 5;
+
+            for (int i = 0; i < 58; i++)
+            {
+                pScene.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.LocalID + "): " + prim.Position);
+
+                // 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);
+
+            pScene.RemovePrim(prim);
+            Assert.That(oprim.m_taintremove);
+            pScene.Simulate(0.133f);
+            Assert.That(oprim.Body == (IntPtr)0);
+        }
+    }
+}
diff --git a/OpenSim/Region/PhysicsModules/Ode/drawstuff.cs b/OpenSim/Region/PhysicsModules/Ode/drawstuff.cs
new file mode 100644
index 0000000..87ca446
--- /dev/null
+++ b/OpenSim/Region/PhysicsModules/Ode/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);
+    }
+}