5 files changed, 89 insertions, 116 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
index 5ef6992..c9e3ca0 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
@@ -43,12 +43,10 @@ public sealed class BSCharacter : BSPhysObject
     private OMV.Vector3 _size;
     private bool _grabbed;
     private bool _selected;
-    private OMV.Vector3 _position;
     private float _mass;
     private float _avatarVolume;
     private float _collisionScore;
     private OMV.Vector3 _acceleration;
-    private OMV.Quaternion _orientation;
     private int _physicsActorType;
     private bool _isPhysical;
     private bool _flying;
@@ -70,10 +68,10 @@ public sealed class BSCharacter : BSPhysObject
             : base(parent_scene, localID, avName, "BSCharacter")
     {
         _physicsActorType = (int)ActorTypes.Agent;
-        _position = pos;
+        RawPosition = pos;
 
         _flying = isFlying;
-        _orientation = OMV.Quaternion.Identity;
+        RawOrientation = OMV.Quaternion.Identity;
         RawVelocity = OMV.Vector3.Zero;
         _buoyancy = ComputeBuoyancyFromFlying(isFlying);
         Friction = BSParam.AvatarStandingFriction;
@@ -133,7 +131,7 @@ public sealed class BSCharacter : BSPhysObject
         PhysScene.PE.RemoveObjectFromWorld(PhysScene.World, PhysBody);
 
         ZeroMotion(true);
-        ForcePosition = _position;
+        ForcePosition = RawPosition;
 
         // Set the velocity
         if (m_moveActor != null)
@@ -272,38 +270,33 @@ public sealed class BSCharacter : BSPhysObject
 
     public override void LockAngularMotion(OMV.Vector3 axis) { return; }
 
-    public override OMV.Vector3 RawPosition
-    {
-        get { return _position; }
-        set { _position = value; }
-    }
     public override OMV.Vector3 Position {
         get {
             // Don't refetch the position because this function is called a zillion times
-            // _position = PhysicsScene.PE.GetObjectPosition(Scene.World, LocalID);
-            return _position;
+            // RawPosition = PhysicsScene.PE.GetObjectPosition(Scene.World, LocalID);
+            return RawPosition;
         }
         set {
-            _position = value;
+            RawPosition = value;
 
             PhysScene.TaintedObject("BSCharacter.setPosition", delegate()
             {
-                DetailLog("{0},BSCharacter.SetPosition,taint,pos={1},orient={2}", LocalID, _position, _orientation);
+                DetailLog("{0},BSCharacter.SetPosition,taint,pos={1},orient={2}", LocalID, RawPosition, RawOrientation);
                 PositionSanityCheck();
-                ForcePosition = _position;
+                ForcePosition = RawPosition;
             });
         }
     }
     public override OMV.Vector3 ForcePosition {
         get {
-            _position = PhysScene.PE.GetPosition(PhysBody);
-            return _position;
+            RawPosition = PhysScene.PE.GetPosition(PhysBody);
+            return RawPosition;
         }
         set {
-            _position = value;
+            RawPosition = value;
             if (PhysBody.HasPhysicalBody)
             {
-                PhysScene.PE.SetTranslation(PhysBody, _position, _orientation);
+                PhysScene.PE.SetTranslation(PhysBody, RawPosition, RawOrientation);
             }
         }
     }
@@ -331,16 +324,16 @@ public sealed class BSCharacter : BSPhysObject
         float terrainHeight = PhysScene.TerrainManager.GetTerrainHeightAtXYZ(RawPosition);
         if (Position.Z < terrainHeight)
         {
-            DetailLog("{0},BSCharacter.PositionSanityCheck,adjustForUnderGround,pos={1},terrain={2}", LocalID, _position, terrainHeight);
-            _position.Z = terrainHeight + BSParam.AvatarBelowGroundUpCorrectionMeters;
+            DetailLog("{0},BSCharacter.PositionSanityCheck,adjustForUnderGround,pos={1},terrain={2}", LocalID, RawPosition, terrainHeight);
+            RawPosition = new OMV.Vector3(RawPosition.X, RawPosition.Y, terrainHeight + BSParam.AvatarBelowGroundUpCorrectionMeters);
             ret = true;
         }
         if ((CurrentCollisionFlags & CollisionFlags.BS_FLOATS_ON_WATER) != 0)
         {
-            float waterHeight = PhysScene.TerrainManager.GetWaterLevelAtXYZ(_position);
+            float waterHeight = PhysScene.TerrainManager.GetWaterLevelAtXYZ(RawPosition);
             if (Position.Z < waterHeight)
             {
-                _position.Z = waterHeight;
+                RawPosition = new OMV.Vector3(RawPosition.X, RawPosition.Y, waterHeight);
                 ret = true;
             }
         }
@@ -360,8 +353,8 @@ public sealed class BSCharacter : BSPhysObject
             //    just assign to "Position" because of potential call loops.
             PhysScene.TaintedObject(inTaintTime, "BSCharacter.PositionSanityCheck", delegate()
             {
-                DetailLog("{0},BSCharacter.PositionSanityCheck,taint,pos={1},orient={2}", LocalID, _position, _orientation);
-                ForcePosition = _position;
+                DetailLog("{0},BSCharacter.PositionSanityCheck,taint,pos={1},orient={2}", LocalID, RawPosition, RawOrientation);
+                ForcePosition = RawPosition;
             });
             ret = true;
         }
