BulletSim: call ForcePosition and ForceOrientation in BSDynamics so there is no lag between what the vehicle code sees and what the physics engine is using.

1 files changed, 57 insertions, 39 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
index cbfd7e3..4ba2f62 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
@@ -92,7 +92,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         private float m_angularMotorDecayTimescale = 0;                 // motor angular velocity decay rate
         private Vector3 m_angularFrictionTimescale = Vector3.Zero;      // body angular velocity  decay rate
         private Vector3 m_lastAngularVelocity = Vector3.Zero;           // what was last applied to body
- //       private Vector3 m_lastVertAttractor = Vector3.Zero;             // what VA was last applied to body
+        private Vector3 m_lastVertAttractor = Vector3.Zero;             // what VA was last applied to body
 
         //Deflection properties
         // private float m_angularDeflectionEfficiency = 0;
@@ -352,8 +352,9 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     // m_bankingMix = 1;
                     // m_bankingTimescale = 1;
                     // m_referenceFrame = Quaternion.Identity;
-                    m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY |
-                                VehicleFlag.LIMIT_MOTOR_UP);
+                    m_flags |= (VehicleFlag.NO_DEFLECTION_UP 
+                                | VehicleFlag.LIMIT_ROLL_ONLY
+                                | VehicleFlag.LIMIT_MOTOR_UP);
                     m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT);
                     m_flags |= (VehicleFlag.HOVER_UP_ONLY);
                     break;
@@ -380,12 +381,13 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     // m_bankingMix = 0.8f;
                     // m_bankingTimescale = 1;
                     // m_referenceFrame = Quaternion.Identity;
-                    m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY |
-                            VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
-                    m_flags &= ~(VehicleFlag.LIMIT_ROLL_ONLY);
-                    m_flags |= (VehicleFlag.NO_DEFLECTION_UP |
-                                VehicleFlag.LIMIT_MOTOR_UP);
-                    m_flags |= (VehicleFlag.HOVER_WATER_ONLY);
+                    m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY
+                                    | VehicleFlag.HOVER_GLOBAL_HEIGHT 
+                                    | VehicleFlag.LIMIT_ROLL_ONLY
+                                    | VehicleFlag.HOVER_UP_ONLY);
+                    m_flags |= (VehicleFlag.NO_DEFLECTION_UP
+                                    | VehicleFlag.LIMIT_MOTOR_UP
+                                    | VehicleFlag.HOVER_WATER_ONLY);
                     break;
                 case Vehicle.TYPE_AIRPLANE:
                     m_linearFrictionTimescale = new Vector3(200, 10, 5);
@@ -410,9 +412,12 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     // m_bankingMix = 0.7f;
                     // m_bankingTimescale = 2;
                     // m_referenceFrame = Quaternion.Identity;
-                    m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
-                        VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
-                    m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_MOTOR_UP);
+                    m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY
+                                    | VehicleFlag.HOVER_TERRAIN_ONLY
+                                    | VehicleFlag.HOVER_GLOBAL_HEIGHT
+                                    | VehicleFlag.HOVER_UP_ONLY
+                                    | VehicleFlag.NO_DEFLECTION_UP
+                                    | VehicleFlag.LIMIT_MOTOR_UP);
                     m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY);
                     break;
                 case Vehicle.TYPE_BALLOON:
@@ -438,11 +443,13 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     // m_bankingMix = 0.7f;
                     // m_bankingTimescale = 5;
                     // m_referenceFrame = Quaternion.Identity;
-                    m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
-                        VehicleFlag.HOVER_UP_ONLY);
-                    m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_MOTOR_UP);
-                    m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY);
-                    m_flags |= (VehicleFlag.HOVER_GLOBAL_HEIGHT);
+                    m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY
+                                    | VehicleFlag.HOVER_TERRAIN_ONLY
+                                    | VehicleFlag.HOVER_UP_ONLY
+                                    | VehicleFlag.NO_DEFLECTION_UP
+                                    | VehicleFlag.LIMIT_MOTOR_UP);
+                    m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY
+                                    | VehicleFlag.HOVER_GLOBAL_HEIGHT);
                     break;
             }
         }//end SetDefaultsForType
