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