1 files changed, 366 insertions, 392 deletions

diff --git a/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs
index 2a66060..73c1c02 100644
--- a/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs
+++ b/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs
@@ -39,7 +39,6 @@ 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,
@@ -64,31 +63,32 @@ namespace OpenSim.Region.Physics.OdePlugin
         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 m_taintTargetVelocity;
         private Vector3 _target_velocity;
         private Vector3 _acceleration;
         private Vector3 m_rotationalVelocity;
         private float m_mass = 80f;
-        public float m_density = 60f;
+        private float m_density = 60f;
         private bool m_pidControllerActive = true;
-        public float PID_D = 800.0f;
-        public float PID_P = 900.0f;
+        private float PID_D = 800.0f;
+        private 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 float CAPSULE_RADIUS = 0.37f;
+        private float CAPSULE_LENGTH = 2.140599f;
+        private float m_tensor = 3800000f;
+//        private float heightFudgeFactor = 0.52f;
+        private float walkDivisor = 1.3f;
+        private float runDivisor = 0.8f;
         private bool flying = false;
         private bool m_iscolliding = false;
         private bool m_iscollidingGround = false;
@@ -99,24 +99,29 @@ namespace OpenSim.Region.Physics.OdePlugin
         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;
+        private Vector3 m_taintPosition;
+
+        /// <summary>
+        /// Hold set forces so we can process them outside physics calculations.  This prevents race conditions if we set force
+        /// while calculatios are going on
+        /// </summary>
+        private Vector3 m_taintForce;
+
         // taints and their non-tainted counterparts
-        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 bool m_isPhysical = false; // the current physical status
+        private bool m_tainted_isPhysical = false; // set when the physical status is tainted (false=not existing in physics engine, true=existing)
+        internal float MinimumGroundFlightOffset = 3f;
 
-        private float m_tainted_CAPSULE_LENGTH; // set when the capsule length changes. 
-        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_tainted_CAPSULE_LENGTH; // set when the capsule length changes.
 
+        /// <summary>
+        /// Used to introduce a fixed tilt because a straight-up capsule falls through terrain, probably a bug in terrain collider
+        /// </summary>
+        private float m_tiltMagnitudeWhenProjectedOnXYPlane = 0.1131371f;
 
         private float m_buoyancy = 0f;
 
         // private CollisionLocker ode;
-
-        private string m_name = String.Empty;
-
         private bool[] m_colliderarr = new bool[11];
         private bool[] m_colliderGroundarr = new bool[11];
 
@@ -129,21 +134,23 @@ namespace OpenSim.Region.Physics.OdePlugin
                                                         | CollisionCategories.Body
                                                         | CollisionCategories.Character
                                                         | CollisionCategories.Land);
-        public IntPtr Body = IntPtr.Zero;
+        internal 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;
+        internal IntPtr Shell = IntPtr.Zero;
+        internal IntPtr Amotor = IntPtr.Zero;
+        private d.Mass ShellMass;
 
-        public int m_eventsubscription = 0;
+        private int m_eventsubscription = 0;
         private CollisionEventUpdate CollisionEventsThisFrame = new CollisionEventUpdate();
 
         // unique UUID of this character object
