1 files changed, 137 insertions, 295 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
index 90c2d9c..48f842e 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
@@ -1,4 +1,4 @@
-/*
+/*
  * Copyright (c) Contributors, http://opensimulator.org/
  * See CONTRIBUTORS.TXT for a full list of copyright holders.
  *
@@ -46,9 +46,6 @@ public sealed class BSCharacter : BSPhysObject
     private OMV.Vector3 _position;
     private float _mass;
     private float _avatarVolume;
-    private OMV.Vector3 _force;
-    private OMV.Vector3 _velocity;
-    private OMV.Vector3 _torque;
     private float _collisionScore;
     private OMV.Vector3 _acceleration;
     private OMV.Quaternion _orientation;
@@ -62,15 +59,12 @@ public sealed class BSCharacter : BSPhysObject
     private bool _kinematic;
     private float _buoyancy;
 
-    private BSVMotor _velocityMotor;
+    private BSActorAvatarMove m_moveActor;
+    private const string AvatarMoveActorName = "BSCharacter.AvatarMove";
 
     private OMV.Vector3 _PIDTarget;
     private bool _usePID;
     private float _PIDTau;
-    private bool _useHoverPID;
-    private float _PIDHoverHeight;
-    private PIDHoverType _PIDHoverType;
-    private float _PIDHoverTao;
 
     public BSCharacter(uint localID, String avName, BSScene parent_scene, OMV.Vector3 pos, OMV.Vector3 size, bool isFlying)
             : base(parent_scene, localID, avName, "BSCharacter")
@@ -80,7 +74,7 @@ public sealed class BSCharacter : BSPhysObject
 
         _flying = isFlying;
         _orientation = OMV.Quaternion.Identity;
-        _velocity = OMV.Vector3.Zero;
+        RawVelocity = OMV.Vector3.Zero;
         _buoyancy = ComputeBuoyancyFromFlying(isFlying);
         Friction = BSParam.AvatarStandingFriction;
         Density = BSParam.AvatarDensity / BSParam.DensityScaleFactor;
@@ -97,17 +91,22 @@ public sealed class BSCharacter : BSPhysObject
         // set _avatarVolume and _mass based on capsule size, _density and Scale
         ComputeAvatarVolumeAndMass();
 
-        SetupMovementMotor();
+        // The avatar's movement is controlled by this motor that speeds up and slows down
+        //    the avatar seeking to reach the motor's target speed.
+        // This motor runs as a prestep action for the avatar so it will keep the avatar
+        //    standing as well as moving. Destruction of the avatar will destroy the pre-step action.
+        m_moveActor = new BSActorAvatarMove(PhysScene, this, AvatarMoveActorName);
+        PhysicalActors.Add(AvatarMoveActorName, m_moveActor);
 
         DetailLog("{0},BSCharacter.create,call,size={1},scale={2},density={3},volume={4},mass={5}",
                             LocalID, _size, Scale, Density, _avatarVolume, RawMass);
 
         // do actual creation in taint time
-        PhysicsScene.TaintedObject("BSCharacter.create", delegate()
+        PhysScene.TaintedObject("BSCharacter.create", delegate()
         {
             DetailLog("{0},BSCharacter.create,taint", LocalID);
             // New body and shape into PhysBody and PhysShape
-            PhysicsScene.Shapes.GetBodyAndShape(true, PhysicsScene.World, this);
+            PhysScene.Shapes.GetBodyAndShape(true, PhysScene.World, this);
 
             SetPhysicalProperties();
         });
@@ -120,214 +119,63 @@ public sealed class BSCharacter : BSPhysObject
         base.Destroy();
 
         DetailLog("{0},BSCharacter.Destroy", LocalID);
-        PhysicsScene.TaintedObject("BSCharacter.destroy", delegate()
+        PhysScene.TaintedObject("BSCharacter.destroy", delegate()
         {
-            PhysicsScene.Shapes.DereferenceBody(PhysBody, null /* bodyCallback */);
+            PhysScene.Shapes.DereferenceBody(PhysBody, null /* bodyCallback */);
             PhysBody.Clear();