@@ -451,7 +458,8 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         // Do any updating needed for a vehicle
         public void Refresh()
         {
-            if (Type == Vehicle.TYPE_NONE) return;
+            if (!IsActive) 
+                return;
 
             // Set the prim's inertia to zero. The vehicle code handles that and this
             //    removes the torque action introduced by Bullet.
@@ -470,7 +478,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             LimitRotation(pTimestep);
 
             // remember the position so next step we can limit absolute movement effects
-            m_lastPositionVector = Prim.Position;
+            m_lastPositionVector = Prim.ForcePosition;
 
             VDetailLog("{0},BSDynamics.Step,done,pos={1},force={2},velocity={3},angvel={4}",
                     Prim.LocalID, Prim.Position, Prim.Force, Prim.Velocity, Prim.RotationalVelocity);
@@ -524,7 +532,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             }
 
             // convert requested object velocity to object relative vector
-            Quaternion rotq = Prim.Orientation;
+            Quaternion rotq = Prim.ForceOrientation;
             m_newVelocity = m_lastLinearVelocityVector * rotq;
 
             // Add the various forces into m_dir which will be our new direction vector (velocity)
@@ -541,19 +549,19 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             m_dir.Z = vel_now.Z;        // Preserve the accumulated falling velocity
              */
 
-            Vector3 pos = Prim.Position;
+            Vector3 pos = Prim.ForcePosition;
 //            Vector3 accel = new Vector3(-(m_dir.X - m_lastLinearVelocityVector.X / 0.1f), -(m_dir.Y - m_lastLinearVelocityVector.Y / 0.1f), m_dir.Z - m_lastLinearVelocityVector.Z / 0.1f);
 
             // If below the terrain, move us above the ground a little.
             float terrainHeight = Prim.PhysicsScene.TerrainManager.GetTerrainHeightAtXYZ(pos);
             // Taking the rotated size doesn't work here because m_prim.Size is the size of the root prim and not the linkset.
             //     Need to add a m_prim.LinkSet.Size similar to m_prim.LinkSet.Mass.
-            // Vector3 rotatedSize = m_prim.Size * m_prim.Orientation;
+            // Vector3 rotatedSize = m_prim.Size * m_prim.ForceOrientation;
             // if (rotatedSize.Z < terrainHeight)
             if (pos.Z < terrainHeight)
             {
                 pos.Z = terrainHeight + 2;
-                Prim.Position = pos;
+                Prim.ForcePosition = pos;
                 VDetailLog("{0},MoveLinear,terrainHeight,terrainHeight={1},pos={2}", Prim.LocalID, terrainHeight, pos);
             }
 
@@ -583,7 +591,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 {
                     if ((pos.Z - m_VhoverTargetHeight) > .2 || (pos.Z - m_VhoverTargetHeight) < -.2)
                     {
-                        Prim.Position = pos;
+                        Prim.ForcePosition = pos;
                     }
                 }
                 else
@@ -635,12 +643,13 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 }
                 if (changed)
                 {
-                    Prim.Position = pos;
+                    Prim.ForcePosition = pos;
                     VDetailLog("{0},MoveLinear,blockingEndPoint,block={1},origPos={2},pos={3}",
                                 Prim.LocalID, m_BlockingEndPoint, posChange, pos);
                 }
             }
 
+            // Limit absolute vertical change
             float Zchange = Math.Abs(posChange.Z);
             if ((m_flags & (VehicleFlag.LIMIT_MOTOR_UP)) != 0)
             {
@@ -659,6 +668,8 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     grav.Z = (float)(grav.Z * 1.037125);
                 VDetailLog("{0},MoveLinear,limitMotorUp,grav={1}", Prim.LocalID, grav);
             }
+
+            // If not changing some axis, reduce out velocity
             if ((m_flags & (VehicleFlag.NO_X)) != 0)
                 m_newVelocity.X = 0;
             if ((m_flags & (VehicleFlag.NO_Y)) != 0)
@@ -701,19 +712,19 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 //    a newly set velocity, this routine steps the value from the previous
                 //    value (m_angularMotorVelocity) to the requested value (m_angularMotorDirection).
                 // There are m_angularMotorApply steps.
-                Vector3 origAngularVelocity = m_angularMotorVelocity;
+                Vector3 origVel = m_angularMotorVelocity;
+                Vector3 origDir = m_angularMotorDirection;
+
                 // ramp up to new value