-        public UUID m_uuid;
-        public bool bad = false;
+        internal UUID m_uuid { get; private set; }
+        internal 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)
+        public OdeCharacter(
+            String avName, OdeScene parent_scene, Vector3 pos, Vector3 size, float pid_d, float pid_p,
+            float capsule_radius, float tensor, float density,
+            float walk_divisor, float rundivisor)
         {
             m_uuid = UUID.Random();
 
@@ -157,18 +164,20 @@ namespace OpenSim.Region.Physics.OdePlugin
                 {
                     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;
+                m_taintPosition = pos;
             }
             else
             {
-                _position = new Vector3(((float)_parent_scene.WorldExtents.X * 0.5f), ((float)_parent_scene.WorldExtents.Y * 0.5f), parent_scene.GetTerrainHeightAtXY(128f, 128f) + 10f);
-                m_taintPosition.X = _position.X;
-                m_taintPosition.Y = _position.Y;
-                m_taintPosition.Z = _position.Z;
-                m_log.Warn("[PHYSICS]: Got NaN Position on Character Create");
+                _position
+                    = new Vector3(
+                        (float)_parent_scene.WorldExtents.X * 0.5f,
+                        (float)_parent_scene.WorldExtents.Y * 0.5f,
+                        parent_scene.GetTerrainHeightAtXY(128f, 128f) + 10f);
+                m_taintPosition = _position;
+
+                m_log.WarnFormat("[ODE CHARACTER]: Got NaN Position on Character Create for {0}", avName);
             }
 
             _parent_scene = parent_scene;
@@ -178,7 +187,7 @@ namespace OpenSim.Region.Physics.OdePlugin
             CAPSULE_RADIUS = capsule_radius;
             m_tensor = tensor;
             m_density = density;
-            heightFudgeFactor = height_fudge_factor;
+//            heightFudgeFactor = height_fudge_factor;
             walkDivisor = walk_divisor;
             runDivisor = rundivisor;
 
@@ -191,7 +200,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                 m_colliderarr[i] = false;
             }
             CAPSULE_LENGTH = (size.Z * 1.15f) - CAPSULE_RADIUS * 2.0f;
-            //m_log.Info("[SIZE]: " + CAPSULE_LENGTH.ToString());
+            //m_log.Info("[ODE CHARACTER]: " + CAPSULE_LENGTH.ToString());
             m_tainted_CAPSULE_LENGTH = CAPSULE_LENGTH;
 
             m_isPhysical = false; // current status: no ODE information exists
@@ -199,7 +208,7 @@ namespace OpenSim.Region.Physics.OdePlugin
 
             _parent_scene.AddPhysicsActorTaint(this);
             
-            m_name = avName;
+            Name = avName;
         }
 
         public override int PhysicsActorType
@@ -217,11 +226,6 @@ namespace OpenSim.Region.Physics.OdePlugin
             set { m_alwaysRun = value; }
         }
 
-        public override uint LocalID
-        {
-            set { m_localID = value; }
-        }
-
         public override bool Grabbed
         {
             set { return; }
@@ -258,7 +262,11 @@ namespace OpenSim.Region.Physics.OdePlugin
         public override bool Flying
         {
             get { return flying; }
-            set { flying = value; }
+            set
+            {
+                flying = value;
+//                m_log.DebugFormat("[ODE CHARACTER]: Set OdeCharacter Flying to {0}", flying);
+            }
         }
 
         /// <summary>
@@ -305,10 +313,12 @@ namespace OpenSim.Region.Physics.OdePlugin
                 {
                     m_iscolliding = true;
                 }
+
                 if (m_wascolliding != m_iscolliding)
                 {
                     //base.SendCollisionUpdate(new CollisionEventUpdate());
                 }
+
                 m_wascolliding = m_iscolliding;
             }
         }
@@ -421,18 +431,12 @@ namespace OpenSim.Region.Physics.OdePlugin
                             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;
+                        m_taintPosition = value;                        
                         _parent_scene.AddPhysicsActorTaint(this);
                     }
                     else
                     {
-                        m_log.Warn("[PHYSICS]: Got a NaN Position from Scene on a Character");
+                        m_log.WarnFormat("[ODE CHARACTER]: Got a NaN Position from Scene on character {0}", Name);
                     }
                 }
             }
@@ -458,16 +462,18 @@ namespace OpenSim.Region.Physics.OdePlugin
                     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());
+                    m_tainted_CAPSULE_LENGTH = (SetSize.Z * 1.15f) - CAPSULE_RADIUS * 2.0f;
+//                    m_log.Info("[ODE CHARACTER]: " + CAPSULE_LENGTH);
 
-                    Velocity = Vector3.Zero;
+                    // If we reset velocity here, then an avatar stalls when it crosses a border for the first time
+                    // (as the height of the new root agent is set).
+//                    Velocity = Vector3.Zero;
 
                     _parent_scene.AddPhysicsActorTaint(this);
                 }
                 else
                 {
-                    m_log.Warn("[PHYSICS]: Got a NaN Size from Scene on a Character");
+                    m_log.WarnFormat("[ODE CHARACTER]: Got a NaN Size from Scene on {0}", Name);
                 }
             }
         }
