BulletSim: vehicle tweeking.

Add AddTorque() method to BSPrim. Remove some manual motor actions
in computing angular force (will eventually be replaced with motor class).
Remove some experimental changes.
integration
Robert Adams 2012-10-31 09:26:58 -07:00
parent 52be581f71
commit 28e2cd3fa2
3 changed files with 90 additions and 93 deletions

View File

@ -511,21 +511,6 @@ 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()
{ {
/*
* Doesnt work unless BSDynamics senses and corrects for all collisions
if (IsActive)
BulletSimAPI.AddToCollisionFlags2(Prim.BSBody.ptr, CollisionFlags.CF_KINEMATIC_OBJECT);
else
BulletSimAPI.RemoveFromCollisionFlags2(Prim.BSBody.ptr, CollisionFlags.CF_KINEMATIC_OBJECT);
*/
/*
* Doesn't work because with zero inertia, Bullet will not apply any forces to the object.
if (IsActive)
{
BulletSimAPI.SetMassProps2(Prim.BSBody.ptr, Prim.MassRaw, Vector3.Zero);
BulletSimAPI.UpdateInertiaTensor2(Prim.BSBody.ptr);
}
*/
if (IsActive) if (IsActive)
{ {
// Friction effects are handled by this vehicle code // Friction effects are handled by this vehicle code
@ -539,29 +524,21 @@ namespace OpenSim.Region.Physics.BulletSPlugin
{ {
if (!IsActive) return; if (!IsActive) return;
m_lastAngularVelocity = Prim.ForceRotationalVelocity; // DEBUG: account for what Bullet did last time // DEBUG
// Because Bullet does apply forces to the vehicle, our last computed
// linear and angular velocities are not what is happening now.
// Vector3 externalAngularVelocity = Prim.ForceRotationalVelocity - m_lastAngularVelocity;
// m_lastAngularVelocity += (externalAngularVelocity * 0.5f) * pTimestep;
// m_lastAngularVelocity = Prim.ForceRotationalVelocity; // DEBUG: account for what Bullet did last time
// m_lastLinearVelocityVector = Prim.ForceVelocity * Quaternion.Inverse(Prim.ForceOrientation); // DEBUG:
// END DEBUG
MoveLinear(pTimestep); MoveLinear(pTimestep);
MoveAngular(pTimestep); MoveAngular(pTimestep);
LimitRotation(pTimestep); LimitRotation(pTimestep);
/* Experimental
// Wonder if Bullet could handle collision penetration while this applies the forces.
// Apply the computed forces on the vehicle
Prim.ForcePosition += Prim.ForceVelocity * Prim.MassRaw * pTimestep;
if (Prim.ForceRotationalVelocity != Vector3.Zero)
{
Quaternion newOrientation = Prim.ForceOrientation;
newOrientation.Normalize();
Quaternion appliedRotation = new Quaternion((Prim.ForceRotationalVelocity * pTimestep), 0f);
newOrientation += (appliedRotation * newOrientation) * 0.5f;
newOrientation.Normalize();
Prim.ForceOrientation = newOrientation;
}
*/
// DEBUG: Trying to figure out why Bullet goes crazy when the root prim is moved. // DEBUG: Trying to figure out why Bullet goes crazy when the root prim is moved.
BulletSimAPI.SetInterpolationVelocity2(Prim.BSBody.ptr, m_newVelocity, m_lastAngularVelocity); // DEBUG DEBUG DEBUG // BulletSimAPI.SetInterpolationVelocity2(Prim.BSBody.ptr, m_newVelocity, m_lastAngularVelocity); // DEBUG DEBUG DEBUG
// 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.ForcePosition; m_lastPositionVector = Prim.ForcePosition;
@ -583,7 +560,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
Vector3 vehicleVelocity = Prim.ForceVelocity * Quaternion.Inverse(Prim.ForceOrientation); // DEBUG Vector3 vehicleVelocity = Prim.ForceVelocity * Quaternion.Inverse(Prim.ForceOrientation); // DEBUG
// add drive to body // add drive to body
Vector3 addAmount = (m_linearMotorDirection - m_lastLinearVelocityVector)/(m_linearMotorTimescale / pTimestep); Vector3 addAmount = (m_linearMotorDirection - m_lastLinearVelocityVector)/(m_linearMotorTimescale) * pTimestep;
// lastLinearVelocityVector is the current body velocity vector // lastLinearVelocityVector is the current body velocity vector
m_lastLinearVelocityVector += addAmount; m_lastLinearVelocityVector += addAmount;
@ -593,11 +570,12 @@ namespace OpenSim.Region.Physics.BulletSPlugin
Vector3 frictionFactor = (Vector3.One / m_linearFrictionTimescale) * pTimestep; Vector3 frictionFactor = (Vector3.One / m_linearFrictionTimescale) * pTimestep;
m_lastLinearVelocityVector *= (Vector3.One - frictionFactor); m_lastLinearVelocityVector *= (Vector3.One - frictionFactor);
VDetailLog("{0},MoveLinear,nonZero,origdir={1},origvel={2},vehVel={3},add={4},decay={5},lmDir={6},lmVel={7}", // Rotate new object velocity from vehicle relative to world coordinates
Prim.LocalID, origDir, origVel, vehicleVelocity, addAmount, decayFactor, m_linearMotorDirection, m_lastLinearVelocityVector);
// convert requested object velocity to object relative vector
m_newVelocity = m_lastLinearVelocityVector * Prim.ForceOrientation; m_newVelocity = m_lastLinearVelocityVector * Prim.ForceOrientation;
VDetailLog("{0},MoveLinear,nonZero,origdir={1},origvel={2},vehVel={3},add={4},decay={5},frict={6},lmDir={7},lmVel={8},newVel={9}",
Prim.LocalID, origDir, origVel, vehicleVelocity, addAmount, decayFactor, frictionFactor,
m_linearMotorDirection, m_lastLinearVelocityVector, m_newVelocity);
} }
else else
{ {
@ -609,18 +587,15 @@ namespace OpenSim.Region.Physics.BulletSPlugin
VDetailLog("{0},MoveLinear,zeroed", Prim.LocalID); VDetailLog("{0},MoveLinear,zeroed", Prim.LocalID);
} }
// m_newVelocity is velocity computed from linear motor // m_newVelocity is velocity computed from linear motor in world coordinates
// Add the various forces into m_dir which will be our new direction vector (velocity) // Gravity and Buoyancy
// add Gravity and Buoyancy
// There is some gravity, make a gravity force vector that is applied after object velocity. // There is some gravity, make a gravity force vector that is applied after object velocity.
// m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g; // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g;
// Vector3 grav = Prim.PhysicsScene.DefaultGravity * (Prim.Linkset.LinksetMass * (1f - m_VehicleBuoyancy));
Vector3 grav = Prim.PhysicsScene.DefaultGravity * (1f - m_VehicleBuoyancy); Vector3 grav = Prim.PhysicsScene.DefaultGravity * (1f - m_VehicleBuoyancy);
/* /*
* RA: Not sure why one would do this * RA: Not sure why one would do this unless we are hoping external forces are doing gravity, ...
// Preserve the current Z velocity // Preserve the current Z velocity
Vector3 vel_now = m_prim.Velocity; Vector3 vel_now = m_prim.Velocity;
m_dir.Z = vel_now.Z; // Preserve the accumulated falling velocity m_dir.Z = vel_now.Z; // Preserve the accumulated falling velocity
@ -676,7 +651,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
else else
{ {
float verticalError = pos.Z - m_VhoverTargetHeight; float verticalError = pos.Z - m_VhoverTargetHeight;
// RA: where does the 50 come from> // RA: where does the 50 come from?
float verticalCorrectionVelocity = pTimestep * ((verticalError * 50.0f) / m_VhoverTimescale); float verticalCorrectionVelocity = pTimestep * ((verticalError * 50.0f) / m_VhoverTimescale);
// Replace Vertical speed with correction figure if significant // Replace Vertical speed with correction figure if significant
if (Math.Abs(verticalError) > 0.01f) if (Math.Abs(verticalError) > 0.01f)
@ -784,37 +759,24 @@ namespace OpenSim.Region.Physics.BulletSPlugin
// m_angularFrictionTimescale // body angular velocity decay rate // m_angularFrictionTimescale // body angular velocity decay rate
// m_lastAngularVelocity // what was last applied to body // m_lastAngularVelocity // what was last applied to body
// Get what the body is doing, this includes 'external' influences if (m_angularMotorDirection.LengthSquared() > 0.0001)
Vector3 angularVelocity = Prim.ForceRotationalVelocity;
if (m_angularMotorApply > 0)
{ {
// Rather than snapping the angular motor velocity from the old value to
// 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 origVel = m_angularMotorVelocity; Vector3 origVel = m_angularMotorVelocity;
Vector3 origDir = m_angularMotorDirection; Vector3 origDir = m_angularMotorDirection;
// ramp up to new value
// new 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 += (m_angularMotorDirection - m_angularMotorVelocity) / (m_angularMotorTimescale / pTimestep); m_angularMotorVelocity += (m_angularMotorDirection - m_angularMotorVelocity) / (m_angularMotorTimescale / pTimestep);
// decay requested direction
m_angularMotorDirection *= (1.0f - (pTimestep * 1.0f/m_angularMotorDecayTimescale));
VDetailLog("{0},MoveAngular,angularMotorApply,apply={1},angTScale={2},timeStep={3},origvel={4},origDir={5},vel={6}", VDetailLog("{0},MoveAngular,angularMotorApply,angTScale={1},timeStep={2},origvel={3},origDir={4},vel={5}",
Prim.LocalID, m_angularMotorApply, m_angularMotorTimescale, pTimestep, origVel, origDir, m_angularMotorVelocity); Prim.LocalID, m_angularMotorTimescale, pTimestep, origVel, origDir, m_angularMotorVelocity);
m_angularMotorApply--;
} }
else else
{ {
// No motor recently applied, keep the body velocity
// and decay the velocity
if (m_angularMotorVelocity.LengthSquared() < 0.0001)
m_angularMotorVelocity = Vector3.Zero; m_angularMotorVelocity = Vector3.Zero;
else }
m_angularMotorVelocity -= m_angularMotorVelocity / (m_angularMotorDecayTimescale / pTimestep);
} // end motor section
#region Vertical attactor #region Vertical attactor
@ -824,22 +786,19 @@ namespace OpenSim.Region.Physics.BulletSPlugin
if (m_verticalAttractionTimescale < 300 && m_lastAngularVelocity != Vector3.Zero) if (m_verticalAttractionTimescale < 300 && m_lastAngularVelocity != Vector3.Zero)
{ {
float VAservo = 0.2f; float VAservo = pTimestep * 0.2f / m_verticalAttractionTimescale;
if (Prim.Linkset.LinksetIsColliding) if (Prim.Linkset.LinksetIsColliding)
VAservo = 0.05f / (m_verticalAttractionTimescale / pTimestep); VAservo = pTimestep * 0.05f / (m_verticalAttractionTimescale);
VAservo *= (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency); VAservo *= (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency);
// get present body rotation // Create a vector of the vehicle "up" in world coordinates
Quaternion rotq = Prim.ForceOrientation; Vector3 verticalError = Vector3.UnitZ * Prim.ForceOrientation;
// vector pointing up // verticalError.X and .Y are the World error amounts. They are 0 when there is no
Vector3 verticalError = Vector3.UnitZ; // 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
// rotate it to Body Angle // and .Z will go // negative. Similar for tilt and |.Y|. .X and .Y must be
verticalError = verticalError * rotq; // modulated to prevent a stable inverted body.
// verticalError.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) // Error is 0 (no error) to +/- 2 (max error)
if (verticalError.Z < 0.0f) if (verticalError.Z < 0.0f)
@ -850,13 +809,15 @@ namespace OpenSim.Region.Physics.BulletSPlugin
// scale it by VAservo // scale it by VAservo
verticalError = verticalError * VAservo; verticalError = verticalError * VAservo;
// As the body rotates around the X axis, then verticalError.Y increases; Rotated around Y then .X increases, so // As the body rotates around the X axis, then verticalError.Y increases; Rotated around Y
// Change Body angular velocity X based on Y, and Y based on X. Z is not changed. // then .X increases, so change Body angular velocity X based on Y, and Y based on X.
// Z is not changed.
vertattr.X = verticalError.Y; vertattr.X = verticalError.Y;
vertattr.Y = - verticalError.X; vertattr.Y = - verticalError.X;
vertattr.Z = 0f; vertattr.Z = 0f;
// scaling appears better usingsquare-law // scaling appears better usingsquare-law
Vector3 angularVelocity = Prim.ForceRotationalVelocity;
float bounce = 1.0f - (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency); float bounce = 1.0f - (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency);
vertattr.X += bounce * angularVelocity.X; vertattr.X += bounce * angularVelocity.X;
vertattr.Y += bounce * angularVelocity.Y; vertattr.Y += bounce * angularVelocity.Y;
@ -956,15 +917,22 @@ namespace OpenSim.Region.Physics.BulletSPlugin
VDetailLog("{0},MoveAngular,zeroSmallValues,lastAngular={1}", Prim.LocalID, m_lastAngularVelocity); VDetailLog("{0},MoveAngular,zeroSmallValues,lastAngular={1}", Prim.LocalID, m_lastAngularVelocity);
} }
// apply friction
Vector3 decayamount = Vector3.One / (m_angularFrictionTimescale / pTimestep);
m_lastAngularVelocity -= m_lastAngularVelocity * decayamount;
// Apply to the body // Apply to the body
// The above calculates the absolute angular velocity needed // The above calculates the absolute angular velocity needed
Prim.ForceRotationalVelocity = m_lastAngularVelocity; // Prim.ForceRotationalVelocity = m_lastAngularVelocity;
VDetailLog("{0},MoveAngular,done,decay={1},lastAngular={2}", Prim.LocalID, decayamount, m_lastAngularVelocity); // Apply a force to overcome current angular velocity
Vector3 applyAngularForce = (m_lastAngularVelocity - Prim.ForceRotationalVelocity) * Prim.Linkset.LinksetMass;
// Vector3 applyAngularForce = (m_lastAngularVelocity - Prim.ForceRotationalVelocity);
// Prim.AddAngularForce(applyAngularForce, false);
Prim.ApplyTorqueImpulse(applyAngularForce, false);
// Apply friction for next time
Vector3 decayamount = (Vector3.One / m_angularFrictionTimescale) * pTimestep;
m_lastAngularVelocity *= Vector3.One - decayamount;
VDetailLog("{0},MoveAngular,done,applyAForce={1},decay={2},lastAngular={3}",
Prim.LocalID, applyAngularForce, decayamount, m_lastAngularVelocity);
} //end MoveAngular } //end MoveAngular
internal void LimitRotation(float timestep) internal void LimitRotation(float timestep)

View File

@ -299,7 +299,7 @@ public sealed class BSLinksetConstraints : BSLinkset
// DEBUG: see of inter-linkset collisions are causing problems // DEBUG: see of inter-linkset collisions are causing problems
// BulletSimAPI.SetCollisionFilterMask2(LinksetRoot.BSBody.ptr, // BulletSimAPI.SetCollisionFilterMask2(LinksetRoot.BSBody.ptr,
// (uint)CollisionFilterGroups.LinksetFilter, (uint)CollisionFilterGroups.LinksetMask); // (uint)CollisionFilterGroups.LinksetFilter, (uint)CollisionFilterGroups.LinksetMask);
DetailLog("{0},BSLinksetConstraint.RecomputeLinksetConstraints,setCenterOfMass,rBody={1},linksetMass={2}", DetailLog("{0},BSLinksetConstraint.RecomputeLinksetConstraints,set,rBody={1},linksetMass={2}",
LinksetRoot.LocalID, LinksetRoot.BSBody.ptr.ToString("X"), linksetMass); LinksetRoot.LocalID, LinksetRoot.BSBody.ptr.ToString("X"), linksetMass);
foreach (BSPhysObject child in m_children) foreach (BSPhysObject child in m_children)

View File

@ -378,7 +378,9 @@ public sealed class BSPrim : BSPhysObject
{ {
OMV.Vector3 localInertia = BulletSimAPI.CalculateLocalInertia2(BSShape.ptr, physMass); OMV.Vector3 localInertia = BulletSimAPI.CalculateLocalInertia2(BSShape.ptr, physMass);
BulletSimAPI.SetMassProps2(BSBody.ptr, physMass, localInertia); BulletSimAPI.SetMassProps2(BSBody.ptr, physMass, localInertia);
BulletSimAPI.UpdateInertiaTensor2(BSBody.ptr); // center of mass is at the zero of the object
BulletSimAPI.SetCenterOfMassByPosRot2(BSBody.ptr, ForcePosition, ForceOrientation);
// BulletSimAPI.UpdateInertiaTensor2(BSBody.ptr);
DetailLog("{0},BSPrim.UpdateMassProperties,mass={1},localInertia={2}", LocalID, physMass, localInertia); DetailLog("{0},BSPrim.UpdateMassProperties,mass={1},localInertia={2}", LocalID, physMass, localInertia);
} }
} }
@ -462,9 +464,16 @@ public sealed class BSPrim : BSPhysObject
// Called from Scene when doing simulation step so we're in taint processing time. // Called from Scene when doing simulation step so we're in taint processing time.
public override void StepVehicle(float timeStep) public override void StepVehicle(float timeStep)
{ {
if (IsPhysical) if (IsPhysical && _vehicle.IsActive)
{ {
_vehicle.Step(timeStep); _vehicle.Step(timeStep);
/* // TEST TEST DEBUG DEBUG -- trying to reduce the extra action of Bullet simulation step
PhysicsScene.PostTaintObject("BSPrim.StepVehicles", LocalID, delegate()
{
// This resets the interpolation values and recomputes the tensor variables
BulletSimAPI.SetCenterOfMassByPosRot2(BSBody.ptr, ForcePosition, ForceOrientation);
});
*/
} }
} }
@ -502,7 +511,9 @@ public sealed class BSPrim : BSPhysObject
} }
public override OMV.Vector3 Torque { public override OMV.Vector3 Torque {
get { return _torque; } get { return _torque; }
set { _torque = value; set {
_torque = value;
AddAngularForce(_torque, false, false);
// DetailLog("{0},BSPrim.SetTorque,call,torque={1}", LocalID, _torque); // DetailLog("{0},BSPrim.SetTorque,call,torque={1}", LocalID, _torque);
} }
} }
@ -831,7 +842,7 @@ public sealed class BSPrim : BSPhysObject
// m_log.DebugFormat("{0}: RotationalVelocity={1}", LogHeader, _rotationalVelocity); // m_log.DebugFormat("{0}: RotationalVelocity={1}", LogHeader, _rotationalVelocity);
PhysicsScene.TaintedObject("BSPrim.setRotationalVelocity", delegate() PhysicsScene.TaintedObject("BSPrim.setRotationalVelocity", delegate()
{ {
// DetailLog("{0},BSPrim.SetRotationalVel,taint,rotvel={1}", LocalID, _rotationalVelocity); DetailLog("{0},BSPrim.SetRotationalVel,taint,rotvel={1}", LocalID, _rotationalVelocity);
BulletSimAPI.SetAngularVelocity2(BSBody.ptr, _rotationalVelocity); BulletSimAPI.SetAngularVelocity2(BSBody.ptr, _rotationalVelocity);
}); });
} }
@ -972,14 +983,31 @@ public sealed class BSPrim : BSPhysObject
} }
m_accumulatedAngularForces.Clear(); m_accumulatedAngularForces.Clear();
} }
// DetailLog("{0},BSPrim.AddAngularForce,taint,aForce={1}", LocalID, fSum); DetailLog("{0},BSPrim.AddAngularForce,taint,aForce={1}", LocalID, fSum);
if (fSum != OMV.Vector3.Zero) if (fSum != OMV.Vector3.Zero)
{
BulletSimAPI.ApplyTorque2(BSBody.ptr, fSum); BulletSimAPI.ApplyTorque2(BSBody.ptr, fSum);
_torque = fSum;
}
}; };
if (inTaintTime) if (inTaintTime)
addAngularForceOperation(); addAngularForceOperation();
else else
PhysicsScene.TaintedObject("BSPrim.AddForce", addAngularForceOperation); PhysicsScene.TaintedObject("BSPrim.AddAngularForce", addAngularForceOperation);
}
// A torque impulse.
public void ApplyTorqueImpulse(OMV.Vector3 impulse, bool inTaintTime)
{
OMV.Vector3 applyImpulse = impulse;
BSScene.TaintCallback applyTorqueImpulseOperation = delegate()
{
DetailLog("{0},BSPrim.ApplyTorqueImpulse,taint,tImpulse={1}", LocalID, applyImpulse);
BulletSimAPI.ApplyTorqueImpulse2(BSBody.ptr, applyImpulse);
};
if (inTaintTime)
applyTorqueImpulseOperation();
else
PhysicsScene.TaintedObject("BSPrim.ApplyTorqueImpulse", applyTorqueImpulseOperation);
} }
public override void SetMomentum(OMV.Vector3 momentum) { public override void SetMomentum(OMV.Vector3 momentum) {
// DetailLog("{0},BSPrim.SetMomentum,call,mom={1}", LocalID, momentum); // DetailLog("{0},BSPrim.SetMomentum,call,mom={1}", LocalID, momentum);
@ -1418,8 +1446,9 @@ public sealed class BSPrim : BSPhysObject
PositionSanityCheck(true); PositionSanityCheck(true);
DetailLog("{0},BSPrim.UpdateProperties,call,pos={1},orient={2},vel={3},accel={4},rotVel={5}", OMV.Vector3 direction = OMV.Vector3.UnitX * _orientation;
LocalID, _position, _orientation, _velocity, _acceleration, _rotationalVelocity); DetailLog("{0},BSPrim.UpdateProperties,call,pos={1},orient={2},dir={3},vel={4},rotVel={5}",
LocalID, _position, _orientation, direction, _velocity, _rotationalVelocity);
// BulletSimAPI.DumpRigidBody2(PhysicsScene.World.ptr, BSBody.ptr); // DEBUG DEBUG DEBUG // BulletSimAPI.DumpRigidBody2(PhysicsScene.World.ptr, BSBody.ptr); // DEBUG DEBUG DEBUG