diff options
author | Robert Adams | 2013-08-07 07:56:37 -0700 |
---|---|---|
committer | Justin Clark-Casey (justincc) | 2013-09-20 21:01:53 +0100 |
commit | 87aedc44a09d671744c46a4a261a2afa9e7346a1 (patch) | |
tree | 641b5204a81bfe96d34dbe9b8537875b6b15f732 | |
parent | BulletSim: add spring constraint to linkset constraint types. (diff) | |
download | opensim-SC_OLD-87aedc44a09d671744c46a4a261a2afa9e7346a1.zip opensim-SC_OLD-87aedc44a09d671744c46a4a261a2afa9e7346a1.tar.gz opensim-SC_OLD-87aedc44a09d671744c46a4a261a2afa9e7346a1.tar.bz2 opensim-SC_OLD-87aedc44a09d671744c46a4a261a2afa9e7346a1.tar.xz |
BulletSim: complete linkage of spring constraint into linkset constraint.
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; | |||
32 | namespace OpenSim.Region.Physics.BulletSPlugin | 32 | namespace OpenSim.Region.Physics.BulletSPlugin |
33 | { | 33 | { |
34 | 34 | ||
35 | public sealed class BSConstraint6Dof : BSConstraint | 35 | public class BSConstraint6Dof : BSConstraint |
36 | { | 36 | { |
37 | private static string LogHeader = "[BULLETSIM 6DOF CONSTRAINT]"; | 37 | private static string LogHeader = "[BULLETSIM 6DOF CONSTRAINT]"; |
38 | 38 | ||
39 | public override ConstraintType Type { get { return ConstraintType.D6_CONSTRAINT_TYPE; } } | 39 | public override ConstraintType Type { get { return ConstraintType.D6_CONSTRAINT_TYPE; } } |
40 | 40 | ||
41 | public BSConstraint6Dof(BulletWorld world, BulletBody obj1, BulletBody obj2) :base(world) | ||
42 | { | ||
43 | m_body1 = obj1; | ||
44 | m_body2 = obj2; | ||
45 | m_enabled = false; | ||
46 | } | ||
47 | |||
41 | // Create a btGeneric6DofConstraint | 48 | // Create a btGeneric6DofConstraint |
42 | public BSConstraint6Dof(BulletWorld world, BulletBody obj1, BulletBody obj2, | 49 | public BSConstraint6Dof(BulletWorld world, BulletBody obj1, BulletBody obj2, |
43 | Vector3 frame1, Quaternion frame1rot, | 50 | 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 | |||
77 | { | 77 | { |
78 | member = pMember; | 78 | member = pMember; |
79 | } | 79 | } |
80 | public virtual void ResetLink() { } | ||
81 | public virtual void SetLinkParameters(BSConstraint constrain) { } | ||
80 | } | 82 | } |
81 | 83 | ||
82 | public LinksetImplementation LinksetImpl { get; protected set; } | 84 | 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 | |||
51 | public float cfm; | 51 | public float cfm; |
52 | public float erp; | 52 | public float erp; |
53 | public float solverIterations; | 53 | public float solverIterations; |
54 | // | ||
55 | public OMV.Vector3 frameInAloc; | ||
56 | public OMV.Quaternion frameInArot; | ||
57 | public OMV.Vector3 frameInBloc; | ||
58 | public OMV.Quaternion frameInBrot; | ||
59 | // Spring | ||
60 | public float springDamping; | ||
61 | public float springStiffness; | ||
62 | |||
63 | |||
54 | 64 | ||
55 | public BSLinkInfoConstraint(BSPrimLinkable pMember) | 65 | public BSLinkInfoConstraint(BSPrimLinkable pMember) |
56 | : base(pMember) | 66 | : base(pMember) |
57 | { | 67 | { |
58 | constraint = null; | 68 | constraint = null; |
59 | ResetToFixedConstraint(); | 69 | ResetLink(); |
60 | } | 70 | } |
61 | 71 | ||
62 | // Set all the parameters for this constraint to a fixed, non-movable constraint. | 72 | // Set all the parameters for this constraint to a fixed, non-movable constraint. |
63 | public void ResetToFixedConstraint() | 73 | public override void ResetLink() |
64 | { | 74 | { |
65 | constraintType = ConstraintType.D6_CONSTRAINT_TYPE; | 75 | constraintType = ConstraintType.D6_CONSTRAINT_TYPE; |
66 | linearLimitLow = OMV.Vector3.Zero; | 76 | linearLimitLow = OMV.Vector3.Zero; |
@@ -74,10 +84,16 @@ public sealed class BSLinksetConstraints : BSLinkset | |||
74 | cfm = BSParam.LinkConstraintCFM; | 84 | cfm = BSParam.LinkConstraintCFM; |
75 | erp = BSParam.LinkConstraintERP; | 85 | erp = BSParam.LinkConstraintERP; |
76 | solverIterations = BSParam.LinkConstraintSolverIterations; | 86 | solverIterations = BSParam.LinkConstraintSolverIterations; |
87 | frameInAloc = OMV.Vector3.Zero; | ||
88 | frameInArot = OMV.Quaternion.Identity; | ||
89 | frameInBloc = OMV.Vector3.Zero; | ||
90 | frameInBrot = OMV.Quaternion.Identity; | ||
91 | springDamping = -1f; | ||
92 | springStiffness = -1f; | ||
77 | } | 93 | } |
78 | 94 | ||
79 | // Given a constraint, apply the current constraint parameters to same. | 95 | // Given a constraint, apply the current constraint parameters to same. |
80 | public void SetConstraintParameters(BSConstraint constrain) | 96 | public override void SetLinkParameters(BSConstraint constrain) |
81 | { | 97 | { |
82 | switch (constraintType) | 98 | switch (constraintType) |
83 | { | 99 | { |
@@ -85,6 +101,7 @@ public sealed class BSLinksetConstraints : BSLinkset | |||
85 | BSConstraint6Dof constrain6dof = constrain as BSConstraint6Dof; | 101 | BSConstraint6Dof constrain6dof = constrain as BSConstraint6Dof; |
86 | if (constrain6dof != null) | 102 | if (constrain6dof != null) |
87 | { | 103 | { |
104 | // NOTE: D6_SPRING_CONSTRAINT_TYPE should be updated if you change any of this code. | ||
88 | // zero linear and angular limits makes the objects unable to move in relation to each other | 105 | // zero linear and angular limits makes the objects unable to move in relation to each other |
89 | constrain6dof.SetLinearLimits(linearLimitLow, linearLimitHigh); | 106 | constrain6dof.SetLinearLimits(linearLimitLow, linearLimitHigh); |
90 | constrain6dof.SetAngularLimits(angularLimitLow, angularLimitHigh); | 107 | constrain6dof.SetAngularLimits(angularLimitLow, angularLimitHigh); |
@@ -99,6 +116,31 @@ public sealed class BSLinksetConstraints : BSLinkset | |||
99 | } | 116 | } |
100 | } | 117 | } |
101 | break; | 118 | break; |
119 | case ConstraintType.D6_SPRING_CONSTRAINT_TYPE: | ||
120 | BSConstraintSpring constrainSpring = constrain as BSConstraintSpring; | ||
121 | if (constrainSpring != null) | ||
122 | { | ||
123 | // zero linear and angular limits makes the objects unable to move in relation to each other | ||
124 | constrainSpring.SetLinearLimits(linearLimitLow, linearLimitHigh); | ||
125 | constrainSpring.SetAngularLimits(angularLimitLow, angularLimitHigh); | ||
126 | |||
127 | // tweek the constraint to increase stability | ||
128 | constrainSpring.UseFrameOffset(useFrameOffset); | ||
129 | constrainSpring.TranslationalLimitMotor(enableTransMotor, transMotorMaxVel, transMotorMaxForce); | ||
130 | constrainSpring.SetCFMAndERP(cfm, erp); | ||
131 | if (solverIterations != 0f) | ||
132 | { | ||
133 | constrainSpring.SetSolverIterations(solverIterations); | ||
134 | } | ||
135 | for (int ii = 0; ii < 6; ii++) | ||
136 | { | ||
137 | if (springDamping != -1) | ||
138 | constrainSpring.SetDamping(ii, springDamping); | ||
139 | if (springStiffness != -1) | ||
140 | constrainSpring.SetStiffness(ii, springStiffness); | ||
141 | } | ||
142 | } | ||
143 | break; | ||
102 | default: | 144 | default: |
103 | break; | 145 | break; |
104 | } | 146 | } |
@@ -262,8 +304,8 @@ public sealed class BSLinksetConstraints : BSLinkset | |||
262 | // Create a static constraint between the two passed objects | 304 | // Create a static constraint between the two passed objects |
263 | private BSConstraint BuildConstraint(BSPrimLinkable rootPrim, BSLinkInfo li) | 305 | private BSConstraint BuildConstraint(BSPrimLinkable rootPrim, BSLinkInfo li) |
264 | { | 306 | { |
265 | BSLinkInfoConstraint liConstraint = li as BSLinkInfoConstraint; | 307 | BSLinkInfoConstraint linkInfo = li as BSLinkInfoConstraint; |
266 | if (liConstraint == null) | 308 | if (linkInfo == null) |
267 | return null; | 309 | return null; |
268 | 310 | ||
269 | // Zero motion for children so they don't interpolate | 311 | // Zero motion for children so they don't interpolate |
@@ -271,27 +313,25 @@ public sealed class BSLinksetConstraints : BSLinkset | |||
271 | 313 | ||
272 | BSConstraint constrain = null; | 314 | BSConstraint constrain = null; |
273 | 315 | ||
274 | switch (liConstraint.constraintType) | 316 | switch (linkInfo.constraintType) |
275 | { | 317 | { |
276 | case ConstraintType.D6_CONSTRAINT_TYPE: | 318 | case ConstraintType.D6_CONSTRAINT_TYPE: |
277 | // Relative position normalized to the root prim | 319 | // Relative position normalized to the root prim |
278 | // Essentually a vector pointing from center of rootPrim to center of li.member | 320 | // Essentually a vector pointing from center of rootPrim to center of li.member |
279 | OMV.Vector3 childRelativePosition = liConstraint.member.Position - rootPrim.Position; | 321 | OMV.Vector3 childRelativePosition = linkInfo.member.Position - rootPrim.Position; |
280 | 322 | ||
281 | // real world coordinate of midpoint between the two objects | 323 | // real world coordinate of midpoint between the two objects |
282 | OMV.Vector3 midPoint = rootPrim.Position + (childRelativePosition / 2); | 324 | OMV.Vector3 midPoint = rootPrim.Position + (childRelativePosition / 2); |
283 | 325 | ||
284 | DetailLog("{0},BSLinksetConstraint.BuildConstraint,taint,root={1},rBody={2},child={3},cBody={4},rLoc={5},cLoc={6},midLoc={7}", | 326 | DetailLog("{0},BSLinksetConstraint.BuildConstraint,6Dof,rBody={1},cBody={2},rLoc={3},cLoc={4},midLoc={7}", |
285 | rootPrim.LocalID, | 327 | rootPrim.LocalID, rootPrim.PhysBody, linkInfo.member.PhysBody, |
286 | rootPrim.LocalID, rootPrim.PhysBody.AddrString, | 328 | rootPrim.Position, linkInfo.member.Position, midPoint); |
287 | liConstraint.member.LocalID, liConstraint.member.PhysBody.AddrString, | ||
288 | rootPrim.Position, liConstraint.member.Position, midPoint); | ||
289 | 329 | ||
290 | // create a constraint that allows no freedom of movement between the two objects | 330 | // create a constraint that allows no freedom of movement between the two objects |
291 | // http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?t=4818 | 331 | // http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?t=4818 |
292 | 332 | ||
293 | constrain = new BSConstraint6Dof( | 333 | constrain = new BSConstraint6Dof( |
294 | m_physicsScene.World, rootPrim.PhysBody, liConstraint.member.PhysBody, midPoint, true, true ); | 334 | m_physicsScene.World, rootPrim.PhysBody, linkInfo.member.PhysBody, midPoint, true, true ); |
295 | 335 | ||
296 | /* NOTE: below is an attempt to build constraint with full frame computation, etc. | 336 | /* NOTE: below is an attempt to build constraint with full frame computation, etc. |
297 | * Using the midpoint is easier since it lets the Bullet code manipulate the transforms | 337 | * Using the midpoint is easier since it lets the Bullet code manipulate the transforms |
@@ -320,11 +360,23 @@ public sealed class BSLinksetConstraints : BSLinkset | |||
320 | */ | 360 | */ |
321 | 361 | ||
322 | break; | 362 | break; |
363 | case ConstraintType.D6_SPRING_CONSTRAINT_TYPE: | ||
364 | constrain = new BSConstraintSpring(m_physicsScene.World, rootPrim.PhysBody, linkInfo.member.PhysBody, | ||
365 | linkInfo.frameInAloc, linkInfo.frameInArot, linkInfo.frameInBloc, linkInfo.frameInBrot, | ||
366 | true /*useLinearReferenceFrameA*/, | ||
367 | true /*disableCollisionsBetweenLinkedBodies*/); | ||
368 | DetailLog("{0},BSLinksetConstraint.BuildConstraint,spring,root={1},rBody={2},child={3},cBody={4},rLoc={5},cLoc={6},midLoc={7}", | ||
369 | rootPrim.LocalID, | ||
370 | rootPrim.LocalID, rootPrim.PhysBody.AddrString, | ||
371 | linkInfo.member.LocalID, linkInfo.member.PhysBody.AddrString, | ||
372 | rootPrim.Position, linkInfo.member.Position); | ||
373 | |||
374 | break; | ||
323 | default: | 375 | default: |
324 | break; | 376 | break; |
325 | } | 377 | } |
326 | 378 | ||
327 | liConstraint.SetConstraintParameters(constrain); | 379 | linkInfo.SetLinkParameters(constrain); |
328 | 380 | ||
329 | m_physicsScene.Constraints.AddConstraint(constrain); | 381 | m_physicsScene.Constraints.AddConstraint(constrain); |
330 | 382 | ||
@@ -401,6 +453,7 @@ public sealed class BSLinksetConstraints : BSLinkset | |||
401 | // If constraint doesn't exist yet, create it. | 453 | // If constraint doesn't exist yet, create it. |
402 | constrain = BuildConstraint(LinksetRoot, li); | 454 | constrain = BuildConstraint(LinksetRoot, li); |
403 | } | 455 | } |
456 | li.SetLinkParameters(constrain); | ||
404 | constrain.RecomputeConstraintVariables(linksetMass); | 457 | constrain.RecomputeConstraintVariables(linksetMass); |
405 | 458 | ||
406 | // PhysicsScene.PE.DumpConstraint(PhysicsScene.World, constrain.Constraint); // DEBUG DEBUG | 459 | // PhysicsScene.PE.DumpConstraint(PhysicsScene.World, constrain.Constraint); // DEBUG DEBUG |