From 28e2cd3fa21835b124552dec024745f5784f6b3a Mon Sep 17 00:00:00 2001 From: Robert Adams Date: Wed, 31 Oct 2012 09:26:58 -0700 Subject: 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. --- OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs | 136 ++++++++------------- .../Physics/BulletSPlugin/BSLinksetConstraints.cs | 2 +- OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs | 45 +++++-- 3 files changed, 90 insertions(+), 93 deletions(-) diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs index 4981007..5c61774 100644 --- a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs @@ -511,21 +511,6 @@ namespace OpenSim.Region.Physics.BulletSPlugin // Do any updating needed for a vehicle 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) { // Friction effects are handled by this vehicle code @@ -539,29 +524,21 @@ namespace OpenSim.Region.Physics.BulletSPlugin { 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); MoveAngular(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. - 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 m_lastPositionVector = Prim.ForcePosition; @@ -583,7 +560,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin Vector3 vehicleVelocity = Prim.ForceVelocity * Quaternion.Inverse(Prim.ForceOrientation); // DEBUG // 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 m_lastLinearVelocityVector += addAmount; @@ -593,11 +570,12 @@ namespace OpenSim.Region.Physics.BulletSPlugin Vector3 frictionFactor = (Vector3.One / m_linearFrictionTimescale) * pTimestep; m_lastLinearVelocityVector *= (Vector3.One - frictionFactor); - VDetailLog("{0},MoveLinear,nonZero,origdir={1},origvel={2},vehVel={3},add={4},decay={5},lmDir={6},lmVel={7}", - Prim.LocalID, origDir, origVel, vehicleVelocity, addAmount, decayFactor, m_linearMotorDirection, m_lastLinearVelocityVector); - - // convert requested object velocity to object relative vector + // Rotate new object velocity from vehicle relative to world coordinates 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 { @@ -609,18 +587,15 @@ namespace OpenSim.Region.Physics.BulletSPlugin 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) - - // add Gravity and Buoyancy + // Gravity and Buoyancy // There is some gravity, make a gravity force vector that is applied after object velocity. // 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); /* - * 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 Vector3 vel_now = m_prim.Velocity; m_dir.Z = vel_now.Z; // Preserve the accumulated falling velocity @@ -676,7 +651,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin else { 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); // Replace Vertical speed with correction figure if significant if (Math.Abs(verticalError) > 0.01f) @@ -784,37 +759,24 @@ namespace OpenSim.Region.Physics.BulletSPlugin // m_angularFrictionTimescale // body angular velocity decay rate // m_lastAngularVelocity // what was last applied to body - // Get what the body is doing, this includes 'external' influences - Vector3 angularVelocity = Prim.ForceRotationalVelocity; - - if (m_angularMotorApply > 0) + if (m_angularMotorDirection.LengthSquared() > 0.0001) { - // 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 origDir = m_angularMotorDirection; - // ramp up to new value // new velocity += error / ( time to get there / step interval) // requested speed - last motor speed 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}", - Prim.LocalID, m_angularMotorApply, m_angularMotorTimescale, pTimestep, origVel, origDir, m_angularMotorVelocity); - - m_angularMotorApply--; + VDetailLog("{0},MoveAngular,angularMotorApply,angTScale={1},timeStep={2},origvel={3},origDir={4},vel={5}", + Prim.LocalID, m_angularMotorTimescale, pTimestep, origVel, origDir, m_angularMotorVelocity); } else { - // No motor recently applied, keep the body velocity - // and decay the velocity - if (m_angularMotorVelocity.LengthSquared() < 0.0001) - m_angularMotorVelocity = Vector3.Zero; - else - m_angularMotorVelocity -= m_angularMotorVelocity / (m_angularMotorDecayTimescale / pTimestep); - } // end motor section + m_angularMotorVelocity = Vector3.Zero; + } #region Vertical attactor @@ -824,22 +786,19 @@ namespace OpenSim.Region.Physics.BulletSPlugin if (m_verticalAttractionTimescale < 300 && m_lastAngularVelocity != Vector3.Zero) { - float VAservo = 0.2f; + float VAservo = pTimestep * 0.2f / m_verticalAttractionTimescale; if (Prim.Linkset.LinksetIsColliding) - VAservo = 0.05f / (m_verticalAttractionTimescale / pTimestep); + VAservo = pTimestep * 0.05f / (m_verticalAttractionTimescale); VAservo *= (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency); - // get present body rotation - Quaternion rotq = Prim.ForceOrientation; - // vector pointing up - Vector3 verticalError = Vector3.UnitZ; - - // rotate it to Body Angle - verticalError = verticalError * rotq; - // 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. + // Create a vector of the vehicle "up" in world coordinates + Vector3 verticalError = Vector3.UnitZ * Prim.ForceOrientation; + // 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) if (verticalError.Z < 0.0f) @@ -850,13 +809,15 @@ namespace OpenSim.Region.Physics.BulletSPlugin // scale it by VAservo verticalError = verticalError * VAservo; - // As the body rotates around the X axis, then verticalError.Y increases; Rotated around Y then .X increases, so - // Change Body angular velocity X based on Y, and Y based on X. Z is not changed. + // As the body rotates around the X axis, then verticalError.Y increases; Rotated around Y + // 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.Y = - verticalError.X; vertattr.Z = 0f; // scaling appears better usingsquare-law + Vector3 angularVelocity = Prim.ForceRotationalVelocity; float bounce = 1.0f - (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency); vertattr.X += bounce * angularVelocity.X; vertattr.Y += bounce * angularVelocity.Y; @@ -956,15 +917,22 @@ namespace OpenSim.Region.Physics.BulletSPlugin 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 // The above calculates the absolute angular velocity needed - Prim.ForceRotationalVelocity = m_lastAngularVelocity; + // Prim.ForceRotationalVelocity = 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,decay={1},lastAngular={2}", Prim.LocalID, decayamount, m_lastAngularVelocity); + VDetailLog("{0},MoveAngular,done,applyAForce={1},decay={2},lastAngular={3}", + Prim.LocalID, applyAngularForce, decayamount, m_lastAngularVelocity); } //end MoveAngular internal void LimitRotation(float timestep) diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs index ecb6ad4..8cdbd9b 100755 --- a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs @@ -299,7 +299,7 @@ public sealed class BSLinksetConstraints : BSLinkset // DEBUG: see of inter-linkset collisions are causing problems // BulletSimAPI.SetCollisionFilterMask2(LinksetRoot.BSBody.ptr, // (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); foreach (BSPhysObject child in m_children) diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs index 7851a40..9ced61fa 100644 --- a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs @@ -378,7 +378,9 @@ public sealed class BSPrim : BSPhysObject { OMV.Vector3 localInertia = BulletSimAPI.CalculateLocalInertia2(BSShape.ptr, physMass); 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); } } @@ -462,9 +464,16 @@ public sealed class BSPrim : BSPhysObject // Called from Scene when doing simulation step so we're in taint processing time. public override void StepVehicle(float timeStep) { - if (IsPhysical) + if (IsPhysical && _vehicle.IsActive) { _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 { get { return _torque; } - set { _torque = value; + set { + _torque = value; + AddAngularForce(_torque, false, false); // 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); 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); }); } @@ -972,14 +983,31 @@ public sealed class BSPrim : BSPhysObject } 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) + { BulletSimAPI.ApplyTorque2(BSBody.ptr, fSum); + _torque = fSum; + } }; if (inTaintTime) addAngularForceOperation(); 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) { // DetailLog("{0},BSPrim.SetMomentum,call,mom={1}", LocalID, momentum); @@ -1418,8 +1446,9 @@ public sealed class BSPrim : BSPhysObject PositionSanityCheck(true); - DetailLog("{0},BSPrim.UpdateProperties,call,pos={1},orient={2},vel={3},accel={4},rotVel={5}", - LocalID, _position, _orientation, _velocity, _acceleration, _rotationalVelocity); + OMV.Vector3 direction = OMV.Vector3.UnitX * _orientation; + 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 -- cgit v1.1