-            PhysicsScene.Shapes.DereferenceShape(PhysShape, null /* bodyCallback */);
-            PhysShape.Clear();
+            PhysShape.Dereference(PhysScene);
+            PhysShape = new BSShapeNull();
         });
     }
 
     private void SetPhysicalProperties()
     {
-        PhysicsScene.PE.RemoveObjectFromWorld(PhysicsScene.World, PhysBody);
+        PhysScene.PE.RemoveObjectFromWorld(PhysScene.World, PhysBody);
 
         ZeroMotion(true);
         ForcePosition = _position;
 
         // Set the velocity
-        _velocityMotor.Reset();
-        _velocityMotor.SetTarget(_velocity);
-        _velocityMotor.SetCurrent(_velocity);
-        ForceVelocity = _velocity;
+        if (m_moveActor != null)
+            m_moveActor.SetVelocityAndTarget(RawVelocity, RawVelocity, false);
+
+        ForceVelocity = RawVelocity;
 
         // This will enable or disable the flying buoyancy of the avatar.
         // Needs to be reset especially when an avatar is recreated after crossing a region boundry.
         Flying = _flying;
 
-        PhysicsScene.PE.SetRestitution(PhysBody, BSParam.AvatarRestitution);
-        PhysicsScene.PE.SetMargin(PhysShape, PhysicsScene.Params.collisionMargin);
-        PhysicsScene.PE.SetLocalScaling(PhysShape, Scale);
-        PhysicsScene.PE.SetContactProcessingThreshold(PhysBody, BSParam.ContactProcessingThreshold);
+        PhysScene.PE.SetRestitution(PhysBody, BSParam.AvatarRestitution);
+        PhysScene.PE.SetMargin(PhysShape.physShapeInfo, PhysScene.Params.collisionMargin);
+        PhysScene.PE.SetLocalScaling(PhysShape.physShapeInfo, Scale);
+        PhysScene.PE.SetContactProcessingThreshold(PhysBody, BSParam.ContactProcessingThreshold);
         if (BSParam.CcdMotionThreshold > 0f)
         {
-            PhysicsScene.PE.SetCcdMotionThreshold(PhysBody, BSParam.CcdMotionThreshold);
-            PhysicsScene.PE.SetCcdSweptSphereRadius(PhysBody, BSParam.CcdSweptSphereRadius);
+            PhysScene.PE.SetCcdMotionThreshold(PhysBody, BSParam.CcdMotionThreshold);
+            PhysScene.PE.SetCcdSweptSphereRadius(PhysBody, BSParam.CcdSweptSphereRadius);
         }
 
         UpdatePhysicalMassProperties(RawMass, false);
 
         // Make so capsule does not fall over
-        PhysicsScene.PE.SetAngularFactorV(PhysBody, OMV.Vector3.Zero);
+        PhysScene.PE.SetAngularFactorV(PhysBody, OMV.Vector3.Zero);
+
+        // The avatar mover sets some parameters.
+        PhysicalActors.Refresh();
 
-        PhysicsScene.PE.AddToCollisionFlags(PhysBody, CollisionFlags.CF_CHARACTER_OBJECT);
+        PhysScene.PE.AddToCollisionFlags(PhysBody, CollisionFlags.CF_CHARACTER_OBJECT);
 
-        PhysicsScene.PE.AddObjectToWorld(PhysicsScene.World, PhysBody);
+        PhysScene.PE.AddObjectToWorld(PhysScene.World, PhysBody);
 
         // PhysicsScene.PE.ForceActivationState(PhysBody, ActivationState.ACTIVE_TAG);
-        PhysicsScene.PE.ForceActivationState(PhysBody, ActivationState.DISABLE_DEACTIVATION);
-        PhysicsScene.PE.UpdateSingleAabb(PhysicsScene.World, PhysBody);
+        PhysScene.PE.ForceActivationState(PhysBody, ActivationState.DISABLE_DEACTIVATION);
+        PhysScene.PE.UpdateSingleAabb(PhysScene.World, PhysBody);
 
         // Do this after the object has been added to the world
         PhysBody.collisionType = CollisionType.Avatar;
-        PhysBody.ApplyCollisionMask(PhysicsScene);
-    }
-
-    // The avatar's movement is controlled by this motor that speeds up and slows down
-    //    the avatar seeking to reach the motor's target speed.
-    // This motor runs as a prestep action for the avatar so it will keep the avatar
-    //    standing as well as moving. Destruction of the avatar will destroy the pre-step action.
-    private void SetupMovementMotor()
-    {
-        // Infinite decay and timescale values so motor only changes current to target values.
-        _velocityMotor = new BSVMotor("BSCharacter.Velocity", 
-                                            0.2f,                       // time scale
-                                            BSMotor.Infinite,           // decay time scale
-                                            BSMotor.InfiniteVector,     // friction timescale
-                                            1f                          // efficiency
-        );
-        // _velocityMotor.PhysicsScene = PhysicsScene; // DEBUG DEBUG so motor will output detail log messages.
-
-        RegisterPreStepAction("BSCharactor.Movement", LocalID, delegate(float timeStep)
-        {
-            // TODO: Decide if the step parameters should be changed depending on the avatar's
-            //     state (flying, colliding, ...). There is code in ODE to do this.
-
-            // COMMENTARY: when the user is making the avatar walk, except for falling, the velocity
-            //   specified for the avatar is the one that should be used. For falling, if the avatar
-            //   is not flying and is not colliding then it is presumed to be falling and the Z
-            //   component is not fooled with (thus allowing gravity to do its thing).
-            // When the avatar is standing, though, the user has specified a velocity of zero and
-            //   the avatar should be standing. But if the avatar is pushed by something in the world
-            //   (raising elevator platform, moving vehicle, ...) the avatar should be allowed to
-            //   move. Thus, the velocity cannot be forced to zero. The problem is that small velocity
-            //   errors can creap in and the avatar will slowly float off in some direction.
-            // So, the problem is that, when an avatar is standing, we cannot tell creaping error
-            //   from real pushing.
-            // The code below uses whether the collider is static or moving to decide whether to zero motion.
-
-            _velocityMotor.Step(timeStep);
-
-            // If we're not supposed to be moving, make sure things are zero.
-            if (_velocityMotor.ErrorIsZero() && _velocityMotor.TargetValue == OMV.Vector3.Zero)
-            {
-                // The avatar shouldn't be moving
-                _velocityMotor.Zero();
-
-                if (IsColliding)
-                {
-                    // If we are colliding with a stationary object, presume we're standing and don't move around
-                    if (!ColliderIsMoving)
-                    {
-                        DetailLog("{0},BSCharacter.MoveMotor,collidingWithStationary,zeroingMotion", LocalID);
-                        ZeroMotion(true /* inTaintTime */);
-                    }
-
-                    // Standing has more friction on the ground
-                    if (Friction != BSParam.AvatarStandingFriction)
-                    {
-                        Friction = BSParam.AvatarStandingFriction;
-                        PhysicsScene.PE.SetFriction(PhysBody, Friction);
-                    }
-                }
-                else
-                {
-                    if (Flying)
-                    {
-                        // Flying and not collising and velocity nearly zero.
-                        ZeroMotion(true /* inTaintTime */);
-                    }
-                }
-
-                DetailLog("{0},BSCharacter.MoveMotor,taint,stopping,target={1},colliding={2}", LocalID, _velocityMotor.TargetValue, IsColliding);
-            }
-            else
-            {
-                // Supposed to be moving.
-                OMV.Vector3 stepVelocity = _velocityMotor.CurrentValue;
-
-                if (Friction != BSParam.AvatarFriction)
-                {
-                    // Probably starting up walking. Set friction to moving friction.
-                    Friction = BSParam.AvatarFriction;
-                    PhysicsScene.PE.SetFriction(PhysBody, Friction);
-                }
-
-                // If falling, we keep the world's downward vector no matter what the other axis specify.
-                // The check for _velocity.Z < 0 makes jumping work (temporary upward force).
-                if (!Flying && !IsColliding)
-                {
-                    if (_velocity.Z < 0)
-                        stepVelocity.Z = _velocity.Z;
-                    // DetailLog("{0},BSCharacter.MoveMotor,taint,overrideStepZWithWorldZ,stepVel={1}", LocalID, stepVelocity);
-                }
-
-                // 'stepVelocity' is now the speed we'd like the avatar to move in. Turn that into an instantanous force.
-                OMV.Vector3 moveForce = (stepVelocity - _velocity) * Mass;
-
-                // Should we check for move force being small and forcing velocity to zero?
-
-                // Add special movement force to allow avatars to walk up stepped surfaces.
-                moveForce += WalkUpStairs();
-
-                DetailLog("{0},BSCharacter.MoveMotor,move,stepVel={1},vel={2},mass={3},moveForce={4}", LocalID, stepVelocity, _velocity, Mass, moveForce);
-                PhysicsScene.PE.ApplyCentralImpulse(PhysBody, moveForce);
-            }
-        });
+        PhysBody.ApplyCollisionMask(PhysScene);
     }
 
