diff options
Diffstat (limited to 'OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs')
-rwxr-xr-x | OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs | 438 |
1 files changed, 342 insertions, 96 deletions
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs index b9c2cf9..0c4db40 100755 --- a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs | |||
@@ -28,22 +28,79 @@ using System; | |||
28 | using System.Collections.Generic; | 28 | using System.Collections.Generic; |
29 | using System.Text; | 29 | using System.Text; |
30 | 30 | ||
31 | using OpenSim.Framework; | ||
32 | |||
31 | using OMV = OpenMetaverse; | 33 | using OMV = OpenMetaverse; |
32 | 34 | ||
33 | namespace OpenSim.Region.Physics.BulletSPlugin | 35 | namespace OpenSim.Region.Physics.BulletSPlugin |
34 | { | 36 | { |
37 | |||
38 | // When a child is linked, the relationship position of the child to the parent | ||
39 | // is remembered so the child's world position can be recomputed when it is | ||
40 | // removed from the linkset. | ||
41 | sealed class BSLinksetCompoundInfo : BSLinksetInfo | ||
42 | { | ||
43 | public int Index; | ||
44 | public OMV.Vector3 OffsetFromRoot; | ||
45 | public OMV.Vector3 OffsetFromCenterOfMass; | ||
46 | public OMV.Quaternion OffsetRot; | ||
47 | public BSLinksetCompoundInfo(int indx, OMV.Vector3 p, OMV.Quaternion r) | ||
48 | { | ||
49 | Index = indx; | ||
50 | OffsetFromRoot = p; | ||
51 | OffsetFromCenterOfMass = p; | ||
52 | OffsetRot = r; | ||
53 | } | ||
54 | // 'centerDisplacement' is the distance from the root the the center-of-mass (Bullet 'zero' of the shape) | ||
55 | public BSLinksetCompoundInfo(int indx, BSPhysObject root, BSPhysObject child, OMV.Vector3 centerDisplacement) | ||
56 | { | ||
57 | // Each child position and rotation is given relative to the center-of-mass. | ||
58 | OMV.Quaternion invRootOrientation = OMV.Quaternion.Inverse(root.RawOrientation); | ||
59 | OMV.Vector3 displacementFromRoot = (child.RawPosition - root.RawPosition) * invRootOrientation; | ||
60 | OMV.Vector3 displacementFromCOM = displacementFromRoot - centerDisplacement; | ||
61 | OMV.Quaternion displacementRot = child.RawOrientation * invRootOrientation; | ||
62 | |||
63 | // Save relative position for recomputing child's world position after moving linkset. | ||
64 | Index = indx; | ||
65 | OffsetFromRoot = displacementFromRoot; | ||
66 | OffsetFromCenterOfMass = displacementFromCOM; | ||
67 | OffsetRot = displacementRot; | ||
68 | } | ||
69 | public override void Clear() | ||
70 | { | ||
71 | Index = 0; | ||
72 | OffsetFromRoot = OMV.Vector3.Zero; | ||
73 | OffsetFromCenterOfMass = OMV.Vector3.Zero; | ||
74 | OffsetRot = OMV.Quaternion.Identity; | ||
75 | } | ||
76 | public override string ToString() | ||
77 | { | ||
78 | StringBuilder buff = new StringBuilder(); | ||
79 | buff.Append("<i="); | ||
80 | buff.Append(Index.ToString()); | ||
81 | buff.Append(",p="); | ||
82 | buff.Append(OffsetFromRoot.ToString()); | ||
83 | buff.Append(",m="); | ||
84 | buff.Append(OffsetFromCenterOfMass.ToString()); | ||
85 | buff.Append(",r="); | ||
86 | buff.Append(OffsetRot.ToString()); | ||
87 | buff.Append(">"); | ||
88 | return buff.ToString(); | ||
89 | } | ||
90 | }; | ||
91 | |||
35 | public sealed class BSLinksetCompound : BSLinkset | 92 | public sealed class BSLinksetCompound : BSLinkset |
36 | { | 93 | { |
37 | private static string LogHeader = "[BULLETSIM LINKSET COMPOUND]"; | 94 | private static string LogHeader = "[BULLETSIM LINKSET COMPOUND]"; |
38 | 95 | ||
39 | public BSLinksetCompound(BSScene scene, BSPhysObject parent) | 96 | public BSLinksetCompound(BSScene scene, BSPhysObject parent) : base(scene, parent) |
40 | { | 97 | { |
41 | base.Initialize(scene, parent); | ||
42 | } | 98 | } |
43 | 99 | ||
44 | // For compound implimented linksets, if there are children, use compound shape for the root. | 100 | // For compound implimented linksets, if there are children, use compound shape for the root. |
45 | public override BSPhysicsShapeType PreferredPhysicalShape(BSPhysObject requestor) | 101 | public override BSPhysicsShapeType PreferredPhysicalShape(BSPhysObject requestor) |
46 | { | 102 | { |
103 | // Returning 'unknown' means we don't have a preference. | ||
47 | BSPhysicsShapeType ret = BSPhysicsShapeType.SHAPE_UNKNOWN; | 104 | BSPhysicsShapeType ret = BSPhysicsShapeType.SHAPE_UNKNOWN; |
48 | if (IsRoot(requestor) && HasAnyChildren) | 105 | if (IsRoot(requestor) && HasAnyChildren) |
49 | { | 106 | { |
@@ -55,27 +112,33 @@ public sealed class BSLinksetCompound : BSLinkset | |||
55 | 112 | ||
56 | // When physical properties are changed the linkset needs to recalculate | 113 | // When physical properties are changed the linkset needs to recalculate |
57 | // its internal properties. | 114 | // its internal properties. |
58 | // This is queued in the 'post taint' queue so the | ||
59 | // refresh will happen once after all the other taints are applied. | ||
60 | public override void Refresh(BSPhysObject requestor) | 115 | public override void Refresh(BSPhysObject requestor) |
61 | { | 116 | { |
62 | // External request for Refresh (from BSPrim) is not necessary | 117 | base.Refresh(requestor); |
63 | // InternalRefresh(requestor); | 118 | |
119 | // Something changed so do the rebuilding thing | ||
120 | // ScheduleRebuild(); | ||
64 | } | 121 | } |
65 | 122 | ||
66 | private void InternalRefresh(BSPhysObject requestor) | 123 | // Schedule a refresh to happen after all the other taint processing. |
124 | private void ScheduleRebuild(BSPhysObject requestor) | ||
67 | { | 125 | { |
68 | DetailLog("{0},BSLinksetCompound.Refresh,schedulingRefresh,requestor={1}", LinksetRoot.LocalID, requestor.LocalID); | 126 | DetailLog("{0},BSLinksetCompound.ScheduleRebuild,,rebuilding={1},hasChildren={2},actuallyScheduling={3}", |
69 | // Queue to happen after all the other taint processing | 127 | requestor.LocalID, Rebuilding, HasAnyChildren, (!Rebuilding && HasAnyChildren)); |
70 | PhysicsScene.PostTaintObject("BSLinksetCompound.Refresh", requestor.LocalID, delegate() | 128 | // When rebuilding, it is possible to set properties that would normally require a rebuild. |
129 | // If already rebuilding, don't request another rebuild. | ||
130 | // If a linkset with just a root prim (simple non-linked prim) don't bother rebuilding. | ||
131 | if (!Rebuilding && HasAnyChildren) | ||
71 | { | 132 | { |
72 | if (IsRoot(requestor) && HasAnyChildren) | 133 | PhysicsScene.PostTaintObject("BSLinksetCompound.ScheduleRebuild", LinksetRoot.LocalID, delegate() |
73 | RecomputeLinksetCompound(); | 134 | { |
74 | }); | 135 | if (HasAnyChildren) |
136 | RecomputeLinksetCompound(); | ||
137 | }); | ||
138 | } | ||
75 | } | 139 | } |
76 | 140 | ||
77 | // The object is going dynamic (physical). Do any setup necessary | 141 | // The object is going dynamic (physical). Do any setup necessary for a dynamic linkset. |
78 | // for a dynamic linkset. | ||
79 | // Only the state of the passed object can be modified. The rest of the linkset | 142 | // Only the state of the passed object can be modified. The rest of the linkset |
80 | // has not yet been fully constructed. | 143 | // has not yet been fully constructed. |
81 | // Return 'true' if any properties updated on the passed object. | 144 | // Return 'true' if any properties updated on the passed object. |
@@ -84,12 +147,22 @@ public sealed class BSLinksetCompound : BSLinkset | |||
84 | { | 147 | { |
85 | bool ret = false; | 148 | bool ret = false; |
86 | DetailLog("{0},BSLinksetCompound.MakeDynamic,call,IsRoot={1}", child.LocalID, IsRoot(child)); | 149 | DetailLog("{0},BSLinksetCompound.MakeDynamic,call,IsRoot={1}", child.LocalID, IsRoot(child)); |
87 | if (!IsRoot(child)) | 150 | if (IsRoot(child)) |
151 | { | ||
152 | // The root is going dynamic. Rebuild the linkset so parts and mass get computed properly. | ||
153 | ScheduleRebuild(LinksetRoot); | ||
154 | } | ||
155 | else | ||
88 | { | 156 | { |
89 | // Physical children are removed from the world as the shape ofthe root compound | 157 | // The origional prims are removed from the world as the shape of the root compound |
90 | // shape takes over. | 158 | // shape takes over. |
91 | BulletSimAPI.AddToCollisionFlags2(child.PhysBody.ptr, CollisionFlags.CF_NO_CONTACT_RESPONSE); | 159 | PhysicsScene.PE.AddToCollisionFlags(child.PhysBody, CollisionFlags.CF_NO_CONTACT_RESPONSE); |
92 | BulletSimAPI.ForceActivationState2(child.PhysBody.ptr, ActivationState.DISABLE_SIMULATION); | 160 | PhysicsScene.PE.ForceActivationState(child.PhysBody, ActivationState.DISABLE_SIMULATION); |
161 | // We don't want collisions from the old linkset children. | ||
162 | PhysicsScene.PE.RemoveFromCollisionFlags(child.PhysBody, CollisionFlags.BS_SUBSCRIBE_COLLISION_EVENTS); | ||
163 | |||
164 | child.PhysBody.collisionType = CollisionType.LinksetChild; | ||
165 | |||
93 | ret = true; | 166 | ret = true; |
94 | } | 167 | } |
95 | return ret; | 168 | return ret; |
@@ -104,33 +177,114 @@ public sealed class BSLinksetCompound : BSLinkset | |||
104 | { | 177 | { |
105 | bool ret = false; | 178 | bool ret = false; |
106 | DetailLog("{0},BSLinksetCompound.MakeStatic,call,IsRoot={1}", child.LocalID, IsRoot(child)); | 179 | DetailLog("{0},BSLinksetCompound.MakeStatic,call,IsRoot={1}", child.LocalID, IsRoot(child)); |
107 | if (!IsRoot(child)) | 180 | if (IsRoot(child)) |
181 | { | ||
182 | ScheduleRebuild(LinksetRoot); | ||
183 | } | ||
184 | else | ||
108 | { | 185 | { |
109 | // The non-physical children can come back to life. | 186 | // The non-physical children can come back to life. |
110 | BulletSimAPI.RemoveFromCollisionFlags2(child.PhysBody.ptr, CollisionFlags.CF_NO_CONTACT_RESPONSE); | 187 | PhysicsScene.PE.RemoveFromCollisionFlags(child.PhysBody, CollisionFlags.CF_NO_CONTACT_RESPONSE); |
111 | // Don't force activation so setting of DISABLE_SIMULATION can stay. | 188 | |
112 | BulletSimAPI.Activate2(child.PhysBody.ptr, false); | 189 | child.PhysBody.collisionType = CollisionType.LinksetChild; |
190 | |||
191 | // Don't force activation so setting of DISABLE_SIMULATION can stay if used. | ||
192 | PhysicsScene.PE.Activate(child.PhysBody, false); | ||
113 | ret = true; | 193 | ret = true; |
114 | } | 194 | } |
115 | return ret; | 195 | return ret; |
116 | } | 196 | } |
117 | 197 | ||
118 | // Called at taint-time!! | 198 | // 'physicalUpdate' is true if these changes came directly from the physics engine. Don't need to rebuild then. |
119 | public override void UpdateProperties(BSPhysObject updated) | 199 | // Called at taint-time. |
200 | public override void UpdateProperties(UpdatedProperties whichUpdated, BSPhysObject updated) | ||
120 | { | 201 | { |
121 | // Nothing to do for constraints on property updates | 202 | // The user moving a child around requires the rebuilding of the linkset compound shape |
122 | } | 203 | // One problem is this happens when a border is crossed -- the simulator implementation |
204 | // stores the position into the group which causes the move of the object | ||
205 | // but it also means all the child positions get updated. | ||
206 | // What would cause an unnecessary rebuild so we make sure the linkset is in a | ||
207 | // region before bothering to do a rebuild. | ||
208 | if (!IsRoot(updated) && PhysicsScene.TerrainManager.IsWithinKnownTerrain(LinksetRoot.RawPosition)) | ||
209 | { | ||
210 | // If a child of the linkset is updating only the position or rotation, that can be done | ||
211 | // without rebuilding the linkset. | ||
212 | // If a handle for the child can be fetch, we update the child here. If a rebuild was | ||
213 | // scheduled by someone else, the rebuild will just replace this setting. | ||
214 | |||
215 | bool updatedChild = false; | ||
216 | // Anything other than updating position or orientation usually means a physical update | ||
217 | // and that is caused by us updating the object. | ||
218 | if ((whichUpdated & ~(UpdatedProperties.Position | UpdatedProperties.Orientation)) == 0) | ||
219 | { | ||
220 | // Gather the child info. It might not be there if the linkset is in transition. | ||
221 | BSLinksetCompoundInfo lsi = updated.LinksetInfo as BSLinksetCompoundInfo; | ||
222 | if (lsi != null) | ||
223 | { | ||
224 | // Since the child moved or rotationed, it needs a new relative position within the linkset | ||
225 | BSLinksetCompoundInfo newLsi = new BSLinksetCompoundInfo(lsi.Index, LinksetRoot, updated, LinksetRoot.PositionDisplacement); | ||
226 | updated.LinksetInfo = newLsi; | ||
123 | 227 | ||
124 | // The children move around in relationship to the root. | 228 | // Find the physical instance of the child |
125 | // Just grab the current values of wherever it is right now. | 229 | if (LinksetRoot.PhysShape.HasPhysicalShape && PhysicsScene.PE.IsCompound(LinksetRoot.PhysShape)) |
126 | public override OMV.Vector3 Position(BSPhysObject member) | 230 | { |
127 | { | 231 | // It is possible that the linkset is still under construction and the child is not yet |
128 | return BulletSimAPI.GetPosition2(member.PhysBody.ptr); | 232 | // inserted into the compound shape. A rebuild of the linkset in a pre-step action will |
129 | } | 233 | // build the whole thing with the new position or rotation. |
234 | // The index must be checked because Bullet references the child array but does no validity | ||
235 | // checking of the child index passed. | ||
236 | int numLinksetChildren = PhysicsScene.PE.GetNumberOfCompoundChildren(LinksetRoot.PhysShape); | ||
237 | if (lsi.Index < numLinksetChildren) | ||
238 | { | ||
239 | BulletShape linksetChildShape = PhysicsScene.PE.GetChildShapeFromCompoundShapeIndex(LinksetRoot.PhysShape, lsi.Index); | ||
240 | if (linksetChildShape.HasPhysicalShape) | ||
241 | { | ||
242 | // Found the child shape within the compound shape | ||
243 | PhysicsScene.PE.UpdateChildTransform(LinksetRoot.PhysShape, lsi.Index, | ||
244 | newLsi.OffsetFromCenterOfMass, | ||
245 | newLsi.OffsetRot, | ||
246 | true /* shouldRecalculateLocalAabb */); | ||
247 | updatedChild = true; | ||
248 | DetailLog("{0},BSLinksetCompound.UpdateProperties,changeChildPosRot,whichUpdated={1},newLsi={2}", | ||
249 | updated.LocalID, whichUpdated, newLsi); | ||
250 | } | ||
251 | else // DEBUG DEBUG | ||
252 | { // DEBUG DEBUG | ||
253 | DetailLog("{0},BSLinksetCompound.UpdateProperties,couldNotUpdateChild,noChildShape,shape={1}", | ||
254 | updated.LocalID, linksetChildShape); | ||
255 | } // DEBUG DEBUG | ||
256 | } | ||
257 | else // DEBUG DEBUG | ||
258 | { // DEBUG DEBUG | ||
259 | // the child is not yet in the compound shape. This is non-fatal. | ||
260 | DetailLog("{0},BSLinksetCompound.UpdateProperties,couldNotUpdateChild,childNotInCompoundShape,numChildren={1},index={2}", | ||
261 | updated.LocalID, numLinksetChildren, lsi.Index); | ||
262 | } // DEBUG DEBUG | ||
263 | } | ||
264 | else // DEBUG DEBUG | ||
265 | { // DEBUG DEBUG | ||
266 | DetailLog("{0},BSLinksetCompound.UpdateProperties,couldNotUpdateChild,noBodyOrNotCompound", updated.LocalID); | ||
267 | } // DEBUG DEBUG | ||
268 | } | ||
269 | else // DEBUG DEBUG | ||
270 | { // DEBUG DEBUG | ||
271 | DetailLog("{0},BSLinksetCompound.UpdateProperties,couldNotUpdateChild,noLinkSetInfo,rootPhysShape={1}", | ||
272 | updated.LocalID, LinksetRoot.PhysShape); | ||
273 | } // DEBUG DEBUG | ||
130 | 274 | ||
131 | public override OMV.Quaternion Orientation(BSPhysObject member) | 275 | if (!updatedChild) |
132 | { | 276 | { |
133 | return BulletSimAPI.GetOrientation2(member.PhysBody.ptr); | 277 | // If couldn't do the individual child, the linkset needs a rebuild to incorporate the new child info. |
278 | // Note: there are several ways through this code that will not update the child if | ||
279 | // the linkset is being rebuilt. In this case, scheduling a rebuild is a NOOP since | ||
280 | // there will already be a rebuild scheduled. | ||
281 | DetailLog("{0},BSLinksetCompound.UpdateProperties,couldNotUpdateChild.schedulingRebuild,whichUpdated={1}", | ||
282 | updated.LocalID, whichUpdated); | ||
283 | updated.LinksetInfo = null; // setting to 'null' causes relative position to be recomputed. | ||
284 | ScheduleRebuild(updated); | ||
285 | } | ||
286 | } | ||
287 | } | ||
134 | } | 288 | } |
135 | 289 | ||
136 | // Routine called when rebuilding the body of some member of the linkset. | 290 | // Routine called when rebuilding the body of some member of the linkset. |
@@ -142,24 +296,65 @@ public sealed class BSLinksetCompound : BSLinkset | |||
142 | bool ret = false; | 296 | bool ret = false; |
143 | 297 | ||
144 | DetailLog("{0},BSLinksetCompound.RemoveBodyDependencies,refreshIfChild,rID={1},rBody={2},isRoot={3}", | 298 | DetailLog("{0},BSLinksetCompound.RemoveBodyDependencies,refreshIfChild,rID={1},rBody={2},isRoot={3}", |
145 | child.LocalID, LinksetRoot.LocalID, LinksetRoot.PhysBody.ptr.ToString("X"), IsRoot(child)); | 299 | child.LocalID, LinksetRoot.LocalID, LinksetRoot.PhysBody, IsRoot(child)); |
146 | 300 | ||
147 | if (!IsRoot(child)) | 301 | if (!IsRoot(child)) |
148 | { | 302 | { |
149 | // Cause the current shape to be freed and the new one to be built. | 303 | // Because it is a convenient time, recompute child world position and rotation based on |
150 | InternalRefresh(LinksetRoot); | 304 | // its position in the linkset. |
151 | ret = true; | 305 | RecomputeChildWorldPosition(child, true /* inTaintTime */); |
306 | child.LinksetInfo = null; | ||
152 | } | 307 | } |
153 | 308 | ||
309 | // Cannot schedule a refresh/rebuild here because this routine is called when | ||
310 | // the linkset is being rebuilt. | ||
311 | // InternalRefresh(LinksetRoot); | ||
312 | |||
154 | return ret; | 313 | return ret; |
155 | } | 314 | } |
156 | 315 | ||
157 | // Companion to RemoveBodyDependencies(). If RemoveBodyDependencies() returns 'true', | 316 | // When the linkset is built, the child shape is added to the compound shape relative to the |
158 | // this routine will restore the removed constraints. | 317 | // root shape. The linkset then moves around but this does not move the actual child |
159 | // Called at taint-time!! | 318 | // prim. The child prim's location must be recomputed based on the location of the root shape. |
160 | public override void RestoreBodyDependencies(BSPrim child) | 319 | private void RecomputeChildWorldPosition(BSPhysObject child, bool inTaintTime) |
161 | { | 320 | { |
162 | // The Refresh operation queued by RemoveBodyDependencies() will build any missing constraints. | 321 | // For the moment (20130201), disable this computation (converting the child physical addr back to |
322 | // a region address) until we have a good handle on center-of-mass offsets and what the physics | ||
323 | // engine moving a child actually means. | ||
324 | // The simulator keeps track of where children should be as the linkset moves. Setting | ||
325 | // the pos/rot here does not effect that knowledge as there is no good way for the | ||
326 | // physics engine to send the simulator an update for a child. | ||
327 | |||
328 | /* | ||
329 | BSLinksetCompoundInfo lci = child.LinksetInfo as BSLinksetCompoundInfo; | ||
330 | if (lci != null) | ||
331 | { | ||
332 | if (inTaintTime) | ||
333 | { | ||
334 | OMV.Vector3 oldPos = child.RawPosition; | ||
335 | child.ForcePosition = LinksetRoot.RawPosition + lci.OffsetFromRoot; | ||
336 | child.ForceOrientation = LinksetRoot.RawOrientation * lci.OffsetRot; | ||
337 | DetailLog("{0},BSLinksetCompound.RecomputeChildWorldPosition,oldPos={1},lci={2},newPos={3}", | ||
338 | child.LocalID, oldPos, lci, child.RawPosition); | ||
339 | } | ||
340 | else | ||
341 | { | ||
342 | // TaintedObject is not used here so the raw position is set now and not at taint-time. | ||
343 | child.Position = LinksetRoot.RawPosition + lci.OffsetFromRoot; | ||
344 | child.Orientation = LinksetRoot.RawOrientation * lci.OffsetRot; | ||
345 | } | ||
346 | } | ||
347 | else | ||
348 | { | ||
349 | // This happens when children have been added to the linkset but the linkset | ||
350 | // has not been constructed yet. So like, at taint time, adding children to a linkset | ||
351 | // and then changing properties of the children (makePhysical, for instance) | ||
352 | // but the post-print action of actually rebuilding the linkset has not yet happened. | ||
353 | // PhysicsScene.Logger.WarnFormat("{0} Restoring linkset child position failed because of no relative position computed. ID={1}", | ||
354 | // LogHeader, child.LocalID); | ||
355 | DetailLog("{0},BSLinksetCompound.recomputeChildWorldPosition,noRelativePositonInfo", child.LocalID); | ||
356 | } | ||
357 | */ | ||
163 | } | 358 | } |
164 | 359 | ||
165 | // ================================================================ | 360 | // ================================================================ |
@@ -174,24 +369,26 @@ public sealed class BSLinksetCompound : BSLinkset | |||
174 | 369 | ||
175 | DetailLog("{0},BSLinksetCompound.AddChildToLinkset,call,child={1}", LinksetRoot.LocalID, child.LocalID); | 370 | DetailLog("{0},BSLinksetCompound.AddChildToLinkset,call,child={1}", LinksetRoot.LocalID, child.LocalID); |
176 | 371 | ||
177 | // Cause constraints and assorted properties to be recomputed before the next simulation step. | 372 | // Rebuild the compound shape with the new child shape included |
178 | InternalRefresh(LinksetRoot); | 373 | ScheduleRebuild(child); |
179 | } | 374 | } |
180 | return; | 375 | return; |
181 | } | 376 | } |
182 | 377 | ||
183 | // Remove the specified child from the linkset. | 378 | // Remove the specified child from the linkset. |
184 | // Safe to call even if the child is not really in my linkset. | 379 | // Safe to call even if the child is not really in the linkset. |
185 | protected override void RemoveChildFromLinkset(BSPhysObject child) | 380 | protected override void RemoveChildFromLinkset(BSPhysObject child) |
186 | { | 381 | { |
187 | if (m_children.Remove(child)) | 382 | if (m_children.Remove(child)) |
188 | { | 383 | { |
189 | DetailLog("{0},BSLinksetCompound.RemoveChildFromLinkset,call,rID={1},rBody={2},cID={3},cBody={4}", | 384 | DetailLog("{0},BSLinksetCompound.RemoveChildFromLinkset,call,rID={1},rBody={2},cID={3},cBody={4}", |
190 | child.LocalID, | 385 | child.LocalID, |
191 | LinksetRoot.LocalID, LinksetRoot.PhysBody.ptr.ToString("X"), | 386 | LinksetRoot.LocalID, LinksetRoot.PhysBody.AddrString, |
192 | child.LocalID, child.PhysBody.ptr.ToString("X")); | 387 | child.LocalID, child.PhysBody.AddrString); |
193 | 388 | ||
194 | // Cause the child's body to be rebuilt and thus restored to normal operation | 389 | // Cause the child's body to be rebuilt and thus restored to normal operation |
390 | RecomputeChildWorldPosition(child, false); | ||
391 | child.LinksetInfo = null; | ||
195 | child.ForceBodyShapeRebuild(false); | 392 | child.ForceBodyShapeRebuild(false); |
196 | 393 | ||
197 | if (!HasAnyChildren) | 394 | if (!HasAnyChildren) |
@@ -201,8 +398,8 @@ public sealed class BSLinksetCompound : BSLinkset | |||
201 | } | 398 | } |
202 | else | 399 | else |
203 | { | 400 | { |
204 | // Schedule a rebuild of the linkset before the next simulation tick. | 401 | // Rebuild the compound shape with the child removed |
205 | InternalRefresh(LinksetRoot); | 402 | ScheduleRebuild(LinksetRoot); |
206 | } | 403 | } |
207 | } | 404 | } |
208 | return; | 405 | return; |
@@ -213,63 +410,112 @@ public sealed class BSLinksetCompound : BSLinkset | |||
213 | // Constraint linksets are rebuilt every time. | 410 | // Constraint linksets are rebuilt every time. |
214 | // Note that this works for rebuilding just the root after a linkset is taken apart. | 411 | // Note that this works for rebuilding just the root after a linkset is taken apart. |
215 | // Called at taint time!! | 412 | // Called at taint time!! |
413 | private bool disableCOM = true; // DEBUG DEBUG: disable until we get this debugged | ||
216 | private void RecomputeLinksetCompound() | 414 | private void RecomputeLinksetCompound() |
217 | { | 415 | { |
218 | // Cause the root shape to be rebuilt as a compound object with just the root in it | 416 | try |
219 | LinksetRoot.ForceBodyShapeRebuild(true); | ||
220 | |||
221 | DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,start,rBody={1},rShape={2},numChildren={3}", | ||
222 | LinksetRoot.LocalID, LinksetRoot.PhysBody, LinksetRoot.PhysShape, NumberOfChildren); | ||
223 | |||
224 | // Add a shape for each of the other children in the linkset | ||
225 | ForEachMember(delegate(BSPhysObject cPrim) | ||
226 | { | 417 | { |
227 | if (!IsRoot(cPrim)) | 418 | // Suppress rebuilding while rebuilding. (We know rebuilding is on only one thread.) |
419 | Rebuilding = true; | ||
420 | |||
421 | // Cause the root shape to be rebuilt as a compound object with just the root in it | ||
422 | LinksetRoot.ForceBodyShapeRebuild(true /* inTaintTime */); | ||
423 | |||
424 | // The center of mass for the linkset is the geometric center of the group. | ||
425 | // Compute a displacement for each component so it is relative to the center-of-mass. | ||
426 | // Bullet presumes an object's origin (relative <0,0,0>) is its center-of-mass | ||
427 | OMV.Vector3 centerOfMass; | ||
428 | OMV.Vector3 centerDisplacement = OMV.Vector3.Zero; | ||
429 | if (disableCOM) // DEBUG DEBUG | ||
430 | { // DEBUG DEBUG | ||
431 | centerOfMass = LinksetRoot.RawPosition; // DEBUG DEBUG | ||
432 | LinksetRoot.PositionDisplacement = OMV.Vector3.Zero; | ||
433 | } // DEBUG DEBUG | ||
434 | else | ||
228 | { | 435 | { |
229 | // Each child position and rotation is given relative to the root. | 436 | centerOfMass = ComputeLinksetCenterOfMass(); |
230 | OMV.Quaternion invRootOrientation = OMV.Quaternion.Inverse(LinksetRoot.RawOrientation); | 437 | // 'centerDisplacement' is the value to *add* to all the shape offsets |
231 | OMV.Vector3 displacementPos = (cPrim.RawPosition - LinksetRoot.RawPosition) * invRootOrientation; | 438 | centerDisplacement = LinksetRoot.RawPosition - centerOfMass; |
232 | OMV.Quaternion displacementRot = cPrim.RawOrientation * invRootOrientation; | 439 | |
440 | // Since we're displacing the center of the shape, we need to move the body in the world | ||
441 | LinksetRoot.PositionDisplacement = centerDisplacement; | ||
442 | |||
443 | // This causes the root prim position to be set properly based on the new PositionDisplacement | ||
444 | LinksetRoot.ForcePosition = LinksetRoot.RawPosition; | ||
445 | // Update the local transform for the root child shape so it is offset from the <0,0,0> which is COM | ||
446 | PhysicsScene.PE.UpdateChildTransform(LinksetRoot.PhysShape, 0, -centerDisplacement, OMV.Quaternion.Identity, false); | ||
447 | DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,COM,com={1},rootPos={2},centerDisp={3}", | ||
448 | LinksetRoot.LocalID, centerOfMass, LinksetRoot.RawPosition, centerDisplacement); | ||
449 | } | ||
233 | 450 | ||
234 | DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,addMemberToShape,mID={1},mShape={2},dispPos={3},dispRot={4}", | 451 | DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,start,rBody={1},rShape={2},numChildren={3}", |
235 | LinksetRoot.LocalID, cPrim.LocalID, cPrim.PhysShape, displacementPos, displacementRot); | 452 | LinksetRoot.LocalID, LinksetRoot.PhysBody, LinksetRoot.PhysShape, NumberOfChildren); |
236 | 453 | ||
237 | if (cPrim.PhysShape.isNativeShape) | 454 | // Add a shape for each of the other children in the linkset |
238 | { | 455 | int memberIndex = 1; |
239 | // Native shapes are not shared so we need to create a new one. | 456 | ForEachMember(delegate(BSPhysObject cPrim) |
240 | // A mesh or hull is created because scale is not available on a native shape. | 457 | { |
241 | // (TODO: Bullet does have a btScaledCollisionShape. Can that be used?) | 458 | if (!IsRoot(cPrim)) |
242 | BulletShape saveShape = cPrim.PhysShape; | ||
243 | cPrim.PhysShape.ptr = IntPtr.Zero; // Don't let the create free the child's shape | ||
244 | PhysicsScene.Shapes.CreateGeomMeshOrHull(cPrim, null); | ||
245 | BulletShape newShape = cPrim.PhysShape; | ||
246 | cPrim.PhysShape = saveShape; | ||
247 | BulletSimAPI.AddChildShapeToCompoundShape2(LinksetRoot.PhysShape.ptr, newShape.ptr, displacementPos, displacementRot); | ||
248 | } | ||
249 | else | ||
250 | { | 459 | { |
251 | // For the shared shapes (meshes and hulls), just use the shape in the child. | 460 | // Compute the displacement of the child from the root of the linkset. |
252 | if (PhysicsScene.Shapes.ReferenceShape(cPrim.PhysShape)) | 461 | // This info is saved in the child prim so the relationship does not |
462 | // change over time and the new child position can be computed | ||
463 | // when the linkset is being disassembled (the linkset may have moved). | ||
464 | BSLinksetCompoundInfo lci = cPrim.LinksetInfo as BSLinksetCompoundInfo; | ||
465 | if (lci == null) | ||
253 | { | 466 | { |
254 | PhysicsScene.Logger.ErrorFormat("{0} Rebuilt sharable shape when building linkset! Region={1}, primID={2}, shape={3}", | 467 | lci = new BSLinksetCompoundInfo(memberIndex, LinksetRoot, cPrim, centerDisplacement); |
255 | LogHeader, PhysicsScene.RegionName, cPrim.LocalID, cPrim.PhysShape); | 468 | cPrim.LinksetInfo = lci; |
469 | DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,creatingRelPos,lci={1}", cPrim.LocalID, lci); | ||
256 | } | 470 | } |
257 | BulletSimAPI.AddChildShapeToCompoundShape2(LinksetRoot.PhysShape.ptr, cPrim.PhysShape.ptr, displacementPos, displacementRot); | ||
258 | } | ||
259 | } | ||
260 | return false; // 'false' says to move onto the next child in the list | ||
261 | }); | ||
262 | 471 | ||
263 | // With all of the linkset packed into the root prim, it has the mass of everyone. | 472 | DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,addMemberToShape,mID={1},mShape={2},lci={3}", |
264 | float linksetMass = LinksetMass; | 473 | LinksetRoot.LocalID, cPrim.LocalID, cPrim.PhysShape, lci); |
265 | LinksetRoot.UpdatePhysicalMassProperties(linksetMass); | ||
266 | 474 | ||
267 | BulletSimAPI.RecalculateCompoundShapeLocalAabb2(LinksetRoot.PhysShape.ptr); | 475 | if (cPrim.PhysShape.isNativeShape) |
476 | { | ||
477 | // A native shape is turned into a hull collision shape because native | ||
478 | // shapes are not shared so we have to hullify it so it will be tracked | ||
479 | // and freed at the correct time. This also solves the scaling problem | ||
480 | // (native shapes scaled but hull/meshes are assumed to not be). | ||
481 | // TODO: decide of the native shape can just be used in the compound shape. | ||
482 | // Use call to CreateGeomNonSpecial(). | ||
483 | BulletShape saveShape = cPrim.PhysShape; | ||
484 | cPrim.PhysShape.Clear(); // Don't let the create free the child's shape | ||
485 | // PhysicsScene.Shapes.CreateGeomNonSpecial(true, cPrim, null); | ||
486 | PhysicsScene.Shapes.CreateGeomMeshOrHull(cPrim, null); | ||
487 | BulletShape newShape = cPrim.PhysShape; | ||
488 | cPrim.PhysShape = saveShape; | ||
489 | PhysicsScene.PE.AddChildShapeToCompoundShape(LinksetRoot.PhysShape, newShape, lci.OffsetFromCenterOfMass, lci.OffsetRot); | ||
490 | } | ||
491 | else | ||
492 | { | ||
493 | // For the shared shapes (meshes and hulls), just use the shape in the child. | ||
494 | // The reference count added here will be decremented when the compound shape | ||
495 | // is destroyed in BSShapeCollection (the child shapes are looped over and dereferenced). | ||
496 | if (PhysicsScene.Shapes.ReferenceShape(cPrim.PhysShape)) | ||
497 | { | ||
498 | PhysicsScene.Logger.ErrorFormat("{0} Rebuilt sharable shape when building linkset! Region={1}, primID={2}, shape={3}", | ||
499 | LogHeader, PhysicsScene.RegionName, cPrim.LocalID, cPrim.PhysShape); | ||
500 | } | ||
501 | PhysicsScene.PE.AddChildShapeToCompoundShape(LinksetRoot.PhysShape, cPrim.PhysShape, lci.OffsetFromCenterOfMass, lci.OffsetRot); | ||
502 | } | ||
503 | lci.Index = memberIndex; | ||
504 | memberIndex++; | ||
505 | } | ||
506 | return false; // 'false' says to move onto the next child in the list | ||
507 | }); | ||
268 | 508 | ||
269 | // DEBUG: see of inter-linkset collisions are causing problems for constraint linksets. | 509 | // With all of the linkset packed into the root prim, it has the mass of everyone. |
270 | // BulletSimAPI.SetCollisionFilterMask2(LinksetRoot.BSBody.ptr, | 510 | LinksetMass = ComputeLinksetMass(); |
271 | // (uint)CollisionFilterGroups.LinksetFilter, (uint)CollisionFilterGroups.LinksetMask); | 511 | LinksetRoot.UpdatePhysicalMassProperties(LinksetMass, true); |
512 | } | ||
513 | finally | ||
514 | { | ||
515 | Rebuilding = false; | ||
516 | } | ||
272 | 517 | ||
518 | PhysicsScene.PE.RecalculateCompoundShapeLocalAabb(LinksetRoot.PhysShape); | ||
273 | } | 519 | } |
274 | } | 520 | } |
275 | } \ No newline at end of file | 521 | } \ No newline at end of file |