@@ -519,7 +525,6 @@ namespace OpenSim.Region.Physics.OdePlugin
                 }
             }
 
-
             // movementVector.Z is zero
 
             // calculate tilt components based on desired amount of tilt and current (snapped) heading.
@@ -527,7 +532,7 @@ namespace OpenSim.Region.Physics.OdePlugin
             float xTiltComponent = -movementVector.X * m_tiltMagnitudeWhenProjectedOnXYPlane;
             float yTiltComponent = -movementVector.Y * m_tiltMagnitudeWhenProjectedOnXYPlane;
 
-            //m_log.Debug("[PHYSICS] changing avatar tilt");
+            //m_log.Debug("[ODE CHARACTER]: changing avatar tilt");
             d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, xTiltComponent);
             d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, xTiltComponent); // must be same as lowstop, else a different, spurious tilt is introduced
             d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, yTiltComponent);
@@ -537,127 +542,6 @@ namespace OpenSim.Region.Physics.OdePlugin
         }
 
         /// <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>
@@ -669,20 +553,12 @@ namespace OpenSim.Region.Physics.OdePlugin
                 return m_density*AVvolume;
             }
         }
-        public override void link(PhysicsActor obj)
-        {
 
-        }
+        public override void link(PhysicsActor obj) {}
 
-        public override void delink()
-        {
+        public override void delink() {}
 
-        }
-
-        public override void LockAngularMotion(Vector3 axis)
-        {
-
-        }
+        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.
@@ -704,7 +580,7 @@ namespace OpenSim.Region.Physics.OdePlugin
 //             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);
+//             //m_log.Info("[PHYSICSAV]: Rotation: " + bodyrotation.M00 + " : " + bodyFArotation.M01 + " : " + bodyrotation.M02 + " : " + bodyrotation.M10 + " : " + bodyrotation.M11 + " : " + bodyrotation.M12 + " : " + bodyrotation.M20 + " : " + bodyrotation.M21 + " : " + bodyrotation.M22);
 //         }
 
         public override Vector3 Force
@@ -721,27 +597,22 @@ namespace OpenSim.Region.Physics.OdePlugin
 
         public override void VehicleFloatParam(int param, float value)
         {
-
         }
 
         public override void VehicleVectorParam(int param, Vector3 value)
         {
-
         }
 
         public override void VehicleRotationParam(int param, Quaternion rotation)
         {
-
         }
 
         public override void VehicleFlags(int param, bool remove)
         {
-
         }
 
         public override void SetVolumeDetect(int param)
         {
-
         }
 
         public override Vector3 CenterOfMass
@@ -761,24 +632,29 @@ namespace OpenSim.Region.Physics.OdePlugin
 
         public override Vector3 Velocity
         {
-            get {
+            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;
+                    m_taintTargetVelocity = value;
+                    _parent_scene.AddPhysicsActorTaint(this);
                 }
                 else
                 {
-                    m_log.Warn("[PHYSICS]: Got a NaN velocity from Scene in a Character");
+                    m_log.WarnFormat("[ODE CHARACTER]: Got a NaN velocity from Scene for {0}", Name);
                 }
+
+//                m_log.DebugFormat("[PHYSICS]: Set target velocity of {0}", m_taintTargetVelocity);
             }
         }
 
@@ -834,63 +710,48 @@ namespace OpenSim.Region.Physics.OdePlugin
                 {
                     m_pidControllerActive = false;
                     force *= 100f;
-                    doForce(force);
+                    m_taintForce += force;
+                    _parent_scene.AddPhysicsActorTaint(this);
+
                     // 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;
+                    // m_taintTargetVelocity.X += force.X;
+                    // m_taintTargetVelocity.Y += force.Y;
+                    // m_taintTargetVelocity.Z += force.Z;
                 }
                 else
                 {
                     m_pidControllerActive = true;
-                    _target_velocity.X += force.X;
-                    _target_velocity.Y += force.Y;
-                    _target_velocity.Z += force.Z;
+                    m_taintTargetVelocity += force;
                 }
             }
             else
             {
-                m_log.Warn("[PHYSICS]: Got a NaN force applied to a Character");
+                m_log.WarnFormat("[ODE CHARACTER]: Got a NaN force applied to {0}", Name);
             }
             //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)
