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.cs730
1 files changed, 636 insertions, 94 deletions
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs
index 6d252ca..aaf92c8 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs
@@ -1,4 +1,4 @@
1/* 1/*
2 * Copyright (c) Contributors, http://opensimulator.org/ 2 * Copyright (c) Contributors, http://opensimulator.org/
3 * See CONTRIBUTORS.TXT for a full list of copyright holders. 3 * See CONTRIBUTORS.TXT for a full list of copyright holders.
4 * 4 *
@@ -28,6 +28,8 @@ using System;
28using System.Collections.Generic; 28using System.Collections.Generic;
29using System.Text; 29using System.Text;
30 30
31using OpenSim.Region.OptionalModules.Scripting;
32
31using OMV = OpenMetaverse; 33using OMV = OpenMetaverse;
32 34
33namespace OpenSim.Region.Physics.BulletSPlugin 35namespace OpenSim.Region.Physics.BulletSPlugin
@@ -36,26 +38,194 @@ public sealed class BSLinksetConstraints : BSLinkset
36{ 38{
37 // private static string LogHeader = "[BULLETSIM LINKSET CONSTRAINTS]"; 39 // private static string LogHeader = "[BULLETSIM LINKSET CONSTRAINTS]";
38 40
41 public class BSLinkInfoConstraint : BSLinkInfo
42 {
43 public ConstraintType constraintType;
44 public BSConstraint constraint;
45 public OMV.Vector3 linearLimitLow;
46 public OMV.Vector3 linearLimitHigh;
47 public OMV.Vector3 angularLimitLow;
48 public OMV.Vector3 angularLimitHigh;
49 public bool useFrameOffset;
50 public bool enableTransMotor;
51 public float transMotorMaxVel;
52 public float transMotorMaxForce;
53 public float cfm;
54 public float erp;
55 public float solverIterations;
56 //
57 public OMV.Vector3 frameInAloc;
58 public OMV.Quaternion frameInArot;
59 public OMV.Vector3 frameInBloc;
60 public OMV.Quaternion frameInBrot;
61 public bool useLinearReferenceFrameA;
62 // Spring
63 public bool[] springAxisEnable;
64 public float[] springDamping;
65 public float[] springStiffness;
66 public OMV.Vector3 springLinearEquilibriumPoint;
67 public OMV.Vector3 springAngularEquilibriumPoint;
68
69 public BSLinkInfoConstraint(BSPrimLinkable pMember)
70 : base(pMember)
71 {
72 constraint = null;
73 ResetLink();
74 member.PhysScene.DetailLog("{0},BSLinkInfoConstraint.creation", member.LocalID);
75 }
76
77 // Set all the parameters for this constraint to a fixed, non-movable constraint.
78 public override void ResetLink()
79 {
80 // constraintType = ConstraintType.D6_CONSTRAINT_TYPE;
81 constraintType = ConstraintType.FIXED_CONSTRAINT_TYPE;
82 linearLimitLow = OMV.Vector3.Zero;
83 linearLimitHigh = OMV.Vector3.Zero;
84 angularLimitLow = OMV.Vector3.Zero;
85 angularLimitHigh = OMV.Vector3.Zero;
86 useFrameOffset = BSParam.LinkConstraintUseFrameOffset;
87 enableTransMotor = BSParam.LinkConstraintEnableTransMotor;
88 transMotorMaxVel = BSParam.LinkConstraintTransMotorMaxVel;
89 transMotorMaxForce = BSParam.LinkConstraintTransMotorMaxForce;
90 cfm = BSParam.LinkConstraintCFM;
91 erp = BSParam.LinkConstraintERP;
92 solverIterations = BSParam.LinkConstraintSolverIterations;
93 frameInAloc = OMV.Vector3.Zero;
94 frameInArot = OMV.Quaternion.Identity;
95 frameInBloc = OMV.Vector3.Zero;
96 frameInBrot = OMV.Quaternion.Identity;
97 useLinearReferenceFrameA = true;
98 springAxisEnable = new bool[6];
99 springDamping = new float[6];
100 springStiffness = new float[6];
101 for (int ii = 0; ii < springAxisEnable.Length; ii++)
102 {
103 springAxisEnable[ii] = false;
104 springDamping[ii] = BSAPITemplate.SPRING_NOT_SPECIFIED;
105 springStiffness[ii] = BSAPITemplate.SPRING_NOT_SPECIFIED;
106 }
107 springLinearEquilibriumPoint = OMV.Vector3.Zero;
108 springAngularEquilibriumPoint = OMV.Vector3.Zero;
109 member.PhysScene.DetailLog("{0},BSLinkInfoConstraint.ResetLink", member.LocalID);
110 }
111
112 // Given a constraint, apply the current constraint parameters to same.
113 public override void SetLinkParameters(BSConstraint constrain)
114 {
115 member.PhysScene.DetailLog("{0},BSLinkInfoConstraint.SetLinkParameters,type={1}", member.LocalID, constraintType);
116 switch (constraintType)
117 {
118 case ConstraintType.FIXED_CONSTRAINT_TYPE:
119 case ConstraintType.D6_CONSTRAINT_TYPE:
120 BSConstraint6Dof constrain6dof = constrain as BSConstraint6Dof;
121 if (constrain6dof != null)
122 {
123 // NOTE: D6_SPRING_CONSTRAINT_TYPE should be updated if you change any of this code.
124 // zero linear and angular limits makes the objects unable to move in relation to each other
125 constrain6dof.SetLinearLimits(linearLimitLow, linearLimitHigh);
126 constrain6dof.SetAngularLimits(angularLimitLow, angularLimitHigh);
127
128 // tweek the constraint to increase stability
129 constrain6dof.UseFrameOffset(useFrameOffset);
130 constrain6dof.TranslationalLimitMotor(enableTransMotor, transMotorMaxVel, transMotorMaxForce);
131 constrain6dof.SetCFMAndERP(cfm, erp);
132 if (solverIterations != 0f)
133 {
134 constrain6dof.SetSolverIterations(solverIterations);
135 }
136 }
137 break;
138 case ConstraintType.D6_SPRING_CONSTRAINT_TYPE:
139 BSConstraintSpring constrainSpring = constrain as BSConstraintSpring;
140 if (constrainSpring != null)
141 {
142 // zero linear and angular limits makes the objects unable to move in relation to each other
143 constrainSpring.SetLinearLimits(linearLimitLow, linearLimitHigh);
144 constrainSpring.SetAngularLimits(angularLimitLow, angularLimitHigh);
145
146 // tweek the constraint to increase stability
147 constrainSpring.UseFrameOffset(useFrameOffset);
148 constrainSpring.TranslationalLimitMotor(enableTransMotor, transMotorMaxVel, transMotorMaxForce);
149 constrainSpring.SetCFMAndERP(cfm, erp);
150 if (solverIterations != 0f)
151 {
152 constrainSpring.SetSolverIterations(solverIterations);
153 }
154 for (int ii = 0; ii < springAxisEnable.Length; ii++)
155 {
156 constrainSpring.SetAxisEnable(ii, springAxisEnable[ii]);
157 if (springDamping[ii] != BSAPITemplate.SPRING_NOT_SPECIFIED)
158 constrainSpring.SetDamping(ii, springDamping[ii]);
159 if (springStiffness[ii] != BSAPITemplate.SPRING_NOT_SPECIFIED)
160 constrainSpring.SetStiffness(ii, springStiffness[ii]);
161 }
162 constrainSpring.CalculateTransforms();
163
164 if (springLinearEquilibriumPoint != OMV.Vector3.Zero)
165 constrainSpring.SetEquilibriumPoint(springLinearEquilibriumPoint, springAngularEquilibriumPoint);
166 else
167 constrainSpring.SetEquilibriumPoint(BSAPITemplate.SPRING_NOT_SPECIFIED, BSAPITemplate.SPRING_NOT_SPECIFIED);
168 }
169 break;
170 default:
171 break;
172 }
173 }
174
175 // Return 'true' if the property updates from the physics engine should be reported
176 // to the simulator.
177 // If the constraint is fixed, we don't need to report as the simulator and viewer will
178 // report the right things.
179 public override bool ShouldUpdateChildProperties()
180 {
181 bool ret = true;
182 if (constraintType == ConstraintType.FIXED_CONSTRAINT_TYPE)
183 ret = false;
184
185 return ret;
186 }
187 }
188
39 public BSLinksetConstraints(BSScene scene, BSPrimLinkable parent) : base(scene, parent) 189 public BSLinksetConstraints(BSScene scene, BSPrimLinkable parent) : base(scene, parent)
40 { 190 {
191 LinksetImpl = LinksetImplementation.Constraint;
41 } 192 }
42 193
194 private static string LogHeader = "[BULLETSIM LINKSET CONSTRAINT]";
195
43 // When physical properties are changed the linkset needs to recalculate 196 // When physical properties are changed the linkset needs to recalculate
44 // its internal properties. 197 // its internal properties.
45 // This is queued in the 'post taint' queue so the 198 // This is queued in the 'post taint' queue so the
46 // refresh will happen once after all the other taints are applied. 199 // refresh will happen once after all the other taints are applied.
47 public override void Refresh(BSPrimLinkable requestor) 200 public override void Refresh(BSPrimLinkable requestor)
48 { 201 {
202 ScheduleRebuild(requestor);
49 base.Refresh(requestor); 203 base.Refresh(requestor);
50 204
51 if (HasAnyChildren && IsRoot(requestor)) 205 }
206
207 private void ScheduleRebuild(BSPrimLinkable requestor)
208 {
209 DetailLog("{0},BSLinksetConstraint.ScheduleRebuild,,rebuilding={1},hasChildren={2},actuallyScheduling={3}",
210 requestor.LocalID, Rebuilding, HasAnyChildren, (!Rebuilding && HasAnyChildren));
211
212 // When rebuilding, it is possible to set properties that would normally require a rebuild.
213 // If already rebuilding, don't request another rebuild.
214 // If a linkset with just a root prim (simple non-linked prim) don't bother rebuilding.
215 if (!Rebuilding && HasAnyChildren)
52 { 216 {
53 // Queue to happen after all the other taint processing 217 // Queue to happen after all the other taint processing
54 PhysicsScene.PostTaintObject("BSLinksetContraints.Refresh", requestor.LocalID, delegate() 218 m_physicsScene.PostTaintObject("BSLinksetContraints.Refresh", requestor.LocalID, delegate()
219 {
220 if (HasAnyChildren)
55 { 221 {
56 if (HasAnyChildren && IsRoot(requestor)) 222 // Constraints that have not been changed are not rebuild but make sure
57 RecomputeLinksetConstraints(); 223 // the constraint of the requestor is rebuilt.
58 }); 224 PhysicallyUnlinkAChildFromRoot(LinksetRoot, requestor);
225 // Rebuild the linkset and all its constraints.
226 RecomputeLinksetConstraints();
227 }
228 });
59 } 229 }
60 } 230 }
61 231
@@ -67,8 +237,14 @@ public sealed class BSLinksetConstraints : BSLinkset
67 // Called at taint-time! 237 // Called at taint-time!
68 public override bool MakeDynamic(BSPrimLinkable child) 238 public override bool MakeDynamic(BSPrimLinkable child)
69 { 239 {
70 // What is done for each object in BSPrim is what we want. 240 bool ret = false;
71 return false; 241 DetailLog("{0},BSLinksetConstraints.MakeDynamic,call,IsRoot={1}", child.LocalID, IsRoot(child));
242 if (IsRoot(child))
243 {
244 // The root is going dynamic. Rebuild the linkset so parts and mass get computed properly.
245 Refresh(LinksetRoot);
246 }
247 return ret;
72 } 248 }
73 249
74 // The object is going static (non-physical). Do any setup necessary for a static linkset. 250 // The object is going static (non-physical). Do any setup necessary for a static linkset.
@@ -78,8 +254,16 @@ public sealed class BSLinksetConstraints : BSLinkset
78 // Called at taint-time! 254 // Called at taint-time!
79 public override bool MakeStatic(BSPrimLinkable child) 255 public override bool MakeStatic(BSPrimLinkable child)
80 { 256 {
81 // What is done for each object in BSPrim is what we want. 257 bool ret = false;
82 return false; 258
259 DetailLog("{0},BSLinksetConstraint.MakeStatic,call,IsRoot={1}", child.LocalID, IsRoot(child));
260 child.ClearDisplacement();
261 if (IsRoot(child))
262 {
263 // Schedule a rebuild to verify that the root shape is set to the real shape.
264 Refresh(LinksetRoot);
265 }
266 return ret;
83 } 267 }
84 268
85 // Called at taint-time!! 269 // Called at taint-time!!
@@ -93,11 +277,11 @@ public sealed class BSLinksetConstraints : BSLinkset
93 // up to rebuild the constraints before the next simulation step. 277 // up to rebuild the constraints before the next simulation step.
94 // Returns 'true' of something was actually removed and would need restoring 278 // Returns 'true' of something was actually removed and would need restoring
95 // Called at taint-time!! 279 // Called at taint-time!!
96 public override bool RemoveBodyDependencies(BSPrimLinkable child) 280 public override bool RemoveDependencies(BSPrimLinkable child)
97 { 281 {
98 bool ret = false; 282 bool ret = false;
99 283
100 DetailLog("{0},BSLinksetConstraint.RemoveBodyDependencies,removeChildrenForRoot,rID={1},rBody={2}", 284 DetailLog("{0},BSLinksetConstraint.RemoveDependencies,removeChildrenForRoot,rID={1},rBody={2}",
101 child.LocalID, LinksetRoot.LocalID, LinksetRoot.PhysBody.AddrString); 285 child.LocalID, LinksetRoot.LocalID, LinksetRoot.PhysBody.AddrString);
102 286
103 lock (m_linksetActivityLock) 287 lock (m_linksetActivityLock)
@@ -118,7 +302,7 @@ public sealed class BSLinksetConstraints : BSLinkset
118 { 302 {
119 if (!HasChild(child)) 303 if (!HasChild(child))
120 { 304 {
121 m_children.Add(child); 305 m_children.Add(child, new BSLinkInfoConstraint(child));
122 306
123 DetailLog("{0},BSLinksetConstraints.AddChildToLinkset,call,child={1}", LinksetRoot.LocalID, child.LocalID); 307 DetailLog("{0},BSLinksetConstraints.AddChildToLinkset,call,child={1}", LinksetRoot.LocalID, child.LocalID);
124 308
@@ -130,7 +314,7 @@ public sealed class BSLinksetConstraints : BSLinkset
130 314
131 // Remove the specified child from the linkset. 315 // Remove the specified child from the linkset.
132 // Safe to call even if the child is not really in my linkset. 316 // Safe to call even if the child is not really in my linkset.
133 protected override void RemoveChildFromLinkset(BSPrimLinkable child) 317 protected override void RemoveChildFromLinkset(BSPrimLinkable child, bool inTaintTime)
134 { 318 {
135 if (m_children.Remove(child)) 319 if (m_children.Remove(child))
136 { 320 {
@@ -142,7 +326,7 @@ public sealed class BSLinksetConstraints : BSLinkset
142 rootx.LocalID, rootx.PhysBody.AddrString, 326 rootx.LocalID, rootx.PhysBody.AddrString,
143 childx.LocalID, childx.PhysBody.AddrString); 327 childx.LocalID, childx.PhysBody.AddrString);
144 328
145 PhysicsScene.TaintedObject("BSLinksetConstraints.RemoveChildFromLinkset", delegate() 329 m_physicsScene.TaintedObject(inTaintTime, childx.LocalID, "BSLinksetConstraints.RemoveChildFromLinkset", delegate()
146 { 330 {
147 PhysicallyUnlinkAChildFromRoot(rootx, childx); 331 PhysicallyUnlinkAChildFromRoot(rootx, childx);
148 }); 332 });
@@ -165,73 +349,86 @@ public sealed class BSLinksetConstraints : BSLinkset
165 Refresh(rootPrim); 349 Refresh(rootPrim);
166 } 350 }
167 351
168 private BSConstraint BuildConstraint(BSPrimLinkable rootPrim, BSPrimLinkable childPrim) 352 // Create a static constraint between the two passed objects
353 private BSConstraint BuildConstraint(BSPrimLinkable rootPrim, BSLinkInfo li)
169 { 354 {
355 BSLinkInfoConstraint linkInfo = li as BSLinkInfoConstraint;
356 if (linkInfo == null)
357 return null;
358
170 // Zero motion for children so they don't interpolate 359 // Zero motion for children so they don't interpolate
171 childPrim.ZeroMotion(true); 360 li.member.ZeroMotion(true);
172 361
173 // Relative position normalized to the root prim 362 BSConstraint constrain = null;
174 // Essentually a vector pointing from center of rootPrim to center of childPrim 363
175 OMV.Vector3 childRelativePosition = childPrim.Position - rootPrim.Position; 364 switch (linkInfo.constraintType)
176
177 // real world coordinate of midpoint between the two objects
178 OMV.Vector3 midPoint = rootPrim.Position + (childRelativePosition / 2);
179
180 DetailLog("{0},BSLinksetConstraint.BuildConstraint,taint,root={1},rBody={2},child={3},cBody={4},rLoc={5},cLoc={6},midLoc={7}",
181 rootPrim.LocalID,
182 rootPrim.LocalID, rootPrim.PhysBody.AddrString,
183 childPrim.LocalID, childPrim.PhysBody.AddrString,
184 rootPrim.Position, childPrim.Position, midPoint);
185
186 // create a constraint that allows no freedom of movement between the two objects
187 // http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?t=4818
188
189 BSConstraint6Dof constrain = new BSConstraint6Dof(
190 PhysicsScene.World, rootPrim.PhysBody, childPrim.PhysBody, midPoint, true, true );
191 // PhysicsScene.World, childPrim.BSBody, rootPrim.BSBody, midPoint, true, true );
192
193 /* NOTE: below is an attempt to build constraint with full frame computation, etc.
194 * Using the midpoint is easier since it lets the Bullet code manipulate the transforms
195 * of the objects.
196 * Code left for future programmers.
197 // ==================================================================================
198 // relative position normalized to the root prim
199 OMV.Quaternion invThisOrientation = OMV.Quaternion.Inverse(rootPrim.Orientation);
200 OMV.Vector3 childRelativePosition = (childPrim.Position - rootPrim.Position) * invThisOrientation;
201
202 // relative rotation of the child to the parent
203 OMV.Quaternion childRelativeRotation = invThisOrientation * childPrim.Orientation;
204 OMV.Quaternion inverseChildRelativeRotation = OMV.Quaternion.Inverse(childRelativeRotation);
205
206 DetailLog("{0},BSLinksetConstraint.PhysicallyLinkAChildToRoot,taint,root={1},child={2}", rootPrim.LocalID, rootPrim.LocalID, childPrim.LocalID);
207 BS6DofConstraint constrain = new BS6DofConstraint(
208 PhysicsScene.World, rootPrim.Body, childPrim.Body,
209 OMV.Vector3.Zero,
210 OMV.Quaternion.Inverse(rootPrim.Orientation),
211 OMV.Vector3.Zero,
212 OMV.Quaternion.Inverse(childPrim.Orientation),
213 true,
214 true
215 );
216 // ==================================================================================
217 */
218
219 PhysicsScene.Constraints.AddConstraint(constrain);
220
221 // zero linear and angular limits makes the objects unable to move in relation to each other
222 constrain.SetLinearLimits(OMV.Vector3.Zero, OMV.Vector3.Zero);
223 constrain.SetAngularLimits(OMV.Vector3.Zero, OMV.Vector3.Zero);
224
225 // tweek the constraint to increase stability
226 constrain.UseFrameOffset(BSParam.LinkConstraintUseFrameOffset);
227 constrain.TranslationalLimitMotor(BSParam.LinkConstraintEnableTransMotor,
228 BSParam.LinkConstraintTransMotorMaxVel,
229 BSParam.LinkConstraintTransMotorMaxForce);
230 constrain.SetCFMAndERP(BSParam.LinkConstraintCFM, BSParam.LinkConstraintERP);
231 if (BSParam.LinkConstraintSolverIterations != 0f)
232 { 365 {
233 constrain.SetSolverIterations(BSParam.LinkConstraintSolverIterations); 366 case ConstraintType.FIXED_CONSTRAINT_TYPE:
367 case ConstraintType.D6_CONSTRAINT_TYPE:
368 // Relative position normalized to the root prim
369 // Essentually a vector pointing from center of rootPrim to center of li.member
370 OMV.Vector3 childRelativePosition = linkInfo.member.Position - rootPrim.Position;
371
372 // real world coordinate of midpoint between the two objects
373 OMV.Vector3 midPoint = rootPrim.Position + (childRelativePosition / 2);
374
375 DetailLog("{0},BSLinksetConstraint.BuildConstraint,6Dof,rBody={1},cBody={2},rLoc={3},cLoc={4},midLoc={5}",
376 rootPrim.LocalID, rootPrim.PhysBody, linkInfo.member.PhysBody,
377 rootPrim.Position, linkInfo.member.Position, midPoint);
378
379 // create a constraint that allows no freedom of movement between the two objects
380 // http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?t=4818
381
382 constrain = new BSConstraint6Dof(
383 m_physicsScene.World, rootPrim.PhysBody, linkInfo.member.PhysBody, midPoint, true, true );
384
385 /* NOTE: below is an attempt to build constraint with full frame computation, etc.
386 * Using the midpoint is easier since it lets the Bullet code manipulate the transforms
387 * of the objects.
388 * Code left for future programmers.
389 // ==================================================================================
390 // relative position normalized to the root prim
391 OMV.Quaternion invThisOrientation = OMV.Quaternion.Inverse(rootPrim.Orientation);
392 OMV.Vector3 childRelativePosition = (liConstraint.member.Position - rootPrim.Position) * invThisOrientation;
393
394 // relative rotation of the child to the parent
395 OMV.Quaternion childRelativeRotation = invThisOrientation * liConstraint.member.Orientation;
396 OMV.Quaternion inverseChildRelativeRotation = OMV.Quaternion.Inverse(childRelativeRotation);
397
398 DetailLog("{0},BSLinksetConstraint.PhysicallyLinkAChildToRoot,taint,root={1},child={2}", rootPrim.LocalID, rootPrim.LocalID, liConstraint.member.LocalID);
399 constrain = new BS6DofConstraint(
400 PhysicsScene.World, rootPrim.Body, liConstraint.member.Body,
401 OMV.Vector3.Zero,
402 OMV.Quaternion.Inverse(rootPrim.Orientation),
403 OMV.Vector3.Zero,
404 OMV.Quaternion.Inverse(liConstraint.member.Orientation),
405 true,
406 true
407 );
408 // ==================================================================================
409 */
410
411 break;
412 case ConstraintType.D6_SPRING_CONSTRAINT_TYPE:
413 constrain = new BSConstraintSpring(m_physicsScene.World, rootPrim.PhysBody, linkInfo.member.PhysBody,
414 linkInfo.frameInAloc, linkInfo.frameInArot, linkInfo.frameInBloc, linkInfo.frameInBrot,
415 linkInfo.useLinearReferenceFrameA,
416 true /*disableCollisionsBetweenLinkedBodies*/);
417 DetailLog("{0},BSLinksetConstraint.BuildConstraint,spring,root={1},rBody={2},child={3},cBody={4},rLoc={5},cLoc={6}",
418 rootPrim.LocalID,
419 rootPrim.LocalID, rootPrim.PhysBody.AddrString,
420 linkInfo.member.LocalID, linkInfo.member.PhysBody.AddrString,
421 rootPrim.Position, linkInfo.member.Position);
422
423 break;
424 default:
425 break;
234 } 426 }
427
428 linkInfo.SetLinkParameters(constrain);
429
430 m_physicsScene.Constraints.AddConstraint(constrain);
431
235 return constrain; 432 return constrain;
236 } 433 }
237 434
@@ -247,13 +444,22 @@ public sealed class BSLinksetConstraints : BSLinkset
247 rootPrim.LocalID, rootPrim.PhysBody.AddrString, 444 rootPrim.LocalID, rootPrim.PhysBody.AddrString,
248 childPrim.LocalID, childPrim.PhysBody.AddrString); 445 childPrim.LocalID, childPrim.PhysBody.AddrString);
249 446
250 // Find the constraint for this link and get rid of it from the overall collection and from my list 447 // If asked to unlink root from root, just remove all the constraints
251 if (PhysicsScene.Constraints.RemoveAndDestroyConstraint(rootPrim.PhysBody, childPrim.PhysBody)) 448 if (rootPrim == childPrim || childPrim == LinksetRoot)
252 { 449 {
253 // Make the child refresh its location 450 PhysicallyUnlinkAllChildrenFromRoot(LinksetRoot);
254 PhysicsScene.PE.PushUpdate(childPrim.PhysBody);
255 ret = true; 451 ret = true;
256 } 452 }
453 else
454 {
455 // Find the constraint for this link and get rid of it from the overall collection and from my list
456 if (m_physicsScene.Constraints.RemoveAndDestroyConstraint(rootPrim.PhysBody, childPrim.PhysBody))
457 {
458 // Make the child refresh its location
459 m_physicsScene.PE.PushUpdate(childPrim.PhysBody);
460 ret = true;
461 }
462 }
257 463
258 return ret; 464 return ret;
259 } 465 }
@@ -265,7 +471,7 @@ public sealed class BSLinksetConstraints : BSLinkset
265 { 471 {
266 DetailLog("{0},BSLinksetConstraint.PhysicallyUnlinkAllChildren,taint", rootPrim.LocalID); 472 DetailLog("{0},BSLinksetConstraint.PhysicallyUnlinkAllChildren,taint", rootPrim.LocalID);
267 473
268 return PhysicsScene.Constraints.RemoveAndDestroyConstraint(rootPrim.PhysBody); 474 return m_physicsScene.Constraints.RemoveAndDestroyConstraint(rootPrim.PhysBody);
269 } 475 }
270 476
271 // Call each of the constraints that make up this linkset and recompute the 477 // Call each of the constraints that make up this linkset and recompute the
@@ -281,24 +487,360 @@ public sealed class BSLinksetConstraints : BSLinkset
281 DetailLog("{0},BSLinksetConstraint.RecomputeLinksetConstraints,set,rBody={1},linksetMass={2}", 487 DetailLog("{0},BSLinksetConstraint.RecomputeLinksetConstraints,set,rBody={1},linksetMass={2}",
282 LinksetRoot.LocalID, LinksetRoot.PhysBody.AddrString, linksetMass); 488 LinksetRoot.LocalID, LinksetRoot.PhysBody.AddrString, linksetMass);
283 489
284 foreach (BSPrimLinkable child in m_children) 490 try
285 { 491 {
286 // A child in the linkset physically shows the mass of the whole linkset. 492 Rebuilding = true;
287 // This allows Bullet to apply enough force on the child to move the whole linkset.
288 // (Also do the mass stuff before recomputing the constraint so mass is not zero.)
289 child.UpdatePhysicalMassProperties(linksetMass, true);
290 493
291 BSConstraint constrain; 494 // There is no reason to build all this physical stuff for a non-physical linkset.
292 if (!PhysicsScene.Constraints.TryGetConstraint(LinksetRoot.PhysBody, child.PhysBody, out constrain)) 495 if (!LinksetRoot.IsPhysicallyActive || !HasAnyChildren)
293 { 496 {
294 // If constraint doesn't exist yet, create it. 497 DetailLog("{0},BSLinksetConstraint.RecomputeLinksetCompound,notPhysicalOrNoChildren", LinksetRoot.LocalID);
295 constrain = BuildConstraint(LinksetRoot, child); 498 return; // Note the 'finally' clause at the botton which will get executed.
296 } 499 }
297 constrain.RecomputeConstraintVariables(linksetMass);
298 500
299 // PhysicsScene.PE.DumpConstraint(PhysicsScene.World, constrain.Constraint); // DEBUG DEBUG 501 ForEachLinkInfo((li) =>
502 {
503 // A child in the linkset physically shows the mass of the whole linkset.
504 // This allows Bullet to apply enough force on the child to move the whole linkset.
505 // (Also do the mass stuff before recomputing the constraint so mass is not zero.)
506 li.member.UpdatePhysicalMassProperties(linksetMass, true);
507
508 BSConstraint constrain;
509 if (!m_physicsScene.Constraints.TryGetConstraint(LinksetRoot.PhysBody, li.member.PhysBody, out constrain))
510 {
511 // If constraint doesn't exist yet, create it.
512 constrain = BuildConstraint(LinksetRoot, li);
513 }
514 li.SetLinkParameters(constrain);
515 constrain.RecomputeConstraintVariables(linksetMass);
516
517 // PhysicsScene.PE.DumpConstraint(PhysicsScene.World, constrain.Constraint); // DEBUG DEBUG
518 return false; // 'false' says to keep processing other members
519 });
300 } 520 }
521 finally
522 {
523 Rebuilding = false;
524 }
525 }
301 526
527 #region Extension
528 public override object Extension(string pFunct, params object[] pParams)
529 {
530 object ret = null;
531 switch (pFunct)
532 {
533 // pParams = [ BSPhysObject root, BSPhysObject child, integer linkType ]
534 case ExtendedPhysics.PhysFunctChangeLinkType:
535 if (pParams.Length > 2)
536 {
537 int requestedType = (int)pParams[2];
538 DetailLog("{0},BSLinksetConstraint.ChangeLinkType,requestedType={1}", LinksetRoot.LocalID, requestedType);
539 if (requestedType == (int)ConstraintType.FIXED_CONSTRAINT_TYPE
540 || requestedType == (int)ConstraintType.D6_CONSTRAINT_TYPE
541 || requestedType == (int)ConstraintType.D6_SPRING_CONSTRAINT_TYPE
542 || requestedType == (int)ConstraintType.HINGE_CONSTRAINT_TYPE
543 || requestedType == (int)ConstraintType.CONETWIST_CONSTRAINT_TYPE
544 || requestedType == (int)ConstraintType.SLIDER_CONSTRAINT_TYPE)
545 {
546 BSPrimLinkable child = pParams[1] as BSPrimLinkable;
547 if (child != null)
548 {
549 DetailLog("{0},BSLinksetConstraint.ChangeLinkType,rootID={1},childID={2},type={3}",
550 LinksetRoot.LocalID, LinksetRoot.LocalID, child.LocalID, requestedType);
551 m_physicsScene.TaintedObject(child.LocalID, "BSLinksetConstraint.PhysFunctChangeLinkType", delegate()
552 {
553 // Pick up all the constraints currently created.
554 RemoveDependencies(child);
555
556 BSLinkInfo linkInfo = null;
557 if (TryGetLinkInfo(child, out linkInfo))
558 {
559 BSLinkInfoConstraint linkInfoC = linkInfo as BSLinkInfoConstraint;
560 if (linkInfoC != null)
561 {
562 linkInfoC.constraintType = (ConstraintType)requestedType;
563 ret = (object)true;
564 DetailLog("{0},BSLinksetConstraint.ChangeLinkType,link={1},type={2}",
565 linkInfo.member.LocalID, linkInfo.member.LocalID, linkInfoC.constraintType);
566 }
567 else
568 {
569 DetailLog("{0},BSLinksetConstraint.ChangeLinkType,linkInfoNotConstraint,childID={1}", LinksetRoot.LocalID, child.LocalID);
570 }
571 }
572 else
573 {
574 DetailLog("{0},BSLinksetConstraint.ChangeLinkType,noLinkInfoForChild,childID={1}", LinksetRoot.LocalID, child.LocalID);
575 }
576 // Cause the whole linkset to be rebuilt in post-taint time.
577 Refresh(child);
578 });
579 }
580 else
581 {
582 DetailLog("{0},BSLinksetConstraint.SetLinkType,childNotBSPrimLinkable", LinksetRoot.LocalID);
583 }
584 }
585 else
586 {
587 DetailLog("{0},BSLinksetConstraint.SetLinkType,illegalRequestedType,reqested={1},spring={2}",
588 LinksetRoot.LocalID, requestedType, ((int)ConstraintType.D6_SPRING_CONSTRAINT_TYPE));
589 }
590 }
591 break;
592 // pParams = [ BSPhysObject root, BSPhysObject child ]
593 case ExtendedPhysics.PhysFunctGetLinkType:
594 if (pParams.Length > 0)
595 {
596 BSPrimLinkable child = pParams[1] as BSPrimLinkable;
597 if (child != null)
598 {
599 BSLinkInfo linkInfo = null;
600 if (TryGetLinkInfo(child, out linkInfo))
601 {
602 BSLinkInfoConstraint linkInfoC = linkInfo as BSLinkInfoConstraint;
603 if (linkInfoC != null)
604 {
605 ret = (object)(int)linkInfoC.constraintType;
606 DetailLog("{0},BSLinksetConstraint.GetLinkType,link={1},type={2}",
607 linkInfo.member.LocalID, linkInfo.member.LocalID, linkInfoC.constraintType);
608
609 }
610 }
611 }
612 }
613 break;
614 // pParams = [ BSPhysObject root, BSPhysObject child, int op, object opParams, int op, object opParams, ... ]
615 case ExtendedPhysics.PhysFunctChangeLinkParams:
616 // There should be two parameters: the childActor and a list of parameters to set
617 if (pParams.Length > 2)
618 {
619 BSPrimLinkable child = pParams[1] as BSPrimLinkable;
620 BSLinkInfo baseLinkInfo = null;
621 if (TryGetLinkInfo(child, out baseLinkInfo))
622 {
623 BSLinkInfoConstraint linkInfo = baseLinkInfo as BSLinkInfoConstraint;
624 if (linkInfo != null)
625 {
626 int valueInt;
627 float valueFloat;
628 bool valueBool;
629 OMV.Vector3 valueVector;
630 OMV.Vector3 valueVector2;
631 OMV.Quaternion valueQuaternion;
632 int axisLow, axisHigh;
633
634 int opIndex = 2;
635 while (opIndex < pParams.Length)
636 {
637 int thisOp = 0;
638 string errMsg = "";
639 try
640 {
641 thisOp = (int)pParams[opIndex];
642 DetailLog("{0},BSLinksetConstraint.ChangeLinkParams2,op={1},val={2}",
643 linkInfo.member.LocalID, thisOp, pParams[opIndex + 1]);
644 switch (thisOp)
645 {
646 case ExtendedPhysics.PHYS_PARAM_LINK_TYPE:
647 valueInt = (int)pParams[opIndex + 1];
648 ConstraintType valueType = (ConstraintType)valueInt;
649 if (valueType == ConstraintType.FIXED_CONSTRAINT_TYPE
650 || valueType == ConstraintType.D6_CONSTRAINT_TYPE
651 || valueType == ConstraintType.D6_SPRING_CONSTRAINT_TYPE
652 || valueType == ConstraintType.HINGE_CONSTRAINT_TYPE
653 || valueType == ConstraintType.CONETWIST_CONSTRAINT_TYPE
654 || valueType == ConstraintType.SLIDER_CONSTRAINT_TYPE)
655 {
656 linkInfo.constraintType = valueType;
657 }
658 opIndex += 2;
659 break;
660 case ExtendedPhysics.PHYS_PARAM_FRAMEINA_LOC:
661 errMsg = "PHYS_PARAM_FRAMEINA_LOC takes one parameter of type vector";
662 valueVector = (OMV.Vector3)pParams[opIndex + 1];
663 linkInfo.frameInAloc = valueVector;
664 opIndex += 2;
665 break;
666 case ExtendedPhysics.PHYS_PARAM_FRAMEINA_ROT:
667 errMsg = "PHYS_PARAM_FRAMEINA_ROT takes one parameter of type rotation";
668 valueQuaternion = (OMV.Quaternion)pParams[opIndex + 1];
669 linkInfo.frameInArot = valueQuaternion;
670 opIndex += 2;
671 break;
672 case ExtendedPhysics.PHYS_PARAM_FRAMEINB_LOC:
673 errMsg = "PHYS_PARAM_FRAMEINB_LOC takes one parameter of type vector";
674 valueVector = (OMV.Vector3)pParams[opIndex + 1];
675 linkInfo.frameInBloc = valueVector;
676 opIndex += 2;
677 break;
678 case ExtendedPhysics.PHYS_PARAM_FRAMEINB_ROT:
679 errMsg = "PHYS_PARAM_FRAMEINB_ROT takes one parameter of type rotation";
680 valueQuaternion = (OMV.Quaternion)pParams[opIndex + 1];
681 linkInfo.frameInBrot = valueQuaternion;
682 opIndex += 2;
683 break;
684 case ExtendedPhysics.PHYS_PARAM_LINEAR_LIMIT_LOW:
685 errMsg = "PHYS_PARAM_LINEAR_LIMIT_LOW takes one parameter of type vector";
686 valueVector = (OMV.Vector3)pParams[opIndex + 1];
687 linkInfo.linearLimitLow = valueVector;
688 opIndex += 2;
689 break;
690 case ExtendedPhysics.PHYS_PARAM_LINEAR_LIMIT_HIGH:
691 errMsg = "PHYS_PARAM_LINEAR_LIMIT_HIGH takes one parameter of type vector";
692 valueVector = (OMV.Vector3)pParams[opIndex + 1];
693 linkInfo.linearLimitHigh = valueVector;
694 opIndex += 2;
695 break;
696 case ExtendedPhysics.PHYS_PARAM_ANGULAR_LIMIT_LOW:
697 errMsg = "PHYS_PARAM_ANGULAR_LIMIT_LOW takes one parameter of type vector";
698 valueVector = (OMV.Vector3)pParams[opIndex + 1];
699 linkInfo.angularLimitLow = valueVector;
700 opIndex += 2;
701 break;
702 case ExtendedPhysics.PHYS_PARAM_ANGULAR_LIMIT_HIGH:
703 errMsg = "PHYS_PARAM_ANGULAR_LIMIT_HIGH takes one parameter of type vector";
704 valueVector = (OMV.Vector3)pParams[opIndex + 1];
705 linkInfo.angularLimitHigh = valueVector;
706 opIndex += 2;
707 break;
708 case ExtendedPhysics.PHYS_PARAM_USE_FRAME_OFFSET:
709 errMsg = "PHYS_PARAM_USE_FRAME_OFFSET takes one parameter of type integer (bool)";
710 valueBool = ((int)pParams[opIndex + 1]) != 0;
711 linkInfo.useFrameOffset = valueBool;
712 opIndex += 2;
713 break;
714 case ExtendedPhysics.PHYS_PARAM_ENABLE_TRANSMOTOR:
715 errMsg = "PHYS_PARAM_ENABLE_TRANSMOTOR takes one parameter of type integer (bool)";
716 valueBool = ((int)pParams[opIndex + 1]) != 0;
717 linkInfo.enableTransMotor = valueBool;
718 opIndex += 2;
719 break;
720 case ExtendedPhysics.PHYS_PARAM_TRANSMOTOR_MAXVEL:
721 errMsg = "PHYS_PARAM_TRANSMOTOR_MAXVEL takes one parameter of type float";
722 valueFloat = (float)pParams[opIndex + 1];
723 linkInfo.transMotorMaxVel = valueFloat;
724 opIndex += 2;
725 break;
726 case ExtendedPhysics.PHYS_PARAM_TRANSMOTOR_MAXFORCE:
727 errMsg = "PHYS_PARAM_TRANSMOTOR_MAXFORCE takes one parameter of type float";
728 valueFloat = (float)pParams[opIndex + 1];
729 linkInfo.transMotorMaxForce = valueFloat;
730 opIndex += 2;
731 break;
732 case ExtendedPhysics.PHYS_PARAM_CFM:
733 errMsg = "PHYS_PARAM_CFM takes one parameter of type float";
734 valueFloat = (float)pParams[opIndex + 1];
735 linkInfo.cfm = valueFloat;
736 opIndex += 2;
737 break;
738 case ExtendedPhysics.PHYS_PARAM_ERP:
739 errMsg = "PHYS_PARAM_ERP takes one parameter of type float";
740 valueFloat = (float)pParams[opIndex + 1];
741 linkInfo.erp = valueFloat;
742 opIndex += 2;
743 break;
744 case ExtendedPhysics.PHYS_PARAM_SOLVER_ITERATIONS:
745 errMsg = "PHYS_PARAM_SOLVER_ITERATIONS takes one parameter of type float";
746 valueFloat = (float)pParams[opIndex + 1];
747 linkInfo.solverIterations = valueFloat;
748 opIndex += 2;
749 break;
750 case ExtendedPhysics.PHYS_PARAM_SPRING_AXIS_ENABLE:
751 errMsg = "PHYS_PARAM_SPRING_AXIS_ENABLE takes two parameters of types integer and integer (bool)";
752 valueInt = (int)pParams[opIndex + 1];
753 valueBool = ((int)pParams[opIndex + 2]) != 0;
754 GetAxisRange(valueInt, out axisLow, out axisHigh);
755 for (int ii = axisLow; ii <= axisHigh; ii++)
756 linkInfo.springAxisEnable[ii] = valueBool;
757 opIndex += 3;
758 break;
759 case ExtendedPhysics.PHYS_PARAM_SPRING_DAMPING:
760 errMsg = "PHYS_PARAM_SPRING_DAMPING takes two parameters of types integer and float";
761 valueInt = (int)pParams[opIndex + 1];
762 valueFloat = (float)pParams[opIndex + 2];
763 GetAxisRange(valueInt, out axisLow, out axisHigh);
764 for (int ii = axisLow; ii <= axisHigh; ii++)
765 linkInfo.springDamping[ii] = valueFloat;
766 opIndex += 3;
767 break;
768 case ExtendedPhysics.PHYS_PARAM_SPRING_STIFFNESS:
769 errMsg = "PHYS_PARAM_SPRING_STIFFNESS takes two parameters of types integer and float";
770 valueInt = (int)pParams[opIndex + 1];
771 valueFloat = (float)pParams[opIndex + 2];
772 GetAxisRange(valueInt, out axisLow, out axisHigh);
773 for (int ii = axisLow; ii <= axisHigh; ii++)
774 linkInfo.springStiffness[ii] = valueFloat;
775 opIndex += 3;
776 break;
777 case ExtendedPhysics.PHYS_PARAM_SPRING_EQUILIBRIUM_POINT:
778 errMsg = "PHYS_PARAM_SPRING_EQUILIBRIUM_POINT takes two parameters of type vector";
779 valueVector = (OMV.Vector3)pParams[opIndex + 1];
780 valueVector2 = (OMV.Vector3)pParams[opIndex + 2];
781 linkInfo.springLinearEquilibriumPoint = valueVector;
782 linkInfo.springAngularEquilibriumPoint = valueVector2;
783 opIndex += 3;
784 break;
785 case ExtendedPhysics.PHYS_PARAM_USE_LINEAR_FRAMEA:
786 errMsg = "PHYS_PARAM_USE_LINEAR_FRAMEA takes one parameter of type integer (bool)";
787 valueBool = ((int)pParams[opIndex + 1]) != 0;
788 linkInfo.useLinearReferenceFrameA = valueBool;
789 opIndex += 2;
790 break;
791 default:
792 break;
793 }
794 }
795 catch (InvalidCastException e)
796 {
797 m_physicsScene.Logger.WarnFormat("{0} value of wrong type in physSetLinksetParams: {1}, err={2}",
798 LogHeader, errMsg, e);
799 }
800 catch (Exception e)
801 {
802 m_physicsScene.Logger.WarnFormat("{0} bad parameters in physSetLinksetParams: {1}", LogHeader, e);
803 }
804 }
805 }
806 // Something changed so a rebuild is in order
807 Refresh(child);
808 }
809 }
810 break;
811 default:
812 ret = base.Extension(pFunct, pParams);
813 break;
814 }
815 return ret;
816 }
817
818 // Bullet constraints keep some limit parameters for each linear and angular axis.
819 // Setting same is easier if there is an easy way to see all or types.
820 // This routine returns the array limits for the set of axis.
821 private void GetAxisRange(int rangeSpec, out int low, out int high)
822 {
823 switch (rangeSpec)
824 {
825 case ExtendedPhysics.PHYS_AXIS_LINEAR_ALL:
826 low = 0;
827 high = 2;
828 break;
829 case ExtendedPhysics.PHYS_AXIS_ANGULAR_ALL:
830 low = 3;
831 high = 5;
832 break;
833 case ExtendedPhysics.PHYS_AXIS_ALL:
834 low = 0;
835 high = 5;
836 break;
837 default:
838 low = high = rangeSpec;
839 break;
840 }
841 return;
302 } 842 }
843 #endregion // Extension
844
303} 845}
304} 846}
generated by cgit v1.1 at 2024-12-14 21:08:44 +0000