Include ChOdePlugin

10 files changed, 10493 insertions, 0 deletions

diff --git a/OpenSim/Region/Physics/ChOdePlugin/AssemblyInfo.cs b/OpenSim/Region/Physics/ChOdePlugin/AssemblyInfo.cs
new file mode 100644
index 0000000..d65929a
--- /dev/null
+++ b/OpenSim/Region/Physics/ChOdePlugin/AssemblyInfo.cs
@@ -0,0 +1,58 @@
+/*
+ * Copyright (c) Contributors, http://opensimulator.org/
+ * See CONTRIBUTORS.TXT for a full list of copyright holders.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the OpenSimulator Project nor the
+ *       names of its contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+using System.Reflection;
+using System.Runtime.InteropServices;
+
+// Information about this assembly is defined by the following
+// attributes.
+//
+// change them to the information which is associated with the assembly
+// you compile.
+
+[assembly : AssemblyTitle("OdePlugin")]
+[assembly : AssemblyDescription("")]
+[assembly : AssemblyConfiguration("")]
+[assembly : AssemblyCompany("http://opensimulator.org")]
+[assembly : AssemblyProduct("OdePlugin")]
+[assembly : AssemblyCopyright("Copyright (c) OpenSimulator.org Developers 2007-2009")]
+[assembly : AssemblyTrademark("")]
+[assembly : AssemblyCulture("")]
+
+// This sets the default COM visibility of types in the assembly to invisible.
+// If you need to expose a type to COM, use [ComVisible(true)] on that type.
+
+[assembly : ComVisible(false)]
+
+// The assembly version has following format :
+//
+// Major.Minor.Build.Revision
+//
+// You can specify all values by your own or you can build default build and revision
+// numbers with the '*' character (the default):
+
+[assembly : AssemblyVersion("0.6.5.*")]
diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs
new file mode 100644
index 0000000..aa0acb7
--- /dev/null
+++ b/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs
@@ -0,0 +1,1353 @@
+/*
+ * Copyright (c) Contributors, http://opensimulator.org/
+ * See CONTRIBUTORS.TXT for a full list of copyright holders.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the OpenSimulator Project nor the
+ *       names of its contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+using System;
+using System.Collections.Generic;
+using System.Reflection;
+using OpenMetaverse;
+using Ode.NET;
+using OpenSim.Framework;
+using OpenSim.Region.Physics.Manager;
+using log4net;
+
+namespace OpenSim.Region.Physics.OdePlugin
+{
+    /// <summary>
+    /// Various properties that ODE uses for AMotors but isn't exposed in ODE.NET so we must define them ourselves.
+    /// </summary>
+
+    public enum dParam : int
+    {
+        LowStop = 0,
+        HiStop = 1,
+        Vel = 2,
+        FMax = 3,
+        FudgeFactor = 4,
+        Bounce = 5,
+        CFM = 6,
+        StopERP = 7,
+        StopCFM = 8,
+        LoStop2 = 256,
+        HiStop2 = 257,
+        Vel2 = 258,
+        FMax2 = 259,
+        StopERP2 = 7 + 256,
+        StopCFM2 = 8 + 256,
+        LoStop3 = 512,
+        HiStop3 = 513,
+        Vel3 = 514,
+        FMax3 = 515,
+        StopERP3 = 7 + 512,
+        StopCFM3 = 8 + 512
+    }
+    public class OdeCharacter : PhysicsActor
+    {
+        private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
+
+        private Vector3 _position;
+        private d.Vector3 _zeroPosition;
+        // private d.Matrix3 m_StandUpRotation;
+        private bool _zeroFlag = false;
+        private bool m_lastUpdateSent = false;
+        private Vector3 _velocity;
+        private Vector3 _target_velocity;
+        private Vector3 _acceleration;
+        private Vector3 m_rotationalVelocity;
+        private float m_mass = 80f;
+        public float m_density = 60f;
+        private bool m_pidControllerActive = true;
+        public float PID_D = 800.0f;
+        public float PID_P = 900.0f;
+        //private static float POSTURE_SERVO = 10000.0f;
+        public float CAPSULE_RADIUS = 0.37f;
+        public float CAPSULE_LENGTH = 2.140599f;
+        public float m_tensor = 3800000f;
+        public float heightFudgeFactor = 0.52f;
+        public float walkDivisor = 1.3f;
+        public float runDivisor = 0.8f;
+        private bool flying = false;
+        private bool m_iscolliding = false;
+        private bool m_iscollidingGround = false;
+        private bool m_wascolliding = false;
+        private bool m_wascollidingGround = false;
+        private bool m_iscollidingObj = false;
+        private bool m_alwaysRun = false;
+        private bool m_hackSentFall = false;
+        private bool m_hackSentFly = false;
+        private int m_requestedUpdateFrequency = 0;
+        private Vector3 m_taintPosition = Vector3.Zero;
+        public uint m_localID = 0;
+        public bool m_returnCollisions = false;
+        // taints and their non-tainted counterparts
+        public bool m_isPhysical = false; // the current physical status
+        public bool m_tainted_isPhysical = false; // set when the physical status is tainted (false=not existing in physics engine, true=existing)
+        public float MinimumGroundFlightOffset = 3f;
+
+        private float m_tainted_CAPSULE_LENGTH; // set when the capsule length changes. 
+        private float m_tiltMagnitudeWhenProjectedOnXYPlane = 0.1131371f; // used to introduce a fixed tilt because a straight-up capsule falls through terrain, probably a bug in terrain collider
+
+
+        private float m_buoyancy = 0f;
+
+        // private CollisionLocker ode;
+
+        private string m_name = String.Empty;
+
+        private bool[] m_colliderarr = new bool[11];
+        private bool[] m_colliderGroundarr = new bool[11];
+
+        // Default we're a Character
+        private CollisionCategories m_collisionCategories = (CollisionCategories.Character);
+
+        // Default, Collide with Other Geometries, spaces, bodies and characters.
+        private CollisionCategories m_collisionFlags = (CollisionCategories.Geom
+                                                        | CollisionCategories.Space
+                                                        | CollisionCategories.Body
+                                                        | CollisionCategories.Character
+                                                        | CollisionCategories.Land);
+        public IntPtr Body = IntPtr.Zero;
+        private OdeScene _parent_scene;
+        public IntPtr Shell = IntPtr.Zero;
+        public IntPtr Amotor = IntPtr.Zero;
+        public d.Mass ShellMass;
+        public bool collidelock = false;
+
+        public int m_eventsubscription = 0;
+        private CollisionEventUpdate CollisionEventsThisFrame = new CollisionEventUpdate();
+
+        // unique UUID of this character object
+        public UUID m_uuid;
+        public bool bad = false;
+
+        public OdeCharacter(String avName, OdeScene parent_scene, Vector3 pos, CollisionLocker dode, Vector3 size, float pid_d, float pid_p, float capsule_radius, float tensor, float density, float height_fudge_factor, float walk_divisor, float rundivisor)
+        {
+            m_uuid = UUID.Random();
+
+            if (pos.IsFinite())
+            {
+                if (pos.Z > 9999999f)
+                {
+                    pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
+                }
+                if (pos.Z < -90000f)
+                {
+                    pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
+                }
+                _position = pos;
+                m_taintPosition.X = pos.X;
+                m_taintPosition.Y = pos.Y;
+                m_taintPosition.Z = pos.Z;
+            }
+            else
+            {
+                _position = new Vector3(((float)_parent_scene.WorldExtents.X * 0.5f), ((float)_parent_scene.WorldExtents.Y * 0.5f), parent_scene.GetTerrainHeightAtXY(128f, 128f) + 10f);
+                m_taintPosition.X = _position.X;
+                m_taintPosition.Y = _position.Y;
+                m_taintPosition.Z = _position.Z;
+                m_log.Warn("[PHYSICS]: Got NaN Position on Character Create");
+            }
+
+            _parent_scene = parent_scene;
+
+            PID_D = pid_d;
+            PID_P = pid_p;
+            CAPSULE_RADIUS = capsule_radius;
+            m_tensor = tensor;
+            m_density = density;
+            heightFudgeFactor = height_fudge_factor;
+            walkDivisor = walk_divisor;
+            runDivisor = rundivisor;
+
+            // m_StandUpRotation =
+            //     new d.Matrix3(0.5f, 0.7071068f, 0.5f, -0.7071068f, 0f, 0.7071068f, 0.5f, -0.7071068f,
+            //                   0.5f);
+
+            for (int i = 0; i < 11; i++)
+            {
+                m_colliderarr[i] = false;
+            }
+            CAPSULE_LENGTH = (size.Z * 1.15f) - CAPSULE_RADIUS * 2.0f;
+            //m_log.Info("[SIZE]: " + CAPSULE_LENGTH.ToString());
+            m_tainted_CAPSULE_LENGTH = CAPSULE_LENGTH;
+
+            m_isPhysical = false; // current status: no ODE information exists
+            m_tainted_isPhysical = true; // new tainted status: need to create ODE information
+
+            _parent_scene.AddPhysicsActorTaint(this);
+            
+            m_name = avName;
+        }
+
+        public override int PhysicsActorType
+        {
+            get { return (int) ActorTypes.Agent; }
+            set { return; }
+        }
+
+        /// <summary>
+        /// If this is set, the avatar will move faster
+        /// </summary>
+        public override bool SetAlwaysRun
+        {
+            get { return m_alwaysRun; }
+            set { m_alwaysRun = value; }
+        }
+
+        public override uint LocalID
+        {
+            set { m_localID = value; }
+        }
+
+        public override bool Grabbed
+        {
+            set { return; }
+        }
+
+        public override bool Selected
+        {
+            set { return; }
+        }
+
+        public override float Buoyancy
+        {
+            get { return m_buoyancy; }
+            set { m_buoyancy = value; }
+        }
+
+        public override bool FloatOnWater
+        {
+            set { return; }
+        }
+
+        public override bool IsPhysical
+        {
+            get { return false; }
+            set { return; }
+        }
+
+        public override bool ThrottleUpdates
+        {
+            get { return false; }
+            set { return; }
+        }
+
+        public override bool Flying
+        {
+            get { return flying; }
+            set { flying = value; }
+        }
+
+        /// <summary>
+        /// Returns if the avatar is colliding in general.
+        /// This includes the ground and objects and avatar.
+        /// </summary>
+        public override bool IsColliding
+        {
+            get { return m_iscolliding; }
+            set
+            {
+                int i;
+                int truecount = 0;
+                int falsecount = 0;
+
+                if (m_colliderarr.Length >= 10)
+                {
+                    for (i = 0; i < 10; i++)
+                    {
+                        m_colliderarr[i] = m_colliderarr[i + 1];
+                    }
+                }
+                m_colliderarr[10] = value;
+
+                for (i = 0; i < 11; i++)
+                {
+                    if (m_colliderarr[i])
+                    {
+                        truecount++;
+                    }
+                    else
+                    {
+                        falsecount++;
+                    }
+                }
+
+                // Equal truecounts and false counts means we're colliding with something.
+
+                if (falsecount > 1.2*truecount)
+                {
+                    m_iscolliding = false;
+                }
+                else
+                {
+                    m_iscolliding = true;
+                }
+                if (m_wascolliding != m_iscolliding)
+                {
+                    //base.SendCollisionUpdate(new CollisionEventUpdate());
+                }
+                m_wascolliding = m_iscolliding;
+            }
+        }
+
+        /// <summary>
+        /// Returns if an avatar is colliding with the ground
+        /// </summary>
+        public override bool CollidingGround
+        {
+            get { return m_iscollidingGround; }
+            set
+            {
+                // Collisions against the ground are not really reliable
+                // So, to get a consistant value we have to average the current result over time
+                // Currently we use 1 second = 10 calls to this.
+                int i;
+                int truecount = 0;
+                int falsecount = 0;
+
+                if (m_colliderGroundarr.Length >= 10)
+                {
+                    for (i = 0; i < 10; i++)
+                    {
+                        m_colliderGroundarr[i] = m_colliderGroundarr[i + 1];
+                    }
+                }
+                m_colliderGroundarr[10] = value;
+
+                for (i = 0; i < 11; i++)
+                {
+                    if (m_colliderGroundarr[i])
+                    {
+                        truecount++;
+                    }
+                    else
+                    {
+                        falsecount++;
+                    }
+                }
+
+                // Equal truecounts and false counts means we're colliding with something.
+
+                if (falsecount > 1.2*truecount)
+                {
+                    m_iscollidingGround = false;
+                }
+                else
+                {
+                    m_iscollidingGround = true;
+                }
+                if (m_wascollidingGround != m_iscollidingGround)
+                {
+                    //base.SendCollisionUpdate(new CollisionEventUpdate());
+                }
+                m_wascollidingGround = m_iscollidingGround;
+            }
+        }
+
+        /// <summary>
+        /// Returns if the avatar is colliding with an object
+        /// </summary>
+        public override bool CollidingObj
+        {
+            get { return m_iscollidingObj; }
+            set
+            {
+                m_iscollidingObj = value;
+                if (value)
+                    m_pidControllerActive = false;
+                else
+                    m_pidControllerActive = true;
+            }
+        }
+
+        /// <summary>
+        /// turn the PID controller on or off.
+        /// The PID Controller will turn on all by itself in many situations
+        /// </summary>
+        /// <param name="status"></param>
+        public void SetPidStatus(bool status)
+        {
+            m_pidControllerActive = status;
+        }
+
+        public override bool Stopped
+        {
+            get { return _zeroFlag; }
+        }
+
+        /// <summary>
+        /// This 'puts' an avatar somewhere in the physics space.
+        /// Not really a good choice unless you 'know' it's a good
+        /// spot otherwise you're likely to orbit the avatar.
+        /// </summary>
+        public override Vector3 Position
+        {
+            get { return _position; }
+            set
+            {
+                if (Body == IntPtr.Zero || Shell == IntPtr.Zero)
+                {
+                    if (value.IsFinite())
+                    {
+                        if (value.Z > 9999999f)
+                        {
+                            value.Z = _parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
+                        }
+                        if (value.Z < -90000f)
+                        {
+                            value.Z = _parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
+                        }
+
+                        _position.X = value.X;
+                        _position.Y = value.Y;
+                        _position.Z = value.Z;
+
+                        m_taintPosition.X = value.X;
+                        m_taintPosition.Y = value.Y;
+                        m_taintPosition.Z = value.Z;
+                        _parent_scene.AddPhysicsActorTaint(this);
+                    }
+                    else
+                    {
+                        m_log.Warn("[PHYSICS]: Got a NaN Position from Scene on a Character");
+                    }
+                }
+            }
+        }
+
+        public override Vector3 RotationalVelocity
+        {
+            get { return m_rotationalVelocity; }
+            set { m_rotationalVelocity = value; }
+        }
+
+        /// <summary>
+        /// This property sets the height of the avatar only.  We use the height to make sure the avatar stands up straight
+        /// and use it to offset landings properly
+        /// </summary>
+        public override Vector3 Size
+        {
+            get { return new Vector3(CAPSULE_RADIUS * 2, CAPSULE_RADIUS * 2, CAPSULE_LENGTH); }
+            set
+            {
+                if (value.IsFinite())
+                {
+                    m_pidControllerActive = true;
+
+                    Vector3 SetSize = value;
+                    m_tainted_CAPSULE_LENGTH = (SetSize.Z*1.15f) - CAPSULE_RADIUS*2.0f;
+                    //m_log.Info("[SIZE]: " + CAPSULE_LENGTH.ToString());
+
+                    Velocity = Vector3.Zero;
+
+                    _parent_scene.AddPhysicsActorTaint(this);
+                }
+                else
+                {
+                    m_log.Warn("[PHYSICS]: Got a NaN Size from Scene on a Character");
+                }
+            }
+        }
+
+        private void AlignAvatarTiltWithCurrentDirectionOfMovement(Vector3 movementVector)
+        {
+            movementVector.Z = 0f;
+            float magnitude = (float)Math.Sqrt((double)(movementVector.X * movementVector.X + movementVector.Y * movementVector.Y));
+            if (magnitude < 0.1f) return;
+
+            // normalize the velocity vector
+            float invMagnitude = 1.0f / magnitude;
+            movementVector.X *= invMagnitude;
+            movementVector.Y *= invMagnitude;
+
+            // if we change the capsule heading too often, the capsule can fall down
+            // therefore we snap movement vector to just 1 of 4 predefined directions (ne, nw, se, sw),
+            // meaning only 4 possible capsule tilt orientations
+            if (movementVector.X > 0)
+            {
+                // east
+                if (movementVector.Y > 0)
+                {
+                    // northeast
+                    movementVector.X = (float)Math.Sqrt(2.0);
+                    movementVector.Y = (float)Math.Sqrt(2.0);
+                }
+                else
+                {
+                    // southeast
+                    movementVector.X = (float)Math.Sqrt(2.0);
+                    movementVector.Y = -(float)Math.Sqrt(2.0);
+                }
+            }
+            else
+            {
+                // west
+                if (movementVector.Y > 0)
+                {
+                    // northwest
+                    movementVector.X = -(float)Math.Sqrt(2.0);
+                    movementVector.Y = (float)Math.Sqrt(2.0);
+                }
+                else
+                {
+                    // southwest
+                    movementVector.X = -(float)Math.Sqrt(2.0);
+                    movementVector.Y = -(float)Math.Sqrt(2.0);
+                }
+            }
+
+
+            // movementVector.Z is zero
+
+            // calculate tilt components based on desired amount of tilt and current (snapped) heading.
+            // the "-" sign is to force the tilt to be OPPOSITE the direction of movement.
+            float xTiltComponent = -movementVector.X * m_tiltMagnitudeWhenProjectedOnXYPlane;
+            float yTiltComponent = -movementVector.Y * m_tiltMagnitudeWhenProjectedOnXYPlane;
+
+            //m_log.Debug("[PHYSICS] changing avatar tilt");
+            d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, xTiltComponent);
+            d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, xTiltComponent); // must be same as lowstop, else a different, spurious tilt is introduced
+            d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, yTiltComponent);
+            d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, yTiltComponent); // same as lowstop
+            d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, 0f);
+            d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0f); // same as lowstop
+        }
+
+        /// <summary>
+        /// This creates the Avatar's physical Surrogate at the position supplied
+        /// </summary>
+        /// <param name="npositionX"></param>
+        /// <param name="npositionY"></param>
+        /// <param name="npositionZ"></param>
+
+        // WARNING: This MUST NOT be called outside of ProcessTaints, else we can have unsynchronized access
+        // to ODE internals. ProcessTaints is called from within thread-locked Simulate(), so it is the only 
+        // place that is safe to call this routine AvatarGeomAndBodyCreation.
+        private void AvatarGeomAndBodyCreation(float npositionX, float npositionY, float npositionZ, float tensor)
+        {
+            //CAPSULE_LENGTH = -5;
+            //CAPSULE_RADIUS = -5;
+            int dAMotorEuler = 1;
+            _parent_scene.waitForSpaceUnlock(_parent_scene.space);
+            if (CAPSULE_LENGTH <= 0)
+            {
+                m_log.Warn("[PHYSICS]: The capsule size you specified in opensim.ini is invalid!  Setting it to the smallest possible size!");
+                CAPSULE_LENGTH = 0.01f;
+
+            }
+
+            if (CAPSULE_RADIUS <= 0)
+            {
+                m_log.Warn("[PHYSICS]: The capsule size you specified in opensim.ini is invalid!  Setting it to the smallest possible size!");
+                CAPSULE_RADIUS = 0.01f;
+
+            }
+            Shell = d.CreateCapsule(_parent_scene.space, CAPSULE_RADIUS, CAPSULE_LENGTH);
+
+            d.GeomSetCategoryBits(Shell, (int)m_collisionCategories);
+            d.GeomSetCollideBits(Shell, (int)m_collisionFlags);
+
+            d.MassSetCapsuleTotal(out ShellMass, m_mass, 2, CAPSULE_RADIUS, CAPSULE_LENGTH);
+            Body = d.BodyCreate(_parent_scene.world);
+            d.BodySetPosition(Body, npositionX, npositionY, npositionZ);
+            
+            _position.X = npositionX;
+            _position.Y = npositionY;
+            _position.Z = npositionZ;
+
+            
+            m_taintPosition.X = npositionX;
+            m_taintPosition.Y = npositionY;
+            m_taintPosition.Z = npositionZ;
+
+            d.BodySetMass(Body, ref ShellMass);
+            d.Matrix3 m_caprot;
+            // 90 Stand up on the cap of the capped cyllinder
+            if (_parent_scene.IsAvCapsuleTilted)
+            {
+                d.RFromAxisAndAngle(out m_caprot, 1, 0, 1, (float)(Math.PI / 2));
+            }
+            else
+            {
+                d.RFromAxisAndAngle(out m_caprot, 0, 0, 1, (float)(Math.PI / 2));
+            }
+
+
+            d.GeomSetRotation(Shell, ref m_caprot);
+            d.BodySetRotation(Body, ref m_caprot);
+
+            d.GeomSetBody(Shell, Body);
+
+
+            // The purpose of the AMotor here is to keep the avatar's physical
+            // surrogate from rotating while moving
+            Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero);
+            d.JointAttach(Amotor, Body, IntPtr.Zero);
+            d.JointSetAMotorMode(Amotor, dAMotorEuler);
+            d.JointSetAMotorNumAxes(Amotor, 3);
+            d.JointSetAMotorAxis(Amotor, 0, 0, 1, 0, 0);
+            d.JointSetAMotorAxis(Amotor, 1, 0, 0, 1, 0);
+            d.JointSetAMotorAxis(Amotor, 2, 0, 0, 0, 1);
+            d.JointSetAMotorAngle(Amotor, 0, 0);
+            d.JointSetAMotorAngle(Amotor, 1, 0);
+            d.JointSetAMotorAngle(Amotor, 2, 0);
+
+            // These lowstops and high stops are effectively (no wiggle room)
+            if (_parent_scene.IsAvCapsuleTilted)
+            {
+                d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, -0.000000000001f);
+                d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -0.000000000001f);
+                d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, -0.000000000001f);
+                d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0.000000000001f);
+                d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0.000000000001f);
+                d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0.000000000001f);
+            }
+            else
+            {
+                #region Documentation of capsule motor LowStop and HighStop parameters
+                // Intentionally introduce some tilt into the capsule by setting
+                // the motor stops to small epsilon values. This small tilt prevents
+                // the capsule from falling into the terrain; a straight-up capsule
+                // (with -0..0 motor stops) falls into the terrain for reasons yet
+                // to be comprehended in their entirety.
+                #endregion
+                AlignAvatarTiltWithCurrentDirectionOfMovement(Vector3.Zero);
+                d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, 0.08f);
+                d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -0f);
+                d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, 0.08f);
+                d.JointSetAMotorParam(Amotor, (int)dParam.HiStop,  0.08f); // must be same as lowstop, else a different, spurious tilt is introduced
+                d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0f); // same as lowstop
+                d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0.08f); // same as lowstop
+            }
+
+            // Fudge factor is 1f by default, we're setting it to 0.  We don't want it to Fudge or the
+            // capped cyllinder will fall over
+            d.JointSetAMotorParam(Amotor, (int)dParam.FudgeFactor, 0f);
+            d.JointSetAMotorParam(Amotor, (int)dParam.FMax, tensor);
+
+            //d.Matrix3 bodyrotation = d.BodyGetRotation(Body);
+            //d.QfromR(
+            //d.Matrix3 checkrotation = new d.Matrix3(0.7071068,0.5, -0.7071068,
+            //
+            //m_log.Info("[PHYSICSAV]: Rotation: " + bodyrotation.M00 + " : " + bodyrotation.M01 + " : " + bodyrotation.M02 + " : " + bodyrotation.M10 + " : " + bodyrotation.M11 + " : " + bodyrotation.M12 + " : " + bodyrotation.M20 + " : " + bodyrotation.M21 + " : " + bodyrotation.M22);
+            //standupStraight();
+        }
+
+        //
+        /// <summary>
+        /// Uses the capped cyllinder volume formula to calculate the avatar's mass.
+        /// This may be used in calculations in the scene/scenepresence
+        /// </summary>
+        public override float Mass
+        {
+            get
+            {
+                float AVvolume = (float) (Math.PI*Math.Pow(CAPSULE_RADIUS, 2)*CAPSULE_LENGTH);
+                return m_density*AVvolume;
+            }
+        }
+        public override void link(PhysicsActor obj)
+        {
+
+        }
+
+        public override void delink()
+        {
+
+        }
+
+        public override void LockAngularMotion(Vector3 axis)
+        {
+
+        }
+
+//      This code is very useful. Written by DanX0r. We're just not using it right now.
+//      Commented out to prevent a warning.
+//
+//         private void standupStraight()
+//         {
+//             // The purpose of this routine here is to quickly stabilize the Body while it's popped up in the air.
+//             // The amotor needs a few seconds to stabilize so without it, the avatar shoots up sky high when you
+//             // change appearance and when you enter the simulator
+//             // After this routine is done, the amotor stabilizes much quicker
+//             d.Vector3 feet;
+//             d.Vector3 head;
+//             d.BodyGetRelPointPos(Body, 0.0f, 0.0f, -1.0f, out feet);
+//             d.BodyGetRelPointPos(Body, 0.0f, 0.0f, 1.0f, out head);
+//             float posture = head.Z - feet.Z;
+
+//             // restoring force proportional to lack of posture:
+//             float servo = (2.5f - posture) * POSTURE_SERVO;
+//             d.BodyAddForceAtRelPos(Body, 0.0f, 0.0f, servo, 0.0f, 0.0f, 1.0f);
+//             d.BodyAddForceAtRelPos(Body, 0.0f, 0.0f, -servo, 0.0f, 0.0f, -1.0f);
+//             //d.Matrix3 bodyrotation = d.BodyGetRotation(Body);
+//             //m_log.Info("[PHYSICSAV]: Rotation: " + bodyrotation.M00 + " : " + bodyrotation.M01 + " : " + bodyrotation.M02 + " : " + bodyrotation.M10 + " : " + bodyrotation.M11 + " : " + bodyrotation.M12 + " : " + bodyrotation.M20 + " : " + bodyrotation.M21 + " : " + bodyrotation.M22);
+//         }
+
+        public override Vector3 Force
+        {
+            get { return _target_velocity; }
+            set { return; }
+        }
+
+        public override int VehicleType
+        {
+            get { return 0; }
+            set { return; }
+        }
+
+        public override void VehicleFloatParam(int param, float value)
+        {
+
+        }
+
+        public override void VehicleVectorParam(int param, Vector3 value)
+        {
+
+        }
+
+        public override void VehicleRotationParam(int param, Quaternion rotation)
+        {
+
+        }
+
+        public override void SetVolumeDetect(int param)
+        {
+
+        }
+
+        public override Vector3 CenterOfMass
+        {
+            get { return Vector3.Zero; }
+        }
+
+        public override Vector3 GeometricCenter
+        {
+            get { return Vector3.Zero; }
+        }
+
+        public override PrimitiveBaseShape Shape
+        {
+            set { return; }
+        }
+
+        public override Vector3 Velocity
+        {
+            get {
+                // There's a problem with Vector3.Zero! Don't Use it Here!
+                if (_zeroFlag)
+                    return Vector3.Zero;
+                m_lastUpdateSent = false;
+                return _velocity;
+            }
+            set
+            {
+                if (value.IsFinite())
+                {
+                    m_pidControllerActive = true;
+                    _target_velocity = value;
+                }
+                else
+                {
+                    m_log.Warn("[PHYSICS]: Got a NaN velocity from Scene in a Character");
+                }
+            }
+        }
+
+        public override Vector3 Torque
+        {
+            get { return Vector3.Zero; }
+            set { return; }
+        }
+
+        public override float CollisionScore
+        {
+            get { return 0f; }
+            set { }
+        }
+
+        public override bool Kinematic
+        {
+            get { return false; }
+            set { }
+        }
+
+        public override Quaternion Orientation
+        {
+            get { return Quaternion.Identity; }
+            set {
+                //Matrix3 or = Orientation.ToRotationMatrix();
+                //d.Matrix3 ord = new d.Matrix3(or.m00, or.m10, or.m20, or.m01, or.m11, or.m21, or.m02, or.m12, or.m22);
+                //d.BodySetRotation(Body, ref ord);
+            }
+        }
+
+        public override Vector3 Acceleration
+        {
+            get { return _acceleration; }
+        }
+
+        public void SetAcceleration(Vector3 accel)
+        {
+            m_pidControllerActive = true;
+            _acceleration = accel;
+        }
+
+        /// <summary>
+        /// Adds the force supplied to the Target Velocity
+        /// The PID controller takes this target velocity and tries to make it a reality
+        /// </summary>
+        /// <param name="force"></param>
+        public override void AddForce(Vector3 force, bool pushforce)
+        {
+            if (force.IsFinite())
+            {
+                if (pushforce)
+                {
+                    m_pidControllerActive = false;
+                    force *= 100f;
+                    doForce(force);
+                    // If uncommented, things get pushed off world
+                    //
+                    // m_log.Debug("Push!");
+                    // _target_velocity.X += force.X;
+                    // _target_velocity.Y += force.Y;
+                    // _target_velocity.Z += force.Z;
+                }
+                else
+                {
+                    m_pidControllerActive = true;
+                    _target_velocity.X += force.X;
+                    _target_velocity.Y += force.Y;
+                    _target_velocity.Z += force.Z;
+                }
+            }
+            else
+            {
+                m_log.Warn("[PHYSICS]: Got a NaN force applied to a Character");
+            }
+            //m_lastUpdateSent = false;
+        }
+
+        public override void AddAngularForce(Vector3 force, bool pushforce)
+        {
+
+        }
+
+        /// <summary>
+        /// After all of the forces add up with 'add force' we apply them with doForce
+        /// </summary>
+        /// <param name="force"></param>
+        public void doForce(Vector3 force)
+        {
+            if (!collidelock)
+            {
+                d.BodyAddForce(Body, force.X, force.Y, force.Z);
+                //d.BodySetRotation(Body, ref m_StandUpRotation);
+                //standupStraight();
+
+            }
+        }
+
+        public override void SetMomentum(Vector3 momentum)
+        {
+        }
+
+
+        /// <summary>
+        /// Called from Simulate
+        /// This is the avatar's movement control + PID Controller
+        /// </summary>
+        /// <param name="timeStep"></param>
+        public void Move(float timeStep, List<OdeCharacter> defects)
+        {
+            //  no lock; for now it's only called from within Simulate()
+
+            // If the PID Controller isn't active then we set our force
+            // calculating base velocity to the current position
+
+            if (Body == IntPtr.Zero)
+                return;
+
+            if (m_pidControllerActive == false)
+            {
+                _zeroPosition = d.BodyGetPosition(Body);
+            }
+            //PidStatus = true;
+
+            d.Vector3 localpos = d.BodyGetPosition(Body);
+            Vector3 localPos = new Vector3(localpos.X, localpos.Y, localpos.Z);
+            
+            if (!localPos.IsFinite())
+            {
+
+                m_log.Warn("[PHYSICS]: Avatar Position is non-finite!");
+                defects.Add(this);
+                // _parent_scene.RemoveCharacter(this);
+
+                // destroy avatar capsule and related ODE data
+                if (Amotor != IntPtr.Zero)
+                {
+                    // Kill the Amotor
+                    d.JointDestroy(Amotor);
+                    Amotor = IntPtr.Zero;
+                }
+
+                //kill the Geometry
+                _parent_scene.waitForSpaceUnlock(_parent_scene.space);
+
+                if (Body != IntPtr.Zero)
+                {
+                    //kill the body
+                    d.BodyDestroy(Body);
+
+                    Body = IntPtr.Zero;
+                }
+
+                if (Shell != IntPtr.Zero)
+                {
+                    d.GeomDestroy(Shell);
+                    _parent_scene.geom_name_map.Remove(Shell);
+                    Shell = IntPtr.Zero;
+                }
+
+                return;
+            }
+
+            Vector3 vec = Vector3.Zero;
+            d.Vector3 vel = d.BodyGetLinearVel(Body);
+
+            float movementdivisor = 1f;
+
+            if (!m_alwaysRun)
+            {
+                movementdivisor = walkDivisor;
+            }
+            else
+            {
+                movementdivisor = runDivisor;
+            }
+
+            //  if velocity is zero, use position control; otherwise, velocity control
+            if (_target_velocity.X == 0.0f && _target_velocity.Y == 0.0f && _target_velocity.Z == 0.0f && m_iscolliding)
+            {
+                //  keep track of where we stopped.  No more slippin' & slidin'
+                if (!_zeroFlag)
+                {
+                    _zeroFlag = true;
+                    _zeroPosition = d.BodyGetPosition(Body);
+                }
+                if (m_pidControllerActive)
+                {
+                    // We only want to deactivate the PID Controller if we think we want to have our surrogate
+                    // react to the physics scene by moving it's position.
+                    // Avatar to Avatar collisions
+                    // Prim to avatar collisions
+
+                    d.Vector3 pos = d.BodyGetPosition(Body);
+                    vec.X = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2);
+                    vec.Y = (_target_velocity.Y - vel.Y)*(PID_D) + (_zeroPosition.Y - pos.Y)* (PID_P * 2);
+                    if (flying)
+                    {
+                        vec.Z = (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P;
+                    }
+                }
+                //PidStatus = true;
+            }
+            else
+            {
+                m_pidControllerActive = true;
+                _zeroFlag = false;
+                if (m_iscolliding && !flying)
+                {
+                    // We're standing on something
+                    vec.X = ((_target_velocity.X / movementdivisor) - vel.X) * (PID_D);
+                    vec.Y = ((_target_velocity.Y / movementdivisor) - vel.Y) * (PID_D);
+                }
+                else if (m_iscolliding && flying)
+                {
+                    // We're flying and colliding with something
+                    vec.X = ((_target_velocity.X/movementdivisor) - vel.X)*(PID_D / 16);
+                    vec.Y = ((_target_velocity.Y/movementdivisor) - vel.Y)*(PID_D / 16);
+                }
+                else if (!m_iscolliding && flying)
+                {
+                    // we're in mid air suspended
+                    vec.X = ((_target_velocity.X / movementdivisor) - vel.X) * (PID_D/6);
+                    vec.Y = ((_target_velocity.Y / movementdivisor) - vel.Y) * (PID_D/6);
+                }
+
+                if (m_iscolliding && !flying && _target_velocity.Z > 0.0f)
+                {
+                    // We're colliding with something and we're not flying but we're moving
+                    // This means we're walking or running.
+                    d.Vector3 pos = d.BodyGetPosition(Body);
+                    vec.Z = (_target_velocity.Z - vel.Z)*PID_D + (_zeroPosition.Z - pos.Z)*PID_P;
+                    if (_target_velocity.X > 0)
+                    {
+                        vec.X = ((_target_velocity.X - vel.X)/1.2f)*PID_D;
+                    }
+                    if (_target_velocity.Y > 0)
+                    {
+                        vec.Y = ((_target_velocity.Y - vel.Y)/1.2f)*PID_D;
+                    }
+                }
+                else if (!m_iscolliding && !flying)
+                {
+                    // we're not colliding and we're not flying so that means we're falling!
+                    // m_iscolliding includes collisions with the ground.
+
+                    // d.Vector3 pos = d.BodyGetPosition(Body);
+                    if (_target_velocity.X > 0)
+                    {
+                        vec.X = ((_target_velocity.X - vel.X)/1.2f)*PID_D;
+                    }
+                    if (_target_velocity.Y > 0)
+                    {
+                        vec.Y = ((_target_velocity.Y - vel.Y)/1.2f)*PID_D;
+                    }
+                }
+
+                if (flying)
+                {
+                    vec.Z = (_target_velocity.Z - vel.Z) * (PID_D);
+                }
+            }
+            if (flying)
+            {
+                vec.Z += ((-1 * _parent_scene.gravityz)*m_mass);
+
+                //Added for auto fly height. Kitto Flora
+                //d.Vector3 pos = d.BodyGetPosition(Body);
+                float target_altitude = _parent_scene.GetTerrainHeightAtXY(_position.X, _position.Y) + MinimumGroundFlightOffset;
+                
+                if (_position.Z < target_altitude)
+                {
+                    vec.Z += (target_altitude - _position.Z) * PID_P * 5.0f;
+                }
+                // end add Kitto Flora
+            }
+            if (vec.IsFinite())
+            {
+                doForce(vec);
+                if (!_zeroFlag)
+                {
+                  AlignAvatarTiltWithCurrentDirectionOfMovement(vec);
+                }
+            }
+            else
+            {
+                m_log.Warn("[PHYSICS]: Got a NaN force vector in Move()");
+                m_log.Warn("[PHYSICS]: Avatar Position is non-finite!");
+                defects.Add(this);
+                // _parent_scene.RemoveCharacter(this);
+                // destroy avatar capsule and related ODE data
+                if (Amotor != IntPtr.Zero)
+                {
+                    // Kill the Amotor
+                    d.JointDestroy(Amotor);
+                    Amotor = IntPtr.Zero;
+                }
+                //kill the Geometry
+                _parent_scene.waitForSpaceUnlock(_parent_scene.space);
+
+                if (Body != IntPtr.Zero)
+                {
+                    //kill the body
+                    d.BodyDestroy(Body);
+
+                    Body = IntPtr.Zero;
+                }
+
+                if (Shell != IntPtr.Zero)
+                {
+                    d.GeomDestroy(Shell);
+                    _parent_scene.geom_name_map.Remove(Shell);
+                    Shell = IntPtr.Zero;
+                }
+            }
+        }
+
+        /// <summary>
+        /// Updates the reported position and velocity.  This essentially sends the data up to ScenePresence.
+        /// </summary>
+        public void UpdatePositionAndVelocity()
+        {
+            //  no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit!
+            d.Vector3 vec;
+            try
+            {
+                vec = d.BodyGetPosition(Body);
+            }
+            catch (NullReferenceException)
+            {
+                bad = true;
+                _parent_scene.BadCharacter(this);
+                vec = new d.Vector3(_position.X, _position.Y, _position.Z);
+                base.RaiseOutOfBounds(_position); // Tells ScenePresence that there's a problem!
+                m_log.WarnFormat("[ODEPLUGIN]: Avatar Null reference for Avatar {0}, physical actor {1}", m_name, m_uuid);
+            }
+            
+
+            //  kluge to keep things in bounds.  ODE lets dead avatars drift away (they should be removed!)
+            if (vec.X < 0.0f) vec.X = 0.0f;
+            if (vec.Y < 0.0f) vec.Y = 0.0f;
+            if (vec.X > (int)_parent_scene.WorldExtents.X - 0.05f) vec.X = (int)_parent_scene.WorldExtents.X - 0.05f;
+            if (vec.Y > (int)_parent_scene.WorldExtents.Y - 0.05f) vec.Y = (int)_parent_scene.WorldExtents.Y - 0.05f;
+
+            _position.X = vec.X;
+            _position.Y = vec.Y;
+            _position.Z = vec.Z;
+
+            // Did we move last? = zeroflag
+            // This helps keep us from sliding all over
+
+            if (_zeroFlag)
+            {
+                _velocity.X = 0.0f;
+                _velocity.Y = 0.0f;
+                _velocity.Z = 0.0f;
+
+                // Did we send out the 'stopped' message?
+                if (!m_lastUpdateSent)
+                {
+                    m_lastUpdateSent = true;
+                    //base.RequestPhysicsterseUpdate();
+
+                }
+            }
+            else
+            {
+                m_lastUpdateSent = false;
+                try
+                {
+                    vec = d.BodyGetLinearVel(Body);
+                }
+                catch (NullReferenceException)
+                {
+                    vec.X = _velocity.X;
+                    vec.Y = _velocity.Y;
+                    vec.Z = _velocity.Z;
+                }
+                _velocity.X = (vec.X);
+                _velocity.Y = (vec.Y);
+
+                _velocity.Z = (vec.Z);
+
+                if (_velocity.Z < -6 && !m_hackSentFall)
+                {
+                    m_hackSentFall = true;
+                    m_pidControllerActive = false;
+                }
+                else if (flying && !m_hackSentFly)
+                {
+                    //m_hackSentFly = true;
+                    //base.SendCollisionUpdate(new CollisionEventUpdate());
+                }
+                else
+                {
+                    m_hackSentFly = false;
+                    m_hackSentFall = false;
+                }
+            }
+        }
+
+        /// <summary>
+        /// Cleanup the things we use in the scene.
+        /// </summary>
+        public void Destroy()
+        {
+            m_tainted_isPhysical = false;
+            _parent_scene.AddPhysicsActorTaint(this);
+        }
+
+        public override void CrossingFailure()
+        {
+        }
+
+        public override Vector3 PIDTarget { set { return; } }
+        public override bool PIDActive { set { return; } }
+        public override float PIDTau { set { return; } }
+
+        public override float PIDHoverHeight { set { return; } }
+        public override bool PIDHoverActive { set { return; } }
+        public override PIDHoverType PIDHoverType { set { return; } }
+        public override float PIDHoverTau { set { return; } }
+
+        public override Quaternion APIDTarget{ set { return; } }
+
+        public override bool APIDActive{ set { return; } }
+
+        public override float APIDStrength{ set { return; } }
+
+        public override float APIDDamping{ set { return; } }
+
+                
+        public override void SubscribeEvents(int ms)
+        {
+            m_requestedUpdateFrequency = ms;
+            m_eventsubscription = ms;
+            _parent_scene.addCollisionEventReporting(this);
+        }
+        public override void UnSubscribeEvents()
+        {
+            _parent_scene.remCollisionEventReporting(this);
+            m_requestedUpdateFrequency = 0;
+            m_eventsubscription = 0;
+        }
+        public void AddCollisionEvent(uint CollidedWith, ContactPoint contact)
+        {
+            if (m_eventsubscription > 0)
+            {
+                CollisionEventsThisFrame.addCollider(CollidedWith, contact);
+            }
+        }
+
+        public void SendCollisions()
+        {
+            if (m_eventsubscription > m_requestedUpdateFrequency)
+            {
+                if (CollisionEventsThisFrame != null)
+                {
+                    base.SendCollisionUpdate(CollisionEventsThisFrame);
+                }
+                CollisionEventsThisFrame = new CollisionEventUpdate();
+                m_eventsubscription = 0;
+            }
+        }
+        public override bool SubscribedEvents()
+        {
+            if (m_eventsubscription > 0)
+                return true;
+            return false;
+        }
+
+        public void ProcessTaints(float timestep)
+        {
+
+            if (m_tainted_isPhysical != m_isPhysical)
+            {
+                if (m_tainted_isPhysical)
+                {
+                    // Create avatar capsule and related ODE data
+                    if (!(Shell == IntPtr.Zero && Body == IntPtr.Zero && Amotor == IntPtr.Zero))
+                    {
+                        m_log.Warn("[PHYSICS]: re-creating the following avatar ODE data, even though it already exists - "
+                            + (Shell!=IntPtr.Zero ? "Shell ":"")
+                            + (Body!=IntPtr.Zero ? "Body ":"")
+                            + (Amotor!=IntPtr.Zero ? "Amotor ":""));
+                    }
+                    AvatarGeomAndBodyCreation(_position.X, _position.Y, _position.Z, m_tensor);
+                    
+                    _parent_scene.geom_name_map[Shell] = m_name;
+                    _parent_scene.actor_name_map[Shell] = (PhysicsActor)this;
+                    _parent_scene.AddCharacter(this);
+                }
+                else
+                {
+                    _parent_scene.RemoveCharacter(this);
+                    // destroy avatar capsule and related ODE data
+                    if (Amotor != IntPtr.Zero)
+                    {
+                        // Kill the Amotor
+                        d.JointDestroy(Amotor);
+                        Amotor = IntPtr.Zero;
+                    }
+                    //kill the Geometry
+                    _parent_scene.waitForSpaceUnlock(_parent_scene.space);
+
+                    if (Body != IntPtr.Zero)
+                    {
+                        //kill the body
+                        d.BodyDestroy(Body);
+
+                        Body = IntPtr.Zero;
+                    }
+
+                    if (Shell != IntPtr.Zero)
+                    {
+                        d.GeomDestroy(Shell);
+                        _parent_scene.geom_name_map.Remove(Shell);
+                        Shell = IntPtr.Zero;
+                    }
+
+                }
+
+                m_isPhysical = m_tainted_isPhysical;
+            }
+
+            if (m_tainted_CAPSULE_LENGTH != CAPSULE_LENGTH)
+            {
+                if (Shell != IntPtr.Zero && Body != IntPtr.Zero && Amotor != IntPtr.Zero)
+                {
+
+                    m_pidControllerActive = true;
+                    // no lock needed on _parent_scene.OdeLock because we are called from within the thread lock in OdePlugin's simulate()
+                    d.JointDestroy(Amotor);
+                    float prevCapsule = CAPSULE_LENGTH;
+                    CAPSULE_LENGTH = m_tainted_CAPSULE_LENGTH;
+                    //m_log.Info("[SIZE]: " + CAPSULE_LENGTH.ToString());
+                    d.BodyDestroy(Body);
+                    d.GeomDestroy(Shell);
+                    AvatarGeomAndBodyCreation(_position.X, _position.Y,
+                                      _position.Z + (Math.Abs(CAPSULE_LENGTH - prevCapsule) * 2), m_tensor);
+                    Velocity = Vector3.Zero;
+
+                    _parent_scene.geom_name_map[Shell] = m_name;
+                    _parent_scene.actor_name_map[Shell] = (PhysicsActor)this;
+                }
+                else
+                {
+                    m_log.Warn("[PHYSICS]: trying to change capsule size, but the following ODE data is missing - " 
+                        + (Shell==IntPtr.Zero ? "Shell ":"")
+                        + (Body==IntPtr.Zero ? "Body ":"")
+                        + (Amotor==IntPtr.Zero ? "Amotor ":""));
+                }
+            }
+
+            if (!m_taintPosition.ApproxEquals(_position, 0.05f))
+            {
+                if (Body != IntPtr.Zero)
+                {
+                    d.BodySetPosition(Body, m_taintPosition.X, m_taintPosition.Y, m_taintPosition.Z);
+
+                    _position.X = m_taintPosition.X;
+                    _position.Y = m_taintPosition.Y;
+                    _position.Z = m_taintPosition.Z;
+                }
+            }
+
+        }
+
+        internal void AddCollisionFrameTime(int p)
+        {
+            // protect it from overflow crashing
+            if (m_eventsubscription + p >= int.MaxValue)
+                m_eventsubscription = 0;
+            m_eventsubscription += p;
+        }
+    }
+}
diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.c_comments b/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.c_comments
new file mode 100644
index 0000000..1060aa6
--- /dev/null
+++ b/OpenSim/Region/Physics/ChOdePlugin/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/Physics/ChOdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs
new file mode 100644
index 0000000..78b15be
--- /dev/null
+++ b/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs
@@ -0,0 +1,673 @@
+/*
+ * 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.
+ * 
+ */
+
+/* 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.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;                 // 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;                   // 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;
+            }
+            
+        }//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)
+        {
+                        // 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 = 1f;
+                    m_verticalAttractionTimescale = 10f;
+                    // m_bankingEfficiency = -0.2f;
+                    // m_bankingMix = 1;
+                    // m_bankingTimescale = 1;
+                    // m_referenceFrame = Quaternion.Identity;
+                    m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT);
+                    m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.HOVER_UP_ONLY |
+                                VehicleFlag.LIMIT_MOTOR_UP);
+                    break;
+                case Vehicle.TYPE_BOAT:
+                    m_linearFrictionTimescale = new Vector3(10, 3, 2);
+                    m_angularFrictionTimescale = new Vector3(10,10,10);
+                    m_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_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 = 2f;
+                    // m_bankingEfficiency = 1;
+                    // m_bankingMix = 0.7f;
+                    // m_bankingTimescale = 2;
+                    // m_referenceFrame = Quaternion.Identity;
+                    m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
+                        VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY | VehicleFlag.LIMIT_MOTOR_UP);
+                    m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY);
+                    break;
+                case Vehicle.TYPE_BALLOON:
+                    m_linearFrictionTimescale = new Vector3(5, 5, 5);
+                    m_angularFrictionTimescale = new Vector3(10, 10, 10);
+                    m_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_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
+                        VehicleFlag.HOVER_UP_ONLY | VehicleFlag.LIMIT_MOTOR_UP);
+                    m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT);
+                    break;
+
+            }
+        }//end SetDefaultsForType
+
+        internal void Enable(IntPtr pBody, OdeScene pParentScene)
+        {
+            if (m_type == Vehicle.TYPE_NONE)
+                return;
+
+            m_body = pBody;
+        }
+
+        internal void 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 * 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 and Buoyancy
+                        // 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;
+                                // 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);
+                                        //KF: 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);        
+            // 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
+                        */
+//if(frcount == 0) Console.WriteLine("MoveAngular ");   
+        
+                // 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; // tba: + bank + deflection
+                        
+                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
+        }
+}
diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
new file mode 100644
index 0000000..983431d
--- /dev/null
+++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
@@ -0,0 +1,3271 @@
+/* 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.
+ */
+
+/*
+ * 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.
+ */
+using System;
+using System.Collections.Generic;
+using System.Reflection;
+using System.Runtime.InteropServices;
+using System.Threading;
+using log4net;
+using OpenMetaverse;
+using Ode.NET;
+using OpenSim.Framework;
+using OpenSim.Region.Physics.Manager;
+
+namespace OpenSim.Region.Physics.OdePlugin
+{
+    /// <summary>
+    /// Various properties that ODE uses for AMotors but isn't exposed in ODE.NET so we must define them ourselves.
+    /// </summary>
+
+    public class OdePrim : PhysicsActor
+    {
+        private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
+
+        private Vector3 _position;
+        private Vector3 _velocity;
+        private Vector3 _torque;
+        private Vector3 m_lastVelocity;
+        private Vector3 m_lastposition;
+        private Quaternion m_lastorientation = new Quaternion();
+        private Vector3 m_rotationalVelocity;
+        private Vector3 _size;
+        private Vector3 _acceleration;
+        // private d.Vector3 _zeroPosition = new d.Vector3(0.0f, 0.0f, 0.0f);
+        private Quaternion _orientation;
+        private Vector3 m_taintposition;
+        private Vector3 m_taintsize;
+        private Vector3 m_taintVelocity;
+        private Vector3 m_taintTorque;
+        private Quaternion m_taintrot;
+        private Vector3 m_angularlock = Vector3.One;
+        private Vector3 m_taintAngularLock = Vector3.One;
+        private IntPtr Amotor = IntPtr.Zero;
+
+        private Vector3 m_PIDTarget;
+        private float m_PIDTau;
+        private float PID_D = 35f;
+        private float PID_G = 25f;
+        private bool m_usePID = false;
+        
+        private Quaternion m_APIDTarget = new Quaternion();
+        private float m_APIDStrength = 0.5f;
+        private float m_APIDDamping = 0.5f;
+        private bool m_useAPID = false;
+
+        // 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;
+        public bool m_returnCollisions;
+
+        // Default we're a Geometry
+        private CollisionCategories m_collisionCategories = (CollisionCategories.Geom);
+
+        // Default, Collide with Other Geometries, spaces and Bodies
+        private CollisionCategories m_collisionFlags = m_default_collisionFlags;
+
+        public bool m_taintremove;
+        public bool m_taintdisable;
+        public bool m_disabled;
+        public bool m_taintadd;
+        public bool m_taintselected;
+        public bool m_taintCollidesWater;
+
+        public uint m_localID;
+
+        //public GCHandle gc;
+        private CollisionLocker ode;
+
+        private bool m_taintforce = false;
+        private bool m_taintaddangularforce = false;
+        private Vector3 m_force;
+        private List<Vector3> m_forcelist = new List<Vector3>();
+        private List<Vector3> m_angularforcelist = new List<Vector3>();
+
+        private IMesh _mesh;
+        private PrimitiveBaseShape _pbs;
+        private OdeScene _parent_scene;
+        public IntPtr m_targetSpace = IntPtr.Zero;
+        public IntPtr prim_geom;
+        public IntPtr prev_geom;
+        public IntPtr _triMeshData;
+
+        private IntPtr _linkJointGroup = IntPtr.Zero;
+        private PhysicsActor _parent;
+        private PhysicsActor m_taintparent;
+
+        private List<OdePrim> childrenPrim = new List<OdePrim>();
+
+        private bool iscolliding;
+        private bool m_isphysical;
+        private bool m_isSelected;
+
+        internal bool m_isVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively
+
+        private bool m_throttleUpdates;
+        private int throttleCounter;
+        public int m_interpenetrationcount;
+        public float m_collisionscore;
+        public int m_roundsUnderMotionThreshold;
+        private int m_crossingfailures;
+
+        public bool outofBounds;
+        private float m_density = 10.000006836f; // Aluminum g/cm3;
+
+        public bool _zeroFlag;
+        private bool m_lastUpdateSent;
+
+        public IntPtr Body = IntPtr.Zero;
+        public String m_primName;
+        private Vector3 _target_velocity;
+        public d.Mass pMass;
+
+        public int m_eventsubscription;
+        private CollisionEventUpdate CollisionEventsThisFrame;
+
+        private IntPtr m_linkJoint = IntPtr.Zero;
+
+        public volatile bool childPrim;
+
+        private ODEDynamics m_vehicle;
+
+        internal int m_material = (int)Material.Wood;
+        
+        private int frcount = 0;                                                                                // Used to limit dynamics debug output to 
+
+
+        public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size,
+                       Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical, CollisionLocker dode)
+        {
+            m_vehicle = new ODEDynamics();
+            //gc = GCHandle.Alloc(prim_geom, GCHandleType.Pinned);
+            ode = dode;
+            if (!pos.IsFinite())
+            {
+                pos = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f),
+                    parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f);
+                m_log.Warn("[PHYSICS]: Got nonFinite Object create Position");
+            }
+            _position = pos;
+            m_taintposition = pos;
+            PID_D = parent_scene.bodyPIDD;
+            PID_G = parent_scene.bodyPIDG;
+            m_density = parent_scene.geomDefaultDensity;
+            // m_tensor = parent_scene.bodyMotorJointMaxforceTensor;
+            body_autodisable_frames = parent_scene.bodyFramesAutoDisable;
+
+
+            prim_geom = IntPtr.Zero;
+            prev_geom = IntPtr.Zero;
+
+            if (!pos.IsFinite())
+            {
+                size = new Vector3(0.5f, 0.5f, 0.5f);
+                m_log.Warn("[PHYSICS]: Got nonFinite Object create Size");
+            }
+
+            if (size.X <= 0) size.X = 0.01f;
+            if (size.Y <= 0) size.Y = 0.01f;
+            if (size.Z <= 0) size.Z = 0.01f;
+
+            _size = size;
+            m_taintsize = _size;
+
+            if (!QuaternionIsFinite(rotation))
+            {
+                rotation = Quaternion.Identity;
+                m_log.Warn("[PHYSICS]: Got nonFinite Object create Rotation");
+            }
+
+            _orientation = rotation;
+            m_taintrot = _orientation;
+            _mesh = mesh;
+            _pbs = pbs;
+
+            _parent_scene = parent_scene;
+            m_targetSpace = (IntPtr)0;
+
+            if (pos.Z < 0)
+                m_isphysical = false;
+            else
+            {
+                m_isphysical = pisPhysical;
+                // If we're physical, we need to be in the master space for now.
+                // linksets *should* be in a space together..  but are not currently
+                if (m_isphysical)
+                    m_targetSpace = _parent_scene.space;
+            }
+            m_primName = primName;
+            m_taintadd = true;
+            _parent_scene.AddPhysicsActorTaint(this);
+            //  don't do .add() here; old geoms get recycled with the same hash
+        }
+
+        public override int PhysicsActorType
+        {
+            get { return (int) ActorTypes.Prim; }
+            set { return; }
+        }
+
+        public override bool SetAlwaysRun
+        {
+            get { return false; }
+            set { return; }
+        }
+
+        public override uint LocalID
+        {
+            set {
+                //m_log.Info("[PHYSICS]: Setting TrackerID: " + value);
+                m_localID = value; }
+        }
+
+        public override bool Grabbed
+        {
+            set { return; }
+        }
+
+        public override bool Selected
+        {
+            set {
+        
+            
+                // This only makes the object not collidable if the object
+                // is physical or the object is modified somehow *IN THE FUTURE*
+                // without this, if an avatar selects prim, they can walk right
+                // through it while it's selected
+                m_collisionscore = 0;
+                if ((m_isphysical && !_zeroFlag) || !value)
+                {
+                    m_taintselected = value;
+                    _parent_scene.AddPhysicsActorTaint(this);
+                }
+                else
+                {
+                    m_taintselected = value;
+                    m_isSelected = value;
+                }
+                if(m_isSelected) disableBodySoft();         
+            }
+        }
+
+        public void SetGeom(IntPtr geom)
+        {
+            prev_geom = prim_geom;
+            prim_geom = geom;
+//Console.WriteLine("SetGeom to " + prim_geom + " for " + m_primName);     
+            if (prim_geom != IntPtr.Zero)
+            {
+                d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
+                d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
+            }
+
+            if (childPrim)
+            {
+                if (_parent != null && _parent is OdePrim)
+                {
+                    OdePrim parent = (OdePrim)_parent;
+//Console.WriteLine("SetGeom calls ChildSetGeom");                    
+                    parent.ChildSetGeom(this);
+                }
+            }
+            //m_log.Warn("Setting Geom to: " + prim_geom);
+        }
+
+        
+
+        public void enableBodySoft()
+        {
+            if (!childPrim)
+            {
+                if (m_isphysical && Body != IntPtr.Zero)
+                {
+                    d.BodyEnable(Body);
+                    if (m_vehicle.Type != Vehicle.TYPE_NONE)
+                            m_vehicle.Enable(Body, _parent_scene);
+                }
+
+                m_disabled = false;
+            }
+        }
+
+        public void disableBodySoft()
+        {
+            m_disabled = true;
+
+            if (m_isphysical && Body != IntPtr.Zero)
+            {
+                d.BodyDisable(Body);
+            }
+        }
+
+        public void enableBody()
+        {
+            // Don't enable this body if we're a child prim
+            // this should be taken care of in the parent function not here
+            if (!childPrim)
+            {
+                // Sets the geom to a body
+                Body = d.BodyCreate(_parent_scene.world);
+
+                setMass();
+                d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
+                d.Quaternion myrot = new d.Quaternion();
+                myrot.X = _orientation.X;
+                myrot.Y = _orientation.Y;
+                myrot.Z = _orientation.Z;
+                myrot.W = _orientation.W;
+                d.BodySetQuaternion(Body, ref myrot);
+                d.GeomSetBody(prim_geom, Body);
+                m_collisionCategories |= CollisionCategories.Body;
+                m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
+
+                d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
+                d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
+
+                d.BodySetAutoDisableFlag(Body, true);
+                d.BodySetAutoDisableSteps(Body, body_autodisable_frames);
+                
+                // disconnect from world gravity so we can apply buoyancy
+                                d.BodySetGravityMode (Body, false);
+
+                m_interpenetrationcount = 0;
+                m_collisionscore = 0;
+                m_disabled = false;
+
+                // 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.addActivePrim(this);
+            }
+        }
+
+        #region Mass Calculation
+
+        private float CalculateMass()
+        {
+            float volume = 0;
+
+            // No material is passed to the physics engines yet..  soo..
+            // we're using the m_density constant in the class definition
+
+            float returnMass = 0;
+
+            switch (_pbs.ProfileShape)
+            {
+                case ProfileShape.Square:
+                    // Profile Volume
+
+                    volume = _size.X*_size.Y*_size.Z;
+
+                    // If the user has 'hollowed out'
+                    // ProfileHollow is one of those 0 to 50000 values :P
+                    // we like percentages better..   so turning into a percentage
+
+                    if (((float) _pbs.ProfileHollow/50000f) > 0.0)
+                    {
+                        float hollowAmount = (float) _pbs.ProfileHollow/50000f;
+
+                        // calculate the hollow volume by it's shape compared to the prim shape
+                        float hollowVolume = 0;
+                        switch (_pbs.HollowShape)
+                        {
+                            case HollowShape.Square:
+                            case HollowShape.Same:
+                                // Cube Hollow volume calculation
+                                float hollowsizex = _size.X*hollowAmount;
+                                float hollowsizey = _size.Y*hollowAmount;
+                                float hollowsizez = _size.Z*hollowAmount;
+                                hollowVolume = hollowsizex*hollowsizey*hollowsizez;
+                                break;
+
+                            case HollowShape.Circle:
+                                // Hollow shape is a perfect cyllinder in respect to the cube's scale
+                                // Cyllinder hollow volume calculation
+                                float hRadius = _size.X/2;
+                                float hLength = _size.Z;
+
+                                // pi * r2 * h
+                                hollowVolume = ((float) (Math.PI*Math.Pow(hRadius, 2)*hLength)*hollowAmount);
+                                break;
+
+                            case HollowShape.Triangle:
+                                // Equilateral Triangular Prism volume hollow calculation
+                                // Triangle is an Equilateral Triangular Prism with aLength = to _size.Y
+
+                                float aLength = _size.Y;
+                                // 1/2 abh
+                                hollowVolume = (float) ((0.5*aLength*_size.X*_size.Z)*hollowAmount);
+                                break;
+
+                            default:
+                                hollowVolume = 0;
+                                break;
+                        }
+                        volume = volume - hollowVolume;
+                    }
+
+                    break;
+                case ProfileShape.Circle:
+                    if (_pbs.PathCurve == (byte)Extrusion.Straight)
+                    {
+                        // Cylinder
+                        float volume1 = (float)(Math.PI * Math.Pow(_size.X/2, 2) * _size.Z);
+                        float volume2 = (float)(Math.PI * Math.Pow(_size.Y/2, 2) * _size.Z);
+
+                        // Approximating the cylinder's irregularity.
+                        if (volume1 > volume2)
+                        {
+                            volume = (float)volume1 - (volume1 - volume2);
+                        }
+                        else if (volume2 > volume1)
+                        {
+                            volume = (float)volume2 - (volume2 - volume1);
+                        }
+                        else
+                        {
+                            // Regular cylinder
+                            volume = volume1;
+                        }
+                    }
+                    else
+                    {
+                        // We don't know what the shape is yet, so use default
+                        volume = _size.X * _size.Y * _size.Z;
+                    }
+                    // If the user has 'hollowed out'
+                    // ProfileHollow is one of those 0 to 50000 values :P
+                    // we like percentages better..   so turning into a percentage
+
+                    if (((float)_pbs.ProfileHollow / 50000f) > 0.0)
+                    {
+                        float hollowAmount = (float)_pbs.ProfileHollow / 50000f;
+
+                        // calculate the hollow volume by it's shape compared to the prim shape
+                        float hollowVolume = 0;
+                        switch (_pbs.HollowShape)
+                        {
+                            case HollowShape.Same:
+                            case HollowShape.Circle:
+                                // Hollow shape is a perfect cyllinder in respect to the cube's scale
+                                // Cyllinder hollow volume calculation
+                                float hRadius = _size.X / 2;
+                                float hLength = _size.Z;
+
+                                // pi * r2 * h
+                                hollowVolume = ((float)(Math.PI * Math.Pow(hRadius, 2) * hLength) * hollowAmount);
+                                break;
+
+                            case HollowShape.Square:
+                                // Cube Hollow volume calculation
+                                float hollowsizex = _size.X * hollowAmount;
+                                float hollowsizey = _size.Y * hollowAmount;
+                                float hollowsizez = _size.Z * hollowAmount;
+                                hollowVolume = hollowsizex * hollowsizey * hollowsizez;
+                                break;
+
+                            case HollowShape.Triangle:
+                                // Equilateral Triangular Prism volume hollow calculation
+                                // Triangle is an Equilateral Triangular Prism with aLength = to _size.Y
+
+                                float aLength = _size.Y;
+                                // 1/2 abh
+                                hollowVolume = (float)((0.5 * aLength * _size.X * _size.Z) * hollowAmount);
+                                break;
+
+                            default:
+                                hollowVolume = 0;
+                                break;
+                        }
+                        volume = volume - hollowVolume;
+                    }
+                    break;
+
+                case ProfileShape.HalfCircle:
+                    if (_pbs.PathCurve == (byte)Extrusion.Curve1)
+                    {
+                        if (_size.X == _size.Y && _size.Y == _size.Z)
+                        {
+                            // regular sphere
+                            // v = 4/3 * pi * r^3
+                            float sradius3 = (float)Math.Pow((_size.X / 2), 3);
+                            volume = (float)((4f / 3f) * Math.PI * sradius3);
+                        }
+                        else
+                        {
+                            // we treat this as a box currently
+                            volume = _size.X * _size.Y * _size.Z;
+                        }
+                    }
+                    else
+                    {
+                        // We don't know what the shape is yet, so use default
+                        volume = _size.X * _size.Y * _size.Z;
+                    }
+                    break;
+
+                case ProfileShape.EquilateralTriangle:
+                    /*
+                        v = (abs((xB*yA-xA*yB)+(xC*yB-xB*yC)+(xA*yC-xC*yA))/2) * h
+
+                        // seed mesh
+                        Vertex MM = new Vertex(-0.25f, -0.45f, 0.0f);
+                        Vertex PM = new Vertex(+0.5f, 0f, 0.0f);
+                        Vertex PP = new Vertex(-0.25f, +0.45f, 0.0f);
+                     */
+                    float xA = -0.25f * _size.X;
+                    float yA = -0.45f * _size.Y;
+
+                    float xB = 0.5f * _size.X;
+                    float yB = 0;
+
+                    float xC = -0.25f * _size.X;
+                    float yC = 0.45f * _size.Y;
+
+                    volume = (float)((Math.Abs((xB * yA - xA * yB) + (xC * yB - xB * yC) + (xA * yC - xC * yA)) / 2) * _size.Z);
+
+                    // If the user has 'hollowed out'
+                    // ProfileHollow is one of those 0 to 50000 values :P
+                    // we like percentages better..   so turning into a percentage
+                    float fhollowFactor = ((float)_pbs.ProfileHollow / 1.9f);
+                    if (((float)fhollowFactor / 50000f) > 0.0)
+                    {
+                        float hollowAmount = (float)fhollowFactor / 50000f;
+
+                        // calculate the hollow volume by it's shape compared to the prim shape
+                        float hollowVolume = 0;
+                        switch (_pbs.HollowShape)
+                        {
+                            case HollowShape.Same:
+                            case HollowShape.Triangle:
+                                // Equilateral Triangular Prism volume hollow calculation
+                                // Triangle is an Equilateral Triangular Prism with aLength = to _size.Y
+
+                                float aLength = _size.Y;
+                                // 1/2 abh
+                                hollowVolume = (float)((0.5 * aLength * _size.X * _size.Z) * hollowAmount);
+                                break;
+
+                            case HollowShape.Square:
+                                // Cube Hollow volume calculation
+                                float hollowsizex = _size.X * hollowAmount;
+                                float hollowsizey = _size.Y * hollowAmount;
+                                float hollowsizez = _size.Z * hollowAmount;
+                                hollowVolume = hollowsizex * hollowsizey * hollowsizez;
+                                break;
+
+                            case HollowShape.Circle:
+                                // Hollow shape is a perfect cyllinder in respect to the cube's scale
+                                // Cyllinder hollow volume calculation
+                                float hRadius = _size.X / 2;
+                                float hLength = _size.Z;
+
+                                // pi * r2 * h
+                                hollowVolume = ((float)((Math.PI * Math.Pow(hRadius, 2) * hLength)/2) * hollowAmount);
+                                break;
+
+                            default:
+                                hollowVolume = 0;
+                                break;
+                        }
+                        volume = volume - hollowVolume;
+                    }
+                    break;
+
+                default:
+                    // we don't have all of the volume formulas yet so
+                    // use the common volume formula for all
+                    volume = _size.X*_size.Y*_size.Z;
+                    break;
+            }
+
+            // Calculate Path cut effect on volume
+            // Not exact, in the triangle hollow example
+            // They should never be zero or less then zero..
+            // we'll ignore it if it's less then zero
+
+            // ProfileEnd and ProfileBegin are values
+            // from 0 to 50000
+
+            // Turning them back into percentages so that I can cut that percentage off the volume
+
+            float PathCutEndAmount = _pbs.ProfileEnd;
+            float PathCutStartAmount = _pbs.ProfileBegin;
+            if (((PathCutStartAmount + PathCutEndAmount)/50000f) > 0.0f)
+            {
+                float pathCutAmount = ((PathCutStartAmount + PathCutEndAmount)/50000f);
+
+                // Check the return amount for sanity
+                if (pathCutAmount >= 0.99f)
+                    pathCutAmount = 0.99f;
+
+                volume = volume - (volume*pathCutAmount);
+            }
+            UInt16 taperX = _pbs.PathScaleX;
+            UInt16 taperY = _pbs.PathScaleY;
+            float taperFactorX = 0;
+            float taperFactorY = 0;
+
+            // Mass = density * volume
+            if (taperX != 100)
+            {
+                if (taperX > 100)
+                {
+                    taperFactorX = 1.0f - ((float)taperX / 200);
+                    //m_log.Warn("taperTopFactorX: " + extr.taperTopFactorX.ToString());
+                }
+                else
+                {
+                    taperFactorX = 1.0f - ((100 - (float)taperX) / 100);
+                    //m_log.Warn("taperBotFactorX: " + extr.taperBotFactorX.ToString());
+                }
+                volume = (float)volume * ((taperFactorX / 3f) + 0.001f);
+            }
+
+            if (taperY != 100)
+            {
+                if (taperY > 100)
+                {
+                    taperFactorY = 1.0f - ((float)taperY / 200);
+                    //m_log.Warn("taperTopFactorY: " + extr.taperTopFactorY.ToString());
+                }
+                else
+                {
+                    taperFactorY = 1.0f - ((100 - (float)taperY) / 100);
+                    //m_log.Warn("taperBotFactorY: " + extr.taperBotFactorY.ToString());
+                }
+                volume = (float)volume * ((taperFactorY / 3f) + 0.001f);
+            }
+            returnMass = m_density*volume;
+            if (returnMass <= 0) returnMass = 0.0001f;//ckrinke: Mass must be greater then zero.
+
+
+
+            // Recursively calculate mass
+            bool HasChildPrim = false;
+            lock (childrenPrim)
+            {
+                if (childrenPrim.Count > 0)
+                {
+                    HasChildPrim = true;
+                }
+
+            }
+            if (HasChildPrim)
+            {
+                OdePrim[] childPrimArr = new OdePrim[0];
+
+                lock (childrenPrim)
+                    childPrimArr = childrenPrim.ToArray();
+
+                for (int i = 0; i < childPrimArr.Length; i++)
+                {
+                    if (childPrimArr[i] != null && !childPrimArr[i].m_taintremove)
+                        returnMass += childPrimArr[i].CalculateMass();
+                    // failsafe, this shouldn't happen but with OpenSim, you never know :)
+                    if (i > 256)
+                        break;
+                }
+            }
+            if (returnMass > _parent_scene.maximumMassObject)
+                returnMass = _parent_scene.maximumMassObject;
+            return returnMass;
+        }// end CalculateMass
+
+        #endregion
+
+        public void setMass()
+        {
+            if (Body != (IntPtr) 0)
+            {
+                float newmass = CalculateMass();
+
+                //m_log.Info("[PHYSICS]: New Mass: " + newmass.ToString());
+
+                d.MassSetBoxTotal(out pMass, newmass, _size.X, _size.Y, _size.Z);
+                d.BodySetMass(Body, ref pMass);
+            }
+        }
+
+        public void disableBody()
+        {
+            //this kills the body so things like 'mesh' can re-create it.
+            lock (this)
+            {
+                if (!childPrim)
+                {
+                    if (Body != IntPtr.Zero)
+                    {
+                        _parent_scene.remActivePrim(this);
+                        m_collisionCategories &= ~CollisionCategories.Body;
+                        m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land);
+
+                        if (prim_geom != IntPtr.Zero)
+                        {
+                            d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
+                            d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
+                        }
+
+                        
+                        d.BodyDestroy(Body);
+                        lock (childrenPrim)
+                        {
+                            if (childrenPrim.Count > 0)
+                            {
+                                foreach (OdePrim prm in childrenPrim)
+                                {
+                                    _parent_scene.remActivePrim(prm);
+                                    prm.Body = IntPtr.Zero;
+                                }
+                            }
+                        }
+                        Body = IntPtr.Zero;
+                    }
+                }
+                else
+                {
+                    _parent_scene.remActivePrim(this);
+                    
+                    m_collisionCategories &= ~CollisionCategories.Body;
+                    m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land);
+
+                    if (prim_geom != IntPtr.Zero)
+                    {
+                        d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
+                        d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
+                    }
+
+                    
+                    Body = IntPtr.Zero;
+                }
+            }
+            m_disabled = true;
+            m_collisionscore = 0;
+        }
+
+        private static Dictionary<IMesh, IntPtr> m_MeshToTriMeshMap = new Dictionary<IMesh, IntPtr>();
+
+        public void setMesh(OdeScene parent_scene, IMesh mesh)
+        {
+            // This sleeper is there to moderate how long it takes between
+            // setting up the mesh and pre-processing it when we get rapid fire mesh requests on a single object
+
+            //Thread.Sleep(10);
+
+            //Kill Body so that mesh can re-make the geom
+            if (IsPhysical && Body != IntPtr.Zero)
+            {
+                if (childPrim)
+                {
+                    if (_parent != null)
+                    {
+                        OdePrim parent = (OdePrim)_parent;
+                        parent.ChildDelink(this);
+                    }
+                }
+                else
+                {
+                    disableBody();
+                }
+            }
+
+            IntPtr vertices, indices;
+            int vertexCount, indexCount;
+            int vertexStride, triStride;
+            mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); // Note, that vertices are fixed in unmanaged heap
+            mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); // Also fixed, needs release after usage
+
+            mesh.releaseSourceMeshData(); // free up the original mesh data to save memory
+            if (m_MeshToTriMeshMap.ContainsKey(mesh))
+            {
+                _triMeshData = m_MeshToTriMeshMap[mesh];
+            }
+            else
+            {
+                _triMeshData = d.GeomTriMeshDataCreate();
+
+                d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride);
+                d.GeomTriMeshDataPreprocess(_triMeshData);
+                m_MeshToTriMeshMap[mesh] = _triMeshData;
+            }
+
+            _parent_scene.waitForSpaceUnlock(m_targetSpace);
+            try
+            {
+                if (prim_geom == IntPtr.Zero)
+                {
+                    SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, parent_scene.triCallback, null, null));
+                }
+            }
+            catch (AccessViolationException)
+            {
+                m_log.Error("[PHYSICS]: MESH LOCKED");
+                return;
+            }
+
+
+           // if (IsPhysical && Body == (IntPtr) 0)
+           // {
+                // Recreate the body
+          //     m_interpenetrationcount = 0;
+           //     m_collisionscore = 0;
+
+           //     enableBody();
+           // }
+        }
+
+        public void ProcessTaints(float timestep)
+        {
+//Console.WriteLine("ProcessTaints for " + m_primName );
+            if (m_taintadd)
+            {
+                changeadd(timestep);
+            }
+            
+            if (prim_geom != IntPtr.Zero)
+            {
+                         if (!_position.ApproxEquals(m_taintposition, 0f))
+                            changemove(timestep);
+
+                     if (m_taintrot != _orientation)
+                     {
+                        if(childPrim && IsPhysical)     // For physical child prim...
+                        {
+                            rotate(timestep);       
+                            // KF: ODE will also rotate the parent prim!
+                            // so rotate the root back to where it was
+                            OdePrim parent = (OdePrim)_parent;
+                            parent.rotate(timestep);
+                    }
+                    else
+                    {
+                            //Just rotate the prim
+                            rotate(timestep);
+                    }  
+                }
+                //
+            
+                if (m_taintPhysics != m_isphysical && !(m_taintparent != _parent))
+                    changePhysicsStatus(timestep);
+                //
+
+                if (!_size.ApproxEquals(m_taintsize,0f))
+                    changesize(timestep);
+                //
+
+                if (m_taintshape)
+                    changeshape(timestep);
+                //
+
+                if (m_taintforce)
+                    changeAddForce(timestep);
+
+                if (m_taintaddangularforce)
+                    changeAddAngularForce(timestep);
+
+                if (!m_taintTorque.ApproxEquals(Vector3.Zero, 0.001f))
+                    changeSetTorque(timestep);
+
+                if (m_taintdisable)
+                    changedisable(timestep);
+
+                if (m_taintselected != m_isSelected)
+                    changeSelectedStatus(timestep);
+
+                if (!m_taintVelocity.ApproxEquals(Vector3.Zero, 0.001f))
+                    changevelocity(timestep);
+
+                if (m_taintparent != _parent)
+                    changelink(timestep);
+
+                if (m_taintCollidesWater != m_collidesWater)
+                    changefloatonwater(timestep);
+
+                if (!m_angularlock.ApproxEquals(m_taintAngularLock,0f))
+                    changeAngularLock(timestep);
+ 
+            }
+            else
+            {
+                m_log.Error("[PHYSICS]: The scene reused a disposed PhysActor! *waves finger*, Don't be evil.  A couple of things can cause this.   An improper prim breakdown(be sure to set prim_geom to zero after d.GeomDestroy!   An improper buildup (creating the geom failed).   Or, the Scene Reused a physics actor after disposing it.)");
+            }
+        }
+
+
+        private void changeAngularLock(float timestep)
+        {
+            // 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;
+            
+        }
+
+        private void changelink(float timestep)
+        {
+            // If the newly set parent is not null
+            // create link
+            if (_parent == null && m_taintparent != null)
+            {
+                if (m_taintparent.PhysicsActorType == (int)ActorTypes.Prim)
+                {
+                    OdePrim obj = (OdePrim)m_taintparent;
+                    //obj.disableBody();
+//Console.WriteLine("changelink calls ParentPrim");
+                    obj.ParentPrim(this);
+
+                    /*
+                    if (obj.Body != (IntPtr)0 && Body != (IntPtr)0 && obj.Body != Body)
+                    {
+                        _linkJointGroup = d.JointGroupCreate(0);
+                        m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup);
+                        d.JointAttach(m_linkJoint, obj.Body, Body);
+                        d.JointSetFixed(m_linkJoint);
+                    }
+                     */
+                }
+            }
+            // If the newly set parent is null
+            // destroy link
+            else if (_parent != null && m_taintparent == null)
+            {
+//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 = m_isphysical;
+        }
+
+        // I'm the parent
+        // prim is the child
+        public void ParentPrim(OdePrim prim)
+        {
+//Console.WriteLine("ParentPrim  " + m_primName);        
+            if (this.m_localID != prim.m_localID)
+            {
+                if (Body == IntPtr.Zero)
+                {
+                    Body = d.BodyCreate(_parent_scene.world);
+                    setMass();
+                }
+                if (Body != IntPtr.Zero)
+                {
+                    lock (childrenPrim)
+                    {
+                        if (!childrenPrim.Contains(prim))
+                        {
+//Console.WriteLine("childrenPrim.Add " + prim);                          
+                            childrenPrim.Add(prim);
+                            
+                            foreach (OdePrim prm in childrenPrim)
+                            {
+                                d.Mass m2;
+                                d.MassSetZero(out m2);
+                                d.MassSetBoxTotal(out m2, prim.CalculateMass(), prm._size.X, prm._size.Y, prm._size.Z);
+
+
+                                d.Quaternion quat = new d.Quaternion();
+                                quat.W = prm._orientation.W;
+                                quat.X = prm._orientation.X;
+                                quat.Y = prm._orientation.Y;
+                                quat.Z = prm._orientation.Z;
+
+                                d.Matrix3 mat = new d.Matrix3();
+                                d.RfromQ(out mat, ref quat);
+                                d.MassRotate(ref m2, ref mat);
+                                d.MassTranslate(ref m2, Position.X - prm.Position.X, Position.Y - prm.Position.Y, Position.Z - prm.Position.Z);
+                                d.MassAdd(ref pMass, ref m2);
+                            }
+                            foreach (OdePrim prm in childrenPrim)
+                            {
+                       
+                                prm.m_collisionCategories |= CollisionCategories.Body;
+                                prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
+
+                                if (prm.prim_geom == IntPtr.Zero)
+                                {
+                                    m_log.Warn("[PHYSICS]: Unable to link one of the linkset elements.  No geom yet");
+                                    continue;
+                                }
+//Console.WriteLine(" GeomSetCategoryBits 1: " + prm.prim_geom + " - " + (int)prm.m_collisionCategories + " for " + m_primName);    
+                                d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories);
+                                d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags);
+
+
+                                d.Quaternion quat = new d.Quaternion();
+                                quat.W = prm._orientation.W;
+                                quat.X = prm._orientation.X;
+                                quat.Y = prm._orientation.Y;
+                                quat.Z = prm._orientation.Z;
+
+                                d.Matrix3 mat = new d.Matrix3();
+                                d.RfromQ(out mat, ref quat);
+                                if (Body != IntPtr.Zero)
+                                {
+                                    d.GeomSetBody(prm.prim_geom, Body);
+                                    prm.childPrim = true;
+                                    d.GeomSetOffsetWorldPosition(prm.prim_geom, prm.Position.X , prm.Position.Y, prm.Position.Z);
+                                    //d.GeomSetOffsetPosition(prim.prim_geom,
+                                    //    (Position.X - prm.Position.X) - pMass.c.X,
+                                    //    (Position.Y - prm.Position.Y) - pMass.c.Y,
+                                    //    (Position.Z - prm.Position.Z) - pMass.c.Z);
+                                    d.GeomSetOffsetWorldRotation(prm.prim_geom, ref mat);
+                                    //d.GeomSetOffsetRotation(prm.prim_geom, ref mat);
+                                    d.MassTranslate(ref pMass, -pMass.c.X, -pMass.c.Y, -pMass.c.Z);
+                                    d.BodySetMass(Body, ref pMass);
+                                }
+                                else
+                                {
+                                    m_log.Debug("[PHYSICS]:I ain't got no boooooooooddy, no body");
+                                }
+
+
+                                prm.m_interpenetrationcount = 0;
+                                prm.m_collisionscore = 0;
+                                prm.m_disabled = false;
+
+                                // 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.addActivePrim(prm);
+                            }
+                            m_collisionCategories |= CollisionCategories.Body;
+                            m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
+
+//Console.WriteLine("GeomSetCategoryBits 2: " + prim_geom + " - " + (int)m_collisionCategories + " for " + m_primName);  
+                            d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
+//Console.WriteLine(" Post GeomSetCategoryBits 2");
+                            d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
+
+
+                            d.Quaternion quat2 = new d.Quaternion();
+                            quat2.W = _orientation.W;
+                            quat2.X = _orientation.X;
+                            quat2.Y = _orientation.Y;
+                            quat2.Z = _orientation.Z;
+
+                            d.Matrix3 mat2 = new d.Matrix3();
+                            d.RfromQ(out mat2, ref quat2);
+                            d.GeomSetBody(prim_geom, Body);
+                            d.GeomSetOffsetWorldPosition(prim_geom, Position.X - pMass.c.X, Position.Y - pMass.c.Y, Position.Z - pMass.c.Z);
+                            //d.GeomSetOffsetPosition(prim.prim_geom,
+                            //    (Position.X - prm.Position.X) - pMass.c.X,
+                            //    (Position.Y - prm.Position.Y) - pMass.c.Y,
+                            //    (Position.Z - prm.Position.Z) - pMass.c.Z);
+                            //d.GeomSetOffsetRotation(prim_geom, ref mat2);
+                            d.MassTranslate(ref pMass, -pMass.c.X, -pMass.c.Y, -pMass.c.Z);
+                            d.BodySetMass(Body, ref pMass);
+
+                            d.BodySetAutoDisableFlag(Body, true);
+                            d.BodySetAutoDisableSteps(Body, body_autodisable_frames);
+
+
+                            m_interpenetrationcount = 0;
+                            m_collisionscore = 0;
+                            m_disabled = false;
+
+                            // 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.addActivePrim(this);
+                        }
+                    }
+                }
+            }
+
+        }
+
+        private void ChildSetGeom(OdePrim odePrim)
+        {
+            //if (m_isphysical && Body != IntPtr.Zero)
+            lock (childrenPrim)
+            {
+                foreach (OdePrim prm in childrenPrim)
+                {
+                    //prm.childPrim = true;
+                    prm.disableBody();
+                    //prm.m_taintparent = null;
+                    //prm._parent = null;
+                    //prm.m_taintPhysics = false;
+                    //prm.m_disabled = true;
+                    //prm.childPrim = false;
+                }
+            }
+            disableBody();
+
+
+            if (Body != IntPtr.Zero)
+            {
+                _parent_scene.remActivePrim(this);
+            }
+
+            lock (childrenPrim)
+            {
+                foreach (OdePrim prm in childrenPrim)
+                {
+//Console.WriteLine("ChildSetGeom calls ParentPrim");               
+                    ParentPrim(prm);
+                }
+            }
+            
+        }
+
+        private void ChildDelink(OdePrim odePrim)
+        {
+            // Okay, we have a delinked child..   need to rebuild the body.
+            lock (childrenPrim)
+            {
+                foreach (OdePrim prm in childrenPrim)
+                {
+                    prm.childPrim = true;
+                    prm.disableBody();
+                    //prm.m_taintparent = null;
+                    //prm._parent = null;
+                    //prm.m_taintPhysics = false;
+                    //prm.m_disabled = true;
+                    //prm.childPrim = false;
+                }
+            }
+            disableBody();
+
+            lock (childrenPrim)
+            {
+ //Console.WriteLine("childrenPrim.Remove " + odePrim);          
+                childrenPrim.Remove(odePrim);
+            }
+            
+            
+            
+
+            if (Body != IntPtr.Zero)
+            {
+                _parent_scene.remActivePrim(this);
+            }
+
+            
+
+            lock (childrenPrim)
+            {
+                foreach (OdePrim prm in childrenPrim)
+                {
+//Console.WriteLine("ChildDelink calls ParentPrim");                
+                    ParentPrim(prm);
+                }
+            }
+
+           
+        }
+
+        private void changeSelectedStatus(float timestep)
+        {
+            if (m_taintselected)
+            {
+                m_collisionCategories = CollisionCategories.Selected;
+                m_collisionFlags = (CollisionCategories.Sensor | CollisionCategories.Space);
+
+                // We do the body disable soft twice because 'in theory' a collision could have happened
+                // in between the disabling and the collision properties setting
+                // which would wake the physical body up from a soft disabling and potentially cause it to fall
+                // through the ground.
+                
+                // NOTE FOR JOINTS: this doesn't always work for jointed assemblies because if you select
+                // just one part of the assembly, the rest of the assembly is non-selected and still simulating,
+                // so that causes the selected part to wake up and continue moving.
+
+                // even if you select all parts of a jointed assembly, it is not guaranteed that the entire
+                // assembly will stop simulating during the selection, because of the lack of atomicity
+                // of select operations (their processing could be interrupted by a thread switch, causing
+                // simulation to continue before all of the selected object notifications trickle down to
+                // the physics engine).
+
+                // e.g. we select 100 prims that are connected by joints. non-atomically, the first 50 are
+                // selected and disabled. then, due to a thread switch, the selection processing is
+                // interrupted and the physics engine continues to simulate, so the last 50 items, whose
+                // selection was not yet processed, continues to simulate. this wakes up ALL of the 
+                // first 50 again. then the last 50 are disabled. then the first 50, which were just woken
+                // up, start simulating again, which in turn wakes up the last 50.
+
+                if (m_isphysical)
+                {
+                    disableBodySoft();
+                }
+
+                if (prim_geom != IntPtr.Zero)
+                {
+                    d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
+                    d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
+                }
+
+                if (m_isphysical)
+                {
+                    disableBodySoft();
+                }
+            }
+            else
+            {
+                m_collisionCategories = CollisionCategories.Geom;
+
+                if (m_isphysical)
+                    m_collisionCategories |= CollisionCategories.Body;
+
+                m_collisionFlags = m_default_collisionFlags;
+
+                if (m_collidesLand)
+                    m_collisionFlags |= CollisionCategories.Land;
+                if (m_collidesWater)
+                    m_collisionFlags |= CollisionCategories.Water;
+
+                if (prim_geom != IntPtr.Zero)
+                {
+                    d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
+                    d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
+                }
+                if (m_isphysical)
+                {
+                    if (Body != IntPtr.Zero)
+                    {
+                        d.BodySetLinearVel(Body, 0f, 0f, 0f);
+                        d.BodySetForce(Body, 0, 0, 0);
+                        enableBodySoft();
+                    }
+                }
+            }
+
+            resetCollisionAccounting();
+            m_isSelected = m_taintselected;
+        }//end changeSelectedStatus
+
+        public void ResetTaints()
+        {
+            m_taintposition = _position;
+            m_taintrot = _orientation;
+            m_taintPhysics = m_isphysical;
+            m_taintselected = m_isSelected;
+            m_taintsize = _size;
+            m_taintshape = false;
+            m_taintforce = false;
+            m_taintdisable = false;
+            m_taintVelocity = Vector3.Zero;
+        }
+
+        public void CreateGeom(IntPtr m_targetSpace, IMesh _mesh)
+        {
+//Console.WriteLine("CreateGeom:");         
+            if (_mesh != null)
+            {
+                setMesh(_parent_scene, _mesh);
+            }
+            else
+            {
+                if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1)
+                {
+                    if (_size.X == _size.Y && _size.Y == _size.Z && _size.X == _size.Z)
+                    {
+                        if (((_size.X / 2f) > 0f))
+                        {
+                            _parent_scene.waitForSpaceUnlock(m_targetSpace);
+                            try
+                            {
+//Console.WriteLine(" CreateGeom 1");
+                                SetGeom(d.CreateSphere(m_targetSpace, _size.X / 2));
+                            }
+                            catch (AccessViolationException)
+                            {
+                                m_log.Warn("[PHYSICS]: Unable to create physics proxy for object");
+                                ode.dunlock(_parent_scene.world);
+                                return;
+                            }
+                        }
+                        else
+                        {
+                            _parent_scene.waitForSpaceUnlock(m_targetSpace);
+                            try
+                            {
+//Console.WriteLine(" CreateGeom 2");                           
+                                SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
+                            }
+                            catch (AccessViolationException)
+                            {
+                                m_log.Warn("[PHYSICS]: Unable to create physics proxy for object");
+                                ode.dunlock(_parent_scene.world);
+                                return;
+                            }
+                        }
+                    }
+                    else
+                    {
+                        _parent_scene.waitForSpaceUnlock(m_targetSpace);
+                        try
+                        {
+//Console.WriteLine("  CreateGeom 3");                       
+                            SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
+                        }
+                        catch (AccessViolationException)
+                        {
+                            m_log.Warn("[PHYSICS]: Unable to create physics proxy for object");
+                            ode.dunlock(_parent_scene.world);
+                            return;
+                        }
+                    }
+                }
+
+                else
+                {
+                    _parent_scene.waitForSpaceUnlock(m_targetSpace);
+                    try
+                    {
+//Console.WriteLine("  CreateGeom 4");                  
+                        SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
+                    }
+                    catch (AccessViolationException)
+                    {
+                        m_log.Warn("[PHYSICS]: Unable to create physics proxy for object");
+                        ode.dunlock(_parent_scene.world);
+                        return;
+                    }
+                }
+            }
+        }
+
+        public void changeadd(float timestep)
+        {
+            int[] iprimspaceArrItem = _parent_scene.calculateSpaceArrayItemFromPos(_position);
+            IntPtr targetspace = _parent_scene.calculateSpaceForGeom(_position);
+
+            if (targetspace == IntPtr.Zero)
+                targetspace = _parent_scene.createprimspace(iprimspaceArrItem[0], iprimspaceArrItem[1]);
+
+            m_targetSpace = targetspace;
+
+            if (_mesh == null)
+            {
+                if (_parent_scene.needsMeshing(_pbs))
+                {
+                    // Don't need to re-enable body..   it's done in SetMesh
+                    _mesh = _parent_scene.mesher.CreateMesh(m_primName, _pbs, _size, _parent_scene.meshSculptLOD, IsPhysical);
+                    // createmesh returns null when it's a shape that isn't a cube.
+                   // m_log.Debug(m_localID);
+                }
+            }
+
+
+            lock (_parent_scene.OdeLock)
+            {
+//Console.WriteLine("changeadd 1");           
+                CreateGeom(m_targetSpace, _mesh);
+
+                if (prim_geom != IntPtr.Zero)
+                {
+                    d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
+                    d.Quaternion myrot = new d.Quaternion();
+                    myrot.X = _orientation.X;
+                    myrot.Y = _orientation.Y;
+                    myrot.Z = _orientation.Z;
+                    myrot.W = _orientation.W;
+                    d.GeomSetQuaternion(prim_geom, ref myrot);
+                }
+
+                if (m_isphysical && Body == IntPtr.Zero)
+                {
+                    enableBody();
+                }
+            }
+
+            _parent_scene.geom_name_map[prim_geom] = this.m_primName;
+            _parent_scene.actor_name_map[prim_geom] = (PhysicsActor)this;
+
+            changeSelectedStatus(timestep);
+
+            m_taintadd = false;
+        }
+
+        public void changemove(float timestep)
+        {
+            if (m_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.Warn("[PHYSICS]: Body Still null after enableBody().  This is a crash scenario.");
+                    }
+                }
+                //else
+               // {
+                    //m_log.Debug("[BUG]: race!");
+                //}
+            }
+            else
+            {
+                // string primScenAvatarIn = _parent_scene.whichspaceamIin(_position);
+                // int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position);
+                _parent_scene.waitForSpaceUnlock(m_targetSpace);
+
+                IntPtr tempspace = _parent_scene.recalculateSpaceForGeom(prim_geom, _position, m_targetSpace);
+                m_targetSpace = tempspace;
+
+                _parent_scene.waitForSpaceUnlock(m_targetSpace);
+                if (prim_geom != IntPtr.Zero)
+                {
+                    d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
+
+                    _parent_scene.waitForSpaceUnlock(m_targetSpace);
+                    d.SpaceAdd(m_targetSpace, prim_geom);
+                }
+            }
+
+            changeSelectedStatus(timestep);
+
+            resetCollisionAccounting();
+            m_taintposition = _position;
+        }
+
+        public void Move(float timestep)
+        {
+            float fx = 0;
+            float fy = 0;
+            float fz = 0;
+
+            frcount++;                                  // used to limit debug comment output
+            if (frcount > 100)
+                frcount = 0;
+                
+            if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim)             // KF: Only move root prims.
+            {
+//if(frcount == 0) Console.WriteLine("Move " +  m_primName + "  VTyp " + m_vehicle.Type +
+                        //                      "    usePID=" + m_usePID  + "    seHover=" + m_useHoverPID  + "  useAPID=" + m_useAPID);                
+                if (m_vehicle.Type != Vehicle.TYPE_NONE)
+                {
+                        // 'VEHICLES' are dealt with in ODEDynamics.cs
+                        m_vehicle.Step(timestep, _parent_scene);
+                }
+                else
+                {
+                        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 is set by llSetBuoyancy() and is for non-vehicle.
+                        // m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up 
+                        // NB Prims in ODE are no subject to global gravity
+                        fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass; // force = acceleration * mass
+
+                        if (m_usePID)
+                        {
+//if(frcount == 0) Console.WriteLine("PID " +  m_primName);             
+                        // KF - this is for object MoveToTarget.
+                        
+                            //if (!d.BodyIsEnabled(Body))
+                            //d.BodySetForce(Body, 0f, 0f, 0f);
+
+                            //  no lock; for now it's only called from within Simulate()
+        
+                            // If the PID Controller isn't active then we set our force
+                            // calculating base velocity to the current position
+
+                            if ((m_PIDTau < 1) && (m_PIDTau != 0))
+                            {
+                                //PID_G = PID_G / m_PIDTau;
+                                m_PIDTau = 1;
+                            }
+        
+                            if ((PID_G - m_PIDTau) <= 0)
+                            {
+                                PID_G = m_PIDTau + 1;
+                            }
+                            //PidStatus = true;
+
+                            // PhysicsVector vec = new PhysicsVector();
+                            d.Vector3 vel = d.BodyGetLinearVel(Body);
+
+                            d.Vector3 pos = d.BodyGetPosition(Body);
+                            _target_velocity =
+                            new Vector3(
+                                    (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep),
+                                    (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep),
+                                    (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep)
+                                    );
+
+                            //  if velocity is zero, use position control; otherwise, velocity control
+
+                            if (_target_velocity.ApproxEquals(Vector3.Zero,0.1f))
+                            {
+                                //  keep track of where we stopped.  No more slippin' & slidin'
+        
+                                // We only want to deactivate the PID Controller if we think we want to have our surrogate
+                                // react to the physics scene by moving it's position.
+                                // Avatar to Avatar collisions
+                                // Prim to avatar collisions
+
+                                //fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2);
+                                //fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2);
+                                //fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P;
+                                d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);
+                                d.BodySetLinearVel(Body, 0, 0, 0);
+                                d.BodyAddForce(Body, 0, 0, fz);
+                                return;
+                            }
+                            else
+                            {
+                                _zeroFlag = false;
+
+                                // We're flying and colliding with something
+                                fx = ((_target_velocity.X) - vel.X) * (PID_D);
+                                fy = ((_target_velocity.Y) - vel.Y) * (PID_D);
+        
+                                // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P;
+
+                                fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
+                            }
+                        }               // end if (m_usePID)
+
+                        // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller
+                        if (m_useHoverPID && !m_usePID)
+                        {
+//Console.WriteLine("Hover " +  m_primName);            
+                        
+                            // If we're using the PID controller, then we have no gravity
+                            fz = (-1 * _parent_scene.gravityz) * m_mass;
+
+                            //  no lock; for now it's only called from within Simulate()
+
+                            // If the PID Controller isn't active then we set our force
+                            // calculating base velocity to the current position
+
+                            if ((m_PIDTau < 1))
+                            {
+                                PID_G = PID_G / m_PIDTau;
+                            }
+
+                            if ((PID_G - m_PIDTau) <= 0)
+                            {
+                                PID_G = m_PIDTau + 1;
+                            }
+                    
+
+                            // Where are we, and where are we headed?
+                            d.Vector3 pos = d.BodyGetPosition(Body);
+                            d.Vector3 vel = d.BodyGetLinearVel(Body);
+
+
+                                                //    Non-Vehicles have a limited set of Hover options.
+                            // determine what our target height really is based on HoverType
+                            switch (m_PIDHoverType)
+                            {
+                                case PIDHoverType.Ground:
+                                    m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
+                                    m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
+                                    break;
+                                case PIDHoverType.GroundAndWater:
+                                    m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
+                                    m_waterHeight  = _parent_scene.GetWaterLevel();
+                                    if (m_groundHeight > m_waterHeight)
+                                    {
+                                        m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
+                                    }
+                                    else
+                                    {
+                                        m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
+                                    }
+                                    break;
+
+                            }   // end switch (m_PIDHoverType)
+
+
+                            _target_velocity =
+                            new Vector3(0.0f, 0.0f,
+                                (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep)
+                                );
+
+                        //  if velocity is zero, use position control; otherwise, velocity control
+
+                        if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f))
+                        {
+                            //  keep track of where we stopped.  No more slippin' & slidin'
+        
+                            // We only want to deactivate the PID Controller if we think we want to have our surrogate
+                            // react to the physics scene by moving it's position.
+                            // Avatar to Avatar collisions
+                            // Prim to avatar collisions
+
+                            d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight);
+                            d.BodySetLinearVel(Body, vel.X, vel.Y, 0);
+                            d.BodyAddForce(Body, 0, 0, fz);
+                    //KF this prevents furthur motions        return;
+                        }
+                        else
+                        {
+                            _zeroFlag = false;
+
+                            // We're flying and colliding with something
+                            fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
+                        }
+                    } // end m_useHoverPID && !m_usePID
+                    
+                        if (m_useAPID)
+                        {
+                                // RotLookAt, apparently overrides all other rotation sources. Inputs:
+                                // Quaternion m_APIDTarget
+                                                // float m_APIDStrength         // From SL experiments, this is the time to get there
+                                                // float m_APIDDamping          // From SL experiments, this is damping, 1.0 = damped, 0.1 = wobbly
+                                                                                                        // Also in SL the mass of the object has no effect on time to get there.
+                                                // Factors:
+//if(frcount == 0) Console.WriteLine("APID ");                                                  
+                                    // get present body rotation
+                                    float limit = 1.0f;
+                                    float scaler = 50f;         // adjusts damping time
+                                    float RLAservo = 0f;
+                                    
+                                    d.Quaternion rot = d.BodyGetQuaternion(Body);
+                                    Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);
+                                    Quaternion rot_diff = Quaternion.Inverse(rotq) * m_APIDTarget;
+                        float diff_angle;
+                        Vector3 diff_axis;
+                        rot_diff.GetAxisAngle(out diff_axis, out diff_angle);
+                        diff_axis.Normalize();
+                                                if(diff_angle > 0.01f)                  // diff_angle is always +ve
+                                                {
+//                              PhysicsVector rotforce = new PhysicsVector(diff_axis.X, diff_axis.Y, diff_axis.Z);
+                                Vector3 rotforce = new Vector3(diff_axis.X, diff_axis.Y, diff_axis.Z);
+                            rotforce = rotforce * rotq;
+                            if(diff_angle > limit) diff_angle = limit;          // cap the rotate rate
+//                          RLAservo = timestep / m_APIDStrength * m_mass * scaler;
+  //                        rotforce = rotforce * RLAservo * diff_angle ;
+    //                      d.BodyAddRelTorque(Body, rotforce.X, rotforce.Y, rotforce.Z);
+                            RLAservo = timestep / m_APIDStrength * scaler;
+                            rotforce = rotforce * RLAservo * diff_angle ;
+                                                        d.BodySetAngularVel (Body,  rotforce.X, rotforce.Y, rotforce.Z);
+//Console.WriteLine("axis= " + diff_axis + "    angle= " + diff_angle + "servo= " + RLAservo);                                                  
+                                                }
+//if(frcount == 0) Console.WriteLine("mass= " + m_mass + "  servo= " + RLAservo + "   angle= " + diff_angle);                                                   
+                        } // end m_useAPID
+                        
+                    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 " +  m_primName);                  
+               
+            }
+        }
+
+
+
+        public void rotate(float timestep)
+        {
+            d.Quaternion myrot = new d.Quaternion();
+            myrot.X = _orientation.X;
+            myrot.Y = _orientation.Y;
+            myrot.Z = _orientation.Z;
+            myrot.W = _orientation.W;
+            if (Body != IntPtr.Zero)
+            {
+                    // KF: If this is a root prim do BodySet
+                d.BodySetQuaternion(Body, ref myrot);
+                    if (m_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;
+        }
+
+        public void changedisable(float timestep)
+        {
+            m_disabled = true;
+            if (Body != IntPtr.Zero)
+            {
+                d.BodyDisable(Body);
+                Body = IntPtr.Zero;
+            }
+
+            m_taintdisable = false;
+        }
+
+        public void changePhysicsStatus(float timestep)
+        {
+            if (m_isphysical == true)
+            {
+                if (Body == IntPtr.Zero)
+                {
+                    if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim)
+                    {
+                        changeshape(2f);
+                    }
+                    else
+                    {
+                        enableBody();
+                    }
+                }
+            }
+            else
+            {
+                if (Body != IntPtr.Zero)
+                {
+                    if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim)
+                    {
+                        
+
+                        if (prim_geom != IntPtr.Zero)
+                        {
+                            try
+                            {
+                                d.GeomDestroy(prim_geom);
+                                prim_geom = IntPtr.Zero;
+                                _mesh = null;
+                            }
+                            catch (System.AccessViolationException)
+                            {
+                                prim_geom = IntPtr.Zero;
+                                m_log.Error("[PHYSICS]: PrimGeom dead");
+                            }
+                        }
+//Console.WriteLine("changePhysicsStatus for " + m_primName );
+                        changeadd(2f);
+                    }
+                    if (childPrim)
+                    {
+                        if (_parent != null)
+                        {
+                            OdePrim parent = (OdePrim)_parent;
+                            parent.ChildDelink(this);
+                        }
+                    }
+                    else
+                    {
+                        disableBody();
+                    }
+                }
+            }
+
+            changeSelectedStatus(timestep);
+
+            resetCollisionAccounting();
+            m_taintPhysics = m_isphysical;
+        }
+
+        public void changesize(float timestamp)
+        {
+            
+            string oldname = _parent_scene.geom_name_map[prim_geom];
+
+            if (_size.X <= 0) _size.X = 0.01f;
+            if (_size.Y <= 0) _size.Y = 0.01f;
+            if (_size.Z <= 0) _size.Z = 0.01f;
+
+            // Cleanup of old prim geometry
+            if (_mesh != null)
+            {
+                // Cleanup meshing here
+            }
+            //kill body to rebuild
+            if (IsPhysical && Body != IntPtr.Zero)
+            {
+                if (childPrim)
+                {
+                    if (_parent != null)
+                    {
+                        OdePrim parent = (OdePrim)_parent;
+                        parent.ChildDelink(this);
+                    }
+                }
+                else
+                {
+                    disableBody();
+                }
+            }
+            if (d.SpaceQuery(m_targetSpace, prim_geom))
+            {
+                _parent_scene.waitForSpaceUnlock(m_targetSpace);
+                d.SpaceRemove(m_targetSpace, prim_geom);
+            }
+            d.GeomDestroy(prim_geom);
+            prim_geom = IntPtr.Zero;
+            // we don't need to do space calculation because the client sends a position update also.
+
+            // Construction of new prim
+            if (_parent_scene.needsMeshing(_pbs))
+            {
+                float meshlod = _parent_scene.meshSculptLOD;
+
+                if (IsPhysical)
+                    meshlod = _parent_scene.MeshSculptphysicalLOD;
+                // Don't need to re-enable body..   it's done in SetMesh
+
+                IMesh mesh = null;
+
+                if (_parent_scene.needsMeshing(_pbs))
+                    mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical);
+
+                //IMesh mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical);
+//Console.WriteLine("changesize 1");
+                CreateGeom(m_targetSpace, mesh);
+
+               
+            }
+            else
+            {
+                _mesh = null;
+//Console.WriteLine("changesize 2");    
+                CreateGeom(m_targetSpace, _mesh);
+            }
+
+            d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
+            d.Quaternion myrot = new d.Quaternion();
+            myrot.X = _orientation.X;
+            myrot.Y = _orientation.Y;
+            myrot.Z = _orientation.Z;
+            myrot.W = _orientation.W;
+            d.GeomSetQuaternion(prim_geom, ref myrot);
+
+            //d.GeomBoxSetLengths(prim_geom, _size.X, _size.Y, _size.Z);
+            if (IsPhysical && Body == IntPtr.Zero && !childPrim)
+            {
+                // Re creates body on size.
+                // EnableBody also does setMass()
+                enableBody();
+                d.BodyEnable(Body);
+            }
+
+            _parent_scene.geom_name_map[prim_geom] = oldname;
+
+            changeSelectedStatus(timestamp);
+            if (childPrim)
+            {
+                if (_parent is OdePrim)
+                {
+                    OdePrim parent = (OdePrim)_parent;
+                    parent.ChildSetGeom(this);
+                }
+            }
+            resetCollisionAccounting();
+            m_taintsize = _size;
+        }
+
+       
+
+        public void changefloatonwater(float timestep)
+        {
+            m_collidesWater = m_taintCollidesWater;
+
+            if (prim_geom != IntPtr.Zero)
+            {
+                if (m_collidesWater)
+                {
+                    m_collisionFlags |= CollisionCategories.Water;
+                }
+                else
+                {
+                    m_collisionFlags &= ~CollisionCategories.Water;
+                }
+                d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
+            }
+        }
+
+        public void changeshape(float timestamp)
+        {
+            string oldname = _parent_scene.geom_name_map[prim_geom];
+
+            // Cleanup of old prim geometry and Bodies
+            if (IsPhysical && Body != IntPtr.Zero)
+            {
+                if (childPrim)
+                {
+                    if (_parent != null)
+                    {
+                        OdePrim parent = (OdePrim)_parent;
+                        parent.ChildDelink(this);
+                    }
+                }
+                else
+                {
+                    disableBody();
+                }
+            }
+            try
+            {
+                d.GeomDestroy(prim_geom);
+            }
+            catch (System.AccessViolationException)
+            {
+                prim_geom = IntPtr.Zero;
+                m_log.Error("[PHYSICS]: PrimGeom dead");
+            }
+            prim_geom = IntPtr.Zero;
+            // we don't need to do space calculation because the client sends a position update also.
+            if (_size.X <= 0) _size.X = 0.01f;
+            if (_size.Y <= 0) _size.Y = 0.01f;
+            if (_size.Z <= 0) _size.Z = 0.01f;
+            // Construction of new prim
+
+            if (_parent_scene.needsMeshing(_pbs))
+            {
+                // Don't need to re-enable body..   it's done in SetMesh
+                float meshlod = _parent_scene.meshSculptLOD;
+
+                if (IsPhysical)
+                    meshlod = _parent_scene.MeshSculptphysicalLOD;
+
+                IMesh mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical);
+                // createmesh returns null when it doesn't mesh.
+                CreateGeom(m_targetSpace, mesh);
+            }
+            else
+            {
+                _mesh = null;
+//Console.WriteLine("changeshape");              
+                CreateGeom(m_targetSpace, null);
+            }
+
+            d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
+            d.Quaternion myrot = new d.Quaternion();
+            //myrot.W = _orientation.w;
+            myrot.W = _orientation.W;
+            myrot.X = _orientation.X;
+            myrot.Y = _orientation.Y;
+            myrot.Z = _orientation.Z;
+            d.GeomSetQuaternion(prim_geom, ref myrot);
+
+            //d.GeomBoxSetLengths(prim_geom, _size.X, _size.Y, _size.Z);
+            if (IsPhysical && Body == IntPtr.Zero)
+            {
+                // Re creates body on size.
+                // EnableBody also does setMass()
+                enableBody();
+                if (Body != IntPtr.Zero)
+                {
+                    d.BodyEnable(Body);
+                }
+            }
+            _parent_scene.geom_name_map[prim_geom] = oldname;
+
+            changeSelectedStatus(timestamp);
+            if (childPrim)
+            {
+                if (_parent is OdePrim)
+                {
+                    OdePrim parent = (OdePrim)_parent;
+                    parent.ChildSetGeom(this);
+                }
+            }
+            resetCollisionAccounting();
+            m_taintshape = false;
+        }
+
+        public void changeAddForce(float timestamp)
+        {
+            if (!m_isSelected)
+            {
+                lock (m_forcelist)
+                {
+                    //m_log.Info("[PHYSICS]: dequeing forcelist");
+                    if (IsPhysical)
+                    {
+                        Vector3 iforce = Vector3.Zero;
+                        int i = 0;
+                        try
+                        {
+                            for (i = 0; i < m_forcelist.Count; i++)
+                            {
+
+                                iforce = iforce + (m_forcelist[i] * 100);
+                            }
+                        }
+                        catch (IndexOutOfRangeException)
+                        {
+                            m_forcelist = new List<Vector3>();
+                            m_collisionscore = 0;
+                            m_interpenetrationcount = 0;
+                            m_taintforce = false;
+                            return;
+                        }
+                        catch (ArgumentOutOfRangeException)
+                        {
+                            m_forcelist = new List<Vector3>();
+                            m_collisionscore = 0;
+                            m_interpenetrationcount = 0;
+                            m_taintforce = false;
+                            return;
+                        }
+                        d.BodyEnable(Body);
+                        d.BodyAddForce(Body, iforce.X, iforce.Y, iforce.Z);
+                    }
+                    m_forcelist.Clear();
+                }
+
+                m_collisionscore = 0;
+                m_interpenetrationcount = 0;
+            }
+
+            m_taintforce = false;
+
+        }
+
+
+
+        public void changeSetTorque(float timestamp)
+        {
+            if (!m_isSelected)
+            {
+                if (IsPhysical && Body != IntPtr.Zero)
+                {
+                    d.BodySetTorque(Body, m_taintTorque.X, m_taintTorque.Y, m_taintTorque.Z);
+                }
+            }
+
+            m_taintTorque = Vector3.Zero;
+        }
+
+        public void changeAddAngularForce(float timestamp)
+        {
+            if (!m_isSelected)
+            {
+                lock (m_angularforcelist)
+                {
+                    //m_log.Info("[PHYSICS]: dequeing forcelist");
+                    if (IsPhysical)
+                    {
+                        Vector3 iforce = Vector3.Zero;
+                        for (int i = 0; i < m_angularforcelist.Count; i++)
+                        {
+                            iforce = iforce + (m_angularforcelist[i] * 100);
+                        }
+                        d.BodyEnable(Body);
+                        d.BodyAddTorque(Body, iforce.X, iforce.Y, iforce.Z);
+                        
+                    }
+                    m_angularforcelist.Clear();
+                }
+
+                m_collisionscore = 0;
+                m_interpenetrationcount = 0;
+            }
+
+            m_taintaddangularforce = false;
+        }
+
+        private void changevelocity(float timestep)
+        {
+            if (!m_isSelected)
+            {
+                Thread.Sleep(20);
+                if (IsPhysical)
+                {
+                    if (Body != IntPtr.Zero)
+                    {
+                        d.BodySetLinearVel(Body, m_taintVelocity.X, m_taintVelocity.Y, m_taintVelocity.Z);
+                    }
+                }
+                
+                //resetCollisionAccounting();
+            }
+            m_taintVelocity = Vector3.Zero;
+        }
+
+        public override bool IsPhysical
+        {
+            get { return m_isphysical; }
+            set { 
+                  m_isphysical = value;
+                  if (!m_isphysical) // Zero the remembered last velocity
+                      m_lastVelocity = Vector3.Zero;
+                }
+        }
+
+        public void setPrimForRemoval()
+        {
+            m_taintremove = true;
+        }
+
+        public override bool Flying
+        {
+            // no flying prims for you
+            get { return false; }
+            set { }
+        }
+
+        public override bool IsColliding
+        {
+            get { return iscolliding; }
+            set { iscolliding = value; }
+        }
+
+        public override bool CollidingGround
+        {
+            get { return false; }
+            set { return; }
+        }
+
+        public override bool CollidingObj
+        {
+            get { return false; }
+            set { return; }
+        }
+
+        public override bool ThrottleUpdates
+        {
+            get { return m_throttleUpdates; }
+            set { m_throttleUpdates = value; }
+        }
+
+        public override bool Stopped
+        {
+            get { return _zeroFlag; }
+        }
+
+        public override Vector3 Position
+        {
+            get { return _position; }
+
+            set { _position = value;
+                //m_log.Info("[PHYSICS]: " + _position.ToString());
+            }
+        }
+
+        public override Vector3 Size
+        {
+            get { return _size; }
+            set
+            {
+                if (value.IsFinite())
+                {
+                    _size = value;
+                }
+                else
+                {
+                    m_log.Warn("[PHYSICS]: Got NaN Size on object");
+                }
+            }
+        }
+
+        public override float Mass
+        {
+            get { return CalculateMass(); }
+        }
+
+        public override Vector3 Force
+        {
+            //get { return Vector3.Zero; }
+            get { return m_force; }
+            set
+            {
+                if (value.IsFinite())
+                {
+                    m_force = value;
+                }
+                else
+                {
+                    m_log.Warn("[PHYSICS]: NaN in Force Applied to an Object");
+                }
+            }
+        }
+
+        public override int VehicleType
+        {
+            get { return (int)m_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 SetVolumeDetect(int param)
+        {
+            lock (_parent_scene.OdeLock)
+            {
+                m_isVolumeDetect = (param!=0);
+            }
+        }
+
+        public override Vector3 CenterOfMass
+        {
+            get { return Vector3.Zero; }
+        }
+
+        public override Vector3 GeometricCenter
+        {
+            get { return Vector3.Zero; }
+        }
+
+        public override PrimitiveBaseShape Shape
+        {
+            set
+            {
+                _pbs = value;
+                m_taintshape = true;
+            }
+        }
+
+        public override Vector3 Velocity
+        {
+            get
+            {
+                // Averate previous velocity with the new one so
+                // client object interpolation works a 'little' better
+                if (_zeroFlag)
+                    return Vector3.Zero;
+
+                Vector3 returnVelocity = Vector3.Zero;
+                returnVelocity.X = (m_lastVelocity.X + _velocity.X)/2;
+                returnVelocity.Y = (m_lastVelocity.Y + _velocity.Y)/2;
+                returnVelocity.Z = (m_lastVelocity.Z + _velocity.Z)/2;
+                return returnVelocity;
+            }
+            set
+            {
+                if (value.IsFinite())
+                {
+                    _velocity = value;
+
+                    m_taintVelocity = value;
+                    _parent_scene.AddPhysicsActorTaint(this);
+                }
+                else
+                {
+                    m_log.Warn("[PHYSICS]: Got NaN Velocity in Object");
+                }
+
+            }
+        }
+
+        public override Vector3 Torque
+        {
+            get
+            {
+                if (!m_isphysical || Body == IntPtr.Zero)
+                    return Vector3.Zero;
+
+                return _torque;
+            }
+
+            set
+            {
+                if (value.IsFinite())
+                {
+                    m_taintTorque = value;
+                    _parent_scene.AddPhysicsActorTaint(this);
+                }
+                else
+                {
+                    m_log.Warn("[PHYSICS]: Got NaN Torque in Object");
+                }
+            }
+        }
+
+        public override float CollisionScore
+        {
+            get { return m_collisionscore; }
+            set { m_collisionscore = value; }
+        }
+
+        public override bool Kinematic
+        {
+            get { return false; }
+            set { }
+        }
+
+        public override Quaternion Orientation
+        {
+            get { return _orientation; }
+            set
+            {
+                if (QuaternionIsFinite(value))
+                {
+                    _orientation = value;
+                                }
+                else
+                    m_log.Warn("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object");
+
+            }
+        }
+
+        internal static bool QuaternionIsFinite(Quaternion q)
+        {
+            if (Single.IsNaN(q.X) || Single.IsInfinity(q.X))
+                return false;
+            if (Single.IsNaN(q.Y) || Single.IsInfinity(q.Y))
+                return false;
+            if (Single.IsNaN(q.Z) || Single.IsInfinity(q.Z))
+                return false;
+            if (Single.IsNaN(q.W) || Single.IsInfinity(q.W))
+                return false;
+            return true;
+        }
+
+        public override Vector3 Acceleration
+        {
+            get { return _acceleration; }
+        }
+
+
+        public void SetAcceleration(Vector3 accel)
+        {
+            _acceleration = accel;
+        }
+
+        public override void AddForce(Vector3 force, bool pushforce)
+        {
+            if (force.IsFinite())
+            {
+                lock (m_forcelist)
+                    m_forcelist.Add(force);
+
+                m_taintforce = true;
+            }
+            else
+            {
+                m_log.Warn("[PHYSICS]: Got Invalid linear force vector from Scene in Object");
+            }
+            //m_log.Info("[PHYSICS]: Added Force:" + force.ToString() +  " to prim at " + Position.ToString());
+        }
+
+        public override void AddAngularForce(Vector3 force, bool pushforce)
+        {
+            if (force.IsFinite())
+            {
+                m_angularforcelist.Add(force);
+                m_taintaddangularforce = true;
+            }
+            else
+            {
+                m_log.Warn("[PHYSICS]: Got Invalid Angular force vector from Scene in Object");
+            }
+        }
+
+        public override Vector3 RotationalVelocity
+        {
+            get
+            {
+                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;
+                }
+                else
+                {
+                    m_log.Warn("[PHYSICS]: Got NaN RotationalVelocity in Object");
+                }
+            }
+        }
+
+        public override void CrossingFailure()
+        {
+            m_crossingfailures++;
+            if (m_crossingfailures > _parent_scene.geomCrossingFailuresBeforeOutofbounds)
+            {
+                base.RaiseOutOfBounds(_position);
+                return;
+            }
+            else if (m_crossingfailures == _parent_scene.geomCrossingFailuresBeforeOutofbounds)
+            {
+                m_log.Warn("[PHYSICS]: Too many crossing failures for: " + m_primName);
+            }
+        }
+
+        public override float Buoyancy
+        {
+            get { return m_buoyancy; }
+            set { m_buoyancy = value; }
+        }
+
+        public override void link(PhysicsActor obj)
+        {
+            m_taintparent = obj;
+        }
+
+        public override void delink()
+        {
+            m_taintparent = null;
+        }
+
+        public override void LockAngularMotion(Vector3 axis)
+        {
+            // reverse the zero/non zero values for ODE.
+            if (axis.IsFinite())
+            {
+                axis.X = (axis.X > 0) ? 1f : 0f;
+                axis.Y = (axis.Y > 0) ? 1f : 0f;
+                axis.Z = (axis.Z > 0) ? 1f : 0f;
+                m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z);
+                m_taintAngularLock = axis;
+            }
+            else
+            {
+                m_log.Warn("[PHYSICS]: Got NaN locking axis from Scene on Object");
+            }
+        }
+
+        public void UpdatePositionAndVelocity()
+        {
+            //  no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit!
+            if (_parent == null)
+            {
+                Vector3 pv = Vector3.Zero;
+                bool lastZeroFlag = _zeroFlag;
+                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;
+
+                    //  kluge to keep things in bounds.  ODE lets dead avatars drift away (they should be removed!)
+                    //if (vec.X < 0.0f) { vec.X = 0.0f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); }
+                    //if (vec.Y < 0.0f) { vec.Y = 0.0f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); }
+                    //if (vec.X > 255.95f) { vec.X = 255.95f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); }
+                    //if (vec.Y > 255.95f) { vec.Y = 255.95f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); }
+
+                    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.Y != m_lastposition.Y){
+//              Console.WriteLine("UP&V {0}  {1}", m_primName, l_position);
+//      }
+
+                    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;
+                        }
+                    }
+
+                    if (l_position.Z < 0)
+                    {
+                        // This is so prim that get lost underground don't fall forever and suck up
+                        //
+                        // Sim resources and memory.
+                        // Disables the prim's movement physics....
+                        // It's a hack and will generate a console message if it fails.
+
+                        //IsPhysical = false;
+                        if (_parent == null)
+                            base.RaiseOutOfBounds(_position);
+
+                        _acceleration.X = 0;
+                        _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.RequestPhysicsterseUpdate();
+
+                        m_throttleUpdates = false;
+                        throttleCounter = 0;
+                        _zeroFlag = true;
+                        //outofBounds = true;
+                    }
+
+                                        //float Adiff = 1.0f - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation));
+//Console.WriteLine("Adiff " + m_primName + " = " + 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());
+
+                        if (_velocity.ApproxEquals(pv, 0.5f))
+                        {
+                            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.Warn("[PHYSICS]: Got NaN PIDTarget from Scene on Object");
+            } 
+        }
+        public override bool PIDActive { set { m_usePID = value; } }
+        public override float PIDTau { set { m_PIDTau = value; } }
+        
+                // For RotLookAt        
+        public  override Quaternion APIDTarget { set { m_APIDTarget = value; } }
+        public  override bool APIDActive { set { m_useAPID = value; } }
+        public  override float APIDStrength { set { m_APIDStrength = value; } }
+        public  override float APIDDamping { set { m_APIDDamping = value; } }
+
+        public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } }
+        public override bool PIDHoverActive { set { m_useHoverPID = value; } }
+        public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } }
+        public override float PIDHoverTau { set { m_PIDHoverTau = value; } }
+
+        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);//
+            
+        }
+
+        public Matrix4 FromDMass(d.Mass pMass)
+        {
+            Matrix4 obj;
+            obj.M11 = pMass.I.M00;
+            obj.M12 = pMass.I.M01;
+            obj.M13 = pMass.I.M02;
+            obj.M14 = 0;
+            obj.M21 = pMass.I.M10;
+            obj.M22 = pMass.I.M11;
+            obj.M23 = pMass.I.M12;
+            obj.M24 = 0;
+            obj.M31 = pMass.I.M20;
+            obj.M32 = pMass.I.M21;
+            obj.M33 = pMass.I.M22;
+            obj.M34 = 0;
+            obj.M41 = 0;
+            obj.M42 = 0;
+            obj.M43 = 0;
+            obj.M44 = 1;
+            return obj;
+        }
+
+        public d.Mass FromMatrix4(Matrix4 pMat, ref d.Mass obj)
+        {
+            obj.I.M00 = pMat[0, 0];
+            obj.I.M01 = pMat[0, 1];
+            obj.I.M02 = pMat[0, 2];
+            obj.I.M10 = pMat[1, 0];
+            obj.I.M11 = pMat[1, 1];
+            obj.I.M12 = pMat[1, 2];
+            obj.I.M20 = pMat[2, 0];
+            obj.I.M21 = pMat[2, 1];
+            obj.I.M22 = pMat[2, 2];
+            return obj;
+        }
+
+        public override void SubscribeEvents(int ms)
+        {
+            m_eventsubscription = ms;
+            _parent_scene.addCollisionEventReporting(this);
+        }
+
+        public override void UnSubscribeEvents()
+        {
+            _parent_scene.remCollisionEventReporting(this);
+            m_eventsubscription = 0;
+        }
+
+        public void AddCollisionEvent(uint CollidedWith, ContactPoint contact)
+        {
+            if (CollisionEventsThisFrame == null)
+                CollisionEventsThisFrame = new CollisionEventUpdate();
+            CollisionEventsThisFrame.addCollider(CollidedWith, contact);
+        }
+
+        public void SendCollisions()
+        {
+            if (CollisionEventsThisFrame == null)
+                return;
+
+            base.SendCollisionUpdate(CollisionEventsThisFrame);
+
+            if (CollisionEventsThisFrame.m_objCollisionList.Count == 0)
+                CollisionEventsThisFrame = null;
+            else
+                CollisionEventsThisFrame = new CollisionEventUpdate();
+        }
+
+        public override bool SubscribedEvents()
+        {
+            if (m_eventsubscription > 0)
+                return true;
+            return false;
+        }
+
+        public static Matrix4 Inverse(Matrix4 pMat)
+        {
+            if (determinant3x3(pMat) == 0)
+            {
+                return Matrix4.Identity; // should probably throw an error.  singluar matrix inverse not possible
+            }
+
+
+
+            return (Adjoint(pMat) / determinant3x3(pMat));
+        }
+
+        public static Matrix4 Adjoint(Matrix4 pMat)
+        {
+            Matrix4 adjointMatrix = new Matrix4();
+            for (int i=0; i<4; i++)
+            {
+                for (int j=0; j<4; j++)
+                {
+                    Matrix4SetValue(ref adjointMatrix, i, j, (float)(Math.Pow(-1, i + j) * (determinant3x3(Minor(pMat, i, j)))));
+                }
+            }
+
+            adjointMatrix = Transpose(adjointMatrix);
+            return adjointMatrix;
+        }
+
+        public static Matrix4 Minor(Matrix4 matrix, int iRow, int iCol)
+        {
+            Matrix4 minor = new Matrix4();
+            int m = 0, n = 0;
+            for (int i = 0; i < 4; i++)
+            {
+                if (i == iRow)
+                    continue;
+                n = 0;
+                for (int j = 0; j < 4; j++)
+                {
+                    if (j == iCol)
+                        continue;
+                    Matrix4SetValue(ref minor, m,n, matrix[i, j]);
+                    n++;
+                }
+                m++;
+            }
+            return minor;
+        }
+
+        public static Matrix4 Transpose(Matrix4 pMat)
+        {
+            Matrix4 transposeMatrix = new Matrix4();
+            for (int i = 0; i < 4; i++)
+                for (int j = 0; j < 4; j++)
+                    Matrix4SetValue(ref transposeMatrix, i, j, pMat[j, i]);
+            return transposeMatrix;
+        }
+
+        public static void Matrix4SetValue(ref Matrix4 pMat, int r, int c, float val)
+        {
+            switch (r)
+            {
+                case 0:
+                    switch (c)
+                    {
+                        case 0:
+                            pMat.M11 = val;
+                            break;
+                        case 1:
+                            pMat.M12 = val;
+                            break;
+                        case 2:
+                            pMat.M13 = val;
+                            break;
+                        case 3:
+                            pMat.M14 = val;
+                            break;
+                    }
+
+                    break;
+                case 1:
+                    switch (c)
+                    {
+                        case 0:
+                            pMat.M21 = val;
+                            break;
+                        case 1:
+                            pMat.M22 = val;
+                            break;
+                        case 2:
+                            pMat.M23 = val;
+                            break;
+                        case 3:
+                            pMat.M24 = val;
+                            break;
+                    }
+
+                    break;
+                case 2:
+                    switch (c)
+                    {
+                        case 0:
+                            pMat.M31 = val;
+                            break;
+                        case 1:
+                            pMat.M32 = val;
+                            break;
+                        case 2:
+                            pMat.M33 = val;
+                            break;
+                        case 3:
+                            pMat.M34 = val;
+                            break;
+                    }
+
+                    break;
+                case 3:
+                    switch (c)
+                    {
+                        case 0:
+                            pMat.M41 = val;
+                            break;
+                        case 1:
+                            pMat.M42 = val;
+                            break;
+                        case 2:
+                            pMat.M43 = val;
+                            break;
+                        case 3:
+                            pMat.M44 = val;
+                            break;
+                    }
+
+                    break;
+            }
+        }
+        private static float determinant3x3(Matrix4 pMat)
+        {
+            float det = 0;
+            float diag1 = pMat[0, 0]*pMat[1, 1]*pMat[2, 2];
+            float diag2 = pMat[0, 1]*pMat[2, 1]*pMat[2, 0];
+            float diag3 = pMat[0, 2]*pMat[1, 0]*pMat[2, 1];
+            float diag4 = pMat[2, 0]*pMat[1, 1]*pMat[0, 2];
+            float diag5 = pMat[2, 1]*pMat[1, 2]*pMat[0, 0];
+            float diag6 = pMat[2, 2]*pMat[1, 0]*pMat[0, 1];
+
+            det = diag1 + diag2 + diag3 - (diag4 + diag5 + diag6);
+            return det;
+
+        }
+        
+        private static void DMassCopy(ref d.Mass src, ref d.Mass dst)
+        {
+            dst.c.W = src.c.W;
+            dst.c.X = src.c.X;
+            dst.c.Y = src.c.Y;
+            dst.c.Z = src.c.Z;
+            dst.mass = src.mass;
+            dst.I.M00 = src.I.M00;
+            dst.I.M01 = src.I.M01;
+            dst.I.M02 = src.I.M02;
+            dst.I.M10 = src.I.M10;
+            dst.I.M11 = src.I.M11;
+            dst.I.M12 = src.I.M12;
+            dst.I.M20 = src.I.M20;
+            dst.I.M21 = src.I.M21;
+            dst.I.M22 = src.I.M22;
+        }
+
+        public override void SetMaterial(int pMaterial)
+        {
+            m_material = pMaterial;
+        }
+
+    }
+}
diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODERayCastRequestManager.cs b/OpenSim/Region/Physics/ChOdePlugin/ODERayCastRequestManager.cs
new file mode 100644
index 0000000..7314107
--- /dev/null
+++ b/OpenSim/Region/Physics/ChOdePlugin/ODERayCastRequestManager.cs
@@ -0,0 +1,375 @@
+/*
+ * Copyright (c) Contributors, http://opensimulator.org/
+ * See CONTRIBUTORS.TXT for a full list of copyright holders.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the OpenSimulator Project nor the
+ *       names of its contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+using System;
+using System.Collections.Generic;
+using System.Reflection;
+using System.Runtime.InteropServices;
+using System.Text;
+using OpenMetaverse;
+using OpenSim.Region.Physics.Manager;
+using Ode.NET;
+using log4net;
+
+namespace OpenSim.Region.Physics.OdePlugin
+{
+    /// <summary>
+    /// Processes raycast requests as ODE is in a state to be able to do them.
+    /// This ensures that it's thread safe and there will be no conflicts.
+    /// Requests get returned by a different thread then they were requested by.
+    /// </summary>
+    public class ODERayCastRequestManager
+    {
+        /// <summary>
+        /// Pending Raycast Requests
+        /// </summary>
+        protected List<ODERayCastRequest> m_PendingRequests = new List<ODERayCastRequest>();
+
+        /// <summary>
+        /// Scene that created this object.
+        /// </summary>
+        private OdeScene m_scene;
+
+        /// <summary>
+        /// ODE contact array to be filled by the collision testing
+        /// </summary>
+        d.ContactGeom[] contacts = new d.ContactGeom[5];
+
+        /// <summary>
+        /// ODE near callback delegate
+        /// </summary>
+        private d.NearCallback nearCallback;
+        private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
+        private List<ContactResult> m_contactResults = new List<ContactResult>();
+
+
+        public ODERayCastRequestManager(OdeScene pScene)
+        {
+            m_scene = pScene;
+            nearCallback = near;
+            
+        }
+
+        /// <summary>
+        /// Queues a raycast
+        /// </summary>
+        /// <param name="position">Origin of Ray</param>
+        /// <param name="direction">Ray normal</param>
+        /// <param name="length">Ray length</param>
+        /// <param name="retMethod">Return method to send the results</param>
+        public void QueueRequest(Vector3 position, Vector3 direction, float length, RaycastCallback retMethod)
+        {
+            lock (m_PendingRequests)
+            {
+                ODERayCastRequest req = new ODERayCastRequest();
+                req.callbackMethod = retMethod;
+                req.length = length;
+                req.Normal = direction;
+                req.Origin = position;
+
+                m_PendingRequests.Add(req);
+            }
+        }
+
+        /// <summary>
+        /// Process all queued raycast requests
+        /// </summary>
+        /// <returns>Time in MS the raycasts took to process.</returns>
+        public int ProcessQueuedRequests()
+        {
+            int time = System.Environment.TickCount;
+            lock (m_PendingRequests)
+            {
+                if (m_PendingRequests.Count > 0)
+                {
+                    ODERayCastRequest[] reqs = m_PendingRequests.ToArray();
+                    for (int i = 0; i < reqs.Length; i++)
+                    {
+                        if (reqs[i].callbackMethod != null) // quick optimization here, don't raycast 
+                            RayCast(reqs[i]);               // if there isn't anyone to send results   
+                    }
+                    /*
+                    foreach (ODERayCastRequest req in m_PendingRequests)
+                    {
+                        if (req.callbackMethod != null) // quick optimization here, don't raycast 
+                            RayCast(req);               // if there isn't anyone to send results to
+                            
+                    }
+                    */
+                    m_PendingRequests.Clear();
+                }
+            }
+
+            lock (m_contactResults)
+                m_contactResults.Clear();
+
+            return System.Environment.TickCount - time;
+        }
+
+        /// <summary>
+        /// Method that actually initiates the raycast
+        /// </summary>
+        /// <param name="req"></param>
+        private void RayCast(ODERayCastRequest req)
+        {
+            // Create the ray
+            IntPtr ray = d.CreateRay(m_scene.space, req.length);
+            d.GeomRaySet(ray, req.Origin.X, req.Origin.Y, req.Origin.Z, req.Normal.X, req.Normal.Y, req.Normal.Z);
+
+            // Collide test
+            d.SpaceCollide2(m_scene.space, ray, IntPtr.Zero, nearCallback);
+
+            // Remove Ray
+            d.GeomDestroy(ray);
+
+
+            // Define default results
+            bool hitYN = false;
+            uint hitConsumerID = 0;
+            float distance = 999999999999f;
+            Vector3 closestcontact = new Vector3(99999f, 99999f, 99999f);
+            Vector3 snormal = Vector3.Zero;
+
+            // Find closest contact and object.
+            lock (m_contactResults)
+            {
+                foreach (ContactResult cResult in m_contactResults)
+                {
+                    if (Vector3.Distance(req.Origin, cResult.Pos) < Vector3.Distance(req.Origin, closestcontact))
+                    {
+                        closestcontact = cResult.Pos;
+                        hitConsumerID = cResult.ConsumerID;
+                        distance = cResult.Depth;
+                        hitYN = true;
+                        snormal = cResult.Normal;
+                    }
+                }
+
+                m_contactResults.Clear();
+            }
+
+            // Return results
+            if (req.callbackMethod != null)
+                req.callbackMethod(hitYN, closestcontact, hitConsumerID, distance, snormal);
+        }
+
+        // This is the standard Near.   Uses space AABBs to speed up detection.
+        private void near(IntPtr space, IntPtr g1, IntPtr g2)
+        {
+
+            //Don't test against heightfield Geom, or you'll be sorry!
+            
+            /*
+             terminate called after throwing an instance of 'std::bad_alloc'
+                  what():  std::bad_alloc
+                Stacktrace:
+                 
+                  at (wrapper managed-to-native) Ode.NET.d.Collide (intptr,intptr,int,Ode.NET.d/ContactGeom[],int) <0x00004>
+                  at (wrapper managed-to-native) Ode.NET.d.Collide (intptr,intptr,int,Ode.NET.d/ContactGeom[],int) <0xffffffff>
+                  at OpenSim.Region.Physics.OdePlugin.ODERayCastRequestManager.near (intptr,intptr,intptr) <0x00280>
+                  at (wrapper native-to-managed) OpenSim.Region.Physics.OdePlugin.ODERayCastRequestManager.near (intptr,intptr,intptr) <0xfff
+                fffff>
+                  at (wrapper managed-to-native) Ode.NET.d.SpaceCollide2 (intptr,intptr,intptr,Ode.NET.d/NearCallback) <0x00004>
+                  at (wrapper managed-to-native) Ode.NET.d.SpaceCollide2 (intptr,intptr,intptr,Ode.NET.d/NearCallback) <0xffffffff>
+                  at OpenSim.Region.Physics.OdePlugin.ODERayCastRequestManager.RayCast (OpenSim.Region.Physics.OdePlugin.ODERayCastRequest) <
+                0x00114>
+                  at OpenSim.Region.Physics.OdePlugin.ODERayCastRequestManager.ProcessQueuedRequests () <0x000eb>
+                  at OpenSim.Region.Physics.OdePlugin.OdeScene.Simulate (single) <0x017e6>
+                  at OpenSim.Region.Framework.Scenes.SceneGraph.UpdatePhysics (double) <0x00042>
+                  at OpenSim.Region.Framework.Scenes.Scene.Update () <0x0039e>
+                  at OpenSim.Region.Framework.Scenes.Scene.Heartbeat (object) <0x00019>
+                  at (wrapper runtime-invoke) object.runtime_invoke_void__this___object (object,intptr,intptr,intptr) <0xffffffff>
+                 
+                Native stacktrace:
+                 
+                        mono [0x80d2a42]
+                        [0xb7f5840c]
+                        /lib/i686/cmov/libc.so.6(abort+0x188) [0xb7d1a018]
+                        /usr/lib/libstdc++.so.6(_ZN9__gnu_cxx27__verbose_terminate_handlerEv+0x158) [0xb45fc988]
+                        /usr/lib/libstdc++.so.6 [0xb45fa865]
+                        /usr/lib/libstdc++.so.6 [0xb45fa8a2]
+                        /usr/lib/libstdc++.so.6 [0xb45fa9da]
+                        /usr/lib/libstdc++.so.6(_Znwj+0x83) [0xb45fb033]
+                        /usr/lib/libstdc++.so.6(_Znaj+0x1d) [0xb45fb11d]
+                        libode.so(_ZN13dxHeightfield23dCollideHeightfieldZoneEiiiiP6dxGeomiiP12dContactGeomi+0xd04) [0xb46678e4]
+                        libode.so(_Z19dCollideHeightfieldP6dxGeomS0_iP12dContactGeomi+0x54b) [0xb466832b]
+                        libode.so(dCollide+0x102) [0xb46571b2]
+                        [0x95cfdec9]
+                        [0x8ea07fe1]
+                        [0xab260146]
+                        libode.so [0xb465a5c4]
+                        libode.so(_ZN11dxHashSpace8collide2EPvP6dxGeomPFvS0_S2_S2_E+0x75) [0xb465bcf5]
+                        libode.so(dSpaceCollide2+0x177) [0xb465ac67]
+                        [0x95cf978e]
+                        [0x8ea07945]
+                        [0x95cf2bbc]
+                        [0xab2787e7]
+                        [0xab419fb3]
+                        [0xab416657]
+                        [0xab415bda]
+                        [0xb609b08e]
+                        mono(mono_runtime_delegate_invoke+0x34) [0x8192534]
+                        mono [0x81a2f0f]
+                        mono [0x81d28b6]
+                        mono [0x81ea2c6]
+                        /lib/i686/cmov/libpthread.so.0 [0xb7e744c0]
+                        /lib/i686/cmov/libc.so.6(clone+0x5e) [0xb7dcd6de]
+             */
+
+            // Exclude heightfield geom
+
+            if (g1 == IntPtr.Zero || g2 == IntPtr.Zero)
+                return;
+            if (d.GeomGetClass(g1) == d.GeomClassID.HeightfieldClass || d.GeomGetClass(g2) == d.GeomClassID.HeightfieldClass)
+                return;
+
+            // Raytest against AABBs of spaces first, then dig into the spaces it hits for actual geoms.
+            if (d.GeomIsSpace(g1) || d.GeomIsSpace(g2))
+            {
+                if (g1 == IntPtr.Zero || g2 == IntPtr.Zero)
+                    return;
+                
+                // Separating static prim geometry spaces.
+                // We'll be calling near recursivly if one
+                // of them is a space to find all of the
+                // contact points in the space
+                try
+                {
+                    d.SpaceCollide2(g1, g2, IntPtr.Zero, nearCallback);
+                }
+                catch (AccessViolationException)
+                {
+                    m_log.Warn("[PHYSICS]: Unable to collide test a space");
+                    return;
+                }
+                //Colliding a space or a geom with a space or a geom. so drill down
+
+                //Collide all geoms in each space..
+                //if (d.GeomIsSpace(g1)) d.SpaceCollide(g1, IntPtr.Zero, nearCallback);
+                //if (d.GeomIsSpace(g2)) d.SpaceCollide(g2, IntPtr.Zero, nearCallback);
+                return;
+            }
+
+            if (g1 == IntPtr.Zero || g2 == IntPtr.Zero)
+                return;
+
+            int count = 0;
+            try
+            {
+                
+                if (g1 == g2)
+                    return; // Can't collide with yourself
+
+                lock (contacts)
+                {
+                    count = d.Collide(g1, g2, contacts.GetLength(0), contacts, d.ContactGeom.SizeOf);
+                }
+            }
+            catch (SEHException)
+            {
+                m_log.Error("[PHYSICS]: The Operating system shut down ODE because of corrupt memory.  This could be a result of really irregular terrain.  If this repeats continuously, restart using Basic Physics and terrain fill your terrain.  Restarting the sim.");
+            }
+            catch (Exception e)
+            {
+                m_log.WarnFormat("[PHYSICS]: Unable to collide test an object: {0}", e.Message);
+                return;
+            }
+
+            PhysicsActor p1 = null;
+            PhysicsActor p2 = null;
+
+            if (g1 != IntPtr.Zero)
+                m_scene.actor_name_map.TryGetValue(g1, out p1);
+
+            if (g2 != IntPtr.Zero)
+                m_scene.actor_name_map.TryGetValue(g1, out p2);
+
+            // Loop over contacts, build results.
+            for (int i = 0; i < count; i++)
+            {
+                if (p1 != null) { 
+                    if (p1 is OdePrim)
+                    {
+                        ContactResult collisionresult = new ContactResult();
+                    
+                        collisionresult.ConsumerID = ((OdePrim)p1).m_localID;
+                        collisionresult.Pos = new Vector3(contacts[i].pos.X, contacts[i].pos.Y, contacts[i].pos.Z);
+                        collisionresult.Depth = contacts[i].depth;
+                        collisionresult.Normal = new Vector3(contacts[i].normal.X, contacts[i].normal.Y,
+                                                             contacts[i].normal.Z);
+                        lock (m_contactResults)
+                            m_contactResults.Add(collisionresult);
+                    }
+                }
+
+                if (p2 != null)
+                {
+                    if (p2 is OdePrim)
+                    {
+                        ContactResult collisionresult = new ContactResult();
+
+                        collisionresult.ConsumerID = ((OdePrim)p2).m_localID;
+                        collisionresult.Pos = new Vector3(contacts[i].pos.X, contacts[i].pos.Y, contacts[i].pos.Z);
+                        collisionresult.Depth = contacts[i].depth;
+                        collisionresult.Normal = new Vector3(contacts[i].normal.X, contacts[i].normal.Y,
+                                      contacts[i].normal.Z);
+
+                        lock (m_contactResults)
+                            m_contactResults.Add(collisionresult);
+                    }
+                }
+
+                
+            }
+
+        }
+
+        /// <summary>
+        /// Dereference the creator scene so that it can be garbage collected if needed.
+        /// </summary>
+        internal void Dispose()
+        {
+            m_scene = null;
+        }
+    }
+
+    public struct ODERayCastRequest
+    {
+        public Vector3 Origin;
+        public Vector3 Normal;
+        public float length;
+        public RaycastCallback callbackMethod;
+    }
+
+    public struct ContactResult
+    {
+        public Vector3 Pos;
+        public float Depth;
+        public uint ConsumerID;
+        public Vector3 Normal;
+    }
+}
diff --git a/OpenSim/Region/Physics/ChOdePlugin/OdePhysicsJoint.cs b/OpenSim/Region/Physics/ChOdePlugin/OdePhysicsJoint.cs
new file mode 100644
index 0000000..b4a3c48
--- /dev/null
+++ b/OpenSim/Region/Physics/ChOdePlugin/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.Physics.Manager;
+using OpenSim.Region.Physics.OdePlugin;
+
+namespace OpenSim.Region.Physics.OdePlugin
+{
+    class OdePhysicsJoint : PhysicsJoint
+    {
+        public override bool IsInPhysicsEngine
+        {
+            get
+            {
+                return (jointID != IntPtr.Zero);
+            }
+        }
+        public IntPtr jointID;
+    }
+}
diff --git a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs
new file mode 100644
index 0000000..f48649e
--- /dev/null
+++ b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs
@@ -0,0 +1,3865 @@
+/*
+ * Copyright (c) Contributors, http://opensimulator.org/
+ * See CONTRIBUTORS.TXT for a full list of copyright holders.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the OpenSimulator Project nor the
+ *       names of its contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+//#define USE_DRAWSTUFF
+
+using System;
+using System.Collections.Generic;
+using System.Reflection;
+using System.Runtime.InteropServices;
+using System.Threading;
+using System.IO;
+using System.Diagnostics;
+using log4net;
+using Nini.Config;
+using Ode.NET;
+#if USE_DRAWSTUFF
+using Drawstuff.NET;
+#endif 
+using OpenSim.Framework;
+using OpenSim.Region.Physics.Manager;
+using OpenMetaverse;
+
+//using OpenSim.Region.Physics.OdePlugin.Meshing;
+
+namespace OpenSim.Region.Physics.OdePlugin
+{
+    /// <summary>
+    /// ODE plugin
+    /// </summary>
+    public class OdePlugin : IPhysicsPlugin
+    {
+        //private static readonly log4net.ILog m_log = log4net.LogManager.GetLogger(System.Reflection.MethodBase.GetCurrentMethod().DeclaringType);
+
+        private CollisionLocker ode;
+        private OdeScene _mScene;
+
+        public OdePlugin()
+        {
+            ode = new CollisionLocker();
+        }
+
+        public bool Init()
+        {
+            return true;
+        }
+
+        public PhysicsScene GetScene(String sceneIdentifier)
+        {
+            if (_mScene == null)
+            {
+                // Initializing ODE only when a scene is created allows alternative ODE plugins to co-habit (according to
+                // http://opensimulator.org/mantis/view.php?id=2750).
+                d.InitODE();
+                
+                _mScene = new OdeScene(ode, sceneIdentifier);
+            }
+            return (_mScene);
+        }
+
+        public string GetName()
+        {
+            return ("ChODE");
+        }
+
+        public void Dispose()
+        {
+        }
+    }
+
+    public enum StatusIndicators : int
+    {
+        Generic = 0,
+        Start = 1,
+        End = 2
+    }
+
+    public struct sCollisionData
+    {
+        public uint ColliderLocalId;
+        public uint CollidedWithLocalId;
+        public int NumberOfCollisions;
+        public int CollisionType;
+        public int StatusIndicator;
+        public int lastframe;
+    }
+
+    [Flags]
+    public enum CollisionCategories : int
+    {
+        Disabled = 0,
+        Geom = 0x00000001,
+        Body = 0x00000002,
+        Space = 0x00000004,
+        Character = 0x00000008,
+        Land = 0x00000010,
+        Water = 0x00000020,
+        Wind = 0x00000040,
+        Sensor = 0x00000080,
+        Selected = 0x00000100
+    }
+
+    /// <summary>
+    /// Material type for a primitive
+    /// </summary>
+    public enum Material : int
+    {
+        /// <summary></summary>
+        Stone = 0,
+        /// <summary></summary>
+        Metal = 1,
+        /// <summary></summary>
+        Glass = 2,
+        /// <summary></summary>
+        Wood = 3,
+        /// <summary></summary>
+        Flesh = 4,
+        /// <summary></summary>
+        Plastic = 5,
+        /// <summary></summary>
+        Rubber = 6
+
+    }
+
+    public sealed class OdeScene : PhysicsScene
+    {
+        private readonly ILog m_log;
+        // private Dictionary<string, sCollisionData> m_storedCollisions = new Dictionary<string, sCollisionData>();
+
+        CollisionLocker ode;
+
+        private Random fluidRandomizer = new Random(Environment.TickCount);
+
+        private const uint m_regionWidth = Constants.RegionSize;
+        private const uint m_regionHeight = Constants.RegionSize;
+
+        private float ODE_STEPSIZE = 0.020f;
+        private float metersInSpace = 29.9f;
+        private float m_timeDilation = 1.0f;
+
+        public float gravityx = 0f;
+        public float gravityy = 0f;
+        public float gravityz = -9.8f;
+
+        private float contactsurfacelayer = 0.001f;
+
+        private int worldHashspaceLow = -4;
+        private int worldHashspaceHigh = 128;
+
+        private int smallHashspaceLow = -4;
+        private int smallHashspaceHigh = 66;
+
+        private float waterlevel = 0f;
+        private int framecount = 0;
+        //private int m_returncollisions = 10;
+
+        private readonly IntPtr contactgroup;
+
+        internal IntPtr LandGeom;
+        internal IntPtr WaterGeom;
+
+        private float nmTerrainContactFriction = 255.0f;
+        private float nmTerrainContactBounce = 0.1f;
+        private float nmTerrainContactERP = 0.1025f;
+
+        private float mTerrainContactFriction = 75f;
+        private float mTerrainContactBounce = 0.1f;
+        private float mTerrainContactERP = 0.05025f;
+
+        private float nmAvatarObjectContactFriction = 250f;
+        private float nmAvatarObjectContactBounce = 0.1f;
+
+        private float mAvatarObjectContactFriction = 75f;
+        private float mAvatarObjectContactBounce = 0.1f;
+
+        private float avPIDD = 3200f;
+        private float avPIDP = 1400f;
+        private float avCapRadius = 0.37f;
+        private float avStandupTensor = 2000000f;
+        private bool avCapsuleTilted = true; // true = old compatibility mode with leaning capsule; false = new corrected mode
+        public bool IsAvCapsuleTilted { get { return avCapsuleTilted; } set { avCapsuleTilted = value; } }
+        private float avDensity = 80f;
+        private float avHeightFudgeFactor = 0.52f;
+        private float avMovementDivisorWalk = 1.3f;
+        private float avMovementDivisorRun = 0.8f;
+        private float minimumGroundFlightOffset = 3f;
+        public float maximumMassObject = 10000.01f;
+
+        public bool meshSculptedPrim = true;
+        public bool forceSimplePrimMeshing = false;
+
+        public float meshSculptLOD = 32;
+        public float MeshSculptphysicalLOD = 16;
+
+        public float geomDefaultDensity = 10.000006836f;
+
+        public int geomContactPointsStartthrottle = 3;
+        public int geomUpdatesPerThrottledUpdate = 15;
+
+        public float bodyPIDD = 35f;
+        public float bodyPIDG = 25;
+
+        public int geomCrossingFailuresBeforeOutofbounds = 5;
+
+        public float bodyMotorJointMaxforceTensor = 2;
+
+        public int bodyFramesAutoDisable = 20;
+
+        
+
+        private float[] _watermap;
+        private bool m_filterCollisions = true;
+
+        private d.NearCallback nearCallback;
+        public d.TriCallback triCallback;
+        public d.TriArrayCallback triArrayCallback;
+        private readonly HashSet<OdeCharacter> _characters = new HashSet<OdeCharacter>();
+        private readonly HashSet<OdePrim> _prims = new HashSet<OdePrim>();
+        private readonly HashSet<OdePrim> _activeprims = new HashSet<OdePrim>();
+        private readonly HashSet<OdePrim> _taintedPrimH = new HashSet<OdePrim>();
+        private readonly Object _taintedPrimLock = new Object();
+        private readonly List<OdePrim> _taintedPrimL = new List<OdePrim>();
+        private readonly HashSet<OdeCharacter> _taintedActors = new HashSet<OdeCharacter>();
+        private readonly List<d.ContactGeom> _perloopContact = new List<d.ContactGeom>();
+        private readonly List<PhysicsActor> _collisionEventPrim = new List<PhysicsActor>();
+        private readonly HashSet<OdeCharacter> _badCharacter = new HashSet<OdeCharacter>();
+        public Dictionary<IntPtr, String> geom_name_map = new Dictionary<IntPtr, String>();
+        public Dictionary<IntPtr, PhysicsActor> actor_name_map = new Dictionary<IntPtr, PhysicsActor>();
+        private bool m_NINJA_physics_joints_enabled = false;
+        //private Dictionary<String, IntPtr> jointpart_name_map = new Dictionary<String,IntPtr>();
+        private readonly Dictionary<String, List<PhysicsJoint>> joints_connecting_actor = new Dictionary<String, List<PhysicsJoint>>();
+        private d.ContactGeom[] contacts;
+        private readonly List<PhysicsJoint> requestedJointsToBeCreated = new List<PhysicsJoint>(); // lock only briefly. accessed by external code (to request new joints) and by OdeScene.Simulate() to move those joints into pending/active
+        private readonly List<PhysicsJoint> pendingJoints = new List<PhysicsJoint>(); // can lock for longer. accessed only by OdeScene.
+        private readonly List<PhysicsJoint> activeJoints = new List<PhysicsJoint>(); // can lock for longer. accessed only by OdeScene.
+        private readonly List<string> requestedJointsToBeDeleted = new List<string>(); // lock only briefly. accessed by external code (to request deletion of joints) and by OdeScene.Simulate() to move those joints out of pending/active
+        private Object externalJointRequestsLock = new Object();
+        private readonly Dictionary<String, PhysicsJoint> SOPName_to_activeJoint = new Dictionary<String, PhysicsJoint>();
+        private readonly Dictionary<String, PhysicsJoint> SOPName_to_pendingJoint = new Dictionary<String, PhysicsJoint>();
+        private readonly DoubleDictionary<Vector3, IntPtr, IntPtr> RegionTerrain = new DoubleDictionary<Vector3, IntPtr, IntPtr>();
+        private readonly Dictionary<IntPtr,float[]> TerrainHeightFieldHeights = new Dictionary<IntPtr, float[]>();
+
+        private d.Contact contact;
+        private d.Contact TerrainContact;
+        private d.Contact AvatarMovementprimContact;
+        private d.Contact AvatarMovementTerrainContact;
+        private d.Contact WaterContact;
+        private d.Contact[,] m_materialContacts;
+
+//Ckrinke: Comment out until used. We declare it, initialize it, but do not use it
+//Ckrinke        private int m_randomizeWater = 200;
+        private int m_physicsiterations = 10;
+        private const float m_SkipFramesAtms = 0.40f; // Drop frames gracefully at a 400 ms lag
+        private readonly PhysicsActor PANull = new NullPhysicsActor();
+        private float step_time = 0.0f;
+//Ckrinke: Comment out until used. We declare it, initialize it, but do not use it
+//Ckrinke        private int ms = 0;
+        public IntPtr world;
+        //private bool returncollisions = false;
+        // private uint obj1LocalID = 0;
+        private uint obj2LocalID = 0;
+        //private int ctype = 0;
+        private OdeCharacter cc1;
+        private OdePrim cp1;
+        private OdeCharacter cc2;
+        private OdePrim cp2;
+        //private int cStartStop = 0;
+        //private string cDictKey = "";
+
+        public IntPtr space;
+
+        //private IntPtr tmpSpace;
+        // split static geometry collision handling into spaces of 30 meters
+        public IntPtr[,] staticPrimspace;
+
+        public Object OdeLock;
+
+        public IMesher mesher;
+
+        private IConfigSource m_config;
+
+        public bool physics_logging = false;
+        public int physics_logging_interval = 0;
+        public bool physics_logging_append_existing_logfile = false;
+
+        public d.Vector3 xyz = new d.Vector3(128.1640f, 128.3079f, 25.7600f);
+        public d.Vector3 hpr = new d.Vector3(125.5000f, -17.0000f, 0.0000f);
+
+        // TODO: unused: private uint heightmapWidth = m_regionWidth + 1;
+        // TODO: unused: private uint heightmapHeight = m_regionHeight + 1;
+        // TODO: unused: private uint heightmapWidthSamples;
+        // TODO: unused: private uint heightmapHeightSamples;
+
+        private volatile int m_global_contactcount = 0;
+
+        private Vector3 m_worldOffset = Vector3.Zero;
+        public Vector2 WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize);
+        private PhysicsScene m_parentScene = null;
+
+        private ODERayCastRequestManager m_rayCastManager;
+
+        /// <summary>
+        /// Initiailizes the scene
+        /// Sets many properties that ODE requires to be stable
+        /// These settings need to be tweaked 'exactly' right or weird stuff happens.
+        /// </summary>
+        public OdeScene(CollisionLocker dode, string sceneIdentifier)
+        {
+            m_log 
+                = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType.ToString() + "." + sceneIdentifier);
+
+            OdeLock = new Object();
+            ode = dode;
+            nearCallback = near;
+            triCallback = TriCallback;
+            triArrayCallback = TriArrayCallback;
+            m_rayCastManager = new ODERayCastRequestManager(this);
+            lock (OdeLock)
+            {
+                // Create the world and the first space
+                world = d.WorldCreate();
+                space = d.HashSpaceCreate(IntPtr.Zero);
+                
+
+                contactgroup = d.JointGroupCreate(0);
+                //contactgroup
+
+                d.WorldSetAutoDisableFlag(world, false);
+                #if USE_DRAWSTUFF
+                
+                Thread viewthread = new Thread(new ParameterizedThreadStart(startvisualization));
+                viewthread.Start();
+                #endif
+            }
+
+
+            _watermap = new float[258 * 258];
+
+            // Zero out the prim spaces array (we split our space into smaller spaces so
+            // we can hit test less.
+        }
+
+#if USE_DRAWSTUFF
+        public void startvisualization(object o)
+        {
+            ds.Functions fn;
+            fn.version = ds.VERSION;
+            fn.start = new ds.CallbackFunction(start);
+            fn.step = new ds.CallbackFunction(step);
+            fn.command = new ds.CallbackFunction(command);
+            fn.stop = null;
+            fn.path_to_textures = "./textures";
+            string[] args = new string[0];
+            ds.SimulationLoop(args.Length, args, 352, 288, ref fn);
+        }
+#endif
+
+        // Initialize the mesh plugin
+        public override void Initialise(IMesher meshmerizer, IConfigSource config)
+        {
+            mesher = meshmerizer;
+            m_config = config;
+            // Defaults
+
+            if (Environment.OSVersion.Platform == PlatformID.Unix)
+            {
+                avPIDD = 3200.0f;
+                avPIDP = 1400.0f;
+                avStandupTensor = 2000000f;
+            }
+            else
+            {
+                avPIDD = 2200.0f;
+                avPIDP = 900.0f;
+                avStandupTensor = 550000f;
+            }
+
+            int contactsPerCollision = 80;
+
+            if (m_config != null)
+            {
+                IConfig physicsconfig = m_config.Configs["ODEPhysicsSettings"];
+                if (physicsconfig != null)
+                {
+                    gravityx = physicsconfig.GetFloat("world_gravityx", 0f);
+                    gravityy = physicsconfig.GetFloat("world_gravityy", 0f);
+                    gravityz = physicsconfig.GetFloat("world_gravityz", -9.8f);
+
+                    worldHashspaceLow = physicsconfig.GetInt("world_hashspace_size_low", -4);
+                    worldHashspaceHigh = physicsconfig.GetInt("world_hashspace_size_high", 128);
+
+                    metersInSpace = physicsconfig.GetFloat("meters_in_small_space", 29.9f);
+                    smallHashspaceLow = physicsconfig.GetInt("small_hashspace_size_low", -4);
+                    smallHashspaceHigh = physicsconfig.GetInt("small_hashspace_size_high", 66);
+
+                    contactsurfacelayer = physicsconfig.GetFloat("world_contact_surface_layer", 0.001f);
+
+                    nmTerrainContactFriction = physicsconfig.GetFloat("nm_terraincontact_friction", 255.0f);
+                    nmTerrainContactBounce = physicsconfig.GetFloat("nm_terraincontact_bounce", 0.1f);
+                    nmTerrainContactERP = physicsconfig.GetFloat("nm_terraincontact_erp", 0.1025f);
+
+                    mTerrainContactFriction = physicsconfig.GetFloat("m_terraincontact_friction", 75f);
+                    mTerrainContactBounce = physicsconfig.GetFloat("m_terraincontact_bounce", 0.05f);
+                    mTerrainContactERP = physicsconfig.GetFloat("m_terraincontact_erp", 0.05025f);
+
+                    nmAvatarObjectContactFriction = physicsconfig.GetFloat("objectcontact_friction", 250f);
+                    nmAvatarObjectContactBounce = physicsconfig.GetFloat("objectcontact_bounce", 0.2f);
+
+                    mAvatarObjectContactFriction = physicsconfig.GetFloat("m_avatarobjectcontact_friction", 75f);
+                    mAvatarObjectContactBounce = physicsconfig.GetFloat("m_avatarobjectcontact_bounce", 0.1f);
+
+                    ODE_STEPSIZE = physicsconfig.GetFloat("world_stepsize", 0.020f);
+                    m_physicsiterations = physicsconfig.GetInt("world_internal_steps_without_collisions", 10);
+
+                    avDensity = physicsconfig.GetFloat("av_density", 80f);
+                    avHeightFudgeFactor = physicsconfig.GetFloat("av_height_fudge_factor", 0.52f);
+                    avMovementDivisorWalk = physicsconfig.GetFloat("av_movement_divisor_walk", 1.3f);
+                    avMovementDivisorRun = physicsconfig.GetFloat("av_movement_divisor_run", 0.8f);
+                    avCapRadius = physicsconfig.GetFloat("av_capsule_radius", 0.37f);
+                    avCapsuleTilted = physicsconfig.GetBoolean("av_capsule_tilted", false);
+
+                    contactsPerCollision = physicsconfig.GetInt("contacts_per_collision", 80);
+
+                    geomContactPointsStartthrottle = physicsconfig.GetInt("geom_contactpoints_start_throttling", 3);
+                    geomUpdatesPerThrottledUpdate = physicsconfig.GetInt("geom_updates_before_throttled_update", 15);
+                    geomCrossingFailuresBeforeOutofbounds = physicsconfig.GetInt("geom_crossing_failures_before_outofbounds", 5);
+
+                    geomDefaultDensity = physicsconfig.GetFloat("geometry_default_density", 10.000006836f);
+                    bodyFramesAutoDisable = physicsconfig.GetInt("body_frames_auto_disable", 20);
+
+                    bodyPIDD = physicsconfig.GetFloat("body_pid_derivative", 35f);
+                    bodyPIDG = physicsconfig.GetFloat("body_pid_gain", 25f);
+
+                    forceSimplePrimMeshing = physicsconfig.GetBoolean("force_simple_prim_meshing", forceSimplePrimMeshing);
+                    meshSculptedPrim = physicsconfig.GetBoolean("mesh_sculpted_prim", true);
+                    meshSculptLOD = physicsconfig.GetFloat("mesh_lod", 32f);
+                    MeshSculptphysicalLOD = physicsconfig.GetFloat("mesh_physical_lod", 16f);
+                    m_filterCollisions = physicsconfig.GetBoolean("filter_collisions", false);
+
+                    if (Environment.OSVersion.Platform == PlatformID.Unix)
+                    {
+                        avPIDD = physicsconfig.GetFloat("av_pid_derivative_linux", 2200.0f);
+                        avPIDP = physicsconfig.GetFloat("av_pid_proportional_linux", 900.0f);
+                        avStandupTensor = physicsconfig.GetFloat("av_capsule_standup_tensor_linux", 550000f);
+                        bodyMotorJointMaxforceTensor = physicsconfig.GetFloat("body_motor_joint_maxforce_tensor_linux", 5f);
+                    }
+                    else
+                    {
+                        avPIDD = physicsconfig.GetFloat("av_pid_derivative_win", 2200.0f);
+                        avPIDP = physicsconfig.GetFloat("av_pid_proportional_win", 900.0f);
+                        avStandupTensor = physicsconfig.GetFloat("av_capsule_standup_tensor_win", 550000f);
+                        bodyMotorJointMaxforceTensor = physicsconfig.GetFloat("body_motor_joint_maxforce_tensor_win", 5f);
+                    }
+
+                    physics_logging = physicsconfig.GetBoolean("physics_logging", false);
+                    physics_logging_interval = physicsconfig.GetInt("physics_logging_interval", 0);
+                    physics_logging_append_existing_logfile = physicsconfig.GetBoolean("physics_logging_append_existing_logfile", false);
+
+                    m_NINJA_physics_joints_enabled = physicsconfig.GetBoolean("use_NINJA_physics_joints", false);
+                    minimumGroundFlightOffset = physicsconfig.GetFloat("minimum_ground_flight_offset", 3f);
+                    maximumMassObject = physicsconfig.GetFloat("maximum_mass_object", 10000.01f);
+                }
+            }
+
+            contacts = new d.ContactGeom[contactsPerCollision];
+
+            staticPrimspace = new IntPtr[(int)(300 / metersInSpace), (int)(300 / metersInSpace)];
+
+            // Centeral contact friction and bounce
+            // ckrinke 11/10/08 Enabling soft_erp but not soft_cfm until I figure out why
+            // an avatar falls through in Z but not in X or Y when walking on a prim.
+            contact.surface.mode |= d.ContactFlags.SoftERP;
+            contact.surface.mu = nmAvatarObjectContactFriction;
+            contact.surface.bounce = nmAvatarObjectContactBounce;
+            contact.surface.soft_cfm = 0.010f;
+            contact.surface.soft_erp = 0.010f;
+
+            // Terrain contact friction and Bounce
+            // This is the *non* moving version.   Use this when an avatar
+            // isn't moving to keep it in place better
+            TerrainContact.surface.mode |= d.ContactFlags.SoftERP;
+            TerrainContact.surface.mu = nmTerrainContactFriction;
+            TerrainContact.surface.bounce = nmTerrainContactBounce;
+            TerrainContact.surface.soft_erp = nmTerrainContactERP;
+
+            WaterContact.surface.mode |= (d.ContactFlags.SoftERP | d.ContactFlags.SoftCFM);
+            WaterContact.surface.mu = 0f; // No friction
+            WaterContact.surface.bounce = 0.0f; // No bounce
+            WaterContact.surface.soft_cfm = 0.010f;
+            WaterContact.surface.soft_erp = 0.010f;
+
+            // Prim contact friction and bounce
+            // THis is the *non* moving version of friction and bounce
+            // Use this when an avatar comes in contact with a prim
+            // and is moving
+            AvatarMovementprimContact.surface.mu = mAvatarObjectContactFriction;
+            AvatarMovementprimContact.surface.bounce = mAvatarObjectContactBounce;
+
+            // Terrain contact friction bounce and various error correcting calculations
+            // Use this when an avatar is in contact with the terrain and moving.
+            AvatarMovementTerrainContact.surface.mode |= d.ContactFlags.SoftERP;
+            AvatarMovementTerrainContact.surface.mu = mTerrainContactFriction;
+            AvatarMovementTerrainContact.surface.bounce = mTerrainContactBounce;
+            AvatarMovementTerrainContact.surface.soft_erp = mTerrainContactERP;
+
+
+            /*
+                <summary></summary>
+                Stone = 0,
+                /// <summary></summary>
+                Metal = 1,
+                /// <summary></summary>
+                Glass = 2,
+                /// <summary></summary>
+                Wood = 3,
+                /// <summary></summary>
+                Flesh = 4,
+                /// <summary></summary>
+                Plastic = 5,
+                /// <summary></summary>
+                Rubber = 6
+             */
+
+            m_materialContacts = new d.Contact[7,2];
+
+            m_materialContacts[(int)Material.Stone, 0] = new d.Contact();
+            m_materialContacts[(int)Material.Stone, 0].surface.mode |= d.ContactFlags.SoftERP;
+            m_materialContacts[(int)Material.Stone, 0].surface.mu = nmAvatarObjectContactFriction;
+            m_materialContacts[(int)Material.Stone, 0].surface.bounce = nmAvatarObjectContactBounce;
+            m_materialContacts[(int)Material.Stone, 0].surface.soft_cfm = 0.010f;
+            m_materialContacts[(int)Material.Stone, 0].surface.soft_erp = 0.010f;
+
+            m_materialContacts[(int)Material.Stone, 1] = new d.Contact();
+            m_materialContacts[(int)Material.Stone, 1].surface.mode |= d.ContactFlags.SoftERP;
+            m_materialContacts[(int)Material.Stone, 1].surface.mu = mAvatarObjectContactFriction;
+            m_materialContacts[(int)Material.Stone, 1].surface.bounce = mAvatarObjectContactBounce;
+            m_materialContacts[(int)Material.Stone, 1].surface.soft_cfm = 0.010f;
+            m_materialContacts[(int)Material.Stone, 1].surface.soft_erp = 0.010f;
+
+            m_materialContacts[(int)Material.Metal, 0] = new d.Contact();
+            m_materialContacts[(int)Material.Metal, 0].surface.mode |= d.ContactFlags.SoftERP;
+            m_materialContacts[(int)Material.Metal, 0].surface.mu = nmAvatarObjectContactFriction;
+            m_materialContacts[(int)Material.Metal, 0].surface.bounce = nmAvatarObjectContactBounce;
+            m_materialContacts[(int)Material.Metal, 0].surface.soft_cfm = 0.010f;
+            m_materialContacts[(int)Material.Metal, 0].surface.soft_erp = 0.010f;
+
+            m_materialContacts[(int)Material.Metal, 1] = new d.Contact();
+            m_materialContacts[(int)Material.Metal, 1].surface.mode |= d.ContactFlags.SoftERP;
+            m_materialContacts[(int)Material.Metal, 1].surface.mu = mAvatarObjectContactFriction;
+            m_materialContacts[(int)Material.Metal, 1].surface.bounce = mAvatarObjectContactBounce;
+            m_materialContacts[(int)Material.Metal, 1].surface.soft_cfm = 0.010f;
+            m_materialContacts[(int)Material.Metal, 1].surface.soft_erp = 0.010f;
+
+            m_materialContacts[(int)Material.Glass, 0] = new d.Contact();
+            m_materialContacts[(int)Material.Glass, 0].surface.mode |= d.ContactFlags.SoftERP;
+            m_materialContacts[(int)Material.Glass, 0].surface.mu = 1f;
+            m_materialContacts[(int)Material.Glass, 0].surface.bounce = 0.5f;
+            m_materialContacts[(int)Material.Glass, 0].surface.soft_cfm = 0.010f;
+            m_materialContacts[(int)Material.Glass, 0].surface.soft_erp = 0.010f;
+
+            /*
+                private float nmAvatarObjectContactFriction = 250f;
+                private float nmAvatarObjectContactBounce = 0.1f;
+
+                private float mAvatarObjectContactFriction = 75f;
+                private float mAvatarObjectContactBounce = 0.1f;
+            */
+            m_materialContacts[(int)Material.Glass, 1] = new d.Contact();
+            m_materialContacts[(int)Material.Glass, 1].surface.mode |= d.ContactFlags.SoftERP;
+            m_materialContacts[(int)Material.Glass, 1].surface.mu = 1f;
+            m_materialContacts[(int)Material.Glass, 1].surface.bounce = 0.5f;
+            m_materialContacts[(int)Material.Glass, 1].surface.soft_cfm = 0.010f;
+            m_materialContacts[(int)Material.Glass, 1].surface.soft_erp = 0.010f;
+
+            m_materialContacts[(int)Material.Wood, 0] = new d.Contact();
+            m_materialContacts[(int)Material.Wood, 0].surface.mode |= d.ContactFlags.SoftERP;
+            m_materialContacts[(int)Material.Wood, 0].surface.mu = nmAvatarObjectContactFriction;
+            m_materialContacts[(int)Material.Wood, 0].surface.bounce = nmAvatarObjectContactBounce;
+            m_materialContacts[(int)Material.Wood, 0].surface.soft_cfm = 0.010f;
+            m_materialContacts[(int)Material.Wood, 0].surface.soft_erp = 0.010f;
+
+            m_materialContacts[(int)Material.Wood, 1] = new d.Contact();
+            m_materialContacts[(int)Material.Wood, 1].surface.mode |= d.ContactFlags.SoftERP;
+            m_materialContacts[(int)Material.Wood, 1].surface.mu = mAvatarObjectContactFriction;
+            m_materialContacts[(int)Material.Wood, 1].surface.bounce = mAvatarObjectContactBounce;
+            m_materialContacts[(int)Material.Wood, 1].surface.soft_cfm = 0.010f;
+            m_materialContacts[(int)Material.Wood, 1].surface.soft_erp = 0.010f;
+
+            m_materialContacts[(int)Material.Flesh, 0] = new d.Contact();
+            m_materialContacts[(int)Material.Flesh, 0].surface.mode |= d.ContactFlags.SoftERP;
+            m_materialContacts[(int)Material.Flesh, 0].surface.mu = nmAvatarObjectContactFriction;
+            m_materialContacts[(int)Material.Flesh, 0].surface.bounce = nmAvatarObjectContactBounce;
+            m_materialContacts[(int)Material.Flesh, 0].surface.soft_cfm = 0.010f;
+            m_materialContacts[(int)Material.Flesh, 0].surface.soft_erp = 0.010f;
+
+            m_materialContacts[(int)Material.Flesh, 1] = new d.Contact();
+            m_materialContacts[(int)Material.Flesh, 1].surface.mode |= d.ContactFlags.SoftERP;
+            m_materialContacts[(int)Material.Flesh, 1].surface.mu = mAvatarObjectContactFriction;
+            m_materialContacts[(int)Material.Flesh, 1].surface.bounce = mAvatarObjectContactBounce;
+            m_materialContacts[(int)Material.Flesh, 1].surface.soft_cfm = 0.010f;
+            m_materialContacts[(int)Material.Flesh, 1].surface.soft_erp = 0.010f;
+
+            m_materialContacts[(int)Material.Plastic, 0] = new d.Contact();
+            m_materialContacts[(int)Material.Plastic, 0].surface.mode |= d.ContactFlags.SoftERP;
+            m_materialContacts[(int)Material.Plastic, 0].surface.mu = nmAvatarObjectContactFriction;
+            m_materialContacts[(int)Material.Plastic, 0].surface.bounce = nmAvatarObjectContactBounce;
+            m_materialContacts[(int)Material.Plastic, 0].surface.soft_cfm = 0.010f;
+            m_materialContacts[(int)Material.Plastic, 0].surface.soft_erp = 0.010f;
+
+            m_materialContacts[(int)Material.Plastic, 1] = new d.Contact();
+            m_materialContacts[(int)Material.Plastic, 1].surface.mode |= d.ContactFlags.SoftERP;
+            m_materialContacts[(int)Material.Plastic, 1].surface.mu = mAvatarObjectContactFriction;
+            m_materialContacts[(int)Material.Plastic, 1].surface.bounce = mAvatarObjectContactBounce;
+            m_materialContacts[(int)Material.Plastic, 1].surface.soft_cfm = 0.010f;
+            m_materialContacts[(int)Material.Plastic, 1].surface.soft_erp = 0.010f;
+
+            m_materialContacts[(int)Material.Rubber, 0] = new d.Contact();
+            m_materialContacts[(int)Material.Rubber, 0].surface.mode |= d.ContactFlags.SoftERP;
+            m_materialContacts[(int)Material.Rubber, 0].surface.mu = nmAvatarObjectContactFriction;
+            m_materialContacts[(int)Material.Rubber, 0].surface.bounce = nmAvatarObjectContactBounce;
+            m_materialContacts[(int)Material.Rubber, 0].surface.soft_cfm = 0.010f;
+            m_materialContacts[(int)Material.Rubber, 0].surface.soft_erp = 0.010f;
+
+            m_materialContacts[(int)Material.Rubber, 1] = new d.Contact();
+            m_materialContacts[(int)Material.Rubber, 1].surface.mode |= d.ContactFlags.SoftERP;
+            m_materialContacts[(int)Material.Rubber, 1].surface.mu = mAvatarObjectContactFriction;
+            m_materialContacts[(int)Material.Rubber, 1].surface.bounce = mAvatarObjectContactBounce;
+            m_materialContacts[(int)Material.Rubber, 1].surface.soft_cfm = 0.010f;
+            m_materialContacts[(int)Material.Rubber, 1].surface.soft_erp = 0.010f;
+
+            d.HashSpaceSetLevels(space, worldHashspaceLow, worldHashspaceHigh);
+
+            // Set the gravity,, don't disable things automatically (we set it explicitly on some things)
+
+            d.WorldSetGravity(world, gravityx, gravityy, gravityz);
+            d.WorldSetContactSurfaceLayer(world, contactsurfacelayer);
+
+            d.WorldSetLinearDamping(world, 256f);
+            d.WorldSetAngularDamping(world, 256f);
+            d.WorldSetAngularDampingThreshold(world, 256f);
+            d.WorldSetLinearDampingThreshold(world, 256f);
+            d.WorldSetMaxAngularSpeed(world, 256f);
+
+            // Set how many steps we go without running collision testing
+            // This is in addition to the step size.
+            // Essentially Steps * m_physicsiterations
+            d.WorldSetQuickStepNumIterations(world, m_physicsiterations);
+            //d.WorldSetContactMaxCorrectingVel(world, 1000.0f);
+
+            
+
+            for (int i = 0; i < staticPrimspace.GetLength(0); i++)
+            {
+                for (int j = 0; j < staticPrimspace.GetLength(1); j++)
+                {
+                    staticPrimspace[i, j] = IntPtr.Zero;
+                }
+            }
+        }
+
+        internal void waitForSpaceUnlock(IntPtr space)
+        {
+            //if (space != IntPtr.Zero)
+                //while (d.SpaceLockQuery(space)) { } // Wait and do nothing
+        }
+
+        /// <summary>
+        /// Debug space message for printing the space that a prim/avatar is in.
+        /// </summary>
+        /// <param name="pos"></param>
+        /// <returns>Returns which split up space the given position is in.</returns>
+        public string whichspaceamIin(Vector3 pos)
+        {
+            return calculateSpaceForGeom(pos).ToString();
+        }
+
+        #region Collision Detection
+
+        /// <summary>
+        /// This is our near callback.  A geometry is near a body
+        /// </summary>
+        /// <param name="space">The space that contains the geoms.  Remember, spaces are also geoms</param>
+        /// <param name="g1">a geometry or space</param>
+        /// <param name="g2">another geometry or space</param>
+        private void near(IntPtr space, IntPtr g1, IntPtr g2)
+        {
+            //  no lock here!  It's invoked from within Simulate(), which is thread-locked
+
+            // Test if we're colliding a geom with a space.
+            // If so we have to drill down into the space recursively
+
+            if (d.GeomIsSpace(g1) || d.GeomIsSpace(g2))
+            {
+                if (g1 == IntPtr.Zero || g2 == IntPtr.Zero)
+                    return;
+                
+                // Separating static prim geometry spaces.
+                // We'll be calling near recursivly if one
+                // of them is a space to find all of the
+                // contact points in the space
+                try
+                {
+                    d.SpaceCollide2(g1, g2, IntPtr.Zero, nearCallback);
+                }
+                catch (AccessViolationException)
+                {
+                    m_log.Warn("[PHYSICS]: Unable to collide test a space");
+                    return;
+                }
+                //Colliding a space or a geom with a space or a geom. so drill down
+
+                //Collide all geoms in each space..
+                //if (d.GeomIsSpace(g1)) d.SpaceCollide(g1, IntPtr.Zero, nearCallback);
+                //if (d.GeomIsSpace(g2)) d.SpaceCollide(g2, IntPtr.Zero, nearCallback);
+                return;
+            }
+
+            if (g1 == IntPtr.Zero || g2 == IntPtr.Zero)
+                return;
+
+            IntPtr b1 = d.GeomGetBody(g1);
+            IntPtr b2 = d.GeomGetBody(g2);
+
+            // d.GeomClassID id = d.GeomGetClass(g1);
+
+            String name1 = null;
+            String name2 = null;
+
+            if (!geom_name_map.TryGetValue(g1, out name1))
+            {
+                name1 = "null";
+            }
+            if (!geom_name_map.TryGetValue(g2, out name2))
+            {
+                name2 = "null";
+            }
+
+            //if (id == d.GeomClassId.TriMeshClass)
+            //{
+                //               m_log.InfoFormat("near: A collision was detected between {1} and {2}", 0, name1, name2);
+                //m_log.Debug("near: A collision was detected between {1} and {2}", 0, name1, name2);
+            //}
+
+            // Figure out how many contact points we have
+            int count = 0;
+            try
+            {
+                // Colliding Geom To Geom
+                // This portion of the function 'was' blatantly ripped off from BoxStack.cs
+
+                if (g1 == g2)
+                    return; // Can't collide with yourself
+
+                if (b1 != IntPtr.Zero && b2 != IntPtr.Zero && d.AreConnectedExcluding(b1, b2, d.JointType.Contact))
+                    return;
+
+                lock (contacts)
+                {
+                    count = d.Collide(g1, g2, contacts.Length, contacts, d.ContactGeom.SizeOf);
+                    if (count > contacts.Length)
+                        m_log.Error("[PHYSICS]: Got " + count + " contacts when we asked for a maximum of " + contacts.Length);
+                }
+            }
+            catch (SEHException)
+            {
+                m_log.Error("[PHYSICS]: The Operating system shut down ODE because of corrupt memory.  This could be a result of really irregular terrain.  If this repeats continuously, restart using Basic Physics and terrain fill your terrain.  Restarting the sim.");
+                ode.drelease(world);
+                base.TriggerPhysicsBasedRestart();
+            }
+            catch (Exception e)
+            {
+                m_log.WarnFormat("[PHYSICS]: Unable to collide test an object: {0}", e.Message);
+                return;
+            }
+
+            PhysicsActor p1;
+            PhysicsActor p2;
+
+            if (!actor_name_map.TryGetValue(g1, out p1))
+            {
+                p1 = PANull;
+            }
+
+            if (!actor_name_map.TryGetValue(g2, out p2))
+            {
+                p2 = PANull;
+            }
+
+            ContactPoint maxDepthContact = new ContactPoint();
+            if (p1.CollisionScore + count >= float.MaxValue)
+                p1.CollisionScore = 0;
+            p1.CollisionScore += count;
+
+            if (p2.CollisionScore + count >= float.MaxValue)
+                p2.CollisionScore = 0;
+            p2.CollisionScore += count;
+
+            for (int i = 0; i < count; i++)
+            {
+                d.ContactGeom curContact = contacts[i];
+
+                if (curContact.depth > maxDepthContact.PenetrationDepth)
+                {
+                    maxDepthContact = new ContactPoint(
+                        new Vector3(curContact.pos.X, curContact.pos.Y, curContact.pos.Z),
+                        new Vector3(curContact.normal.X, curContact.normal.Y, curContact.normal.Z),
+                        curContact.depth
+                    );
+                }
+
+                //m_log.Warn("[CCOUNT]: " + count);
+                IntPtr joint;
+                // If we're colliding with terrain, use 'TerrainContact' instead of contact.
+                // allows us to have different settings
+                
+                // We only need to test p2 for 'jump crouch purposes'
+                if (p2 is OdeCharacter && p1.PhysicsActorType == (int)ActorTypes.Prim)
+                {
+                    // Testing if the collision is at the feet of the avatar
+
+                    //m_log.DebugFormat("[PHYSICS]: {0} - {1} - {2} - {3}", curContact.pos.Z, p2.Position.Z, (p2.Position.Z - curContact.pos.Z), (p2.Size.Z * 0.6f));
+                    if ((p2.Position.Z - curContact.pos.Z) > (p2.Size.Z * 0.6f))
+                        p2.IsColliding = true;
+                }
+                else
+                {
+                    p2.IsColliding = true;
+                }
+                
+                //if ((framecount % m_returncollisions) == 0)
+
+                switch (p1.PhysicsActorType)
+                {
+                    case (int)ActorTypes.Agent:
+                        p2.CollidingObj = true;
+                        break;
+                    case (int)ActorTypes.Prim:
+                        if (p2.Velocity.LengthSquared() > 0.0f)
+                            p2.CollidingObj = true;
+                        break;
+                    case (int)ActorTypes.Unknown:
+                        p2.CollidingGround = true;
+                        break;
+                    default:
+                        p2.CollidingGround = true;
+                        break;
+                }
+
+                // we don't want prim or avatar to explode
+
+                #region InterPenetration Handling - Unintended physics explosions
+# region disabled code1
+
+                if (curContact.depth >= 0.08f)
+                {
+                    //This is disabled at the moment only because it needs more tweaking
+                    //It will eventually be uncommented
+                    /*
+                    if (contact.depth >= 1.00f)
+                    {
+                        //m_log.Debug("[PHYSICS]: " + contact.depth.ToString());
+                    }
+
+                    //If you interpenetrate a prim with an agent
+                    if ((p2.PhysicsActorType == (int) ActorTypes.Agent &&
+                         p1.PhysicsActorType == (int) ActorTypes.Prim) ||
+                        (p1.PhysicsActorType == (int) ActorTypes.Agent &&
+                         p2.PhysicsActorType == (int) ActorTypes.Prim))
+                    {
+                        
+                        //contact.depth = contact.depth * 4.15f;
+                        /*
+                        if (p2.PhysicsActorType == (int) ActorTypes.Agent)
+                        {
+                            p2.CollidingObj = true;
+                            contact.depth = 0.003f;
+                            p2.Velocity = p2.Velocity + new PhysicsVector(0, 0, 2.5f);
+                            OdeCharacter character = (OdeCharacter) p2;
+                            character.SetPidStatus(true);
+                            contact.pos = new d.Vector3(contact.pos.X + (p1.Size.X / 2), contact.pos.Y + (p1.Size.Y / 2), contact.pos.Z + (p1.Size.Z / 2));
+
+                        }
+                        else
+                        {
+
+                            //contact.depth = 0.0000000f;
+                        }
+                        if (p1.PhysicsActorType == (int) ActorTypes.Agent)
+                        {
+
+                            p1.CollidingObj = true;
+                            contact.depth = 0.003f;
+                            p1.Velocity = p1.Velocity + new PhysicsVector(0, 0, 2.5f);
+                            contact.pos = new d.Vector3(contact.pos.X + (p2.Size.X / 2), contact.pos.Y + (p2.Size.Y / 2), contact.pos.Z + (p2.Size.Z / 2));
+                            OdeCharacter character = (OdeCharacter)p1;
+                            character.SetPidStatus(true);
+                        }
+                        else
+                        {
+
+                            //contact.depth = 0.0000000f;
+                        }
+                          
+                        
+                     
+                    }
+*/
+                    // If you interpenetrate a prim with another prim
+                /*
+                    if (p1.PhysicsActorType == (int) ActorTypes.Prim && p2.PhysicsActorType == (int) ActorTypes.Prim)
+                    {
+                        #region disabledcode2
+                        //OdePrim op1 = (OdePrim)p1;
+                        //OdePrim op2 = (OdePrim)p2;
+                        //op1.m_collisionscore++;
+                        //op2.m_collisionscore++;
+
+                        //if (op1.m_collisionscore > 8000 || op2.m_collisionscore > 8000)
+                        //{
+                            //op1.m_taintdisable = true;
+                            //AddPhysicsActorTaint(p1);
+                            //op2.m_taintdisable = true;
+                            //AddPhysicsActorTaint(p2);
+                        //}
+
+                        //if (contact.depth >= 0.25f)
+                        //{
+                            // Don't collide, one or both prim will expld.
+
+                            //op1.m_interpenetrationcount++;
+                            //op2.m_interpenetrationcount++;
+                            //interpenetrations_before_disable = 200;
+                            //if (op1.m_interpenetrationcount >= interpenetrations_before_disable)
+                            //{
+                                //op1.m_taintdisable = true;
+                                //AddPhysicsActorTaint(p1);
+                            //}
+                            //if (op2.m_interpenetrationcount >= interpenetrations_before_disable)
+                            //{
+                               // op2.m_taintdisable = true;
+                                //AddPhysicsActorTaint(p2);
+                            //}
+
+                            //contact.depth = contact.depth / 8f;
+                            //contact.normal = new d.Vector3(0, 0, 1);
+                        //}
+                        //if (op1.m_disabled || op2.m_disabled)
+                        //{
+                            //Manually disabled objects stay disabled
+                            //contact.depth = 0f;
+                        //}
+                        #endregion
+                    }
+                    */
+#endregion
+                    if (curContact.depth >= 1.00f)
+                    {
+                        //m_log.Info("[P]: " + contact.depth.ToString());
+                        if ((p2.PhysicsActorType == (int) ActorTypes.Agent &&
+                             p1.PhysicsActorType == (int) ActorTypes.Unknown) ||
+                            (p1.PhysicsActorType == (int) ActorTypes.Agent &&
+                             p2.PhysicsActorType == (int) ActorTypes.Unknown))
+                        {
+                            if (p2.PhysicsActorType == (int) ActorTypes.Agent)
+                            {
+                                if (p2 is OdeCharacter)
+                                {
+                                    OdeCharacter character = (OdeCharacter) p2;
+
+                                    //p2.CollidingObj = true;
+                                    curContact.depth = 0.00000003f;
+                                    p2.Velocity = p2.Velocity + new Vector3(0f, 0f, 0.5f);
+                                    curContact.pos =
+                                        new d.Vector3(curContact.pos.X + (p1.Size.X/2),
+                                                      curContact.pos.Y + (p1.Size.Y/2),
+                                                      curContact.pos.Z + (p1.Size.Z/2));
+                                    character.SetPidStatus(true);
+                                }
+                            }
+                            
+
+                            if (p1.PhysicsActorType == (int) ActorTypes.Agent)
+                            {
+                                if (p1 is OdeCharacter)
+                                {
+                                    OdeCharacter character = (OdeCharacter) p1;
+
+                                    //p2.CollidingObj = true;
+                                    curContact.depth = 0.00000003f;
+                                    p1.Velocity = p1.Velocity + new Vector3(0f, 0f, 0.5f);
+                                    curContact.pos =
+                                        new d.Vector3(curContact.pos.X + (p1.Size.X/2),
+                                                      curContact.pos.Y + (p1.Size.Y/2),
+                                                      curContact.pos.Z + (p1.Size.Z/2));
+                                    character.SetPidStatus(true);
+                                }
+                            }
+                        }
+                    }
+                }
+
+                #endregion
+
+                // Logic for collision handling
+                // Note, that if *all* contacts are skipped (VolumeDetect)
+                // The prim still detects (and forwards) collision events but 
+                // appears to be phantom for the world
+                Boolean skipThisContact = false;
+
+                if ((p1 is OdePrim) && (((OdePrim)p1).m_isVolumeDetect))
+                    skipThisContact = true;   // No collision on volume detect prims
+
+                if (!skipThisContact && (p2 is OdePrim) && (((OdePrim)p2).m_isVolumeDetect))
+                    skipThisContact = true;   // No collision on volume detect prims
+
+                if (!skipThisContact && curContact.depth < 0f)
+                    skipThisContact = true;
+
+                if (!skipThisContact && checkDupe(curContact, p2.PhysicsActorType))
+                    skipThisContact = true;
+
+                const int maxContactsbeforedeath = 4000;
+                joint = IntPtr.Zero;
+
+                if (!skipThisContact)
+                {
+                    // If we're colliding against terrain
+                    if (name1 == "Terrain" || name2 == "Terrain")
+                    {
+                        // If we're moving
+                        if ((p2.PhysicsActorType == (int) ActorTypes.Agent) &&
+                            (Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f))
+                        {
+                            // Use the movement terrain contact
+                            AvatarMovementTerrainContact.geom = curContact;
+                            _perloopContact.Add(curContact);
+                            if (m_global_contactcount < maxContactsbeforedeath)
+                            {
+                                joint = d.JointCreateContact(world, contactgroup, ref AvatarMovementTerrainContact);
+                                m_global_contactcount++;
+                            }
+                        }
+                        else
+                        {
+                            if (p2.PhysicsActorType == (int)ActorTypes.Agent)
+                            {
+                                // Use the non moving terrain contact
+                                TerrainContact.geom = curContact;
+                                _perloopContact.Add(curContact);
+                                if (m_global_contactcount < maxContactsbeforedeath)
+                                {
+                                    joint = d.JointCreateContact(world, contactgroup, ref TerrainContact);
+                                    m_global_contactcount++;
+                                }
+                            }
+                            else
+                            {
+                                if (p2.PhysicsActorType == (int)ActorTypes.Prim && p1.PhysicsActorType == (int)ActorTypes.Prim)
+                                {
+                                    // prim prim contact
+                                    // int pj294950 = 0;
+                                    int movintYN = 0;
+                                    int material = (int) Material.Wood;
+                                    // prim terrain contact
+                                    if (Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f)
+                                    {
+                                        movintYN = 1;
+                                    }
+
+                                    if (p2 is OdePrim)
+                                        material = ((OdePrim)p2).m_material;
+
+                                    //m_log.DebugFormat("Material: {0}", material);
+                                    m_materialContacts[material, movintYN].geom = curContact;
+                                    _perloopContact.Add(curContact);
+
+                                    if (m_global_contactcount < maxContactsbeforedeath)
+                                    {
+                                        joint = d.JointCreateContact(world, contactgroup, ref m_materialContacts[material, movintYN]);
+                                        m_global_contactcount++;
+
+                                    }
+                                    
+                                }
+                                else
+                                {
+
+                                    int movintYN = 0;
+                                    // prim terrain contact
+                                    if (Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f)
+                                    {
+                                        movintYN = 1;
+                                    }
+
+                                    int material = (int)Material.Wood;
+
+                                    if (p2 is OdePrim)
+                                        material = ((OdePrim)p2).m_material;
+                                    //m_log.DebugFormat("Material: {0}", material);
+                                    m_materialContacts[material, movintYN].geom = curContact;
+                                    _perloopContact.Add(curContact);
+
+                                    if (m_global_contactcount < maxContactsbeforedeath)
+                                    {
+                                        joint = d.JointCreateContact(world, contactgroup, ref m_materialContacts[material, movintYN]);
+                                        m_global_contactcount++;
+
+                                    }
+                                }
+                            }
+                        }
+                        //if (p2.PhysicsActorType == (int)ActorTypes.Prim)
+                        //{
+                        //m_log.Debug("[PHYSICS]: prim contacting with ground");
+                        //}
+                    }
+                    else if (name1 == "Water" || name2 == "Water")
+                    {
+                        /*
+                        if ((p2.PhysicsActorType == (int) ActorTypes.Prim))
+                        {
+                        }
+                        else
+                        {
+                        }
+                        */
+                        //WaterContact.surface.soft_cfm = 0.0000f;
+                        //WaterContact.surface.soft_erp = 0.00000f;
+                        if (curContact.depth > 0.1f)
+                        {
+                            curContact.depth *= 52;
+                            //contact.normal = new d.Vector3(0, 0, 1);
+                            //contact.pos = new d.Vector3(0, 0, contact.pos.Z - 5f);
+                        }
+                        WaterContact.geom = curContact;
+                        _perloopContact.Add(curContact);
+                        if (m_global_contactcount < maxContactsbeforedeath)
+                        {
+                            joint = d.JointCreateContact(world, contactgroup, ref WaterContact);
+                            m_global_contactcount++;
+                        }
+                        //m_log.Info("[PHYSICS]: Prim Water Contact" + contact.depth);
+                    }
+                    else
+                    {
+                        // we're colliding with prim or avatar
+                        // check if we're moving
+                        if ((p2.PhysicsActorType == (int)ActorTypes.Agent))
+                        {
+                            if ((Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f))
+                            {
+                                // Use the Movement prim contact
+                                AvatarMovementprimContact.geom = curContact;
+                                _perloopContact.Add(curContact);
+                                if (m_global_contactcount < maxContactsbeforedeath)
+                                {
+                                    joint = d.JointCreateContact(world, contactgroup, ref AvatarMovementprimContact);
+                                    m_global_contactcount++;
+                                }
+                            }
+                            else
+                            {
+                                // Use the non movement contact
+                                contact.geom = curContact;
+                                _perloopContact.Add(curContact);
+
+                                if (m_global_contactcount < maxContactsbeforedeath)
+                                {
+                                    joint = d.JointCreateContact(world, contactgroup, ref contact);
+                                    m_global_contactcount++;
+                                }
+                            }
+                        }
+                        else if (p2.PhysicsActorType == (int)ActorTypes.Prim)
+                        {
+                            //p1.PhysicsActorType
+                            int material = (int)Material.Wood;
+
+                            if (p2 is OdePrim)
+                                material = ((OdePrim)p2).m_material;
+                            
+                            //m_log.DebugFormat("Material: {0}", material);
+                            m_materialContacts[material, 0].geom = curContact;
+                            _perloopContact.Add(curContact);
+
+                            if (m_global_contactcount < maxContactsbeforedeath)
+                            {
+                                joint = d.JointCreateContact(world, contactgroup, ref m_materialContacts[material, 0]);
+                                m_global_contactcount++;
+
+                            }
+                        }
+                    }
+
+                    if (m_global_contactcount < maxContactsbeforedeath && joint != IntPtr.Zero) // stack collide!
+                    {
+                        d.JointAttach(joint, b1, b2);
+                        m_global_contactcount++;
+                    }
+
+                }
+                collision_accounting_events(p1, p2, maxDepthContact);
+                if (count > geomContactPointsStartthrottle)
+                {
+                    // If there are more then 3 contact points, it's likely
+                    // that we've got a pile of objects, so ...
+                    // We don't want to send out hundreds of terse updates over and over again
+                    // so lets throttle them and send them again after it's somewhat sorted out.
+                    p2.ThrottleUpdates = true;
+                }
+                //m_log.Debug(count.ToString());
+                //m_log.Debug("near: A collision was detected between {1} and {2}", 0, name1, name2);
+            }
+        }
+
+        private bool checkDupe(d.ContactGeom contactGeom, int atype)
+        {
+            bool result = false;
+            //return result;
+            if (!m_filterCollisions)
+                return false;
+
+            ActorTypes at = (ActorTypes)atype;
+            lock (_perloopContact)
+            {
+                foreach (d.ContactGeom contact in _perloopContact)
+                {
+                    //if ((contact.g1 == contactGeom.g1 && contact.g2 == contactGeom.g2))
+                    //{
+                        // || (contact.g2 == contactGeom.g1 && contact.g1 == contactGeom.g2)
+                    if (at == ActorTypes.Agent)
+                    {
+                            if (((Math.Abs(contactGeom.normal.X - contact.normal.X) < 1.026f) && (Math.Abs(contactGeom.normal.Y - contact.normal.Y) < 0.303f) && (Math.Abs(contactGeom.normal.Z - contact.normal.Z) < 0.065f)) && contactGeom.g1 != LandGeom && contactGeom.g2 != LandGeom)
+                            {
+                                
+                                if (Math.Abs(contact.depth - contactGeom.depth) < 0.052f)
+                                {
+                                    //contactGeom.depth *= .00005f;
+                                   //m_log.DebugFormat("[Collsion]: Depth {0}", Math.Abs(contact.depth - contactGeom.depth));
+                                    // m_log.DebugFormat("[Collision]: <{0},{1},{2}>", Math.Abs(contactGeom.normal.X - contact.normal.X), Math.Abs(contactGeom.normal.Y - contact.normal.Y), Math.Abs(contactGeom.normal.Z - contact.normal.Z));
+                                    result = true;
+                                    break;
+                                }
+                                else
+                                {
+                                    //m_log.DebugFormat("[Collsion]: Depth {0}", Math.Abs(contact.depth - contactGeom.depth));
+                                }
+                            }
+                            else
+                            {
+                                //m_log.DebugFormat("[Collision]: <{0},{1},{2}>", Math.Abs(contactGeom.normal.X - contact.normal.X), Math.Abs(contactGeom.normal.Y - contact.normal.Y), Math.Abs(contactGeom.normal.Z - contact.normal.Z));
+                                //int i = 0;
+                            }
+                    } 
+                    else if (at == ActorTypes.Prim)
+                    {
+                            //d.AABB aabb1 = new d.AABB();
+                            //d.AABB aabb2 = new d.AABB();
+
+                            //d.GeomGetAABB(contactGeom.g2, out aabb2);
+                            //d.GeomGetAABB(contactGeom.g1, out aabb1);
+                            //aabb1.
+                            if (((Math.Abs(contactGeom.normal.X - contact.normal.X) < 1.026f) && (Math.Abs(contactGeom.normal.Y - contact.normal.Y) < 0.303f) && (Math.Abs(contactGeom.normal.Z - contact.normal.Z) < 0.065f)) && contactGeom.g1 != LandGeom && contactGeom.g2 != LandGeom)
+                            {
+                                if (contactGeom.normal.X == contact.normal.X && contactGeom.normal.Y == contact.normal.Y && contactGeom.normal.Z == contact.normal.Z)
+                                {
+                                    if (Math.Abs(contact.depth - contactGeom.depth) < 0.272f)
+                                    {
+                                        result = true;
+                                        break;
+                                    }
+                                }
+                                //m_log.DebugFormat("[Collsion]: Depth {0}", Math.Abs(contact.depth - contactGeom.depth));
+                                //m_log.DebugFormat("[Collision]: <{0},{1},{2}>", Math.Abs(contactGeom.normal.X - contact.normal.X), Math.Abs(contactGeom.normal.Y - contact.normal.Y), Math.Abs(contactGeom.normal.Z - contact.normal.Z));
+                            }
+                            
+                    }
+                    
+                    //}
+
+                }
+            }
+            return result;
+        }
+
+        private void collision_accounting_events(PhysicsActor p1, PhysicsActor p2, ContactPoint contact)
+        {
+            // obj1LocalID = 0;
+            //returncollisions = false;
+            obj2LocalID = 0;
+            //ctype = 0;
+            //cStartStop = 0;
+            if (!p2.SubscribedEvents() && !p1.SubscribedEvents())
+                return;
+
+            switch ((ActorTypes)p2.PhysicsActorType)
+            {
+                case ActorTypes.Agent:
+                    cc2 = (OdeCharacter)p2;
+
+                    // obj1LocalID = cc2.m_localID;
+                    switch ((ActorTypes)p1.PhysicsActorType)
+                    {
+                        case ActorTypes.Agent:
+                            cc1 = (OdeCharacter)p1;
+                            obj2LocalID = cc1.m_localID;
+                            cc1.AddCollisionEvent(cc2.m_localID, contact);
+                            //ctype = (int)CollisionCategories.Character;
+
+                            //if (cc1.CollidingObj)
+                            //cStartStop = (int)StatusIndicators.Generic;
+                            //else
+                            //cStartStop = (int)StatusIndicators.Start;
+
+                            //returncollisions = true;
+                            break;
+                        case ActorTypes.Prim:
+                            if (p1 is OdePrim)
+                            {
+                                cp1 = (OdePrim) p1;
+                                obj2LocalID = cp1.m_localID;
+                                cp1.AddCollisionEvent(cc2.m_localID, contact);
+                            }
+                            //ctype = (int)CollisionCategories.Geom;
+
+                            //if (cp1.CollidingObj)
+                            //cStartStop = (int)StatusIndicators.Generic;
+                            //else
+                            //cStartStop = (int)StatusIndicators.Start;
+
+                            //returncollisions = true;
+                            break;
+
+                        case ActorTypes.Ground:
+                        case ActorTypes.Unknown:
+                            obj2LocalID = 0;
+                            //ctype = (int)CollisionCategories.Land;
+                            //returncollisions = true;
+                            break;
+                    }
+
+                    cc2.AddCollisionEvent(obj2LocalID, contact);
+                    break;
+                case ActorTypes.Prim:
+
+                    if (p2 is OdePrim)
+                    {
+                        cp2 = (OdePrim) p2;
+
+                        // obj1LocalID = cp2.m_localID;
+                        switch ((ActorTypes) p1.PhysicsActorType)
+                        {
+                            case ActorTypes.Agent:
+                                if (p1 is OdeCharacter)
+                                {
+                                    cc1 = (OdeCharacter) p1;
+                                    obj2LocalID = cc1.m_localID;
+                                    cc1.AddCollisionEvent(cp2.m_localID, contact);
+                                    //ctype = (int)CollisionCategories.Character;
+
+                                    //if (cc1.CollidingObj)
+                                    //cStartStop = (int)StatusIndicators.Generic;
+                                    //else
+                                    //cStartStop = (int)StatusIndicators.Start;
+                                    //returncollisions = true;
+                                }
+                                break;
+                            case ActorTypes.Prim:
+
+                                if (p1 is OdePrim)
+                                {
+                                    cp1 = (OdePrim) p1;
+                                    obj2LocalID = cp1.m_localID;
+                                    cp1.AddCollisionEvent(cp2.m_localID, contact);
+                                    //ctype = (int)CollisionCategories.Geom;
+
+                                    //if (cp1.CollidingObj)
+                                    //cStartStop = (int)StatusIndicators.Generic;
+                                    //else
+                                    //cStartStop = (int)StatusIndicators.Start;
+
+                                    //returncollisions = true;
+                                }
+                                break;
+
+                            case ActorTypes.Ground:
+                            case ActorTypes.Unknown:
+                                obj2LocalID = 0;
+                                //ctype = (int)CollisionCategories.Land;
+
+                                //returncollisions = true;
+                                break;
+                        }
+
+                        cp2.AddCollisionEvent(obj2LocalID, contact);
+                    }
+                    break;
+            }
+            //if (returncollisions)
+            //{
+
+                //lock (m_storedCollisions)
+                //{
+                    //cDictKey = obj1LocalID.ToString() + obj2LocalID.ToString() + cStartStop.ToString() + ctype.ToString();
+                    //if (m_storedCollisions.ContainsKey(cDictKey))
+                    //{
+                        //sCollisionData objd = m_storedCollisions[cDictKey];
+                        //objd.NumberOfCollisions += 1;
+                        //objd.lastframe = framecount;
+                        //m_storedCollisions[cDictKey] = objd;
+                    //}
+                    //else
+                    //{
+                        //sCollisionData objd = new sCollisionData();
+                        //objd.ColliderLocalId = obj1LocalID;
+                        //objd.CollidedWithLocalId = obj2LocalID;
+                        //objd.CollisionType = ctype;
+                        //objd.NumberOfCollisions = 1;
+                        //objd.lastframe = framecount;
+                        //objd.StatusIndicator = cStartStop;
+                        //m_storedCollisions.Add(cDictKey, objd);
+                    //}
+                //}
+           // }
+        }
+
+        public int TriArrayCallback(IntPtr trimesh, IntPtr refObject, int[] triangleIndex, int triCount)
+        {
+            /*            String name1 = null;
+                        String name2 = null;
+
+                        if (!geom_name_map.TryGetValue(trimesh, out name1))
+                        {
+                            name1 = "null";
+                        }
+                        if (!geom_name_map.TryGetValue(refObject, out name2))
+                        {
+                            name2 = "null";
+                        }
+
+                        m_log.InfoFormat("TriArrayCallback: A collision was detected between {1} and {2}", 0, name1, name2);
+            */
+            return 1;
+        }
+
+        public int TriCallback(IntPtr trimesh, IntPtr refObject, int triangleIndex)
+        {
+            String name1 = null;
+            String name2 = null;
+
+            if (!geom_name_map.TryGetValue(trimesh, out name1))
+            {
+                name1 = "null";
+            }
+
+            if (!geom_name_map.TryGetValue(refObject, out name2))
+            {
+                name2 = "null";
+            }
+
+            //            m_log.InfoFormat("TriCallback: A collision was detected between {1} and {2}. Index was {3}", 0, name1, name2, triangleIndex);
+
+            d.Vector3 v0 = new d.Vector3();
+            d.Vector3 v1 = new d.Vector3();
+            d.Vector3 v2 = new d.Vector3();
+
+            d.GeomTriMeshGetTriangle(trimesh, 0, ref v0, ref v1, ref v2);
+            //            m_log.DebugFormat("Triangle {0} is <{1},{2},{3}>, <{4},{5},{6}>, <{7},{8},{9}>", triangleIndex, v0.X, v0.Y, v0.Z, v1.X, v1.Y, v1.Z, v2.X, v2.Y, v2.Z);
+
+            return 1;
+        }
+
+        /// <summary>
+        /// This is our collision testing routine in ODE
+        /// </summary>
+        /// <param name="timeStep"></param>
+        private void collision_optimized(float timeStep)
+        {
+            _perloopContact.Clear();
+
+            lock (_characters)
+            {
+                foreach (OdeCharacter chr in _characters)
+                {
+                    // Reset the collision values to false
+                    // since we don't know if we're colliding yet
+                    
+                    // For some reason this can happen. Don't ask...
+                    //
+                    if (chr == null)
+                        continue;
+                    
+                    if (chr.Shell == IntPtr.Zero || chr.Body == IntPtr.Zero)
+                        continue;
+                    
+                    chr.IsColliding = false;
+                    chr.CollidingGround = false;
+                    chr.CollidingObj = false;
+                    
+                    // test the avatar's geometry for collision with the space
+                    // This will return near and the space that they are the closest to
+                    // And we'll run this again against the avatar and the space segment
+                    // This will return with a bunch of possible objects in the space segment
+                    // and we'll run it again on all of them.
+                    try
+                    {
+                        d.SpaceCollide2(space, chr.Shell, IntPtr.Zero, nearCallback);
+                    }
+                    catch (AccessViolationException)
+                    {
+                        m_log.Warn("[PHYSICS]: Unable to space collide");
+                    }
+                    //float terrainheight = GetTerrainHeightAtXY(chr.Position.X, chr.Position.Y);
+                    //if (chr.Position.Z + (chr.Velocity.Z * timeStep) < terrainheight + 10)
+                    //{
+                    //chr.Position.Z = terrainheight + 10.0f;
+                    //forcedZ = true;
+                    //}
+                }
+            }
+
+            lock (_activeprims)
+            {
+                List<OdePrim> removeprims = null;
+                foreach (OdePrim chr in _activeprims)
+                {
+                    if (chr.Body != IntPtr.Zero && d.BodyIsEnabled(chr.Body) && (!chr.m_disabled))
+                    {
+                        try
+                        {
+                            lock (chr)
+                            {
+                                if (space != IntPtr.Zero && chr.prim_geom != IntPtr.Zero && chr.m_taintremove == false)
+                                {
+                                    d.SpaceCollide2(space, chr.prim_geom, IntPtr.Zero, nearCallback);
+                                }
+                                else
+                                {
+                                    if (removeprims == null)
+                                    {
+                                        removeprims = new List<OdePrim>();
+                                    }
+                                    removeprims.Add(chr);
+                                    m_log.Debug("[PHYSICS]: unable to collide test active prim against space.  The space was zero, the geom was zero or it was in the process of being removed.  Removed it from the active prim list.  This needs to be fixed!");
+                                }
+                            }
+                        }
+                        catch (AccessViolationException)
+                        {
+                            m_log.Warn("[PHYSICS]: Unable to space collide");
+                        }
+                    }
+                }
+                if (removeprims != null)
+                {
+                    foreach (OdePrim chr in removeprims)
+                    {
+                        _activeprims.Remove(chr);
+                    }
+                }
+            }
+
+            _perloopContact.Clear();
+        }
+
+        #endregion
+
+        public override void Combine(PhysicsScene pScene, Vector3 offset, Vector3 extents)
+        {
+            m_worldOffset = offset;
+            WorldExtents = new Vector2(extents.X, extents.Y);
+            m_parentScene = pScene;
+            
+        }
+        
+        // Recovered for use by fly height. Kitto Flora
+        public float GetTerrainHeightAtXY(float x, float y)
+        {
+
+            int offsetX = ((int)(x / (int)Constants.RegionSize)) * (int)Constants.RegionSize;
+            int offsetY = ((int)(y / (int)Constants.RegionSize)) * (int)Constants.RegionSize;
+
+            IntPtr heightFieldGeom = IntPtr.Zero;
+
+            if (RegionTerrain.TryGetValue(new Vector3(offsetX,offsetY,0), out heightFieldGeom))
+            {
+                if (heightFieldGeom != IntPtr.Zero)
+                {
+                    if (TerrainHeightFieldHeights.ContainsKey(heightFieldGeom))
+                    {
+                        
+                        int index;
+
+
+                        if ((int)x > WorldExtents.X || (int)y > WorldExtents.Y ||
+                            (int)x < 0.001f || (int)y < 0.001f)
+                            return 0;
+
+                        x = x - offsetX;
+                        y = y - offsetY;
+
+                        index = (int)((int)x * ((int)Constants.RegionSize + 2) + (int)y);
+
+                        if (index < TerrainHeightFieldHeights[heightFieldGeom].Length)
+                        {
+                            //m_log.DebugFormat("x{0} y{1} = {2}", x, y, (float)TerrainHeightFieldHeights[heightFieldGeom][index]);
+                            return (float)TerrainHeightFieldHeights[heightFieldGeom][index];
+                        }
+                            
+                        else
+                            return 0f;
+                    }
+                    else
+                    {
+                        return 0f;
+                    }
+
+                }
+                else
+                {
+                    return 0f;
+                }
+
+            }
+            else
+            {
+                return 0f;
+            }
+
+
+        } 
+// End recovered. Kitto Flora
+
+        public void addCollisionEventReporting(PhysicsActor obj)
+        {
+            lock (_collisionEventPrim)
+            {
+                if (!_collisionEventPrim.Contains(obj))
+                    _collisionEventPrim.Add(obj);
+            }
+        }
+
+        public void remCollisionEventReporting(PhysicsActor obj)
+        {
+            lock (_collisionEventPrim)
+            {
+                if (!_collisionEventPrim.Contains(obj))
+                    _collisionEventPrim.Remove(obj);
+            }
+        }
+
+        #region Add/Remove Entities
+
+        public override PhysicsActor AddAvatar(string avName, Vector3 position, Vector3 size, bool isFlying)
+        {
+            Vector3 pos;
+            pos.X = position.X;
+            pos.Y = position.Y;
+            pos.Z = position.Z;
+            OdeCharacter newAv = new OdeCharacter(avName, this, pos, ode, size, avPIDD, avPIDP, avCapRadius, avStandupTensor, avDensity, avHeightFudgeFactor, avMovementDivisorWalk, avMovementDivisorRun);
+            newAv.Flying = isFlying;
+            newAv.MinimumGroundFlightOffset = minimumGroundFlightOffset;
+            
+            return newAv;
+        }
+
+        public void AddCharacter(OdeCharacter chr)
+        {
+            lock (_characters)
+            {
+                if (!_characters.Contains(chr))
+                {
+                    _characters.Add(chr);
+                    if (chr.bad)
+                        m_log.DebugFormat("[PHYSICS] Added BAD actor {0} to characters list", chr.m_uuid);
+                }
+            }
+        }
+
+        public void RemoveCharacter(OdeCharacter chr)
+        {
+            lock (_characters)
+            {
+                if (_characters.Contains(chr))
+                {
+                    _characters.Remove(chr);
+                }
+            }
+        }
+        public void BadCharacter(OdeCharacter chr)
+        {
+            lock (_badCharacter)
+            {
+                if (!_badCharacter.Contains(chr))
+                    _badCharacter.Add(chr);
+            }
+        }
+
+        public override void RemoveAvatar(PhysicsActor actor)
+        {
+            //m_log.Debug("[PHYSICS]:ODELOCK");
+            ((OdeCharacter) actor).Destroy();
+            
+        }
+
+        private PhysicsActor AddPrim(String name, Vector3 position, Vector3 size, Quaternion rotation,
+                                     IMesh mesh, PrimitiveBaseShape pbs, bool isphysical)
+        {
+
+            Vector3 pos = position;
+            Vector3 siz = size;
+            Quaternion rot = rotation;
+
+            OdePrim newPrim;
+            lock (OdeLock)
+            {
+                newPrim = new OdePrim(name, this, pos, siz, rot, mesh, pbs, isphysical, ode);
+
+                lock (_prims)
+                    _prims.Add(newPrim);
+            }
+
+            return newPrim;
+        }
+
+        public void addActivePrim(OdePrim activatePrim)
+        {
+            // adds active prim..   (ones that should be iterated over in collisions_optimized
+            lock (_activeprims)
+            {
+                if (!_activeprims.Contains(activatePrim))
+                    _activeprims.Add(activatePrim);
+                //else
+                  //  m_log.Warn("[PHYSICS]: Double Entry in _activeprims detected, potential crash immenent");
+            }
+        }
+
+        public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
+                                                  Vector3 size, Quaternion rotation) //To be removed
+        {
+            return AddPrimShape(primName, pbs, position, size, rotation, false);
+        }
+
+        public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
+                                                  Vector3 size, Quaternion rotation, bool isPhysical)
+        {
+            PhysicsActor result;
+            IMesh mesh = null;
+
+            if (needsMeshing(pbs))
+                mesh = mesher.CreateMesh(primName, pbs, size, 32f, isPhysical);
+
+            result = AddPrim(primName, position, size, rotation, mesh, pbs, isPhysical);
+
+            return result;
+        }
+
+        public override float TimeDilation
+        {
+            get { return m_timeDilation; }
+        }
+
+        public override bool SupportsNINJAJoints
+        {
+            get { return m_NINJA_physics_joints_enabled; }
+        }
+
+        // internal utility function: must be called within a lock (OdeLock)
+        private void InternalAddActiveJoint(PhysicsJoint joint)
+        {
+            activeJoints.Add(joint);
+            SOPName_to_activeJoint.Add(joint.ObjectNameInScene, joint);
+        }
+
+        // internal utility function: must be called within a lock (OdeLock)
+        private void InternalAddPendingJoint(OdePhysicsJoint joint)
+        {
+            pendingJoints.Add(joint);
+            SOPName_to_pendingJoint.Add(joint.ObjectNameInScene, joint);
+        }
+
+        // internal utility function: must be called within a lock (OdeLock)
+        private void InternalRemovePendingJoint(PhysicsJoint joint)
+        {
+            pendingJoints.Remove(joint);
+            SOPName_to_pendingJoint.Remove(joint.ObjectNameInScene);
+        }
+
+        // internal utility function: must be called within a lock (OdeLock)
+        private void InternalRemoveActiveJoint(PhysicsJoint joint)
+        {
+            activeJoints.Remove(joint);
+            SOPName_to_activeJoint.Remove(joint.ObjectNameInScene);
+        }
+
+        public override void DumpJointInfo()
+        {
+            string hdr = "[NINJA] JOINTINFO: ";
+            foreach (PhysicsJoint j in pendingJoints)
+            {
+                m_log.Debug(hdr + " pending joint, Name: " + j.ObjectNameInScene + " raw parms:" + j.RawParams);
+            }
+            m_log.Debug(hdr + pendingJoints.Count + " total pending joints");
+            foreach (string jointName in SOPName_to_pendingJoint.Keys)
+            {
+                m_log.Debug(hdr + " pending joints dict contains Name: " + jointName);
+            }
+            m_log.Debug(hdr + SOPName_to_pendingJoint.Keys.Count + " total pending joints dict entries");
+            foreach (PhysicsJoint j in activeJoints)
+            {
+                m_log.Debug(hdr + " active joint, Name: " + j.ObjectNameInScene + " raw parms:" + j.RawParams);
+            }
+            m_log.Debug(hdr + activeJoints.Count + " total active joints");
+            foreach (string jointName in SOPName_to_activeJoint.Keys)
+            {
+                m_log.Debug(hdr + " active joints dict contains Name: " + jointName);
+            }
+            m_log.Debug(hdr + SOPName_to_activeJoint.Keys.Count + " total active joints dict entries");
+
+            m_log.Debug(hdr + " Per-body joint connectivity information follows.");
+            m_log.Debug(hdr + joints_connecting_actor.Keys.Count + " bodies are connected by joints.");
+            foreach (string actorName in joints_connecting_actor.Keys)
+            {
+                m_log.Debug(hdr + " Actor " + actorName + " has the following joints connecting it");
+                foreach (PhysicsJoint j in joints_connecting_actor[actorName])
+                {
+                    m_log.Debug(hdr + " * joint Name: " + j.ObjectNameInScene + " raw parms:" + j.RawParams);
+                }
+                m_log.Debug(hdr + joints_connecting_actor[actorName].Count + " connecting joints total for this actor");
+            }
+        }
+
+        public override void RequestJointDeletion(string ObjectNameInScene)
+        {
+            lock (externalJointRequestsLock)
+            {
+                if (!requestedJointsToBeDeleted.Contains(ObjectNameInScene)) // forbid same deletion request from entering twice to prevent spurious deletions processed asynchronously
+                {
+                    requestedJointsToBeDeleted.Add(ObjectNameInScene);
+                }
+            }
+        }
+
+        private void DeleteRequestedJoints()
+        {
+            List<string> myRequestedJointsToBeDeleted;
+            lock (externalJointRequestsLock)
+            {
+                // make a local copy of the shared list for processing (threading issues)
+                myRequestedJointsToBeDeleted = new List<string>(requestedJointsToBeDeleted);
+            }
+
+            foreach (string jointName in myRequestedJointsToBeDeleted)
+            {
+                lock (OdeLock)
+                {
+                    //m_log.Debug("[NINJA] trying to deleting requested joint " + jointName);
+                    if (SOPName_to_activeJoint.ContainsKey(jointName) || SOPName_to_pendingJoint.ContainsKey(jointName))
+                    {
+                        OdePhysicsJoint joint = null;
+                        if (SOPName_to_activeJoint.ContainsKey(jointName))
+                        {
+                            joint = SOPName_to_activeJoint[jointName] as OdePhysicsJoint;
+                            InternalRemoveActiveJoint(joint);
+                        }
+                        else if (SOPName_to_pendingJoint.ContainsKey(jointName))
+                        {
+                            joint = SOPName_to_pendingJoint[jointName] as OdePhysicsJoint;
+                            InternalRemovePendingJoint(joint);
+                        }
+
+                        if (joint != null)
+                        {
+                            //m_log.Debug("joint.BodyNames.Count is " + joint.BodyNames.Count + " and contents " + joint.BodyNames);
+                            for (int iBodyName = 0; iBodyName < 2; iBodyName++)
+                            {
+                                string bodyName = joint.BodyNames[iBodyName];
+                                if (bodyName != "NULL")
+                                {
+                                    joints_connecting_actor[bodyName].Remove(joint);
+                                    if (joints_connecting_actor[bodyName].Count == 0)
+                                    {
+                                        joints_connecting_actor.Remove(bodyName);
+                                    }
+                                }
+                            }
+
+                            DoJointDeactivated(joint);
+                            if (joint.jointID != IntPtr.Zero)
+                            {
+                                d.JointDestroy(joint.jointID);
+                                joint.jointID = IntPtr.Zero;
+                                //DoJointErrorMessage(joint, "successfully destroyed joint " + jointName);
+                            }
+                            else
+                            {
+                                //m_log.Warn("[NINJA] Ignoring re-request to destroy joint " + jointName);
+                            }
+                        }
+                        else
+                        {
+                            // DoJointErrorMessage(joint, "coult not find joint to destroy based on name " + jointName);
+                        }
+                    }
+                    else
+                    {
+                        // DoJointErrorMessage(joint, "WARNING - joint removal failed, joint " + jointName);
+                    }
+                }
+            }
+
+            // remove processed joints from the shared list
+            lock (externalJointRequestsLock)
+            {
+                foreach (string jointName in myRequestedJointsToBeDeleted)
+                {
+                    requestedJointsToBeDeleted.Remove(jointName);
+                }
+            }
+        }
+
+        // for pending joints we don't know if their associated bodies exist yet or not.
+        // the joint is actually created during processing of the taints
+        private void CreateRequestedJoints()
+        {
+            List<PhysicsJoint> myRequestedJointsToBeCreated;
+            lock (externalJointRequestsLock)
+            {
+                // make a local copy of the shared list for processing (threading issues)
+                myRequestedJointsToBeCreated = new List<PhysicsJoint>(requestedJointsToBeCreated);
+            }
+
+            foreach (PhysicsJoint joint in myRequestedJointsToBeCreated)
+            {
+                lock (OdeLock)
+                {
+                    if (SOPName_to_pendingJoint.ContainsKey(joint.ObjectNameInScene) && SOPName_to_pendingJoint[joint.ObjectNameInScene] != null)
+                    {
+                        DoJointErrorMessage(joint, "WARNING: ignoring request to re-add already pending joint Name:" + joint.ObjectNameInScene + " type:" + joint.Type + " parms: " + joint.RawParams + " pos: " + joint.Position + " rot:" + joint.Rotation);
+                        continue;
+                    }
+                    if (SOPName_to_activeJoint.ContainsKey(joint.ObjectNameInScene) && SOPName_to_activeJoint[joint.ObjectNameInScene] != null)
+                    {
+                        DoJointErrorMessage(joint, "WARNING: ignoring request to re-add already active joint Name:" + joint.ObjectNameInScene + " type:" + joint.Type + " parms: " + joint.RawParams + " pos: " + joint.Position + " rot:" + joint.Rotation);
+                        continue;
+                    }
+
+                    InternalAddPendingJoint(joint as OdePhysicsJoint);
+
+                    if (joint.BodyNames.Count >= 2)
+                    {
+                        for (int iBodyName = 0; iBodyName < 2; iBodyName++)
+                        {
+                            string bodyName = joint.BodyNames[iBodyName];
+                            if (bodyName != "NULL")
+                            {
+                                if (!joints_connecting_actor.ContainsKey(bodyName))
+                                {
+                                    joints_connecting_actor.Add(bodyName, new List<PhysicsJoint>());
+                                }
+                                joints_connecting_actor[bodyName].Add(joint);
+                            }
+                        }
+                    }
+                }
+            }
+
+            // remove processed joints from shared list
+            lock (externalJointRequestsLock)
+            {
+                foreach (PhysicsJoint joint in myRequestedJointsToBeCreated)
+                {
+                    requestedJointsToBeCreated.Remove(joint);
+                }
+            }
+
+        }
+
+        // public function to add an request for joint creation
+        // this joint will just be added to a waiting list that is NOT processed during the main
+        // Simulate() loop (to avoid deadlocks). After Simulate() is finished, we handle unprocessed joint requests.
+
+        public override PhysicsJoint RequestJointCreation(string objectNameInScene, PhysicsJointType jointType, Vector3 position,
+                                            Quaternion rotation, string parms, List<string> bodyNames, string trackedBodyName, Quaternion localRotation)
+
+        {
+
+            OdePhysicsJoint joint = new OdePhysicsJoint();
+            joint.ObjectNameInScene = objectNameInScene;
+            joint.Type = jointType;
+            joint.Position = position;
+            joint.Rotation = rotation;
+            joint.RawParams = parms;
+            joint.BodyNames = new List<string>(bodyNames);
+            joint.TrackedBodyName = trackedBodyName; 
+            joint.LocalRotation = localRotation;
+            joint.jointID = IntPtr.Zero;
+            joint.ErrorMessageCount = 0;
+
+            lock (externalJointRequestsLock)
+            {
+                if (!requestedJointsToBeCreated.Contains(joint)) // forbid same creation request from entering twice 
+                {
+                    requestedJointsToBeCreated.Add(joint);
+                }
+            }
+            return joint;
+        }
+
+        private void RemoveAllJointsConnectedToActor(PhysicsActor actor)
+        {
+            //m_log.Debug("RemoveAllJointsConnectedToActor: start");
+            if (actor.SOPName != null && joints_connecting_actor.ContainsKey(actor.SOPName) && joints_connecting_actor[actor.SOPName] != null)
+            {
+
+                List<PhysicsJoint> jointsToRemove = new List<PhysicsJoint>();
+                //TODO: merge these 2 loops (originally it was needed to avoid altering a list being iterated over, but it is no longer needed due to the joint request queue mechanism)
+                foreach (PhysicsJoint j in joints_connecting_actor[actor.SOPName])
+                {
+                    jointsToRemove.Add(j);
+                }
+                foreach (PhysicsJoint j in jointsToRemove)
+                {
+                    //m_log.Debug("RemoveAllJointsConnectedToActor: about to request deletion of " + j.ObjectNameInScene);
+                    RequestJointDeletion(j.ObjectNameInScene);
+                    //m_log.Debug("RemoveAllJointsConnectedToActor: done request deletion of " + j.ObjectNameInScene);
+                    j.TrackedBodyName = null; // *IMMEDIATELY* prevent any further movement of this joint (else a deleted actor might cause spurious tracking motion of the joint for a few frames, leading to the joint proxy object disappearing)
+                }
+            }
+        }
+
+        public override void RemoveAllJointsConnectedToActorThreadLocked(PhysicsActor actor)
+        {
+            //m_log.Debug("RemoveAllJointsConnectedToActorThreadLocked: start");
+            lock (OdeLock)
+            {
+                //m_log.Debug("RemoveAllJointsConnectedToActorThreadLocked: got lock");
+                RemoveAllJointsConnectedToActor(actor);
+            }
+        }
+
+        // normally called from within OnJointMoved, which is called from within a lock (OdeLock)
+        public override Vector3 GetJointAnchor(PhysicsJoint joint)
+        {
+            Debug.Assert(joint.IsInPhysicsEngine);
+            d.Vector3 pos = new d.Vector3();
+
+            if (!(joint is OdePhysicsJoint))
+            {
+                DoJointErrorMessage(joint, "warning: non-ODE joint requesting anchor: " + joint.ObjectNameInScene);
+            }
+            else
+            {
+                OdePhysicsJoint odeJoint = (OdePhysicsJoint)joint;
+                switch (odeJoint.Type)
+                {
+                    case PhysicsJointType.Ball:
+                        d.JointGetBallAnchor(odeJoint.jointID, out pos);
+                        break;
+                    case PhysicsJointType.Hinge:
+                        d.JointGetHingeAnchor(odeJoint.jointID, out pos);
+                        break;
+                }
+            }
+            return new Vector3(pos.X, pos.Y, pos.Z);
+        }
+
+        // normally called from within OnJointMoved, which is called from within a lock (OdeLock)
+        // WARNING: ODE sometimes returns <0,0,0> as the joint axis! Therefore this function
+        // appears to be unreliable. Fortunately we can compute the joint axis ourselves by
+        // keeping track of the joint's original orientation relative to one of the involved bodies.
+        public override Vector3 GetJointAxis(PhysicsJoint joint)
+        {
+            Debug.Assert(joint.IsInPhysicsEngine);
+            d.Vector3 axis = new d.Vector3();
+
+            if (!(joint is OdePhysicsJoint))
+            {
+                DoJointErrorMessage(joint, "warning: non-ODE joint requesting anchor: " + joint.ObjectNameInScene);
+            }
+            else
+            {
+                OdePhysicsJoint odeJoint = (OdePhysicsJoint)joint;
+                switch (odeJoint.Type)
+                {
+                    case PhysicsJointType.Ball:
+                        DoJointErrorMessage(joint, "warning - axis requested for ball joint: " + joint.ObjectNameInScene);
+                        break;
+                    case PhysicsJointType.Hinge:
+                        d.JointGetHingeAxis(odeJoint.jointID, out axis);
+                        break;
+                }
+            }
+            return new Vector3(axis.X, axis.Y, axis.Z);
+        }
+
+
+        public void remActivePrim(OdePrim deactivatePrim)
+        {
+            lock (_activeprims)
+            {
+                _activeprims.Remove(deactivatePrim);
+            }
+        }
+
+        public override void RemovePrim(PhysicsActor prim)
+        {
+            if (prim is OdePrim)
+            {
+                lock (OdeLock)
+                {
+                    OdePrim p = (OdePrim) prim;
+
+                    p.setPrimForRemoval();
+                    AddPhysicsActorTaint(prim);
+                    //RemovePrimThreadLocked(p);
+                }
+            }
+        }
+
+        /// <summary>
+        /// This is called from within simulate but outside the locked portion
+        /// We need to do our own locking here
+        /// Essentially, we need to remove the prim from our space segment, whatever segment it's in.
+        ///
+        /// If there are no more prim in the segment, we need to empty (spacedestroy)the segment and reclaim memory
+        /// that the space was using.
+        /// </summary>
+        /// <param name="prim"></param>
+        public void RemovePrimThreadLocked(OdePrim prim)
+        {
+//Console.WriteLine("RemovePrimThreadLocked " +  prim.m_primName);            
+            lock (prim)
+            {
+                remCollisionEventReporting(prim);
+                lock (ode)
+                {
+                    if (prim.prim_geom != IntPtr.Zero)
+                    {
+                        prim.ResetTaints();
+
+                        if (prim.IsPhysical)
+                        {
+                            prim.disableBody();
+                            if (prim.childPrim)
+                            {
+                                prim.childPrim = false;
+                                prim.Body = IntPtr.Zero;
+                                prim.m_disabled = true;
+                                prim.IsPhysical = false;
+                            }
+
+
+                        }
+                        // we don't want to remove the main space
+
+                        // If the geometry is in the targetspace, remove it from the target space
+                        //m_log.Warn(prim.m_targetSpace);
+
+                        //if (prim.m_targetSpace != IntPtr.Zero)
+                        //{
+                        //if (d.SpaceQuery(prim.m_targetSpace, prim.prim_geom))
+                        //{
+
+                        //if (d.GeomIsSpace(prim.m_targetSpace))
+                        //{
+                        //waitForSpaceUnlock(prim.m_targetSpace);
+                        //d.SpaceRemove(prim.m_targetSpace, prim.prim_geom);
+                        prim.m_targetSpace = IntPtr.Zero;
+                        //}
+                        //else
+                        //{
+                        // m_log.Info("[Physics]: Invalid Scene passed to 'removeprim from scene':" +
+                        //((OdePrim)prim).m_targetSpace.ToString());
+                        //}
+
+                        //}
+                        //}
+                        //m_log.Warn(prim.prim_geom);
+                        try
+                        {
+                            if (prim.prim_geom != IntPtr.Zero)
+                            {
+                                d.GeomDestroy(prim.prim_geom);
+                                prim.prim_geom = IntPtr.Zero;
+                            }
+                            else
+                            {
+                                m_log.Warn("[PHYSICS]: Unable to remove prim from physics scene");
+                            }
+                        }
+                        catch (AccessViolationException)
+                        {
+                            m_log.Info("[PHYSICS]: Couldn't remove prim from physics scene, it was already be removed.");
+                        }
+                        lock (_prims)
+                        _prims.Remove(prim);
+
+                        //If there are no more geometries in the sub-space, we don't need it in the main space anymore
+                        //if (d.SpaceGetNumGeoms(prim.m_targetSpace) == 0)
+                        //{
+                        //if (prim.m_targetSpace != null)
+                        //{
+                        //if (d.GeomIsSpace(prim.m_targetSpace))
+                        //{
+                        //waitForSpaceUnlock(prim.m_targetSpace);
+                        //d.SpaceRemove(space, prim.m_targetSpace);
+                        // free up memory used by the space.
+                        //d.SpaceDestroy(prim.m_targetSpace);
+                        //int[] xyspace = calculateSpaceArrayItemFromPos(prim.Position);
+                        //resetSpaceArrayItemToZero(xyspace[0], xyspace[1]);
+                        //}
+                        //else
+                        //{
+                        //m_log.Info("[Physics]: Invalid Scene passed to 'removeprim from scene':" +
+                        //((OdePrim) prim).m_targetSpace.ToString());
+                        //}
+                        //}
+                        //}
+
+                        if (SupportsNINJAJoints)
+                        {
+                            RemoveAllJointsConnectedToActorThreadLocked(prim);
+                        }
+                    }
+                }
+            }
+        }
+
+        #endregion
+
+        #region Space Separation Calculation
+
+        /// <summary>
+        /// Takes a space pointer and zeros out the array we're using to hold the spaces
+        /// </summary>
+        /// <param name="pSpace"></param>
+        public void resetSpaceArrayItemToZero(IntPtr pSpace)
+        {
+            for (int x = 0; x < staticPrimspace.GetLength(0); x++)
+            {
+                for (int y = 0; y < staticPrimspace.GetLength(1); y++)
+                {
+                    if (staticPrimspace[x, y] == pSpace)
+                        staticPrimspace[x, y] = IntPtr.Zero;
+                }
+            }
+        }
+
+        public void resetSpaceArrayItemToZero(int arrayitemX, int arrayitemY)
+        {
+            staticPrimspace[arrayitemX, arrayitemY] = IntPtr.Zero;
+        }
+
+        /// <summary>
+        /// Called when a static prim moves.  Allocates a space for the prim based on its position
+        /// </summary>
+        /// <param name="geom">the pointer to the geom that moved</param>
+        /// <param name="pos">the position that the geom moved to</param>
+        /// <param name="currentspace">a pointer to the space it was in before it was moved.</param>
+        /// <returns>a pointer to the new space it's in</returns>
+        public IntPtr recalculateSpaceForGeom(IntPtr geom, Vector3 pos, IntPtr currentspace)
+        {
+            // Called from setting the Position and Size of an ODEPrim so
+            // it's already in locked space.
+
+            // we don't want to remove the main space
+            // we don't need to test physical here because this function should
+            // never be called if the prim is physical(active)
+
+            // All physical prim end up in the root space
+            //Thread.Sleep(20);
+            if (currentspace != space)
+            {
+                //m_log.Info("[SPACE]: C:" + currentspace.ToString() + " g:" + geom.ToString());
+                //if (currentspace == IntPtr.Zero)
+               //{
+                    //int adfadf = 0;
+                //}
+                if (d.SpaceQuery(currentspace, geom) && currentspace != IntPtr.Zero)
+                {
+                    if (d.GeomIsSpace(currentspace))
+                    {
+                        waitForSpaceUnlock(currentspace);
+                        d.SpaceRemove(currentspace, geom);
+                    }
+                    else
+                    {
+                        m_log.Info("[Physics]: Invalid Scene passed to 'recalculatespace':" + currentspace +
+                                   " Geom:" + geom);
+                    }
+                }
+                else
+                {
+                    IntPtr sGeomIsIn = d.GeomGetSpace(geom);
+                    if (sGeomIsIn != IntPtr.Zero)
+                    {
+                        if (d.GeomIsSpace(currentspace))
+                        {
+                            waitForSpaceUnlock(sGeomIsIn);
+                            d.SpaceRemove(sGeomIsIn, geom);
+                        }
+                        else
+                        {
+                            m_log.Info("[Physics]: Invalid Scene passed to 'recalculatespace':" +
+                                       sGeomIsIn + " Geom:" + geom);
+                        }
+                    }
+                }
+
+                //If there are no more geometries in the sub-space, we don't need it in the main space anymore
+                if (d.SpaceGetNumGeoms(currentspace) == 0)
+                {
+                    if (currentspace != IntPtr.Zero)
+                    {
+                        if (d.GeomIsSpace(currentspace))
+                        {
+                            waitForSpaceUnlock(currentspace);
+                            waitForSpaceUnlock(space);
+                            d.SpaceRemove(space, currentspace);
+                            // free up memory used by the space.
+
+                            //d.SpaceDestroy(currentspace);
+                            resetSpaceArrayItemToZero(currentspace);
+                        }
+                        else
+                        {
+                            m_log.Info("[Physics]: Invalid Scene passed to 'recalculatespace':" +
+                                       currentspace + " Geom:" + geom);
+                        }
+                    }
+                }
+            }
+            else
+            {
+                // this is a physical object that got disabled. ;.;
+                if (currentspace != IntPtr.Zero && geom != IntPtr.Zero)
+                {
+                    if (d.SpaceQuery(currentspace, geom))
+                    {
+                        if (d.GeomIsSpace(currentspace))
+                        {
+                            waitForSpaceUnlock(currentspace);
+                            d.SpaceRemove(currentspace, geom);
+                        }
+                        else
+                        {
+                            m_log.Info("[Physics]: Invalid Scene passed to 'recalculatespace':" +
+                                       currentspace + " Geom:" + geom);
+                        }
+                    }
+                    else
+                    {
+                        IntPtr sGeomIsIn = d.GeomGetSpace(geom);
+                        if (sGeomIsIn != IntPtr.Zero)
+                        {
+                            if (d.GeomIsSpace(sGeomIsIn))
+                            {
+                                waitForSpaceUnlock(sGeomIsIn);
+                                d.SpaceRemove(sGeomIsIn, geom);
+                            }
+                            else
+                            {
+                                m_log.Info("[Physics]: Invalid Scene passed to 'recalculatespace':" +
+                                           sGeomIsIn + " Geom:" + geom);
+                            }
+                        }
+                    }
+                }
+            }
+
+            // The routines in the Position and Size sections do the 'inserting' into the space,
+            // so all we have to do is make sure that the space that we're putting the prim into
+            // is in the 'main' space.
+            int[] iprimspaceArrItem = calculateSpaceArrayItemFromPos(pos);
+            IntPtr newspace = calculateSpaceForGeom(pos);
+
+            if (newspace == IntPtr.Zero)
+            {
+                newspace = createprimspace(iprimspaceArrItem[0], iprimspaceArrItem[1]);
+                d.HashSpaceSetLevels(newspace, smallHashspaceLow, smallHashspaceHigh);
+            }
+
+            return newspace;
+        }
+
+        /// <summary>
+        /// Creates a new space at X Y
+        /// </summary>
+        /// <param name="iprimspaceArrItemX"></param>
+        /// <param name="iprimspaceArrItemY"></param>
+        /// <returns>A pointer to the created space</returns>
+        public IntPtr createprimspace(int iprimspaceArrItemX, int iprimspaceArrItemY)
+        {
+            // creating a new space for prim and inserting it into main space.
+            staticPrimspace[iprimspaceArrItemX, iprimspaceArrItemY] = d.HashSpaceCreate(IntPtr.Zero);
+            d.GeomSetCategoryBits(staticPrimspace[iprimspaceArrItemX, iprimspaceArrItemY], (int)CollisionCategories.Space);
+            waitForSpaceUnlock(space);
+            d.SpaceSetSublevel(space, 1);
+            d.SpaceAdd(space, staticPrimspace[iprimspaceArrItemX, iprimspaceArrItemY]);
+            return staticPrimspace[iprimspaceArrItemX, iprimspaceArrItemY];
+        }
+
+        /// <summary>
+        /// Calculates the space the prim should be in by its position
+        /// </summary>
+        /// <param name="pos"></param>
+        /// <returns>a pointer to the space. This could be a new space or reused space.</returns>
+        public IntPtr calculateSpaceForGeom(Vector3 pos)
+        {
+            int[] xyspace = calculateSpaceArrayItemFromPos(pos);
+            //m_log.Info("[Physics]: Attempting to use arrayItem: " + xyspace[0].ToString() + "," + xyspace[1].ToString());
+            return staticPrimspace[xyspace[0], xyspace[1]];
+        }
+
+        /// <summary>
+        /// Holds the space allocation logic
+        /// </summary>
+        /// <param name="pos"></param>
+        /// <returns>an array item based on the position</returns>
+        public int[] calculateSpaceArrayItemFromPos(Vector3 pos)
+        {
+            int[] returnint = new int[2];
+
+            returnint[0] = (int) (pos.X/metersInSpace);
+
+            if (returnint[0] > ((int) (259f/metersInSpace)))
+                returnint[0] = ((int) (259f/metersInSpace));
+            if (returnint[0] < 0)
+                returnint[0] = 0;
+
+            returnint[1] = (int) (pos.Y/metersInSpace);
+            if (returnint[1] > ((int) (259f/metersInSpace)))
+                returnint[1] = ((int) (259f/metersInSpace));
+            if (returnint[1] < 0)
+                returnint[1] = 0;
+
+            return returnint;
+        }
+
+        #endregion
+
+        /// <summary>
+        /// Routine to figure out if we need to mesh this prim with our mesher
+        /// </summary>
+        /// <param name="pbs"></param>
+        /// <returns></returns>
+        public bool needsMeshing(PrimitiveBaseShape pbs)
+        {
+            // most of this is redundant now as the mesher will return null if it cant mesh a prim
+            // but we still need to check for sculptie meshing being enabled so this is the most
+            // convenient place to do it for now...
+
+        //    //if (pbs.PathCurve == (byte)Primitive.PathCurve.Circle && pbs.ProfileCurve == (byte)Primitive.ProfileCurve.Circle && pbs.PathScaleY <= 0.75f)
+        //    //m_log.Debug("needsMeshing: " + " pathCurve: " + pbs.PathCurve.ToString() + " profileCurve: " + pbs.ProfileCurve.ToString() + " pathScaleY: " + Primitive.UnpackPathScale(pbs.PathScaleY).ToString());
+            int iPropertiesNotSupportedDefault = 0;
+
+            if (pbs.SculptEntry && !meshSculptedPrim)
+            {
+#if SPAM
+                m_log.Warn("NonMesh");
+#endif
+                return false;
+            }
+
+            // if it's a standard box or sphere with no cuts, hollows, twist or top shear, return false since ODE can use an internal representation for the prim
+            if (!forceSimplePrimMeshing)
+            {
+                if ((pbs.ProfileShape == ProfileShape.Square && pbs.PathCurve == (byte)Extrusion.Straight)
+                    || (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1
+                    && pbs.Scale.X == pbs.Scale.Y && pbs.Scale.Y == pbs.Scale.Z))
+                {
+
+                    if (pbs.ProfileBegin == 0 && pbs.ProfileEnd == 0
+                        && pbs.ProfileHollow == 0
+                        && pbs.PathTwist == 0 && pbs.PathTwistBegin == 0
+                        && pbs.PathBegin == 0 && pbs.PathEnd == 0
+                        && pbs.PathTaperX == 0 && pbs.PathTaperY == 0
+                        && pbs.PathScaleX == 100 && pbs.PathScaleY == 100
+                        && pbs.PathShearX == 0 && pbs.PathShearY == 0)
+                    {
+#if SPAM
+                    m_log.Warn("NonMesh");
+#endif
+                        return false;
+                    }
+                }
+            }
+
+            if (pbs.ProfileHollow != 0)
+                iPropertiesNotSupportedDefault++;
+
+            if ((pbs.PathTwistBegin != 0) || (pbs.PathTwist != 0))
+                iPropertiesNotSupportedDefault++; 
+
+            if ((pbs.ProfileBegin != 0) || pbs.ProfileEnd != 0)
+                iPropertiesNotSupportedDefault++;
+
+            if ((pbs.PathScaleX != 100) || (pbs.PathScaleY != 100))
+                iPropertiesNotSupportedDefault++;
+
+            if ((pbs.PathShearX != 0) || (pbs.PathShearY != 0))
+                iPropertiesNotSupportedDefault++;
+
+            if (pbs.ProfileShape == ProfileShape.Circle && pbs.PathCurve == (byte)Extrusion.Straight)
+                iPropertiesNotSupportedDefault++;
+
+            if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1 && (pbs.Scale.X != pbs.Scale.Y || pbs.Scale.Y != pbs.Scale.Z || pbs.Scale.Z != pbs.Scale.X))
+                iPropertiesNotSupportedDefault++;
+
+            if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte) Extrusion.Curve1)
+                iPropertiesNotSupportedDefault++;
+
+            // test for torus
+            if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Square)
+            {
+                if (pbs.PathCurve == (byte)Extrusion.Curve1)
+                {
+                    iPropertiesNotSupportedDefault++;
+                }
+            }
+            else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Circle)
+            {
+                if (pbs.PathCurve == (byte)Extrusion.Straight)
+                {
+                    iPropertiesNotSupportedDefault++;
+                }
+
+                // ProfileCurve seems to combine hole shape and profile curve so we need to only compare against the lower 3 bits
+                else if (pbs.PathCurve == (byte)Extrusion.Curve1)
+                {
+                    iPropertiesNotSupportedDefault++;
+                }
+            }
+            else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.HalfCircle)
+            {
+                if (pbs.PathCurve == (byte)Extrusion.Curve1 || pbs.PathCurve == (byte)Extrusion.Curve2)
+                {
+                    iPropertiesNotSupportedDefault++;
+                }
+            }
+            else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.EquilateralTriangle)
+            {
+                if (pbs.PathCurve == (byte)Extrusion.Straight)
+                {
+                    iPropertiesNotSupportedDefault++;
+                }
+                else if (pbs.PathCurve == (byte)Extrusion.Curve1)
+                {
+                    iPropertiesNotSupportedDefault++;
+                }
+            }
+
+
+            if (iPropertiesNotSupportedDefault == 0)
+            {
+#if SPAM
+                m_log.Warn("NonMesh");
+#endif
+                return false;
+            }
+#if SPAM
+            m_log.Debug("Mesh");
+#endif
+            return true; 
+        }
+
+        /// <summary>
+        /// Called after our prim properties are set Scale, position etc.
+        /// We use this event queue like method to keep changes to the physical scene occuring in the threadlocked mutex
+        /// This assures us that we have no race conditions
+        /// </summary>
+        /// <param name="prim"></param>
+        public override void AddPhysicsActorTaint(PhysicsActor prim)
+        {
+            
+            if (prim is OdePrim)
+            {
+                OdePrim taintedprim = ((OdePrim) prim);
+                lock (_taintedPrimLock)
+                {
+                    if (!(_taintedPrimH.Contains(taintedprim))) 
+                                {
+//Console.WriteLine("AddPhysicsActorTaint to " +  taintedprim.m_primName);                              
+                        _taintedPrimH.Add(taintedprim);                                 // HashSet for searching
+                        _taintedPrimL.Add(taintedprim);                                 // List for ordered readout
+                                        }
+                                }
+                return;
+            }
+            else if (prim is OdeCharacter)
+            {
+                OdeCharacter taintedchar = ((OdeCharacter)prim);
+                lock (_taintedActors)
+                {
+                    if (!(_taintedActors.Contains(taintedchar)))
+                    {
+                        _taintedActors.Add(taintedchar);
+                        if (taintedchar.bad)
+                            m_log.DebugFormat("[PHYSICS]: Added BAD actor {0} to tainted actors", taintedchar.m_uuid);
+                    }
+                }
+            }
+        }
+
+        /// <summary>
+        /// This is our main simulate loop
+        /// It's thread locked by a Mutex in the scene.
+        /// It holds Collisions, it instructs ODE to step through the physical reactions
+        /// It moves the objects around in memory
+        /// It calls the methods that report back to the object owners.. (scenepresence, SceneObjectGroup)
+        /// </summary>
+        /// <param name="timeStep"></param>
+        /// <returns></returns>
+        public override float Simulate(float timeStep)
+        {
+            if (framecount >= int.MaxValue)
+                framecount = 0;
+
+            //if (m_worldOffset != Vector3.Zero)
+            //    return 0;
+
+            framecount++;
+
+            float fps = 0;
+            //m_log.Info(timeStep.ToString());
+            step_time += timeStep;
+            
+            // If We're loaded down by something else,
+            // or debugging with the Visual Studio project on pause
+            // skip a few frames to catch up gracefully.
+            // without shooting the physicsactors all over the place
+
+            if (step_time >= m_SkipFramesAtms)
+            {
+                // Instead of trying to catch up, it'll do 5 physics frames only
+                step_time = ODE_STEPSIZE;
+                m_physicsiterations = 5;
+            }
+            else
+            {
+                m_physicsiterations = 10;
+            }
+
+            if (SupportsNINJAJoints)
+            {
+                DeleteRequestedJoints(); // this must be outside of the lock (OdeLock) to avoid deadlocks
+                CreateRequestedJoints(); // this must be outside of the lock (OdeLock) to avoid deadlocks
+            }
+
+            lock (OdeLock)
+            {
+                // Process 10 frames if the sim is running normal..
+                // process 5 frames if the sim is running slow
+                //try
+                //{
+                    //d.WorldSetQuickStepNumIterations(world, m_physicsiterations);
+                //}
+                //catch (StackOverflowException)
+                //{
+                   // m_log.Error("[PHYSICS]: The operating system wasn't able to allocate enough memory for the simulation.  Restarting the sim.");
+                   // ode.drelease(world);
+                    //base.TriggerPhysicsBasedRestart();
+                //}
+
+                int i = 0;
+
+                // Figure out the Frames Per Second we're going at.
+                //(step_time == 0.004f, there's 250 of those per second.   Times the step time/step size
+
+                fps = (step_time / ODE_STEPSIZE) * 1000;
+                // HACK: Using a time dilation of 1.0 to debug rubberbanding issues
+                //m_timeDilation = Math.Min((step_time / ODE_STEPSIZE) / (0.09375f / ODE_STEPSIZE), 1.0f);
+
+                step_time = 0.09375f;
+
+                while (step_time > 0.0f)
+                {
+                    //lock (ode)
+                    //{
+                        //if (!ode.lockquery())
+                        //{
+                           // ode.dlock(world);
+                            try
+                            { 
+                                // Insert, remove Characters
+                                bool processedtaints = false;
+
+                                lock (_taintedActors)
+                                {
+                                    if (_taintedActors.Count > 0)
+                                    {
+                                        foreach (OdeCharacter character in _taintedActors)
+                                        {
+
+                                            character.ProcessTaints(timeStep);
+
+                                            processedtaints = true;
+                                            //character.m_collisionscore = 0;
+                                        }
+
+                                        if (processedtaints)
+                                            _taintedActors.Clear();
+                                    }
+                                }
+
+                                // Modify other objects in the scene.
+                                processedtaints = false;
+
+                                lock (_taintedPrimLock)
+                                {
+                                    foreach (OdePrim prim in _taintedPrimL)
+                                    {
+                                        if (prim.m_taintremove)
+                                        {
+                                            //Console.WriteLine("Simulate calls RemovePrimThreadLocked");                                          
+                                            RemovePrimThreadLocked(prim);
+                                        }
+                                        else
+                                        {
+                                            //Console.WriteLine("Simulate calls ProcessTaints");                                        
+                                            prim.ProcessTaints(timeStep);
+                                        }
+                                        processedtaints = true;
+                                        prim.m_collisionscore = 0;
+
+                                        // This loop can block up the Heartbeat for a very long time on large regions.
+                                        // We need to let the Watchdog know that the Heartbeat is not dead
+                                        // NOTE: This is currently commented out, but if things like OAR loading are 
+                                        // timing the heartbeat out we will need to uncomment it
+                                        //Watchdog.UpdateThread();
+                                    }
+
+                                    if (SupportsNINJAJoints)
+                                    {
+                                        // Create pending joints, if possible
+
+                                        // joints can only be processed after ALL bodies are processed (and exist in ODE), since creating
+                                        // a joint requires specifying the body id of both involved bodies
+                                        if (pendingJoints.Count > 0)
+                                        {
+                                            List<PhysicsJoint> successfullyProcessedPendingJoints = new List<PhysicsJoint>();
+                                            //DoJointErrorMessage(joints_connecting_actor, "taint: " + pendingJoints.Count + " pending joints");
+                                            foreach (PhysicsJoint joint in pendingJoints)
+                                            {
+                                                //DoJointErrorMessage(joint, "taint: time to create joint with parms: " + joint.RawParams);
+                                                string[] jointParams = joint.RawParams.Split(" ".ToCharArray(), System.StringSplitOptions.RemoveEmptyEntries);
+                                                List<IntPtr> jointBodies = new List<IntPtr>();
+                                                bool allJointBodiesAreReady = true;
+                                                foreach (string jointParam in jointParams)
+                                                {
+                                                    if (jointParam == "NULL")
+                                                    {
+                                                        //DoJointErrorMessage(joint, "attaching NULL joint to world");
+                                                        jointBodies.Add(IntPtr.Zero);
+                                                    }
+                                                    else
+                                                    {
+                                                        //DoJointErrorMessage(joint, "looking for prim name: " + jointParam);
+                                                        bool foundPrim = false;
+                                                        lock (_prims)
+                                                        {
+                                                            foreach (OdePrim prim in _prims) // FIXME: inefficient
+                                                            {
+                                                                if (prim.SOPName == jointParam)
+                                                                {
+                                                                    //DoJointErrorMessage(joint, "found for prim name: " + jointParam);
+                                                                    if (prim.IsPhysical && prim.Body != IntPtr.Zero)
+                                                                    {
+                                                                        jointBodies.Add(prim.Body);
+                                                                        foundPrim = true;
+                                                                        break;
+                                                                    }
+                                                                    else
+                                                                    {
+                                                                        DoJointErrorMessage(joint, "prim name " + jointParam +
+                                                                            " exists but is not (yet) physical; deferring joint creation. " +
+                                                                            "IsPhysical property is " + prim.IsPhysical +
+                                                                            " and body is " + prim.Body);
+                                                                        foundPrim = false;
+                                                                        break;
+                                                                    }
+                                                                }
+                                                            }
+                                                        }
+                                                        if (foundPrim)
+                                                        {
+                                                            // all is fine
+                                                        }
+                                                        else
+                                                        {
+                                                            allJointBodiesAreReady = false;
+                                                            break;
+                                                        }
+                                                    }
+                                                }
+                                                if (allJointBodiesAreReady)
+                                                {
+                                                    //DoJointErrorMessage(joint, "allJointBodiesAreReady for " + joint.ObjectNameInScene + " with parms " + joint.RawParams);
+                                                    if (jointBodies[0] == jointBodies[1])
+                                                    {
+                                                        DoJointErrorMessage(joint, "ERROR: joint cannot be created; the joint bodies are the same, body1==body2. Raw body is " + jointBodies[0] + ". raw parms: " + joint.RawParams);
+                                                    }
+                                                    else
+                                                    {
+                                                        switch (joint.Type)
+                                                        {
+                                                            case PhysicsJointType.Ball:
+                                                                {
+                                                                    IntPtr odeJoint;
+                                                                    //DoJointErrorMessage(joint, "ODE creating ball joint ");
+                                                                    odeJoint = d.JointCreateBall(world, IntPtr.Zero);
+                                                                    //DoJointErrorMessage(joint, "ODE attaching ball joint: " + odeJoint + " with b1:" + jointBodies[0] + " b2:" + jointBodies[1]);
+                                                                    d.JointAttach(odeJoint, jointBodies[0], jointBodies[1]);
+                                                                    //DoJointErrorMessage(joint, "ODE setting ball anchor: " + odeJoint + " to vec:" + joint.Position);
+                                                                    d.JointSetBallAnchor(odeJoint,
+                                                                                        joint.Position.X,
+                                                                                        joint.Position.Y,
+                                                                                        joint.Position.Z);
+                                                                    //DoJointErrorMessage(joint, "ODE joint setting OK");
+                                                                    //DoJointErrorMessage(joint, "The ball joint's bodies are here: b0: ");
+                                                                    //DoJointErrorMessage(joint, "" + (jointBodies[0] != IntPtr.Zero ? "" + d.BodyGetPosition(jointBodies[0]) : "fixed environment"));
+                                                                    //DoJointErrorMessage(joint, "The ball joint's bodies are here: b1: ");
+                                                                    //DoJointErrorMessage(joint, "" + (jointBodies[1] != IntPtr.Zero ? "" + d.BodyGetPosition(jointBodies[1]) : "fixed environment"));
+
+                                                                    if (joint is OdePhysicsJoint)
+                                                                    {
+                                                                        ((OdePhysicsJoint)joint).jointID = odeJoint;
+                                                                    }
+                                                                    else
+                                                                    {
+                                                                        DoJointErrorMessage(joint, "WARNING: non-ode joint in ODE!");
+                                                                    }
+                                                                }
+                                                                break;
+                                                            case PhysicsJointType.Hinge:
+                                                                {
+                                                                    IntPtr odeJoint;
+                                                                    //DoJointErrorMessage(joint, "ODE creating hinge joint ");
+                                                                    odeJoint = d.JointCreateHinge(world, IntPtr.Zero);
+                                                                    //DoJointErrorMessage(joint, "ODE attaching hinge joint: " + odeJoint + " with b1:" + jointBodies[0] + " b2:" + jointBodies[1]);
+                                                                    d.JointAttach(odeJoint, jointBodies[0], jointBodies[1]);
+                                                                    //DoJointErrorMessage(joint, "ODE setting hinge anchor: " + odeJoint + " to vec:" + joint.Position);
+                                                                    d.JointSetHingeAnchor(odeJoint,
+                                                                                          joint.Position.X,
+                                                                                          joint.Position.Y,
+                                                                                          joint.Position.Z);
+                                                                    // We use the orientation of the x-axis of the joint's coordinate frame
+                                                                    // as the axis for the hinge. 
+
+                                                                    // Therefore, we must get the joint's coordinate frame based on the
+                                                                    // joint.Rotation field, which originates from the orientation of the 
+                                                                    // joint's proxy object in the scene.
+
+                                                                    // The joint's coordinate frame is defined as the transformation matrix
+                                                                    // that converts a vector from joint-local coordinates into world coordinates.
+                                                                    // World coordinates are defined as the XYZ coordinate system of the sim,
+                                                                    // as shown in the top status-bar of the viewer.
+
+                                                                    // Once we have the joint's coordinate frame, we extract its X axis (AtAxis)
+                                                                    // and use that as the hinge axis.
+
+                                                                    //joint.Rotation.Normalize();
+                                                                    Matrix4 proxyFrame = Matrix4.CreateFromQuaternion(joint.Rotation);
+
+                                                                    // Now extract the X axis of the joint's coordinate frame.
+
+                                                                    // Do not try to use proxyFrame.AtAxis or you will become mired in the
+                                                                    // tar pit of transposed, inverted, and generally messed-up orientations.
+                                                                    // (In other words, Matrix4.AtAxis() is borked.)
+                                                                    // Vector3 jointAxis = proxyFrame.AtAxis; <--- this path leadeth to madness
+
+                                                                    // Instead, compute the X axis of the coordinate frame by transforming
+                                                                    // the (1,0,0) vector. At least that works.
+
+                                                                    //m_log.Debug("PHY: making axis: complete matrix is " + proxyFrame);
+                                                                    Vector3 jointAxis = Vector3.Transform(Vector3.UnitX, proxyFrame);
+                                                                    //m_log.Debug("PHY: making axis: hinge joint axis is " + jointAxis); 
+                                                                    //DoJointErrorMessage(joint, "ODE setting hinge axis: " + odeJoint + " to vec:" + jointAxis);
+                                                                    d.JointSetHingeAxis(odeJoint,
+                                                                                        jointAxis.X,
+                                                                                        jointAxis.Y,
+                                                                                        jointAxis.Z);
+                                                                    //d.JointSetHingeParam(odeJoint, (int)dParam.CFM, 0.1f);
+                                                                    if (joint is OdePhysicsJoint)
+                                                                    {
+                                                                        ((OdePhysicsJoint)joint).jointID = odeJoint;
+                                                                    }
+                                                                    else
+                                                                    {
+                                                                        DoJointErrorMessage(joint, "WARNING: non-ode joint in ODE!");
+                                                                    }
+                                                                }
+                                                                break;
+                                                        }
+                                                        successfullyProcessedPendingJoints.Add(joint);
+                                                    }
+                                                }
+                                                else
+                                                {
+                                                    DoJointErrorMessage(joint, "joint could not yet be created; still pending");
+                                                }
+                                            }
+                                            foreach (PhysicsJoint successfullyProcessedJoint in successfullyProcessedPendingJoints)
+                                            {
+                                                //DoJointErrorMessage(successfullyProcessedJoint, "finalizing succesfully procsssed joint " + successfullyProcessedJoint.ObjectNameInScene + " parms " + successfullyProcessedJoint.RawParams);
+                                                //DoJointErrorMessage(successfullyProcessedJoint, "removing from pending");
+                                                InternalRemovePendingJoint(successfullyProcessedJoint);
+                                                //DoJointErrorMessage(successfullyProcessedJoint, "adding to active");
+                                                InternalAddActiveJoint(successfullyProcessedJoint);
+                                                //DoJointErrorMessage(successfullyProcessedJoint, "done");
+                                            }
+                                        }
+                                    }
+
+                                    if (processedtaints)
+//Console.WriteLine("Simulate calls Clear of _taintedPrim list");                                      
+                                        _taintedPrimH.Clear();
+                                        _taintedPrimL.Clear();
+                                }
+
+                                // Move characters
+                                lock (_characters)
+                                {
+                                    List<OdeCharacter> defects = new List<OdeCharacter>();
+                                    foreach (OdeCharacter actor in _characters)
+                                    {
+                                        if (actor != null)
+                                            actor.Move(timeStep, defects);
+                                    }
+                                    if (0 != defects.Count)
+                                    {
+                                        foreach (OdeCharacter defect in defects)
+                                        {
+                                            RemoveCharacter(defect);
+                                        }
+                                    }
+                                }
+
+                                // Move other active objects
+                                lock (_activeprims)
+                                {
+                                    foreach (OdePrim prim in _activeprims)
+                                    {
+                                        prim.m_collisionscore = 0;
+                                        prim.Move(timeStep);
+                                    }
+                                }
+
+                                //if ((framecount % m_randomizeWater) == 0)
+                                   // randomizeWater(waterlevel);
+
+                                //int RayCastTimeMS = m_rayCastManager.ProcessQueuedRequests();
+                                m_rayCastManager.ProcessQueuedRequests();
+
+                                collision_optimized(timeStep);
+
+                                lock (_collisionEventPrim)
+                                {
+                                    foreach (PhysicsActor obj in _collisionEventPrim)
+                                    {
+                                        if (obj == null)
+                                            continue;
+
+                                        switch ((ActorTypes)obj.PhysicsActorType)
+                                        {
+                                            case ActorTypes.Agent:
+                                                OdeCharacter cobj = (OdeCharacter)obj;
+                                                cobj.AddCollisionFrameTime(100);
+                                                cobj.SendCollisions();
+                                                break;
+                                            case ActorTypes.Prim:
+                                                OdePrim pobj = (OdePrim)obj;
+                                                pobj.SendCollisions();
+                                                break;
+                                        }
+                                    }
+                                }
+
+                                //if (m_global_contactcount > 5)
+                                //{
+                                //    m_log.DebugFormat("[PHYSICS]: Contacts:{0}", m_global_contactcount);
+                                //}
+
+                                m_global_contactcount = 0;
+                                
+                                d.WorldQuickStep(world, ODE_STEPSIZE);
+                                d.JointGroupEmpty(contactgroup);
+                                //ode.dunlock(world);
+                            }
+                            catch (Exception e)
+                            {
+                                m_log.ErrorFormat("[PHYSICS]: {0}, {1}, {2}", e.Message, e.TargetSite, e);
+                                ode.dunlock(world);
+                            }
+
+                            step_time -= ODE_STEPSIZE;
+                            i++;
+                        //}
+                        //else
+                        //{
+                            //fps = 0;
+                        //}
+                    //}
+                }
+
+                lock (_characters)
+                {
+                    foreach (OdeCharacter actor in _characters)
+                    {
+                        if (actor != null)
+                        {
+                            if (actor.bad)
+                                m_log.WarnFormat("[PHYSICS]: BAD Actor {0} in _characters list was not removed?", actor.m_uuid);
+                            actor.UpdatePositionAndVelocity();
+                        }
+                    }
+                }
+
+                lock (_badCharacter)
+                {
+                    if (_badCharacter.Count > 0)
+                    {
+                        foreach (OdeCharacter chr in _badCharacter)
+                        {
+                            RemoveCharacter(chr);
+                        }
+                        _badCharacter.Clear();
+                    }
+                }
+
+                lock (_activeprims)
+                {
+                    //if (timeStep < 0.2f)
+                    {
+                        foreach (OdePrim actor in _activeprims)
+                        {
+                            if (actor.IsPhysical && (d.BodyIsEnabled(actor.Body) || !actor._zeroFlag))
+                            {
+                                actor.UpdatePositionAndVelocity();
+
+                                if (SupportsNINJAJoints)
+                                {
+                                    // If an actor moved, move its joint proxy objects as well.
+                                    // There seems to be an event PhysicsActor.OnPositionUpdate that could be used
+                                    // for this purpose but it is never called! So we just do the joint
+                                    // movement code here.
+
+                                    if (actor.SOPName != null &&
+                                        joints_connecting_actor.ContainsKey(actor.SOPName) &&
+                                        joints_connecting_actor[actor.SOPName] != null &&
+                                        joints_connecting_actor[actor.SOPName].Count > 0)
+                                    {
+                                        foreach (PhysicsJoint affectedJoint in joints_connecting_actor[actor.SOPName])
+                                        {
+                                            if (affectedJoint.IsInPhysicsEngine)
+                                            {
+                                                DoJointMoved(affectedJoint);
+                                            }
+                                            else
+                                            {
+                                                DoJointErrorMessage(affectedJoint, "a body connected to a joint was moved, but the joint doesn't exist yet! this will lead to joint error. joint was: " + affectedJoint.ObjectNameInScene + " parms:" + affectedJoint.RawParams);
+                                            }
+                                        }
+                                    }
+                                }
+                            }
+                        }
+                    }
+                }
+
+                //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);
+                }
+            }
+
+            return fps;
+        }
+
+        public override void GetResults()
+        {
+        }
+
+        public override bool IsThreaded
+        {
+            // for now we won't be multithreaded
+            get { return (false); }
+        }
+
+        #region ODE Specific Terrain Fixes
+        public float[] ResizeTerrain512NearestNeighbour(float[] heightMap)
+        {
+            float[] returnarr = new float[262144];
+            float[,] resultarr = new float[(int)WorldExtents.X, (int)WorldExtents.Y];
+
+            // Filling out the array into its multi-dimensional components
+            for (int y = 0; y < WorldExtents.Y; y++)
+            {
+                for (int x = 0; x < WorldExtents.X; x++)
+                {
+                    resultarr[y, x] = heightMap[y * (int)WorldExtents.Y + x];
+                }
+            }
+
+            // Resize using Nearest Neighbour
+
+            // This particular way is quick but it only works on a multiple of the original
+
+            // The idea behind this method can be described with the following diagrams
+            // second pass and third pass happen in the same loop really..  just separated
+            // them to show what this does.
+
+            // First Pass
+            // ResultArr:
+            // 1,1,1,1,1,1
+            // 1,1,1,1,1,1
+            // 1,1,1,1,1,1
+            // 1,1,1,1,1,1
+            // 1,1,1,1,1,1
+            // 1,1,1,1,1,1
+
+            // Second Pass
+            // ResultArr2:
+            // 1,,1,,1,,1,,1,,1,
+            // ,,,,,,,,,,
+            // 1,,1,,1,,1,,1,,1,
+            // ,,,,,,,,,,
+            // 1,,1,,1,,1,,1,,1,
+            // ,,,,,,,,,,
+            // 1,,1,,1,,1,,1,,1,
+            // ,,,,,,,,,,
+            // 1,,1,,1,,1,,1,,1,
+            // ,,,,,,,,,,
+            // 1,,1,,1,,1,,1,,1,
+
+            // Third pass fills in the blanks
+            // ResultArr2:
+            // 1,1,1,1,1,1,1,1,1,1,1,1
+            // 1,1,1,1,1,1,1,1,1,1,1,1
+            // 1,1,1,1,1,1,1,1,1,1,1,1
+            // 1,1,1,1,1,1,1,1,1,1,1,1
+            // 1,1,1,1,1,1,1,1,1,1,1,1
+            // 1,1,1,1,1,1,1,1,1,1,1,1
+            // 1,1,1,1,1,1,1,1,1,1,1,1
+            // 1,1,1,1,1,1,1,1,1,1,1,1
+            // 1,1,1,1,1,1,1,1,1,1,1,1
+            // 1,1,1,1,1,1,1,1,1,1,1,1
+            // 1,1,1,1,1,1,1,1,1,1,1,1
+
+            // X,Y = .
+            // X+1,y = ^
+            // X,Y+1 = *
+            // X+1,Y+1 = #
+
+            // Filling in like this;
+            // .*
+            // ^#
+            // 1st .
+            // 2nd *
+            // 3rd ^
+            // 4th #
+            // on single loop.
+
+            float[,] resultarr2 = new float[512, 512];
+            for (int y = 0; y < WorldExtents.Y; y++)
+            {
+                for (int x = 0; x < WorldExtents.X; x++)
+                {
+                    resultarr2[y * 2, x * 2] = resultarr[y, x];
+
+                    if (y < WorldExtents.Y)
+                    {
+                        resultarr2[(y * 2) + 1, x * 2] = resultarr[y, x];
+                    }
+                    if (x < WorldExtents.X)
+                    {
+                        resultarr2[y * 2, (x * 2) + 1] = resultarr[y, x];
+                    }
+                    if (x < WorldExtents.X && y < WorldExtents.Y)
+                    {
+                        resultarr2[(y * 2) + 1, (x * 2) + 1] = resultarr[y, x];
+                    }
+                }
+            }
+
+            //Flatten out the array
+            int i = 0;
+            for (int y = 0; y < 512; y++)
+            {
+                for (int x = 0; x < 512; x++)
+                {
+                    if (resultarr2[y, x] <= 0)
+                        returnarr[i] = 0.0000001f;
+                    else
+                        returnarr[i] = resultarr2[y, x];
+
+                    i++;
+                }
+            }
+
+            return returnarr;
+        }
+
+        public float[] ResizeTerrain512Interpolation(float[] heightMap)
+        {
+            float[] returnarr = new float[262144];
+            float[,] resultarr = new float[512,512];
+
+            // Filling out the array into its multi-dimensional components
+            for (int y = 0; y < 256; y++)
+            {
+                for (int x = 0; x < 256; x++)
+                {
+                    resultarr[y, x] = heightMap[y * 256 + x];
+                }
+            }
+
+            // Resize using interpolation
+
+            // This particular way is quick but it only works on a multiple of the original
+
+            // The idea behind this method can be described with the following diagrams
+            // second pass and third pass happen in the same loop really..  just separated
+            // them to show what this does.
+
+            // First Pass
+            // ResultArr:
+            // 1,1,1,1,1,1
+            // 1,1,1,1,1,1
+            // 1,1,1,1,1,1
+            // 1,1,1,1,1,1
+            // 1,1,1,1,1,1
+            // 1,1,1,1,1,1
+
+            // Second Pass
+            // ResultArr2:
+            // 1,,1,,1,,1,,1,,1,
+            // ,,,,,,,,,,
+            // 1,,1,,1,,1,,1,,1,
+            // ,,,,,,,,,,
+            // 1,,1,,1,,1,,1,,1,
+            // ,,,,,,,,,,
+            // 1,,1,,1,,1,,1,,1,
+            // ,,,,,,,,,,
+            // 1,,1,,1,,1,,1,,1,
+            // ,,,,,,,,,,
+            // 1,,1,,1,,1,,1,,1,
+
+            // Third pass fills in the blanks
+            // ResultArr2:
+            // 1,1,1,1,1,1,1,1,1,1,1,1
+            // 1,1,1,1,1,1,1,1,1,1,1,1
+            // 1,1,1,1,1,1,1,1,1,1,1,1
+            // 1,1,1,1,1,1,1,1,1,1,1,1
+            // 1,1,1,1,1,1,1,1,1,1,1,1
+            // 1,1,1,1,1,1,1,1,1,1,1,1
+            // 1,1,1,1,1,1,1,1,1,1,1,1
+            // 1,1,1,1,1,1,1,1,1,1,1,1
+            // 1,1,1,1,1,1,1,1,1,1,1,1
+            // 1,1,1,1,1,1,1,1,1,1,1,1
+            // 1,1,1,1,1,1,1,1,1,1,1,1
+
+            // X,Y = .
+            // X+1,y = ^
+            // X,Y+1 = *
+            // X+1,Y+1 = #
+
+            // Filling in like this;
+            // .*
+            // ^#
+            // 1st .
+            // 2nd *
+            // 3rd ^
+            // 4th #
+            // on single loop.
+
+            float[,] resultarr2 = new float[512,512];
+            for (int y = 0; y < (int)Constants.RegionSize; y++)
+            {
+                for (int x = 0; x < (int)Constants.RegionSize; x++)
+                {
+                    resultarr2[y*2, x*2] = resultarr[y, x];
+
+                    if (y < (int)Constants.RegionSize)
+                    {
+                        if (y + 1 < (int)Constants.RegionSize)
+                        {
+                            if (x + 1 < (int)Constants.RegionSize)
+                            {
+                                resultarr2[(y*2) + 1, x*2] = ((resultarr[y, x] + resultarr[y + 1, x] +
+                                                               resultarr[y, x + 1] + resultarr[y + 1, x + 1])/4);
+                            }
+                            else
+                            {
+                                resultarr2[(y*2) + 1, x*2] = ((resultarr[y, x] + resultarr[y + 1, x])/2);
+                            }
+                        }
+                        else
+                        {
+                            resultarr2[(y*2) + 1, x*2] = resultarr[y, x];
+                        }
+                    }
+                    if (x < (int)Constants.RegionSize)
+                    {
+                        if (x + 1 < (int)Constants.RegionSize)
+                        {
+                            if (y + 1 < (int)Constants.RegionSize)
+                            {
+                                resultarr2[y*2, (x*2) + 1] = ((resultarr[y, x] + resultarr[y + 1, x] +
+                                                               resultarr[y, x + 1] + resultarr[y + 1, x + 1])/4);
+                            }
+                            else
+                            {
+                                resultarr2[y*2, (x*2) + 1] = ((resultarr[y, x] + resultarr[y, x + 1])/2);
+                            }
+                        }
+                        else
+                        {
+                            resultarr2[y*2, (x*2) + 1] = resultarr[y, x];
+                        }
+                    }
+                    if (x < (int)Constants.RegionSize && y < (int)Constants.RegionSize)
+                    {
+                        if ((x + 1 < (int)Constants.RegionSize) && (y + 1 < (int)Constants.RegionSize))
+                        {
+                            resultarr2[(y*2) + 1, (x*2) + 1] = ((resultarr[y, x] + resultarr[y + 1, x] +
+                                                                 resultarr[y, x + 1] + resultarr[y + 1, x + 1])/4);
+                        }
+                        else
+                        {
+                            resultarr2[(y*2) + 1, (x*2) + 1] = resultarr[y, x];
+                        }
+                    }
+                }
+            }
+            //Flatten out the array
+            int i = 0;
+            for (int y = 0; y < 512; y++)
+            {
+                for (int x = 0; x < 512; x++)
+                {
+                    if (Single.IsNaN(resultarr2[y, x]) || Single.IsInfinity(resultarr2[y, x]))
+                    {
+                        m_log.Warn("[PHYSICS]: Non finite heightfield element detected.  Setting it to 0");
+                        resultarr2[y, x] = 0;
+                    }
+                    returnarr[i] = resultarr2[y, x];
+                    i++;
+                }
+            }
+
+            return returnarr;
+        }
+
+        #endregion
+
+        public override void SetTerrain(float[] heightMap)
+        {
+            if (m_worldOffset != Vector3.Zero && m_parentScene != null)
+            {
+                if (m_parentScene is OdeScene)
+                {
+                    ((OdeScene)m_parentScene).SetTerrain(heightMap, m_worldOffset);
+                }
+            }
+            else
+            {
+                SetTerrain(heightMap, m_worldOffset);
+            }
+        }
+
+        public void SetTerrain(float[] heightMap, Vector3 pOffset)
+        {
+            // this._heightmap[i] = (double)heightMap[i];
+            // dbm (danx0r) -- creating a buffer zone of one extra sample all around
+            //_origheightmap = heightMap;
+           
+            float[] _heightmap;
+
+            // zero out a heightmap array float array (single dimension [flattened]))
+            //if ((int)Constants.RegionSize == 256)
+            //    _heightmap = new float[514 * 514];
+            //else
+
+            _heightmap = new float[(((int)Constants.RegionSize + 2) * ((int)Constants.RegionSize + 2))];
+
+            uint heightmapWidth = Constants.RegionSize + 1;
+            uint heightmapHeight = Constants.RegionSize + 1;
+
+            uint heightmapWidthSamples;
+
+            uint heightmapHeightSamples;
+
+            //if (((int)Constants.RegionSize) == 256)
+            //{
+            //    heightmapWidthSamples = 2 * (uint)Constants.RegionSize + 2;
+            //    heightmapHeightSamples = 2 * (uint)Constants.RegionSize + 2;
+            //    heightmapWidth++;
+            //    heightmapHeight++;
+            //}
+            //else
+            //{
+
+                heightmapWidthSamples = (uint)Constants.RegionSize + 1;
+                heightmapHeightSamples = (uint)Constants.RegionSize + 1;
+            //}
+
+            const float scale = 1.0f;
+            const float offset = 0.0f;
+            const float thickness = 0.2f;
+            const int wrap = 0;
+
+            int regionsize = (int) Constants.RegionSize + 2;
+            //Double resolution
+            //if (((int)Constants.RegionSize) == 256)
+            //    heightMap = ResizeTerrain512Interpolation(heightMap);
+
+
+           // if (((int)Constants.RegionSize) == 256 && (int)Constants.RegionSize == 256)
+           //     regionsize = 512;
+
+            float hfmin = 2000;
+            float hfmax = -2000;
+            
+                for (int x = 0; x < heightmapWidthSamples; x++)
+                {
+                    for (int y = 0; y < heightmapHeightSamples; y++)
+                    {
+                        int xx = Util.Clip(x - 1, 0, regionsize - 1);
+                        int yy = Util.Clip(y - 1, 0, regionsize - 1);
+                        
+                        
+                        float val= heightMap[yy * (int)Constants.RegionSize + xx];
+                         _heightmap[x * ((int)Constants.RegionSize + 2) + y] = val;
+                        
+                        hfmin = (val < hfmin) ? val : hfmin;
+                        hfmax = (val > hfmax) ? val : hfmax;
+                    }
+                }
+                
+            
+            
+
+            lock (OdeLock)
+            {
+                IntPtr GroundGeom = IntPtr.Zero;
+                if (RegionTerrain.TryGetValue(pOffset, out GroundGeom))
+                {
+                    RegionTerrain.Remove(pOffset);
+                    if (GroundGeom != IntPtr.Zero)
+                    {
+                        if (TerrainHeightFieldHeights.ContainsKey(GroundGeom))
+                        {
+                            TerrainHeightFieldHeights.Remove(GroundGeom);
+                        }
+                        d.SpaceRemove(space, GroundGeom);
+                        d.GeomDestroy(GroundGeom);
+                    }
+
+                }
+                IntPtr HeightmapData = d.GeomHeightfieldDataCreate();
+                d.GeomHeightfieldDataBuildSingle(HeightmapData, _heightmap, 0, heightmapWidth + 1, heightmapHeight + 1,
+                                                 (int)heightmapWidthSamples + 1, (int)heightmapHeightSamples + 1, scale,
+                                                 offset, thickness, wrap);
+                d.GeomHeightfieldDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1);
+                GroundGeom = d.CreateHeightfield(space, HeightmapData, 1);
+                if (GroundGeom != IntPtr.Zero)
+                {
+                    d.GeomSetCategoryBits(GroundGeom, (int)(CollisionCategories.Land));
+                    d.GeomSetCollideBits(GroundGeom, (int)(CollisionCategories.Space));
+
+                }
+                geom_name_map[GroundGeom] = "Terrain";
+
+                d.Matrix3 R = new d.Matrix3();
+
+                Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f);
+                Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f);
+                //Axiom.Math.Quaternion q3 = Axiom.Math.Quaternion.FromAngleAxis(3.14f, new Axiom.Math.Vector3(0, 0, 1));
+
+                q1 = q1 * q2;
+                //q1 = q1 * q3;
+                Vector3 v3;
+                float angle;
+                q1.GetAxisAngle(out v3, out angle);
+
+                d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle);
+                d.GeomSetRotation(GroundGeom, ref R);
+                d.GeomSetPosition(GroundGeom, (pOffset.X + ((int)Constants.RegionSize * 0.5f)) - 1, (pOffset.Y + ((int)Constants.RegionSize * 0.5f)) - 1, 0);
+                IntPtr testGround = IntPtr.Zero;
+                if (RegionTerrain.TryGetValue(pOffset, out testGround))
+                {
+                    RegionTerrain.Remove(pOffset);
+                }
+                RegionTerrain.Add(pOffset, GroundGeom, GroundGeom);
+                TerrainHeightFieldHeights.Add(GroundGeom,_heightmap);
+                
+            }
+        }
+
+        public override void DeleteTerrain()
+        {
+        }
+
+        public float GetWaterLevel()
+        {
+            return waterlevel;
+        }
+
+        public override bool SupportsCombining()
+        {
+            return true;
+        }
+
+        public override void UnCombine(PhysicsScene pScene)
+        {
+            IntPtr localGround = IntPtr.Zero;
+//            float[] localHeightfield;
+            bool proceed = false;
+            List<IntPtr> geomDestroyList = new List<IntPtr>();
+
+            lock (OdeLock)
+            {
+                if (RegionTerrain.TryGetValue(Vector3.Zero, out localGround))
+                {
+                    foreach (IntPtr geom in TerrainHeightFieldHeights.Keys)
+                    {
+                        if (geom == localGround)
+                        {
+//                            localHeightfield = TerrainHeightFieldHeights[geom];
+                            proceed = true;
+                        }
+                        else
+                        {
+                            geomDestroyList.Add(geom);
+                        }
+                    }
+
+                    if (proceed)
+                    {
+                        m_worldOffset = Vector3.Zero;
+                        WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize);
+                        m_parentScene = null;
+
+                        foreach (IntPtr g in geomDestroyList)
+                        {
+                            // removingHeightField needs to be done or the garbage collector will
+                            // collect the terrain data before we tell ODE to destroy it causing 
+                            // memory corruption
+                            if (TerrainHeightFieldHeights.ContainsKey(g))
+                            {
+//                                float[] removingHeightField = TerrainHeightFieldHeights[g];
+                                TerrainHeightFieldHeights.Remove(g);
+
+                                if (RegionTerrain.ContainsKey(g))
+                                {
+                                    RegionTerrain.Remove(g);
+                                }
+
+                                d.GeomDestroy(g);
+                                //removingHeightField = new float[0];
+                            }
+                        }
+
+                    }
+                    else
+                    {
+                        m_log.Warn("[PHYSICS]: Couldn't proceed with UnCombine.  Region has inconsistant data.");
+
+                    }
+                }
+            }
+        }
+
+        public override void SetWaterLevel(float baseheight)
+        {
+            waterlevel = baseheight;
+            randomizeWater(waterlevel);
+        }
+
+        public void randomizeWater(float baseheight)
+        {
+            const uint heightmapWidth = m_regionWidth + 2;
+            const uint heightmapHeight = m_regionHeight + 2;
+            const uint heightmapWidthSamples = m_regionWidth + 2;
+            const uint heightmapHeightSamples = m_regionHeight + 2;
+            const float scale = 1.0f;
+            const float offset = 0.0f;
+            const float thickness = 2.9f;
+            const int wrap = 0;
+
+            for (int i = 0; i < (258 * 258); i++)
+            {
+                _watermap[i] = (baseheight-0.1f) + ((float)fluidRandomizer.Next(1,9) / 10f);
+               // m_log.Info((baseheight - 0.1f) + ((float)fluidRandomizer.Next(1, 9) / 10f));
+            }
+
+            lock (OdeLock)
+            {
+                if (WaterGeom != IntPtr.Zero)
+                {
+                    d.SpaceRemove(space, WaterGeom);
+                }
+                IntPtr HeightmapData = d.GeomHeightfieldDataCreate();
+                d.GeomHeightfieldDataBuildSingle(HeightmapData, _watermap, 0, heightmapWidth, heightmapHeight,
+                                                 (int)heightmapWidthSamples, (int)heightmapHeightSamples, scale,
+                                                 offset, thickness, wrap);
+                d.GeomHeightfieldDataSetBounds(HeightmapData, m_regionWidth, m_regionHeight);
+                WaterGeom = d.CreateHeightfield(space, HeightmapData, 1);
+                if (WaterGeom != IntPtr.Zero)
+                {
+                    d.GeomSetCategoryBits(WaterGeom, (int)(CollisionCategories.Water));
+                    d.GeomSetCollideBits(WaterGeom, (int)(CollisionCategories.Space));
+
+                }
+                geom_name_map[WaterGeom] = "Water";
+
+                d.Matrix3 R = new d.Matrix3();
+
+                Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f);
+                Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f);
+                //Axiom.Math.Quaternion q3 = Axiom.Math.Quaternion.FromAngleAxis(3.14f, new Axiom.Math.Vector3(0, 0, 1));
+
+                q1 = q1 * q2;
+                //q1 = q1 * q3;
+                Vector3 v3;
+                float angle;
+                q1.GetAxisAngle(out v3, out angle);
+
+                d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle);
+                d.GeomSetRotation(WaterGeom, ref R);
+                d.GeomSetPosition(WaterGeom, 128, 128, 0);
+                
+            }
+
+        }
+
+        public override void Dispose()
+        {
+            m_rayCastManager.Dispose();
+            m_rayCastManager = null;
+
+            lock (OdeLock)
+            {
+                lock (_prims)
+                {
+                    foreach (OdePrim prm in _prims)
+                    {
+                        RemovePrim(prm);
+                    }
+                }
+
+                //foreach (OdeCharacter act in _characters)
+                //{
+                    //RemoveAvatar(act);
+                //}
+                d.WorldDestroy(world);
+                //d.CloseODE();
+            }
+        }
+        public override Dictionary<uint, float> GetTopColliders()
+        {
+            Dictionary<uint, float> returncolliders = new Dictionary<uint, float>();
+            int cnt = 0;
+            lock (_prims)
+            {
+                foreach (OdePrim prm in _prims)
+                {
+                    if (prm.CollisionScore > 0)
+                    {
+                        returncolliders.Add(prm.m_localID, prm.CollisionScore);
+                        cnt++;
+                        prm.CollisionScore = 0f;
+                        if (cnt > 25)
+                        {
+                            break;
+                        }
+                    }
+                }
+            }
+            return returncolliders;
+        }
+
+        public override bool SupportsRayCast()
+        {
+            return true;
+        }
+
+        public override void RaycastWorld(Vector3 position, Vector3 direction, float length, RaycastCallback retMethod)
+        {
+            if (retMethod != null)
+            {
+                m_rayCastManager.QueueRequest(position, direction, length, retMethod);
+            }
+        }
+
+#if USE_DRAWSTUFF
+        // Keyboard callback
+        public void command(int cmd)
+        {
+            IntPtr geom;
+            d.Mass mass;
+            d.Vector3 sides = new d.Vector3(d.RandReal() * 0.5f + 0.1f, d.RandReal() * 0.5f + 0.1f, d.RandReal() * 0.5f + 0.1f);
+
+            
+
+            Char ch = Char.ToLower((Char)cmd);
+            switch ((Char)ch)
+            {
+                case 'w':
+                    try
+                    {
+                        Vector3 rotate = (new Vector3(1, 0, 0) * Quaternion.CreateFromEulers(hpr.Z * Utils.DEG_TO_RAD, hpr.Y * Utils.DEG_TO_RAD, hpr.X * Utils.DEG_TO_RAD));
+
+                        xyz.X += rotate.X; xyz.Y += rotate.Y; xyz.Z += rotate.Z;
+                        ds.SetViewpoint(ref xyz, ref hpr);
+                    }
+                    catch (ArgumentException)
+                    { hpr.X = 0; }
+                    break;
+
+                case 'a':
+                    hpr.X++;
+                    ds.SetViewpoint(ref xyz, ref hpr);
+                    break;
+
+                case 's':
+                    try
+                    {
+                        Vector3 rotate2 = (new Vector3(-1, 0, 0) * Quaternion.CreateFromEulers(hpr.Z * Utils.DEG_TO_RAD, hpr.Y * Utils.DEG_TO_RAD, hpr.X * Utils.DEG_TO_RAD));
+
+                        xyz.X += rotate2.X; xyz.Y += rotate2.Y; xyz.Z += rotate2.Z;
+                        ds.SetViewpoint(ref xyz, ref hpr);
+                    }
+                    catch (ArgumentException)
+                    { hpr.X = 0; }
+                    break;
+                case 'd':
+                    hpr.X--;
+                    ds.SetViewpoint(ref xyz, ref hpr);
+                    break;
+                case 'r':
+                    xyz.Z++;
+                    ds.SetViewpoint(ref xyz, ref hpr);
+                    break;
+                case 'f':
+                    xyz.Z--;
+                    ds.SetViewpoint(ref xyz, ref hpr);
+                    break;
+                case 'e':
+                    xyz.Y++;
+                    ds.SetViewpoint(ref xyz, ref hpr);
+                    break;
+                case 'q':
+                    xyz.Y--;
+                    ds.SetViewpoint(ref xyz, ref hpr);
+                    break;
+            }
+        }
+
+        public void step(int pause)
+        {
+            
+            ds.SetColor(1.0f, 1.0f, 0.0f);
+            ds.SetTexture(ds.Texture.Wood);
+            lock (_prims)
+            {
+                foreach (OdePrim prm in _prims)
+                {
+                    //IntPtr body = d.GeomGetBody(prm.prim_geom);
+                    if (prm.prim_geom != IntPtr.Zero)
+                    {
+                        d.Vector3 pos;
+                        d.GeomCopyPosition(prm.prim_geom, out pos);
+                        //d.BodyCopyPosition(body, out pos);
+
+                        d.Matrix3 R;
+                        d.GeomCopyRotation(prm.prim_geom, out R);
+                        //d.BodyCopyRotation(body, out R);
+
+
+                        d.Vector3 sides = new d.Vector3();
+                        sides.X = prm.Size.X;
+                        sides.Y = prm.Size.Y;
+                        sides.Z = prm.Size.Z;
+
+                        ds.DrawBox(ref pos, ref R, ref sides);
+                    }
+                }
+            }
+            ds.SetColor(1.0f, 0.0f, 0.0f);
+            lock (_characters)
+            {
+                foreach (OdeCharacter chr in _characters)
+                {
+                    if (chr.Shell != IntPtr.Zero)
+                    {
+                        IntPtr body = d.GeomGetBody(chr.Shell);
+
+                        d.Vector3 pos;
+                        d.GeomCopyPosition(chr.Shell, out pos);
+                        //d.BodyCopyPosition(body, out pos);
+
+                        d.Matrix3 R;
+                        d.GeomCopyRotation(chr.Shell, out R);
+                        //d.BodyCopyRotation(body, out R);
+
+                        ds.DrawCapsule(ref pos, ref R, chr.Size.Z, 0.35f);
+                        d.Vector3 sides = new d.Vector3();
+                        sides.X = 0.5f;
+                        sides.Y = 0.5f;
+                        sides.Z = 0.5f;
+
+                        ds.DrawBox(ref pos, ref R, ref sides);
+                    }
+                }
+            }
+        }
+
+        public void start(int unused)
+        {
+            ds.SetViewpoint(ref xyz, ref hpr);
+        }
+#endif
+    }
+}
diff --git a/OpenSim/Region/Physics/ChOdePlugin/Tests/ODETestClass.cs b/OpenSim/Region/Physics/ChOdePlugin/Tests/ODETestClass.cs
new file mode 100644
index 0000000..69e2d03
--- /dev/null
+++ b/OpenSim/Region/Physics/ChOdePlugin/Tests/ODETestClass.cs
@@ -0,0 +1,122 @@
+/*
+ * Copyright (c) Contributors, http://opensimulator.org/
+ * See CONTRIBUTORS.TXT for a full list of copyright holders.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the OpenSimulator Project nor the
+ *       names of its contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+using System;
+using Nini.Config;
+using NUnit.Framework;
+using OpenMetaverse;
+using OpenSim.Framework;
+using OpenSim.Region.Physics.Manager;
+using log4net;
+using System.Reflection;
+
+namespace OpenSim.Region.Physics.OdePlugin
+{
+    [TestFixture]
+    public class ODETestClass
+    {
+        private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
+
+        private OdePlugin cbt;
+        private PhysicsScene ps;
+        private IMeshingPlugin imp;
+
+        [SetUp]
+        public void Initialize()
+        {
+            // Loading ODEPlugin
+            cbt = new OdePlugin();
+            // Loading Zero Mesher
+            imp = new ZeroMesherPlugin();
+            // Getting Physics Scene
+            ps = cbt.GetScene("test");
+            // Initializing Physics Scene.
+            ps.Initialise(imp.GetMesher(),null);
+            float[] _heightmap = new float[(int)Constants.RegionSize * (int)Constants.RegionSize];
+            for (int i = 0; i < ((int)Constants.RegionSize * (int)Constants.RegionSize); i++)
+            {
+                _heightmap[i] = 21f;
+            }
+            ps.SetTerrain(_heightmap);
+        }
+
+        [TearDown]
+        public void Terminate()
+        {
+            ps.DeleteTerrain();
+            ps.Dispose();
+
+        }
+
+        [Test]
+        public void CreateAndDropPhysicalCube()
+        {
+            PrimitiveBaseShape newcube = PrimitiveBaseShape.CreateBox();
+            Vector3 position = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f), 128f);
+            Vector3 size = new Vector3(0.5f, 0.5f, 0.5f);
+            Quaternion rot = Quaternion.Identity;
+            PhysicsActor prim = ps.AddPrimShape("CoolShape", newcube, position, size, rot, true);
+            OdePrim oprim = (OdePrim)prim;
+            OdeScene pscene = (OdeScene) ps;
+
+            Assert.That(oprim.m_taintadd);
+
+            prim.LocalID = 5;
+
+            for (int i = 0; i < 58; i++)
+            {
+                ps.Simulate(0.133f);
+
+                Assert.That(oprim.prim_geom != (IntPtr)0);
+
+                Assert.That(oprim.m_targetSpace != (IntPtr)0);
+
+                //Assert.That(oprim.m_targetSpace == pscene.space);
+                m_log.Info("TargetSpace: " + oprim.m_targetSpace + " - SceneMainSpace: " + pscene.space);
+
+                Assert.That(!oprim.m_taintadd);
+                m_log.Info("Prim Position (" + oprim.m_localID + "): " + prim.Position.ToString());
+
+                // Make sure we're above the ground
+                //Assert.That(prim.Position.Z > 20f);
+                //m_log.Info("PrimCollisionScore (" + oprim.m_localID + "): " + oprim.m_collisionscore);
+
+                // Make sure we've got a Body
+                Assert.That(oprim.Body != (IntPtr)0);
+                //m_log.Info(
+            }
+
+            // Make sure we're not somewhere above the ground
+            Assert.That(prim.Position.Z < 21.5f);
+
+            ps.RemovePrim(prim);
+            Assert.That(oprim.m_taintremove);
+            ps.Simulate(0.133f);
+            Assert.That(oprim.Body == (IntPtr)0);
+        }
+    }
+}
diff --git a/OpenSim/Region/Physics/ChOdePlugin/drawstuff.cs b/OpenSim/Region/Physics/ChOdePlugin/drawstuff.cs
new file mode 100644
index 0000000..87ca446
--- /dev/null
+++ b/OpenSim/Region/Physics/ChOdePlugin/drawstuff.cs
@@ -0,0 +1,98 @@
+/*
+ * Copyright ODE
+ * Ode.NET - .NET bindings for ODE
+ *  Jason Perkins (starkos@industriousone.com)
+ *  Licensed under the New BSD
+ *  Part of the OpenDynamicsEngine
+Open Dynamics Engine
+Copyright (c) 2001-2007, Russell L. Smith.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions
+are met:
+
+Redistributions of source code must retain the above copyright notice,
+this list of conditions and the following disclaimer.
+
+Redistributions in binary form must reproduce the above copyright notice,
+this list of conditions and the following disclaimer in the documentation
+and/or other materials provided with the distribution.
+
+Neither the names of ODE's copyright owner nor the names of its
+contributors may be used to endorse or promote products derived from
+this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ * 
+ * 
+ */
+
+using System;
+using System.Runtime.InteropServices;
+using Ode.NET;
+
+namespace Drawstuff.NET
+{
+#if dDOUBLE
+    using dReal = System.Double;
+#else
+    using dReal = System.Single;
+#endif
+
+    public static class ds
+    {
+        public const int VERSION = 2;
+
+        public enum Texture
+        {
+            None,
+            Wood
+        }
+
+        [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
+        public delegate void CallbackFunction(int arg);
+
+        [StructLayout(LayoutKind.Sequential)]
+        public struct Functions
+        {
+            public int version;
+            public CallbackFunction start;
+            public CallbackFunction step;
+            public CallbackFunction command;
+            public CallbackFunction stop;
+            public string path_to_textures;
+        }
+
+        [DllImport("drawstuff", EntryPoint = "dsDrawBox")]
+        public static extern void DrawBox(ref d.Vector3 pos, ref d.Matrix3 R, ref d.Vector3 sides);
+
+        [DllImport("drawstuff", EntryPoint = "dsDrawCapsule")]
+        public static extern void DrawCapsule(ref d.Vector3 pos, ref d.Matrix3 R, dReal length, dReal radius);
+
+        [DllImport("drawstuff", EntryPoint = "dsDrawConvex")]
+        public static extern void DrawConvex(ref d.Vector3 pos, ref d.Matrix3 R, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons);
+
+        [DllImport("drawstuff", EntryPoint = "dsSetColor")]
+        public static extern void SetColor(float red, float green, float blue);
+
+        [DllImport("drawstuff", EntryPoint = "dsSetTexture")]
+        public static extern void SetTexture(Texture texture);
+
+        [DllImport("drawstuff", EntryPoint = "dsSetViewpoint")]
+        public static extern void SetViewpoint(ref d.Vector3 xyz, ref d.Vector3 hpr);
+
+        [DllImport("drawstuff", EntryPoint = "dsSimulationLoop")]
+        public static extern void SimulationLoop(int argc, string[] argv, int window_width, int window_height, ref Functions fn);
+    }
+}