@@ -466,19 +459,14 @@ public sealed class BSCharacter : BSPhysObject
         get { return _acceleration; }
         set { _acceleration = value; }
     }
-    public override OMV.Quaternion RawOrientation
-    {
-        get { return _orientation; }
-        set { _orientation = value; }
-    }
     public override OMV.Quaternion Orientation {
-        get { return _orientation; }
+        get { return RawOrientation; }
         set {
             // Orientation is set zillions of times when an avatar is walking. It's like
             //      the viewer doesn't trust us.
-            if (_orientation != value)
+            if (RawOrientation != value)
             {
-                _orientation = value;
+                RawOrientation = value;
                 PhysScene.TaintedObject("BSCharacter.setOrientation", delegate()
                 {
                     // Bullet assumes we know what we are doing when forcing orientation
@@ -486,10 +474,10 @@ public sealed class BSCharacter : BSPhysObject
                     // 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);
+                    RawOrientation.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,
+                    //                 LocalID, RawOrientation, OMV.Vector3.UnitX * RawOrientation,
                     //                 trimmedOrientation, OMV.Vector3.UnitX * trimmedOrientation);
                     ForceOrientation = trimmedOrientation;
                 });
@@ -501,16 +489,16 @@ public sealed class BSCharacter : BSPhysObject
     {
         get
         {
-            _orientation = PhysScene.PE.GetOrientation(PhysBody);
-            return _orientation;
+            RawOrientation = PhysScene.PE.GetOrientation(PhysBody);
+            return RawOrientation;
         }
         set
         {
-            _orientation = value;
+            RawOrientation = value;
             if (PhysBody.HasPhysicalBody)
             {
-                // _position = PhysicsScene.PE.GetPosition(BSBody);
-                PhysScene.PE.SetTranslation(PhysBody, _position, _orientation);
+                // RawPosition = PhysicsScene.PE.GetPosition(BSBody);
+                PhysScene.PE.SetTranslation(PhysBody, RawPosition, RawOrientation);
             }
         }
     }
@@ -723,9 +711,9 @@ public sealed class BSCharacter : BSPhysObject
     {
         // Don't change position if standing on a stationary object.
         if (!IsStationary)
-            _position = entprop.Position;
+            RawPosition = entprop.Position;
 
-        _orientation = entprop.Rotation;
+        RawOrientation = 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
@@ -740,8 +728,8 @@ public sealed class BSCharacter : BSPhysObject
         // Do some sanity checking for the avatar. Make sure it's above ground and inbounds.
         if (PositionSanityCheck(true))
         {
-            DetailLog("{0},BSCharacter.UpdateProperties,updatePosForSanity,pos={1}", LocalID, _position);
-            entprop.Position = _position;
+            DetailLog("{0},BSCharacter.UpdateProperties,updatePosForSanity,pos={1}", LocalID, RawPosition);
+            entprop.Position = RawPosition;
         }
 
         // remember the current and last set values
@@ -755,7 +743,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, RawVelocity, _acceleration, _rotationalVelocity);
+                LocalID, RawPosition, RawOrientation, RawVelocity, _acceleration, _rotationalVelocity);
     }
 }
 }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
