aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/BulletSPlugin/BSAPIXNA.cs
diff options
context:
space:
mode:
Diffstat (limited to 'OpenSim/Region/Physics/BulletSPlugin/BSAPIXNA.cs')
-rwxr-xr-xOpenSim/Region/Physics/BulletSPlugin/BSAPIXNA.cs1622
1 files changed, 0 insertions, 1622 deletions
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSAPIXNA.cs b/OpenSim/Region/Physics/BulletSPlugin/BSAPIXNA.cs
deleted file mode 100755
index 30a7bee..0000000
--- a/OpenSim/Region/Physics/BulletSPlugin/BSAPIXNA.cs
+++ /dev/null
@@ -1,1622 +0,0 @@
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.IO;
30using System.Text;
31
32using OpenSim.Framework;
33
34using OpenMetaverse;
35
36using BulletXNA;
37using BulletXNA.LinearMath;
38using BulletXNA.BulletCollision;
39using BulletXNA.BulletDynamics;
40using BulletXNA.BulletCollision.CollisionDispatch;
41
42namespace OpenSim.Region.Physics.BulletSPlugin
43{
44public sealed class BSAPIXNA : BSAPITemplate
45{
46private sealed class BulletWorldXNA : BulletWorld
47{
48 public DiscreteDynamicsWorld world;
49 public BulletWorldXNA(uint id, BSScene physScene, DiscreteDynamicsWorld xx)
50 : base(id, physScene)
51 {
52 world = xx;
53 }
54}
55
56private sealed class BulletBodyXNA : BulletBody
57{
58 public CollisionObject body;
59 public RigidBody rigidBody { get { return RigidBody.Upcast(body); } }
60
61 public BulletBodyXNA(uint id, CollisionObject xx)
62 : base(id)
63 {
64 body = xx;
65 }
66 public override bool HasPhysicalBody
67 {
68 get { return body != null; }
69 }
70 public override void Clear()
71 {
72 body = null;
73 }
74 public override string AddrString
75 {
76 get { return "XNARigidBody"; }
77 }
78}
79
80private sealed class BulletShapeXNA : BulletShape
81{
82 public CollisionShape shape;
83 public BulletShapeXNA(CollisionShape xx, BSPhysicsShapeType typ)
84 : base()
85 {
86 shape = xx;
87 type = typ;
88 }
89 public override bool HasPhysicalShape
90 {
91 get { return shape != null; }
92 }
93 public override void Clear()
94 {
95 shape = null;
96 }
97 public override BulletShape Clone()
98 {
99 return new BulletShapeXNA(shape, type);
100 }
101 public override bool ReferenceSame(BulletShape other)
102 {
103 BulletShapeXNA otheru = other as BulletShapeXNA;
104 return (otheru != null) && (this.shape == otheru.shape);
105
106 }
107 public override string AddrString
108 {
109 get { return "XNACollisionShape"; }
110 }
111}
112private sealed class BulletConstraintXNA : BulletConstraint
113{
114 public TypedConstraint constrain;
115 public BulletConstraintXNA(TypedConstraint xx) : base()
116 {
117 constrain = xx;
118 }
119
120 public override void Clear()
121 {
122 constrain = null;
123 }
124 public override bool HasPhysicalConstraint { get { return constrain != null; } }
125
126 // Used for log messages for a unique display of the memory/object allocated to this instance
127 public override string AddrString
128 {
129 get { return "XNAConstraint"; }
130 }
131}
132
133 private static int m_collisionsThisFrame;
134 private BSScene PhysicsScene { get; set; }
135
136 public override string BulletEngineName { get { return "BulletXNA"; } }
137 public override string BulletEngineVersion { get; protected set; }
138
139 public BSAPIXNA(string paramName, BSScene physScene)
140 {
141 PhysicsScene = physScene;
142 }
143
144 /// <summary>
145 ///
146 /// </summary>
147 /// <param name="p"></param>
148 /// <param name="p_2"></param>
149 public override bool RemoveObjectFromWorld(BulletWorld pWorld, BulletBody pBody)
150 {
151 DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world;
152 RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
153 world.RemoveRigidBody(body);
154 return true;
155 }
156
157 public override bool AddConstraintToWorld(BulletWorld world, BulletConstraint constrain, bool disableCollisionsBetweenLinkedObjects)
158 {
159 /* TODO */
160 return false;
161 }
162
163 public override bool RemoveConstraintFromWorld(BulletWorld world, BulletConstraint constrain)
164 {
165 /* TODO */
166 return false;
167 }
168
169 public override void SetRestitution(BulletBody pBody, float pRestitution)
170 {
171 RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
172 body.SetRestitution(pRestitution);
173 }
174
175 public override int GetShapeType(BulletShape pShape)
176 {
177 CollisionShape shape = ((BulletShapeXNA)pShape).shape;
178 return (int)shape.GetShapeType();
179 }
180 public override void SetMargin(BulletShape pShape, float pMargin)
181 {
182 CollisionShape shape = ((BulletShapeXNA)pShape).shape;
183 shape.SetMargin(pMargin);
184 }
185
186 public override float GetMargin(BulletShape pShape)
187 {
188 CollisionShape shape = ((BulletShapeXNA)pShape).shape;
189 return shape.GetMargin();
190 }
191
192 public override void SetLocalScaling(BulletShape pShape, Vector3 pScale)
193 {
194 CollisionShape shape = ((BulletShapeXNA)pShape).shape;
195 IndexedVector3 vec = new IndexedVector3(pScale.X, pScale.Y, pScale.Z);
196 shape.SetLocalScaling(ref vec);
197
198 }
199
200 public override void SetContactProcessingThreshold(BulletBody pBody, float contactprocessingthreshold)
201 {
202 RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
203 body.SetContactProcessingThreshold(contactprocessingthreshold);
204 }
205
206 public override void SetCcdMotionThreshold(BulletBody pBody, float pccdMotionThreashold)
207 {
208 RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
209 body.SetCcdMotionThreshold(pccdMotionThreashold);
210 }
211
212 public override void SetCcdSweptSphereRadius(BulletBody pBody, float pCcdSweptSphereRadius)
213 {
214 RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
215 body.SetCcdSweptSphereRadius(pCcdSweptSphereRadius);
216 }
217
218 public override void SetAngularFactorV(BulletBody pBody, Vector3 pAngularFactor)
219 {
220 RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
221 body.SetAngularFactor(new IndexedVector3(pAngularFactor.X, pAngularFactor.Y, pAngularFactor.Z));
222 }
223
224 public override CollisionFlags AddToCollisionFlags(BulletBody pBody, CollisionFlags pcollisionFlags)
225 {
226 CollisionObject body = ((BulletBodyXNA)pBody).body;
227 CollisionFlags existingcollisionFlags = (CollisionFlags)(uint)body.GetCollisionFlags();
228 existingcollisionFlags |= pcollisionFlags;
229 body.SetCollisionFlags((BulletXNA.BulletCollision.CollisionFlags)(uint)existingcollisionFlags);
230 return (CollisionFlags) (uint) existingcollisionFlags;
231 }
232
233 public override bool AddObjectToWorld(BulletWorld pWorld, BulletBody pBody)
234 {
235 // Bullet resets several variables when an object is added to the world. In particular,
236 // BulletXNA resets position and rotation. Gravity is also reset depending on the static/dynamic
237 // type. Of course, the collision flags in the broadphase proxy are initialized to default.
238 DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world;
239 RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
240
241 IndexedMatrix origPos = body.GetWorldTransform();
242 IndexedVector3 origGrav = body.GetGravity();
243
244 //if (!(body.GetCollisionShape().GetShapeType() == BroadphaseNativeTypes.STATIC_PLANE_PROXYTYPE && body.GetCollisionShape().GetShapeType() == BroadphaseNativeTypes.TERRAIN_SHAPE_PROXYTYPE))
245
246 world.AddRigidBody(body);
247
248 body.SetWorldTransform(origPos);
249 body.SetGravity(origGrav);
250
251 pBody.ApplyCollisionMask(pWorld.physicsScene);
252
253 //if (body.GetBroadphaseHandle() != null)
254 // world.UpdateSingleAabb(body);
255 return true;
256 }
257
258 public override void ForceActivationState(BulletBody pBody, ActivationState pActivationState)
259 {
260 CollisionObject body = ((BulletBodyXNA)pBody).body;
261 body.ForceActivationState((BulletXNA.BulletCollision.ActivationState)(uint)pActivationState);
262 }
263
264 public override void UpdateSingleAabb(BulletWorld pWorld, BulletBody pBody)
265 {
266 DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world;
267 CollisionObject body = ((BulletBodyXNA)pBody).body;
268 world.UpdateSingleAabb(body);
269 }
270
271 public override void UpdateAabbs(BulletWorld world) { /* TODO */ }
272 public override bool GetForceUpdateAllAabbs(BulletWorld world) { /* TODO */ return false; }
273 public override void SetForceUpdateAllAabbs(BulletWorld world, bool force) { /* TODO */ }
274
275 public override bool SetCollisionGroupMask(BulletBody pBody, uint pGroup, uint pMask)
276 {
277 RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
278 body.GetBroadphaseHandle().m_collisionFilterGroup = (BulletXNA.BulletCollision.CollisionFilterGroups) pGroup;
279 body.GetBroadphaseHandle().m_collisionFilterGroup = (BulletXNA.BulletCollision.CollisionFilterGroups) pGroup;
280 if ((uint) body.GetBroadphaseHandle().m_collisionFilterGroup == 0)
281 return false;
282 return true;
283 }
284
285 public override void ClearAllForces(BulletBody pBody)
286 {
287 CollisionObject body = ((BulletBodyXNA)pBody).body;
288 IndexedVector3 zeroVector = new IndexedVector3(0, 0, 0);
289 body.SetInterpolationLinearVelocity(ref zeroVector);
290 body.SetInterpolationAngularVelocity(ref zeroVector);
291 IndexedMatrix bodytransform = body.GetWorldTransform();
292
293 body.SetInterpolationWorldTransform(ref bodytransform);
294
295 if (body is RigidBody)
296 {
297 RigidBody rigidbody = body as RigidBody;
298 rigidbody.SetLinearVelocity(zeroVector);
299 rigidbody.SetAngularVelocity(zeroVector);
300 rigidbody.ClearForces();
301 }
302 }
303
304 public override void SetInterpolationAngularVelocity(BulletBody pBody, Vector3 pVector3)
305 {
306 RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
307 IndexedVector3 vec = new IndexedVector3(pVector3.X, pVector3.Y, pVector3.Z);
308 body.SetInterpolationAngularVelocity(ref vec);
309 }
310
311 public override void SetAngularVelocity(BulletBody pBody, Vector3 pVector3)
312 {
313 RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
314 IndexedVector3 vec = new IndexedVector3(pVector3.X, pVector3.Y, pVector3.Z);
315 body.SetAngularVelocity(ref vec);
316 }
317 public override Vector3 GetTotalForce(BulletBody pBody)
318 {
319 RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
320 IndexedVector3 iv3 = body.GetTotalForce();
321 return new Vector3(iv3.X, iv3.Y, iv3.Z);
322 }
323 public override Vector3 GetTotalTorque(BulletBody pBody)
324 {
325 RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
326 IndexedVector3 iv3 = body.GetTotalTorque();
327 return new Vector3(iv3.X, iv3.Y, iv3.Z);
328 }
329 public override Vector3 GetInvInertiaDiagLocal(BulletBody pBody)
330 {
331 RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
332 IndexedVector3 iv3 = body.GetInvInertiaDiagLocal();
333 return new Vector3(iv3.X, iv3.Y, iv3.Z);
334 }
335 public override void SetInvInertiaDiagLocal(BulletBody pBody, Vector3 inert)
336 {
337 RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
338 IndexedVector3 iv3 = new IndexedVector3(inert.X, inert.Y, inert.Z);
339 body.SetInvInertiaDiagLocal(ref iv3);
340 }
341 public override void ApplyForce(BulletBody pBody, Vector3 force, Vector3 pos)
342 {
343 RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
344 IndexedVector3 forceiv3 = new IndexedVector3(force.X, force.Y, force.Z);
345 IndexedVector3 posiv3 = new IndexedVector3(pos.X, pos.Y, pos.Z);
346 body.ApplyForce(ref forceiv3, ref posiv3);
347 }
348 public override void ApplyImpulse(BulletBody pBody, Vector3 imp, Vector3 pos)
349 {
350 RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
351 IndexedVector3 impiv3 = new IndexedVector3(imp.X, imp.Y, imp.Z);
352 IndexedVector3 posiv3 = new IndexedVector3(pos.X, pos.Y, pos.Z);
353 body.ApplyImpulse(ref impiv3, ref posiv3);
354 }
355
356 public override void ClearForces(BulletBody pBody)
357 {
358 RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
359 body.ClearForces();
360 }
361
362 public override void SetTranslation(BulletBody pBody, Vector3 _position, Quaternion _orientation)
363 {
364 RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
365 IndexedVector3 vposition = new IndexedVector3(_position.X, _position.Y, _position.Z);
366 IndexedQuaternion vquaternion = new IndexedQuaternion(_orientation.X, _orientation.Y, _orientation.Z,
367 _orientation.W);
368 IndexedMatrix mat = IndexedMatrix.CreateFromQuaternion(vquaternion);
369 mat._origin = vposition;
370 body.SetWorldTransform(mat);
371
372 }
373
374 public override Vector3 GetPosition(BulletBody pBody)
375 {
376 RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
377 IndexedVector3 pos = body.GetInterpolationWorldTransform()._origin;
378 return new Vector3(pos.X, pos.Y, pos.Z);
379 }
380
381 public override Vector3 CalculateLocalInertia(BulletShape pShape, float pphysMass)
382 {
383 CollisionShape shape = ((BulletShapeXNA)pShape).shape;
384 IndexedVector3 inertia = IndexedVector3.Zero;
385 shape.CalculateLocalInertia(pphysMass, out inertia);
386 return new Vector3(inertia.X, inertia.Y, inertia.Z);
387 }
388
389 public override void SetMassProps(BulletBody pBody, float pphysMass, Vector3 plocalInertia)
390 {
391 RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
392 IndexedVector3 inertia = new IndexedVector3(plocalInertia.X, plocalInertia.Y, plocalInertia.Z);
393 body.SetMassProps(pphysMass, inertia);
394 }
395
396
397 public override void SetObjectForce(BulletBody pBody, Vector3 _force)
398 {
399 RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
400 IndexedVector3 force = new IndexedVector3(_force.X, _force.Y, _force.Z);
401 body.SetTotalForce(ref force);
402 }
403
404 public override void SetFriction(BulletBody pBody, float _currentFriction)
405 {
406 RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
407 body.SetFriction(_currentFriction);
408 }
409
410 public override void SetLinearVelocity(BulletBody pBody, Vector3 _velocity)
411 {
412 RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
413 IndexedVector3 velocity = new IndexedVector3(_velocity.X, _velocity.Y, _velocity.Z);
414 body.SetLinearVelocity(velocity);
415 }
416
417 public override void Activate(BulletBody pBody, bool pforceactivation)
418 {
419 RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
420 body.Activate(pforceactivation);
421
422 }
423
424 public override Quaternion GetOrientation(BulletBody pBody)
425 {
426 RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
427 IndexedQuaternion mat = body.GetInterpolationWorldTransform().GetRotation();
428 return new Quaternion(mat.X, mat.Y, mat.Z, mat.W);
429 }
430
431 public override CollisionFlags RemoveFromCollisionFlags(BulletBody pBody, CollisionFlags pcollisionFlags)
432 {
433 RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
434 CollisionFlags existingcollisionFlags = (CollisionFlags)(uint)body.GetCollisionFlags();
435 existingcollisionFlags &= ~pcollisionFlags;
436 body.SetCollisionFlags((BulletXNA.BulletCollision.CollisionFlags)(uint)existingcollisionFlags);
437 return (CollisionFlags)(uint)existingcollisionFlags;
438 }
439
440 public override float GetCcdMotionThreshold(BulletBody obj) { /* TODO */ return 0f; }
441
442 public override float GetCcdSweptSphereRadius(BulletBody obj) { /* TODO */ return 0f; }
443
444 public override IntPtr GetUserPointer(BulletBody obj) { /* TODO */ return IntPtr.Zero; }
445
446 public override void SetUserPointer(BulletBody obj, IntPtr val) { /* TODO */ }
447
448 public override void SetGravity(BulletBody pBody, Vector3 pGravity)
449 {
450 RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
451 IndexedVector3 gravity = new IndexedVector3(pGravity.X, pGravity.Y, pGravity.Z);
452 body.SetGravity(gravity);
453 }
454
455 public override bool DestroyConstraint(BulletWorld pWorld, BulletConstraint pConstraint)
456 {
457 DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world;
458 TypedConstraint constraint = ((BulletConstraintXNA)pConstraint).constrain;
459 world.RemoveConstraint(constraint);
460 return true;
461 }
462
463 public override bool SetLinearLimits(BulletConstraint pConstraint, Vector3 low, Vector3 high)
464 {
465 Generic6DofConstraint constraint = ((BulletConstraintXNA)pConstraint).constrain as Generic6DofConstraint;
466 IndexedVector3 lowlimit = new IndexedVector3(low.X, low.Y, low.Z);
467 IndexedVector3 highlimit = new IndexedVector3(high.X, high.Y, high.Z);
468 constraint.SetLinearLowerLimit(lowlimit);
469 constraint.SetLinearUpperLimit(highlimit);
470 return true;
471 }
472
473 public override bool SetAngularLimits(BulletConstraint pConstraint, Vector3 low, Vector3 high)
474 {
475 Generic6DofConstraint constraint = ((BulletConstraintXNA)pConstraint).constrain as Generic6DofConstraint;
476 IndexedVector3 lowlimit = new IndexedVector3(low.X, low.Y, low.Z);
477 IndexedVector3 highlimit = new IndexedVector3(high.X, high.Y, high.Z);
478 constraint.SetAngularLowerLimit(lowlimit);
479 constraint.SetAngularUpperLimit(highlimit);
480 return true;
481 }
482
483 public override void SetConstraintNumSolverIterations(BulletConstraint pConstraint, float cnt)
484 {
485 Generic6DofConstraint constraint = ((BulletConstraintXNA)pConstraint).constrain as Generic6DofConstraint;
486 constraint.SetOverrideNumSolverIterations((int)cnt);
487 }
488
489 public override bool CalculateTransforms(BulletConstraint pConstraint)
490 {
491 Generic6DofConstraint constraint = ((BulletConstraintXNA)pConstraint).constrain as Generic6DofConstraint;
492 constraint.CalculateTransforms();
493 return true;
494 }
495
496 public override void SetConstraintEnable(BulletConstraint pConstraint, float p_2)
497 {
498 Generic6DofConstraint constraint = ((BulletConstraintXNA)pConstraint).constrain as Generic6DofConstraint;
499 constraint.SetEnabled((p_2 == 0) ? false : true);
500 }
501
502
503 //BulletSimAPI.Create6DofConstraint(m_world.ptr, m_body1.ptr, m_body2.ptr,frame1, frame1rot,frame2, frame2rot,useLinearReferenceFrameA, disableCollisionsBetweenLinkedBodies));
504 public override BulletConstraint Create6DofConstraint(BulletWorld pWorld, BulletBody pBody1, BulletBody pBody2, Vector3 pframe1, Quaternion pframe1rot, Vector3 pframe2, Quaternion pframe2rot, bool puseLinearReferenceFrameA, bool pdisableCollisionsBetweenLinkedBodies)
505
506 {
507 DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world;
508 RigidBody body1 = ((BulletBodyXNA)pBody1).rigidBody;
509 RigidBody body2 = ((BulletBodyXNA)pBody2).rigidBody;
510 IndexedVector3 frame1v = new IndexedVector3(pframe1.X, pframe1.Y, pframe1.Z);
511 IndexedQuaternion frame1rot = new IndexedQuaternion(pframe1rot.X, pframe1rot.Y, pframe1rot.Z, pframe1rot.W);
512 IndexedMatrix frame1 = IndexedMatrix.CreateFromQuaternion(frame1rot);
513 frame1._origin = frame1v;
514
515 IndexedVector3 frame2v = new IndexedVector3(pframe2.X, pframe2.Y, pframe2.Z);
516 IndexedQuaternion frame2rot = new IndexedQuaternion(pframe2rot.X, pframe2rot.Y, pframe2rot.Z, pframe2rot.W);
517 IndexedMatrix frame2 = IndexedMatrix.CreateFromQuaternion(frame2rot);
518 frame2._origin = frame1v;
519
520 Generic6DofConstraint consttr = new Generic6DofConstraint(body1, body2, ref frame1, ref frame2,
521 puseLinearReferenceFrameA);
522 consttr.CalculateTransforms();
523 world.AddConstraint(consttr,pdisableCollisionsBetweenLinkedBodies);
524
525 return new BulletConstraintXNA(consttr);
526 }
527
528
529 /// <summary>
530 ///
531 /// </summary>
532 /// <param name="pWorld"></param>
533 /// <param name="pBody1"></param>
534 /// <param name="pBody2"></param>
535 /// <param name="pjoinPoint"></param>
536 /// <param name="puseLinearReferenceFrameA"></param>
537 /// <param name="pdisableCollisionsBetweenLinkedBodies"></param>
538 /// <returns></returns>
539 public override BulletConstraint Create6DofConstraintToPoint(BulletWorld pWorld, BulletBody pBody1, BulletBody pBody2, Vector3 pjoinPoint, bool puseLinearReferenceFrameA, bool pdisableCollisionsBetweenLinkedBodies)
540 {
541 DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world;
542 RigidBody body1 = ((BulletBodyXNA)pBody1).rigidBody;
543 RigidBody body2 = ((BulletBodyXNA)pBody2).rigidBody;
544 IndexedMatrix frame1 = new IndexedMatrix(IndexedBasisMatrix.Identity, new IndexedVector3(0, 0, 0));
545 IndexedMatrix frame2 = new IndexedMatrix(IndexedBasisMatrix.Identity, new IndexedVector3(0, 0, 0));
546
547 IndexedVector3 joinPoint = new IndexedVector3(pjoinPoint.X, pjoinPoint.Y, pjoinPoint.Z);
548 IndexedMatrix mat = IndexedMatrix.Identity;
549 mat._origin = new IndexedVector3(pjoinPoint.X, pjoinPoint.Y, pjoinPoint.Z);
550 frame1._origin = body1.GetWorldTransform().Inverse()*joinPoint;
551 frame2._origin = body2.GetWorldTransform().Inverse()*joinPoint;
552
553 Generic6DofConstraint consttr = new Generic6DofConstraint(body1, body2, ref frame1, ref frame2, puseLinearReferenceFrameA);
554 consttr.CalculateTransforms();
555 world.AddConstraint(consttr, pdisableCollisionsBetweenLinkedBodies);
556
557 return new BulletConstraintXNA(consttr);
558 }
559 //SetFrames(m_constraint.ptr, frameA, frameArot, frameB, frameBrot);
560 public override bool SetFrames(BulletConstraint pConstraint, Vector3 pframe1, Quaternion pframe1rot, Vector3 pframe2, Quaternion pframe2rot)
561 {
562 Generic6DofConstraint constraint = ((BulletConstraintXNA)pConstraint).constrain as Generic6DofConstraint;
563 IndexedVector3 frame1v = new IndexedVector3(pframe1.X, pframe1.Y, pframe1.Z);
564 IndexedQuaternion frame1rot = new IndexedQuaternion(pframe1rot.X, pframe1rot.Y, pframe1rot.Z, pframe1rot.W);
565 IndexedMatrix frame1 = IndexedMatrix.CreateFromQuaternion(frame1rot);
566 frame1._origin = frame1v;
567
568 IndexedVector3 frame2v = new IndexedVector3(pframe2.X, pframe2.Y, pframe2.Z);
569 IndexedQuaternion frame2rot = new IndexedQuaternion(pframe2rot.X, pframe2rot.Y, pframe2rot.Z, pframe2rot.W);
570 IndexedMatrix frame2 = IndexedMatrix.CreateFromQuaternion(frame2rot);
571 frame2._origin = frame1v;
572 constraint.SetFrames(ref frame1, ref frame2);
573 return true;
574 }
575
576 public override Vector3 GetLinearVelocity(BulletBody pBody)
577 {
578 RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
579 IndexedVector3 iv3 = body.GetLinearVelocity();
580 return new Vector3(iv3.X, iv3.Y, iv3.Z);
581 }
582 public override Vector3 GetAngularVelocity(BulletBody pBody)
583 {
584 RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
585 IndexedVector3 iv3 = body.GetAngularVelocity();
586 return new Vector3(iv3.X, iv3.Y, iv3.Z);
587 }
588 public override Vector3 GetVelocityInLocalPoint(BulletBody pBody, Vector3 pos)
589 {
590 RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
591 IndexedVector3 posiv3 = new IndexedVector3(pos.X, pos.Y, pos.Z);
592 IndexedVector3 iv3 = body.GetVelocityInLocalPoint(ref posiv3);
593 return new Vector3(iv3.X, iv3.Y, iv3.Z);
594 }
595 public override void Translate(BulletBody pBody, Vector3 trans)
596 {
597 RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
598 }
599 public override void UpdateDeactivation(BulletBody pBody, float timeStep)
600 {
601 RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
602 body.UpdateDeactivation(timeStep);
603 }
604
605 public override bool WantsSleeping(BulletBody pBody)
606 {
607 RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
608 return body.WantsSleeping();
609 }
610
611 public override void SetAngularFactor(BulletBody pBody, float factor)
612 {
613 RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
614 body.SetAngularFactor(factor);
615 }
616
617 public override Vector3 GetAngularFactor(BulletBody pBody)
618 {
619 RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
620 IndexedVector3 iv3 = body.GetAngularFactor();
621 return new Vector3(iv3.X, iv3.Y, iv3.Z);
622 }
623
624 public override bool IsInWorld(BulletWorld pWorld, BulletBody pBody)
625 {
626 DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world;
627 CollisionObject body = ((BulletBodyXNA)pBody).body;
628 return world.IsInWorld(body);
629 }
630
631 public override void AddConstraintRef(BulletBody pBody, BulletConstraint pConstrain)
632 {
633 RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
634 TypedConstraint constrain = ((BulletConstraintXNA)pConstrain).constrain;
635 body.AddConstraintRef(constrain);
636 }
637
638 public override void RemoveConstraintRef(BulletBody pBody, BulletConstraint pConstrain)
639 {
640 RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
641 TypedConstraint constrain = ((BulletConstraintXNA)pConstrain).constrain;
642 body.RemoveConstraintRef(constrain);
643 }
644
645 public override BulletConstraint GetConstraintRef(BulletBody pBody, int index)
646 {
647 RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
648 return new BulletConstraintXNA(body.GetConstraintRef(index));
649 }
650
651 public override int GetNumConstraintRefs(BulletBody pBody)
652 {
653 RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
654 return body.GetNumConstraintRefs();
655 }
656
657 public override void SetInterpolationLinearVelocity(BulletBody pBody, Vector3 VehicleVelocity)
658 {
659 RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
660 IndexedVector3 velocity = new IndexedVector3(VehicleVelocity.X, VehicleVelocity.Y, VehicleVelocity.Z);
661 body.SetInterpolationLinearVelocity(ref velocity);
662 }
663
664 public override bool UseFrameOffset(BulletConstraint pConstraint, float onOff)
665 {
666 Generic6DofConstraint constraint = ((BulletConstraintXNA)pConstraint).constrain as Generic6DofConstraint;
667 constraint.SetUseFrameOffset((onOff == 0) ? false : true);
668 return true;
669 }
670 //SetBreakingImpulseThreshold(m_constraint.ptr, threshold);
671 public override bool SetBreakingImpulseThreshold(BulletConstraint pConstraint, float threshold)
672 {
673 Generic6DofConstraint constraint = ((BulletConstraintXNA)pConstraint).constrain as Generic6DofConstraint;
674 constraint.SetBreakingImpulseThreshold(threshold);
675 return true;
676 }
677 //BulletSimAPI.SetAngularDamping(Prim.PhysBody.ptr, angularDamping);
678 public override void SetAngularDamping(BulletBody pBody, float angularDamping)
679 {
680 RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
681 float lineardamping = body.GetLinearDamping();
682 body.SetDamping(lineardamping, angularDamping);
683
684 }
685
686 public override void UpdateInertiaTensor(BulletBody pBody)
687 {
688 RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
689 body.UpdateInertiaTensor();
690 }
691
692 public override void RecalculateCompoundShapeLocalAabb(BulletShape pCompoundShape)
693 {
694 CompoundShape shape = ((BulletShapeXNA)pCompoundShape).shape as CompoundShape;
695 shape.RecalculateLocalAabb();
696 }
697
698 //BulletSimAPI.GetCollisionFlags(PhysBody.ptr)
699 public override CollisionFlags GetCollisionFlags(BulletBody pBody)
700 {
701 RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
702 uint flags = (uint)body.GetCollisionFlags();
703 return (CollisionFlags) flags;
704 }
705
706 public override void SetDamping(BulletBody pBody, float pLinear, float pAngular)
707 {
708 RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
709 body.SetDamping(pLinear, pAngular);
710 }
711 //PhysBody.ptr, PhysicsScene.Params.deactivationTime);
712 public override void SetDeactivationTime(BulletBody pBody, float pDeactivationTime)
713 {
714 RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
715 body.SetDeactivationTime(pDeactivationTime);
716 }
717 //SetSleepingThresholds(PhysBody.ptr, PhysicsScene.Params.linearSleepingThreshold, PhysicsScene.Params.angularSleepingThreshold);
718 public override void SetSleepingThresholds(BulletBody pBody, float plinearSleepingThreshold, float pangularSleepingThreshold)
719 {
720 RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
721 body.SetSleepingThresholds(plinearSleepingThreshold, pangularSleepingThreshold);
722 }
723
724 public override CollisionObjectTypes GetBodyType(BulletBody pBody)
725 {
726 RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
727 return (CollisionObjectTypes)(int) body.GetInternalType();
728 }
729
730 public override void ApplyGravity(BulletBody obj) { /* TODO */ }
731
732 public override Vector3 GetGravity(BulletBody obj) { /* TODO */ return Vector3.Zero; }
733
734 public override void SetLinearDamping(BulletBody obj, float lin_damping) { /* TODO */ }
735
736 public override float GetLinearDamping(BulletBody obj) { /* TODO */ return 0f; }
737
738 public override float GetAngularDamping(BulletBody obj) { /* TODO */ return 0f; }
739
740 public override float GetLinearSleepingThreshold(BulletBody obj) { /* TODO */ return 0f; }
741
742 public override void ApplyDamping(BulletBody obj, float timeStep) { /* TODO */ }
743
744 public override Vector3 GetLinearFactor(BulletBody obj) { /* TODO */ return Vector3.Zero; }
745
746 public override void SetLinearFactor(BulletBody obj, Vector3 factor) { /* TODO */ }
747
748 public override void SetCenterOfMassByPosRot(BulletBody obj, Vector3 pos, Quaternion rot) { /* TODO */ }
749
750 //BulletSimAPI.ApplyCentralForce(PhysBody.ptr, fSum);
751 public override void ApplyCentralForce(BulletBody pBody, Vector3 pfSum)
752 {
753 RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
754 IndexedVector3 fSum = new IndexedVector3(pfSum.X, pfSum.Y, pfSum.Z);
755 body.ApplyCentralForce(ref fSum);
756 }
757 public override void ApplyCentralImpulse(BulletBody pBody, Vector3 pfSum)
758 {
759 RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
760 IndexedVector3 fSum = new IndexedVector3(pfSum.X, pfSum.Y, pfSum.Z);
761 body.ApplyCentralImpulse(ref fSum);
762 }
763 public override void ApplyTorque(BulletBody pBody, Vector3 pfSum)
764 {
765 RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
766 IndexedVector3 fSum = new IndexedVector3(pfSum.X, pfSum.Y, pfSum.Z);
767 body.ApplyTorque(ref fSum);
768 }
769 public override void ApplyTorqueImpulse(BulletBody pBody, Vector3 pfSum)
770 {
771 RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
772 IndexedVector3 fSum = new IndexedVector3(pfSum.X, pfSum.Y, pfSum.Z);
773 body.ApplyTorqueImpulse(ref fSum);
774 }
775
776 public override void DumpRigidBody(BulletWorld p, BulletBody p_2)
777 {
778 //TODO:
779 }
780
781 public override void DumpCollisionShape(BulletWorld p, BulletShape p_2)
782 {
783 //TODO:
784 }
785 public override void DumpConstraint(BulletWorld world, BulletConstraint constrain)
786 {
787 //TODO:
788 }
789
790 public override void DumpActivationInfo(BulletWorld world)
791 {
792 //TODO:
793 }
794
795 public override void DumpAllInfo(BulletWorld world)
796 {
797 //TODO:
798 }
799
800 public override void DumpPhysicsStatistics(BulletWorld world)
801 {
802 //TODO:
803 }
804
805 public override void DestroyObject(BulletWorld p, BulletBody p_2)
806 {
807 //TODO:
808 }
809
810 public override void Shutdown(BulletWorld pWorld)
811 {
812 DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world;
813 world.Cleanup();
814 }
815
816 public override BulletShape DuplicateCollisionShape(BulletWorld sim, BulletShape srcShape, uint id)
817 {
818 return null;
819 }
820
821 public override bool DeleteCollisionShape(BulletWorld p, BulletShape p_2)
822 {
823 //TODO:
824 return false;
825 }
826 //(sim.ptr, shape.ptr, prim.LocalID, prim.RawPosition, prim.RawOrientation);
827
828 public override BulletBody CreateBodyFromShape(BulletWorld pWorld, BulletShape pShape, uint pLocalID, Vector3 pRawPosition, Quaternion pRawOrientation)
829 {
830 CollisionWorld world = ((BulletWorldXNA)pWorld).world;
831 IndexedMatrix mat =
832 IndexedMatrix.CreateFromQuaternion(new IndexedQuaternion(pRawOrientation.X, pRawOrientation.Y,
833 pRawOrientation.Z, pRawOrientation.W));
834 mat._origin = new IndexedVector3(pRawPosition.X, pRawPosition.Y, pRawPosition.Z);
835 CollisionShape shape = ((BulletShapeXNA)pShape).shape;
836 //UpdateSingleAabb(world, shape);
837 // TODO: Feed Update array into null
838 RigidBody body = new RigidBody(0,new SimMotionState(world,pLocalID,mat,null),shape,IndexedVector3.Zero);
839
840 body.SetUserPointer(pLocalID);
841 return new BulletBodyXNA(pLocalID, body);
842 }
843
844
845 public override BulletBody CreateBodyWithDefaultMotionState( BulletShape pShape, uint pLocalID, Vector3 pRawPosition, Quaternion pRawOrientation)
846 {
847
848 IndexedMatrix mat =
849 IndexedMatrix.CreateFromQuaternion(new IndexedQuaternion(pRawOrientation.X, pRawOrientation.Y,
850 pRawOrientation.Z, pRawOrientation.W));
851 mat._origin = new IndexedVector3(pRawPosition.X, pRawPosition.Y, pRawPosition.Z);
852
853 CollisionShape shape = ((BulletShapeXNA)pShape).shape;
854
855 // TODO: Feed Update array into null
856 RigidBody body = new RigidBody(0, new DefaultMotionState( mat, IndexedMatrix.Identity), shape, IndexedVector3.Zero);
857 body.SetWorldTransform(mat);
858 body.SetUserPointer(pLocalID);
859 return new BulletBodyXNA(pLocalID, body);
860 }
861 //(m_mapInfo.terrainBody.ptr, CollisionFlags.CF_STATIC_OBJECT);
862 public override CollisionFlags SetCollisionFlags(BulletBody pBody, CollisionFlags collisionFlags)
863 {
864 RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
865 body.SetCollisionFlags((BulletXNA.BulletCollision.CollisionFlags) (uint) collisionFlags);
866 return (CollisionFlags)body.GetCollisionFlags();
867 }
868
869 public override Vector3 GetAnisotripicFriction(BulletConstraint pconstrain) { /* TODO */ return Vector3.Zero; }
870 public override Vector3 SetAnisotripicFriction(BulletConstraint pconstrain, Vector3 frict) { /* TODO */ return Vector3.Zero; }
871 public override bool HasAnisotripicFriction(BulletConstraint pconstrain) { /* TODO */ return false; }
872 public override float GetContactProcessingThreshold(BulletBody pBody) { /* TODO */ return 0f; }
873 public override bool IsStaticObject(BulletBody pBody) { /* TODO */ return false; }
874 public override bool IsKinematicObject(BulletBody pBody) { /* TODO */ return false; }
875 public override bool IsStaticOrKinematicObject(BulletBody pBody) { /* TODO */ return false; }
876 public override bool HasContactResponse(BulletBody pBody) { /* TODO */ return false; }
877 public override int GetActivationState(BulletBody pBody) { /* TODO */ return 0; }
878 public override void SetActivationState(BulletBody pBody, int state) { /* TODO */ }
879 public override float GetDeactivationTime(BulletBody pBody) { /* TODO */ return 0f; }
880 public override bool IsActive(BulletBody pBody) { /* TODO */ return false; }
881 public override float GetRestitution(BulletBody pBody) { /* TODO */ return 0f; }
882 public override float GetFriction(BulletBody pBody) { /* TODO */ return 0f; }
883 public override void SetInterpolationVelocity(BulletBody pBody, Vector3 linearVel, Vector3 angularVel) { /* TODO */ }
884 public override float GetHitFraction(BulletBody pBody) { /* TODO */ return 0f; }
885
886 //(m_mapInfo.terrainBody.ptr, PhysicsScene.Params.terrainHitFraction);
887 public override void SetHitFraction(BulletBody pBody, float pHitFraction)
888 {
889 RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
890 body.SetHitFraction(pHitFraction);
891 }
892 //BuildCapsuleShape(physicsScene.World.ptr, 1f, 1f, prim.Scale);
893 public override BulletShape BuildCapsuleShape(BulletWorld pWorld, float pRadius, float pHeight, Vector3 pScale)
894 {
895 DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world;
896 IndexedVector3 scale = new IndexedVector3(pScale.X, pScale.Y, pScale.Z);
897 CapsuleShapeZ capsuleShapeZ = new CapsuleShapeZ(pRadius, pHeight);
898 capsuleShapeZ.SetMargin(world.WorldSettings.Params.collisionMargin);
899 capsuleShapeZ.SetLocalScaling(ref scale);
900
901 return new BulletShapeXNA(capsuleShapeZ, BSPhysicsShapeType.SHAPE_CAPSULE); ;
902 }
903
904 public override BulletWorld Initialize(Vector3 maxPosition, ConfigurationParameters parms,
905 int maxCollisions, ref CollisionDesc[] collisionArray,
906 int maxUpdates, ref EntityProperties[] updateArray
907 )
908 {
909 /* TODO */
910 return new BulletWorldXNA(1, null, null);
911 }
912
913 private static object Initialize2(Vector3 worldExtent,
914 ConfigurationParameters[] o,
915 int mMaxCollisionsPerFrame, ref List<BulletXNA.CollisionDesc> collisionArray,
916 int mMaxUpdatesPerFrame, ref List<BulletXNA.EntityProperties> updateArray,
917 object mDebugLogCallbackHandle)
918 {
919 CollisionWorld.WorldData.ParamData p = new CollisionWorld.WorldData.ParamData();
920
921 p.angularDamping = o[0].XangularDamping;
922 p.defaultFriction = o[0].defaultFriction;
923 p.defaultFriction = o[0].defaultFriction;
924 p.defaultDensity = o[0].defaultDensity;
925 p.defaultRestitution = o[0].defaultRestitution;
926 p.collisionMargin = o[0].collisionMargin;
927 p.gravity = o[0].gravity;
928
929 p.linearDamping = o[0].XlinearDamping;
930 p.angularDamping = o[0].XangularDamping;
931 p.deactivationTime = o[0].XdeactivationTime;
932 p.linearSleepingThreshold = o[0].XlinearSleepingThreshold;
933 p.angularSleepingThreshold = o[0].XangularSleepingThreshold;
934 p.ccdMotionThreshold = o[0].XccdMotionThreshold;
935 p.ccdSweptSphereRadius = o[0].XccdSweptSphereRadius;
936 p.contactProcessingThreshold = o[0].XcontactProcessingThreshold;
937
938 p.terrainImplementation = o[0].XterrainImplementation;
939 p.terrainFriction = o[0].XterrainFriction;
940
941 p.terrainHitFraction = o[0].XterrainHitFraction;
942 p.terrainRestitution = o[0].XterrainRestitution;
943 p.terrainCollisionMargin = o[0].XterrainCollisionMargin;
944
945 p.avatarFriction = o[0].XavatarFriction;
946 p.avatarStandingFriction = o[0].XavatarStandingFriction;
947 p.avatarDensity = o[0].XavatarDensity;
948 p.avatarRestitution = o[0].XavatarRestitution;
949 p.avatarCapsuleWidth = o[0].XavatarCapsuleWidth;
950 p.avatarCapsuleDepth = o[0].XavatarCapsuleDepth;
951 p.avatarCapsuleHeight = o[0].XavatarCapsuleHeight;
952 p.avatarContactProcessingThreshold = o[0].XavatarContactProcessingThreshold;
953
954 p.vehicleAngularDamping = o[0].XvehicleAngularDamping;
955
956 p.maxPersistantManifoldPoolSize = o[0].maxPersistantManifoldPoolSize;
957 p.maxCollisionAlgorithmPoolSize = o[0].maxCollisionAlgorithmPoolSize;
958 p.shouldDisableContactPoolDynamicAllocation = o[0].shouldDisableContactPoolDynamicAllocation;
959 p.shouldForceUpdateAllAabbs = o[0].shouldForceUpdateAllAabbs;
960 p.shouldRandomizeSolverOrder = o[0].shouldRandomizeSolverOrder;
961 p.shouldSplitSimulationIslands = o[0].shouldSplitSimulationIslands;
962 p.shouldEnableFrictionCaching = o[0].shouldEnableFrictionCaching;
963 p.numberOfSolverIterations = o[0].numberOfSolverIterations;
964
965 p.linksetImplementation = o[0].XlinksetImplementation;
966 p.linkConstraintUseFrameOffset = o[0].XlinkConstraintUseFrameOffset;
967 p.linkConstraintEnableTransMotor = o[0].XlinkConstraintEnableTransMotor;
968 p.linkConstraintTransMotorMaxVel = o[0].XlinkConstraintTransMotorMaxVel;
969 p.linkConstraintTransMotorMaxForce = o[0].XlinkConstraintTransMotorMaxForce;
970 p.linkConstraintERP = o[0].XlinkConstraintERP;
971 p.linkConstraintCFM = o[0].XlinkConstraintCFM;
972 p.linkConstraintSolverIterations = o[0].XlinkConstraintSolverIterations;
973 p.physicsLoggingFrames = o[0].XphysicsLoggingFrames;
974 DefaultCollisionConstructionInfo ccci = new DefaultCollisionConstructionInfo();
975
976 DefaultCollisionConfiguration cci = new DefaultCollisionConfiguration();
977 CollisionDispatcher m_dispatcher = new CollisionDispatcher(cci);
978
979
980 if (p.maxPersistantManifoldPoolSize > 0)
981 cci.m_persistentManifoldPoolSize = (int)p.maxPersistantManifoldPoolSize;
982 if (p.shouldDisableContactPoolDynamicAllocation !=0)
983 m_dispatcher.SetDispatcherFlags(DispatcherFlags.CD_DISABLE_CONTACTPOOL_DYNAMIC_ALLOCATION);
984 //if (p.maxCollisionAlgorithmPoolSize >0 )
985
986 DbvtBroadphase m_broadphase = new DbvtBroadphase();
987 //IndexedVector3 aabbMin = new IndexedVector3(0, 0, 0);
988 //IndexedVector3 aabbMax = new IndexedVector3(256, 256, 256);
989
990 //AxisSweep3Internal m_broadphase2 = new AxisSweep3Internal(ref aabbMin, ref aabbMax, Convert.ToInt32(0xfffe), 0xffff, ushort.MaxValue/2, null, true);
991 m_broadphase.GetOverlappingPairCache().SetInternalGhostPairCallback(new GhostPairCallback());
992
993 SequentialImpulseConstraintSolver m_solver = new SequentialImpulseConstraintSolver();
994
995 DiscreteDynamicsWorld world = new DiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_solver, cci);
996 world.UpdatedObjects = updateArray;
997 world.UpdatedCollisions = collisionArray;
998 world.WorldSettings.Params = p;
999 world.SetForceUpdateAllAabbs(p.shouldForceUpdateAllAabbs != 0);
1000 world.GetSolverInfo().m_solverMode = SolverMode.SOLVER_USE_WARMSTARTING | SolverMode.SOLVER_SIMD;
1001 if (p.shouldRandomizeSolverOrder != 0)
1002 world.GetSolverInfo().m_solverMode |= SolverMode.SOLVER_RANDMIZE_ORDER;
1003
1004 world.GetSimulationIslandManager().SetSplitIslands(p.shouldSplitSimulationIslands != 0);
1005 //world.GetDispatchInfo().m_enableSatConvex Not implemented in C# port
1006
1007 if (p.shouldEnableFrictionCaching != 0)
1008 world.GetSolverInfo().m_solverMode |= SolverMode.SOLVER_ENABLE_FRICTION_DIRECTION_CACHING;
1009
1010 if (p.numberOfSolverIterations > 0)
1011 world.GetSolverInfo().m_numIterations = (int) p.numberOfSolverIterations;
1012
1013
1014 world.GetSolverInfo().m_damping = world.WorldSettings.Params.linearDamping;
1015 world.GetSolverInfo().m_restitution = world.WorldSettings.Params.defaultRestitution;
1016 world.GetSolverInfo().m_globalCfm = 0.0f;
1017 world.GetSolverInfo().m_tau = 0.6f;
1018 world.GetSolverInfo().m_friction = 0.3f;
1019 world.GetSolverInfo().m_maxErrorReduction = 20f;
1020 world.GetSolverInfo().m_numIterations = 10;
1021 world.GetSolverInfo().m_erp = 0.2f;
1022 world.GetSolverInfo().m_erp2 = 0.1f;
1023 world.GetSolverInfo().m_sor = 1.0f;
1024 world.GetSolverInfo().m_splitImpulse = false;
1025 world.GetSolverInfo().m_splitImpulsePenetrationThreshold = -0.02f;
1026 world.GetSolverInfo().m_linearSlop = 0.0f;
1027 world.GetSolverInfo().m_warmstartingFactor = 0.85f;
1028 world.GetSolverInfo().m_restingContactRestitutionThreshold = 2;
1029 world.SetForceUpdateAllAabbs(true);
1030
1031
1032 world.SetGravity(new IndexedVector3(0,0,p.gravity));
1033
1034 return world;
1035 }
1036 //m_constraint.ptr, ConstraintParams.BT_CONSTRAINT_STOP_CFM, cfm, ConstraintParamAxis.AXIS_ALL
1037 public override bool SetConstraintParam(BulletConstraint pConstraint, ConstraintParams paramIndex, float paramvalue, ConstraintParamAxis axis)
1038 {
1039 Generic6DofConstraint constrain = ((BulletConstraintXNA)pConstraint).constrain as Generic6DofConstraint;
1040 if (axis == ConstraintParamAxis.AXIS_LINEAR_ALL || axis == ConstraintParamAxis.AXIS_ALL)
1041 {
1042 constrain.SetParam((BulletXNA.BulletDynamics.ConstraintParams) (int) paramIndex, paramvalue, 0);
1043 constrain.SetParam((BulletXNA.BulletDynamics.ConstraintParams) (int) paramIndex, paramvalue, 1);
1044 constrain.SetParam((BulletXNA.BulletDynamics.ConstraintParams) (int) paramIndex, paramvalue, 2);
1045 }
1046 if (axis == ConstraintParamAxis.AXIS_ANGULAR_ALL || axis == ConstraintParamAxis.AXIS_ALL)
1047 {
1048 constrain.SetParam((BulletXNA.BulletDynamics.ConstraintParams)(int)paramIndex, paramvalue, 3);
1049 constrain.SetParam((BulletXNA.BulletDynamics.ConstraintParams)(int)paramIndex, paramvalue, 4);
1050 constrain.SetParam((BulletXNA.BulletDynamics.ConstraintParams)(int)paramIndex, paramvalue, 5);
1051 }
1052 if (axis == ConstraintParamAxis.AXIS_LINEAR_ALL)
1053 {
1054 constrain.SetParam((BulletXNA.BulletDynamics.ConstraintParams)(int)paramIndex, paramvalue, (int)axis);
1055 }
1056 return true;
1057 }
1058
1059 public override bool PushUpdate(BulletBody pCollisionObject)
1060 {
1061 bool ret = false;
1062 RigidBody rb = ((BulletBodyXNA)pCollisionObject).rigidBody;
1063 if (rb != null)
1064 {
1065 SimMotionState sms = rb.GetMotionState() as SimMotionState;
1066 if (sms != null)
1067 {
1068 IndexedMatrix wt = IndexedMatrix.Identity;
1069 sms.GetWorldTransform(out wt);
1070 sms.SetWorldTransform(ref wt, true);
1071 ret = true;
1072 }
1073 }
1074 return ret;
1075
1076 }
1077
1078 public override float GetAngularMotionDisc(BulletShape pShape)
1079 {
1080 CollisionShape shape = ((BulletShapeXNA)pShape).shape;
1081 return shape.GetAngularMotionDisc();
1082 }
1083 public override float GetContactBreakingThreshold(BulletShape pShape, float defaultFactor)
1084 {
1085 CollisionShape shape = ((BulletShapeXNA)pShape).shape;
1086 return shape.GetContactBreakingThreshold(defaultFactor);
1087 }
1088 public override bool IsCompound(BulletShape pShape)
1089 {
1090 CollisionShape shape = ((BulletShapeXNA)pShape).shape;
1091 return shape.IsCompound();
1092 }
1093 public override bool IsSoftBody(BulletShape pShape)
1094 {
1095 CollisionShape shape = ((BulletShapeXNA)pShape).shape;
1096 return shape.IsSoftBody();
1097 }
1098 public override bool IsPolyhedral(BulletShape pShape)
1099 {
1100 CollisionShape shape = ((BulletShapeXNA)pShape).shape;
1101 return shape.IsPolyhedral();
1102 }
1103 public override bool IsConvex2d(BulletShape pShape)
1104 {
1105 CollisionShape shape = ((BulletShapeXNA)pShape).shape;
1106 return shape.IsConvex2d();
1107 }
1108 public override bool IsConvex(BulletShape pShape)
1109 {
1110 CollisionShape shape = ((BulletShapeXNA)pShape).shape;
1111 return shape.IsConvex();
1112 }
1113 public override bool IsNonMoving(BulletShape pShape)
1114 {
1115 CollisionShape shape = ((BulletShapeXNA)pShape).shape;
1116 return shape.IsNonMoving();
1117 }
1118 public override bool IsConcave(BulletShape pShape)
1119 {
1120 CollisionShape shape = ((BulletShapeXNA)pShape).shape;
1121 return shape.IsConcave();
1122 }
1123 public override bool IsInfinite(BulletShape pShape)
1124 {
1125 CollisionShape shape = ((BulletShapeXNA)pShape).shape;
1126 return shape.IsInfinite();
1127 }
1128 public override bool IsNativeShape(BulletShape pShape)
1129 {
1130 CollisionShape shape = ((BulletShapeXNA)pShape).shape;
1131 bool ret;
1132 switch (shape.GetShapeType())
1133 {
1134 case BroadphaseNativeTypes.BOX_SHAPE_PROXYTYPE:
1135 case BroadphaseNativeTypes.CONE_SHAPE_PROXYTYPE:
1136 case BroadphaseNativeTypes.SPHERE_SHAPE_PROXYTYPE:
1137 case BroadphaseNativeTypes.CYLINDER_SHAPE_PROXYTYPE:
1138 ret = true;
1139 break;
1140 default:
1141 ret = false;
1142 break;
1143 }
1144 return ret;
1145 }
1146
1147 public override void SetShapeCollisionMargin(BulletShape shape, float margin) { /* TODO */ }
1148
1149 //sim.ptr, shape.ptr,prim.LocalID, prim.RawPosition, prim.RawOrientation
1150 public override BulletBody CreateGhostFromShape(BulletWorld pWorld, BulletShape pShape, uint pLocalID, Vector3 pRawPosition, Quaternion pRawOrientation)
1151 {
1152 DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world;
1153 IndexedMatrix bodyTransform = new IndexedMatrix();
1154 bodyTransform._origin = new IndexedVector3(pRawPosition.X, pRawPosition.Y, pRawPosition.Z);
1155 bodyTransform.SetRotation(new IndexedQuaternion(pRawOrientation.X,pRawOrientation.Y,pRawOrientation.Z,pRawOrientation.W));
1156 GhostObject gObj = new PairCachingGhostObject();
1157 gObj.SetWorldTransform(bodyTransform);
1158 CollisionShape shape = ((BulletShapeXNA)pShape).shape;
1159 gObj.SetCollisionShape(shape);
1160 gObj.SetUserPointer(pLocalID);
1161 // TODO: Add to Special CollisionObjects!
1162 return new BulletBodyXNA(pLocalID, gObj);
1163 }
1164
1165 public override void SetCollisionShape(BulletWorld pWorld, BulletBody pObj, BulletShape pShape)
1166 {
1167 DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world;
1168 CollisionObject obj = ((BulletBodyXNA)pObj).body;
1169 CollisionShape shape = ((BulletShapeXNA)pShape).shape;
1170 obj.SetCollisionShape(shape);
1171
1172 }
1173 public override BulletShape GetCollisionShape(BulletBody obj) { /* TODO */ return null; }
1174
1175 //(PhysicsScene.World.ptr, nativeShapeData)
1176 public override BulletShape BuildNativeShape(BulletWorld pWorld, ShapeData pShapeData)
1177 {
1178 DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world;
1179 CollisionShape shape = null;
1180 switch (pShapeData.Type)
1181 {
1182 case BSPhysicsShapeType.SHAPE_BOX:
1183 shape = new BoxShape(new IndexedVector3(0.5f,0.5f,0.5f));
1184 break;
1185 case BSPhysicsShapeType.SHAPE_CONE:
1186 shape = new ConeShapeZ(0.5f, 1.0f);
1187 break;
1188 case BSPhysicsShapeType.SHAPE_CYLINDER:
1189 shape = new CylinderShapeZ(new IndexedVector3(0.5f, 0.5f, 0.5f));
1190 break;
1191 case BSPhysicsShapeType.SHAPE_SPHERE:
1192 shape = new SphereShape(0.5f);
1193 break;
1194
1195 }
1196 if (shape != null)
1197 {
1198 IndexedVector3 scaling = new IndexedVector3(pShapeData.Scale.X, pShapeData.Scale.Y, pShapeData.Scale.Z);
1199 shape.SetMargin(world.WorldSettings.Params.collisionMargin);
1200 shape.SetLocalScaling(ref scaling);
1201
1202 }
1203 return new BulletShapeXNA(shape, pShapeData.Type);
1204 }
1205 //PhysicsScene.World.ptr, false
1206 public override BulletShape CreateCompoundShape(BulletWorld pWorld, bool enableDynamicAabbTree)
1207 {
1208 return new BulletShapeXNA(new CompoundShape(enableDynamicAabbTree), BSPhysicsShapeType.SHAPE_COMPOUND);
1209 }
1210
1211 public override int GetNumberOfCompoundChildren(BulletShape pCompoundShape)
1212 {
1213 CompoundShape compoundshape = ((BulletShapeXNA)pCompoundShape).shape as CompoundShape;
1214 return compoundshape.GetNumChildShapes();
1215 }
1216 //LinksetRoot.PhysShape.ptr, newShape.ptr, displacementPos, displacementRot
1217 public override void AddChildShapeToCompoundShape(BulletShape pCShape, BulletShape paddShape, Vector3 displacementPos, Quaternion displacementRot)
1218 {
1219 IndexedMatrix relativeTransform = new IndexedMatrix();
1220 CompoundShape compoundshape = ((BulletShapeXNA)pCShape).shape as CompoundShape;
1221 CollisionShape addshape = ((BulletShapeXNA)paddShape).shape;
1222
1223 relativeTransform._origin = new IndexedVector3(displacementPos.X, displacementPos.Y, displacementPos.Z);
1224 relativeTransform.SetRotation(new IndexedQuaternion(displacementRot.X,displacementRot.Y,displacementRot.Z,displacementRot.W));
1225 compoundshape.AddChildShape(ref relativeTransform, addshape);
1226
1227 }
1228
1229 public override BulletShape RemoveChildShapeFromCompoundShapeIndex(BulletShape pCShape, int pii)
1230 {
1231 CompoundShape compoundshape = ((BulletShapeXNA)pCShape).shape as CompoundShape;
1232 CollisionShape ret = null;
1233 ret = compoundshape.GetChildShape(pii);
1234 compoundshape.RemoveChildShapeByIndex(pii);
1235 return new BulletShapeXNA(ret, BSPhysicsShapeType.SHAPE_UNKNOWN);
1236 }
1237
1238 public override BulletShape GetChildShapeFromCompoundShapeIndex(BulletShape cShape, int indx) { /* TODO */ return null; }
1239 public override void RemoveChildShapeFromCompoundShape(BulletShape cShape, BulletShape removeShape) { /* TODO */ }
1240
1241 public override BulletShape CreateGroundPlaneShape(uint pLocalId, float pheight, float pcollisionMargin)
1242 {
1243 StaticPlaneShape m_planeshape = new StaticPlaneShape(new IndexedVector3(0,0,1),(int)pheight );
1244 m_planeshape.SetMargin(pcollisionMargin);
1245 m_planeshape.SetUserPointer(pLocalId);
1246 return new BulletShapeXNA(m_planeshape, BSPhysicsShapeType.SHAPE_GROUNDPLANE);
1247 }
1248
1249 public override BulletConstraint CreateHingeConstraint(BulletWorld pWorld, BulletBody pBody1, BulletBody ppBody2, Vector3 ppivotInA, Vector3 ppivotInB, Vector3 paxisInA, Vector3 paxisInB, bool puseLinearReferenceFrameA, bool pdisableCollisionsBetweenLinkedBodies)
1250 {
1251 HingeConstraint constrain = null;
1252 DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world;
1253 RigidBody rb1 = ((BulletBodyXNA)pBody1).rigidBody;
1254 RigidBody rb2 = ((BulletBodyXNA)ppBody2).rigidBody;
1255 if (rb1 != null && rb2 != null)
1256 {
1257 IndexedVector3 pivotInA = new IndexedVector3(ppivotInA.X, ppivotInA.Y, ppivotInA.Z);
1258 IndexedVector3 pivotInB = new IndexedVector3(ppivotInB.X, ppivotInB.Y, ppivotInB.Z);
1259 IndexedVector3 axisInA = new IndexedVector3(paxisInA.X, paxisInA.Y, paxisInA.Z);
1260 IndexedVector3 axisInB = new IndexedVector3(paxisInB.X, paxisInB.Y, paxisInB.Z);
1261 world.AddConstraint(constrain, pdisableCollisionsBetweenLinkedBodies);
1262 }
1263 return new BulletConstraintXNA(constrain);
1264 }
1265
1266 public override BulletShape CreateHullShape(BulletWorld pWorld, int pHullCount, float[] pConvHulls)
1267 {
1268 DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world;
1269 CompoundShape compoundshape = new CompoundShape(false);
1270
1271 compoundshape.SetMargin(world.WorldSettings.Params.collisionMargin);
1272 int ii = 1;
1273
1274 for (int i = 0; i < pHullCount; i++)
1275 {
1276 int vertexCount = (int) pConvHulls[ii];
1277
1278 IndexedVector3 centroid = new IndexedVector3(pConvHulls[ii + 1], pConvHulls[ii + 2], pConvHulls[ii + 3]);
1279 IndexedMatrix childTrans = IndexedMatrix.Identity;
1280 childTrans._origin = centroid;
1281
1282 List<IndexedVector3> virts = new List<IndexedVector3>();
1283 int ender = ((ii + 4) + (vertexCount*3));
1284 for (int iii = ii + 4; iii < ender; iii+=3)
1285 {
1286
1287 virts.Add(new IndexedVector3(pConvHulls[iii], pConvHulls[iii + 1], pConvHulls[iii +2]));
1288 }
1289 ConvexHullShape convexShape = new ConvexHullShape(virts, vertexCount);
1290 convexShape.SetMargin(world.WorldSettings.Params.collisionMargin);
1291 compoundshape.AddChildShape(ref childTrans, convexShape);
1292 ii += (vertexCount*3 + 4);
1293 }
1294
1295 return new BulletShapeXNA(compoundshape, BSPhysicsShapeType.SHAPE_HULL);
1296 }
1297
1298 public override BulletShape BuildHullShapeFromMesh(BulletWorld world, BulletShape meshShape) { /* TODO */ return null; }
1299
1300 public override BulletShape CreateMeshShape(BulletWorld pWorld, int pIndicesCount, int[] indices, int pVerticesCount, float[] verticesAsFloats)
1301 {
1302 //DumpRaw(indices,verticesAsFloats,pIndicesCount,pVerticesCount);
1303
1304 for (int iter = 0; iter < pVerticesCount; iter++)
1305 {
1306 if (verticesAsFloats[iter] > 0 && verticesAsFloats[iter] < 0.0001) verticesAsFloats[iter] = 0;
1307 if (verticesAsFloats[iter] < 0 && verticesAsFloats[iter] > -0.0001) verticesAsFloats[iter] = 0;
1308 }
1309
1310 ObjectArray<int> indicesarr = new ObjectArray<int>(indices);
1311 ObjectArray<float> vertices = new ObjectArray<float>(verticesAsFloats);
1312 DumpRaw(indicesarr,vertices,pIndicesCount,pVerticesCount);
1313 DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world;
1314 IndexedMesh mesh = new IndexedMesh();
1315 mesh.m_indexType = PHY_ScalarType.PHY_INTEGER;
1316 mesh.m_numTriangles = pIndicesCount/3;
1317 mesh.m_numVertices = pVerticesCount;
1318 mesh.m_triangleIndexBase = indicesarr;
1319 mesh.m_vertexBase = vertices;
1320 mesh.m_vertexStride = 3;
1321 mesh.m_vertexType = PHY_ScalarType.PHY_FLOAT;
1322 mesh.m_triangleIndexStride = 3;
1323
1324 TriangleIndexVertexArray tribuilder = new TriangleIndexVertexArray();
1325 tribuilder.AddIndexedMesh(mesh, PHY_ScalarType.PHY_INTEGER);
1326 BvhTriangleMeshShape meshShape = new BvhTriangleMeshShape(tribuilder, true,true);
1327 meshShape.SetMargin(world.WorldSettings.Params.collisionMargin);
1328 // world.UpdateSingleAabb(meshShape);
1329 return new BulletShapeXNA(meshShape, BSPhysicsShapeType.SHAPE_MESH);
1330
1331 }
1332 public static void DumpRaw(ObjectArray<int>indices, ObjectArray<float> vertices, int pIndicesCount,int pVerticesCount )
1333 {
1334
1335 String fileName = "objTest3.raw";
1336 String completePath = System.IO.Path.Combine(Util.configDir(), fileName);
1337 StreamWriter sw = new StreamWriter(completePath);
1338 IndexedMesh mesh = new IndexedMesh();
1339
1340 mesh.m_indexType = PHY_ScalarType.PHY_INTEGER;
1341 mesh.m_numTriangles = pIndicesCount / 3;
1342 mesh.m_numVertices = pVerticesCount;
1343 mesh.m_triangleIndexBase = indices;
1344 mesh.m_vertexBase = vertices;
1345 mesh.m_vertexStride = 3;
1346 mesh.m_vertexType = PHY_ScalarType.PHY_FLOAT;
1347 mesh.m_triangleIndexStride = 3;
1348
1349 TriangleIndexVertexArray tribuilder = new TriangleIndexVertexArray();
1350 tribuilder.AddIndexedMesh(mesh, PHY_ScalarType.PHY_INTEGER);
1351
1352
1353
1354 for (int i = 0; i < pVerticesCount; i++)
1355 {
1356
1357 string s = vertices[indices[i * 3]].ToString("0.0000");
1358 s += " " + vertices[indices[i * 3 + 1]].ToString("0.0000");
1359 s += " " + vertices[indices[i * 3 + 2]].ToString("0.0000");
1360
1361 sw.Write(s + "\n");
1362 }
1363
1364 sw.Close();
1365 }
1366 public static void DumpRaw(int[] indices, float[] vertices, int pIndicesCount, int pVerticesCount)
1367 {
1368
1369 String fileName = "objTest6.raw";
1370 String completePath = System.IO.Path.Combine(Util.configDir(), fileName);
1371 StreamWriter sw = new StreamWriter(completePath);
1372 IndexedMesh mesh = new IndexedMesh();
1373
1374 mesh.m_indexType = PHY_ScalarType.PHY_INTEGER;
1375 mesh.m_numTriangles = pIndicesCount / 3;
1376 mesh.m_numVertices = pVerticesCount;
1377 mesh.m_triangleIndexBase = indices;
1378 mesh.m_vertexBase = vertices;
1379 mesh.m_vertexStride = 3;
1380 mesh.m_vertexType = PHY_ScalarType.PHY_FLOAT;
1381 mesh.m_triangleIndexStride = 3;
1382
1383 TriangleIndexVertexArray tribuilder = new TriangleIndexVertexArray();
1384 tribuilder.AddIndexedMesh(mesh, PHY_ScalarType.PHY_INTEGER);
1385
1386
1387 sw.WriteLine("Indices");
1388 sw.WriteLine(string.Format("int[] indices = new int[{0}];",pIndicesCount));
1389 for (int iter = 0; iter < indices.Length; iter++)
1390 {
1391 sw.WriteLine(string.Format("indices[{0}]={1};",iter,indices[iter]));
1392 }
1393 sw.WriteLine("VerticesFloats");
1394 sw.WriteLine(string.Format("float[] vertices = new float[{0}];", pVerticesCount));
1395 for (int iter = 0; iter < vertices.Length; iter++)
1396 {
1397 sw.WriteLine(string.Format("Vertices[{0}]={1};", iter, vertices[iter].ToString("0.0000")));
1398 }
1399
1400 // for (int i = 0; i < pVerticesCount; i++)
1401 // {
1402 //
1403 // string s = vertices[indices[i * 3]].ToString("0.0000");
1404 // s += " " + vertices[indices[i * 3 + 1]].ToString("0.0000");
1405 // s += " " + vertices[indices[i * 3 + 2]].ToString("0.0000");
1406 //
1407 // sw.Write(s + "\n");
1408 //}
1409
1410 sw.Close();
1411 }
1412
1413 public override BulletShape CreateTerrainShape(uint id, Vector3 size, float minHeight, float maxHeight, float[] heightMap,
1414 float scaleFactor, float collisionMargin)
1415 {
1416 const int upAxis = 2;
1417 HeightfieldTerrainShape terrainShape = new HeightfieldTerrainShape((int)size.X, (int)size.Y,
1418 heightMap, scaleFactor,
1419 minHeight, maxHeight, upAxis,
1420 false);
1421 terrainShape.SetMargin(collisionMargin + 0.5f);
1422 terrainShape.SetUseDiamondSubdivision(true);
1423 terrainShape.SetUserPointer(id);
1424 return new BulletShapeXNA(terrainShape, BSPhysicsShapeType.SHAPE_TERRAIN);
1425 }
1426
1427 public override bool TranslationalLimitMotor(BulletConstraint pConstraint, float ponOff, float targetVelocity, float maxMotorForce)
1428 {
1429 TypedConstraint tconstrain = ((BulletConstraintXNA)pConstraint).constrain;
1430 bool onOff = ponOff != 0;
1431 bool ret = false;
1432
1433 switch (tconstrain.GetConstraintType())
1434 {
1435 case TypedConstraintType.D6_CONSTRAINT_TYPE:
1436 Generic6DofConstraint constrain = tconstrain as Generic6DofConstraint;
1437 constrain.GetTranslationalLimitMotor().m_enableMotor[0] = onOff;
1438 constrain.GetTranslationalLimitMotor().m_targetVelocity[0] = targetVelocity;
1439 constrain.GetTranslationalLimitMotor().m_maxMotorForce[0] = maxMotorForce;
1440 ret = true;
1441 break;
1442 }
1443
1444
1445 return ret;
1446
1447 }
1448
1449 public override int PhysicsStep(BulletWorld world, float timeStep, int maxSubSteps, float fixedTimeStep,
1450 out int updatedEntityCount, out int collidersCount)
1451 {
1452 /* TODO */
1453 updatedEntityCount = 0;
1454 collidersCount = 0;
1455 return 1;
1456 }
1457
1458 private int PhysicsStep2(BulletWorld pWorld, float timeStep, int m_maxSubSteps, float m_fixedTimeStep,
1459 out int updatedEntityCount, out List<BulletXNA.EntityProperties> updatedEntities,
1460 out int collidersCount, out List<BulletXNA.CollisionDesc>colliders)
1461 {
1462 int epic = PhysicsStepint(pWorld, timeStep, m_maxSubSteps, m_fixedTimeStep, out updatedEntityCount, out updatedEntities,
1463 out collidersCount, out colliders);
1464 return epic;
1465 }
1466
1467 private static int PhysicsStepint(BulletWorld pWorld,float timeStep, int m_maxSubSteps, float m_fixedTimeStep, out int updatedEntityCount, out List<BulletXNA.EntityProperties> updatedEntities, out int collidersCount, out List<BulletXNA.CollisionDesc> colliders)
1468 {
1469 int numSimSteps = 0;
1470
1471
1472 //if (updatedEntities is null)
1473 // updatedEntities = new List<BulletXNA.EntityProperties>();
1474
1475 //if (colliders is null)
1476 // colliders = new List<BulletXNA.CollisionDesc>();
1477
1478
1479 if (pWorld is BulletWorldXNA)
1480 {
1481 DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world;
1482
1483 numSimSteps = world.StepSimulation(timeStep, m_maxSubSteps, m_fixedTimeStep);
1484 int updates = 0;
1485
1486 updatedEntityCount = world.UpdatedObjects.Count;
1487 updatedEntities = new List<BulletXNA.EntityProperties>(world.UpdatedObjects);
1488 updatedEntityCount = updatedEntities.Count;
1489 world.UpdatedObjects.Clear();
1490
1491
1492 collidersCount = world.UpdatedCollisions.Count;
1493 colliders = new List<BulletXNA.CollisionDesc>(world.UpdatedCollisions);
1494
1495 world.UpdatedCollisions.Clear();
1496 m_collisionsThisFrame = 0;
1497 int numManifolds = world.GetDispatcher().GetNumManifolds();
1498 for (int j = 0; j < numManifolds; j++)
1499 {
1500 PersistentManifold contactManifold = world.GetDispatcher().GetManifoldByIndexInternal(j);
1501 int numContacts = contactManifold.GetNumContacts();
1502 if (numContacts == 0)
1503 continue;
1504
1505 CollisionObject objA = contactManifold.GetBody0() as CollisionObject;
1506 CollisionObject objB = contactManifold.GetBody1() as CollisionObject;
1507
1508 ManifoldPoint manifoldPoint = contactManifold.GetContactPoint(0);
1509 IndexedVector3 contactPoint = manifoldPoint.GetPositionWorldOnB();
1510 IndexedVector3 contactNormal = -manifoldPoint.m_normalWorldOnB; // make relative to A
1511
1512 RecordCollision(world, objA, objB, contactPoint, contactNormal);
1513 m_collisionsThisFrame ++;
1514 if (m_collisionsThisFrame >= 9999999)
1515 break;
1516
1517
1518 }
1519
1520
1521 }
1522 else
1523 {
1524 //if (updatedEntities is null)
1525 updatedEntities = new List<BulletXNA.EntityProperties>();
1526 updatedEntityCount = 0;
1527 //if (colliders is null)
1528 colliders = new List<BulletXNA.CollisionDesc>();
1529 collidersCount = 0;
1530 }
1531 return numSimSteps;
1532 }
1533
1534 private static void RecordCollision(CollisionWorld world, CollisionObject objA, CollisionObject objB, IndexedVector3 contact, IndexedVector3 norm)
1535 {
1536
1537 IndexedVector3 contactNormal = norm;
1538 if ((objA.GetCollisionFlags() & BulletXNA.BulletCollision.CollisionFlags.BS_WANTS_COLLISIONS) == 0 &&
1539 (objB.GetCollisionFlags() & BulletXNA.BulletCollision.CollisionFlags.BS_WANTS_COLLISIONS) == 0)
1540 {
1541 return;
1542 }
1543 uint idA = (uint)objA.GetUserPointer();
1544 uint idB = (uint)objB.GetUserPointer();
1545 if (idA > idB)
1546 {
1547 uint temp = idA;
1548 idA = idB;
1549 idB = temp;
1550 contactNormal = -contactNormal;
1551 }
1552
1553 ulong collisionID = ((ulong) idA << 32) | idB;
1554
1555 BulletXNA.CollisionDesc cDesc = new BulletXNA.CollisionDesc()
1556 {
1557 aID = idA,
1558 bID = idB,
1559 point = contact,
1560 normal = contactNormal
1561 };
1562 world.UpdatedCollisions.Add(cDesc);
1563 m_collisionsThisFrame++;
1564
1565
1566 }
1567 private static EntityProperties GetDebugProperties(BulletWorld pWorld, BulletBody pBody)
1568 {
1569 EntityProperties ent = new EntityProperties();
1570 DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world;
1571 RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
1572 IndexedMatrix transform = body.GetWorldTransform();
1573 IndexedVector3 LinearVelocity = body.GetInterpolationLinearVelocity();
1574 IndexedVector3 AngularVelocity = body.GetInterpolationAngularVelocity();
1575 IndexedQuaternion rotation = transform.GetRotation();
1576 ent.Acceleration = Vector3.Zero;
1577 ent.ID = (uint)body.GetUserPointer();
1578 ent.Position = new Vector3(transform._origin.X,transform._origin.Y,transform._origin.Z);
1579 ent.Rotation = new Quaternion(rotation.X,rotation.Y,rotation.Z,rotation.W);
1580 ent.Velocity = new Vector3(LinearVelocity.X, LinearVelocity.Y, LinearVelocity.Z);
1581 ent.RotationalVelocity = new Vector3(AngularVelocity.X, AngularVelocity.Y, AngularVelocity.Z);
1582 return ent;
1583 }
1584
1585 public override bool UpdateParameter(BulletWorld world, uint localID, String parm, float value) { /* TODO */ return false; }
1586
1587 public override Vector3 GetLocalScaling(BulletShape pShape)
1588 {
1589 CollisionShape shape = ((BulletShapeXNA)pShape).shape;
1590 IndexedVector3 scale = shape.GetLocalScaling();
1591 return new Vector3(scale.X,scale.Y,scale.Z);
1592 }
1593
1594 public bool RayCastGround(BulletWorld pWorld, Vector3 _RayOrigin, float pRayHeight, BulletBody NotMe)
1595 {
1596 DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world;
1597 if (world != null)
1598 {
1599 if (NotMe is BulletBodyXNA && NotMe.HasPhysicalBody)
1600 {
1601 CollisionObject AvoidBody = ((BulletBodyXNA)NotMe).body;
1602
1603 IndexedVector3 rOrigin = new IndexedVector3(_RayOrigin.X, _RayOrigin.Y, _RayOrigin.Z);
1604 IndexedVector3 rEnd = new IndexedVector3(_RayOrigin.X, _RayOrigin.Y, _RayOrigin.Z - pRayHeight);
1605 using (
1606 ClosestNotMeRayResultCallback rayCallback =
1607 new ClosestNotMeRayResultCallback(rOrigin, rEnd, AvoidBody)
1608 )
1609 {
1610 world.RayTest(ref rOrigin, ref rEnd, rayCallback);
1611 if (rayCallback.HasHit())
1612 {
1613 IndexedVector3 hitLocation = rayCallback.m_hitPointWorld;
1614 }
1615 return rayCallback.HasHit();
1616 }
1617 }
1618 }
1619 return false;
1620 }
1621}
1622}
generated by cgit v1.1 at 2024-07-05 09:23:53 +0000