+        /// <param name="defects">The character will be added to this list if there is something wrong (non-finite
+        /// position or velocity).
+        /// </param>
+        internal void Move(List<OdeCharacter> defects)
         {
             //  no lock; for now it's only called from within Simulate()
-
+            
             // If the PID Controller isn't active then we set our force
             // calculating base velocity to the current position
 
@@ -908,36 +769,11 @@ namespace OpenSim.Region.Physics.OdePlugin
             
             if (!localPos.IsFinite())
             {
+                m_log.WarnFormat(
+                    "[ODE CHARACTER]: Avatar position of {0} for {1} is non-finite!  Removing from physics scene.",
+                    localPos, Name);
 
-                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;
             }
@@ -965,6 +801,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                     _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
@@ -974,7 +811,7 @@ namespace OpenSim.Region.Physics.OdePlugin
 
                     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);
+                    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;
@@ -995,14 +832,18 @@ namespace OpenSim.Region.Physics.OdePlugin
                 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);
+                    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);
+                    vec.X = ((_target_velocity.X / movementdivisor) - vel.X) * (PID_D / 6);
+                    vec.Y = ((_target_velocity.Y / movementdivisor) - vel.Y) * (PID_D / 6);
+
+//                    m_log.DebugFormat(
+//                        "[ODE CHARACTER]: !m_iscolliding && flying, vec {0}, _target_velocity {1}, movementdivisor {2}, vel {3}",
+//                        vec, _target_velocity, movementdivisor, vel);
                 }
 
                 if (m_iscolliding && !flying && _target_velocity.Z > 0.0f)
@@ -1013,11 +854,11 @@ namespace OpenSim.Region.Physics.OdePlugin
                     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;
+                        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;
+                        vec.Y = ((_target_velocity.Y - vel.Y) / 1.2f) * PID_D;
                     }
                 }
                 else if (!m_iscolliding && !flying)
@@ -1028,11 +869,11 @@ namespace OpenSim.Region.Physics.OdePlugin
                     // d.Vector3 pos = d.BodyGetPosition(Body);
                     if (_target_velocity.X > 0)
                     {
-                        vec.X = ((_target_velocity.X - vel.X)/1.2f)*PID_D;
+                        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;
+                        vec.Y = ((_target_velocity.Y - vel.Y) / 1.2f) * PID_D;
                     }
                 }
 
@@ -1041,9 +882,10 @@ namespace OpenSim.Region.Physics.OdePlugin
                     vec.Z = (_target_velocity.Z - vel.Z) * (PID_D);
                 }
             }
+
             if (flying)
             {
-                vec.Z += ((-1 * _parent_scene.gravityz)*m_mass);
+                vec.Z += ((-1 * _parent_scene.gravityz) * m_mass);
 
                 //Added for auto fly height. Kitto Flora
                 //d.Vector3 pos = d.BodyGetPosition(Body);
@@ -1055,112 +897,98 @@ namespace OpenSim.Region.Physics.OdePlugin
                 }
                 // end add Kitto Flora
             }
+
             if (vec.IsFinite())
             {
-                doForce(vec);
+                // Apply the total force acting on this avatar
+                d.BodyAddForce(Body, vec.X, vec.Y, vec.Z);
+
                 if (!_zeroFlag)
-                {
-                  AlignAvatarTiltWithCurrentDirectionOfMovement(vec);
-                }
+                    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);
+                m_log.WarnFormat(
+                    "[ODE CHARACTER]: Got a NaN force vector {0} in Move() for {1}.  Removing character from physics scene.",
+                    vec, Name);
 
-                    Body = IntPtr.Zero;
-                }
+                defects.Add(this);
 
-                if (Shell != IntPtr.Zero)
-                {
-                    d.GeomDestroy(Shell);
-                    _parent_scene.geom_name_map.Remove(Shell);
-                    Shell = IntPtr.Zero;
-                }
+                return;
             }
         }
 
         /// <summary>
         /// Updates the reported position and velocity.  This essentially sends the data up to ScenePresence.
         /// </summary>