index c27d3f0..82fe267 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
@@ -144,7 +144,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         public void SetupVehicleDebugging()
         {
             enableAngularVerticalAttraction = true;
-            enableAngularDeflection = false;
+            enableAngularDeflection = true;
             enableAngularBanking = true;
             if (BSParam.VehicleDebuggingEnable)
             {
@@ -173,7 +173,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             switch (pParam)
             {
                 case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY:
-                    m_angularDeflectionEfficiency = Math.Max(pValue, 0.01f);
+                    m_angularDeflectionEfficiency = ClampInRange(0f, pValue, 1f);
                     break;
                 case Vehicle.ANGULAR_DEFLECTION_TIMESCALE:
                     m_angularDeflectionTimescale = Math.Max(pValue, 0.01f);
@@ -1512,11 +1512,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         //     in that direction.
         // TODO: implement reference frame.
         public void ComputeAngularDeflection()
-        {
-            // Since angularMotorUp and angularDeflection are computed independently, they will calculate
-            //     approximately the same X or Y correction. When added together (when contributions are combined)
-            //     this creates an over-correction and then wabbling as the target is overshot.
-            // TODO: rethink how the different correction computations inter-relate.
+        {   
 
             if (enableAngularDeflection && m_angularDeflectionEfficiency != 0 && VehicleForwardSpeed > 0.2)
             {
@@ -1531,10 +1527,14 @@ namespace OpenSim.Region.Physics.BulletSPlugin
 
                 // The direction the vehicle is pointing
                 Vector3 pointingDirection = Vector3.UnitX * VehicleOrientation;
-                pointingDirection.Normalize();
+                //Predict where the Vehicle will be pointing after AngularVelocity change is applied. This will keep
+                //   from overshooting and allow this correction to merge with the Vertical Attraction peacefully.
+                Vector3 predictedPointingDirection = pointingDirection * Quaternion.CreateFromAxisAngle(VehicleRotationalVelocity, 0f);
+                predictedPointingDirection.Normalize();
 
                 // The difference between what is and what should be.
-                Vector3 deflectionError = movingDirection - pointingDirection;
+               // Vector3 deflectionError = movingDirection - predictedPointingDirection;
+                Vector3 deflectionError = Vector3.Cross(movingDirection, predictedPointingDirection);
 
                 // Don't try to correct very large errors (not our job)
                 // if (Math.Abs(deflectionError.X) > PIOverFour) deflectionError.X = PIOverTwo * Math.Sign(deflectionError.X);
@@ -1547,15 +1547,16 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 // ret = m_angularDeflectionCorrectionMotor(1f, deflectionError);
 
                 // Scale the correction by recovery timescale and efficiency
-                deflectContributionV = (-deflectionError) * m_angularDeflectionEfficiency;
-                deflectContributionV /= m_angularDeflectionTimescale;
-
-                VehicleRotationalVelocity += deflectContributionV * VehicleOrientation;
+                //    Not modeling a spring so clamp the scale to no more then the arc
+                deflectContributionV = (-deflectionError) * ClampInRange(0, m_angularDeflectionEfficiency/m_angularDeflectionTimescale,1f);
+                //deflectContributionV /= m_angularDeflectionTimescale;
 
+               // VehicleRotationalVelocity += deflectContributionV * VehicleOrientation;
+                VehicleRotationalVelocity += deflectContributionV;
                 VDetailLog("{0},  MoveAngular,Deflection,movingDir={1},pointingDir={2},deflectError={3},ret={4}",
                     ControllingPrim.LocalID, movingDirection, pointingDirection, deflectionError, deflectContributionV);
-                VDetailLog("{0},  MoveAngular,Deflection,fwdSpd={1},defEff={2},defTS={3}",
-                    ControllingPrim.LocalID, VehicleForwardSpeed, m_angularDeflectionEfficiency, m_angularDeflectionTimescale);
+                VDetailLog("{0},  MoveAngular,Deflection,fwdSpd={1},defEff={2},defTS={3},PredictedPointingDir={4}",
+                    ControllingPrim.LocalID, VehicleForwardSpeed, m_angularDeflectionEfficiency, m_angularDeflectionTimescale, predictedPointingDirection);
             }
         }
 
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
index 738e2d0..a41eaf8 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
@@ -197,10 +197,10 @@ public abstract class BSPhysObject : PhysicsActor
     // Update the physical location and motion of the object. Called with data from Bullet.
     public abstract void UpdateProperties(EntityProperties entprop);
 
-    public abstract OMV.Vector3 RawPosition { get; set; }
+    public virtual OMV.Vector3 RawPosition { get; set; }
     public abstract OMV.Vector3 ForcePosition { get; set; }
 
-    public abstract OMV.Quaternion RawOrientation { get; set; }
+    public virtual OMV.Quaternion RawOrientation { get; set; }
     public abstract OMV.Quaternion ForceOrientation { get; set; }
 
     public OMV.Vector3 RawVelocity { get; set; }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
index ce4c3da..d43448e 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
@@ -51,12 +51,8 @@ public class BSPrim : BSPhysObject
     private bool _isSelected;
     private bool _isVolumeDetect;
 
-    // _position is what the simulator thinks the positions of the prim is.
-    private OMV.Vector3 _position;
-
     private float _mass;    // the mass of this object
     private OMV.Vector3 _acceleration;
-    private OMV.Quaternion _orientation;
     private int _physicsActorType;
     private bool _isPhysical;
     private bool _flying;
@@ -88,10 +84,10 @@ public class BSPrim : BSPhysObject
     {
         // m_log.DebugFormat("{0}: BSPrim creation of {1}, id={2}", LogHeader, primName, localID);
         _physicsActorType = (int)ActorTypes.Prim;
-        _position = pos;
+        RawPosition = pos;
         _size = size;
         Scale = size;   // prims are the size the user wants them to be (different for BSCharactes).
-        _orientation = rotation;
+        RawOrientation = rotation;
         _buoyancy = 0f;
         RawVelocity = OMV.Vector3.Zero;
         _rotationalVelocity = OMV.Vector3.Zero;
@@ -270,46 +266,42 @@ public class BSPrim : BSPhysObject
         return;
     }
 
-    public override OMV.Vector3 RawPosition
-    {
-        get { return _position; }
-        set { _position = value; }
-    }
     public override OMV.Vector3 Position {
         get {
             // don't do the GetObjectPosition for root elements because this function is called a zillion times.
-            // _position = ForcePosition;
-            return _position;
+            // RawPosition = ForcePosition;
+            return RawPosition;
         }
         set {
             // If the position must be forced into the physics engine, use ForcePosition.
             // All positions are given in world positions.
-            if (_position == value)
+            if (RawPosition == value)
             {
-                DetailLog("{0},BSPrim.setPosition,call,positionNotChanging,pos={1},orient={2}", LocalID, _position, _orientation);
+                DetailLog("{0},BSPrim.setPosition,call,positionNotChanging,pos={1},orient={2}", LocalID, RawPosition, RawOrientation);
                 return;
             }
-            _position = value;
+            RawPosition = value;
             PositionSanityCheck(false);
 
             PhysScene.TaintedObject("BSPrim.setPosition", delegate()
             {
-                DetailLog("{0},BSPrim.SetPosition,taint,pos={1},orient={2}", LocalID, _position, _orientation);
-                ForcePosition = _position;
+                DetailLog("{0},BSPrim.SetPosition,taint,pos={1},orient={2}", LocalID, RawPosition, RawOrientation);
+                ForcePosition = RawPosition;
             });
         }
     }
 
+    // NOTE: overloaded by BSPrimDisplaced to handle offset for center-of-gravity.
     public override OMV.Vector3 ForcePosition {
         get {
-            _position = PhysScene.PE.GetPosition(PhysBody);
-            return _position;
+            RawPosition = PhysScene.PE.GetPosition(PhysBody);
+            return RawPosition;
         }
         set {
-            _position = value;
+            RawPosition = value;
             if (PhysBody.HasPhysicalBody)
             {
-                PhysScene.PE.SetTranslation(PhysBody, _position, _orientation);
+                PhysScene.PE.SetTranslation(PhysBody, RawPosition, RawOrientation);
                 ActivateIfPhysical(false);
             }
         }
@@ -343,10 +335,10 @@ public class BSPrim : BSPhysObject
             float targetHeight = terrainHeight + (Size.Z / 2f);
             // If the object is below ground it just has to be moved up because pushing will
             //     not get it through the terrain
-            _position.Z = targetHeight;
+            RawPosition = new OMV.Vector3(RawPosition.X, RawPosition.Y, targetHeight);
             if (inTaintTime)
             {
-                ForcePosition = _position;
+                ForcePosition = RawPosition;
             }
             // If we are throwing the object around, zero its other forces
             ZeroMotion(inTaintTime);
@@ -355,7 +347,7 @@ public class BSPrim : BSPhysObject
 
         if ((CurrentCollisionFlags & CollisionFlags.BS_FLOATS_ON_WATER) != 0)
         {
-            float waterHeight = PhysScene.TerrainManager.GetWaterLevelAtXYZ(_position);
+            float waterHeight = PhysScene.TerrainManager.GetWaterLevelAtXYZ(RawPosition);
             // TODO: a floating motor so object will bob in the water
             if (Math.Abs(RawPosition.Z - waterHeight) > 0.1f)
             {
@@ -364,7 +356,7 @@ public class BSPrim : BSPhysObject
 
                 // Apply upforce and overcome gravity.
                 OMV.Vector3 correctionForce = upForce - PhysScene.DefaultGravity;
-                DetailLog("{0},BSPrim.PositionSanityCheck,applyForce,pos={1},upForce={2},correctionForce={3}", LocalID, _position, upForce, correctionForce);
+                DetailLog("{0},BSPrim.PositionSanityCheck,applyForce,pos={1},upForce={2},correctionForce={3}", LocalID, RawPosition, upForce, correctionForce);
                 AddForce(correctionForce, false, inTaintTime);
                 ret = true;
             }
@@ -383,11 +375,11 @@ public class BSPrim : BSPhysObject
         uint wayOutThere = Constants.RegionSize * Constants.RegionSize;
         // There have been instances of objects getting thrown way out of bounds and crashing
         //    the border crossing code.
-        if (   _position.X < -Constants.RegionSize || _position.X > wayOutThere
-            || _position.Y < -Constants.RegionSize || _position.Y > wayOutThere
-            || _position.Z < -Constants.RegionSize || _position.Z > wayOutThere)
+        if (   RawPosition.X < -Constants.RegionSize || RawPosition.X > wayOutThere
+            || RawPosition.Y < -Constants.RegionSize || RawPosition.Y > wayOutThere
+            || RawPosition.Z < -Constants.RegionSize || RawPosition.Z > wayOutThere)
         {
-            _position = new OMV.Vector3(10, 10, 50);
+            RawPosition = new OMV.Vector3(10, 10, 50);
             ZeroMotion(inTaintTime);
             ret = true;
         }
@@ -713,23 +705,19 @@ public class BSPrim : BSPhysObject
         get { return _acceleration; }
         set { _acceleration = value; }
     }
-    public override OMV.Quaternion RawOrientation
-    {
-        get { return _orientation; }
-        set { _orientation = value; }
-    }
+
     public override OMV.Quaternion Orientation {
         get {
-            return _orientation;
+            return RawOrientation;
         }
         set {
-            if (_orientation == value)
+            if (RawOrientation == value)
                 return;
-            _orientation = value;
+            RawOrientation = value;
 
             PhysScene.TaintedObject("BSPrim.setOrientation", delegate()
             {
-                ForceOrientation = _orientation;
+                ForceOrientation = RawOrientation;
             });
         }
     }
@@ -738,14 +726,14 @@ public class BSPrim : BSPhysObject
     {
         get
         {
-            _orientation = PhysScene.PE.GetOrientation(PhysBody);
-            return _orientation;
+            RawOrientation = PhysScene.PE.GetOrientation(PhysBody);
+            return RawOrientation;
         }
         set
         {
-            _orientation = value;
+            RawOrientation = value;
             if (PhysBody.HasPhysicalBody)
-                PhysScene.PE.SetTranslation(PhysBody, _position, _orientation);
+                PhysScene.PE.SetTranslation(PhysBody, RawPosition, RawOrientation);
         }
     }
     public override int PhysicsActorType {
@@ -889,7 +877,7 @@ public class BSPrim : BSPhysObject
             // PhysicsScene.PE.ClearAllForces(BSBody);
 
             // For good measure, make sure the transform is set through to the motion state
-            ForcePosition = _position;
+            ForcePosition = RawPosition;
             ForceVelocity = RawVelocity;
             ForceRotationalVelocity = _rotationalVelocity;
 
@@ -1543,8 +1531,8 @@ public class BSPrim : BSPhysObject
         // DetailLog("{0},BSPrim.UpdateProperties,entry,entprop={1}", LocalID, entprop);   // DEBUG DEBUG
 
         // Assign directly to the local variables so the normal set actions do not happen
-        _position = entprop.Position;
-        _orientation = entprop.Rotation;
+        RawPosition = entprop.Position;
+        RawOrientation = entprop.Rotation;
         // DEBUG DEBUG DEBUG -- smooth velocity changes a bit. The simulator seems to be
         //    very sensitive to velocity changes.
         if (entprop.Velocity == OMV.Vector3.Zero || !entprop.Velocity.ApproxEquals(RawVelocity, BSParam.UpdateVelocityChangeThreshold))
@@ -1557,13 +1545,13 @@ public class BSPrim : BSPhysObject
         // The sanity check can change the velocity and/or position.
         if (PositionSanityCheck(true /* inTaintTime */ ))
         {
-            entprop.Position = _position;
+            entprop.Position = RawPosition;
             entprop.Velocity = RawVelocity;
             entprop.RotationalVelocity = _rotationalVelocity;
             entprop.Acceleration = _acceleration;
         }
 
-        OMV.Vector3 direction = OMV.Vector3.UnitX * _orientation;   // DEBUG DEBUG DEBUG
+        OMV.Vector3 direction = OMV.Vector3.UnitX * RawOrientation;   // DEBUG DEBUG DEBUG
         DetailLog("{0},BSPrim.UpdateProperties,call,entProp={1},dir={2}", LocalID, entprop, direction);
 
         // remember the current and last set values
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrimDisplaced.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrimDisplaced.cs
index 35d5a08..2eb1440 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPrimDisplaced.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrimDisplaced.cs
@@ -23,11 +23,6 @@
  * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- *
- * The quotations from http://wiki.secondlife.com/wiki/Linden_Vehicle_Tutorial
- * are Copyright (c) 2009 Linden Research, Inc and are used under their license
- * of Creative Commons Attribution-Share Alike 3.0
- * (http://creativecommons.org/licenses/by-sa/3.0/).
  */
 
 using System;
@@ -115,7 +110,7 @@ public class BSPrimDisplaced : BSPrim
     public override Vector3 ForcePosition
     {
         get {
-            OMV.Vector3 physPosition = base.ForcePosition;
+            OMV.Vector3 physPosition = PhysScene.PE.GetPosition(PhysBody);
             if (PositionDisplacement != OMV.Vector3.Zero)
             {
                 // If there is some displacement, return the physical position (center-of-mass)
@@ -125,6 +120,7 @@ public class BSPrimDisplaced : BSPrim
                                 LocalID, physPosition, displacement, physPosition - displacement);
                 physPosition -= displacement;
             }
+            RawPosition = physPosition;
             return physPosition;
         }
         set