-    // Decide if the character is colliding with a low object and compute a force to pop the
-    //    avatar up so it can walk up and over the low objects.
-    private OMV.Vector3 WalkUpStairs()
-    {
-        OMV.Vector3 ret = OMV.Vector3.Zero;
-
-        // This test is done if moving forward, not flying and is colliding with something.
-        // DetailLog("{0},BSCharacter.WalkUpStairs,IsColliding={1},flying={2},targSpeed={3},collisions={4}",
-        //                 LocalID, IsColliding, Flying, TargetSpeed, CollisionsLastTick.Count);
-        if (IsColliding && !Flying && TargetVelocitySpeed > 0.1f /* && ForwardSpeed < 0.1f */)
-        {
-            // The range near the character's feet where we will consider stairs
-            float nearFeetHeightMin = RawPosition.Z - (Size.Z / 2f) + 0.05f;
-            float nearFeetHeightMax = nearFeetHeightMin + BSParam.AvatarStepHeight;
-
-            // Look for a collision point that is near the character's feet and is oriented the same as the charactor is
-            foreach (KeyValuePair<uint, ContactPoint> kvp in CollisionsLastTick.m_objCollisionList)
-            {
-                // Don't care about collisions with the terrain
-                if (kvp.Key > PhysicsScene.TerrainManager.HighestTerrainID)
-                {
-                    OMV.Vector3 touchPosition = kvp.Value.Position;
-                    // DetailLog("{0},BSCharacter.WalkUpStairs,min={1},max={2},touch={3}",
-                    //                 LocalID, nearFeetHeightMin, nearFeetHeightMax, touchPosition);
-                    if (touchPosition.Z >= nearFeetHeightMin && touchPosition.Z <= nearFeetHeightMax)
-                    {
-                        // This contact is within the 'near the feet' range.
-                        // The normal should be our contact point to the object so it is pointing away
-                        //    thus the difference between our facing orientation and the normal should be small.
-                        OMV.Vector3 directionFacing = OMV.Vector3.UnitX * RawOrientation;
-                        OMV.Vector3 touchNormal = OMV.Vector3.Normalize(kvp.Value.SurfaceNormal);
-                        float diff = Math.Abs(OMV.Vector3.Distance(directionFacing, touchNormal));
-                        if (diff < BSParam.AvatarStepApproachFactor)
-                        {
-                            // Found the stairs contact point. Push up a little to raise the character.
-                            float upForce = (touchPosition.Z - nearFeetHeightMin) * Mass * BSParam.AvatarStepForceFactor;
-                            ret = new OMV.Vector3(0f, 0f, upForce);
-
-                            // Also move the avatar up for the new height
-                            OMV.Vector3 displacement = new OMV.Vector3(0f, 0f, BSParam.AvatarStepHeight / 2f);
-                            ForcePosition = RawPosition + displacement;
-                        }
-                        DetailLog("{0},BSCharacter.WalkUpStairs,touchPos={1},nearFeetMin={2},faceDir={3},norm={4},diff={5},ret={6}",
-                                LocalID, touchPosition, nearFeetHeightMin, directionFacing, touchNormal, diff, ret);
-                    }
-                }
-            }
-        }
-
-        return ret;
-    }
 
     public override void RequestPhysicsterseUpdate()
     {
@@ -355,14 +203,14 @@ public sealed class BSCharacter : BSPhysObject
             DetailLog("{0},BSCharacter.setSize,call,size={1},scale={2},density={3},volume={4},mass={5}",
                             LocalID, _size, Scale, Density, _avatarVolume, RawMass);
 
-            PhysicsScene.TaintedObject("BSCharacter.setSize", delegate()
+            PhysScene.TaintedObject("BSCharacter.setSize", delegate()
             {
-                if (PhysBody.HasPhysicalBody && PhysShape.HasPhysicalShape)
+                if (PhysBody.HasPhysicalBody && PhysShape.physShapeInfo.HasPhysicalShape)
                 {
-                    PhysicsScene.PE.SetLocalScaling(PhysShape, Scale);
+                    PhysScene.PE.SetLocalScaling(PhysShape.physShapeInfo, Scale);
                     UpdatePhysicalMassProperties(RawMass, true);
                     // Make sure this change appears as a property update event
-                    PhysicsScene.PE.PushUpdate(PhysBody);
+                    PhysScene.PE.PushUpdate(PhysBody);
                 }
             });
 
@@ -373,11 +221,6 @@ public sealed class BSCharacter : BSPhysObject
     {
         set { BaseShape = value; }
     }
-    // I want the physics engine to make an avatar capsule
-    public override BSPhysicsShapeType PreferredPhysicalShape
-    {
-        get {return BSPhysicsShapeType.SHAPE_CAPSULE; }
-    }
 
     public override bool Grabbed {
         set { _grabbed = value; }
@@ -399,29 +242,29 @@ public sealed class BSCharacter : BSPhysObject
     // Called at taint time!
     public override void ZeroMotion(bool inTaintTime)
     {
-        _velocity = OMV.Vector3.Zero;
+        RawVelocity = OMV.Vector3.Zero;
         _acceleration = OMV.Vector3.Zero;
         _rotationalVelocity = OMV.Vector3.Zero;
 
         // Zero some other properties directly into the physics engine
-        PhysicsScene.TaintedObject(inTaintTime, "BSCharacter.ZeroMotion", delegate()
+        PhysScene.TaintedObject(inTaintTime, "BSCharacter.ZeroMotion", delegate()
         {
             if (PhysBody.HasPhysicalBody)
-                PhysicsScene.PE.ClearAllForces(PhysBody);
+                PhysScene.PE.ClearAllForces(PhysBody);
         });
     }
     public override void ZeroAngularMotion(bool inTaintTime)
     {
         _rotationalVelocity = OMV.Vector3.Zero;
 
-        PhysicsScene.TaintedObject(inTaintTime, "BSCharacter.ZeroMotion", delegate()
+        PhysScene.TaintedObject(inTaintTime, "BSCharacter.ZeroMotion", delegate()
         {
             if (PhysBody.HasPhysicalBody)
             {
-                PhysicsScene.PE.SetInterpolationAngularVelocity(PhysBody, OMV.Vector3.Zero);
-                PhysicsScene.PE.SetAngularVelocity(PhysBody, OMV.Vector3.Zero);
+                PhysScene.PE.SetInterpolationAngularVelocity(PhysBody, OMV.Vector3.Zero);
+                PhysScene.PE.SetAngularVelocity(PhysBody, OMV.Vector3.Zero);
                 // The next also get rid of applied linear force but the linear velocity is untouched.
-                PhysicsScene.PE.ClearForces(PhysBody);
+                PhysScene.PE.ClearForces(PhysBody);
             }
         });
     }
@@ -443,7 +286,7 @@ public sealed class BSCharacter : BSPhysObject
         set {
             _position = value;
 
-            PhysicsScene.TaintedObject("BSCharacter.setPosition", delegate()
+            PhysScene.TaintedObject("BSCharacter.setPosition", delegate()
             {
                 DetailLog("{0},BSCharacter.SetPosition,taint,pos={1},orient={2}", LocalID, _position, _orientation);
                 PositionSanityCheck();
@@ -453,14 +296,14 @@ public sealed class BSCharacter : BSPhysObject
     }
     public override OMV.Vector3 ForcePosition {
         get {
-            _position = PhysicsScene.PE.GetPosition(PhysBody);
+            _position = PhysScene.PE.GetPosition(PhysBody);
             return _position;
         }
         set {
             _position = value;
             if (PhysBody.HasPhysicalBody)
             {
-                PhysicsScene.PE.SetTranslation(PhysBody, _position, _orientation);
+                PhysScene.PE.SetTranslation(PhysBody, _position, _orientation);
             }
         }
     }
@@ -474,18 +317,18 @@ public sealed class BSCharacter : BSPhysObject
         bool ret = false;
 
         // TODO: check for out of bounds
-        if (!PhysicsScene.TerrainManager.IsWithinKnownTerrain(RawPosition))
+        if (!PhysScene.TerrainManager.IsWithinKnownTerrain(RawPosition))
         {
             // The character is out of the known/simulated area.
             // Force the avatar position to be within known. ScenePresence will use the position
             //    plus the velocity to decide if the avatar is moving out of the region.
-            RawPosition = PhysicsScene.TerrainManager.ClampPositionIntoKnownTerrain(RawPosition);
+            RawPosition = PhysScene.TerrainManager.ClampPositionIntoKnownTerrain(RawPosition);
             DetailLog("{0},BSCharacter.PositionSanityCheck,notWithinKnownTerrain,clampedPos={1}", LocalID, RawPosition);
             return true;
         }
 
         // If below the ground, move the avatar up
-        float terrainHeight = PhysicsScene.TerrainManager.GetTerrainHeightAtXYZ(RawPosition);
+        float terrainHeight = PhysScene.TerrainManager.GetTerrainHeightAtXYZ(RawPosition);
         if (Position.Z < terrainHeight)
         {
             DetailLog("{0},BSCharacter.PositionSanityCheck,adjustForUnderGround,pos={1},terrain={2}", LocalID, _position, terrainHeight);
@@ -494,7 +337,7 @@ public sealed class BSCharacter : BSPhysObject
         }
         if ((CurrentCollisionFlags & CollisionFlags.BS_FLOATS_ON_WATER) != 0)
         {
-            float waterHeight = PhysicsScene.TerrainManager.GetWaterLevelAtXYZ(_position);
+            float waterHeight = PhysScene.TerrainManager.GetWaterLevelAtXYZ(_position);
             if (Position.Z < waterHeight)
             {
                 _position.Z = waterHeight;
@@ -515,7 +358,7 @@ public sealed class BSCharacter : BSPhysObject
         {
             // The new position value must be pushed into the physics engine but we can't
             //    just assign to "Position" because of potential call loops.
-            PhysicsScene.TaintedObject(inTaintTime, "BSCharacter.PositionSanityCheck", delegate()
+            PhysScene.TaintedObject(inTaintTime, "BSCharacter.PositionSanityCheck", delegate()
             {
                 DetailLog("{0},BSCharacter.PositionSanityCheck,taint,pos={1},orient={2}", LocalID, _position, _orientation);
                 ForcePosition = _position;
@@ -528,25 +371,25 @@ public sealed class BSCharacter : BSPhysObject
     public override float Mass { get { return _mass; } }
 
     // used when we only want this prim's mass and not the linkset thing
-    public override float RawMass { 
+    public override float RawMass {
         get {return _mass; }
     }
     public override void UpdatePhysicalMassProperties(float physMass, bool inWorld)
     {
-        OMV.Vector3 localInertia = PhysicsScene.PE.CalculateLocalInertia(PhysShape, physMass);
-        PhysicsScene.PE.SetMassProps(PhysBody, physMass, localInertia);
+        OMV.Vector3 localInertia = PhysScene.PE.CalculateLocalInertia(PhysShape.physShapeInfo, physMass);
+        PhysScene.PE.SetMassProps(PhysBody, physMass, localInertia);
     }
 
     public override OMV.Vector3 Force {
-        get { return _force; }
+        get { return RawForce; }
         set {
-            _force = value;
+            RawForce = value;
             // m_log.DebugFormat("{0}: Force = {1}", LogHeader, _force);
-            PhysicsScene.TaintedObject("BSCharacter.SetForce", delegate()
+            PhysScene.TaintedObject("BSCharacter.SetForce", delegate()
             {
-                DetailLog("{0},BSCharacter.setForce,taint,force={1}", LocalID, _force);
+                DetailLog("{0},BSCharacter.setForce,taint,force={1}", LocalID, RawForce);
                 if (PhysBody.HasPhysicalBody)
-                    PhysicsScene.PE.SetObjectForce(PhysBody, _force);
+                    PhysScene.PE.SetObjectForce(PhysBody, RawForce);
             });
         }
     }
@@ -569,61 +412,49 @@ public sealed class BSCharacter : BSPhysObject
     {
         get
         {
-            return m_targetVelocity;
+            return base.m_targetVelocity;
         }
         set
         {
             DetailLog("{0},BSCharacter.setTargetVelocity,call,vel={1}", LocalID, value);
             m_targetVelocity = value;
             OMV.Vector3 targetVel = value;
-            if (_setAlwaysRun)
+            if (_setAlwaysRun && !_flying)
                 targetVel *= new OMV.Vector3(BSParam.AvatarAlwaysRunFactor, BSParam.AvatarAlwaysRunFactor, 0f);
 
-            PhysicsScene.TaintedObject("BSCharacter.setTargetVelocity", delegate()
-            {
-                _velocityMotor.Reset();
-                _velocityMotor.SetTarget(targetVel);
-                _velocityMotor.SetCurrent(_velocity);
-                _velocityMotor.Enabled = true;
-            });
+            if (m_moveActor != null)
+                m_moveActor.SetVelocityAndTarget(RawVelocity, targetVel, false /* inTaintTime */);
         }
     }
-    public override OMV.Vector3 RawVelocity
-    {
-        get { return _velocity; }
-        set { _velocity = value; }
-    }
     // Directly setting velocity means this is what the user really wants now.
     public override OMV.Vector3 Velocity {
-        get { return _velocity; }
+        get { return RawVelocity; }
         set {
-            _velocity = value;
-            // m_log.DebugFormat("{0}: set velocity = {1}", LogHeader, _velocity);
-            PhysicsScene.TaintedObject("BSCharacter.setVelocity", delegate()
+            RawVelocity = value;
+            // m_log.DebugFormat("{0}: set velocity = {1}", LogHeader, RawVelocity);
+            PhysScene.TaintedObject("BSCharacter.setVelocity", delegate()
             {
-                _velocityMotor.Reset();
-                _velocityMotor.SetCurrent(_velocity);
-                _velocityMotor.SetTarget(_velocity);
-                _velocityMotor.Enabled = false;
+                if (m_moveActor != null)
+                    m_moveActor.SetVelocityAndTarget(RawVelocity, RawVelocity, true /* inTaintTime */);
 
-                DetailLog("{0},BSCharacter.setVelocity,taint,vel={1}", LocalID, _velocity);
-                ForceVelocity = _velocity;
+                DetailLog("{0},BSCharacter.setVelocity,taint,vel={1}", LocalID, RawVelocity);
+                ForceVelocity = RawVelocity;
             });
         }
     }
     public override OMV.Vector3 ForceVelocity {
-        get { return _velocity; }
+        get { return RawVelocity; }
         set {
-            PhysicsScene.AssertInTaintTime("BSCharacter.ForceVelocity");
+            PhysScene.AssertInTaintTime("BSCharacter.ForceVelocity");
 
-            _velocity = value;
-            PhysicsScene.PE.SetLinearVelocity(PhysBody, _velocity);
-            PhysicsScene.PE.Activate(PhysBody, true);
+            RawVelocity = value;
+            PhysScene.PE.SetLinearVelocity(PhysBody, RawVelocity);
+            PhysScene.PE.Activate(PhysBody, true);
         }
     }
     public override OMV.Vector3 Torque {
-        get { return _torque; }
-        set { _torque = value;
+        get { return RawTorque; }
+        set { RawTorque = value;
         }
     }
     public override float CollisionScore {
@@ -648,9 +479,19 @@ public sealed class BSCharacter : BSPhysObject
             if (_orientation != value)
             {
                 _orientation = value;
-                PhysicsScene.TaintedObject("BSCharacter.setOrientation", delegate()
+                PhysScene.TaintedObject("BSCharacter.setOrientation", delegate()
                 {
-                    ForceOrientation = _orientation;
+                    // Bullet assumes we know what we are doing when forcing orientation
+                    //    so it lets us go against all the rules and just compensates for them later.
+                    // This forces rotation to be only around the Z axis and doesn't change any of the other axis.
+                    // This keeps us from flipping the capsule over which the veiwer does not understand.
+                    float oRoll, oPitch, oYaw;
+                    _orientation.GetEulerAngles(out oRoll, out oPitch, out oYaw);
+                    OMV.Quaternion trimmedOrientation = OMV.Quaternion.CreateFromEulers(0f, 0f, oYaw);
+                    // DetailLog("{0},BSCharacter.setOrientation,taint,val={1},valDir={2},conv={3},convDir={4}",
+                    //                 LocalID, _orientation, OMV.Vector3.UnitX * _orientation,
+                    //                 trimmedOrientation, OMV.Vector3.UnitX * trimmedOrientation);
+                    ForceOrientation = trimmedOrientation;
                 });
             }
         }
@@ -660,7 +501,7 @@ public sealed class BSCharacter : BSPhysObject
     {
         get
         {
-            _orientation = PhysicsScene.PE.GetOrientation(PhysBody);
+            _orientation = PhysScene.PE.GetOrientation(PhysBody);
             return _orientation;
         }
         set
@@ -669,7 +510,7 @@ public sealed class BSCharacter : BSPhysObject
             if (PhysBody.HasPhysicalBody)
             {
                 // _position = PhysicsScene.PE.GetPosition(BSBody);
-                PhysicsScene.PE.SetTranslation(PhysBody, _position, _orientation);
+                PhysScene.PE.SetTranslation(PhysBody, _position, _orientation);
             }
         }
     }
@@ -718,14 +559,14 @@ public sealed class BSCharacter : BSPhysObject
     public override bool FloatOnWater {
         set {
             _floatOnWater = value;
-            PhysicsScene.TaintedObject("BSCharacter.setFloatOnWater", delegate()
+            PhysScene.TaintedObject("BSCharacter.setFloatOnWater", delegate()
             {
                 if (PhysBody.HasPhysicalBody)
                 {
                     if (_floatOnWater)
-                        CurrentCollisionFlags = PhysicsScene.PE.AddToCollisionFlags(PhysBody, CollisionFlags.BS_FLOATS_ON_WATER);
+                        CurrentCollisionFlags = PhysScene.PE.AddToCollisionFlags(PhysBody, CollisionFlags.BS_FLOATS_ON_WATER);
                     else
-                        CurrentCollisionFlags = PhysicsScene.PE.RemoveFromCollisionFlags(PhysBody, CollisionFlags.BS_FLOATS_ON_WATER);
+                        CurrentCollisionFlags = PhysScene.PE.RemoveFromCollisionFlags(PhysBody, CollisionFlags.BS_FLOATS_ON_WATER);
                 }
             });
         }
@@ -746,7 +587,7 @@ public sealed class BSCharacter : BSPhysObject
     public override float Buoyancy {
         get { return _buoyancy; }
         set { _buoyancy = value;
-            PhysicsScene.TaintedObject("BSCharacter.setBuoyancy", delegate()
+            PhysScene.TaintedObject("BSCharacter.setBuoyancy", delegate()
             {
                 DetailLog("{0},BSCharacter.setBuoyancy,taint,buoy={1}", LocalID, _buoyancy);
                 ForceBuoyancy = _buoyancy;
@@ -755,8 +596,8 @@ public sealed class BSCharacter : BSPhysObject
     }
     public override float ForceBuoyancy {
         get { return _buoyancy; }
-        set { 
-            PhysicsScene.AssertInTaintTime("BSCharacter.ForceBuoyancy");
+        set {
+            PhysScene.AssertInTaintTime("BSCharacter.ForceBuoyancy");
 
             _buoyancy = value;
             DetailLog("{0},BSCharacter.setForceBuoyancy,taint,buoy={1}", LocalID, _buoyancy);
@@ -764,7 +605,7 @@ public sealed class BSCharacter : BSPhysObject
             float  grav = BSParam.Gravity * (1f - _buoyancy);
             Gravity = new OMV.Vector3(0f, 0f, grav);
             if (PhysBody.HasPhysicalBody)
-                PhysicsScene.PE.SetGravity(PhysBody, Gravity);
+                PhysScene.PE.SetGravity(PhysBody, Gravity);
         }
     }
 
@@ -779,31 +620,10 @@ public sealed class BSCharacter : BSPhysObject
         set { _PIDTau = value; }
     }
 
-    // Used for llSetHoverHeight and maybe vehicle height
-    // Hover Height will override MoveTo target's Z
-    public override bool PIDHoverActive {
-        set { _useHoverPID = value; }
-    }
-    public override float PIDHoverHeight {
-        set { _PIDHoverHeight = value; }
-    }
-    public override PIDHoverType PIDHoverType {
-        set { _PIDHoverType = value; }
-    }
-    public override float PIDHoverTau {
-        set { _PIDHoverTao = value; }
-    }
-
-    // For RotLookAt
-    public override OMV.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 AddForce(OMV.Vector3 force, bool pushforce)
     {
         // Since this force is being applied in only one step, make this a force per second.
-        OMV.Vector3 addForce = force / PhysicsScene.LastTimeStep;
+        OMV.Vector3 addForce = force / PhysScene.LastTimeStep;
         AddForce(addForce, pushforce, false);
     }
     private void AddForce(OMV.Vector3 force, bool pushforce, bool inTaintTime) {
@@ -812,13 +632,13 @@ public sealed class BSCharacter : BSPhysObject
             OMV.Vector3 addForce = Util.ClampV(force, BSParam.MaxAddForceMagnitude);
             // DetailLog("{0},BSCharacter.addForce,call,force={1}", LocalID, addForce);
 
-            PhysicsScene.TaintedObject(inTaintTime, "BSCharacter.AddForce", delegate()
+            PhysScene.TaintedObject(inTaintTime, "BSCharacter.AddForce", delegate()
             {
                 // Bullet adds this central force to the total force for this tick
                 // DetailLog("{0},BSCharacter.addForce,taint,force={1}", LocalID, addForce);
                 if (PhysBody.HasPhysicalBody)
                 {
-                    PhysicsScene.PE.ApplyCentralForce(PhysBody, addForce);
+                    PhysScene.PE.ApplyCentralForce(PhysBody, addForce);
                 }
             });
         }
@@ -829,7 +649,7 @@ public sealed class BSCharacter : BSPhysObject
         }
     }
 
-    public override void AddAngularForce(OMV.Vector3 force, bool pushforce) {
+    public override void AddAngularForce(OMV.Vector3 force, bool pushforce, bool inTaintTime) {
     }
     public override void SetMomentum(OMV.Vector3 momentum) {
     }
@@ -837,14 +657,14 @@ public sealed class BSCharacter : BSPhysObject
     private OMV.Vector3 ComputeAvatarScale(OMV.Vector3 size)
     {
         OMV.Vector3 newScale;
-        
+
         // Bullet's capsule total height is the "passed height + radius * 2";
-        // The base capsule is 1 diameter and 2 height (passed radius=0.5, passed height = 1)
+        // The base capsule is 1 unit in diameter and 2 units in height (passed radius=0.5, passed height = 1)
         // The number we pass in for 'scaling' is the multiplier to get that base
         //     shape to be the size desired.
         // So, when creating the scale for the avatar height, we take the passed height
         //     (size.Z) and remove the caps.
-        // Another oddity of the Bullet capsule implementation is that it presumes the Y
+        // An oddity of the Bullet capsule implementation is that it presumes the Y
         //     dimension is the radius of the capsule. Even though some of the code allows
         //     for a asymmetrical capsule, other parts of the code presume it is cylindrical.
 
@@ -852,8 +672,27 @@ public sealed class BSCharacter : BSPhysObject
         newScale.X = size.X / 2f;
         newScale.Y = size.Y / 2f;
 
+        float heightAdjust = BSParam.AvatarHeightMidFudge;
+        if (BSParam.AvatarHeightLowFudge != 0f || BSParam.AvatarHeightHighFudge != 0f)
+        {
+            // An avatar is between 1.61 and 2.12 meters. Midpoint is 1.87m.
+            // The "times 4" relies on the fact that the difference from the midpoint to the extremes is exactly 0.25
+            float midHeightOffset = size.Z - 1.87f;
+            if (midHeightOffset < 0f)
+            {
+                // Small avatar. Add the adjustment based on the distance from midheight
+                heightAdjust += -1f * midHeightOffset * 4f * BSParam.AvatarHeightLowFudge;
+            }
+            else
+            {
+                // Large avatar. Add the adjustment based on the distance from midheight
+                heightAdjust += midHeightOffset * 4f * BSParam.AvatarHeightHighFudge;
+            }
+        }
         // The total scale height is the central cylindar plus the caps on the two ends.
-        newScale.Z = (size.Z + (Math.Min(size.X, size.Y) * 2)) / 2f;
+        newScale.Z = (size.Z + (Math.Min(size.X, size.Y) * 2) + heightAdjust) / 2f;
+        // m_log.DebugFormat("{0} ComputeAvatarScale: size={1},adj={2},scale={3}", LogHeader, size, heightAdjust, newScale);
+
         // If smaller than the endcaps, just fake like we're almost that small
         if (newScale.Z < 0)
             newScale.Z = 0.1f;
@@ -882,15 +721,18 @@ public sealed class BSCharacter : BSPhysObject
     // the world that things have changed.
     public override void UpdateProperties(EntityProperties entprop)
     {
-        _position = entprop.Position;
+        // Don't change position if standing on a stationary object.
+        if (!IsStationary)
+            _position = entprop.Position;
+
         _orientation = entprop.Rotation;
 
         // Smooth velocity. OpenSimulator is VERY sensitive to changes in velocity of the avatar
         //    and will send agent updates to the clients if velocity changes by more than
         //    0.001m/s. Bullet introduces a lot of jitter in the velocity which causes many
         //    extra updates.
-        if (!entprop.Velocity.ApproxEquals(_velocity, 0.1f))
-            _velocity = entprop.Velocity;
+        if (!entprop.Velocity.ApproxEquals(RawVelocity, 0.1f))
+            RawVelocity = entprop.Velocity;
 
         _acceleration = entprop.Acceleration;
         _rotationalVelocity = entprop.RotationalVelocity;
@@ -913,7 +755,7 @@ public sealed class BSCharacter : BSPhysObject
         // base.RequestPhysicsterseUpdate();
 
         DetailLog("{0},BSCharacter.UpdateProperties,call,pos={1},orient={2},vel={3},accel={4},rotVel={5}",
-                LocalID, _position, _orientation, _velocity, _acceleration, _rotationalVelocity);
+                LocalID, _position, _orientation, RawVelocity, _acceleration, _rotationalVelocity);
     }
 }
 }