From 87aedc44a09d671744c46a4a261a2afa9e7346a1 Mon Sep 17 00:00:00 2001
From: Robert Adams
Date: Wed, 7 Aug 2013 07:56:37 -0700
Subject: BulletSim: complete linkage of spring constraint into linkset
constraint.
---
.../Physics/BulletSPlugin/BSConstraint6Dof.cs | 9 ++-
OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs | 2 +
.../Physics/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 d0949f5..5008ff7 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 a051002..d4b1c1e 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 d4ee27d..f3b70c3 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
@@ -320,11 +360,23 @@ 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
--
cgit v1.1