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.

connector_plugin
Robert Adams 2012-10-02 10:50:29 -07:00
parent 1e5869dcf6
commit b6b505163c
1 changed files with 57 additions and 39 deletions

View File

@ -92,7 +92,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
private float m_angularMotorDecayTimescale = 0; // motor angular velocity decay rate private float m_angularMotorDecayTimescale = 0; // motor angular velocity decay rate
private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular velocity decay rate private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular velocity decay rate
private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body 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 //Deflection properties
// private float m_angularDeflectionEfficiency = 0; // private float m_angularDeflectionEfficiency = 0;
@ -352,8 +352,9 @@ namespace OpenSim.Region.Physics.BulletSPlugin
// m_bankingMix = 1; // m_bankingMix = 1;
// m_bankingTimescale = 1; // m_bankingTimescale = 1;
// m_referenceFrame = Quaternion.Identity; // m_referenceFrame = Quaternion.Identity;
m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | m_flags |= (VehicleFlag.NO_DEFLECTION_UP
VehicleFlag.LIMIT_MOTOR_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_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT);
m_flags |= (VehicleFlag.HOVER_UP_ONLY); m_flags |= (VehicleFlag.HOVER_UP_ONLY);
break; break;
@ -380,12 +381,13 @@ namespace OpenSim.Region.Physics.BulletSPlugin
// m_bankingMix = 0.8f; // m_bankingMix = 0.8f;
// m_bankingTimescale = 1; // m_bankingTimescale = 1;
// m_referenceFrame = Quaternion.Identity; // m_referenceFrame = Quaternion.Identity;
m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY | m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY
VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY); | VehicleFlag.HOVER_GLOBAL_HEIGHT
m_flags &= ~(VehicleFlag.LIMIT_ROLL_ONLY); | VehicleFlag.LIMIT_ROLL_ONLY
m_flags |= (VehicleFlag.NO_DEFLECTION_UP | | VehicleFlag.HOVER_UP_ONLY);
VehicleFlag.LIMIT_MOTOR_UP); m_flags |= (VehicleFlag.NO_DEFLECTION_UP
m_flags |= (VehicleFlag.HOVER_WATER_ONLY); | VehicleFlag.LIMIT_MOTOR_UP
| VehicleFlag.HOVER_WATER_ONLY);
break; break;
case Vehicle.TYPE_AIRPLANE: case Vehicle.TYPE_AIRPLANE:
m_linearFrictionTimescale = new Vector3(200, 10, 5); m_linearFrictionTimescale = new Vector3(200, 10, 5);
@ -410,9 +412,12 @@ namespace OpenSim.Region.Physics.BulletSPlugin
// m_bankingMix = 0.7f; // m_bankingMix = 0.7f;
// m_bankingTimescale = 2; // m_bankingTimescale = 2;
// m_referenceFrame = Quaternion.Identity; // m_referenceFrame = Quaternion.Identity;
m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY
VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY); | VehicleFlag.HOVER_TERRAIN_ONLY
m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_MOTOR_UP); | VehicleFlag.HOVER_GLOBAL_HEIGHT
| VehicleFlag.HOVER_UP_ONLY
| VehicleFlag.NO_DEFLECTION_UP
| VehicleFlag.LIMIT_MOTOR_UP);
m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY); m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY);
break; break;
case Vehicle.TYPE_BALLOON: case Vehicle.TYPE_BALLOON:
@ -438,11 +443,13 @@ namespace OpenSim.Region.Physics.BulletSPlugin
// m_bankingMix = 0.7f; // m_bankingMix = 0.7f;
// m_bankingTimescale = 5; // m_bankingTimescale = 5;
// m_referenceFrame = Quaternion.Identity; // m_referenceFrame = Quaternion.Identity;
m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY
VehicleFlag.HOVER_UP_ONLY); | VehicleFlag.HOVER_TERRAIN_ONLY
m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_MOTOR_UP); | VehicleFlag.HOVER_UP_ONLY
m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY); | VehicleFlag.NO_DEFLECTION_UP
m_flags |= (VehicleFlag.HOVER_GLOBAL_HEIGHT); | VehicleFlag.LIMIT_MOTOR_UP);
m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY
| VehicleFlag.HOVER_GLOBAL_HEIGHT);
break; break;
} }
}//end SetDefaultsForType }//end SetDefaultsForType
@ -451,7 +458,8 @@ namespace OpenSim.Region.Physics.BulletSPlugin
// Do any updating needed for a vehicle // Do any updating needed for a vehicle
public void Refresh() 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 // Set the prim's inertia to zero. The vehicle code handles that and this
// removes the torque action introduced by Bullet. // removes the torque action introduced by Bullet.
@ -470,7 +478,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
LimitRotation(pTimestep); LimitRotation(pTimestep);
// remember the position so next step we can limit absolute movement effects // 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}", VDetailLog("{0},BSDynamics.Step,done,pos={1},force={2},velocity={3},angvel={4}",
Prim.LocalID, Prim.Position, Prim.Force, Prim.Velocity, Prim.RotationalVelocity); 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 // convert requested object velocity to object relative vector
Quaternion rotq = Prim.Orientation; Quaternion rotq = Prim.ForceOrientation;
m_newVelocity = m_lastLinearVelocityVector * rotq; m_newVelocity = m_lastLinearVelocityVector * rotq;
// Add the various forces into m_dir which will be our new direction vector (velocity) // 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 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); // 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. // If below the terrain, move us above the ground a little.
float terrainHeight = Prim.PhysicsScene.TerrainManager.GetTerrainHeightAtXYZ(pos); 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. // 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. // 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 (rotatedSize.Z < terrainHeight)
if (pos.Z < terrainHeight) if (pos.Z < terrainHeight)
{ {
pos.Z = terrainHeight + 2; pos.Z = terrainHeight + 2;
Prim.Position = pos; Prim.ForcePosition = pos;
VDetailLog("{0},MoveLinear,terrainHeight,terrainHeight={1},pos={2}", Prim.LocalID, terrainHeight, 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) if ((pos.Z - m_VhoverTargetHeight) > .2 || (pos.Z - m_VhoverTargetHeight) < -.2)
{ {
Prim.Position = pos; Prim.ForcePosition = pos;
} }
} }
else else
@ -635,12 +643,13 @@ namespace OpenSim.Region.Physics.BulletSPlugin
} }
if (changed) if (changed)
{ {
Prim.Position = pos; Prim.ForcePosition = pos;
VDetailLog("{0},MoveLinear,blockingEndPoint,block={1},origPos={2},pos={3}", VDetailLog("{0},MoveLinear,blockingEndPoint,block={1},origPos={2},pos={3}",
Prim.LocalID, m_BlockingEndPoint, posChange, pos); Prim.LocalID, m_BlockingEndPoint, posChange, pos);
} }
} }
// Limit absolute vertical change
float Zchange = Math.Abs(posChange.Z); float Zchange = Math.Abs(posChange.Z);
if ((m_flags & (VehicleFlag.LIMIT_MOTOR_UP)) != 0) if ((m_flags & (VehicleFlag.LIMIT_MOTOR_UP)) != 0)
{ {
@ -659,6 +668,8 @@ namespace OpenSim.Region.Physics.BulletSPlugin
grav.Z = (float)(grav.Z * 1.037125); grav.Z = (float)(grav.Z * 1.037125);
VDetailLog("{0},MoveLinear,limitMotorUp,grav={1}", Prim.LocalID, grav); VDetailLog("{0},MoveLinear,limitMotorUp,grav={1}", Prim.LocalID, grav);
} }
// If not changing some axis, reduce out velocity
if ((m_flags & (VehicleFlag.NO_X)) != 0) if ((m_flags & (VehicleFlag.NO_X)) != 0)
m_newVelocity.X = 0; m_newVelocity.X = 0;
if ((m_flags & (VehicleFlag.NO_Y)) != 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 // a newly set velocity, this routine steps the value from the previous
// value (m_angularMotorVelocity) to the requested value (m_angularMotorDirection). // value (m_angularMotorVelocity) to the requested value (m_angularMotorDirection).
// There are m_angularMotorApply steps. // There are m_angularMotorApply steps.
Vector3 origAngularVelocity = m_angularMotorVelocity; Vector3 origVel = m_angularMotorVelocity;
Vector3 origDir = m_angularMotorDirection;
// ramp up to new value // 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 // requested speed - last motor speed
m_angularMotorVelocity.X += (m_angularMotorDirection.X - m_angularMotorVelocity.X) / (m_angularMotorTimescale / pTimestep); 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.Y += (m_angularMotorDirection.Y - m_angularMotorVelocity.Y) / (m_angularMotorTimescale / pTimestep);
m_angularMotorVelocity.Z += (m_angularMotorDirection.Z - m_angularMotorVelocity.Z) / (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}", VDetailLog("{0},MoveAngular,angularMotorApply,apply={1},angTScale={2},timeStep={3},origvel={4},origDir={5},vel={6}",
Prim.LocalID, m_angularMotorApply, m_angularMotorTimescale, pTimestep, origAngularVelocity, m_angularMotorDirection, m_angularMotorVelocity); 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--; m_angularMotorApply--;
} }
else else
@ -727,25 +738,32 @@ namespace OpenSim.Region.Physics.BulletSPlugin
// Vertical attractor section // Vertical attractor section
Vector3 vertattr = Vector3.Zero; 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); float VAservo = 0.2f / (m_verticalAttractionTimescale / pTimestep);
VAservo *= (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency);
// get present body rotation // get present body rotation
Quaternion rotq = Prim.Orientation; Quaternion rotq = Prim.ForceOrientation;
// make a vector pointing up // vector pointing up
Vector3 verterr = Vector3.Zero; Vector3 verterr = Vector3.Zero;
verterr.Z = 1.0f; verterr.Z = 1.0f;
// rotate it to Body Angle // rotate it to Body Angle
verterr = verterr * rotq; 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 // 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. // 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) if (verterr.Z < 0.0f)
{ {
verterr.X = 2.0f - verterr.X; verterr.X = 2.0f - verterr.X;
verterr.Y = 2.0f - verterr.Y; verterr.Y = 2.0f - verterr.Y;
} }
// Error is 0 (no error) to +/- 2 (max error)
// scale it by VAservo // scale it by VAservo
verterr = verterr * VAservo; verterr = verterr * VAservo;
@ -765,7 +783,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
} // else vertical attractor is off } // else vertical attractor is off
// m_lastVertAttractor = vertattr; m_lastVertAttractor = vertattr;
// Bank section tba // Bank section tba
@ -799,7 +817,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
internal void LimitRotation(float timestep) internal void LimitRotation(float timestep)
{ {
Quaternion rotq = Prim.Orientation; Quaternion rotq = Prim.ForceOrientation;
Quaternion m_rot = rotq; Quaternion m_rot = rotq;
bool changed = false; bool changed = false;
if (m_RollreferenceFrame != Quaternion.Identity) if (m_RollreferenceFrame != Quaternion.Identity)
@ -834,7 +852,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
} }
if (changed) if (changed)
{ {
Prim.Orientation = m_rot; Prim.ForceOrientation = m_rot;
VDetailLog("{0},LimitRotation,done,orig={1},new={2}", Prim.LocalID, rotq, m_rot); VDetailLog("{0},LimitRotation,done,orig={1},new={2}", Prim.LocalID, rotq, m_rot);
} }