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