From 459fcd81c9d6e7c92738b40f1b4b4fe746699379 Mon Sep 17 00:00:00 2001 From: Robert Adams Date: Fri, 11 Jan 2013 16:36:34 -0800 Subject: BulletSim: move center of gravity of linkset to its geometric center. Necessitated allowing simulator and physical position of a body to get out of sync since Bullet assumes that <0,0,0> is the center of mass. Update DLLs and SOs for the UpdateChildTransform so positions of individual prim in a linkset can be implemented. --- OpenSim/Region/Physics/BulletSPlugin/BSAPIUnman.cs | 9 ++++ OpenSim/Region/Physics/BulletSPlugin/BSAPIXNA.cs | 1 + .../Region/Physics/BulletSPlugin/BSApiTemplate.cs | 2 + OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs | 13 +++-- OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs | 4 +- .../Physics/BulletSPlugin/BSLinksetCompound.cs | 55 +++++++++++++++++----- .../Region/Physics/BulletSPlugin/BSPhysObject.cs | 10 ++++ OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs | 40 ++++++++++++---- .../Physics/BulletSPlugin/BSShapeCollection.cs | 2 +- .../Region/Physics/BulletSPlugin/BulletSimTODO.txt | 2 + 10 files changed, 106 insertions(+), 32 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSAPIUnman.cs b/OpenSim/Region/Physics/BulletSPlugin/BSAPIUnman.cs index 14de2eb..0fef67c 100755 --- a/OpenSim/Region/Physics/BulletSPlugin/BSAPIUnman.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BSAPIUnman.cs @@ -327,6 +327,12 @@ public override void RemoveChildShapeFromCompoundShape(BulletShape shape, Bullet BSAPICPP.RemoveChildShapeFromCompoundShape2(shapeu.ptr, removeShapeu.ptr); } +public override void UpdateChildTransform(BulletShape pShape, int childIndex, Vector3 pos, Quaternion rot, bool shouldRecalculateLocalAabb) +{ + BulletShapeUnman shapeu = pShape as BulletShapeUnman; + BSAPICPP.UpdateChildTransform2(shapeu.ptr, childIndex, pos, rot, shouldRecalculateLocalAabb); +} + public override void RecalculateCompoundShapeLocalAabb(BulletShape shape) { BulletShapeUnman shapeu = shape as BulletShapeUnman; @@ -1357,6 +1363,9 @@ public static extern IntPtr RemoveChildShapeFromCompoundShapeIndex2(IntPtr cShap public static extern void RemoveChildShapeFromCompoundShape2(IntPtr cShape, IntPtr removeShape); [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity] +public static extern void UpdateChildTransform2(IntPtr pShape, int childIndex, Vector3 pos, Quaternion rot, bool shouldRecalculateLocalAabb); + +[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity] public static extern void RecalculateCompoundShapeLocalAabb2(IntPtr cShape); [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity] diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSAPIXNA.cs b/OpenSim/Region/Physics/BulletSPlugin/BSAPIXNA.cs index 0c7f315..b6ff52b 100755 --- a/OpenSim/Region/Physics/BulletSPlugin/BSAPIXNA.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BSAPIXNA.cs @@ -1212,6 +1212,7 @@ private sealed class BulletConstraintXNA : BulletConstraint public override BulletShape GetChildShapeFromCompoundShapeIndex(BulletShape cShape, int indx) { /* TODO */ return null; } public override void RemoveChildShapeFromCompoundShape(BulletShape cShape, BulletShape removeShape) { /* TODO */ } + public override void UpdateChildTransform(BulletShape pShape, int childIndex, Vector3 pos, Quaternion rot, bool shouldRecalculateLocalAabb) { /* TODO */ } public override BulletShape CreateGroundPlaneShape(uint pLocalId, float pheight, float pcollisionMargin) { diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSApiTemplate.cs b/OpenSim/Region/Physics/BulletSPlugin/BSApiTemplate.cs index 794ee17..bc163eb 100644 --- a/OpenSim/Region/Physics/BulletSPlugin/BSApiTemplate.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BSApiTemplate.cs @@ -342,6 +342,8 @@ public abstract BulletShape RemoveChildShapeFromCompoundShapeIndex(BulletShape c public abstract void RemoveChildShapeFromCompoundShape(BulletShape cShape, BulletShape removeShape); +public abstract void UpdateChildTransform(BulletShape pShape, int childIndex, Vector3 pos, Quaternion rot, bool shouldRecalculateLocalAabb); + public abstract void RecalculateCompoundShapeLocalAabb(BulletShape cShape); public abstract BulletShape DuplicateCollisionShape(BulletWorld sim, BulletShape srcShape, uint id); diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs index 80fdfb9..bcebaec 100644 --- a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs @@ -148,7 +148,6 @@ namespace OpenSim.Region.Physics.BulletSPlugin enableAngularVerticalAttraction = false; enableAngularDeflection = false; enableAngularBanking = false; - VDetailLog("{0},BSDynamics.SetupVehicleDebugging,settingDebugMode", Prim.LocalID); } } @@ -690,10 +689,10 @@ namespace OpenSim.Region.Physics.BulletSPlugin } if ((m_knownChanged & m_knownChangedForce) != 0) - Prim.AddForce((Vector3)m_knownForce, false, true); + Prim.AddForce((Vector3)m_knownForce, false /*pushForce*/, true /*inTaintTime*/); if ((m_knownChanged & m_knownChangedForceImpulse) != 0) - Prim.AddForceImpulse((Vector3)m_knownForceImpulse, false, true); + Prim.AddForceImpulse((Vector3)m_knownForceImpulse, false /*pushforce*/, true /*inTaintTime*/); if ((m_knownChanged & m_knownChangedRotationalVelocity) != 0) { @@ -703,7 +702,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin if ((m_knownChanged & m_knownChangedRotationalForce) != 0) { - Prim.AddAngularForce((Vector3)m_knownRotationalForce, false, true); + Prim.AddAngularForce((Vector3)m_knownRotationalForce, false /*pushForce*/, true /*inTaintTime*/); } // If we set one of the values (ie, the physics engine didn't do it) we must force @@ -970,7 +969,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin // Act against the inertia of the vehicle linearMotorForce *= m_vehicleMass; - VehicleAddForceImpulse(linearMotorForce); + VehicleAddForceImpulse(linearMotorForce * pTimestep); VDetailLog("{0}, MoveLinear,velocity,vehVel={1},step={2},stepVel={3},mix={4},force={5}", Prim.LocalID, VehicleVelocity, linearMotorStep, linearMotorVelocity, mixFactor, linearMotorForce); @@ -1033,7 +1032,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin { // Error is positive if below the target and negative if above. float verticalError = m_VhoverTargetHeight - VehiclePosition.Z; - float verticalCorrectionVelocity = verticalError / m_VhoverTimescale; + float verticalCorrectionVelocity = verticalError / m_VhoverTimescale * pTimestep; // TODO: implement m_VhoverEfficiency correctly VehicleAddForceImpulse(new Vector3(0f, 0f, verticalCorrectionVelocity)); @@ -1323,7 +1322,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin // this creates an over-correction and then wabbling as the target is overshot. // TODO: rethink how the different correction computations inter-relate. - if (enableAngularDeflection && m_angularDeflectionEfficiency != 0 && VehicleVelocity != Vector3.Zero) + if (enableAngularDeflection && m_angularDeflectionEfficiency != 0 && VehicleForwardSpeed > 0.2) { // The direction the vehicle is moving Vector3 movingDirection = VehicleVelocity; diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs index 756faed..cbd160f 100755 --- a/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs @@ -152,6 +152,7 @@ public abstract class BSLinkset if (IsRoot(child)) { // Cannot remove the root from a linkset. + child.PositionDisplacement = OMV.Vector3.Zero; return this; } RemoveChildFromLinkset(child); @@ -159,6 +160,7 @@ public abstract class BSLinkset } // The child is down to a linkset of just itself + child.PositionDisplacement = OMV.Vector3.Zero; return BSLinkset.Factory(PhysicsScene, child); } @@ -310,7 +312,7 @@ public abstract class BSLinkset foreach (BSPhysObject bp in m_children) { - com += bp.Position * bp.RawMass; + com += bp.Position; } com /= (m_children.Count + 1); } diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs index bd03d31..ad8024c 100755 --- a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs @@ -40,23 +40,33 @@ namespace OpenSim.Region.Physics.BulletSPlugin // removed from the linkset. sealed class BSLinksetCompoundInfo : BSLinksetInfo { - public OMV.Vector3 OffsetPos; + public int Index; + public OMV.Vector3 OffsetFromRoot; + public OMV.Vector3 OffsetFromCenterOfMass; public OMV.Quaternion OffsetRot; - public BSLinksetCompoundInfo(OMV.Vector3 p, OMV.Quaternion r) + public BSLinksetCompoundInfo(int indx, OMV.Vector3 p, OMV.Quaternion r) { - OffsetPos = p; + Index = indx; + OffsetFromRoot = p; + OffsetFromCenterOfMass = p; OffsetRot = r; } public override void Clear() { - OffsetPos = OMV.Vector3.Zero; + Index = 0; + OffsetFromRoot = OMV.Vector3.Zero; + OffsetFromCenterOfMass = OMV.Vector3.Zero; OffsetRot = OMV.Quaternion.Identity; } public override string ToString() { StringBuilder buff = new StringBuilder(); - buff.Append(""); @@ -170,6 +180,8 @@ public sealed class BSLinksetCompound : BSLinkset return ret; } + // 'physicalUpdate' is true if these changes came directly from the physics engine. Don't need to rebuild then. + // Called at taint-time. public override void UpdateProperties(BSPhysObject updated, bool physicalUpdate) { // The user moving a child around requires the rebuilding of the linkset compound shape @@ -182,6 +194,7 @@ public sealed class BSLinksetCompound : BSLinkset && !physicalUpdate && PhysicsScene.TerrainManager.IsWithinKnownTerrain(LinksetRoot.RawPosition)) { + // TODO: replace this with are calculation of the child prim's orientation and pos. updated.LinksetInfo = null; ScheduleRebuild(updated); } @@ -230,7 +243,7 @@ public sealed class BSLinksetCompound : BSLinkset if (inTaintTime) { OMV.Vector3 oldPos = child.RawPosition; - child.ForcePosition = LinksetRoot.RawPosition + lci.OffsetPos; + child.ForcePosition = LinksetRoot.RawPosition + lci.OffsetFromRoot; child.ForceOrientation = LinksetRoot.RawOrientation * lci.OffsetRot; DetailLog("{0},BSLinksetCompound.RecomputeChildWorldPosition,oldPos={1},lci={2},newPos={3}", child.LocalID, oldPos, lci, child.RawPosition); @@ -238,7 +251,7 @@ public sealed class BSLinksetCompound : BSLinkset else { // TaintedObject is not used here so the raw position is set now and not at taint-time. - child.Position = LinksetRoot.RawPosition + lci.OffsetPos; + child.Position = LinksetRoot.RawPosition + lci.OffsetFromRoot; child.Orientation = LinksetRoot.RawOrientation * lci.OffsetRot; } } @@ -316,10 +329,23 @@ public sealed class BSLinksetCompound : BSLinkset // Cause the root shape to be rebuilt as a compound object with just the root in it LinksetRoot.ForceBodyShapeRebuild(true); + // 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. + OMV.Vector3 centerOfMass = ComputeLinksetGeometricCenter(); + OMV.Vector3 centerDisplacement = centerOfMass - LinksetRoot.RawPosition; + + // Since we're displacing the center of the shape, we need to move the body in the world + LinksetRoot.PositionDisplacement = centerDisplacement * LinksetRoot.RawOrientation; + + 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); + DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,start,rBody={1},rShape={2},numChildren={3}", LinksetRoot.LocalID, LinksetRoot.PhysBody, LinksetRoot.PhysShape, NumberOfChildren); // Add a shape for each of the other children in the linkset + int memberIndex = 1; ForEachMember(delegate(BSPhysObject cPrim) { if (!IsRoot(cPrim)) @@ -337,13 +363,14 @@ public sealed class BSLinksetCompound : BSLinkset OMV.Quaternion displacementRot = cPrim.RawOrientation * invRootOrientation; // Save relative position for recomputing child's world position after moving linkset. - lci = new BSLinksetCompoundInfo(displacementPos, displacementRot); + lci = new BSLinksetCompoundInfo(memberIndex, displacementPos, displacementRot); + lci.OffsetFromCenterOfMass = displacementPos - centerDisplacement; cPrim.LinksetInfo = lci; DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,creatingRelPos,lci={1}", cPrim.LocalID, lci); } - DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,addMemberToShape,mID={1},mShape={2},dispPos={3},dispRot={4}", - LinksetRoot.LocalID, cPrim.LocalID, cPrim.PhysShape, lci.OffsetPos, lci.OffsetRot); + DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,addMemberToShape,mID={1},mShape={2},lci={3}", + LinksetRoot.LocalID, cPrim.LocalID, cPrim.PhysShape, lci); if (cPrim.PhysShape.isNativeShape) { @@ -359,7 +386,7 @@ public sealed class BSLinksetCompound : BSLinkset PhysicsScene.Shapes.CreateGeomMeshOrHull(cPrim, null); BulletShape newShape = cPrim.PhysShape; cPrim.PhysShape = saveShape; - PhysicsScene.PE.AddChildShapeToCompoundShape(LinksetRoot.PhysShape, newShape, lci.OffsetPos, lci.OffsetRot); + PhysicsScene.PE.AddChildShapeToCompoundShape(LinksetRoot.PhysShape, newShape, lci.OffsetFromCenterOfMass, lci.OffsetRot); } else { @@ -371,8 +398,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.OffsetPos, lci.OffsetRot); + PhysicsScene.PE.AddChildShapeToCompoundShape(LinksetRoot.PhysShape, cPrim.PhysShape, lci.OffsetFromCenterOfMass, lci.OffsetRot); } + lci.Index = memberIndex; + memberIndex++; } return false; // 'false' says to move onto the next child in the list }); diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs index 2c84293..185f111 100755 --- a/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs @@ -73,6 +73,8 @@ public abstract class BSPhysObject : PhysicsActor // A linkset of just me Linkset = BSLinkset.Factory(PhysicsScene, this); + PositionDisplacement = OMV.Vector3.Zero; + LastAssetBuildFailed = false; // Default material type @@ -157,6 +159,14 @@ public abstract class BSPhysObject : PhysicsActor public abstract OMV.Vector3 RawPosition { get; set; } public abstract OMV.Vector3 ForcePosition { get; set; } + // Position 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. + public virtual OMV.Vector3 PositionDisplacement { get; set; } + public abstract OMV.Quaternion RawOrientation { get; set; } public abstract OMV.Quaternion ForceOrientation { get; set; } diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs index 02d06b4..003dc54 100644 --- a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs @@ -50,7 +50,10 @@ public sealed class BSPrim : BSPhysObject private bool _grabbed; private bool _isSelected; private bool _isVolumeDetect; + + // _position is what the simulator thinks the positions of the prim is. private OMV.Vector3 _position; + private float _mass; // the mass of this object private float _density; private OMV.Vector3 _force; @@ -320,18 +323,37 @@ public sealed class BSPrim : BSPhysObject } public override OMV.Vector3 ForcePosition { get { - _position = PhysicsScene.PE.GetPosition(PhysBody); + _position = PhysicsScene.PE.GetPosition(PhysBody) - PositionDisplacement; return _position; } set { _position = value; if (PhysBody.HasPhysicalBody) { - PhysicsScene.PE.SetTranslation(PhysBody, _position, _orientation); + PhysicsScene.PE.SetTranslation(PhysBody, _position + PositionDisplacement, _orientation); ActivateIfPhysical(false); } } } + // Override to have position displacement immediately update the physical position. + // A feeble attempt to keep the sim and physical positions in sync + // Must be called at taint time. + public override OMV.Vector3 PositionDisplacement + { + get + { + return base.PositionDisplacement; + } + set + { + base.PositionDisplacement = value; + PhysicsScene.TaintedObject(PhysicsScene.InTaintTime, "BSPrim.setPosition", delegate() + { + if (PhysBody.HasPhysicalBody) + PhysicsScene.PE.SetTranslation(PhysBody, _position + base.PositionDisplacement, _orientation); + }); + } + } // Check that the current position is sane and, if not, modify the position to make it so. // Check for being below terrain and being out of bounds. @@ -590,6 +612,7 @@ public sealed class BSPrim : BSPhysObject _velocity = value; if (PhysBody.HasPhysicalBody) { + DetailLog("{0},BSPrim.ForceVelocity,taint,vel={1}", LocalID, _velocity); PhysicsScene.PE.SetLinearVelocity(PhysBody, _velocity); ActivateIfPhysical(false); } @@ -654,12 +677,7 @@ public sealed class BSPrim : BSPhysObject PhysicsScene.TaintedObject("BSPrim.setOrientation", delegate() { - if (PhysBody.HasPhysicalBody) - { - // _position = PhysicsScene.PE.GetObjectPosition(PhysicsScene.World, BSBody); - // DetailLog("{0},BSPrim.setOrientation,taint,pos={1},orient={2}", LocalID, _position, _orientation); - PhysicsScene.PE.SetTranslation(PhysBody, _position, _orientation); - } + ForceOrientation = _orientation; }); } } @@ -674,7 +692,8 @@ public sealed class BSPrim : BSPhysObject set { _orientation = value; - PhysicsScene.PE.SetTranslation(PhysBody, _position, _orientation); + if (PhysBody.HasPhysicalBody) + PhysicsScene.PE.SetTranslation(PhysBody, _position + PositionDisplacement, _orientation); } } public override int PhysicsActorType { @@ -813,7 +832,7 @@ public sealed class BSPrim : BSPhysObject // PhysicsScene.PE.ClearAllForces(BSBody); // For good measure, make sure the transform is set through to the motion state - PhysicsScene.PE.SetTranslation(PhysBody, _position, _orientation); + PhysicsScene.PE.SetTranslation(PhysBody, _position + PositionDisplacement, _orientation); // Center of mass is at the center of the object // DEBUG DEBUG PhysicsScene.PE.SetCenterOfMassByPosRot(Linkset.LinksetRoot.PhysBody, _position, _orientation); @@ -1615,6 +1634,7 @@ public sealed class BSPrim : BSPhysObject } // Assign directly to the local variables so the normal set actions do not happen + entprop.Position -= PositionDisplacement; _position = entprop.Position; _orientation = entprop.Rotation; _velocity = entprop.Velocity; diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs b/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs index f0c6b99..addab29 100755 --- a/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs @@ -906,7 +906,7 @@ public sealed class BSShapeCollection : IDisposable } } - // While we figure out the real problem, stick a simple native shape on the object. + // While we figure out the real problem, stick in a simple box for the object. BulletShape fillinShape = BuildPhysicalNativeShape(prim, BSPhysicsShapeType.SHAPE_BOX, FixedShapeKey.KEY_BOX); diff --git a/OpenSim/Region/Physics/BulletSPlugin/BulletSimTODO.txt b/OpenSim/Region/Physics/BulletSPlugin/BulletSimTODO.txt index 1540dbb..59cbab9 100755 --- a/OpenSim/Region/Physics/BulletSPlugin/BulletSimTODO.txt +++ b/OpenSim/Region/Physics/BulletSPlugin/BulletSimTODO.txt @@ -170,6 +170,8 @@ Avatar attachments have no mass? http://forums-archive.secondlife.com/54/f0/3179 INTERNAL IMPROVEMENT/CLEANUP ================================================= +Can the 'inTaintTime' flag be cleaned up and used? For instance, a call to + BSScene.TaintedObject() could immediately execute the callback if already in taint time. Create the physical wrapper classes (BulletBody, BulletShape) by methods on BSAPITemplate and make their actual implementation Bullet engine specific. For the short term, just call the existing functions in ShapeCollection. -- cgit v1.1