From 5bcccfc305ae4f5a74b9b816781a2a643daa23b3 Mon Sep 17 00:00:00 2001 From: Robert Adams Date: Thu, 1 Aug 2013 12:35:22 -0700 Subject: BulletSim: add BSLinkInfo structure to remember link specific information for each link in a linkset. Extend BSLinksetConstraint to create and use BSLinkInfo with the default static constraint. --- OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs | 51 +++-- .../Physics/BulletSPlugin/BSLinksetCompound.cs | 4 +- .../Physics/BulletSPlugin/BSLinksetConstraints.cs | 205 ++++++++++++++------- 3 files changed, 174 insertions(+), 86 deletions(-) (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs index 7f94666..9613fe0 100755 --- a/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs @@ -70,6 +70,15 @@ public abstract class BSLinkset return ret; } + public class BSLinkInfo + { + public BSPrimLinkable member; + public BSLinkInfo(BSPrimLinkable pMember) + { + member = pMember; + } + } + public BSPrimLinkable LinksetRoot { get; protected set; } protected BSScene m_physicsScene { get; private set; } @@ -78,7 +87,8 @@ public abstract class BSLinkset public int LinksetID { get; private set; } // The children under the root in this linkset. - protected HashSet m_children; + // protected HashSet m_children; + protected Dictionary m_children; // We lock the diddling of linkset classes to prevent any badness. // This locks the modification of the instances of this class. Changes @@ -109,7 +119,7 @@ public abstract class BSLinkset m_nextLinksetID = 1; m_physicsScene = scene; LinksetRoot = parent; - m_children = new HashSet(); + m_children = new Dictionary(); LinksetMass = parent.RawMass; Rebuilding = false; @@ -170,17 +180,7 @@ public abstract class BSLinkset bool ret = false; lock (m_linksetActivityLock) { - ret = m_children.Contains(child); - /* Safer version but the above should work - foreach (BSPrimLinkable bp in m_children) - { - if (child.LocalID == bp.LocalID) - { - ret = true; - break; - } - } - */ + ret = m_children.ContainsKey(child); } return ret; } @@ -194,7 +194,24 @@ public abstract class BSLinkset lock (m_linksetActivityLock) { action(LinksetRoot); - foreach (BSPrimLinkable po in m_children) + foreach (BSPrimLinkable po in m_children.Keys) + { + if (action(po)) + break; + } + } + return ret; + } + + // Perform an action on each member of the linkset including root prim. + // Depends on the action on whether this should be done at taint time. + public delegate bool ForEachLinkInfoAction(BSLinkInfo obj); + public virtual bool ForEachLinkInfo(ForEachLinkInfoAction action) + { + bool ret = false; + lock (m_linksetActivityLock) + { + foreach (BSLinkInfo po in m_children.Values) { if (action(po)) break; @@ -364,7 +381,7 @@ public abstract class BSLinkset { lock (m_linksetActivityLock) { - foreach (BSPrimLinkable bp in m_children) + foreach (BSPrimLinkable bp in m_children.Keys) { mass += bp.RawMass; } @@ -382,7 +399,7 @@ public abstract class BSLinkset com = LinksetRoot.Position * LinksetRoot.RawMass; float totalMass = LinksetRoot.RawMass; - foreach (BSPrimLinkable bp in m_children) + foreach (BSPrimLinkable bp in m_children.Keys) { com += bp.Position * bp.RawMass; totalMass += bp.RawMass; @@ -401,7 +418,7 @@ public abstract class BSLinkset { com = LinksetRoot.Position; - foreach (BSPrimLinkable bp in m_children) + foreach (BSPrimLinkable bp in m_children.Keys) { com += bp.Position; } diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs index 6359046..53c3584 100755 --- a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs @@ -257,7 +257,7 @@ public sealed class BSLinksetCompound : BSLinkset { if (!HasChild(child)) { - m_children.Add(child); + m_children.Add(child, new BSLinkInfo(child)); DetailLog("{0},BSLinksetCompound.AddChildToLinkset,call,child={1}", LinksetRoot.LocalID, child.LocalID); @@ -353,7 +353,7 @@ public sealed class BSLinksetCompound : BSLinkset // Add the shapes of all the components of the linkset int memberIndex = 1; - ForEachMember(delegate(BSPrimLinkable cPrim) + ForEachMember((cPrim) => { if (IsRoot(cPrim)) { diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs index f17d698..d98bf77 100755 --- a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs @@ -36,6 +36,75 @@ public sealed class BSLinksetConstraints : BSLinkset { // private static string LogHeader = "[BULLETSIM LINKSET CONSTRAINTS]"; + public class BSLinkInfoConstraint : BSLinkInfo + { + public ConstraintType constraintType; + public BSConstraint constraint; + public OMV.Vector3 linearLimitLow; + public OMV.Vector3 linearLimitHigh; + public OMV.Vector3 angularLimitLow; + public OMV.Vector3 angularLimitHigh; + public bool useFrameOffset; + public bool enableTransMotor; + public float transMotorMaxVel; + public float transMotorMaxForce; + public float cfm; + public float erp; + public float solverIterations; + + public BSLinkInfoConstraint(BSPrimLinkable pMember) + : base(pMember) + { + constraint = null; + ResetToFixedConstraint(); + } + + // Set all the parameters for this constraint to a fixed, non-movable constraint. + public void ResetToFixedConstraint() + { + constraintType = ConstraintType.D6_CONSTRAINT_TYPE; + linearLimitLow = OMV.Vector3.Zero; + linearLimitHigh = OMV.Vector3.Zero; + angularLimitLow = OMV.Vector3.Zero; + angularLimitHigh = OMV.Vector3.Zero; + useFrameOffset = BSParam.LinkConstraintUseFrameOffset; + enableTransMotor = BSParam.LinkConstraintEnableTransMotor; + transMotorMaxVel = BSParam.LinkConstraintTransMotorMaxVel; + transMotorMaxForce = BSParam.LinkConstraintTransMotorMaxForce; + cfm = BSParam.LinkConstraintCFM; + erp = BSParam.LinkConstraintERP; + solverIterations = BSParam.LinkConstraintSolverIterations; + } + + // Given a constraint, apply the current constraint parameters to same. + public void SetConstraintParameters(BSConstraint constrain) + { + switch (constraintType) + { + case ConstraintType.D6_CONSTRAINT_TYPE: + BSConstraint6Dof constrain6dof = constrain as BSConstraint6Dof; + if (constrain6dof != null) + { + // zero linear and angular limits makes the objects unable to move in relation to each other + constrain6dof.SetLinearLimits(linearLimitLow, linearLimitHigh); + constrain6dof.SetAngularLimits(angularLimitLow, angularLimitHigh); + + // tweek the constraint to increase stability + constrain6dof.UseFrameOffset(useFrameOffset); + constrain6dof.TranslationalLimitMotor(enableTransMotor, transMotorMaxVel, transMotorMaxForce); + constrain6dof.SetCFMAndERP(cfm, erp); + if (solverIterations != 0f) + { + constrain6dof.SetSolverIterations(solverIterations); + } + } + break; + default: + break; + } + } + } + public BSLinksetConstraints(BSScene scene, BSPrimLinkable parent) : base(scene, parent) { } @@ -142,7 +211,7 @@ public sealed class BSLinksetConstraints : BSLinkset { if (!HasChild(child)) { - m_children.Add(child); + m_children.Add(child, new BSLinkInfoConstraint(child)); DetailLog("{0},BSLinksetConstraints.AddChildToLinkset,call,child={1}", LinksetRoot.LocalID, child.LocalID); @@ -190,73 +259,74 @@ public sealed class BSLinksetConstraints : BSLinkset } // Create a static constraint between the two passed objects - private BSConstraint BuildConstraint(BSPrimLinkable rootPrim, BSPrimLinkable childPrim) + private BSConstraint BuildConstraint(BSPrimLinkable rootPrim, BSLinkInfo li) { + BSLinkInfoConstraint liConstraint = li as BSLinkInfoConstraint; + if (liConstraint == null) + return null; + // Zero motion for children so they don't interpolate - childPrim.ZeroMotion(true); - - // Relative position normalized to the root prim - // Essentually a vector pointing from center of rootPrim to center of childPrim - OMV.Vector3 childRelativePosition = childPrim.Position - rootPrim.Position; - - // real world coordinate of midpoint between the two objects - OMV.Vector3 midPoint = rootPrim.Position + (childRelativePosition / 2); - - DetailLog("{0},BSLinksetConstraint.BuildConstraint,taint,root={1},rBody={2},child={3},cBody={4},rLoc={5},cLoc={6},midLoc={7}", - rootPrim.LocalID, - rootPrim.LocalID, rootPrim.PhysBody.AddrString, - childPrim.LocalID, childPrim.PhysBody.AddrString, - rootPrim.Position, childPrim.Position, midPoint); - - // create a constraint that allows no freedom of movement between the two objects - // http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?t=4818 - - BSConstraint6Dof constrain = new BSConstraint6Dof( - m_physicsScene.World, rootPrim.PhysBody, childPrim.PhysBody, midPoint, true, true ); - // PhysicsScene.World, childPrim.BSBody, rootPrim.BSBody, midPoint, true, true ); - - /* NOTE: below is an attempt to build constraint with full frame computation, etc. - * Using the midpoint is easier since it lets the Bullet code manipulate the transforms - * of the objects. - * Code left for future programmers. - // ================================================================================== - // relative position normalized to the root prim - OMV.Quaternion invThisOrientation = OMV.Quaternion.Inverse(rootPrim.Orientation); - OMV.Vector3 childRelativePosition = (childPrim.Position - rootPrim.Position) * invThisOrientation; - - // relative rotation of the child to the parent - OMV.Quaternion childRelativeRotation = invThisOrientation * childPrim.Orientation; - OMV.Quaternion inverseChildRelativeRotation = OMV.Quaternion.Inverse(childRelativeRotation); - - DetailLog("{0},BSLinksetConstraint.PhysicallyLinkAChildToRoot,taint,root={1},child={2}", rootPrim.LocalID, rootPrim.LocalID, childPrim.LocalID); - BS6DofConstraint constrain = new BS6DofConstraint( - PhysicsScene.World, rootPrim.Body, childPrim.Body, - OMV.Vector3.Zero, - OMV.Quaternion.Inverse(rootPrim.Orientation), - OMV.Vector3.Zero, - OMV.Quaternion.Inverse(childPrim.Orientation), - true, - true - ); - // ================================================================================== - */ + li.member.ZeroMotion(true); - m_physicsScene.Constraints.AddConstraint(constrain); + BSConstraint constrain = null; - // zero linear and angular limits makes the objects unable to move in relation to each other - constrain.SetLinearLimits(OMV.Vector3.Zero, OMV.Vector3.Zero); - constrain.SetAngularLimits(OMV.Vector3.Zero, OMV.Vector3.Zero); - - // tweek the constraint to increase stability - constrain.UseFrameOffset(BSParam.LinkConstraintUseFrameOffset); - constrain.TranslationalLimitMotor(BSParam.LinkConstraintEnableTransMotor, - BSParam.LinkConstraintTransMotorMaxVel, - BSParam.LinkConstraintTransMotorMaxForce); - constrain.SetCFMAndERP(BSParam.LinkConstraintCFM, BSParam.LinkConstraintERP); - if (BSParam.LinkConstraintSolverIterations != 0f) + switch (liConstraint.constraintType) { - constrain.SetSolverIterations(BSParam.LinkConstraintSolverIterations); + case ConstraintType.D6_CONSTRAINT_TYPE: + // Relative position normalized to the root prim + // Essentually a vector pointing from center of rootPrim to center of li.member + OMV.Vector3 childRelativePosition = liConstraint.member.Position - rootPrim.Position; + + // real world coordinate of midpoint between the two objects + OMV.Vector3 midPoint = rootPrim.Position + (childRelativePosition / 2); + + DetailLog("{0},BSLinksetConstraint.BuildConstraint,taint,root={1},rBody={2},child={3},cBody={4},rLoc={5},cLoc={6},midLoc={7}", + rootPrim.LocalID, + rootPrim.LocalID, rootPrim.PhysBody.AddrString, + liConstraint.member.LocalID, liConstraint.member.PhysBody.AddrString, + rootPrim.Position, liConstraint.member.Position, midPoint); + + // create a constraint that allows no freedom of movement between the two objects + // http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?t=4818 + + constrain = new BSConstraint6Dof( + m_physicsScene.World, rootPrim.PhysBody, liConstraint.member.PhysBody, midPoint, true, true ); + + /* NOTE: below is an attempt to build constraint with full frame computation, etc. + * Using the midpoint is easier since it lets the Bullet code manipulate the transforms + * of the objects. + * Code left for future programmers. + // ================================================================================== + // relative position normalized to the root prim + OMV.Quaternion invThisOrientation = OMV.Quaternion.Inverse(rootPrim.Orientation); + OMV.Vector3 childRelativePosition = (liConstraint.member.Position - rootPrim.Position) * invThisOrientation; + + // relative rotation of the child to the parent + OMV.Quaternion childRelativeRotation = invThisOrientation * liConstraint.member.Orientation; + OMV.Quaternion inverseChildRelativeRotation = OMV.Quaternion.Inverse(childRelativeRotation); + + DetailLog("{0},BSLinksetConstraint.PhysicallyLinkAChildToRoot,taint,root={1},child={2}", rootPrim.LocalID, rootPrim.LocalID, liConstraint.member.LocalID); + constrain = new BS6DofConstraint( + PhysicsScene.World, rootPrim.Body, liConstraint.member.Body, + OMV.Vector3.Zero, + OMV.Quaternion.Inverse(rootPrim.Orientation), + OMV.Vector3.Zero, + OMV.Quaternion.Inverse(liConstraint.member.Orientation), + true, + true + ); + // ================================================================================== + */ + + break; + default: + break; } + + liConstraint.SetConstraintParameters(constrain); + + m_physicsScene.Constraints.AddConstraint(constrain); + return constrain; } @@ -317,23 +387,24 @@ public sealed class BSLinksetConstraints : BSLinkset return; // Note the 'finally' clause at the botton which will get executed. } - foreach (BSPrimLinkable child in m_children) + ForEachLinkInfo((li) => { // A child in the linkset physically shows the mass of the whole linkset. // This allows Bullet to apply enough force on the child to move the whole linkset. // (Also do the mass stuff before recomputing the constraint so mass is not zero.) - child.UpdatePhysicalMassProperties(linksetMass, true); + li.member.UpdatePhysicalMassProperties(linksetMass, true); BSConstraint constrain; - if (!m_physicsScene.Constraints.TryGetConstraint(LinksetRoot.PhysBody, child.PhysBody, out constrain)) + if (!m_physicsScene.Constraints.TryGetConstraint(LinksetRoot.PhysBody, li.member.PhysBody, out constrain)) { // If constraint doesn't exist yet, create it. - constrain = BuildConstraint(LinksetRoot, child); + constrain = BuildConstraint(LinksetRoot, li); } constrain.RecomputeConstraintVariables(linksetMass); // PhysicsScene.PE.DumpConstraint(PhysicsScene.World, constrain.Constraint); // DEBUG DEBUG - } + return false; // 'false' says to keep processing other members + }); } finally { -- cgit v1.1