From 97698ae3119e01ecd2c98f7a17ad37204b9bfd9c Mon Sep 17 00:00:00 2001 From: Robert Adams Date: Mon, 10 Jun 2013 06:40:07 -0700 Subject: [PATCH] BulletSim: More tweaking on center-of-mass. Almost there. Changes have no effect if LinksetOffsetCenterOfMass=false (the default). --- .../BulletSPlugin/BSLinksetCompound.cs | 78 ++++--------------- .../Physics/BulletSPlugin/BSPrimDisplaced.cs | 63 ++++++--------- .../Physics/BulletSPlugin/BSPrimLinkable.cs | 10 +++ .../Region/Physics/BulletSPlugin/BSScene.cs | 1 - 4 files changed, 51 insertions(+), 101 deletions(-) diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs index 30f6c8c173..96626ca50e 100755 --- a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs @@ -35,62 +35,6 @@ using OMV = OpenMetaverse; namespace OpenSim.Region.Physics.BulletSPlugin { - /* -// When a child is linked, the relationship position of the child to the parent -// is remembered so the child's world position can be recomputed when it is -// removed from the linkset. -sealed class BSLinksetCompoundInfo : BSLinksetInfo -{ - public int Index; - public OMV.Vector3 OffsetFromRoot; - public OMV.Vector3 OffsetFromCenterOfMass; - public OMV.Quaternion OffsetRot; - public BSLinksetCompoundInfo(int indx, OMV.Vector3 p, OMV.Quaternion r) - { - Index = indx; - OffsetFromRoot = p; - OffsetFromCenterOfMass = p; - OffsetRot = r; - } - // 'centerDisplacement' is the distance from the root the the center-of-mass (Bullet 'zero' of the shape) - public BSLinksetCompoundInfo(int indx, BSPrimLinkable root, BSPrimLinkable child, OMV.Vector3 centerDisplacement) - { - // Each child position and rotation is given relative to the center-of-mass. - OMV.Quaternion invRootOrientation = OMV.Quaternion.Inverse(root.RawOrientation); - OMV.Vector3 displacementFromRoot = (child.RawPosition - root.RawPosition) * invRootOrientation; - OMV.Vector3 displacementFromCOM = displacementFromRoot - centerDisplacement; - OMV.Quaternion displacementRot = child.RawOrientation * invRootOrientation; - - // Save relative position for recomputing child's world position after moving linkset. - Index = indx; - OffsetFromRoot = displacementFromRoot; - OffsetFromCenterOfMass = displacementFromCOM; - OffsetRot = displacementRot; - } - public override void Clear() - { - Index = 0; - OffsetFromRoot = OMV.Vector3.Zero; - OffsetFromCenterOfMass = OMV.Vector3.Zero; - OffsetRot = OMV.Quaternion.Identity; - } - public override string ToString() - { - StringBuilder buff = new StringBuilder(); - buff.Append(""); - return buff.ToString(); - } -}; - */ - public sealed class BSLinksetCompound : BSLinkset { private static string LogHeader = "[BULLETSIM LINKSET COMPOUND]"; @@ -151,7 +95,9 @@ public sealed class BSLinksetCompound : BSLinkset public override bool MakeStatic(BSPrimLinkable child) { bool ret = false; + DetailLog("{0},BSLinksetCompound.MakeStatic,call,IsRoot={1}", child.LocalID, IsRoot(child)); + child.ClearDisplacement(); if (IsRoot(child)) { // Schedule a rebuild to verify that the root shape is set to the real shape. @@ -364,16 +310,28 @@ public sealed class BSLinksetCompound : BSLinkset int memberIndex = 1; ForEachMember(delegate(BSPrimLinkable cPrim) { - // Root shape is always index zero. - cPrim.LinksetChildIndex = IsRoot(cPrim) ? 0 : memberIndex; + if (IsRoot(cPrim)) + { + // Root shape is always index zero. + cPrim.LinksetChildIndex = 0; + } + else + { + cPrim.LinksetChildIndex = memberIndex; + memberIndex++; + } // Get a reference to the shape of the child and add that shape to the linkset compound shape BSShape childShape = cPrim.PhysShape.GetReference(m_physicsScene, cPrim); + + // Offset the child shape from the center-of-mass and rotate it to vehicle relative OMV.Vector3 offsetPos = (cPrim.RawPosition - LinksetRoot.RawPosition) * invRootOrientation - centerDisplacementV; OMV.Quaternion offsetRot = OMV.Quaternion.Normalize(cPrim.RawOrientation) * invRootOrientation; + + // Add the child shape to the compound shape being built m_physicsScene.PE.AddChildShapeToCompoundShape(linksetShape.physShapeInfo, childShape.physShapeInfo, offsetPos, offsetRot); DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,addChild,indx={1},cShape={2},offPos={3},offRot={4}", - LinksetRoot.LocalID, memberIndex, childShape, offsetPos, offsetRot); + LinksetRoot.LocalID, cPrim.LinksetChildIndex, childShape, offsetPos, offsetRot); // Since we are borrowing the shape of the child, disable the origional child body if (!IsRoot(cPrim)) @@ -385,8 +343,6 @@ public sealed class BSLinksetCompound : BSLinkset cPrim.PhysBody.collisionType = CollisionType.LinksetChild; } - memberIndex++; - return false; // 'false' says to move onto the next child in the list }); diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrimDisplaced.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrimDisplaced.cs index 9a05349c7f..a11bca0ba1 100755 --- a/OpenSim/Region/Physics/BulletSPlugin/BSPrimDisplaced.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrimDisplaced.cs @@ -51,7 +51,7 @@ public class BSPrimDisplaced : BSPrim // 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 + // This routine works by capturing ForcePosition and // adjusting the simulator values (being set) into the physical values. // The conversion is also done in the opposite direction (physical origin -> simulator origin). // @@ -60,8 +60,6 @@ public class BSPrimDisplaced : BSPrim // class. public virtual OMV.Vector3 PositionDisplacement { get; set; } - public virtual OMV.Quaternion OrientationDisplacement { get; set; } - public virtual OMV.Quaternion OrientationDisplacementOrigInv { get; set; } public BSPrimDisplaced(uint localID, String primName, BSScene parent_scene, OMV.Vector3 pos, OMV.Vector3 size, OMV.Quaternion rotation, PrimitiveBaseShape pbs, bool pisPhysical) @@ -70,15 +68,17 @@ public class BSPrimDisplaced : BSPrim ClearDisplacement(); } + // Clears any center-of-mass displacement introduced by linksets, etc. + // Does not clear the displacement set by the user. public void ClearDisplacement() { - PositionDisplacement = OMV.Vector3.Zero; - OrientationDisplacement = OMV.Quaternion.Identity; + SetEffectiveCenterOfMassDisplacement(OMV.Vector3.Zero); } // 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). + // Called at taint time. public virtual void SetEffectiveCenterOfMassDisplacement(Vector3 centerOfMassDisplacement) { Vector3 comDisp; @@ -87,21 +87,17 @@ public class BSPrimDisplaced : BSPrim else comDisp = centerOfMassDisplacement; + if (comDisp.ApproxEquals(Vector3.Zero, 0.01f) ) + comDisp = Vector3.Zero; + DetailLog("{0},BSPrimDisplaced.SetEffectiveCenterOfMassDisplacement,userSet={1},comDisp={2}", LocalID, UserSetCenterOfMassDisplacement.HasValue, comDisp); - if (comDisp == Vector3.Zero) + if ( comDisp != PositionDisplacement ) { - // If there is no diplacement. Things get reset. - PositionDisplacement = OMV.Vector3.Zero; - OrientationDisplacement = OMV.Quaternion.Identity; - } - else - { - // Remember the displacement from root as well as the origional rotation of the - // new center-of-mass. + // Displacement setting is changing. + // The relationship between the physical object and simulated object must be aligned. PositionDisplacement = comDisp; - OrientationDisplacement = Quaternion.Normalize(RawOrientation); - OrientationDisplacementOrigInv = Quaternion.Inverse(OrientationDisplacement); + this.ForcePosition = RawPosition; } } @@ -112,7 +108,11 @@ public class BSPrimDisplaced : BSPrim { if (PositionDisplacement != OMV.Vector3.Zero) { - OMV.Vector3 displacedPos = value - (PositionDisplacement * (OrientationDisplacementOrigInv * RawOrientation)); + // The displacement is always relative to the vehicle so, when setting position, + // the caller means to set the position of the root prim. + // This offsets the setting value to the center-of-mass and sends that to the + // physics engine. + OMV.Vector3 displacedPos = value - (PositionDisplacement * RawOrientation); DetailLog("{0},BSPrimDisplaced.ForcePosition,val={1},disp={2},newPos={3}", LocalID, value, PositionDisplacement, displacedPos); base.ForcePosition = displacedPos; } @@ -123,26 +123,12 @@ public class BSPrimDisplaced : BSPrim } } - public override Quaternion ForceOrientation - { - get { return base.ForceOrientation; } - set - { - // Changing orientation also changes the position of the center-of-mass since - // the orientation passed is for a rotation around the root prim's center. - 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? + // These are also overridden by BSPrimLinkable if the prim can be part of a linkset public override OMV.Vector3 CenterOfMass { get { return RawPosition; } } - // Is this used? public override OMV.Vector3 GeometricCenter { get { return RawPosition; } @@ -151,18 +137,17 @@ public class BSPrimDisplaced : BSPrim public override void UpdateProperties(EntityProperties entprop) { // Undo any center-of-mass displacement that might have been done. - if (PositionDisplacement != OMV.Vector3.Zero || OrientationDisplacement != OMV.Quaternion.Identity) + if (PositionDisplacement != OMV.Vector3.Zero) { - // The origional shape was offset from 'zero' by PositionDisplacement and rotated by OrientationDisplacement. - // These physical locations and rotation must be back converted to be centered around the displaced + // The origional shape was offset from 'zero' by PositionDisplacement. + // These physical location must be back converted to be centered around the displaced // root shape. // The root position is the reported position displaced by the rotated displacement. - OMV.Vector3 displacedPos = entprop.Position + (PositionDisplacement * (entprop.Rotation * OrientationDisplacementOrigInv)); - - DetailLog("{0},BSPrimDisplaced.ForcePosition,physPos={1},disp={2},newPos={3}", LocalID, entprop.Position, PositionDisplacement, displacedPos); + OMV.Vector3 displacedPos = entprop.Position + (PositionDisplacement * entprop.Rotation); + DetailLog("{0},BSPrimDisplaced.UpdateProperties,physPos={1},disp={2},newPos={3}", + LocalID, entprop.Position, PositionDisplacement, displacedPos); entprop.Position = displacedPos; - // entprop.Rotation = something; } base.UpdateProperties(entprop); diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrimLinkable.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrimLinkable.cs index 6d7de35aba..70586466cf 100755 --- a/OpenSim/Region/Physics/BulletSPlugin/BSPrimLinkable.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrimLinkable.cs @@ -134,6 +134,16 @@ public class BSPrimLinkable : BSPrimDisplaced get { return Linkset.LinksetMass; } } + public override OMV.Vector3 CenterOfMass + { + get { return Linkset.CenterOfMass; } + } + + public override OMV.Vector3 GeometricCenter + { + get { return Linkset.GeometricCenter; } + } + // Refresh the linkset structure and parameters when the prim's physical parameters are changed. public override void UpdatePhysicalParameters() { diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs b/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs index 1645c989d7..e56a6f6725 100644 --- a/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs @@ -785,7 +785,6 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters { // The simulation of the time interval took less than realtime. // Do a sleep for the rest of realtime. - DetailLog("{0},BulletSPluginPhysicsThread,sleeping={1}", BSScene.DetailLogZero, simulationTimeVsRealtimeDifferenceMS); Thread.Sleep(simulationTimeVsRealtimeDifferenceMS); } else