From 0971c7ae77cae3d238be31f46994b4692af949e3 Mon Sep 17 00:00:00 2001 From: Robert Adams Date: Wed, 7 Aug 2013 07:56:37 -0700 Subject: [PATCH] BulletSim: complete linkage of spring constraint into linkset constraint. --- .../Physics/BulletSPlugin/BSConstraint6Dof.cs | 9 ++- .../Region/Physics/BulletSPlugin/BSLinkset.cs | 2 + .../BulletSPlugin/BSLinksetConstraints.cs | 81 +++++++++++++++---- 3 files changed, 77 insertions(+), 15 deletions(-) diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSConstraint6Dof.cs b/OpenSim/Region/Physics/BulletSPlugin/BSConstraint6Dof.cs index d0949f5d95..5008ff7efa 100755 --- a/OpenSim/Region/Physics/BulletSPlugin/BSConstraint6Dof.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BSConstraint6Dof.cs @@ -32,12 +32,19 @@ using OpenMetaverse; namespace OpenSim.Region.Physics.BulletSPlugin { -public sealed class BSConstraint6Dof : BSConstraint +public class BSConstraint6Dof : BSConstraint { private static string LogHeader = "[BULLETSIM 6DOF CONSTRAINT]"; public override ConstraintType Type { get { return ConstraintType.D6_CONSTRAINT_TYPE; } } + public BSConstraint6Dof(BulletWorld world, BulletBody obj1, BulletBody obj2) :base(world) + { + m_body1 = obj1; + m_body2 = obj2; + m_enabled = false; + } + // Create a btGeneric6DofConstraint public BSConstraint6Dof(BulletWorld world, BulletBody obj1, BulletBody obj2, Vector3 frame1, Quaternion frame1rot, diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs index a051002b39..d4b1c1e8f8 100755 --- a/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs @@ -77,6 +77,8 @@ public abstract class BSLinkset { member = pMember; } + public virtual void ResetLink() { } + public virtual void SetLinkParameters(BSConstraint constrain) { } } public LinksetImplementation LinksetImpl { get; protected set; } diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs index d4ee27d31d..f3b70c35f7 100755 --- a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs @@ -51,16 +51,26 @@ public sealed class BSLinksetConstraints : BSLinkset public float cfm; public float erp; public float solverIterations; + // + public OMV.Vector3 frameInAloc; + public OMV.Quaternion frameInArot; + public OMV.Vector3 frameInBloc; + public OMV.Quaternion frameInBrot; + // Spring + public float springDamping; + public float springStiffness; + + public BSLinkInfoConstraint(BSPrimLinkable pMember) : base(pMember) { constraint = null; - ResetToFixedConstraint(); + ResetLink(); } // Set all the parameters for this constraint to a fixed, non-movable constraint. - public void ResetToFixedConstraint() + public override void ResetLink() { constraintType = ConstraintType.D6_CONSTRAINT_TYPE; linearLimitLow = OMV.Vector3.Zero; @@ -74,10 +84,16 @@ public sealed class BSLinksetConstraints : BSLinkset cfm = BSParam.LinkConstraintCFM; erp = BSParam.LinkConstraintERP; solverIterations = BSParam.LinkConstraintSolverIterations; + frameInAloc = OMV.Vector3.Zero; + frameInArot = OMV.Quaternion.Identity; + frameInBloc = OMV.Vector3.Zero; + frameInBrot = OMV.Quaternion.Identity; + springDamping = -1f; + springStiffness = -1f; } // Given a constraint, apply the current constraint parameters to same. - public void SetConstraintParameters(BSConstraint constrain) + public override void SetLinkParameters(BSConstraint constrain) { switch (constraintType) { @@ -85,6 +101,7 @@ public sealed class BSLinksetConstraints : BSLinkset BSConstraint6Dof constrain6dof = constrain as BSConstraint6Dof; if (constrain6dof != null) { + // NOTE: D6_SPRING_CONSTRAINT_TYPE should be updated if you change any of this code. // zero linear and angular limits makes the objects unable to move in relation to each other constrain6dof.SetLinearLimits(linearLimitLow, linearLimitHigh); constrain6dof.SetAngularLimits(angularLimitLow, angularLimitHigh); @@ -99,6 +116,31 @@ public sealed class BSLinksetConstraints : BSLinkset } } break; + case ConstraintType.D6_SPRING_CONSTRAINT_TYPE: + BSConstraintSpring constrainSpring = constrain as BSConstraintSpring; + if (constrainSpring != null) + { + // zero linear and angular limits makes the objects unable to move in relation to each other + constrainSpring.SetLinearLimits(linearLimitLow, linearLimitHigh); + constrainSpring.SetAngularLimits(angularLimitLow, angularLimitHigh); + + // tweek the constraint to increase stability + constrainSpring.UseFrameOffset(useFrameOffset); + constrainSpring.TranslationalLimitMotor(enableTransMotor, transMotorMaxVel, transMotorMaxForce); + constrainSpring.SetCFMAndERP(cfm, erp); + if (solverIterations != 0f) + { + constrainSpring.SetSolverIterations(solverIterations); + } + for (int ii = 0; ii < 6; ii++) + { + if (springDamping != -1) + constrainSpring.SetDamping(ii, springDamping); + if (springStiffness != -1) + constrainSpring.SetStiffness(ii, springStiffness); + } + } + break; default: break; } @@ -262,8 +304,8 @@ public sealed class BSLinksetConstraints : BSLinkset // Create a static constraint between the two passed objects private BSConstraint BuildConstraint(BSPrimLinkable rootPrim, BSLinkInfo li) { - BSLinkInfoConstraint liConstraint = li as BSLinkInfoConstraint; - if (liConstraint == null) + BSLinkInfoConstraint linkInfo = li as BSLinkInfoConstraint; + if (linkInfo == null) return null; // Zero motion for children so they don't interpolate @@ -271,27 +313,25 @@ public sealed class BSLinksetConstraints : BSLinkset BSConstraint constrain = null; - switch (liConstraint.constraintType) + switch (linkInfo.constraintType) { 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; + OMV.Vector3 childRelativePosition = linkInfo.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); + DetailLog("{0},BSLinksetConstraint.BuildConstraint,6Dof,rBody={1},cBody={2},rLoc={3},cLoc={4},midLoc={7}", + rootPrim.LocalID, rootPrim.PhysBody, linkInfo.member.PhysBody, + rootPrim.Position, linkInfo.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 ); + m_physicsScene.World, rootPrim.PhysBody, linkInfo.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 @@ -319,12 +359,24 @@ public sealed class BSLinksetConstraints : BSLinkset // ================================================================================== */ + break; + case ConstraintType.D6_SPRING_CONSTRAINT_TYPE: + constrain = new BSConstraintSpring(m_physicsScene.World, rootPrim.PhysBody, linkInfo.member.PhysBody, + linkInfo.frameInAloc, linkInfo.frameInArot, linkInfo.frameInBloc, linkInfo.frameInBrot, + true /*useLinearReferenceFrameA*/, + true /*disableCollisionsBetweenLinkedBodies*/); + DetailLog("{0},BSLinksetConstraint.BuildConstraint,spring,root={1},rBody={2},child={3},cBody={4},rLoc={5},cLoc={6},midLoc={7}", + rootPrim.LocalID, + rootPrim.LocalID, rootPrim.PhysBody.AddrString, + linkInfo.member.LocalID, linkInfo.member.PhysBody.AddrString, + rootPrim.Position, linkInfo.member.Position); + break; default: break; } - liConstraint.SetConstraintParameters(constrain); + linkInfo.SetLinkParameters(constrain); m_physicsScene.Constraints.AddConstraint(constrain); @@ -401,6 +453,7 @@ public sealed class BSLinksetConstraints : BSLinkset // If constraint doesn't exist yet, create it. constrain = BuildConstraint(LinksetRoot, li); } + li.SetLinkParameters(constrain); constrain.RecomputeConstraintVariables(linksetMass); // PhysicsScene.PE.DumpConstraint(PhysicsScene.World, constrain.Constraint); // DEBUG DEBUG