From fb903ff49089d5fd7a56aa2401528c3e7cf1800c Mon Sep 17 00:00:00 2001 From: Robert Adams Date: Sun, 10 Feb 2013 09:51:34 -0800 Subject: BulletSim: More work on center-of-mass. Remove linksetinfo and rely on simulator to update info. --- .../Region/Physics/BulletSPlugin/BSApiTemplate.cs | 7 +- .../Region/Physics/BulletSPlugin/BSCharacter.cs | 2 +- OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs | 3 + .../Physics/BulletSPlugin/BSLinksetCompound.cs | 150 ++++++++++----------- .../Region/Physics/BulletSPlugin/BSPhysObject.cs | 31 +++-- OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs | 4 +- .../Physics/BulletSPlugin/BSPrimDisplaced.cs | 85 ++++++++---- .../Region/Physics/BulletSPlugin/BSPrimLinkable.cs | 4 +- 8 files changed, 162 insertions(+), 124 deletions(-) (limited to 'OpenSim/Region') diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSApiTemplate.cs b/OpenSim/Region/Physics/BulletSPlugin/BSApiTemplate.cs index 7ab86d2..3f83ef0 100644 --- a/OpenSim/Region/Physics/BulletSPlugin/BSApiTemplate.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BSApiTemplate.cs @@ -225,9 +225,10 @@ public enum CollisionFlags : uint CF_DISABLE_VISUALIZE_OBJECT = 1 << 5, CF_DISABLE_SPU_COLLISION_PROCESS = 1 << 6, // Following used by BulletSim to control collisions and updates - BS_SUBSCRIBE_COLLISION_EVENTS = 1 << 10, - BS_FLOATS_ON_WATER = 1 << 11, - BS_VEHICLE_COLLISIONS = 1 << 12, + BS_SUBSCRIBE_COLLISION_EVENTS = 1 << 10, // return collision events from unmanaged to managed + BS_FLOATS_ON_WATER = 1 << 11, // the object should float at water level + BS_VEHICLE_COLLISIONS = 1 << 12, // return collisions for vehicle ground checking + BS_RETURN_ROOT_COMPOUND_SHAPE = 1 << 13, // return the pos/rot of the root shape in a compound shape BS_NONE = 0, BS_ALL = 0xFFFFFFFF }; diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs index f781aea..04fb05b 100644 --- a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs @@ -274,7 +274,7 @@ public sealed class BSCharacter : BSPhysObject // This test is done if moving forward, not flying and is colliding with something. // DetailLog("{0},BSCharacter.WalkUpStairs,IsColliding={1},flying={2},targSpeed={3},collisions={4}", // LocalID, IsColliding, Flying, TargetSpeed, CollisionsLastTick.Count); - if (IsColliding && !Flying && TargetSpeed > 0.1f /* && ForwardSpeed < 0.1f */) + if (IsColliding && !Flying && TargetVelocitySpeed > 0.1f /* && ForwardSpeed < 0.1f */) { // The range near the character's feet where we will consider stairs float nearFeetHeightMin = RawPosition.Z - (Size.Z / 2f) + 0.05f; diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs index e35311f..4ece1eb 100755 --- a/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs @@ -127,6 +127,8 @@ public abstract class BSLinkset m_children = new HashSet(); LinksetMass = parent.RawMass; Rebuilding = false; + + parent.ClearDisplacement(); } // Link to a linkset where the child knows the parent. @@ -280,6 +282,7 @@ public abstract class BSLinkset return mass; } + // Computes linkset's center of mass in world coordinates. protected virtual OMV.Vector3 ComputeLinksetCenterOfMass() { OMV.Vector3 com; diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs index 36bae9b..1f66b56 100755 --- a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs @@ -93,7 +93,8 @@ public sealed class BSLinksetCompound : BSLinkset { private static string LogHeader = "[BULLETSIM LINKSET COMPOUND]"; - public BSLinksetCompound(BSScene scene, BSPrimLinkable parent) : base(scene, parent) + public BSLinksetCompound(BSScene scene, BSPrimLinkable parent) + : base(scene, parent) { } @@ -217,59 +218,45 @@ public sealed class BSLinksetCompound : BSLinkset // and that is caused by us updating the object. if ((whichUpdated & ~(UpdatedProperties.Position | UpdatedProperties.Orientation)) == 0) { - // Gather the child info. It might not be there if the linkset is in transition. - BSLinksetCompoundInfo lsi = updated.LinksetInfo as BSLinksetCompoundInfo; - if (lsi != null) - { - // Since the child moved or rotationed, it needs a new relative position within the linkset - BSLinksetCompoundInfo newLsi = new BSLinksetCompoundInfo(lsi.Index, LinksetRoot, updated, OMV.Vector3.Zero); - updated.LinksetInfo = newLsi; - // Find the physical instance of the child - if (LinksetRoot.PhysShape.HasPhysicalShape && PhysicsScene.PE.IsCompound(LinksetRoot.PhysShape)) + if (LinksetRoot.PhysShape.HasPhysicalShape && PhysicsScene.PE.IsCompound(LinksetRoot.PhysShape)) + { + // It is possible that the linkset is still under construction and the child is not yet + // inserted into the compound shape. A rebuild of the linkset in a pre-step action will + // build the whole thing with the new position or rotation. + // The index must be checked because Bullet references the child array but does no validity + // checking of the child index passed. + int numLinksetChildren = PhysicsScene.PE.GetNumberOfCompoundChildren(LinksetRoot.PhysShape); + if (updated.LinksetChildIndex < numLinksetChildren) { - // It is possible that the linkset is still under construction and the child is not yet - // inserted into the compound shape. A rebuild of the linkset in a pre-step action will - // build the whole thing with the new position or rotation. - // The index must be checked because Bullet references the child array but does no validity - // checking of the child index passed. - int numLinksetChildren = PhysicsScene.PE.GetNumberOfCompoundChildren(LinksetRoot.PhysShape); - if (lsi.Index < numLinksetChildren) + BulletShape linksetChildShape = PhysicsScene.PE.GetChildShapeFromCompoundShapeIndex(LinksetRoot.PhysShape, updated.LinksetChildIndex); + if (linksetChildShape.HasPhysicalShape) { - BulletShape linksetChildShape = PhysicsScene.PE.GetChildShapeFromCompoundShapeIndex(LinksetRoot.PhysShape, lsi.Index); - if (linksetChildShape.HasPhysicalShape) - { - // Found the child shape within the compound shape - PhysicsScene.PE.UpdateChildTransform(LinksetRoot.PhysShape, lsi.Index, - newLsi.OffsetFromCenterOfMass, - newLsi.OffsetRot, - true /* shouldRecalculateLocalAabb */); - updatedChild = true; - DetailLog("{0},BSLinksetCompound.UpdateProperties,changeChildPosRot,whichUpdated={1},newLsi={2}", - updated.LocalID, whichUpdated, newLsi); - } - else // DEBUG DEBUG - { // DEBUG DEBUG - DetailLog("{0},BSLinksetCompound.UpdateProperties,couldNotUpdateChild,noChildShape,shape={1}", - updated.LocalID, linksetChildShape); - } // DEBUG DEBUG + // Found the child shape within the compound shape + PhysicsScene.PE.UpdateChildTransform(LinksetRoot.PhysShape, updated.LinksetChildIndex, + updated.RawPosition - LinksetRoot.RawPosition, + updated.RawOrientation * OMV.Quaternion.Inverse(LinksetRoot.RawOrientation), + true /* shouldRecalculateLocalAabb */); + updatedChild = true; + DetailLog("{0},BSLinksetCompound.UpdateProperties,changeChildPosRot,whichUpdated={1},pos={2},rot={3}", + updated.LocalID, whichUpdated, updated.RawPosition, updated.RawOrientation); } else // DEBUG DEBUG { // DEBUG DEBUG - // the child is not yet in the compound shape. This is non-fatal. - DetailLog("{0},BSLinksetCompound.UpdateProperties,couldNotUpdateChild,childNotInCompoundShape,numChildren={1},index={2}", - updated.LocalID, numLinksetChildren, lsi.Index); + DetailLog("{0},BSLinksetCompound.UpdateProperties,couldNotUpdateChild,noChildShape,shape={1}", + updated.LocalID, linksetChildShape); } // DEBUG DEBUG } else // DEBUG DEBUG { // DEBUG DEBUG - DetailLog("{0},BSLinksetCompound.UpdateProperties,couldNotUpdateChild,noBodyOrNotCompound", updated.LocalID); + // the child is not yet in the compound shape. This is non-fatal. + DetailLog("{0},BSLinksetCompound.UpdateProperties,couldNotUpdateChild,childNotInCompoundShape,numChildren={1},index={2}", + updated.LocalID, numLinksetChildren, updated.LinksetChildIndex); } // DEBUG DEBUG } else // DEBUG DEBUG { // DEBUG DEBUG - DetailLog("{0},BSLinksetCompound.UpdateProperties,couldNotUpdateChild,noLinkSetInfo,rootPhysShape={1}", - updated.LocalID, LinksetRoot.PhysShape); + DetailLog("{0},BSLinksetCompound.UpdateProperties,couldNotUpdateChild,noBodyOrNotCompound", updated.LocalID); } // DEBUG DEBUG if (!updatedChild) @@ -379,6 +366,8 @@ public sealed class BSLinksetCompound : BSLinkset // Safe to call even if the child is not really in the linkset. protected override void RemoveChildFromLinkset(BSPrimLinkable child) { + child.ClearDisplacement(); + if (m_children.Remove(child)) { DetailLog("{0},BSLinksetCompound.RemoveChildFromLinkset,call,rID={1},rBody={2},cID={3},cBody={4}", @@ -410,7 +399,7 @@ public sealed class BSLinksetCompound : BSLinkset // Constraint linksets are rebuilt every time. // Note that this works for rebuilding just the root after a linkset is taken apart. // Called at taint time!! - private bool disableCOM = true; // DEBUG DEBUG: disable until we get this debugged + private bool disableCOM = false; // DEBUG DEBUG: disable until we get this debugged private void RecomputeLinksetCompound() { try @@ -424,30 +413,31 @@ public sealed class BSLinksetCompound : BSLinkset // The center of mass for the linkset is the geometric center of the group. // Compute a displacement for each component so it is relative to the center-of-mass. // Bullet presumes an object's origin (relative <0,0,0>) is its center-of-mass - OMV.Vector3 centerOfMass; - OMV.Vector3 centerDisplacement = OMV.Vector3.Zero; - if (disableCOM) // DEBUG DEBUG - { // DEBUG DEBUG - centerOfMass = LinksetRoot.RawPosition; // DEBUG DEBUG - // LinksetRoot.PositionDisplacement = OMV.Vector3.Zero; - } // DEBUG DEBUG - else + OMV.Vector3 centerOfMassW = LinksetRoot.RawPosition; + if (!disableCOM) // DEBUG DEBUG { - centerOfMass = ComputeLinksetCenterOfMass(); - // 'centerDisplacement' is the value to *add* to all the shape offsets - centerDisplacement = LinksetRoot.RawPosition - centerOfMass; - - // Since we're displacing the center of the shape, we need to move the body in the world - // LinksetRoot.PositionDisplacement = centerDisplacement; - - // This causes the root prim position to be set properly based on the new PositionDisplacement - LinksetRoot.ForcePosition = LinksetRoot.RawPosition; - // Update the local transform for the root child shape so it is offset from the <0,0,0> which is COM - PhysicsScene.PE.UpdateChildTransform(LinksetRoot.PhysShape, 0, -centerDisplacement, OMV.Quaternion.Identity, false); - DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,COM,com={1},rootPos={2},centerDisp={3}", - LinksetRoot.LocalID, centerOfMass, LinksetRoot.RawPosition, centerDisplacement); + // Compute a center-of-mass in world coordinates. + centerOfMassW = ComputeLinksetCenterOfMass(); } + OMV.Quaternion invRootOrientation = OMV.Quaternion.Inverse(LinksetRoot.RawOrientation); + + // 'centerDisplacement' is the value to subtract from children to give physical offset position + OMV.Vector3 centerDisplacement = (centerOfMassW - LinksetRoot.RawPosition) * invRootOrientation; + LinksetRoot.SetEffectiveCenterOfMassW(centerDisplacement); + + // This causes the physical position of the root prim to be offset to accomodate for the displacements + LinksetRoot.ForcePosition = LinksetRoot.RawPosition; + + // Update the local transform for the root child shape so it is offset from the <0,0,0> which is COM + PhysicsScene.PE.UpdateChildTransform(LinksetRoot.PhysShape, 0, + -centerDisplacement, + LinksetRoot.RawOrientation, + false /* shouldRecalculateLocalAabb */); + + DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,COM,com={1},rootPos={2},centerDisp={3}", + LinksetRoot.LocalID, centerOfMassW, LinksetRoot.RawPosition, centerDisplacement); + DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,start,rBody={1},rShape={2},numChildren={3}", LinksetRoot.LocalID, LinksetRoot.PhysBody, LinksetRoot.PhysShape, NumberOfChildren); @@ -455,29 +445,20 @@ public sealed class BSLinksetCompound : BSLinkset int memberIndex = 1; ForEachMember(delegate(BSPrimLinkable cPrim) { - if (!IsRoot(cPrim)) + if (IsRoot(cPrim)) { - // Compute the displacement of the child from the root of the linkset. - // This info is saved in the child prim so the relationship does not - // change over time and the new child position can be computed - // when the linkset is being disassembled (the linkset may have moved). - BSLinksetCompoundInfo lci = cPrim.LinksetInfo as BSLinksetCompoundInfo; - if (lci == null) - { - lci = new BSLinksetCompoundInfo(memberIndex, LinksetRoot, cPrim, centerDisplacement); - cPrim.LinksetInfo = lci; - DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,creatingRelPos,lci={1}", cPrim.LocalID, lci); - } - - DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,addMemberToShape,mID={1},mShape={2},lci={3}", - LinksetRoot.LocalID, cPrim.LocalID, cPrim.PhysShape, lci); + cPrim.LinksetChildIndex = 0; + } + else + { + cPrim.LinksetChildIndex = memberIndex; if (cPrim.PhysShape.isNativeShape) { // A native shape is turned into a hull collision shape because native // shapes are not shared so we have to hullify it so it will be tracked // and freed at the correct time. This also solves the scaling problem - // (native shapes scaled but hull/meshes are assumed to not be). + // (native shapes scale but hull/meshes are assumed to not be). // TODO: decide of the native shape can just be used in the compound shape. // Use call to CreateGeomNonSpecial(). BulletShape saveShape = cPrim.PhysShape; @@ -486,7 +467,10 @@ public sealed class BSLinksetCompound : BSLinkset PhysicsScene.Shapes.CreateGeomMeshOrHull(cPrim, null); BulletShape newShape = cPrim.PhysShape; cPrim.PhysShape = saveShape; - PhysicsScene.PE.AddChildShapeToCompoundShape(LinksetRoot.PhysShape, newShape, lci.OffsetFromCenterOfMass, lci.OffsetRot); + + OMV.Vector3 offsetPos = (cPrim.RawPosition - LinksetRoot.RawPosition) * invRootOrientation - centerDisplacement; + OMV.Quaternion offsetRot = cPrim.RawOrientation * invRootOrientation; + PhysicsScene.PE.AddChildShapeToCompoundShape(LinksetRoot.PhysShape, newShape, offsetPos, offsetRot); } else { @@ -498,9 +482,10 @@ public sealed class BSLinksetCompound : BSLinkset PhysicsScene.Logger.ErrorFormat("{0} Rebuilt sharable shape when building linkset! Region={1}, primID={2}, shape={3}", LogHeader, PhysicsScene.RegionName, cPrim.LocalID, cPrim.PhysShape); } - PhysicsScene.PE.AddChildShapeToCompoundShape(LinksetRoot.PhysShape, cPrim.PhysShape, lci.OffsetFromCenterOfMass, lci.OffsetRot); + OMV.Vector3 offsetPos = (cPrim.RawPosition - LinksetRoot.RawPosition) * invRootOrientation - centerDisplacement; + OMV.Quaternion offsetRot = cPrim.RawOrientation * invRootOrientation; + PhysicsScene.PE.AddChildShapeToCompoundShape(LinksetRoot.PhysShape, cPrim.PhysShape, offsetPos, offsetRot); } - lci.Index = memberIndex; memberIndex++; } return false; // 'false' says to move onto the next child in the list @@ -509,6 +494,9 @@ public sealed class BSLinksetCompound : BSLinkset // With all of the linkset packed into the root prim, it has the mass of everyone. LinksetMass = ComputeLinksetMass(); LinksetRoot.UpdatePhysicalMassProperties(LinksetMass, true); + + // Enable the physical position updator to return the position and rotation of the root shape + PhysicsScene.PE.AddToCollisionFlags(LinksetRoot.PhysBody, CollisionFlags.BS_RETURN_ROOT_COMPOUND_SHAPE); } finally { diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs index de69fa0..8ebb532 100755 --- a/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs @@ -136,6 +136,7 @@ public abstract class BSPhysObject : PhysicsActor // The objects base shape information. Null if not a prim type shape. public PrimitiveBaseShape BaseShape { get; protected set; } + // Some types of objects have preferred physical representations. // Returns SHAPE_UNKNOWN if there is no preference. public virtual BSPhysicsShapeType PreferredPhysicalShape @@ -150,15 +151,17 @@ public abstract class BSPhysObject : PhysicsActor public EntityProperties LastEntityProperties { get; set; } public virtual OMV.Vector3 Scale { get; set; } - public abstract bool IsSolid { get; } - public abstract bool IsStatic { get; } - public abstract bool IsSelected { get; } // It can be confusing for an actor to know if it should move or update an object // depeneding on the setting of 'selected', 'physical, ... // This flag is the true test -- if true, the object is being acted on in the physical world public abstract bool IsPhysicallyActive { get; } + // Detailed state of the object. + public abstract bool IsSolid { get; } + public abstract bool IsStatic { get; } + public abstract bool IsSelected { get; } + // Materialness public MaterialAttributes.Material Material { get; private set; } public override void SetMaterial(int material) @@ -185,14 +188,6 @@ public abstract class BSPhysObject : PhysicsActor public abstract OMV.Quaternion RawOrientation { get; set; } public abstract OMV.Quaternion ForceOrientation { get; set; } - public virtual float TargetSpeed - { - get - { - OMV.Vector3 characterOrientedVelocity = TargetVelocity * OMV.Quaternion.Inverse(OMV.Quaternion.Normalize(RawOrientation)); - return characterOrientedVelocity.X; - } - } public abstract OMV.Vector3 RawVelocity { get; set; } public abstract OMV.Vector3 ForceVelocity { get; set; } @@ -202,6 +197,7 @@ public abstract class BSPhysObject : PhysicsActor public virtual bool ForceBodyShapeRebuild(bool inTaintTime) { return false; } + // The current velocity forward public virtual float ForwardSpeed { get @@ -210,6 +206,19 @@ public abstract class BSPhysObject : PhysicsActor return characterOrientedVelocity.X; } } + // The forward speed we are trying to achieve (TargetVelocity) + public virtual float TargetVelocitySpeed + { + get + { + OMV.Vector3 characterOrientedVelocity = TargetVelocity * OMV.Quaternion.Inverse(OMV.Quaternion.Normalize(RawOrientation)); + return characterOrientedVelocity.X; + } + } + + // The user can optionally set the center of mass. The user's setting will override any + // computed center-of-mass (like in linksets). + public OMV.Vector3? UserSetCenterOfMass { get; set; } #region Collisions diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs index cf7aa0f..a76f8b9 100644 --- a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs @@ -442,7 +442,7 @@ public class BSPrim : BSPhysObject RegisterPreStepAction("BSPrim.setForce", LocalID, delegate(float timeStep) { - if (!IsPhysicallyActive) + if (!IsPhysicallyActive || _force == OMV.Vector3.Zero) { UnRegisterPreStepAction("BSPrim.setForce", LocalID); return; @@ -647,7 +647,7 @@ public class BSPrim : BSPhysObject RegisterPreStepAction("BSPrim.setTorque", LocalID, delegate(float timeStep) { - if (!IsPhysicallyActive) + if (!IsPhysicallyActive || _torque == OMV.Vector3.Zero) { UnRegisterPreStepAction("BSPrim.setTorque", LocalID); return; diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrimDisplaced.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrimDisplaced.cs index 6401308..b9f2cca 100755 --- a/OpenSim/Region/Physics/BulletSPlugin/BSPrimDisplaced.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrimDisplaced.cs @@ -44,72 +44,107 @@ namespace OpenSim.Region.Physics.BulletSPlugin { public class BSPrimDisplaced : BSPrim { - // 'Position' and 'Orientation' is what the simulator thinks the positions of the prim is. - // Because Bullet needs the zero coordinate to be the center of mass of the linkset, - // sometimes it is necessary to displace the position the physics engine thinks - // the position is. PositionDisplacement must be added and removed from the - // position as the simulator position is stored and fetched from the physics - // engine. Similar to OrientationDisplacement. + // The purpose of this module is to do any mapping between what the simulator thinks + // the prim position and orientation is and what the physical position/orientation. + // This difference happens because Bullet assumes the center-of-mass is the <0,0,0> + // of the prim/linkset. The simulator tracks the location of the prim/linkset by + // the location of the root prim. So, if center-of-mass is anywhere but the origin + // of the root prim, the physical origin is displaced from the simulator origin. + // + // This routine works by capturing the Force* setting of position/orientation/... and + // adjusting the simulator values (being set) into the physical values. + // The conversion is also done in the opposite direction (physical origin -> simulator origin). + // + // The updateParameter call is also captured and the values from the physics engine + // are converted into simulator origin values before being passed to the base + // class. + public virtual OMV.Vector3 PositionDisplacement { get; set; } public virtual OMV.Quaternion OrientationDisplacement { get; set; } - public virtual OMV.Vector3 CenterOfMassLocation { get; set; } - public virtual OMV.Vector3 GeometricCenterLocation { get; set; } public BSPrimDisplaced(uint localID, String primName, BSScene parent_scene, OMV.Vector3 pos, OMV.Vector3 size, OMV.Quaternion rotation, PrimitiveBaseShape pbs, bool pisPhysical) : base(localID, primName, parent_scene, pos, size, rotation, pbs, pisPhysical) { - CenterOfMassLocation = RawPosition; - GeometricCenterLocation = RawPosition; + ClearDisplacement(); } - public override Vector3 ForcePosition + public void ClearDisplacement() + { + PositionDisplacement = OMV.Vector3.Zero; + OrientationDisplacement = OMV.Quaternion.Identity; + } + + // Set this sets and computes the displacement from the passed prim to the center-of-mass. + // A user set value for center-of-mass overrides whatever might be passed in here. + // The displacement is in local coordinates (relative to root prim in linkset oriented coordinates). + public virtual void SetEffectiveCenterOfMassW(Vector3 centerOfMassDisplacement) { - get + Vector3 comDisp; + if (UserSetCenterOfMass.HasValue) + comDisp = (OMV.Vector3)UserSetCenterOfMass; + else + comDisp = centerOfMassDisplacement; + + if (comDisp == Vector3.Zero) { - return base.ForcePosition; + // If there is no diplacement. Things get reset. + PositionDisplacement = OMV.Vector3.Zero; + OrientationDisplacement = OMV.Quaternion.Identity; } - set + else { - base.ForcePosition = value; - CenterOfMassLocation = RawPosition; - GeometricCenterLocation = RawPosition; + // Remember the displacement from root as well as the origional rotation of the + // new center-of-mass. + PositionDisplacement = comDisp; + OrientationDisplacement = OMV.Quaternion.Identity; } } - public override Quaternion ForceOrientation + public override Vector3 ForcePosition { - get + get { return base.ForcePosition; } + set { - return base.ForceOrientation; + if (PositionDisplacement != OMV.Vector3.Zero) + base.ForcePosition = value - (PositionDisplacement * RawOrientation); + else + base.ForcePosition = value; } + } + + public override Quaternion ForceOrientation + { + get { return base.ForceOrientation; } set { base.ForceOrientation = value; } } + // TODO: decide if this is the right place for these variables. + // Somehow incorporate the optional settability by the user. // Is this used? public override OMV.Vector3 CenterOfMass { - get { return CenterOfMassLocation; } + get { return RawPosition; } } // Is this used? public override OMV.Vector3 GeometricCenter { - get { return GeometricCenterLocation; } + get { return RawPosition; } } - public override void UpdateProperties(EntityProperties entprop) { // Undo any center-of-mass displacement that might have been done. - if (PositionDisplacement != OMV.Vector3.Zero) + if (PositionDisplacement != OMV.Vector3.Zero || OrientationDisplacement != OMV.Quaternion.Identity) { // Correct for any rotation around the center-of-mass // TODO!!! - entprop.Position -= PositionDisplacement; + entprop.Position = entprop.Position + (PositionDisplacement * entprop.Rotation); + entprop.Rotation = something; } base.UpdateProperties(entprop); diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrimLinkable.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrimLinkable.cs index 9898562..96f9762 100755 --- a/OpenSim/Region/Physics/BulletSPlugin/BSPrimLinkable.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrimLinkable.cs @@ -38,6 +38,9 @@ namespace OpenSim.Region.Physics.BulletSPlugin public class BSPrimLinkable : BSPrimDisplaced { public BSLinkset Linkset { get; set; } + // The index of this child prim. + public int LinksetChildIndex { get; set; } + public BSLinksetInfo LinksetInfo { get; set; } public BSPrimLinkable(uint localID, String primName, BSScene parent_scene, OMV.Vector3 pos, OMV.Vector3 size, @@ -90,7 +93,6 @@ public class BSPrimLinkable : BSPrimDisplaced DetailLog("{0},BSPrimLinkset.delink,parentBefore={1},childrenBefore={2},parentAfter={3},childrenAfter={4}, ", LocalID, parentBefore.LocalID, childrenBefore, Linkset.LinksetRoot.LocalID, Linkset.NumberOfChildren); return; - base.delink(); } // When simulator changes position, this might be moving a child of the linkset. -- cgit v1.1 From 0194a3d890b95c8a29fcdf130c378e3a8a629c77 Mon Sep 17 00:00:00 2001 From: Robert Adams Date: Tue, 12 Feb 2013 15:45:44 -0800 Subject: BulletSim: fix density since the simulator/viewer track density in a funny unit that is 100 times real density (default 1000). Fix avatar drifting slowly when stationary flying. Fix for physical prims getting corrected for being under terrain when it was just its geometric center that was below terrain. Add PreUpdatePropertyAction allowing plugable modifiction of phys parameters returned from Bullet. Fix an exception setting GravityMultiplier on initialization. Update DLLs and SOs for good measure (no functional change). --- .../Region/Physics/BulletSPlugin/BSCharacter.cs | 13 ++++- .../Physics/BulletSPlugin/BSLinksetCompound.cs | 19 ++++--- OpenSim/Region/Physics/BulletSPlugin/BSParam.cs | 17 ++++-- .../Region/Physics/BulletSPlugin/BSPhysObject.cs | 66 ++++++++++++++++++++-- OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs | 55 +++++++++++++++++- .../Physics/BulletSPlugin/BSPrimDisplaced.cs | 2 +- .../Region/Physics/BulletSPlugin/BSPrimLinkable.cs | 3 +- 7 files changed, 150 insertions(+), 25 deletions(-) (limited to 'OpenSim/Region') diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs index 04fb05b..8dca7c6 100644 --- a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs @@ -83,7 +83,7 @@ public sealed class BSCharacter : BSPhysObject _velocity = OMV.Vector3.Zero; _buoyancy = ComputeBuoyancyFromFlying(isFlying); Friction = BSParam.AvatarStandingFriction; - Density = BSParam.AvatarDensity; + Density = BSParam.AvatarDensity / BSParam.DensityScaleFactor; // Old versions of ScenePresence passed only the height. If width and/or depth are zero, // replace with the default values. @@ -231,6 +231,15 @@ public sealed class BSCharacter : BSPhysObject PhysicsScene.PE.SetFriction(PhysBody, Friction); } } + else + { + if (Flying) + { + // Flying and not collising and velocity nearly zero. + ZeroMotion(true /* inTaintTime */); + } + } + DetailLog("{0},BSCharacter.MoveMotor,taint,stopping,target={1},colliding={2}", LocalID, _velocityMotor.TargetValue, IsColliding); } else @@ -869,7 +878,7 @@ public sealed class BSCharacter : BSPhysObject * Math.Min(Size.X, Size.Y) / 2 * Size.Y / 2f // plus the volume of the capsule end caps ); - _mass = Density * _avatarVolume; + _mass = Density * BSParam.DensityScaleFactor * _avatarVolume; } // The physics engine says that properties have updated. Update same and inform diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs index 1f66b56..4ce58c7 100755 --- a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs @@ -399,7 +399,7 @@ public sealed class BSLinksetCompound : BSLinkset // Constraint linksets are rebuilt every time. // Note that this works for rebuilding just the root after a linkset is taken apart. // Called at taint time!! - private bool disableCOM = false; // DEBUG DEBUG: disable until we get this debugged + private bool disableCOM = true; // DEBUG DEBUG: disable until we get this debugged private void RecomputeLinksetCompound() { try @@ -430,10 +430,10 @@ public sealed class BSLinksetCompound : BSLinkset LinksetRoot.ForcePosition = LinksetRoot.RawPosition; // Update the local transform for the root child shape so it is offset from the <0,0,0> which is COM - PhysicsScene.PE.UpdateChildTransform(LinksetRoot.PhysShape, 0, - -centerDisplacement, - LinksetRoot.RawOrientation, - false /* shouldRecalculateLocalAabb */); + PhysicsScene.PE.UpdateChildTransform(LinksetRoot.PhysShape, 0 /* childIndex */, + -centerDisplacement, + OMV.Quaternion.Identity, // LinksetRoot.RawOrientation, + false /* shouldRecalculateLocalAabb (is done later after linkset built) */); DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,COM,com={1},rootPos={2},centerDisp={3}", LinksetRoot.LocalID, centerOfMassW, LinksetRoot.RawPosition, centerDisplacement); @@ -463,7 +463,6 @@ public sealed class BSLinksetCompound : BSLinkset // Use call to CreateGeomNonSpecial(). BulletShape saveShape = cPrim.PhysShape; cPrim.PhysShape.Clear(); // Don't let the create free the child's shape - // PhysicsScene.Shapes.CreateGeomNonSpecial(true, cPrim, null); PhysicsScene.Shapes.CreateGeomMeshOrHull(cPrim, null); BulletShape newShape = cPrim.PhysShape; cPrim.PhysShape = saveShape; @@ -471,6 +470,8 @@ public sealed class BSLinksetCompound : BSLinkset OMV.Vector3 offsetPos = (cPrim.RawPosition - LinksetRoot.RawPosition) * invRootOrientation - centerDisplacement; OMV.Quaternion offsetRot = cPrim.RawOrientation * invRootOrientation; PhysicsScene.PE.AddChildShapeToCompoundShape(LinksetRoot.PhysShape, newShape, offsetPos, offsetRot); + DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,addNative,indx={1},rShape={2},cShape={3},offPos={4},offRot={5}", + LinksetRoot.LocalID, memberIndex, LinksetRoot.PhysShape, newShape, offsetPos, offsetRot); } else { @@ -484,7 +485,10 @@ public sealed class BSLinksetCompound : BSLinkset } OMV.Vector3 offsetPos = (cPrim.RawPosition - LinksetRoot.RawPosition) * invRootOrientation - centerDisplacement; OMV.Quaternion offsetRot = cPrim.RawOrientation * invRootOrientation; - PhysicsScene.PE.AddChildShapeToCompoundShape(LinksetRoot.PhysShape, cPrim.PhysShape, offsetPos, offsetRot); + PhysicsScene.PE.AddChildShapeToCompoundShape(LinksetRoot.PhysShape, cPrim.PhysShape, offsetPos, offsetRot); + DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,addNonNative,indx={1},rShape={2},cShape={3},offPos={4},offRot={5}", + LinksetRoot.LocalID, memberIndex, LinksetRoot.PhysShape, cPrim.PhysShape, offsetPos, offsetRot); + } memberIndex++; } @@ -503,6 +507,7 @@ public sealed class BSLinksetCompound : BSLinkset Rebuilding = false; } + // See that the Aabb surrounds the new shape PhysicsScene.PE.RecalculateCompoundShapeLocalAabb(LinksetRoot.PhysShape); } } diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSParam.cs b/OpenSim/Region/Physics/BulletSPlugin/BSParam.cs index 3e0b4bc..329169f 100755 --- a/OpenSim/Region/Physics/BulletSPlugin/BSParam.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BSParam.cs @@ -49,6 +49,7 @@ public static class BSParam public static float MaxLinearVelocity { get; private set; } public static float MaxAngularVelocity { get; private set; } public static float MaxAddForceMagnitude { get; private set; } + public static float DensityScaleFactor { get; private set; } public static float LinearDamping { get; private set; } public static float AngularDamping { get; private set; } @@ -281,29 +282,35 @@ public static class BSParam new ParameterDefn("MinObjectMass", "Minimum object mass (0.0001)", 0.0001f, (s,cf,p,v) => { MinimumObjectMass = cf.GetFloat(p, v); }, - (s) => { return (float)MinimumObjectMass; }, + (s) => { return MinimumObjectMass; }, (s,p,l,v) => { MinimumObjectMass = v; } ), new ParameterDefn("MaxObjectMass", "Maximum object mass (10000.01)", 10000.01f, (s,cf,p,v) => { MaximumObjectMass = cf.GetFloat(p, v); }, - (s) => { return (float)MaximumObjectMass; }, + (s) => { return MaximumObjectMass; }, (s,p,l,v) => { MaximumObjectMass = v; } ), new ParameterDefn("MaxLinearVelocity", "Maximum velocity magnitude that can be assigned to an object", 1000.0f, (s,cf,p,v) => { MaxLinearVelocity = cf.GetFloat(p, v); }, - (s) => { return (float)MaxLinearVelocity; }, + (s) => { return MaxLinearVelocity; }, (s,p,l,v) => { MaxLinearVelocity = v; } ), new ParameterDefn("MaxAngularVelocity", "Maximum rotational velocity magnitude that can be assigned to an object", 1000.0f, (s,cf,p,v) => { MaxAngularVelocity = cf.GetFloat(p, v); }, - (s) => { return (float)MaxAngularVelocity; }, + (s) => { return MaxAngularVelocity; }, (s,p,l,v) => { MaxAngularVelocity = v; } ), // LL documentation says thie number should be 20f for llApplyImpulse and 200f for llRezObject new ParameterDefn("MaxAddForceMagnitude", "Maximum force that can be applied by llApplyImpulse (SL says 20f)", 20000.0f, (s,cf,p,v) => { MaxAddForceMagnitude = cf.GetFloat(p, v); }, - (s) => { return (float)MaxAddForceMagnitude; }, + (s) => { return MaxAddForceMagnitude; }, (s,p,l,v) => { MaxAddForceMagnitude = v; } ), + // Density is passed around as 100kg/m3. This scales that to 1kg/m3. + new ParameterDefn("DensityScaleFactor", "Conversion for simulator/viewer density (100kg/m3) to physical density (1kg/m3)", + 0.01f, + (s,cf,p,v) => { DensityScaleFactor = cf.GetFloat(p, v); }, + (s) => { return DensityScaleFactor; }, + (s,p,l,v) => { DensityScaleFactor = v; } ), new ParameterDefn("PID_D", "Derivitive factor for motion smoothing", 2200f, diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs index 8ebb532..f953c1e 100755 --- a/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs @@ -99,6 +99,9 @@ public abstract class BSPhysObject : PhysicsActor CollisionAccumulation = 0; ColliderIsMoving = false; CollisionScore = 0; + + // All axis free. + LockedAxis = LockedAxisFree; } // Tell the object to clean up. @@ -172,7 +175,8 @@ public abstract class BSPhysObject : PhysicsActor MaterialAttributes matAttrib = BSMaterials.GetAttributes(Material, false); Friction = matAttrib.friction; Restitution = matAttrib.restitution; - Density = matAttrib.density; + Density = matAttrib.density / BSParam.DensityScaleFactor; + DetailLog("{0},{1}.SetMaterial,Mat={2},frict={3},rest={4},den={5}", LocalID, TypeName, Material, Friction, Restitution, Density); } // Stop all physical motion. @@ -220,6 +224,9 @@ public abstract class BSPhysObject : PhysicsActor // computed center-of-mass (like in linksets). public OMV.Vector3? UserSetCenterOfMass { get; set; } + public OMV.Vector3 LockedAxis { get; set; } // zero means locked. one means free. + public readonly OMV.Vector3 LockedAxisFree = new OMV.Vector3(1f, 1f, 1f); // All axis are free + #region Collisions // Requested number of milliseconds between collision events. Zero means disabled. @@ -416,9 +423,7 @@ public abstract class BSPhysObject : PhysicsActor { // Clean out any existing action UnRegisterPreStepAction(op, id); - RegisteredPrestepActions[identifier] = actn; - PhysicsScene.BeforeStep += actn; } DetailLog("{0},BSPhysObject.RegisterPreStepAction,id={1}", LocalID, identifier); @@ -464,9 +469,7 @@ public abstract class BSPhysObject : PhysicsActor { // Clean out any existing action UnRegisterPostStepAction(op, id); - RegisteredPoststepActions[identifier] = actn; - PhysicsScene.AfterStep += actn; } DetailLog("{0},BSPhysObject.RegisterPostStepAction,id={1}", LocalID, identifier); @@ -503,7 +506,58 @@ public abstract class BSPhysObject : PhysicsActor } DetailLog("{0},BSPhysObject.UnRegisterAllPostStepActions,", LocalID); } - + + // When an update to the physical properties happens, this event is fired to let + // different actors to modify the update before it is passed around + public delegate void PreUpdatePropertyAction(ref EntityProperties entprop); + public event PreUpdatePropertyAction OnPreUpdateProperty; + protected void TriggerPreUpdatePropertyAction(ref EntityProperties entprop) + { + PreUpdatePropertyAction actions = OnPreUpdateProperty; + if (actions != null) + actions(ref entprop); + } + + private Dictionary RegisteredPreUpdatePropertyActions = new Dictionary(); + public void RegisterPreUpdatePropertyAction(string identifier, PreUpdatePropertyAction actn) + { + lock (RegisteredPreUpdatePropertyActions) + { + // Clean out any existing action + UnRegisterPreUpdatePropertyAction(identifier); + RegisteredPreUpdatePropertyActions[identifier] = actn; + OnPreUpdateProperty += actn; + } + DetailLog("{0},BSPhysObject.RegisterPreUpdatePropertyAction,id={1}", LocalID, identifier); + } + public bool UnRegisterPreUpdatePropertyAction(string identifier) + { + bool removed = false; + lock (RegisteredPreUpdatePropertyActions) + { + if (RegisteredPreUpdatePropertyActions.ContainsKey(identifier)) + { + OnPreUpdateProperty -= RegisteredPreUpdatePropertyActions[identifier]; + RegisteredPreUpdatePropertyActions.Remove(identifier); + removed = true; + } + } + DetailLog("{0},BSPhysObject.UnRegisterPreUpdatePropertyAction,id={1},removed={2}", LocalID, identifier, removed); + return removed; + } + public void UnRegisterAllPreUpdatePropertyActions() + { + lock (RegisteredPreUpdatePropertyActions) + { + foreach (KeyValuePair kvp in RegisteredPreUpdatePropertyActions) + { + OnPreUpdateProperty -= kvp.Value; + } + RegisteredPreUpdatePropertyActions.Clear(); + } + DetailLog("{0},BSPhysObject.UnRegisterAllPreUpdatePropertyAction,", LocalID); + } + #endregion // Per Simulation Step actions // High performance detailed logging routine used by the physical objects. diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs index a76f8b9..0323b0d 100644 --- a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs @@ -242,6 +242,45 @@ public class BSPrim : BSPhysObject public override void LockAngularMotion(OMV.Vector3 axis) { DetailLog("{0},BSPrim.LockAngularMotion,call,axis={1}", LocalID, axis); + + OMV.Vector3 locking = new OMV.Vector3(1f, 1f, 1f); + if (axis.X != 1) locking.X = 0f; + if (axis.Y != 1) locking.Y = 0f; + if (axis.Z != 1) locking.Z = 0f; + LockedAxis = locking; + + /* Not implemented yet + if (LockedAxis != LockedAxisFree) + { + // Something is locked so start the thingy that keeps that axis from changing + RegisterPreUpdatePropertyAction("BSPrim.LockAngularMotion", delegate(ref EntityProperties entprop) + { + if (LockedAxis != LockedAxisFree) + { + if (IsPhysicallyActive) + { + // Bullet can lock axis but it only works for global axis. + // Check if this prim is aligned on global axis and use Bullet's + // system if so. + + ForceOrientation = entprop.Rotation; + ForceRotationalVelocity = entprop.RotationalVelocity; + } + } + else + { + UnRegisterPreUpdatePropertyAction("BSPrim.LockAngularMotion"); + } + + }); + } + else + { + // Everything seems unlocked + UnRegisterPreUpdatePropertyAction("BSPrim.LockAngularMotion"); + } + */ + return; } @@ -311,7 +350,8 @@ public class BSPrim : BSPhysObject float terrainHeight = PhysicsScene.TerrainManager.GetTerrainHeightAtXYZ(RawPosition); OMV.Vector3 upForce = OMV.Vector3.Zero; - if (RawPosition.Z < terrainHeight) + float approxSize = Math.Max(Size.X, Math.Max(Size.Y, Size.Z)); + if ((RawPosition.Z + approxSize / 2f) < terrainHeight) { DetailLog("{0},BSPrim.PositionAdjustUnderGround,call,pos={1},terrain={2}", LocalID, RawPosition, terrainHeight); float targetHeight = terrainHeight + (Size.Z / 2f); @@ -576,6 +616,8 @@ public class BSPrim : BSPhysObject } } } + // The simulator/viewer keep density as 100kg/m3. + // Remember to use BSParam.DensityScaleFactor to create the physical density. public override float Density { get { return base.Density; } @@ -1569,7 +1611,8 @@ public class BSPrim : BSPhysObject profileEnd = 1.0f - (float)BaseShape.ProfileEnd * 2.0e-5f; volume *= (profileEnd - profileBegin); - returnMass = Density * volume; + returnMass = Density * BSParam.DensityScaleFactor * volume; + DetailLog("{0},BSPrim.CalculateMass,den={1},vol={2},mass={3}", LocalID, Density, volume, returnMass); returnMass = Util.Clamp(returnMass, BSParam.MinimumObjectMass, BSParam.MaximumObjectMass); @@ -1607,6 +1650,8 @@ public class BSPrim : BSPhysObject // the world that things have changed. public override void UpdateProperties(EntityProperties entprop) { + TriggerPreUpdatePropertyAction(ref entprop); + // A temporary kludge to suppress the rotational effects introduced on vehicles by Bullet // TODO: handle physics introduced by Bullet with computed vehicle physics. if (VehicleController.IsActive) @@ -1619,7 +1664,11 @@ public class BSPrim : BSPhysObject // Assign directly to the local variables so the normal set actions do not happen _position = entprop.Position; _orientation = entprop.Rotation; - _velocity = entprop.Velocity; + // _velocity = entprop.Velocity; + // DEBUG DEBUG DEBUG -- smooth velocity changes a bit. The simulator seems to be + // very sensitive to velocity changes. + if (!entprop.Velocity.ApproxEquals(_velocity, 0.1f)) + _velocity = entprop.Velocity; _acceleration = entprop.Acceleration; _rotationalVelocity = entprop.RotationalVelocity; diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrimDisplaced.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrimDisplaced.cs index b9f2cca..f1c3b5c 100755 --- a/OpenSim/Region/Physics/BulletSPlugin/BSPrimDisplaced.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrimDisplaced.cs @@ -144,7 +144,7 @@ public class BSPrimDisplaced : BSPrim // Correct for any rotation around the center-of-mass // TODO!!! entprop.Position = entprop.Position + (PositionDisplacement * entprop.Rotation); - entprop.Rotation = something; + // entprop.Rotation = something; } base.UpdateProperties(entprop); diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrimLinkable.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrimLinkable.cs index 96f9762..d65d407 100755 --- a/OpenSim/Region/Physics/BulletSPlugin/BSPrimLinkable.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrimLinkable.cs @@ -135,7 +135,8 @@ public class BSPrimLinkable : BSPrimDisplaced // When going from non-physical to physical, this re-enables the constraints that // had been automatically disabled when the mass was set to zero. // For compound based linksets, this enables and disables interactions of the children. - Linkset.Refresh(this); + if (Linkset != null) // null can happen during initialization + Linkset.Refresh(this); } protected override void MakeDynamic(bool makeStatic) -- cgit v1.1