OpenSim
/
OpenSimMirror
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

96
OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
View File

@ -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);
}