aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs
diff options
context:
space:
mode:
Diffstat (limited to 'OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs')
-rwxr-xr-xOpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs206
1 files changed, 139 insertions, 67 deletions
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs
index f17d698..4bac222 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs
@@ -36,8 +36,78 @@ 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 {
110 LinksetImpl = LinksetImplementation.Constraint;
41 } 111 }
42 112
43 // When physical properties are changed the linkset needs to recalculate 113 // When physical properties are changed the linkset needs to recalculate
@@ -142,7 +212,7 @@ public sealed class BSLinksetConstraints : BSLinkset
142 { 212 {
143 if (!HasChild(child)) 213 if (!HasChild(child))
144 { 214 {
145 m_children.Add(child); 215 m_children.Add(child, new BSLinkInfoConstraint(child));
146 216
147 DetailLog("{0},BSLinksetConstraints.AddChildToLinkset,call,child={1}", LinksetRoot.LocalID, child.LocalID); 217 DetailLog("{0},BSLinksetConstraints.AddChildToLinkset,call,child={1}", LinksetRoot.LocalID, child.LocalID);
148 218
@@ -190,73 +260,74 @@ public sealed class BSLinksetConstraints : BSLinkset
190 } 260 }
191 261
192 // Create a static constraint between the two passed objects 262 // Create a static constraint between the two passed objects
193 private BSConstraint BuildConstraint(BSPrimLinkable rootPrim, BSPrimLinkable childPrim) 263 private BSConstraint BuildConstraint(BSPrimLinkable rootPrim, BSLinkInfo li)
194 { 264 {
265 BSLinkInfoConstraint liConstraint = li as BSLinkInfoConstraint;
266 if (liConstraint == null)
267 return null;
268
195 // Zero motion for children so they don't interpolate 269 // Zero motion for children so they don't interpolate
196 childPrim.ZeroMotion(true); 270 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 271
244 m_physicsScene.Constraints.AddConstraint(constrain); 272 BSConstraint constrain = null;
245 273
246 // zero linear and angular limits makes the objects unable to move in relation to each other 274 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 { 275 {
258 constrain.SetSolverIterations(BSParam.LinkConstraintSolverIterations); 276 case ConstraintType.D6_CONSTRAINT_TYPE:
277 // Relative position normalized to the root prim
278 // Essentually a vector pointing from center of rootPrim to center of li.member
279 OMV.Vector3 childRelativePosition = liConstraint.member.Position - rootPrim.Position;
280
281 // real world coordinate of midpoint between the two objects
282 OMV.Vector3 midPoint = rootPrim.Position + (childRelativePosition / 2);
283
284 DetailLog("{0},BSLinksetConstraint.BuildConstraint,taint,root={1},rBody={2},child={3},cBody={4},rLoc={5},cLoc={6},midLoc={7}",
285 rootPrim.LocalID,
286 rootPrim.LocalID, rootPrim.PhysBody.AddrString,
287 liConstraint.member.LocalID, liConstraint.member.PhysBody.AddrString,
288 rootPrim.Position, liConstraint.member.Position, midPoint);
289
290 // create a constraint that allows no freedom of movement between the two objects
291 // http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?t=4818
292
293 constrain = new BSConstraint6Dof(
294 m_physicsScene.World, rootPrim.PhysBody, liConstraint.member.PhysBody, midPoint, true, true );
295
296 /* 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
298 * of the objects.
299 * Code left for future programmers.
300 // ==================================================================================
301 // relative position normalized to the root prim
302 OMV.Quaternion invThisOrientation = OMV.Quaternion.Inverse(rootPrim.Orientation);
303 OMV.Vector3 childRelativePosition = (liConstraint.member.Position - rootPrim.Position) * invThisOrientation;
304
305 // relative rotation of the child to the parent
306 OMV.Quaternion childRelativeRotation = invThisOrientation * liConstraint.member.Orientation;
307 OMV.Quaternion inverseChildRelativeRotation = OMV.Quaternion.Inverse(childRelativeRotation);
308
309 DetailLog("{0},BSLinksetConstraint.PhysicallyLinkAChildToRoot,taint,root={1},child={2}", rootPrim.LocalID, rootPrim.LocalID, liConstraint.member.LocalID);
310 constrain = new BS6DofConstraint(
311 PhysicsScene.World, rootPrim.Body, liConstraint.member.Body,
312 OMV.Vector3.Zero,
313 OMV.Quaternion.Inverse(rootPrim.Orientation),
314 OMV.Vector3.Zero,
315 OMV.Quaternion.Inverse(liConstraint.member.Orientation),
316 true,
317 true
318 );
319 // ==================================================================================
320 */
321
322 break;
323 default:
324 break;
259 } 325 }
326
327 liConstraint.SetConstraintParameters(constrain);
328
329 m_physicsScene.Constraints.AddConstraint(constrain);
330
260 return constrain; 331 return constrain;
261 } 332 }
262 333
@@ -317,23 +388,24 @@ public sealed class BSLinksetConstraints : BSLinkset
317 return; // Note the 'finally' clause at the botton which will get executed. 388 return; // Note the 'finally' clause at the botton which will get executed.
318 } 389 }
319 390
320 foreach (BSPrimLinkable child in m_children) 391 ForEachLinkInfo((li) =>
321 { 392 {
322 // A child in the linkset physically shows the mass of the whole linkset. 393 // 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. 394 // 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.) 395 // (Also do the mass stuff before recomputing the constraint so mass is not zero.)
325 child.UpdatePhysicalMassProperties(linksetMass, true); 396 li.member.UpdatePhysicalMassProperties(linksetMass, true);
326 397
327 BSConstraint constrain; 398 BSConstraint constrain;
328 if (!m_physicsScene.Constraints.TryGetConstraint(LinksetRoot.PhysBody, child.PhysBody, out constrain)) 399 if (!m_physicsScene.Constraints.TryGetConstraint(LinksetRoot.PhysBody, li.member.PhysBody, out constrain))
329 { 400 {
330 // If constraint doesn't exist yet, create it. 401 // If constraint doesn't exist yet, create it.
331 constrain = BuildConstraint(LinksetRoot, child); 402 constrain = BuildConstraint(LinksetRoot, li);
332 } 403 }
333 constrain.RecomputeConstraintVariables(linksetMass); 404 constrain.RecomputeConstraintVariables(linksetMass);
334 405
335 // PhysicsScene.PE.DumpConstraint(PhysicsScene.World, constrain.Constraint); // DEBUG DEBUG 406 // PhysicsScene.PE.DumpConstraint(PhysicsScene.World, constrain.Constraint); // DEBUG DEBUG
336 } 407 return false; // 'false' says to keep processing other members
408 });
337 } 409 }
338 finally 410 finally
339 { 411 {