aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/PhysicsModules/BulletS/BSLinksetConstraints.cs
diff options
context:
space:
mode:
Diffstat (limited to 'OpenSim/Region/PhysicsModules/BulletS/BSLinksetConstraints.cs')
-rwxr-xr-xOpenSim/Region/PhysicsModules/BulletS/BSLinksetConstraints.cs854
1 files changed, 854 insertions, 0 deletions
diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSLinksetConstraints.cs b/OpenSim/Region/PhysicsModules/BulletS/BSLinksetConstraints.cs
new file mode 100755
index 0000000..4384cdc
--- /dev/null
+++ b/OpenSim/Region/PhysicsModules/BulletS/BSLinksetConstraints.cs
@@ -0,0 +1,854 @@
1/*
2 * Copyright (c) Contributors, http://opensimulator.org/
3 * See CONTRIBUTORS.TXT for a full list of copyright holders.
4 *
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions are met:
7 * * Redistributions of source code must retain the above copyright
8 * notice, this list of conditions and the following disclaimer.
9 * * Redistributions in binary form must reproduce the above copyrightD
10 * notice, this list of conditions and the following disclaimer in the
11 * documentation and/or other materials provided with the distribution.
12 * * Neither the name of the OpenSimulator Project nor the
13 * names of its contributors may be used to endorse or promote products
14 * derived from this software without specific prior written permission.
15 *
16 * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
17 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
20 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27using System;
28using System.Collections.Generic;
29using System.Text;
30
31using OpenSim.Region.OptionalModules.Scripting;
32
33using OMV = OpenMetaverse;
34
35namespace OpenSim.Region.Physics.BulletSPlugin
36{
37public sealed class BSLinksetConstraints : BSLinkset
38{
39 // private static string LogHeader = "[BULLETSIM LINKSET CONSTRAINTS]";
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.BS_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.BS_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.BS_FIXED_CONSTRAINT_TYPE)
183 ret = false;
184
185 return ret;
186 }
187 }
188
189 public BSLinksetConstraints(BSScene scene, BSPrimLinkable parent) : base(scene, parent)
190 {
191 LinksetImpl = LinksetImplementation.Constraint;
192 }
193
194 private static string LogHeader = "[BULLETSIM LINKSET CONSTRAINT]";
195
196 // When physical properties are changed the linkset needs to recalculate
197 // its internal properties.
198 // This is queued in the 'post taint' queue so the
199 // refresh will happen once after all the other taints are applied.
200 public override void Refresh(BSPrimLinkable requestor)
201 {
202 ScheduleRebuild(requestor);
203 base.Refresh(requestor);
204
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 lock (this)
216 {
217 if (!RebuildScheduled)
218 {
219 if (!Rebuilding && HasAnyChildren)
220 {
221 RebuildScheduled = true;
222 // Queue to happen after all the other taint processing
223 m_physicsScene.PostTaintObject("BSLinksetContraints.Refresh", requestor.LocalID, delegate()
224 {
225 if (HasAnyChildren)
226 {
227 // Constraints that have not been changed are not rebuild but make sure
228 // the constraint of the requestor is rebuilt.
229 PhysicallyUnlinkAChildFromRoot(LinksetRoot, requestor);
230 // Rebuild the linkset and all its constraints.
231 RecomputeLinksetConstraints();
232 }
233 RebuildScheduled = false;
234 });
235 }
236 }
237 }
238 }
239
240 // The object is going dynamic (physical). Do any setup necessary
241 // for a dynamic linkset.
242 // Only the state of the passed object can be modified. The rest of the linkset
243 // has not yet been fully constructed.
244 // Return 'true' if any properties updated on the passed object.
245 // Called at taint-time!
246 public override bool MakeDynamic(BSPrimLinkable child)
247 {
248 bool ret = false;
249 DetailLog("{0},BSLinksetConstraints.MakeDynamic,call,IsRoot={1}", child.LocalID, IsRoot(child));
250 if (IsRoot(child))
251 {
252 // The root is going dynamic. Rebuild the linkset so parts and mass get computed properly.
253 Refresh(LinksetRoot);
254 }
255 return ret;
256 }
257
258 // The object is going static (non-physical). Do any setup necessary for a static linkset.
259 // Return 'true' if any properties updated on the passed object.
260 // This doesn't normally happen -- OpenSim removes the objects from the physical
261 // world if it is a static linkset.
262 // Called at taint-time!
263 public override bool MakeStatic(BSPrimLinkable child)
264 {
265 bool ret = false;
266
267 DetailLog("{0},BSLinksetConstraint.MakeStatic,call,IsRoot={1}", child.LocalID, IsRoot(child));
268 child.ClearDisplacement();
269 if (IsRoot(child))
270 {
271 // Schedule a rebuild to verify that the root shape is set to the real shape.
272 Refresh(LinksetRoot);
273 }
274 return ret;
275 }
276
277 // Called at taint-time!!
278 public override void UpdateProperties(UpdatedProperties whichUpdated, BSPrimLinkable pObj)
279 {
280 // Nothing to do for constraints on property updates
281 }
282
283 // Routine called when rebuilding the body of some member of the linkset.
284 // Destroy all the constraints have have been made to root and set
285 // up to rebuild the constraints before the next simulation step.
286 // Returns 'true' of something was actually removed and would need restoring
287 // Called at taint-time!!
288 public override bool RemoveDependencies(BSPrimLinkable child)
289 {
290 bool ret = false;
291
292 DetailLog("{0},BSLinksetConstraint.RemoveDependencies,removeChildrenForRoot,rID={1},rBody={2}",
293 child.LocalID, LinksetRoot.LocalID, LinksetRoot.PhysBody.AddrString);
294
295 lock (m_linksetActivityLock)
296 {
297 // Just undo all the constraints for this linkset. Rebuild at the end of the step.
298 ret = PhysicallyUnlinkAllChildrenFromRoot(LinksetRoot);
299 // Cause the constraints, et al to be rebuilt before the next simulation step.
300 Refresh(LinksetRoot);
301 }
302 return ret;
303 }
304
305 // ================================================================
306
307 // Add a new child to the linkset.
308 // Called while LinkActivity is locked.
309 protected override void AddChildToLinkset(BSPrimLinkable child)
310 {
311 if (!HasChild(child))
312 {
313 m_children.Add(child, new BSLinkInfoConstraint(child));
314
315 DetailLog("{0},BSLinksetConstraints.AddChildToLinkset,call,child={1}", LinksetRoot.LocalID, child.LocalID);
316
317 // Cause constraints and assorted properties to be recomputed before the next simulation step.
318 Refresh(LinksetRoot);
319 }
320 return;
321 }
322
323 // Remove the specified child from the linkset.
324 // Safe to call even if the child is not really in my linkset.
325 protected override void RemoveChildFromLinkset(BSPrimLinkable child, bool inTaintTime)
326 {
327 if (m_children.Remove(child))
328 {
329 BSPrimLinkable rootx = LinksetRoot; // capture the root and body as of now
330 BSPrimLinkable childx = child;
331
332 DetailLog("{0},BSLinksetConstraints.RemoveChildFromLinkset,call,rID={1},rBody={2},cID={3},cBody={4}",
333 childx.LocalID,
334 rootx.LocalID, rootx.PhysBody.AddrString,
335 childx.LocalID, childx.PhysBody.AddrString);
336
337 m_physicsScene.TaintedObject(inTaintTime, childx.LocalID, "BSLinksetConstraints.RemoveChildFromLinkset", delegate()
338 {
339 PhysicallyUnlinkAChildFromRoot(rootx, childx);
340 });
341 // See that the linkset parameters are recomputed at the end of the taint time.
342 Refresh(LinksetRoot);
343 }
344 else
345 {
346 // Non-fatal occurance.
347 // PhysicsScene.Logger.ErrorFormat("{0}: Asked to remove child from linkset that was not in linkset", LogHeader);
348 }
349 return;
350 }
351
352 // Create a constraint between me (root of linkset) and the passed prim (the child).
353 // Called at taint time!
354 private void PhysicallyLinkAChildToRoot(BSPrimLinkable rootPrim, BSPrimLinkable childPrim)
355 {
356 // Don't build the constraint when asked. Put it off until just before the simulation step.
357 Refresh(rootPrim);
358 }
359
360 // Create a static constraint between the two passed objects
361 private BSConstraint BuildConstraint(BSPrimLinkable rootPrim, BSLinkInfo li)
362 {
363 BSLinkInfoConstraint linkInfo = li as BSLinkInfoConstraint;
364 if (linkInfo == null)
365 return null;
366
367 // Zero motion for children so they don't interpolate
368 li.member.ZeroMotion(true);
369
370 BSConstraint constrain = null;
371
372 switch (linkInfo.constraintType)
373 {
374 case ConstraintType.BS_FIXED_CONSTRAINT_TYPE:
375 case ConstraintType.D6_CONSTRAINT_TYPE:
376 // Relative position normalized to the root prim
377 // Essentually a vector pointing from center of rootPrim to center of li.member
378 OMV.Vector3 childRelativePosition = linkInfo.member.Position - rootPrim.Position;
379
380 // real world coordinate of midpoint between the two objects
381 OMV.Vector3 midPoint = rootPrim.Position + (childRelativePosition / 2);
382
383 DetailLog("{0},BSLinksetConstraint.BuildConstraint,6Dof,rBody={1},cBody={2},rLoc={3},cLoc={4},midLoc={5}",
384 rootPrim.LocalID, rootPrim.PhysBody, linkInfo.member.PhysBody,
385 rootPrim.Position, linkInfo.member.Position, midPoint);
386
387 // create a constraint that allows no freedom of movement between the two objects
388 // http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?t=4818
389
390 constrain = new BSConstraint6Dof(
391 m_physicsScene.World, rootPrim.PhysBody, linkInfo.member.PhysBody, midPoint, true, true );
392
393 /* NOTE: below is an attempt to build constraint with full frame computation, etc.
394 * Using the midpoint is easier since it lets the Bullet code manipulate the transforms
395 * of the objects.
396 * Code left for future programmers.
397 // ==================================================================================
398 // relative position normalized to the root prim
399 OMV.Quaternion invThisOrientation = OMV.Quaternion.Inverse(rootPrim.Orientation);
400 OMV.Vector3 childRelativePosition = (liConstraint.member.Position - rootPrim.Position) * invThisOrientation;
401
402 // relative rotation of the child to the parent
403 OMV.Quaternion childRelativeRotation = invThisOrientation * liConstraint.member.Orientation;
404 OMV.Quaternion inverseChildRelativeRotation = OMV.Quaternion.Inverse(childRelativeRotation);
405
406 DetailLog("{0},BSLinksetConstraint.PhysicallyLinkAChildToRoot,taint,root={1},child={2}", rootPrim.LocalID, rootPrim.LocalID, liConstraint.member.LocalID);
407 constrain = new BS6DofConstraint(
408 PhysicsScene.World, rootPrim.Body, liConstraint.member.Body,
409 OMV.Vector3.Zero,
410 OMV.Quaternion.Inverse(rootPrim.Orientation),
411 OMV.Vector3.Zero,
412 OMV.Quaternion.Inverse(liConstraint.member.Orientation),
413 true,
414 true
415 );
416 // ==================================================================================
417 */
418
419 break;
420 case ConstraintType.D6_SPRING_CONSTRAINT_TYPE:
421 constrain = new BSConstraintSpring(m_physicsScene.World, rootPrim.PhysBody, linkInfo.member.PhysBody,
422 linkInfo.frameInAloc, linkInfo.frameInArot, linkInfo.frameInBloc, linkInfo.frameInBrot,
423 linkInfo.useLinearReferenceFrameA,
424 true /*disableCollisionsBetweenLinkedBodies*/);
425 DetailLog("{0},BSLinksetConstraint.BuildConstraint,spring,root={1},rBody={2},child={3},cBody={4},rLoc={5},cLoc={6}",
426 rootPrim.LocalID,
427 rootPrim.LocalID, rootPrim.PhysBody.AddrString,
428 linkInfo.member.LocalID, linkInfo.member.PhysBody.AddrString,
429 rootPrim.Position, linkInfo.member.Position);
430
431 break;
432 default:
433 break;
434 }
435
436 linkInfo.SetLinkParameters(constrain);
437
438 m_physicsScene.Constraints.AddConstraint(constrain);
439
440 return constrain;
441 }
442
443 // Remove linkage between the linkset root and a particular child
444 // The root and child bodies are passed in because we need to remove the constraint between
445 // the bodies that were present at unlink time.
446 // Called at taint time!
447 private bool PhysicallyUnlinkAChildFromRoot(BSPrimLinkable rootPrim, BSPrimLinkable childPrim)
448 {
449 bool ret = false;
450 DetailLog("{0},BSLinksetConstraint.PhysicallyUnlinkAChildFromRoot,taint,root={1},rBody={2},child={3},cBody={4}",
451 rootPrim.LocalID,
452 rootPrim.LocalID, rootPrim.PhysBody.AddrString,
453 childPrim.LocalID, childPrim.PhysBody.AddrString);
454
455 // If asked to unlink root from root, just remove all the constraints
456 if (rootPrim == childPrim || childPrim == LinksetRoot)
457 {
458 PhysicallyUnlinkAllChildrenFromRoot(LinksetRoot);
459 ret = true;
460 }
461 else
462 {
463 // Find the constraint for this link and get rid of it from the overall collection and from my list
464 if (m_physicsScene.Constraints.RemoveAndDestroyConstraint(rootPrim.PhysBody, childPrim.PhysBody))
465 {
466 // Make the child refresh its location
467 m_physicsScene.PE.PushUpdate(childPrim.PhysBody);
468 ret = true;
469 }
470 }
471
472 return ret;
473 }
474
475 // Remove linkage between myself and any possible children I might have.
476 // Returns 'true' of any constraints were destroyed.
477 // Called at taint time!
478 private bool PhysicallyUnlinkAllChildrenFromRoot(BSPrimLinkable rootPrim)
479 {
480 DetailLog("{0},BSLinksetConstraint.PhysicallyUnlinkAllChildren,taint", rootPrim.LocalID);
481
482 return m_physicsScene.Constraints.RemoveAndDestroyConstraint(rootPrim.PhysBody);
483 }
484
485 // Call each of the constraints that make up this linkset and recompute the
486 // various transforms and variables. Create constraints of not created yet.
487 // Called before the simulation step to make sure the constraint based linkset
488 // is all initialized.
489 // Called at taint time!!
490 private void RecomputeLinksetConstraints()
491 {
492 float linksetMass = LinksetMass;
493 LinksetRoot.UpdatePhysicalMassProperties(linksetMass, true);
494
495 DetailLog("{0},BSLinksetConstraint.RecomputeLinksetConstraints,set,rBody={1},linksetMass={2}",
496 LinksetRoot.LocalID, LinksetRoot.PhysBody.AddrString, linksetMass);
497
498 try
499 {
500 Rebuilding = true;
501
502 // There is no reason to build all this physical stuff for a non-physical linkset.
503 if (!LinksetRoot.IsPhysicallyActive || !HasAnyChildren)
504 {
505 DetailLog("{0},BSLinksetConstraint.RecomputeLinksetCompound,notPhysicalOrNoChildren", LinksetRoot.LocalID);
506 return; // Note the 'finally' clause at the botton which will get executed.
507 }
508
509 ForEachLinkInfo((li) =>
510 {
511 // A child in the linkset physically shows the mass of the whole linkset.
512 // This allows Bullet to apply enough force on the child to move the whole linkset.
513 // (Also do the mass stuff before recomputing the constraint so mass is not zero.)
514 li.member.UpdatePhysicalMassProperties(linksetMass, true);
515
516 BSConstraint constrain;
517 if (!m_physicsScene.Constraints.TryGetConstraint(LinksetRoot.PhysBody, li.member.PhysBody, out constrain))
518 {
519 // If constraint doesn't exist yet, create it.
520 constrain = BuildConstraint(LinksetRoot, li);
521 }
522 li.SetLinkParameters(constrain);
523 constrain.RecomputeConstraintVariables(linksetMass);
524
525 // PhysicsScene.PE.DumpConstraint(PhysicsScene.World, constrain.Constraint); // DEBUG DEBUG
526 return false; // 'false' says to keep processing other members
527 });
528 }
529 finally
530 {
531 Rebuilding = false;
532 }
533 }
534
535 #region Extension
536 public override object Extension(string pFunct, params object[] pParams)
537 {
538 object ret = null;
539 switch (pFunct)
540 {
541 // pParams = [ BSPhysObject root, BSPhysObject child, integer linkType ]
542 case ExtendedPhysics.PhysFunctChangeLinkType:
543 if (pParams.Length > 2)
544 {
545 int requestedType = (int)pParams[2];
546 DetailLog("{0},BSLinksetConstraint.ChangeLinkType,requestedType={1}", LinksetRoot.LocalID, requestedType);
547 if (requestedType == (int)ConstraintType.BS_FIXED_CONSTRAINT_TYPE
548 || requestedType == (int)ConstraintType.D6_CONSTRAINT_TYPE
549 || requestedType == (int)ConstraintType.D6_SPRING_CONSTRAINT_TYPE
550 || requestedType == (int)ConstraintType.HINGE_CONSTRAINT_TYPE
551 || requestedType == (int)ConstraintType.CONETWIST_CONSTRAINT_TYPE
552 || requestedType == (int)ConstraintType.SLIDER_CONSTRAINT_TYPE)
553 {
554 BSPrimLinkable child = pParams[1] as BSPrimLinkable;
555 if (child != null)
556 {
557 DetailLog("{0},BSLinksetConstraint.ChangeLinkType,rootID={1},childID={2},type={3}",
558 LinksetRoot.LocalID, LinksetRoot.LocalID, child.LocalID, requestedType);
559 m_physicsScene.TaintedObject(child.LocalID, "BSLinksetConstraint.PhysFunctChangeLinkType", delegate()
560 {
561 // Pick up all the constraints currently created.
562 RemoveDependencies(child);
563
564 BSLinkInfo linkInfo = null;
565 if (TryGetLinkInfo(child, out linkInfo))
566 {
567 BSLinkInfoConstraint linkInfoC = linkInfo as BSLinkInfoConstraint;
568 if (linkInfoC != null)
569 {
570 linkInfoC.constraintType = (ConstraintType)requestedType;
571 ret = (object)true;
572 DetailLog("{0},BSLinksetConstraint.ChangeLinkType,link={1},type={2}",
573 linkInfo.member.LocalID, linkInfo.member.LocalID, linkInfoC.constraintType);
574 }
575 else
576 {
577 DetailLog("{0},BSLinksetConstraint.ChangeLinkType,linkInfoNotConstraint,childID={1}", LinksetRoot.LocalID, child.LocalID);
578 }
579 }
580 else
581 {
582 DetailLog("{0},BSLinksetConstraint.ChangeLinkType,noLinkInfoForChild,childID={1}", LinksetRoot.LocalID, child.LocalID);
583 }
584 // Cause the whole linkset to be rebuilt in post-taint time.
585 Refresh(child);
586 });
587 }
588 else
589 {
590 DetailLog("{0},BSLinksetConstraint.SetLinkType,childNotBSPrimLinkable", LinksetRoot.LocalID);
591 }
592 }
593 else
594 {
595 DetailLog("{0},BSLinksetConstraint.SetLinkType,illegalRequestedType,reqested={1},spring={2}",
596 LinksetRoot.LocalID, requestedType, ((int)ConstraintType.D6_SPRING_CONSTRAINT_TYPE));
597 }
598 }
599 break;
600 // pParams = [ BSPhysObject root, BSPhysObject child ]
601 case ExtendedPhysics.PhysFunctGetLinkType:
602 if (pParams.Length > 0)
603 {
604 BSPrimLinkable child = pParams[1] as BSPrimLinkable;
605 if (child != null)
606 {
607 BSLinkInfo linkInfo = null;
608 if (TryGetLinkInfo(child, out linkInfo))
609 {
610 BSLinkInfoConstraint linkInfoC = linkInfo as BSLinkInfoConstraint;
611 if (linkInfoC != null)
612 {
613 ret = (object)(int)linkInfoC.constraintType;
614 DetailLog("{0},BSLinksetConstraint.GetLinkType,link={1},type={2}",
615 linkInfo.member.LocalID, linkInfo.member.LocalID, linkInfoC.constraintType);
616
617 }
618 }
619 }
620 }
621 break;
622 // pParams = [ BSPhysObject root, BSPhysObject child, int op, object opParams, int op, object opParams, ... ]
623 case ExtendedPhysics.PhysFunctChangeLinkParams:
624 // There should be two parameters: the childActor and a list of parameters to set
625 if (pParams.Length > 2)
626 {
627 BSPrimLinkable child = pParams[1] as BSPrimLinkable;
628 BSLinkInfo baseLinkInfo = null;
629 if (TryGetLinkInfo(child, out baseLinkInfo))
630 {
631 BSLinkInfoConstraint linkInfo = baseLinkInfo as BSLinkInfoConstraint;
632 if (linkInfo != null)
633 {
634 int valueInt;
635 float valueFloat;
636 bool valueBool;
637 OMV.Vector3 valueVector;
638 OMV.Vector3 valueVector2;
639 OMV.Quaternion valueQuaternion;
640 int axisLow, axisHigh;
641
642 int opIndex = 2;
643 while (opIndex < pParams.Length)
644 {
645 int thisOp = 0;
646 string errMsg = "";
647 try
648 {
649 thisOp = (int)pParams[opIndex];
650 DetailLog("{0},BSLinksetConstraint.ChangeLinkParams2,op={1},val={2}",
651 linkInfo.member.LocalID, thisOp, pParams[opIndex + 1]);
652 switch (thisOp)
653 {
654 case ExtendedPhysics.PHYS_PARAM_LINK_TYPE:
655 valueInt = (int)pParams[opIndex + 1];
656 ConstraintType valueType = (ConstraintType)valueInt;
657 if (valueType == ConstraintType.BS_FIXED_CONSTRAINT_TYPE
658 || valueType == ConstraintType.D6_CONSTRAINT_TYPE
659 || valueType == ConstraintType.D6_SPRING_CONSTRAINT_TYPE
660 || valueType == ConstraintType.HINGE_CONSTRAINT_TYPE
661 || valueType == ConstraintType.CONETWIST_CONSTRAINT_TYPE
662 || valueType == ConstraintType.SLIDER_CONSTRAINT_TYPE)
663 {
664 linkInfo.constraintType = valueType;
665 }
666 opIndex += 2;
667 break;
668 case ExtendedPhysics.PHYS_PARAM_FRAMEINA_LOC:
669 errMsg = "PHYS_PARAM_FRAMEINA_LOC takes one parameter of type vector";
670 valueVector = (OMV.Vector3)pParams[opIndex + 1];
671 linkInfo.frameInAloc = valueVector;
672 opIndex += 2;
673 break;
674 case ExtendedPhysics.PHYS_PARAM_FRAMEINA_ROT:
675 errMsg = "PHYS_PARAM_FRAMEINA_ROT takes one parameter of type rotation";
676 valueQuaternion = (OMV.Quaternion)pParams[opIndex + 1];
677 linkInfo.frameInArot = valueQuaternion;
678 opIndex += 2;
679 break;
680 case ExtendedPhysics.PHYS_PARAM_FRAMEINB_LOC:
681 errMsg = "PHYS_PARAM_FRAMEINB_LOC takes one parameter of type vector";
682 valueVector = (OMV.Vector3)pParams[opIndex + 1];
683 linkInfo.frameInBloc = valueVector;
684 opIndex += 2;
685 break;
686 case ExtendedPhysics.PHYS_PARAM_FRAMEINB_ROT:
687 errMsg = "PHYS_PARAM_FRAMEINB_ROT takes one parameter of type rotation";
688 valueQuaternion = (OMV.Quaternion)pParams[opIndex + 1];
689 linkInfo.frameInBrot = valueQuaternion;
690 opIndex += 2;
691 break;
692 case ExtendedPhysics.PHYS_PARAM_LINEAR_LIMIT_LOW:
693 errMsg = "PHYS_PARAM_LINEAR_LIMIT_LOW takes one parameter of type vector";
694 valueVector = (OMV.Vector3)pParams[opIndex + 1];
695 linkInfo.linearLimitLow = valueVector;
696 opIndex += 2;
697 break;
698 case ExtendedPhysics.PHYS_PARAM_LINEAR_LIMIT_HIGH:
699 errMsg = "PHYS_PARAM_LINEAR_LIMIT_HIGH takes one parameter of type vector";
700 valueVector = (OMV.Vector3)pParams[opIndex + 1];
701 linkInfo.linearLimitHigh = valueVector;
702 opIndex += 2;
703 break;
704 case ExtendedPhysics.PHYS_PARAM_ANGULAR_LIMIT_LOW:
705 errMsg = "PHYS_PARAM_ANGULAR_LIMIT_LOW takes one parameter of type vector";
706 valueVector = (OMV.Vector3)pParams[opIndex + 1];
707 linkInfo.angularLimitLow = valueVector;
708 opIndex += 2;
709 break;
710 case ExtendedPhysics.PHYS_PARAM_ANGULAR_LIMIT_HIGH:
711 errMsg = "PHYS_PARAM_ANGULAR_LIMIT_HIGH takes one parameter of type vector";
712 valueVector = (OMV.Vector3)pParams[opIndex + 1];
713 linkInfo.angularLimitHigh = valueVector;
714 opIndex += 2;
715 break;
716 case ExtendedPhysics.PHYS_PARAM_USE_FRAME_OFFSET:
717 errMsg = "PHYS_PARAM_USE_FRAME_OFFSET takes one parameter of type integer (bool)";
718 valueBool = ((int)pParams[opIndex + 1]) != 0;
719 linkInfo.useFrameOffset = valueBool;
720 opIndex += 2;
721 break;
722 case ExtendedPhysics.PHYS_PARAM_ENABLE_TRANSMOTOR:
723 errMsg = "PHYS_PARAM_ENABLE_TRANSMOTOR takes one parameter of type integer (bool)";
724 valueBool = ((int)pParams[opIndex + 1]) != 0;
725 linkInfo.enableTransMotor = valueBool;
726 opIndex += 2;
727 break;
728 case ExtendedPhysics.PHYS_PARAM_TRANSMOTOR_MAXVEL:
729 errMsg = "PHYS_PARAM_TRANSMOTOR_MAXVEL takes one parameter of type float";
730 valueFloat = (float)pParams[opIndex + 1];
731 linkInfo.transMotorMaxVel = valueFloat;
732 opIndex += 2;
733 break;
734 case ExtendedPhysics.PHYS_PARAM_TRANSMOTOR_MAXFORCE:
735 errMsg = "PHYS_PARAM_TRANSMOTOR_MAXFORCE takes one parameter of type float";
736 valueFloat = (float)pParams[opIndex + 1];
737 linkInfo.transMotorMaxForce = valueFloat;
738 opIndex += 2;
739 break;
740 case ExtendedPhysics.PHYS_PARAM_CFM:
741 errMsg = "PHYS_PARAM_CFM takes one parameter of type float";
742 valueFloat = (float)pParams[opIndex + 1];
743 linkInfo.cfm = valueFloat;
744 opIndex += 2;
745 break;
746 case ExtendedPhysics.PHYS_PARAM_ERP:
747 errMsg = "PHYS_PARAM_ERP takes one parameter of type float";
748 valueFloat = (float)pParams[opIndex + 1];
749 linkInfo.erp = valueFloat;
750 opIndex += 2;
751 break;
752 case ExtendedPhysics.PHYS_PARAM_SOLVER_ITERATIONS:
753 errMsg = "PHYS_PARAM_SOLVER_ITERATIONS takes one parameter of type float";
754 valueFloat = (float)pParams[opIndex + 1];
755 linkInfo.solverIterations = valueFloat;
756 opIndex += 2;
757 break;
758 case ExtendedPhysics.PHYS_PARAM_SPRING_AXIS_ENABLE:
759 errMsg = "PHYS_PARAM_SPRING_AXIS_ENABLE takes two parameters of types integer and integer (bool)";
760 valueInt = (int)pParams[opIndex + 1];
761 valueBool = ((int)pParams[opIndex + 2]) != 0;
762 GetAxisRange(valueInt, out axisLow, out axisHigh);
763 for (int ii = axisLow; ii <= axisHigh; ii++)
764 linkInfo.springAxisEnable[ii] = valueBool;
765 opIndex += 3;
766 break;
767 case ExtendedPhysics.PHYS_PARAM_SPRING_DAMPING:
768 errMsg = "PHYS_PARAM_SPRING_DAMPING takes two parameters of types integer and float";
769 valueInt = (int)pParams[opIndex + 1];
770 valueFloat = (float)pParams[opIndex + 2];
771 GetAxisRange(valueInt, out axisLow, out axisHigh);
772 for (int ii = axisLow; ii <= axisHigh; ii++)
773 linkInfo.springDamping[ii] = valueFloat;
774 opIndex += 3;
775 break;
776 case ExtendedPhysics.PHYS_PARAM_SPRING_STIFFNESS:
777 errMsg = "PHYS_PARAM_SPRING_STIFFNESS takes two parameters of types integer and float";
778 valueInt = (int)pParams[opIndex + 1];
779 valueFloat = (float)pParams[opIndex + 2];
780 GetAxisRange(valueInt, out axisLow, out axisHigh);
781 for (int ii = axisLow; ii <= axisHigh; ii++)
782 linkInfo.springStiffness[ii] = valueFloat;
783 opIndex += 3;
784 break;
785 case ExtendedPhysics.PHYS_PARAM_SPRING_EQUILIBRIUM_POINT:
786 errMsg = "PHYS_PARAM_SPRING_EQUILIBRIUM_POINT takes two parameters of type vector";
787 valueVector = (OMV.Vector3)pParams[opIndex + 1];
788 valueVector2 = (OMV.Vector3)pParams[opIndex + 2];
789 linkInfo.springLinearEquilibriumPoint = valueVector;
790 linkInfo.springAngularEquilibriumPoint = valueVector2;
791 opIndex += 3;
792 break;
793 case ExtendedPhysics.PHYS_PARAM_USE_LINEAR_FRAMEA:
794 errMsg = "PHYS_PARAM_USE_LINEAR_FRAMEA takes one parameter of type integer (bool)";
795 valueBool = ((int)pParams[opIndex + 1]) != 0;
796 linkInfo.useLinearReferenceFrameA = valueBool;
797 opIndex += 2;
798 break;
799 default:
800 break;
801 }
802 }
803 catch (InvalidCastException e)
804 {
805 m_physicsScene.Logger.WarnFormat("{0} value of wrong type in physSetLinksetParams: {1}, err={2}",
806 LogHeader, errMsg, e);
807 }
808 catch (Exception e)
809 {
810 m_physicsScene.Logger.WarnFormat("{0} bad parameters in physSetLinksetParams: {1}", LogHeader, e);
811 }
812 }
813 }
814 // Something changed so a rebuild is in order
815 Refresh(child);
816 }
817 }
818 break;
819 default:
820 ret = base.Extension(pFunct, pParams);
821 break;
822 }
823 return ret;
824 }
825
826 // Bullet constraints keep some limit parameters for each linear and angular axis.
827 // Setting same is easier if there is an easy way to see all or types.
828 // This routine returns the array limits for the set of axis.
829 private void GetAxisRange(int rangeSpec, out int low, out int high)
830 {
831 switch (rangeSpec)
832 {
833 case ExtendedPhysics.PHYS_AXIS_LINEAR_ALL:
834 low = 0;
835 high = 2;
836 break;
837 case ExtendedPhysics.PHYS_AXIS_ANGULAR_ALL:
838 low = 3;
839 high = 5;
840 break;
841 case ExtendedPhysics.PHYS_AXIS_ALL:
842 low = 0;
843 high = 5;
844 break;
845 default:
846 low = high = rangeSpec;
847 break;
848 }
849 return;
850 }
851 #endregion // Extension
852
853}
854}
generated by cgit v1.1 at 2024-08-22 22:15:39 +0000