-                //   current velocity    +=                         error                          /    (  time to get there   / step interval)
+                //       new velocity    +=                         error                          /    (  time to get there   / step interval)
                 //                               requested speed       -       last motor speed
                 m_angularMotorVelocity.X += (m_angularMotorDirection.X - m_angularMotorVelocity.X) /  (m_angularMotorTimescale / pTimestep);
                 m_angularMotorVelocity.Y += (m_angularMotorDirection.Y - m_angularMotorVelocity.Y) /  (m_angularMotorTimescale / pTimestep);
                 m_angularMotorVelocity.Z += (m_angularMotorDirection.Z - m_angularMotorVelocity.Z) /  (m_angularMotorTimescale / pTimestep);
 
-                VDetailLog("{0},MoveAngular,angularMotorApply,apply={1},angTScale={2},timeStep={3},origvel={4},dir={5},vel={6}",
-                        Prim.LocalID, m_angularMotorApply, m_angularMotorTimescale, pTimestep, origAngularVelocity, m_angularMotorDirection, m_angularMotorVelocity);
+                VDetailLog("{0},MoveAngular,angularMotorApply,apply={1},angTScale={2},timeStep={3},origvel={4},origDir={5},vel={6}",
+                        Prim.LocalID, m_angularMotorApply, m_angularMotorTimescale, pTimestep, origVel, origDir, m_angularMotorVelocity);
 
-                // This is done so that if script request rate is less than phys frame rate the expected
-                //    velocity may still be acheived.
                 m_angularMotorApply--;
             }
             else
@@ -727,25 +738,32 @@ namespace OpenSim.Region.Physics.BulletSPlugin
 
             // Vertical attractor section
             Vector3 vertattr = Vector3.Zero;
-            if (m_verticalAttractionTimescale < 300)
+            Vector3 deflection = Vector3.Zero;
+            Vector3 banking = Vector3.Zero;
+
+            if (m_verticalAttractionTimescale < 300 && m_lastAngularVelocity != Vector3.Zero)
             {
                 float VAservo = 0.2f / (m_verticalAttractionTimescale / pTimestep);
+                VAservo *= (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency);
+
                 // get present body rotation
-                Quaternion rotq = Prim.Orientation;
-                // make a vector pointing up
+                Quaternion rotq = Prim.ForceOrientation;
+                // vector pointing up
                 Vector3 verterr = Vector3.Zero;
                 verterr.Z = 1.0f;
+
                 // rotate it to Body Angle
                 verterr = verterr * rotq;
-                // verterr.X and .Y are the World error ammounts. They are 0 when there is no error (Vehicle Body is 'vertical'), and .Z will be 1.
+                // verterr.X and .Y are the World error amounts. They are 0 when there is no error (Vehicle Body is 'vertical'), and .Z will be 1.
                 // As the body leans to its side |.X| will increase to 1 and .Z fall to 0. As body inverts |.X| will fall and .Z will go
                 // negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body.
+
+                // Error is 0 (no error) to +/- 2 (max error)
                 if (verterr.Z < 0.0f)
                 {
                     verterr.X = 2.0f - verterr.X;
                     verterr.Y = 2.0f - verterr.Y;
                 }
-                // Error is 0 (no error) to +/- 2 (max error)
                 // scale it by VAservo
                 verterr = verterr * VAservo;
 
@@ -765,7 +783,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
 
             } // else vertical attractor is off
 
-            // m_lastVertAttractor = vertattr;
+            m_lastVertAttractor = vertattr;
 
             // Bank section tba
 
@@ -799,7 +817,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
 
         internal void LimitRotation(float timestep)
         {
-            Quaternion rotq = Prim.Orientation;
+            Quaternion rotq = Prim.ForceOrientation;
             Quaternion m_rot = rotq;
             bool changed = false;
             if (m_RollreferenceFrame != Quaternion.Identity)
@@ -834,7 +852,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             }
             if (changed)
             {
-                Prim.Orientation = m_rot;
+                Prim.ForceOrientation = m_rot;
                 VDetailLog("{0},LimitRotation,done,orig={1},new={2}", Prim.LocalID, rotq, m_rot);
             }