-        public void UpdatePositionAndVelocity()
+        /// <param name="defects">The character will be added to this list if there is something wrong (non-finite
+        /// position or velocity).
+        /// </param>
+        internal void UpdatePositionAndVelocity(List<OdeCharacter> defects)
         {
             //  no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit!
-            d.Vector3 vec;
+            d.Vector3 newPos;
             try
             {
-                vec = d.BodyGetPosition(Body);
+                newPos = d.BodyGetPosition(Body);
             }
             catch (NullReferenceException)
             {
                 bad = true;
-                _parent_scene.BadCharacter(this);
-                vec = new d.Vector3(_position.X, _position.Y, _position.Z);
+                defects.Add(this);
+                newPos = new d.Vector3(_position.X, _position.Y, _position.Z);
                 base.RaiseOutOfBounds(_position); // Tells ScenePresence that there's a problem!
-                m_log.WarnFormat("[ODEPLUGIN]: Avatar Null reference for Avatar {0}, physical actor {1}", m_name, m_uuid);
+                m_log.WarnFormat("[ODE CHARACTER]: Avatar Null reference for Avatar {0}, physical actor {1}", Name, m_uuid);
+
+                return;
             }
-            
 
             //  kluge to keep things in bounds.  ODE lets dead avatars drift away (they should be removed!)
-            if (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;
+            if (newPos.X < 0.0f) newPos.X = 0.0f;
+            if (newPos.Y < 0.0f) newPos.Y = 0.0f;
+            if (newPos.X > (int)_parent_scene.WorldExtents.X - 0.05f) newPos.X = (int)_parent_scene.WorldExtents.X - 0.05f;
+            if (newPos.Y > (int)_parent_scene.WorldExtents.Y - 0.05f) newPos.Y = (int)_parent_scene.WorldExtents.Y - 0.05f;
+
+            _position.X = newPos.X;
+            _position.Y = newPos.Y;
+            _position.Z = newPos.Z;
 
-            _position.X = vec.X;
-            _position.Y = vec.Y;
-            _position.Z = vec.Z;
+            // I think we need to update the taintPosition too -- Diva 12/24/10
+            m_taintPosition = _position;
 
             // Did we move last? = zeroflag
             // This helps keep us from sliding all over
 
             if (_zeroFlag)
             {
-                _velocity.X = 0.0f;
-                _velocity.Y = 0.0f;
-                _velocity.Z = 0.0f;
+                _velocity = Vector3.Zero;
 
                 // Did we send out the 'stopped' message?
                 if (!m_lastUpdateSent)
                 {
                     m_lastUpdateSent = true;
                     //base.RequestPhysicsterseUpdate();
-
                 }
             }
             else
             {
                 m_lastUpdateSent = false;
+                d.Vector3 newVelocity;
+
                 try
                 {
-                    vec = d.BodyGetLinearVel(Body);
+                    newVelocity = d.BodyGetLinearVel(Body);
                 }
                 catch (NullReferenceException)
                 {
-                    vec.X = _velocity.X;
-                    vec.Y = _velocity.Y;
-                    vec.Z = _velocity.Z;
+                    newVelocity.X = _velocity.X;
+                    newVelocity.Y = _velocity.Y;
+                    newVelocity.Z = _velocity.Z;
                 }
-                _velocity.X = (vec.X);
-                _velocity.Y = (vec.Y);
 
-                _velocity.Z = (vec.Z);
+                _velocity.X = newVelocity.X;
+                _velocity.Y = newVelocity.Y;
+                _velocity.Z = newVelocity.Z;
 
                 if (_velocity.Z < -6 && !m_hackSentFall)
                 {
@@ -1181,14 +1009,164 @@ namespace OpenSim.Region.Physics.OdePlugin
         }
 
         /// <summary>
+        /// This creates the Avatar's physical Surrogate in ODE at the position supplied
+        /// </summary>
+        /// <remarks>
+        /// WARNING: This MUST NOT be called outside of ProcessTaints, else we can have unsynchronized access
+        /// to ODE internals. ProcessTaints is called from within thread-locked Simulate(), so it is the only
+        /// place that is safe to call this routine AvatarGeomAndBodyCreation.
+        /// </remarks>
+        /// <param name="npositionX"></param>
+        /// <param name="npositionY"></param>
+        /// <param name="npositionZ"></param>
+        /// <param name="tensor"></param>
+        private void CreateOdeStructures(float npositionX, float npositionY, float npositionZ, float tensor)
+        {
+            int dAMotorEuler = 1;
+//            _parent_scene.waitForSpaceUnlock(_parent_scene.space);
+            if (CAPSULE_LENGTH <= 0)
+            {
+                m_log.Warn("[ODE CHARACTER]: The capsule size you specified in opensim.ini is invalid!  Setting it to the smallest possible size!");
+                CAPSULE_LENGTH = 0.01f;
+            }
+
+            if (CAPSULE_RADIUS <= 0)
+            {
+                m_log.Warn("[ODE CHARACTER]: The capsule size you specified in opensim.ini is invalid!  Setting it to the smallest possible size!");
+                CAPSULE_RADIUS = 0.01f;
+            }
+
+            Shell = d.CreateCapsule(_parent_scene.space, CAPSULE_RADIUS, CAPSULE_LENGTH);
+
+            d.GeomSetCategoryBits(Shell, (int)m_collisionCategories);
+            d.GeomSetCollideBits(Shell, (int)m_collisionFlags);
+
+            d.MassSetCapsuleTotal(out ShellMass, m_mass, 2, CAPSULE_RADIUS, CAPSULE_LENGTH);
+            Body = d.BodyCreate(_parent_scene.world);
+            d.BodySetPosition(Body, npositionX, npositionY, npositionZ);
+
+            _position.X = npositionX;
+            _position.Y = npositionY;
+            _position.Z = npositionZ;
+
+            m_taintPosition = _position;
+
+            d.BodySetMass(Body, ref ShellMass);
+            d.Matrix3 m_caprot;
+            // 90 Stand up on the cap of the capped cyllinder
+            if (_parent_scene.IsAvCapsuleTilted)
+            {
+                d.RFromAxisAndAngle(out m_caprot, 1, 0, 1, (float)(Math.PI / 2));
+            }
+            else
+            {
+                d.RFromAxisAndAngle(out m_caprot, 0, 0, 1, (float)(Math.PI / 2));
+            }
+
+            d.GeomSetRotation(Shell, ref m_caprot);
+            d.BodySetRotation(Body, ref m_caprot);
+
+            d.GeomSetBody(Shell, Body);
+
+            // The purpose of the AMotor here is to keep the avatar's physical
+            // surrogate from rotating while moving
+            Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero);
+            d.JointAttach(Amotor, Body, IntPtr.Zero);
+            d.JointSetAMotorMode(Amotor, dAMotorEuler);
+            d.JointSetAMotorNumAxes(Amotor, 3);
+            d.JointSetAMotorAxis(Amotor, 0, 0, 1, 0, 0);
+            d.JointSetAMotorAxis(Amotor, 1, 0, 0, 1, 0);
+            d.JointSetAMotorAxis(Amotor, 2, 0, 0, 0, 1);
+            d.JointSetAMotorAngle(Amotor, 0, 0);
+            d.JointSetAMotorAngle(Amotor, 1, 0);
+            d.JointSetAMotorAngle(Amotor, 2, 0);
+
+            // These lowstops and high stops are effectively (no wiggle room)
+            if (_parent_scene.IsAvCapsuleTilted)
+            {
+                d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, -0.000000000001f);
+                d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -0.000000000001f);
+                d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, -0.000000000001f);
+                d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0.000000000001f);
+                d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0.000000000001f);
+                d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0.000000000001f);
+            }
+            else
+            {
+                #region Documentation of capsule motor LowStop and HighStop parameters
+                // Intentionally introduce some tilt into the capsule by setting
+                // the motor stops to small epsilon values. This small tilt prevents
+                // the capsule from falling into the terrain; a straight-up capsule
+                // (with -0..0 motor stops) falls into the terrain for reasons yet
+                // to be comprehended in their entirety.
+                #endregion
+                AlignAvatarTiltWithCurrentDirectionOfMovement(Vector3.Zero);
+                d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, 0.08f);
+                d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -0f);
+                d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, 0.08f);
+                d.JointSetAMotorParam(Amotor, (int)dParam.HiStop,  0.08f); // must be same as lowstop, else a different, spurious tilt is introduced
+                d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0f); // same as lowstop
+                d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0.08f); // same as lowstop
+            }
+
+            // Fudge factor is 1f by default, we're setting it to 0.  We don't want it to Fudge or the
+            // capped cyllinder will fall over
+            d.JointSetAMotorParam(Amotor, (int)dParam.FudgeFactor, 0f);
+            d.JointSetAMotorParam(Amotor, (int)dParam.FMax, tensor);
+
+            //d.Matrix3 bodyrotation = d.BodyGetRotation(Body);
+            //d.QfromR(
+            //d.Matrix3 checkrotation = new d.Matrix3(0.7071068,0.5, -0.7071068,
+            //
+            //m_log.Info("[PHYSICSAV]: Rotation: " + bodyrotation.M00 + " : " + bodyrotation.M01 + " : " + bodyrotation.M02 + " : " + bodyrotation.M10 + " : " + bodyrotation.M11 + " : " + bodyrotation.M12 + " : " + bodyrotation.M20 + " : " + bodyrotation.M21 + " : " + bodyrotation.M22);
+            //standupStraight();
+
+            _parent_scene.geom_name_map[Shell] = Name;
+            _parent_scene.actor_name_map[Shell] = this;
+        }
+
+        /// <summary>
         /// Cleanup the things we use in the scene.
         /// </summary>
