From a9b9c0f0351a6749bf55c29b2891fa40928b4c39 Mon Sep 17 00:00:00 2001 From: Robert Adams Date: Tue, 18 Dec 2012 23:05:59 -0800 Subject: BulletSim: improve angularVerticalAttraction calculation to compute angular correction velocity rather than estimating correction (excuse to use trig functions). --- OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs | 33 ++++++++++------------ 1 file changed, 15 insertions(+), 18 deletions(-) (limited to 'OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs') diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs index 5887249..912aadd 100644 --- a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs @@ -1003,7 +1003,6 @@ namespace OpenSim.Region.Physics.BulletSPlugin // Not colliding if the vehicle is off the ground if (!Prim.IsColliding) { - // downForce = new Vector3(0, 0, (-distanceAboveGround / m_bankingTimescale) * pTimestep); // downForce = new Vector3(0, 0, -distanceAboveGround / m_bankingTimescale); ret = new Vector3(0, 0, -distanceAboveGround); } @@ -1135,31 +1134,30 @@ namespace OpenSim.Region.Physics.BulletSPlugin // zero and one. // The value of Z is how far the rotation is off with 1 meaning none and 0 being 90 degrees. + // Y error means needed rotation around X axis and visa versa. + // Since the error goes from zero to one, the asin is the corresponding angle. + ret.X = (float)Math.Asin(verticalError.Y); + ret.Y = (float)Math.Asin(verticalError.X); + // If verticalError.Z is negative, the vehicle is upside down. Add additional push. if (verticalError.Z < 0f) { - verticalError.X = 2f - verticalError.X; - verticalError.Y = 2f - verticalError.Y; + ret.X += (float)Math.PI / 4f; + ret.Y += (float)Math.PI / 4f; } - // Y error means needed rotation around X axis and visa versa. - ret.X = verticalError.Y; - ret.Y = - verticalError.X; - ret.Z = 0f; - - // Scale the correction force by how far we're off from vertical. - // Z error of one says little error. As Z gets smaller, the vehicle is leaning farther over. - float clampedSqrZError = ClampInRange(0.01f, verticalError.Z * verticalError.Z, 1f); - float vertForce = 1f / clampedSqrZError; - - ret *= vertForce; + // Put the signs back on so the rotation is in the correct direction. + ret.X *= (float)Math.Sign(verticalError.Y); + // (Tilt forward (positive X) needs to tilt back (rotate negative) around Y axis.) + ret.Y *= -(float)Math.Sign(verticalError.X); - // Correction happens over a number of seconds. + // 'ret' is now the necessary velocity to correct tilt in one second. + // Correction happens over a number of seconds. Vector3 unscaledContrib = ret; ret /= m_verticalAttractionTimescale; - VDetailLog("{0}, MoveAngular,verticalAttraction,,verticalError={1},unscaled={2},vertForce={3},eff={4},vertAttr={5}", - Prim.LocalID, verticalError, unscaledContrib, vertForce, m_verticalAttractionEfficiency, ret); + VDetailLog("{0}, MoveAngular,verticalAttraction,,verticalError={1},unscaled={2},eff={3},vertAttr={4}", + Prim.LocalID, verticalError, unscaledContrib, m_verticalAttractionEfficiency, ret); } return ret; } @@ -1172,7 +1170,6 @@ namespace OpenSim.Region.Physics.BulletSPlugin public Vector3 ComputeAngularDeflection() { Vector3 ret = Vector3.Zero; - return ret; // DEBUG DEBUG DEBUG debug one force at a time if (m_angularDeflectionEfficiency != 0) { -- cgit v1.1 From e73dac4debcf79cba83b94255d62fce0815871cb Mon Sep 17 00:00:00 2001 From: Robert Adams Date: Thu, 20 Dec 2012 10:19:16 -0800 Subject: BulletSim: angularMotorUp working again (seems a little slow as it takes longer than timescale to correct, but getting better). Disabled angularDeflection (need to resolve interactions between angular corrections). Update TODO list. --- OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs | 65 +++++++++++++++------- 1 file changed, 44 insertions(+), 21 deletions(-) (limited to 'OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs') diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs index 912aadd..77ec76d 100644 --- a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs @@ -91,6 +91,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin private Vector3 m_lastVertAttractor = Vector3.Zero; // what VA was last applied to body //Deflection properties + private BSVMotor m_angularDeflectionMotor = new BSVMotor("AngularDeflection"); private float m_angularDeflectionEfficiency = 0; private float m_angularDeflectionTimescale = 0; private float m_linearDeflectionEfficiency = 0; @@ -102,6 +103,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin private float m_bankingTimescale = 0; //Hover and Buoyancy properties + private BSVMotor m_hoverMotor = new BSVMotor("Hover"); private float m_VhoverHeight = 0f; private float m_VhoverEfficiency = 0f; private float m_VhoverTimescale = 0f; @@ -118,6 +120,10 @@ namespace OpenSim.Region.Physics.BulletSPlugin // Timescale > cutoff means no vert attractor. private float m_verticalAttractionTimescale = 510f; + // Just some recomputed constants: + static readonly float PIOverFour = ((float)Math.PI) / 4f; + static readonly float PIOverTwo = ((float)Math.PI) / 2f; + public BSDynamics(BSScene myScene, BSPrim myPrim) { PhysicsScene = myScene; @@ -563,9 +569,6 @@ namespace OpenSim.Region.Physics.BulletSPlugin // Vehicles report collision events so we know when it's on the ground BulletSimAPI.AddToCollisionFlags2(Prim.PhysBody.ptr, CollisionFlags.BS_VEHICLE_COLLISIONS); - // DEBUG DEBUG DEBUG: use uniform inertia to smooth movement added by Bullet - // Vector3 localInertia = new Vector3(1f, 1f, 1f); - // Vector3 localInertia = new Vector3(m_vehicleMass, m_vehicleMass, m_vehicleMass); Vector3 localInertia = BulletSimAPI.CalculateLocalInertia2(Prim.PhysShape.ptr, m_vehicleMass); BulletSimAPI.SetMassProps2(Prim.PhysBody.ptr, m_vehicleMass, localInertia); BulletSimAPI.UpdateInertiaTensor2(Prim.PhysBody.ptr); @@ -613,7 +616,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin private Quaternion? m_knownOrientation; private Vector3? m_knownRotationalVelocity; private Vector3 m_knownRotationalForce; - private float? m_knownForwardSpeed; + private Vector3? m_knownForwardVelocity; // vehicle relative forward speed private const int m_knownChangedPosition = 1 << 0; private const int m_knownChangedVelocity = 1 << 1; @@ -632,7 +635,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin m_knownOrientation = null; m_knownRotationalVelocity = null; m_knownRotationalForce = Vector3.Zero; - m_knownForwardSpeed = null; + m_knownForwardVelocity = null; m_knownChanged = 0; } private void PushKnownChanged() @@ -755,13 +758,21 @@ namespace OpenSim.Region.Physics.BulletSPlugin m_knownRotationalForce += aForce; m_knownChanged |= m_knownChangedRotationalForce; } + // Vehicle relative forward velocity + private Vector3 VehicleForwardVelocity + { + get + { + if (m_knownForwardVelocity == null) + m_knownForwardVelocity = VehicleVelocity * Quaternion.Inverse(Quaternion.Normalize(VehicleOrientation)); + return (Vector3)m_knownForwardVelocity; + } + } private float VehicleForwardSpeed { get { - if (m_knownForwardSpeed == null) - m_knownForwardSpeed = (VehicleVelocity * Quaternion.Inverse(VehicleOrientation)).X; - return (float)m_knownForwardSpeed; + return VehicleForwardVelocity.X; } } @@ -1025,7 +1036,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin // set directly on the vehicle. private void MoveAngular(float pTimestep) { - // The user wants how many radians per second angular change? + // The user wants this many radians per second angular change? Vector3 angularMotorContribution = m_angularMotor.Step(pTimestep); // ================================================================== @@ -1137,27 +1148,23 @@ namespace OpenSim.Region.Physics.BulletSPlugin // Y error means needed rotation around X axis and visa versa. // Since the error goes from zero to one, the asin is the corresponding angle. ret.X = (float)Math.Asin(verticalError.Y); - ret.Y = (float)Math.Asin(verticalError.X); + // (Tilt forward (positive X) needs to tilt back (rotate negative) around Y axis.) + ret.Y = -(float)Math.Asin(verticalError.X); // If verticalError.Z is negative, the vehicle is upside down. Add additional push. if (verticalError.Z < 0f) { - ret.X += (float)Math.PI / 4f; - ret.Y += (float)Math.PI / 4f; + ret.X += PIOverFour; + ret.Y += PIOverFour; } - // Put the signs back on so the rotation is in the correct direction. - ret.X *= (float)Math.Sign(verticalError.Y); - // (Tilt forward (positive X) needs to tilt back (rotate negative) around Y axis.) - ret.Y *= -(float)Math.Sign(verticalError.X); - // 'ret' is now the necessary velocity to correct tilt in one second. // Correction happens over a number of seconds. Vector3 unscaledContrib = ret; ret /= m_verticalAttractionTimescale; - VDetailLog("{0}, MoveAngular,verticalAttraction,,verticalError={1},unscaled={2},eff={3},vertAttr={4}", - Prim.LocalID, verticalError, unscaledContrib, m_verticalAttractionEfficiency, ret); + VDetailLog("{0}, MoveAngular,verticalAttraction,,verticalError={1},unscaled={2},eff={3},ts={4},vertAttr={5}", + Prim.LocalID, verticalError, unscaledContrib, m_verticalAttractionEfficiency, m_verticalAttractionTimescale, ret); } return ret; } @@ -1170,6 +1177,12 @@ namespace OpenSim.Region.Physics.BulletSPlugin public Vector3 ComputeAngularDeflection() { Vector3 ret = Vector3.Zero; + return ret; // DEBUG DEBUG DEBUG + // Disable angular deflection for the moment. + // Since angularMotorUp and angularDeflection are computed independently, they will calculate + // approximately the same X or Y correction. When added together (when contributions are combined) + // this creates an over-correction and then wabbling as the target is overshot. + // TODO: rethink how the different correction computations inter-relate. if (m_angularDeflectionEfficiency != 0) { @@ -1181,15 +1194,24 @@ namespace OpenSim.Region.Physics.BulletSPlugin Vector3 pointingDirection = Vector3.UnitX * VehicleOrientation; pointingDirection.Normalize(); - // The difference between what is and what should be + // The difference between what is and what should be. Vector3 deflectionError = movingDirection - pointingDirection; + // Don't try to correct very large errors (not our job) + if (Math.Abs(deflectionError.X) > PIOverFour) deflectionError.X = 0f; + if (Math.Abs(deflectionError.Y) > PIOverFour) deflectionError.Y = 0f; + if (Math.Abs(deflectionError.Z) > PIOverFour) deflectionError.Z = 0f; + + // ret = m_angularDeflectionCorrectionMotor(1f, deflectionError); + // Scale the correction by recovery timescale and efficiency - ret = (-deflectionError * VehicleForwardSpeed) * m_angularDeflectionEfficiency; + ret = (-deflectionError) * m_angularDeflectionEfficiency; ret /= m_angularDeflectionTimescale; VDetailLog("{0}, MoveAngular,Deflection,movingDir={1},pointingDir={2},deflectError={3},ret={4}", Prim.LocalID, movingDirection, pointingDirection, deflectionError, ret); + VDetailLog("{0}, MoveAngular,Deflection,fwdSpd={1},defEff={2},defTS={3}", + Prim.LocalID, VehicleForwardSpeed, m_angularDeflectionEfficiency, m_angularDeflectionTimescale); } return ret; } @@ -1305,6 +1327,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin private float ClampInRange(float low, float val, float high) { return Math.Max(low, Math.Min(val, high)); + // return Utils.Clamp(val, low, high); } // Invoke the detailed logger and output something if it's enabled. -- cgit v1.1 From a5b2539cf92aac65c11a36d4eb41d04f1c55b042 Mon Sep 17 00:00:00 2001 From: Robert Adams Date: Thu, 20 Dec 2012 10:32:33 -0800 Subject: BulletSim: replace use of funky nullable values for vehicle property update control (m_known* stuff). Bitmaps will be quicker to test and to clear. --- OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs | 69 ++++++++++++++-------- 1 file changed, 43 insertions(+), 26 deletions(-) (limited to 'OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs') diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs index 77ec76d..c3d75ea 100644 --- a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs @@ -602,21 +602,22 @@ namespace OpenSim.Region.Physics.BulletSPlugin #region Known vehicle value functions // Vehicle physical parameters that we buffer from constant getting and setting. - // The "m_known*" variables are initialized to 'null', fetched only if referenced - // and stored back into the physics engine only if updated. + // The "m_known*" values are unknown until they are fetched and the m_knownHas flag is set. + // Changing is remembered and the parameter is stored back into the physics engine only if updated. // This does two things: 1) saves continuious calls into unmanaged code, and // 2) signals when a physics property update must happen back to the simulator // to update values modified for the vehicle. private int m_knownChanged; - private float? m_knownTerrainHeight; - private float? m_knownWaterLevel; - private Vector3? m_knownPosition; - private Vector3? m_knownVelocity; + private int m_knownHas; + private float m_knownTerrainHeight; + private float m_knownWaterLevel; + private Vector3 m_knownPosition; + private Vector3 m_knownVelocity; private Vector3 m_knownForce; - private Quaternion? m_knownOrientation; - private Vector3? m_knownRotationalVelocity; + private Quaternion m_knownOrientation; + private Vector3 m_knownRotationalVelocity; private Vector3 m_knownRotationalForce; - private Vector3? m_knownForwardVelocity; // vehicle relative forward speed + private Vector3 m_knownForwardVelocity; // vehicle relative forward speed private const int m_knownChangedPosition = 1 << 0; private const int m_knownChangedVelocity = 1 << 1; @@ -624,18 +625,13 @@ namespace OpenSim.Region.Physics.BulletSPlugin private const int m_knownChangedOrientation = 1 << 3; private const int m_knownChangedRotationalVelocity = 1 << 4; private const int m_knownChangedRotationalForce = 1 << 5; + private const int m_knownChangedTerrainHeight = 1 << 6; + private const int m_knownChangedWaterLevel = 1 << 7; + private const int m_knownChangedForwardVelocity = 1 << 7; private void ForgetKnownVehicleProperties() { - m_knownTerrainHeight = null; - m_knownWaterLevel = null; - m_knownPosition = null; - m_knownVelocity = null; - m_knownForce = Vector3.Zero; - m_knownOrientation = null; - m_knownRotationalVelocity = null; - m_knownRotationalForce = Vector3.Zero; - m_knownForwardVelocity = null; + m_knownHas = 0; m_knownChanged = 0; } private void PushKnownChanged() @@ -674,17 +670,23 @@ namespace OpenSim.Region.Physics.BulletSPlugin // is used ot fetch the height only once for each vehicle simulation step. private float GetTerrainHeight(Vector3 pos) { - if (m_knownTerrainHeight == null) + if ((m_knownHas & m_knownChangedTerrainHeight) == 0) + { m_knownTerrainHeight = Prim.PhysicsScene.TerrainManager.GetTerrainHeightAtXYZ(pos); - return (float)m_knownTerrainHeight; + m_knownHas |= m_knownChangedTerrainHeight; + } + return m_knownTerrainHeight; } // Since the computation of water level can be a little involved, this routine // is used ot fetch the level only once for each vehicle simulation step. private float GetWaterLevel(Vector3 pos) { - if (m_knownWaterLevel == null) + if ((m_knownHas & m_knownChangedWaterLevel) == 0) + { m_knownWaterLevel = Prim.PhysicsScene.TerrainManager.GetWaterLevelAtXYZ(pos); + m_knownHas |= m_knownChangedWaterLevel; + } return (float)m_knownWaterLevel; } @@ -692,8 +694,11 @@ namespace OpenSim.Region.Physics.BulletSPlugin { get { - if (m_knownPosition == null) + if ((m_knownHas & m_knownChangedPosition) == 0) + { m_knownPosition = Prim.ForcePosition; + m_knownHas |= m_knownChangedPosition; + } return (Vector3)m_knownPosition; } set @@ -707,8 +712,11 @@ namespace OpenSim.Region.Physics.BulletSPlugin { get { - if (m_knownOrientation == null) + if ((m_knownHas & m_knownChangedOrientation) == 0) + { m_knownOrientation = Prim.ForceOrientation; + m_knownHas |= m_knownChangedOrientation; + } return (Quaternion)m_knownOrientation; } set @@ -722,8 +730,11 @@ namespace OpenSim.Region.Physics.BulletSPlugin { get { - if (m_knownVelocity == null) + if ((m_knownHas & m_knownChangedVelocity) == 0) + { m_knownVelocity = Prim.ForceVelocity; + m_knownHas |= m_knownChangedVelocity; + } return (Vector3)m_knownVelocity; } set @@ -743,8 +754,11 @@ namespace OpenSim.Region.Physics.BulletSPlugin { get { - if (m_knownRotationalVelocity == null) + if ((m_knownHas & m_knownChangedRotationalVelocity) == 0) + { m_knownRotationalVelocity = Prim.ForceRotationalVelocity; + m_knownHas |= m_knownChangedRotationalVelocity; + } return (Vector3)m_knownRotationalVelocity; } set @@ -763,8 +777,11 @@ namespace OpenSim.Region.Physics.BulletSPlugin { get { - if (m_knownForwardVelocity == null) + if ((m_knownHas & m_knownChangedForwardVelocity) == 0) + { m_knownForwardVelocity = VehicleVelocity * Quaternion.Inverse(Quaternion.Normalize(VehicleOrientation)); + m_knownHas |= m_knownChangedForwardVelocity; + } return (Vector3)m_knownForwardVelocity; } } -- cgit v1.1 From 9d2f569d600cad688ac7e06fcac7e94023b3600a Mon Sep 17 00:00:00 2001 From: Robert Adams Date: Thu, 20 Dec 2012 16:06:52 -0800 Subject: BulletSim: fix incorrectly defined property changed flag. --- OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) (limited to 'OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs') diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs index c3d75ea..a5acd86 100644 --- a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs @@ -627,7 +627,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin private const int m_knownChangedRotationalForce = 1 << 5; private const int m_knownChangedTerrainHeight = 1 << 6; private const int m_knownChangedWaterLevel = 1 << 7; - private const int m_knownChangedForwardVelocity = 1 << 7; + private const int m_knownChangedForwardVelocity = 1 << 8; private void ForgetKnownVehicleProperties() { @@ -860,13 +860,11 @@ namespace OpenSim.Region.Physics.BulletSPlugin // ================================================================== // Clamp high or low velocities float newVelocityLengthSq = newVelocity.LengthSquared(); - // if (newVelocityLengthSq > 1e6f) if (newVelocityLengthSq > 1000f) { newVelocity /= newVelocity.Length(); newVelocity *= 1000f; } - // else if (newVelocityLengthSq < 1e-6f) else if (newVelocityLengthSq < 0.001f) newVelocity = Vector3.Zero; -- cgit v1.1