OpenSim
/
OpenSimMirror
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

BulletSim: complete linkage of spring constraint into linkset constraint.

varregion
Robert Adams 2013-08-07 07:56:37 -07:00
parent 9a7d0e489c
commit 0971c7ae77
3 changed files with 77 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
OpenSim/Region/Physics/BulletSPlugin/BSConstraint6Dof.cs
View File

@ -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,

2
OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
View File

@ -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; }

81
OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs
View File

@ -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