-        public void Destroy()
+        internal void Destroy()
         {
             m_tainted_isPhysical = false;
             _parent_scene.AddPhysicsActorTaint(this);
         }
 
+        /// <summary>
+        /// Used internally to destroy the ODE structures associated with this character.
+        /// </summary>
+        internal void DestroyOdeStructures()
+        {
+            // 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);
+                _parent_scene.actor_name_map.Remove(Shell);
+
+                Shell = IntPtr.Zero;
+            }
+        }
+
         public override void CrossingFailure()
         {
         }
@@ -1214,34 +1192,40 @@ namespace OpenSim.Region.Physics.OdePlugin
         {
             m_requestedUpdateFrequency = ms;
             m_eventsubscription = ms;
-            _parent_scene.addCollisionEventReporting(this);
+            CollisionEventsThisFrame.Clear();
+            _parent_scene.AddCollisionEventReporting(this);
         }
+
         public override void UnSubscribeEvents()
         {
-            _parent_scene.remCollisionEventReporting(this);
+            CollisionEventsThisFrame.Clear();
+            _parent_scene.RemoveCollisionEventReporting(this);
             m_requestedUpdateFrequency = 0;
             m_eventsubscription = 0;
         }
-        public void AddCollisionEvent(uint CollidedWith, ContactPoint contact)
+
+        internal void AddCollisionEvent(uint CollidedWith, ContactPoint contact)
         {
             if (m_eventsubscription > 0)
             {
-                CollisionEventsThisFrame.addCollider(CollidedWith, contact);
+//                m_log.DebugFormat(
+//                    "[PHYSICS]: Adding collision event for {0}, collidedWith {1}, contact {2}", "", CollidedWith, contact);
+
+                CollisionEventsThisFrame.AddCollider(CollidedWith, contact);
             }
         }
 
