aboutsummaryrefslogtreecommitdiffstatshomepage
diff options
context:
space:
mode:
authorRobert Adams2012-10-21 16:12:06 -0700
committerRobert Adams2012-10-22 22:24:30 -0700
commita61f20ac74836049cbd24397670c2dcd75fb22da (patch)
tree5a93b9f5437189f871f8e56223239316bcec21bd
parentMake it possible to turn the base UDP object packet pools on and off whilst r... (diff)
downloadopensim-SC_OLD-a61f20ac74836049cbd24397670c2dcd75fb22da.zip
opensim-SC_OLD-a61f20ac74836049cbd24397670c2dcd75fb22da.tar.gz
opensim-SC_OLD-a61f20ac74836049cbd24397670c2dcd75fb22da.tar.bz2
opensim-SC_OLD-a61f20ac74836049cbd24397670c2dcd75fb22da.tar.xz
BulletSim: Create LinkSet abstract class and sparate constraint based linksets into own subclass. Will eventually add manual movement linkset subclass.
-rwxr-xr-xOpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs355
-rwxr-xr-xOpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs396
-rwxr-xr-xOpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs2
3 files changed, 434 insertions, 319 deletions
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
index 43b1262..2e6b104 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
@@ -32,10 +32,27 @@ using OMV = OpenMetaverse;
32 32
33namespace OpenSim.Region.Physics.BulletSPlugin 33namespace OpenSim.Region.Physics.BulletSPlugin
34{ 34{
35public class BSLinkset 35public abstract class BSLinkset
36{ 36{
37 // private static string LogHeader = "[BULLETSIM LINKSET]"; 37 // private static string LogHeader = "[BULLETSIM LINKSET]";
38 38
39 // Create the correct type of linkset for this child
40 public static BSLinkset Factory(BSScene physScene, BSPhysObject parent)
41 {
42 BSLinkset ret = null;
43 /*
44 if (parent.IsPhysical)
45 ret = new BSLinksetConstraints(physScene, parent);
46 else
47 ret = new BSLinksetManual(physScene, parent);
48 */
49
50 // at the moment, there is only one
51 ret = new BSLinksetConstraints(physScene, parent);
52
53 return ret;
54 }
55
39 public BSPhysObject LinksetRoot { get; protected set; } 56 public BSPhysObject LinksetRoot { get; protected set; }
40 57
41 public BSScene PhysicsScene { get; private set; } 58 public BSScene PhysicsScene { get; private set; }
@@ -52,16 +69,16 @@ public class BSLinkset
52 // the physical 'taint' children separately. 69 // the physical 'taint' children separately.
53 // After taint processing and before the simulation step, these 70 // After taint processing and before the simulation step, these
54 // two lists must be the same. 71 // two lists must be the same.
55 private HashSet<BSPhysObject> m_children; 72 protected HashSet<BSPhysObject> m_children;
56 private HashSet<BSPhysObject> m_taintChildren; 73 protected HashSet<BSPhysObject> m_taintChildren;
57 74
58 // We lock the diddling of linkset classes to prevent any badness. 75 // We lock the diddling of linkset classes to prevent any badness.
59 // This locks the modification of the instances of this class. Changes 76 // This locks the modification of the instances of this class. Changes
60 // to the physical representation is done via the tainting mechenism. 77 // to the physical representation is done via the tainting mechenism.
61 private object m_linksetActivityLock = new Object(); 78 protected object m_linksetActivityLock = new Object();
62 79
63 // We keep the prim's mass in the linkset structure since it could be dependent on other prims 80 // We keep the prim's mass in the linkset structure since it could be dependent on other prims
64 private float m_mass; 81 protected float m_mass;
65 public float LinksetMass 82 public float LinksetMass
66 { 83 {
67 get 84 get
@@ -81,7 +98,7 @@ public class BSLinkset
81 get { return ComputeLinksetGeometricCenter(); } 98 get { return ComputeLinksetGeometricCenter(); }
82 } 99 }
83 100
84 public BSLinkset(BSScene scene, BSPhysObject parent) 101 protected void Initialize(BSScene scene, BSPhysObject parent)
85 { 102 {
86 // A simple linkset of one (no children) 103 // A simple linkset of one (no children)
87 LinksetID = m_nextLinksetID++; 104 LinksetID = m_nextLinksetID++;
@@ -128,7 +145,7 @@ public class BSLinkset
128 } 145 }
129 146
130 // The child is down to a linkset of just itself 147 // The child is down to a linkset of just itself
131 return new BSLinkset(PhysicsScene, child); 148 return BSLinkset.Factory(PhysicsScene, child);
132 } 149 }
133 150
134 // Return 'true' if the passed object is the root object of this linkset 151 // Return 'true' if the passed object is the root object of this linkset
@@ -163,24 +180,7 @@ public class BSLinkset
163 // When physical properties are changed the linkset needs to recalculate 180 // When physical properties are changed the linkset needs to recalculate
164 // its internal properties. 181 // its internal properties.
165 // May be called at runtime or taint-time (just pass the appropriate flag). 182 // May be called at runtime or taint-time (just pass the appropriate flag).
166 public void Refresh(BSPhysObject requestor, bool inTaintTime) 183 public abstract void Refresh(BSPhysObject requestor, bool inTaintTime);
167 {
168 // If there are no children, not physical or not root, I am not the one that recomputes the constraints
169 // (For the moment, static linksets do create constraints so remove the test for physical.)
170 if (!HasAnyChildren || /*!requestor.IsPhysical ||*/ !IsRoot(requestor))
171 return;
172
173 BSScene.TaintCallback refreshOperation = delegate()
174 {
175 RecomputeLinksetConstraintVariables();
176 DetailLog("{0},BSLinkset.Refresh,complete,rBody={1}",
177 LinksetRoot.LocalID, LinksetRoot.BSBody.ptr.ToString("X"));
178 };
179 if (inTaintTime)
180 refreshOperation();
181 else
182 PhysicsScene.TaintedObject("BSLinkSet.Refresh", refreshOperation);
183 }
184 184
185 // The object is going dynamic (physical). Do any setup necessary 185 // The object is going dynamic (physical). Do any setup necessary
186 // for a dynamic linkset. 186 // for a dynamic linkset.
@@ -188,102 +188,36 @@ public class BSLinkset
188 // has not yet been fully constructed. 188 // has not yet been fully constructed.
189 // Return 'true' if any properties updated on the passed object. 189 // Return 'true' if any properties updated on the passed object.
190 // Called at taint-time! 190 // Called at taint-time!
191 public bool MakeDynamic(BSPhysObject child) 191 public abstract bool MakeDynamic(BSPhysObject child);
192 {
193 // What is done for each object in BSPrim is what we want.
194 return false;
195 }
196 192
197 // The object is going static (non-physical). Do any setup necessary 193 // The object is going static (non-physical). Do any setup necessary
198 // for a static linkset. 194 // for a static linkset.
199 // Return 'true' if any properties updated on the passed object. 195 // Return 'true' if any properties updated on the passed object.
200 // Called at taint-time! 196 // Called at taint-time!
201 public bool MakeStatic(BSPhysObject child) 197 public abstract bool MakeStatic(BSPhysObject child);
202 {
203 // What is done for each object in BSPrim is what we want.
204 return false;
205 }
206 198
207 // If the software is handling the movement of all the objects in a linkset 199 // If the software is handling the movement of all the objects in a linkset
208 // (like if one doesn't use constraints for static linksets), this is called 200 // (like if one doesn't use constraints for static linksets), this is called
209 // when an update for the root of the linkset is received. 201 // when an update for the root of the linkset is received.
210 // Called at taint-time!! 202 // Called at taint-time!!
211 public void UpdateProperties(BSPhysObject physObject) 203 public abstract void UpdateProperties(BSPhysObject physObject);
212 {
213 // The root local properties have been updated. Apply to the children if appropriate.
214 if (IsRoot(physObject) && HasAnyChildren)
215 {
216 if (!physObject.IsPhysical)
217 {
218 // TODO: implement software linkset update for static object linksets
219 }
220 }
221 }
222 204
223 // Routine used when rebuilding the body of the root of the linkset 205 // Routine used when rebuilding the body of the root of the linkset
224 // Destroy all the constraints have have been made to root. 206 // Destroy all the constraints have have been made to root.
225 // This is called when the root body is changing. 207 // This is called when the root body is changing.
226 // Returns 'true' of something eas actually removed and would need restoring 208 // Returns 'true' of something eas actually removed and would need restoring
227 // Called at taint-time!! 209 // Called at taint-time!!
228 public bool RemoveBodyDependencies(BSPrim child) 210 public abstract bool RemoveBodyDependencies(BSPrim child);
229 {
230 bool ret = false;
231
232 lock (m_linksetActivityLock)
233 {
234 if (IsRoot(child))
235 {
236 // If the one with the dependency is root, must undo all children
237 DetailLog("{0},BSLinkset.RemoveBodyDependencies,removeChildrenForRoot,rID={1},rBody={2}",
238 child.LocalID, LinksetRoot.LocalID, LinksetRoot.BSBody.ptr.ToString("X"));
239
240 ret = PhysicallyUnlinkAllChildrenFromRoot(LinksetRoot);
241 }
242 else
243 {
244 DetailLog("{0},BSLinkset.RemoveBodyDependencies,removeSingleChild,rID={1},rBody={2},cID={3},cBody={4}",
245 child.LocalID,
246 LinksetRoot.LocalID, LinksetRoot.BSBody.ptr.ToString("X"),
247 child.LocalID, child.BSBody.ptr.ToString("X"));
248 // ret = PhysicallyUnlinkAChildFromRoot(LinksetRoot, child);
249 // Despite the function name, this removes any link to the specified object.
250 ret = PhysicallyUnlinkAllChildrenFromRoot(child);
251 }
252 }
253 return ret;
254 }
255 211
256 // Companion to RemoveBodyDependencies(). If RemoveBodyDependencies() returns 'true', 212 // Companion to RemoveBodyDependencies(). If RemoveBodyDependencies() returns 'true',
257 // this routine will restore the removed constraints. 213 // this routine will restore the removed constraints.
258 // Called at taint-time!! 214 // Called at taint-time!!
259 public void RestoreBodyDependencies(BSPrim child) 215 public abstract void RestoreBodyDependencies(BSPrim child);
260 {
261 lock (m_linksetActivityLock)
262 {
263 if (IsRoot(child))
264 {
265 DetailLog("{0},BSLinkset.RestoreBodyDependencies,restoreChildrenForRoot,rID={1},numChild={2}",
266 child.LocalID, LinksetRoot.LocalID, m_taintChildren.Count);
267 foreach (BSPhysObject bpo in m_taintChildren)
268 {
269 PhysicallyLinkAChildToRoot(LinksetRoot, bpo);
270 }
271 }
272 else
273 {
274 DetailLog("{0},BSLinkset.RestoreBodyDependencies,restoreSingleChild,rID={1},rBody={2},cID={3},cBody={4}",
275 LinksetRoot.LocalID,
276 LinksetRoot.LocalID, LinksetRoot.BSBody.ptr.ToString("X"),
277 child.LocalID, child.BSBody.ptr.ToString("X"));
278 PhysicallyLinkAChildToRoot(LinksetRoot, child);
279 }
280 }
281 }
282 216
283 // ================================================================ 217 // ================================================================
284 // Below this point is internal magic 218 // Below this point is internal magic
285 219
286 private float ComputeLinksetMass() 220 protected virtual float ComputeLinksetMass()
287 { 221 {
288 float mass; 222 float mass;
289 lock (m_linksetActivityLock) 223 lock (m_linksetActivityLock)
@@ -297,7 +231,7 @@ public class BSLinkset
297 return mass; 231 return mass;
298 } 232 }
299 233
300 private OMV.Vector3 ComputeLinksetCenterOfMass() 234 protected virtual OMV.Vector3 ComputeLinksetCenterOfMass()
301 { 235 {
302 OMV.Vector3 com; 236 OMV.Vector3 com;
303 lock (m_linksetActivityLock) 237 lock (m_linksetActivityLock)
@@ -317,7 +251,7 @@ public class BSLinkset
317 return com; 251 return com;
318 } 252 }
319 253
320 private OMV.Vector3 ComputeLinksetGeometricCenter() 254 protected virtual OMV.Vector3 ComputeLinksetGeometricCenter()
321 { 255 {
322 OMV.Vector3 com; 256 OMV.Vector3 com;
323 lock (m_linksetActivityLock) 257 lock (m_linksetActivityLock)
@@ -336,236 +270,21 @@ public class BSLinkset
336 270
337 // I am the root of a linkset and a new child is being added 271 // I am the root of a linkset and a new child is being added
338 // Called while LinkActivity is locked. 272 // Called while LinkActivity is locked.
339 private void AddChildToLinkset(BSPhysObject child) 273 protected abstract void AddChildToLinkset(BSPhysObject child);
340 {
341 if (!HasChild(child))
342 {
343 m_children.Add(child);
344
345 BSPhysObject rootx = LinksetRoot; // capture the root as of now
346 BSPhysObject childx = child;
347
348 DetailLog("{0},AddChildToLinkset,call,child={1}", LinksetRoot.LocalID, child.LocalID);
349
350 PhysicsScene.TaintedObject("AddChildToLinkset", delegate()
351 {
352 DetailLog("{0},AddChildToLinkset,taint,rID={1},rBody={2},cID={3},cBody={4}",
353 rootx.LocalID,
354 rootx.LocalID, rootx.BSBody.ptr.ToString("X"),
355 childx.LocalID, childx.BSBody.ptr.ToString("X"));
356 // Since this is taint-time, the body and shape could have changed for the child
357 rootx.ForcePosition = rootx.Position; // DEBUG
358 childx.ForcePosition = childx.Position; // DEBUG
359 PhysicallyLinkAChildToRoot(rootx, childx);
360 m_taintChildren.Add(child);
361 });
362 }
363 return;
364 }
365 274
366 // Forcefully removing a child from a linkset. 275 // Forcefully removing a child from a linkset.
367 // This is not being called by the child so we have to make sure the child doesn't think 276 // This is not being called by the child so we have to make sure the child doesn't think
368 // it's still connected to the linkset. 277 // it's still connected to the linkset.
369 // Normal OpenSimulator operation will never do this because other SceneObjectPart information 278 // Normal OpenSimulator operation will never do this because other SceneObjectPart information
370 // also has to be updated (like pointer to prim's parent). 279 // also has to be updated (like pointer to prim's parent).
371 private void RemoveChildFromOtherLinkset(BSPhysObject pchild) 280 protected abstract void RemoveChildFromOtherLinkset(BSPhysObject pchild);
372 {
373 pchild.Linkset = new BSLinkset(PhysicsScene, pchild);
374 RemoveChildFromLinkset(pchild);
375 }
376 281
377 // I am the root of a linkset and one of my children is being removed. 282 // I am the root of a linkset and one of my children is being removed.
378 // Safe to call even if the child is not really in my linkset. 283 // Safe to call even if the child is not really in my linkset.
379 private void RemoveChildFromLinkset(BSPhysObject child) 284 protected abstract void RemoveChildFromLinkset(BSPhysObject child);
380 {
381 if (m_children.Remove(child))
382 {
383 BSPhysObject rootx = LinksetRoot; // capture the root and body as of now
384 BSPhysObject childx = child;
385
386 DetailLog("{0},RemoveChildFromLinkset,call,rID={1},rBody={2},cID={3},cBody={4}",
387 childx.LocalID,
388 rootx.LocalID, rootx.BSBody.ptr.ToString("X"),
389 childx.LocalID, childx.BSBody.ptr.ToString("X"));
390
391 PhysicsScene.TaintedObject("RemoveChildFromLinkset", delegate()
392 {
393 m_taintChildren.Remove(child);
394 PhysicallyUnlinkAChildFromRoot(rootx, childx);
395 RecomputeLinksetConstraintVariables();
396 });
397
398 }
399 else
400 {
401 // This will happen if we remove the root of the linkset first. Non-fatal occurance.
402 // PhysicsScene.Logger.ErrorFormat("{0}: Asked to remove child from linkset that was not in linkset", LogHeader);
403 }
404 return;
405 }
406
407 // Create a constraint between me (root of linkset) and the passed prim (the child).
408 // Called at taint time!
409 private void PhysicallyLinkAChildToRoot(BSPhysObject rootPrim, BSPhysObject childPrim)
410 {
411 // Zero motion for children so they don't interpolate
412 childPrim.ZeroMotion();
413
414 // Relative position normalized to the root prim
415 // Essentually a vector pointing from center of rootPrim to center of childPrim
416 OMV.Vector3 childRelativePosition = childPrim.Position - rootPrim.Position;
417
418 // real world coordinate of midpoint between the two objects
419 OMV.Vector3 midPoint = rootPrim.Position + (childRelativePosition / 2);
420
421 DetailLog("{0},BSLinkset.PhysicallyLinkAChildToRoot,taint,root={1},rBody={2},child={3},cBody={4},rLoc={5},cLoc={6},midLoc={7}",
422 rootPrim.LocalID,
423 rootPrim.LocalID, rootPrim.BSBody.ptr.ToString("X"),
424 childPrim.LocalID, childPrim.BSBody.ptr.ToString("X"),
425 rootPrim.Position, childPrim.Position, midPoint);
426
427 // create a constraint that allows no freedom of movement between the two objects
428 // http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?t=4818
429
430 BS6DofConstraint constrain = new BS6DofConstraint(
431 PhysicsScene.World, rootPrim.BSBody, childPrim.BSBody, midPoint, true, true );
432
433 /* NOTE: below is an attempt to build constraint with full frame computation, etc.
434 * Using the midpoint is easier since it lets the Bullet code manipulate the transforms
435 * of the objects.
436 * Code left as a warning to future programmers.
437 // ==================================================================================
438 // relative position normalized to the root prim
439 OMV.Quaternion invThisOrientation = OMV.Quaternion.Inverse(rootPrim.Orientation);
440 OMV.Vector3 childRelativePosition = (childPrim.Position - rootPrim.Position) * invThisOrientation;
441
442 // relative rotation of the child to the parent
443 OMV.Quaternion childRelativeRotation = invThisOrientation * childPrim.Orientation;
444 OMV.Quaternion inverseChildRelativeRotation = OMV.Quaternion.Inverse(childRelativeRotation);
445
446 // create a constraint that allows no freedom of movement between the two objects
447 // http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?t=4818
448 DetailLog("{0},BSLinkset.PhysicallyLinkAChildToRoot,taint,root={1},child={2}", rootPrim.LocalID, rootPrim.LocalID, childPrim.LocalID);
449 BS6DofConstraint constrain = new BS6DofConstraint(
450 PhysicsScene.World, rootPrim.Body, childPrim.Body,
451 OMV.Vector3.Zero,
452 OMV.Quaternion.Inverse(rootPrim.Orientation),
453 OMV.Vector3.Zero,
454 OMV.Quaternion.Inverse(childPrim.Orientation),
455 // A point half way between the parent and child
456 // childRelativePosition/2,
457 // childRelativeRotation,
458 // childRelativePosition/2,
459 // inverseChildRelativeRotation,
460 true,
461 true
462 );
463 // ==================================================================================
464 */
465
466 PhysicsScene.Constraints.AddConstraint(constrain);
467
468 // zero linear and angular limits makes the objects unable to move in relation to each other
469 constrain.SetLinearLimits(OMV.Vector3.Zero, OMV.Vector3.Zero);
470 constrain.SetAngularLimits(OMV.Vector3.Zero, OMV.Vector3.Zero);
471
472 // tweek the constraint to increase stability
473 constrain.UseFrameOffset(PhysicsScene.BoolNumeric(PhysicsScene.Params.linkConstraintUseFrameOffset));
474 constrain.TranslationalLimitMotor(PhysicsScene.BoolNumeric(PhysicsScene.Params.linkConstraintEnableTransMotor),
475 PhysicsScene.Params.linkConstraintTransMotorMaxVel,
476 PhysicsScene.Params.linkConstraintTransMotorMaxForce);
477 constrain.SetCFMAndERP(PhysicsScene.Params.linkConstraintCFM, PhysicsScene.Params.linkConstraintERP);
478 if (PhysicsScene.Params.linkConstraintSolverIterations != 0f)
479 {
480 constrain.SetSolverIterations(PhysicsScene.Params.linkConstraintSolverIterations);
481 }
482 }
483
484 // Remove linkage between myself and a particular child
485 // The root and child bodies are passed in because we need to remove the constraint between
486 // the bodies that were at unlink time.
487 // Called at taint time!
488 private bool PhysicallyUnlinkAChildFromRoot(BSPhysObject rootPrim, BSPhysObject childPrim)
489 {
490 bool ret = false;
491 DetailLog("{0},BSLinkset.PhysicallyUnlinkAChildFromRoot,taint,root={1},rBody={2},child={3},cBody={4}",
492 rootPrim.LocalID,
493 rootPrim.LocalID, rootPrim.BSBody.ptr.ToString("X"),
494 childPrim.LocalID, childPrim.BSBody.ptr.ToString("X"));
495
496 // Find the constraint for this link and get rid of it from the overall collection and from my list
497 if (PhysicsScene.Constraints.RemoveAndDestroyConstraint(rootPrim.BSBody, childPrim.BSBody))
498 {
499 // Make the child refresh its location
500 BulletSimAPI.PushUpdate2(childPrim.BSBody.ptr);
501 ret = true;
502 }
503
504 return ret;
505 }
506
507 // Remove linkage between myself and any possible children I might have.
508 // Called at taint time!
509 private bool PhysicallyUnlinkAllChildrenFromRoot(BSPhysObject rootPrim)
510 {
511 DetailLog("{0},BSLinkset.PhysicallyUnlinkAllChildren,taint", rootPrim.LocalID);
512 bool ret = false;
513
514 if (PhysicsScene.Constraints.RemoveAndDestroyConstraint(rootPrim.BSBody))
515 {
516 ret = true;
517 }
518 return ret;
519 }
520
521 // Call each of the constraints that make up this linkset and recompute the
522 // various transforms and variables. Used when objects are added or removed
523 // from a linkset to make sure the constraints know about the new mass and
524 // geometry.
525 // Must only be called at taint time!!
526 private void RecomputeLinksetConstraintVariables()
527 {
528 float linksetMass = LinksetMass;
529 foreach (BSPhysObject child in m_taintChildren)
530 {
531 BSConstraint constrain;
532 if (PhysicsScene.Constraints.TryGetConstraint(LinksetRoot.BSBody, child.BSBody, out constrain))
533 {
534 // DetailLog("{0},BSLinkset.RecomputeLinksetConstraintVariables,taint,child={1},mass={2},A={3},B={4}",
535 // LinksetRoot.LocalID, child.LocalID, linksetMass, constrain.Body1.ID, constrain.Body2.ID);
536 constrain.RecomputeConstraintVariables(linksetMass);
537 }
538 else
539 {
540 // Non-fatal error that happens when children are being added to the linkset but
541 // their constraints have not been created yet.
542 break;
543 }
544 }
545
546 // If the whole linkset is not here, doesn't make sense to recompute linkset wide values
547 if (m_children.Count == m_taintChildren.Count)
548 {
549 // If this is a multiple object linkset, set everybody's center of mass to the set's center of mass
550 OMV.Vector3 centerOfMass = ComputeLinksetCenterOfMass();
551 BulletSimAPI.SetCenterOfMassByPosRot2(LinksetRoot.BSBody.ptr,
552 centerOfMass, OMV.Quaternion.Identity);
553 DetailLog("{0},BSLinkset.RecomputeLinksetConstraintVariables,setCenterOfMass,COM={1},rBody={2}",
554 LinksetRoot.LocalID, centerOfMass, LinksetRoot.BSBody.ptr.ToString("X"));
555 foreach (BSPhysObject child in m_taintChildren)
556 {
557 BulletSimAPI.SetCenterOfMassByPosRot2(child.BSBody.ptr,
558 centerOfMass, OMV.Quaternion.Identity);
559 }
560
561 // BulletSimAPI.DumpAllInfo2(PhysicsScene.World.ptr); // DEBUG DEBUG DEBUG
562 }
563 return;
564 }
565
566 285
567 // Invoke the detailed logger and output something if it's enabled. 286 // Invoke the detailed logger and output something if it's enabled.
568 private void DetailLog(string msg, params Object[] args) 287 protected void DetailLog(string msg, params Object[] args)
569 { 288 {
570 if (PhysicsScene.PhysicsLogging.Enabled) 289 if (PhysicsScene.PhysicsLogging.Enabled)
571 PhysicsScene.DetailLog(msg, args); 290 PhysicsScene.DetailLog(msg, args);
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs
new file mode 100755
index 0000000..ee53d92
--- /dev/null
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs
@@ -0,0 +1,396 @@
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 OMV = OpenMetaverse;
32
33namespace OpenSim.Region.Physics.BulletSPlugin
34{
35public class BSLinksetConstraints : BSLinkset
36{
37 // private static string LogHeader = "[BULLETSIM LINKSET CONSTRAINTS]";
38
39 public BSLinksetConstraints(BSScene scene, BSPhysObject parent)
40 {
41 base.Initialize(scene, parent);
42 }
43
44 // When physical properties are changed the linkset needs to recalculate
45 // its internal properties.
46 // May be called at runtime or taint-time (just pass the appropriate flag).
47 public override void Refresh(BSPhysObject requestor, bool inTaintTime)
48 {
49 // If there are no children, not physical or not root, I am not the one that recomputes the constraints
50 // (For the moment, static linksets do create constraints so remove the test for physical.)
51 if (!HasAnyChildren || /*!requestor.IsPhysical ||*/ !IsRoot(requestor))
52 return;
53
54 BSScene.TaintCallback refreshOperation = delegate()
55 {
56 RecomputeLinksetConstraintVariables();
57 DetailLog("{0},BSLinkset.Refresh,complete,rBody={1}",
58 LinksetRoot.LocalID, LinksetRoot.BSBody.ptr.ToString("X"));
59 };
60 if (inTaintTime)
61 refreshOperation();
62 else
63 PhysicsScene.TaintedObject("BSLinkSet.Refresh", refreshOperation);
64 }
65
66 // The object is going dynamic (physical). Do any setup necessary
67 // for a dynamic linkset.
68 // Only the state of the passed object can be modified. The rest of the linkset
69 // has not yet been fully constructed.
70 // Return 'true' if any properties updated on the passed object.
71 // Called at taint-time!
72 public override bool MakeDynamic(BSPhysObject child)
73 {
74 // What is done for each object in BSPrim is what we want.
75 return false;
76 }
77
78 // The object is going static (non-physical). Do any setup necessary
79 // for a static linkset.
80 // Return 'true' if any properties updated on the passed object.
81 // Called at taint-time!
82 public override bool MakeStatic(BSPhysObject child)
83 {
84 // What is done for each object in BSPrim is what we want.
85 return false;
86 }
87
88 // If the software is handling the movement of all the objects in a linkset
89 // (like if one doesn't use constraints for static linksets), this is called
90 // when an update for the root of the linkset is received.
91 // Called at taint-time!!
92 public override void UpdateProperties(BSPhysObject physObject)
93 {
94 // The root local properties have been updated. Apply to the children if appropriate.
95 if (IsRoot(physObject) && HasAnyChildren)
96 {
97 if (!physObject.IsPhysical)
98 {
99 // TODO: implement software linkset update for static object linksets
100 }
101 }
102 }
103
104 // Routine used when rebuilding the body of the root of the linkset
105 // Destroy all the constraints have have been made to root.
106 // This is called when the root body is changing.
107 // Returns 'true' of something eas actually removed and would need restoring
108 // Called at taint-time!!
109 public override bool RemoveBodyDependencies(BSPrim child)
110 {
111 bool ret = false;
112
113 lock (m_linksetActivityLock)
114 {
115 if (IsRoot(child))
116 {
117 // If the one with the dependency is root, must undo all children
118 DetailLog("{0},BSLinkset.RemoveBodyDependencies,removeChildrenForRoot,rID={1},rBody={2}",
119 child.LocalID, LinksetRoot.LocalID, LinksetRoot.BSBody.ptr.ToString("X"));
120
121 ret = PhysicallyUnlinkAllChildrenFromRoot(LinksetRoot);
122 }
123 else
124 {
125 DetailLog("{0},BSLinkset.RemoveBodyDependencies,removeSingleChild,rID={1},rBody={2},cID={3},cBody={4}",
126 child.LocalID,
127 LinksetRoot.LocalID, LinksetRoot.BSBody.ptr.ToString("X"),
128 child.LocalID, child.BSBody.ptr.ToString("X"));
129 // ret = PhysicallyUnlinkAChildFromRoot(LinksetRoot, child);
130 // Despite the function name, this removes any link to the specified object.
131 ret = PhysicallyUnlinkAllChildrenFromRoot(child);
132 }
133 }
134 return ret;
135 }
136
137 // Companion to RemoveBodyDependencies(). If RemoveBodyDependencies() returns 'true',
138 // this routine will restore the removed constraints.
139 // Called at taint-time!!
140 public override void RestoreBodyDependencies(BSPrim child)
141 {
142 lock (m_linksetActivityLock)
143 {
144 if (IsRoot(child))
145 {
146 DetailLog("{0},BSLinkset.RestoreBodyDependencies,restoreChildrenForRoot,rID={1},numChild={2}",
147 child.LocalID, LinksetRoot.LocalID, m_taintChildren.Count);
148 foreach (BSPhysObject bpo in m_taintChildren)
149 {
150 PhysicallyLinkAChildToRoot(LinksetRoot, bpo);
151 }
152 }
153 else
154 {
155 DetailLog("{0},BSLinkset.RestoreBodyDependencies,restoreSingleChild,rID={1},rBody={2},cID={3},cBody={4}",
156 LinksetRoot.LocalID,
157 LinksetRoot.LocalID, LinksetRoot.BSBody.ptr.ToString("X"),
158 child.LocalID, child.BSBody.ptr.ToString("X"));
159 PhysicallyLinkAChildToRoot(LinksetRoot, child);
160 }
161 }
162 }
163
164 // ================================================================
165 // Below this point is internal magic
166
167 // I am the root of a linkset and a new child is being added
168 // Called while LinkActivity is locked.
169 protected override void AddChildToLinkset(BSPhysObject child)
170 {
171 if (!HasChild(child))
172 {
173 m_children.Add(child);
174
175 BSPhysObject rootx = LinksetRoot; // capture the root as of now
176 BSPhysObject childx = child;
177
178 DetailLog("{0},AddChildToLinkset,call,child={1}", LinksetRoot.LocalID, child.LocalID);
179
180 PhysicsScene.TaintedObject("AddChildToLinkset", delegate()
181 {
182 DetailLog("{0},AddChildToLinkset,taint,rID={1},rBody={2},cID={3},cBody={4}",
183 rootx.LocalID,
184 rootx.LocalID, rootx.BSBody.ptr.ToString("X"),
185 childx.LocalID, childx.BSBody.ptr.ToString("X"));
186 // Since this is taint-time, the body and shape could have changed for the child
187 rootx.ForcePosition = rootx.Position; // DEBUG
188 childx.ForcePosition = childx.Position; // DEBUG
189 PhysicallyLinkAChildToRoot(rootx, childx);
190 m_taintChildren.Add(child);
191 });
192 }
193 return;
194 }
195
196 // Forcefully removing a child from a linkset.
197 // This is not being called by the child so we have to make sure the child doesn't think
198 // it's still connected to the linkset.
199 // Normal OpenSimulator operation will never do this because other SceneObjectPart information
200 // also has to be updated (like pointer to prim's parent).
201 protected override void RemoveChildFromOtherLinkset(BSPhysObject pchild)
202 {
203 pchild.Linkset = BSLinkset.Factory(PhysicsScene, pchild);
204 RemoveChildFromLinkset(pchild);
205 }
206
207 // I am the root of a linkset and one of my children is being removed.
208 // Safe to call even if the child is not really in my linkset.
209 protected override void RemoveChildFromLinkset(BSPhysObject child)
210 {
211 if (m_children.Remove(child))
212 {
213 BSPhysObject rootx = LinksetRoot; // capture the root and body as of now
214 BSPhysObject childx = child;
215
216 DetailLog("{0},RemoveChildFromLinkset,call,rID={1},rBody={2},cID={3},cBody={4}",
217 childx.LocalID,
218 rootx.LocalID, rootx.BSBody.ptr.ToString("X"),
219 childx.LocalID, childx.BSBody.ptr.ToString("X"));
220
221 PhysicsScene.TaintedObject("RemoveChildFromLinkset", delegate()
222 {
223 m_taintChildren.Remove(child);
224 PhysicallyUnlinkAChildFromRoot(rootx, childx);
225 RecomputeLinksetConstraintVariables();
226 });
227
228 }
229 else
230 {
231 // This will happen if we remove the root of the linkset first. Non-fatal occurance.
232 // PhysicsScene.Logger.ErrorFormat("{0}: Asked to remove child from linkset that was not in linkset", LogHeader);
233 }
234 return;
235 }
236
237 // Create a constraint between me (root of linkset) and the passed prim (the child).
238 // Called at taint time!
239 private void PhysicallyLinkAChildToRoot(BSPhysObject rootPrim, BSPhysObject childPrim)
240 {
241 // Zero motion for children so they don't interpolate
242 childPrim.ZeroMotion();
243
244 // Relative position normalized to the root prim
245 // Essentually a vector pointing from center of rootPrim to center of childPrim
246 OMV.Vector3 childRelativePosition = childPrim.Position - rootPrim.Position;
247
248 // real world coordinate of midpoint between the two objects
249 OMV.Vector3 midPoint = rootPrim.Position + (childRelativePosition / 2);
250
251 DetailLog("{0},BSLinkset.PhysicallyLinkAChildToRoot,taint,root={1},rBody={2},child={3},cBody={4},rLoc={5},cLoc={6},midLoc={7}",
252 rootPrim.LocalID,
253 rootPrim.LocalID, rootPrim.BSBody.ptr.ToString("X"),
254 childPrim.LocalID, childPrim.BSBody.ptr.ToString("X"),
255 rootPrim.Position, childPrim.Position, midPoint);
256
257 // create a constraint that allows no freedom of movement between the two objects
258 // http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?t=4818
259
260 BS6DofConstraint constrain = new BS6DofConstraint(
261 PhysicsScene.World, rootPrim.BSBody, childPrim.BSBody, midPoint, true, true );
262
263 /* NOTE: below is an attempt to build constraint with full frame computation, etc.
264 * Using the midpoint is easier since it lets the Bullet code manipulate the transforms
265 * of the objects.
266 * Code left as a warning to future programmers.
267 // ==================================================================================
268 // relative position normalized to the root prim
269 OMV.Quaternion invThisOrientation = OMV.Quaternion.Inverse(rootPrim.Orientation);
270 OMV.Vector3 childRelativePosition = (childPrim.Position - rootPrim.Position) * invThisOrientation;
271
272 // relative rotation of the child to the parent
273 OMV.Quaternion childRelativeRotation = invThisOrientation * childPrim.Orientation;
274 OMV.Quaternion inverseChildRelativeRotation = OMV.Quaternion.Inverse(childRelativeRotation);
275
276 // create a constraint that allows no freedom of movement between the two objects
277 // http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?t=4818
278 DetailLog("{0},BSLinkset.PhysicallyLinkAChildToRoot,taint,root={1},child={2}", rootPrim.LocalID, rootPrim.LocalID, childPrim.LocalID);
279 BS6DofConstraint constrain = new BS6DofConstraint(
280 PhysicsScene.World, rootPrim.Body, childPrim.Body,
281 OMV.Vector3.Zero,
282 OMV.Quaternion.Inverse(rootPrim.Orientation),
283 OMV.Vector3.Zero,
284 OMV.Quaternion.Inverse(childPrim.Orientation),
285 // A point half way between the parent and child
286 // childRelativePosition/2,
287 // childRelativeRotation,
288 // childRelativePosition/2,
289 // inverseChildRelativeRotation,
290 true,
291 true
292 );
293 // ==================================================================================
294 */
295
296 PhysicsScene.Constraints.AddConstraint(constrain);
297
298 // zero linear and angular limits makes the objects unable to move in relation to each other
299 constrain.SetLinearLimits(OMV.Vector3.Zero, OMV.Vector3.Zero);
300 constrain.SetAngularLimits(OMV.Vector3.Zero, OMV.Vector3.Zero);
301
302 // tweek the constraint to increase stability
303 constrain.UseFrameOffset(PhysicsScene.BoolNumeric(PhysicsScene.Params.linkConstraintUseFrameOffset));
304 constrain.TranslationalLimitMotor(PhysicsScene.BoolNumeric(PhysicsScene.Params.linkConstraintEnableTransMotor),
305 PhysicsScene.Params.linkConstraintTransMotorMaxVel,
306 PhysicsScene.Params.linkConstraintTransMotorMaxForce);
307 constrain.SetCFMAndERP(PhysicsScene.Params.linkConstraintCFM, PhysicsScene.Params.linkConstraintERP);
308 if (PhysicsScene.Params.linkConstraintSolverIterations != 0f)
309 {
310 constrain.SetSolverIterations(PhysicsScene.Params.linkConstraintSolverIterations);
311 }
312 }
313
314 // Remove linkage between myself and a particular child
315 // The root and child bodies are passed in because we need to remove the constraint between
316 // the bodies that were at unlink time.
317 // Called at taint time!
318 private bool PhysicallyUnlinkAChildFromRoot(BSPhysObject rootPrim, BSPhysObject childPrim)
319 {
320 bool ret = false;
321 DetailLog("{0},BSLinkset.PhysicallyUnlinkAChildFromRoot,taint,root={1},rBody={2},child={3},cBody={4}",
322 rootPrim.LocalID,
323 rootPrim.LocalID, rootPrim.BSBody.ptr.ToString("X"),
324 childPrim.LocalID, childPrim.BSBody.ptr.ToString("X"));
325
326 // Find the constraint for this link and get rid of it from the overall collection and from my list
327 if (PhysicsScene.Constraints.RemoveAndDestroyConstraint(rootPrim.BSBody, childPrim.BSBody))
328 {
329 // Make the child refresh its location
330 BulletSimAPI.PushUpdate2(childPrim.BSBody.ptr);
331 ret = true;
332 }
333
334 return ret;
335 }
336
337 // Remove linkage between myself and any possible children I might have.
338 // Called at taint time!
339 private bool PhysicallyUnlinkAllChildrenFromRoot(BSPhysObject rootPrim)
340 {
341 DetailLog("{0},BSLinkset.PhysicallyUnlinkAllChildren,taint", rootPrim.LocalID);
342 bool ret = false;
343
344 if (PhysicsScene.Constraints.RemoveAndDestroyConstraint(rootPrim.BSBody))
345 {
346 ret = true;
347 }
348 return ret;
349 }
350
351 // Call each of the constraints that make up this linkset and recompute the
352 // various transforms and variables. Used when objects are added or removed
353 // from a linkset to make sure the constraints know about the new mass and
354 // geometry.
355 // Must only be called at taint time!!
356 private void RecomputeLinksetConstraintVariables()
357 {
358 float linksetMass = LinksetMass;
359 foreach (BSPhysObject child in m_taintChildren)
360 {
361 BSConstraint constrain;
362 if (PhysicsScene.Constraints.TryGetConstraint(LinksetRoot.BSBody, child.BSBody, out constrain))
363 {
364 // DetailLog("{0},BSLinkset.RecomputeLinksetConstraintVariables,taint,child={1},mass={2},A={3},B={4}",
365 // LinksetRoot.LocalID, child.LocalID, linksetMass, constrain.Body1.ID, constrain.Body2.ID);
366 constrain.RecomputeConstraintVariables(linksetMass);
367 }
368 else
369 {
370 // Non-fatal error that happens when children are being added to the linkset but
371 // their constraints have not been created yet.
372 break;
373 }
374 }
375
376 // If the whole linkset is not here, doesn't make sense to recompute linkset wide values
377 if (m_children.Count == m_taintChildren.Count)
378 {
379 // If this is a multiple object linkset, set everybody's center of mass to the set's center of mass
380 OMV.Vector3 centerOfMass = ComputeLinksetCenterOfMass();
381 BulletSimAPI.SetCenterOfMassByPosRot2(LinksetRoot.BSBody.ptr,
382 centerOfMass, OMV.Quaternion.Identity);
383 DetailLog("{0},BSLinkset.RecomputeLinksetConstraintVariables,setCenterOfMass,COM={1},rBody={2}",
384 LinksetRoot.LocalID, centerOfMass, LinksetRoot.BSBody.ptr.ToString("X"));
385 foreach (BSPhysObject child in m_taintChildren)
386 {
387 BulletSimAPI.SetCenterOfMassByPosRot2(child.BSBody.ptr,
388 centerOfMass, OMV.Quaternion.Identity);
389 }
390
391 // BulletSimAPI.DumpAllInfo2(PhysicsScene.World.ptr); // DEBUG DEBUG DEBUG
392 }
393 return;
394 }
395}
396}
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
index ead6a08..51b9196 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
@@ -46,7 +46,7 @@ public abstract class BSPhysObject : PhysicsActor
46 PhysObjectName = name; 46 PhysObjectName = name;
47 TypeName = typeName; 47 TypeName = typeName;
48 48
49 Linkset = new BSLinkset(PhysicsScene, this); 49 Linkset = BSLinkset.Factory(PhysicsScene, this);
50 LastAssetBuildFailed = false; 50 LastAssetBuildFailed = false;
51 51
52 CollisionCollection = new CollisionEventUpdate(); 52 CollisionCollection = new CollisionEventUpdate();