aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs
diff options
context:
space:
mode:
authorRobert Adams2013-08-01 12:35:22 -0700
committerRobert Adams2013-08-02 09:47:11 -0700
commit5bcccfc305ae4f5a74b9b816781a2a643daa23b3 (patch)
treeab852db97111577d07c0d8b7240ca153921d1545 /OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs
parentFix problem with modInvoke defined integer constants being build into (diff)
downloadopensim-SC_OLD-5bcccfc305ae4f5a74b9b816781a2a643daa23b3.zip
opensim-SC_OLD-5bcccfc305ae4f5a74b9b816781a2a643daa23b3.tar.gz
opensim-SC_OLD-5bcccfc305ae4f5a74b9b816781a2a643daa23b3.tar.bz2
opensim-SC_OLD-5bcccfc305ae4f5a74b9b816781a2a643daa23b3.tar.xz
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.
Diffstat (limited to 'OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs')
-rwxr-xr-xOpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs205
1 files changed, 138 insertions, 67 deletions
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
36{ 36{
37 // private static string LogHeader = "[BULLETSIM LINKSET CONSTRAINTS]"; 37 // private static string LogHeader = "[BULLETSIM LINKSET CONSTRAINTS]";
38 38
39 public class BSLinkInfoConstraint : BSLinkInfo
40 {
41 public ConstraintType constraintType;
42 public BSConstraint constraint;
43 public OMV.Vector3 linearLimitLow;
44 public OMV.Vector3 linearLimitHigh;
45 public OMV.Vector3 angularLimitLow;
46 public OMV.Vector3 angularLimitHigh;
47 public bool useFrameOffset;
48 public bool enableTransMotor;
49 public float transMotorMaxVel;
50 public float transMotorMaxForce;
51 public float cfm;
52 public float erp;
53 public float solverIterations;
54
55 public BSLinkInfoConstraint(BSPrimLinkable pMember)
56 : base(pMember)
57 {
58 constraint = null;
59 ResetToFixedConstraint();
60 }
61
62 // Set all the parameters for this constraint to a fixed, non-movable constraint.
63 public void ResetToFixedConstraint()
64 {
65 constraintType = ConstraintType.D6_CONSTRAINT_TYPE;
66 linearLimitLow = OMV.Vector3.Zero;
67 linearLimitHigh = OMV.Vector3.Zero;
68 angularLimitLow = OMV.Vector3.Zero;
69 angularLimitHigh = OMV.Vector3.Zero;
70 useFrameOffset = BSParam.LinkConstraintUseFrameOffset;
71 enableTransMotor = BSParam.LinkConstraintEnableTransMotor;
72 transMotorMaxVel = BSParam.LinkConstraintTransMotorMaxVel;
73 transMotorMaxForce = BSParam.LinkConstraintTransMotorMaxForce;
74 cfm = BSParam.LinkConstraintCFM;
75 erp = BSParam.LinkConstraintERP;
76 solverIterations = BSParam.LinkConstraintSolverIterations;
77 }
78
79 // Given a constraint, apply the current constraint parameters to same.
80 public void SetConstraintParameters(BSConstraint constrain)
81 {
82 switch (constraintType)
83 {
84 case ConstraintType.D6_CONSTRAINT_TYPE:
85 BSConstraint6Dof constrain6dof = constrain as BSConstraint6Dof;
86 if (constrain6dof != null)
87 {
88 // zero linear and angular limits makes the objects unable to move in relation to each other
89 constrain6dof.SetLinearLimits(linearLimitLow, linearLimitHigh);
90 constrain6dof.SetAngularLimits(angularLimitLow, angularLimitHigh);
91
92 // tweek the constraint to increase stability
93 constrain6dof.UseFrameOffset(useFrameOffset);
94 constrain6dof.TranslationalLimitMotor(enableTransMotor, transMotorMaxVel, transMotorMaxForce);
95 constrain6dof.SetCFMAndERP(cfm, erp);
96 if (solverIterations != 0f)
97 {
98 constrain6dof.SetSolverIterations(solverIterations);
99 }
100 }
101 break;
102 default:
103 break;
104 }
105 }
106 }
107
39 public BSLinksetConstraints(BSScene scene, BSPrimLinkable parent) : base(scene, parent) 108 public BSLinksetConstraints(BSScene scene, BSPrimLinkable parent) : base(scene, parent)
40 { 109 {
41 } 110 }
@@ -142,7 +211,7 @@ public sealed class BSLinksetConstraints : BSLinkset
142 { 211 {
143 if (!HasChild(child)) 212 if (!HasChild(child))
144 { 213 {
145 m_children.Add(child); 214 m_children.Add(child, new BSLinkInfoConstraint(child));
146 215
147 DetailLog("{0},BSLinksetConstraints.AddChildToLinkset,call,child={1}", LinksetRoot.LocalID, child.LocalID); 216 DetailLog("{0},BSLinksetConstraints.AddChildToLinkset,call,child={1}", LinksetRoot.LocalID, child.LocalID);
148 217
@@ -190,73 +259,74 @@ public sealed class BSLinksetConstraints : BSLinkset
190 } 259 }
191 260
192 // Create a static constraint between the two passed objects 261 // Create a static constraint between the two passed objects
193 private BSConstraint BuildConstraint(BSPrimLinkable rootPrim, BSPrimLinkable childPrim) 262 private BSConstraint BuildConstraint(BSPrimLinkable rootPrim, BSLinkInfo li)
194 { 263 {
264 BSLinkInfoConstraint liConstraint = li as BSLinkInfoConstraint;
265 if (liConstraint == null)
266 return null;
267
195 // Zero motion for children so they don't interpolate 268 // Zero motion for children so they don't interpolate
196 childPrim.ZeroMotion(true); 269 li.member.ZeroMotion(true);
197
198 // Relative position normalized to the root prim
199 // Essentually a vector pointing from center of rootPrim to center of childPrim
200 OMV.Vector3 childRelativePosition = childPrim.Position - rootPrim.Position;
201
202 // real world coordinate of midpoint between the two objects
203 OMV.Vector3 midPoint = rootPrim.Position + (childRelativePosition / 2);
204
205 DetailLog("{0},BSLinksetConstraint.BuildConstraint,taint,root={1},rBody={2},child={3},cBody={4},rLoc={5},cLoc={6},midLoc={7}",
206 rootPrim.LocalID,
207 rootPrim.LocalID, rootPrim.PhysBody.AddrString,
208 childPrim.LocalID, childPrim.PhysBody.AddrString,
209 rootPrim.Position, childPrim.Position, midPoint);
210
211 // create a constraint that allows no freedom of movement between the two objects
212 // http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?t=4818
213
214 BSConstraint6Dof constrain = new BSConstraint6Dof(
215 m_physicsScene.World, rootPrim.PhysBody, childPrim.PhysBody, midPoint, true, true );
216 // PhysicsScene.World, childPrim.BSBody, rootPrim.BSBody, midPoint, true, true );
217
218 /* NOTE: below is an attempt to build constraint with full frame computation, etc.
219 * Using the midpoint is easier since it lets the Bullet code manipulate the transforms
220 * of the objects.
221 * Code left for future programmers.
222 // ==================================================================================
223 // relative position normalized to the root prim
224 OMV.Quaternion invThisOrientation = OMV.Quaternion.Inverse(rootPrim.Orientation);
225 OMV.Vector3 childRelativePosition = (childPrim.Position - rootPrim.Position) * invThisOrientation;
226
227 // relative rotation of the child to the parent
228 OMV.Quaternion childRelativeRotation = invThisOrientation * childPrim.Orientation;
229 OMV.Quaternion inverseChildRelativeRotation = OMV.Quaternion.Inverse(childRelativeRotation);
230
231 DetailLog("{0},BSLinksetConstraint.PhysicallyLinkAChildToRoot,taint,root={1},child={2}", rootPrim.LocalID, rootPrim.LocalID, childPrim.LocalID);
232 BS6DofConstraint constrain = new BS6DofConstraint(
233 PhysicsScene.World, rootPrim.Body, childPrim.Body,
234 OMV.Vector3.Zero,
235 OMV.Quaternion.Inverse(rootPrim.Orientation),
236 OMV.Vector3.Zero,
237 OMV.Quaternion.Inverse(childPrim.Orientation),
238 true,
239 true
240 );
241 // ==================================================================================
242 */
243 270
244 m_physicsScene.Constraints.AddConstraint(constrain); 271 BSConstraint constrain = null;
245 272
246 // zero linear and angular limits makes the objects unable to move in relation to each other 273 switch (liConstraint.constraintType)
247 constrain.SetLinearLimits(OMV.Vector3.Zero, OMV.Vector3.Zero);
248 constrain.SetAngularLimits(OMV.Vector3.Zero, OMV.Vector3.Zero);
249
250 // tweek the constraint to increase stability
251 constrain.UseFrameOffset(BSParam.LinkConstraintUseFrameOffset);
252 constrain.TranslationalLimitMotor(BSParam.LinkConstraintEnableTransMotor,
253 BSParam.LinkConstraintTransMotorMaxVel,
254 BSParam.LinkConstraintTransMotorMaxForce);
255 constrain.SetCFMAndERP(BSParam.LinkConstraintCFM, BSParam.LinkConstraintERP);
256 if (BSParam.LinkConstraintSolverIterations != 0f)
257 { 274 {
258 constrain.SetSolverIterations(BSParam.LinkConstraintSolverIterations); 275 case ConstraintType.D6_CONSTRAINT_TYPE:
276 // Relative position normalized to the root prim
277 // Essentually a vector pointing from center of rootPrim to center of li.member
278 OMV.Vector3 childRelativePosition = liConstraint.member.Position - rootPrim.Position;
279
280 // real world coordinate of midpoint between the two objects
281 OMV.Vector3 midPoint = rootPrim.Position + (childRelativePosition / 2);
282
283 DetailLog("{0},BSLinksetConstraint.BuildConstraint,taint,root={1},rBody={2},child={3},cBody={4},rLoc={5},cLoc={6},midLoc={7}",
284 rootPrim.LocalID,
285 rootPrim.LocalID, rootPrim.PhysBody.AddrString,
286 liConstraint.member.LocalID, liConstraint.member.PhysBody.AddrString,
287 rootPrim.Position, liConstraint.member.Position, midPoint);
288
289 // create a constraint that allows no freedom of movement between the two objects
290 // http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?t=4818
291
292 constrain = new BSConstraint6Dof(
293 m_physicsScene.World, rootPrim.PhysBody, liConstraint.member.PhysBody, midPoint, true, true );
294
295 /* NOTE: below is an attempt to build constraint with full frame computation, etc.
296 * Using the midpoint is easier since it lets the Bullet code manipulate the transforms
297 * of the objects.
298 * Code left for future programmers.
299 // ==================================================================================
300 // relative position normalized to the root prim
301 OMV.Quaternion invThisOrientation = OMV.Quaternion.Inverse(rootPrim.Orientation);
302 OMV.Vector3 childRelativePosition = (liConstraint.member.Position - rootPrim.Position) * invThisOrientation;
303
304 // relative rotation of the child to the parent
305 OMV.Quaternion childRelativeRotation = invThisOrientation * liConstraint.member.Orientation;
306 OMV.Quaternion inverseChildRelativeRotation = OMV.Quaternion.Inverse(childRelativeRotation);
307
308 DetailLog("{0},BSLinksetConstraint.PhysicallyLinkAChildToRoot,taint,root={1},child={2}", rootPrim.LocalID, rootPrim.LocalID, liConstraint.member.LocalID);
309 constrain = new BS6DofConstraint(
310 PhysicsScene.World, rootPrim.Body, liConstraint.member.Body,
311 OMV.Vector3.Zero,
312 OMV.Quaternion.Inverse(rootPrim.Orientation),
313 OMV.Vector3.Zero,
314 OMV.Quaternion.Inverse(liConstraint.member.Orientation),
315 true,
316 true
317 );
318 // ==================================================================================
319 */
320
321 break;
322 default:
323 break;
259 } 324 }
325
326 liConstraint.SetConstraintParameters(constrain);
327
328 m_physicsScene.Constraints.AddConstraint(constrain);
329
260 return constrain; 330 return constrain;
261 } 331 }
262 332
@@ -317,23 +387,24 @@ public sealed class BSLinksetConstraints : BSLinkset
317 return; // Note the 'finally' clause at the botton which will get executed. 387 return; // Note the 'finally' clause at the botton which will get executed.
318 } 388 }
319 389
320 foreach (BSPrimLinkable child in m_children) 390 ForEachLinkInfo((li) =>
321 { 391 {
322 // A child in the linkset physically shows the mass of the whole linkset. 392 // A child in the linkset physically shows the mass of the whole linkset.
323 // This allows Bullet to apply enough force on the child to move the whole linkset. 393 // This allows Bullet to apply enough force on the child to move the whole linkset.
324 // (Also do the mass stuff before recomputing the constraint so mass is not zero.) 394 // (Also do the mass stuff before recomputing the constraint so mass is not zero.)
325 child.UpdatePhysicalMassProperties(linksetMass, true); 395 li.member.UpdatePhysicalMassProperties(linksetMass, true);
326 396
327 BSConstraint constrain; 397 BSConstraint constrain;
328 if (!m_physicsScene.Constraints.TryGetConstraint(LinksetRoot.PhysBody, child.PhysBody, out constrain)) 398 if (!m_physicsScene.Constraints.TryGetConstraint(LinksetRoot.PhysBody, li.member.PhysBody, out constrain))
329 { 399 {
330 // If constraint doesn't exist yet, create it. 400 // If constraint doesn't exist yet, create it.
331 constrain = BuildConstraint(LinksetRoot, child); 401 constrain = BuildConstraint(LinksetRoot, li);
332 } 402 }
333 constrain.RecomputeConstraintVariables(linksetMass); 403 constrain.RecomputeConstraintVariables(linksetMass);
334 404
335 // PhysicsScene.PE.DumpConstraint(PhysicsScene.World, constrain.Constraint); // DEBUG DEBUG 405 // PhysicsScene.PE.DumpConstraint(PhysicsScene.World, constrain.Constraint); // DEBUG DEBUG
336 } 406 return false; // 'false' says to keep processing other members
407 });
337 } 408 }
338 finally 409 finally
339 { 410 {