-        public void SendCollisions()
+        internal void SendCollisions()
         {
             if (m_eventsubscription > m_requestedUpdateFrequency)
             {
-                if (CollisionEventsThisFrame != null)
-                {
-                    base.SendCollisionUpdate(CollisionEventsThisFrame);
-                }
-                CollisionEventsThisFrame = new CollisionEventUpdate();
+                base.SendCollisionUpdate(CollisionEventsThisFrame);
+
+                CollisionEventsThisFrame.Clear();
                 m_eventsubscription = 0;
             }
         }
+
         public override bool SubscribedEvents()
         {
             if (m_eventsubscription > 0)
@@ -1249,8 +1233,32 @@ namespace OpenSim.Region.Physics.OdePlugin
             return false;
         }
 
-        public void ProcessTaints(float timestep)
+        internal void ProcessTaints()
         {
+            if (m_taintPosition != _position)
+            {
+                if (Body != IntPtr.Zero)
+                {
+                    d.BodySetPosition(Body, m_taintPosition.X, m_taintPosition.Y, m_taintPosition.Z);
+                    _position = m_taintPosition;
+                }
+            }
+
+            if (m_taintForce != Vector3.Zero)
+            {
+                if (Body != IntPtr.Zero)
+                {
+                    // FIXME: This is not a good solution since it's subject to a race condition if a force is another
+                    // thread sets a new force while we're in this loop (since it could be obliterated by
+                    // m_taintForce = Vector3.Zero.  Need to lock ProcessTaints() when we set a new tainted force.
+                    d.BodyAddForce(Body, m_taintForce.X, m_taintForce.Y, m_taintForce.Z);
+                }
+
+                m_taintForce = Vector3.Zero;
+            }
+
+            if (m_taintTargetVelocity != _target_velocity)
+                _target_velocity = m_taintTargetVelocity;
 
             if (m_tainted_isPhysical != m_isPhysical)
             {
@@ -1259,45 +1267,19 @@ namespace OpenSim.Region.Physics.OdePlugin
                     // 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 - "
+                        m_log.Warn("[ODE CHARACTER]: re-creating the following avatar ODE data for " + Name + ", 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;
+
+                    CreateOdeStructures(_position.X, _position.Y, _position.Z, m_tensor);
                     _parent_scene.AddCharacter(this);
                 }
                 else
                 {
                     _parent_scene.RemoveCharacter(this);
-                    // destroy avatar capsule and related ODE data
-                    if (Amotor != IntPtr.Zero)
-                    {
-                        // Kill the Amotor
-                        d.JointDestroy(Amotor);
-                        Amotor = IntPtr.Zero;
-                    }
-                    //kill the Geometry
-                    _parent_scene.waitForSpaceUnlock(_parent_scene.space);
-
-                    if (Body != IntPtr.Zero)
-                    {
-                        //kill the body
-                        d.BodyDestroy(Body);
-
-                        Body = IntPtr.Zero;
-                    }
-
-                    if (Shell != IntPtr.Zero)
-                    {
-                        d.GeomDestroy(Shell);
-                        _parent_scene.geom_name_map.Remove(Shell);
-                        Shell = IntPtr.Zero;
-                    }
-
+                    DestroyOdeStructures();
                 }
 
                 m_isPhysical = m_tainted_isPhysical;
@@ -1307,43 +1289,35 @@ namespace OpenSim.Region.Physics.OdePlugin
             {
                 if (Shell != IntPtr.Zero && Body != IntPtr.Zero && Amotor != IntPtr.Zero)
                 {
-
+//                    m_log.DebugFormat(
+//                        "[ODE CHARACTER]: Changing capsule size from {0} to {1} for {2}",
+//                        CAPSULE_LENGTH, m_tainted_CAPSULE_LENGTH, Name);
+                    
                     m_pidControllerActive = true;
+
                     // no lock needed on _parent_scene.OdeLock because we are called from within the thread lock in OdePlugin's simulate()
-                    d.JointDestroy(Amotor);
+                    DestroyOdeStructures();
+
                     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;
+
+                    CreateOdeStructures(
+                        _position.X,
+                        _position.Y,
+                        _position.Z + (Math.Abs(CAPSULE_LENGTH - prevCapsule) * 2), m_tensor);
+
+                    // As with Size, we reset velocity.  However, this isn't strictly necessary since it doesn't
+                    // appear to stall initial region crossings when done here.  Being done for consistency.
+//                    Velocity = Vector3.Zero;
                 }
                 else
                 {
-                    m_log.Warn("[PHYSICS]: trying to change capsule size, but the following ODE data is missing - " 
+                    m_log.Warn("[ODE CHARACTER]: trying to change capsule size for " + Name + ", but the following ODE data is missing - "
                         + (Shell==IntPtr.Zero ? "Shell ":"")
                         + (Body==IntPtr.Zero ? "Body ":"")
                         + (Amotor==IntPtr.Zero ? "Amotor ":""));
                 }
             }
-
-            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)
@@ -1354,4 +1328,4 @@ namespace OpenSim.Region.Physics.OdePlugin
             m_eventsubscription += p;
         }
     }
-}
+}
\ No newline at end of file