aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/BulletSPlugin/BSAPIXNA.cs
diff options
context:
space:
mode:
Diffstat (limited to '')
-rwxr-xr-xOpenSim/Region/Physics/BulletSPlugin/BSAPIXNA.cs2311
1 files changed, 2311 insertions, 0 deletions
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSAPIXNA.cs b/OpenSim/Region/Physics/BulletSPlugin/BSAPIXNA.cs
new file mode 100755
index 0000000..6fc10e9
--- /dev/null
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSAPIXNA.cs
@@ -0,0 +1,2311 @@
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.Runtime.InteropServices;
31using System.Text;
32
33using OpenSim.Framework;
34
35using OpenMetaverse;
36
37using BulletXNA;
38using BulletXNA.LinearMath;
39using BulletXNA.BulletCollision;
40using BulletXNA.BulletDynamics;
41using BulletXNA.BulletCollision.CollisionDispatch;
42
43namespace OpenSim.Region.Physics.BulletSPlugin
44{
45public sealed class BSAPIXNA : BSAPITemplate
46{
47private sealed class BulletWorldXNA : BulletWorld
48{
49 public DiscreteDynamicsWorld world;
50 public BulletWorldXNA(uint id, BSScene physScene, DiscreteDynamicsWorld xx)
51 : base(id, physScene)
52 {
53 world = xx;
54 }
55}
56
57private sealed class BulletBodyXNA : BulletBody
58{
59 public CollisionObject body;
60 public RigidBody rigidBody { get { return RigidBody.Upcast(body); } }
61
62 public BulletBodyXNA(uint id, CollisionObject xx)
63 : base(id)
64 {
65 body = xx;
66 }
67 public override bool HasPhysicalBody
68 {
69 get { return body != null; }
70 }
71 public override void Clear()
72 {
73 body = null;
74 }
75 public override string AddrString
76 {
77 get { return "XNARigidBody"; }
78 }
79}
80
81private sealed class BulletShapeXNA : BulletShape
82{
83 public CollisionShape shape;
84 public BulletShapeXNA(CollisionShape xx, BSPhysicsShapeType typ)
85 : base()
86 {
87 shape = xx;
88 type = typ;
89 }
90 public override bool HasPhysicalShape
91 {
92 get { return shape != null; }
93 }
94 public override void Clear()
95 {
96 shape = null;
97 }
98 public override BulletShape Clone()
99 {
100 return new BulletShapeXNA(shape, type);
101 }
102 public override bool ReferenceSame(BulletShape other)
103 {
104 BulletShapeXNA otheru = other as BulletShapeXNA;
105 return (otheru != null) && (this.shape == otheru.shape);
106
107 }
108 public override string AddrString
109 {
110 get { return "XNACollisionShape"; }
111 }
112}
113private sealed class BulletConstraintXNA : BulletConstraint
114{
115 public TypedConstraint constrain;
116 public BulletConstraintXNA(TypedConstraint xx) : base()
117 {
118 constrain = xx;
119 }
120
121 public override void Clear()
122 {
123 constrain = null;
124 }
125 public override bool HasPhysicalConstraint { get { return constrain != null; } }
126
127 // Used for log messages for a unique display of the memory/object allocated to this instance
128 public override string AddrString
129 {
130 get { return "XNAConstraint"; }
131 }
132}
133 internal int m_maxCollisions;
134 internal CollisionDesc[] UpdatedCollisions;
135 internal int LastCollisionDesc = 0;
136 internal int m_maxUpdatesPerFrame;
137 internal int LastEntityProperty = 0;
138
139 internal EntityProperties[] UpdatedObjects;
140 internal Dictionary<uint, GhostObject> specialCollisionObjects;
141
142 private static int m_collisionsThisFrame;
143 private BSScene PhysicsScene { get; set; }
144
145 public override string BulletEngineName { get { return "BulletXNA"; } }
146 public override string BulletEngineVersion { get; protected set; }
147
148 public BSAPIXNA(string paramName, BSScene physScene)
149 {
150 PhysicsScene = physScene;
151 }
152
153 /// <summary>
154 ///
155 /// </summary>
156 /// <param name="p"></param>
157 /// <param name="p_2"></param>
158 public override bool RemoveObjectFromWorld(BulletWorld pWorld, BulletBody pBody)
159 {
160 DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
161 RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
162 CollisionObject collisionObject = ((BulletBodyXNA)pBody).body;
163 if (body != null)
164 world.RemoveRigidBody(body);
165 else if (collisionObject != null)
166 world.RemoveCollisionObject(collisionObject);
167 else
168 return false;
169 return true;
170 }
171
172 public override bool AddConstraintToWorld(BulletWorld pWorld, BulletConstraint pConstraint, bool pDisableCollisionsBetweenLinkedObjects)
173 {
174 DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
175 TypedConstraint constraint = (pConstraint as BulletConstraintXNA).constrain;
176 world.AddConstraint(constraint, pDisableCollisionsBetweenLinkedObjects);
177
178 return true;
179
180 }
181
182 public override bool RemoveConstraintFromWorld(BulletWorld pWorld, BulletConstraint pConstraint)
183 {
184 DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
185 TypedConstraint constraint = (pConstraint as BulletConstraintXNA).constrain;
186 world.RemoveConstraint(constraint);
187 return true;
188 }
189
190 public override void SetRestitution(BulletBody pCollisionObject, float pRestitution)
191 {
192 CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).body;
193 collisionObject.SetRestitution(pRestitution);
194 }
195
196 public override int GetShapeType(BulletShape pShape)
197 {
198 CollisionShape shape = (pShape as BulletShapeXNA).shape;
199 return (int)shape.GetShapeType();
200 }
201 public override void SetMargin(BulletShape pShape, float pMargin)
202 {
203 CollisionShape shape = (pShape as BulletShapeXNA).shape;
204 shape.SetMargin(pMargin);
205 }
206
207 public override float GetMargin(BulletShape pShape)
208 {
209 CollisionShape shape = (pShape as BulletShapeXNA).shape;
210 return shape.GetMargin();
211 }
212
213 public override void SetLocalScaling(BulletShape pShape, Vector3 pScale)
214 {
215 CollisionShape shape = (pShape as BulletShapeXNA).shape;
216 IndexedVector3 vec = new IndexedVector3(pScale.X, pScale.Y, pScale.Z);
217 shape.SetLocalScaling(ref vec);
218
219 }
220
221 public override void SetContactProcessingThreshold(BulletBody pCollisionObject, float contactprocessingthreshold)
222 {
223 CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
224 collisionObject.SetContactProcessingThreshold(contactprocessingthreshold);
225 }
226
227 public override void SetCcdMotionThreshold(BulletBody pCollisionObject, float pccdMotionThreashold)
228 {
229 CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).body;
230 collisionObject.SetCcdMotionThreshold(pccdMotionThreashold);
231 }
232
233 public override void SetCcdSweptSphereRadius(BulletBody pCollisionObject, float pCcdSweptSphereRadius)
234 {
235 CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).body;
236 collisionObject.SetCcdSweptSphereRadius(pCcdSweptSphereRadius);
237 }
238
239 public override void SetAngularFactorV(BulletBody pBody, Vector3 pAngularFactor)
240 {
241 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
242 body.SetAngularFactor(new IndexedVector3(pAngularFactor.X, pAngularFactor.Y, pAngularFactor.Z));
243 }
244
245 public override CollisionFlags AddToCollisionFlags(BulletBody pCollisionObject, CollisionFlags pcollisionFlags)
246 {
247 CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).body;
248 CollisionFlags existingcollisionFlags = (CollisionFlags)(uint)collisionObject.GetCollisionFlags();
249 existingcollisionFlags |= pcollisionFlags;
250 collisionObject.SetCollisionFlags((BulletXNA.BulletCollision.CollisionFlags)(uint)existingcollisionFlags);
251 return (CollisionFlags) (uint) existingcollisionFlags;
252 }
253
254 public override bool AddObjectToWorld(BulletWorld pWorld, BulletBody pBody)
255 {
256 DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
257 CollisionObject cbody = (pBody as BulletBodyXNA).body;
258 RigidBody rbody = cbody as RigidBody;
259
260 // Bullet resets several variables when an object is added to the world. In particular,
261 // BulletXNA resets position and rotation. Gravity is also reset depending on the static/dynamic
262 // type. Of course, the collision flags in the broadphase proxy are initialized to default.
263 IndexedMatrix origPos = cbody.GetWorldTransform();
264 if (rbody != null)
265 {
266 IndexedVector3 origGrav = rbody.GetGravity();
267 world.AddRigidBody(rbody);
268 rbody.SetGravity(origGrav);
269 }
270 else
271 {
272 world.AddCollisionObject(cbody);
273 }
274 cbody.SetWorldTransform(origPos);
275
276 pBody.ApplyCollisionMask(pWorld.physicsScene);
277
278 //if (body.GetBroadphaseHandle() != null)
279 // world.UpdateSingleAabb(body);
280 return true;
281 }
282
283 public override void ForceActivationState(BulletBody pCollisionObject, ActivationState pActivationState)
284 {
285 CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).body;
286 collisionObject.ForceActivationState((BulletXNA.BulletCollision.ActivationState)(uint)pActivationState);
287 }
288
289 public override void UpdateSingleAabb(BulletWorld pWorld, BulletBody pCollisionObject)
290 {
291 DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
292 CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).body;
293 world.UpdateSingleAabb(collisionObject);
294 }
295
296 public override void UpdateAabbs(BulletWorld pWorld) {
297 DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
298 world.UpdateAabbs();
299 }
300 public override bool GetForceUpdateAllAabbs(BulletWorld pWorld) {
301 DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
302 return world.GetForceUpdateAllAabbs();
303
304 }
305 public override void SetForceUpdateAllAabbs(BulletWorld pWorld, bool pForce)
306 {
307 DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
308 world.SetForceUpdateAllAabbs(pForce);
309 }
310
311 public override bool SetCollisionGroupMask(BulletBody pCollisionObject, uint pGroup, uint pMask)
312 {
313 CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).body;
314 collisionObject.GetBroadphaseHandle().m_collisionFilterGroup = (BulletXNA.BulletCollision.CollisionFilterGroups) pGroup;
315 collisionObject.GetBroadphaseHandle().m_collisionFilterGroup = (BulletXNA.BulletCollision.CollisionFilterGroups) pGroup;
316 if ((uint) collisionObject.GetBroadphaseHandle().m_collisionFilterGroup == 0)
317 return false;
318 return true;
319 }
320
321 public override void ClearAllForces(BulletBody pCollisionObject)
322 {
323 CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).body;
324 IndexedVector3 zeroVector = new IndexedVector3(0, 0, 0);
325 collisionObject.SetInterpolationLinearVelocity(ref zeroVector);
326 collisionObject.SetInterpolationAngularVelocity(ref zeroVector);
327 IndexedMatrix bodytransform = collisionObject.GetWorldTransform();
328
329 collisionObject.SetInterpolationWorldTransform(ref bodytransform);
330
331 if (collisionObject is RigidBody)
332 {
333 RigidBody rigidbody = collisionObject as RigidBody;
334 rigidbody.SetLinearVelocity(zeroVector);
335 rigidbody.SetAngularVelocity(zeroVector);
336 rigidbody.ClearForces();
337 }
338 }
339
340 public override void SetInterpolationAngularVelocity(BulletBody pCollisionObject, Vector3 pVector3)
341 {
342 CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
343 IndexedVector3 vec = new IndexedVector3(pVector3.X, pVector3.Y, pVector3.Z);
344 collisionObject.SetInterpolationAngularVelocity(ref vec);
345 }
346
347 public override void SetAngularVelocity(BulletBody pBody, Vector3 pVector3)
348 {
349 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
350 IndexedVector3 vec = new IndexedVector3(pVector3.X, pVector3.Y, pVector3.Z);
351 body.SetAngularVelocity(ref vec);
352 }
353 public override Vector3 GetTotalForce(BulletBody pBody)
354 {
355 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
356 IndexedVector3 iv3 = body.GetTotalForce();
357 return new Vector3(iv3.X, iv3.Y, iv3.Z);
358 }
359 public override Vector3 GetTotalTorque(BulletBody pBody)
360 {
361 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
362 IndexedVector3 iv3 = body.GetTotalTorque();
363 return new Vector3(iv3.X, iv3.Y, iv3.Z);
364 }
365 public override Vector3 GetInvInertiaDiagLocal(BulletBody pBody)
366 {
367 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
368 IndexedVector3 iv3 = body.GetInvInertiaDiagLocal();
369 return new Vector3(iv3.X, iv3.Y, iv3.Z);
370 }
371 public override void SetInvInertiaDiagLocal(BulletBody pBody, Vector3 inert)
372 {
373 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
374 IndexedVector3 iv3 = new IndexedVector3(inert.X, inert.Y, inert.Z);
375 body.SetInvInertiaDiagLocal(ref iv3);
376 }
377 public override void ApplyForce(BulletBody pBody, Vector3 force, Vector3 pos)
378 {
379 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
380 IndexedVector3 forceiv3 = new IndexedVector3(force.X, force.Y, force.Z);
381 IndexedVector3 posiv3 = new IndexedVector3(pos.X, pos.Y, pos.Z);
382 body.ApplyForce(ref forceiv3, ref posiv3);
383 }
384 public override void ApplyImpulse(BulletBody pBody, Vector3 imp, Vector3 pos)
385 {
386 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
387 IndexedVector3 impiv3 = new IndexedVector3(imp.X, imp.Y, imp.Z);
388 IndexedVector3 posiv3 = new IndexedVector3(pos.X, pos.Y, pos.Z);
389 body.ApplyImpulse(ref impiv3, ref posiv3);
390 }
391
392 public override void ClearForces(BulletBody pBody)
393 {
394 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
395 body.ClearForces();
396 }
397
398 public override void SetTranslation(BulletBody pCollisionObject, Vector3 _position, Quaternion _orientation)
399 {
400 CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).body;
401 IndexedVector3 vposition = new IndexedVector3(_position.X, _position.Y, _position.Z);
402 IndexedQuaternion vquaternion = new IndexedQuaternion(_orientation.X, _orientation.Y, _orientation.Z,
403 _orientation.W);
404 IndexedMatrix mat = IndexedMatrix.CreateFromQuaternion(vquaternion);
405 mat._origin = vposition;
406 collisionObject.SetWorldTransform(mat);
407
408 }
409
410 public override Vector3 GetPosition(BulletBody pCollisionObject)
411 {
412 CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
413 IndexedVector3 pos = collisionObject.GetInterpolationWorldTransform()._origin;
414 return new Vector3(pos.X, pos.Y, pos.Z);
415 }
416
417 public override Vector3 CalculateLocalInertia(BulletShape pShape, float pphysMass)
418 {
419 CollisionShape shape = (pShape as BulletShapeXNA).shape;
420 IndexedVector3 inertia = IndexedVector3.Zero;
421 shape.CalculateLocalInertia(pphysMass, out inertia);
422 return new Vector3(inertia.X, inertia.Y, inertia.Z);
423 }
424
425 public override void SetMassProps(BulletBody pBody, float pphysMass, Vector3 plocalInertia)
426 {
427 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
428 if (body != null) // Can't set mass props on collision object.
429 {
430 IndexedVector3 inertia = new IndexedVector3(plocalInertia.X, plocalInertia.Y, plocalInertia.Z);
431 body.SetMassProps(pphysMass, inertia);
432 }
433 }
434
435
436 public override void SetObjectForce(BulletBody pBody, Vector3 _force)
437 {
438 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
439 IndexedVector3 force = new IndexedVector3(_force.X, _force.Y, _force.Z);
440 body.SetTotalForce(ref force);
441 }
442
443 public override void SetFriction(BulletBody pCollisionObject, float _currentFriction)
444 {
445 CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).body;
446 collisionObject.SetFriction(_currentFriction);
447 }
448
449 public override void SetLinearVelocity(BulletBody pBody, Vector3 _velocity)
450 {
451 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
452 IndexedVector3 velocity = new IndexedVector3(_velocity.X, _velocity.Y, _velocity.Z);
453 body.SetLinearVelocity(velocity);
454 }
455
456 public override void Activate(BulletBody pCollisionObject, bool pforceactivation)
457 {
458 CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
459 collisionObject.Activate(pforceactivation);
460
461 }
462
463 public override Quaternion GetOrientation(BulletBody pCollisionObject)
464 {
465 CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
466 IndexedQuaternion mat = collisionObject.GetInterpolationWorldTransform().GetRotation();
467 return new Quaternion(mat.X, mat.Y, mat.Z, mat.W);
468 }
469
470 public override CollisionFlags RemoveFromCollisionFlags(BulletBody pCollisionObject, CollisionFlags pcollisionFlags)
471 {
472 CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).body;
473 CollisionFlags existingcollisionFlags = (CollisionFlags)(uint)collisionObject.GetCollisionFlags();
474 existingcollisionFlags &= ~pcollisionFlags;
475 collisionObject.SetCollisionFlags((BulletXNA.BulletCollision.CollisionFlags)(uint)existingcollisionFlags);
476 return (CollisionFlags)(uint)existingcollisionFlags;
477 }
478
479 public override float GetCcdMotionThreshold(BulletBody pCollisionObject)
480 {
481 CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
482 return collisionObject.GetCcdSquareMotionThreshold();
483 }
484
485 public override float GetCcdSweptSphereRadius(BulletBody pCollisionObject)
486 {
487 CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
488 return collisionObject.GetCcdSweptSphereRadius();
489
490 }
491
492 public override IntPtr GetUserPointer(BulletBody pCollisionObject)
493 {
494 CollisionObject shape = (pCollisionObject as BulletBodyXNA).body;
495 return (IntPtr)shape.GetUserPointer();
496 }
497
498 public override void SetUserPointer(BulletBody pCollisionObject, IntPtr val)
499 {
500 CollisionObject shape = (pCollisionObject as BulletBodyXNA).body;
501 shape.SetUserPointer(val);
502 }
503
504 public override void SetGravity(BulletBody pBody, Vector3 pGravity)
505 {
506 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
507 if (body != null) // Can't set collisionobject.set gravity
508 {
509 IndexedVector3 gravity = new IndexedVector3(pGravity.X, pGravity.Y, pGravity.Z);
510 body.SetGravity(gravity);
511 }
512 }
513
514 public override bool DestroyConstraint(BulletWorld pWorld, BulletConstraint pConstraint)
515 {
516 DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
517 TypedConstraint constraint = (pConstraint as BulletConstraintXNA).constrain;
518 world.RemoveConstraint(constraint);
519 return true;
520 }
521
522 public override bool SetLinearLimits(BulletConstraint pConstraint, Vector3 low, Vector3 high)
523 {
524 Generic6DofConstraint constraint = (pConstraint as BulletConstraintXNA).constrain as Generic6DofConstraint;
525 IndexedVector3 lowlimit = new IndexedVector3(low.X, low.Y, low.Z);
526 IndexedVector3 highlimit = new IndexedVector3(high.X, high.Y, high.Z);
527 constraint.SetLinearLowerLimit(lowlimit);
528 constraint.SetLinearUpperLimit(highlimit);
529 return true;
530 }
531
532 public override bool SetAngularLimits(BulletConstraint pConstraint, Vector3 low, Vector3 high)
533 {
534 Generic6DofConstraint constraint = (pConstraint as BulletConstraintXNA).constrain as Generic6DofConstraint;
535 IndexedVector3 lowlimit = new IndexedVector3(low.X, low.Y, low.Z);
536 IndexedVector3 highlimit = new IndexedVector3(high.X, high.Y, high.Z);
537 constraint.SetAngularLowerLimit(lowlimit);
538 constraint.SetAngularUpperLimit(highlimit);
539 return true;
540 }
541
542 public override void SetConstraintNumSolverIterations(BulletConstraint pConstraint, float cnt)
543 {
544 Generic6DofConstraint constraint = (pConstraint as BulletConstraintXNA).constrain as Generic6DofConstraint;
545 constraint.SetOverrideNumSolverIterations((int)cnt);
546 }
547
548 public override bool CalculateTransforms(BulletConstraint pConstraint)
549 {
550 Generic6DofConstraint constraint = (pConstraint as BulletConstraintXNA).constrain as Generic6DofConstraint;
551 constraint.CalculateTransforms();
552 return true;
553 }
554
555 public override void SetConstraintEnable(BulletConstraint pConstraint, float p_2)
556 {
557 Generic6DofConstraint constraint = (pConstraint as BulletConstraintXNA).constrain as Generic6DofConstraint;
558 constraint.SetEnabled((p_2 == 0) ? false : true);
559 }
560
561
562 public override BulletConstraint Create6DofConstraint(BulletWorld pWorld, BulletBody pBody1, BulletBody pBody2,
563 Vector3 pframe1, Quaternion pframe1rot, Vector3 pframe2, Quaternion pframe2rot,
564 bool puseLinearReferenceFrameA, bool pdisableCollisionsBetweenLinkedBodies)
565
566 {
567 DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
568 RigidBody body1 = (pBody1 as BulletBodyXNA).rigidBody;
569 RigidBody body2 = (pBody2 as BulletBodyXNA).rigidBody;
570 IndexedVector3 frame1v = new IndexedVector3(pframe1.X, pframe1.Y, pframe1.Z);
571 IndexedQuaternion frame1rot = new IndexedQuaternion(pframe1rot.X, pframe1rot.Y, pframe1rot.Z, pframe1rot.W);
572 IndexedMatrix frame1 = IndexedMatrix.CreateFromQuaternion(frame1rot);
573 frame1._origin = frame1v;
574
575 IndexedVector3 frame2v = new IndexedVector3(pframe2.X, pframe2.Y, pframe2.Z);
576 IndexedQuaternion frame2rot = new IndexedQuaternion(pframe2rot.X, pframe2rot.Y, pframe2rot.Z, pframe2rot.W);
577 IndexedMatrix frame2 = IndexedMatrix.CreateFromQuaternion(frame2rot);
578 frame2._origin = frame1v;
579
580 Generic6DofConstraint consttr = new Generic6DofConstraint(body1, body2, ref frame1, ref frame2,
581 puseLinearReferenceFrameA);
582 consttr.CalculateTransforms();
583 world.AddConstraint(consttr,pdisableCollisionsBetweenLinkedBodies);
584
585 return new BulletConstraintXNA(consttr);
586 }
587
588 public override BulletConstraint Create6DofConstraintFixed(BulletWorld pWorld, BulletBody pBody1,
589 Vector3 pframe1, Quaternion pframe1rot,
590 bool pUseLinearReferenceFrameB, bool pdisableCollisionsBetweenLinkedBodies)
591 {
592 DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
593 RigidBody body1 = (pBody1 as BulletBodyXNA).rigidBody;
594 IndexedVector3 frame1v = new IndexedVector3(pframe1.X, pframe1.Y, pframe1.Z);
595 IndexedQuaternion frame1rot = new IndexedQuaternion(pframe1rot.X, pframe1rot.Y, pframe1rot.Z, pframe1rot.W);
596 IndexedMatrix frame1 = IndexedMatrix.CreateFromQuaternion(frame1rot);
597 frame1._origin = frame1v;
598
599 Generic6DofConstraint consttr = new Generic6DofConstraint(body1, ref frame1, pUseLinearReferenceFrameB);
600 consttr.CalculateTransforms();
601 world.AddConstraint(consttr,pdisableCollisionsBetweenLinkedBodies);
602
603 return new BulletConstraintXNA(consttr);
604 }
605
606 /// <summary>
607 ///
608 /// </summary>
609 /// <param name="pWorld"></param>
610 /// <param name="pBody1"></param>
611 /// <param name="pBody2"></param>
612 /// <param name="pjoinPoint"></param>
613 /// <param name="puseLinearReferenceFrameA"></param>
614 /// <param name="pdisableCollisionsBetweenLinkedBodies"></param>
615 /// <returns></returns>
616 public override BulletConstraint Create6DofConstraintToPoint(BulletWorld pWorld, BulletBody pBody1, BulletBody pBody2, Vector3 pjoinPoint, bool puseLinearReferenceFrameA, bool pdisableCollisionsBetweenLinkedBodies)
617 {
618 DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
619 RigidBody body1 = (pBody1 as BulletBodyXNA).rigidBody;
620 RigidBody body2 = (pBody2 as BulletBodyXNA).rigidBody;
621 IndexedMatrix frame1 = new IndexedMatrix(IndexedBasisMatrix.Identity, new IndexedVector3(0, 0, 0));
622 IndexedMatrix frame2 = new IndexedMatrix(IndexedBasisMatrix.Identity, new IndexedVector3(0, 0, 0));
623
624 IndexedVector3 joinPoint = new IndexedVector3(pjoinPoint.X, pjoinPoint.Y, pjoinPoint.Z);
625 IndexedMatrix mat = IndexedMatrix.Identity;
626 mat._origin = new IndexedVector3(pjoinPoint.X, pjoinPoint.Y, pjoinPoint.Z);
627 frame1._origin = body1.GetWorldTransform().Inverse()*joinPoint;
628 frame2._origin = body2.GetWorldTransform().Inverse()*joinPoint;
629
630 Generic6DofConstraint consttr = new Generic6DofConstraint(body1, body2, ref frame1, ref frame2, puseLinearReferenceFrameA);
631 consttr.CalculateTransforms();
632 world.AddConstraint(consttr, pdisableCollisionsBetweenLinkedBodies);
633
634 return new BulletConstraintXNA(consttr);
635 }
636 //SetFrames(m_constraint.ptr, frameA, frameArot, frameB, frameBrot);
637 public override bool SetFrames(BulletConstraint pConstraint, Vector3 pframe1, Quaternion pframe1rot, Vector3 pframe2, Quaternion pframe2rot)
638 {
639 Generic6DofConstraint constraint = (pConstraint as BulletConstraintXNA).constrain as Generic6DofConstraint;
640 IndexedVector3 frame1v = new IndexedVector3(pframe1.X, pframe1.Y, pframe1.Z);
641 IndexedQuaternion frame1rot = new IndexedQuaternion(pframe1rot.X, pframe1rot.Y, pframe1rot.Z, pframe1rot.W);
642 IndexedMatrix frame1 = IndexedMatrix.CreateFromQuaternion(frame1rot);
643 frame1._origin = frame1v;
644
645 IndexedVector3 frame2v = new IndexedVector3(pframe2.X, pframe2.Y, pframe2.Z);
646 IndexedQuaternion frame2rot = new IndexedQuaternion(pframe2rot.X, pframe2rot.Y, pframe2rot.Z, pframe2rot.W);
647 IndexedMatrix frame2 = IndexedMatrix.CreateFromQuaternion(frame2rot);
648 frame2._origin = frame1v;
649 constraint.SetFrames(ref frame1, ref frame2);
650 return true;
651 }
652
653 public override Vector3 GetLinearVelocity(BulletBody pBody)
654 {
655 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
656 IndexedVector3 iv3 = body.GetLinearVelocity();
657 return new Vector3(iv3.X, iv3.Y, iv3.Z);
658 }
659 public override Vector3 GetAngularVelocity(BulletBody pBody)
660 {
661 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
662 IndexedVector3 iv3 = body.GetAngularVelocity();
663 return new Vector3(iv3.X, iv3.Y, iv3.Z);
664 }
665 public override Vector3 GetVelocityInLocalPoint(BulletBody pBody, Vector3 pos)
666 {
667 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
668 IndexedVector3 posiv3 = new IndexedVector3(pos.X, pos.Y, pos.Z);
669 IndexedVector3 iv3 = body.GetVelocityInLocalPoint(ref posiv3);
670 return new Vector3(iv3.X, iv3.Y, iv3.Z);
671 }
672 public override void Translate(BulletBody pCollisionObject, Vector3 trans)
673 {
674 CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
675 collisionObject.Translate(new IndexedVector3(trans.X,trans.Y,trans.Z));
676 }
677 public override void UpdateDeactivation(BulletBody pBody, float timeStep)
678 {
679 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
680 body.UpdateDeactivation(timeStep);
681 }
682
683 public override bool WantsSleeping(BulletBody pBody)
684 {
685 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
686 return body.WantsSleeping();
687 }
688
689 public override void SetAngularFactor(BulletBody pBody, float factor)
690 {
691 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
692 body.SetAngularFactor(factor);
693 }
694
695 public override Vector3 GetAngularFactor(BulletBody pBody)
696 {
697 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
698 IndexedVector3 iv3 = body.GetAngularFactor();
699 return new Vector3(iv3.X, iv3.Y, iv3.Z);
700 }
701
702 public override bool IsInWorld(BulletWorld pWorld, BulletBody pCollisionObject)
703 {
704 DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
705 CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).body;
706 return world.IsInWorld(collisionObject);
707 }
708
709 public override void AddConstraintRef(BulletBody pBody, BulletConstraint pConstraint)
710 {
711 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
712 TypedConstraint constrain = (pConstraint as BulletConstraintXNA).constrain;
713 body.AddConstraintRef(constrain);
714 }
715
716 public override void RemoveConstraintRef(BulletBody pBody, BulletConstraint pConstraint)
717 {
718 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
719 TypedConstraint constrain = (pConstraint as BulletConstraintXNA).constrain;
720 body.RemoveConstraintRef(constrain);
721 }
722
723 public override BulletConstraint GetConstraintRef(BulletBody pBody, int index)
724 {
725 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
726 return new BulletConstraintXNA(body.GetConstraintRef(index));
727 }
728
729 public override int GetNumConstraintRefs(BulletBody pBody)
730 {
731 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
732 return body.GetNumConstraintRefs();
733 }
734
735 public override void SetInterpolationLinearVelocity(BulletBody pCollisionObject, Vector3 VehicleVelocity)
736 {
737 CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
738 IndexedVector3 velocity = new IndexedVector3(VehicleVelocity.X, VehicleVelocity.Y, VehicleVelocity.Z);
739 collisionObject.SetInterpolationLinearVelocity(ref velocity);
740 }
741
742 public override bool UseFrameOffset(BulletConstraint pConstraint, float onOff)
743 {
744 Generic6DofConstraint constraint = (pConstraint as BulletConstraintXNA).constrain as Generic6DofConstraint;
745 constraint.SetUseFrameOffset((onOff == 0) ? false : true);
746 return true;
747 }
748 //SetBreakingImpulseThreshold(m_constraint.ptr, threshold);
749 public override bool SetBreakingImpulseThreshold(BulletConstraint pConstraint, float threshold)
750 {
751 Generic6DofConstraint constraint = (pConstraint as BulletConstraintXNA).constrain as Generic6DofConstraint;
752 constraint.SetBreakingImpulseThreshold(threshold);
753 return true;
754 }
755 //BulletSimAPI.SetAngularDamping(Prim.PhysBody.ptr, angularDamping);
756 public override void SetAngularDamping(BulletBody pBody, float angularDamping)
757 {
758 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
759 float lineardamping = body.GetLinearDamping();
760 body.SetDamping(lineardamping, angularDamping);
761
762 }
763
764 public override void UpdateInertiaTensor(BulletBody pBody)
765 {
766 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
767 if (body != null) // can't update inertia tensor on CollisionObject
768 body.UpdateInertiaTensor();
769 }
770
771 public override void RecalculateCompoundShapeLocalAabb(BulletShape pCompoundShape)
772 {
773 CompoundShape shape = (pCompoundShape as BulletShapeXNA).shape as CompoundShape;
774 shape.RecalculateLocalAabb();
775 }
776
777 //BulletSimAPI.GetCollisionFlags(PhysBody.ptr)
778 public override CollisionFlags GetCollisionFlags(BulletBody pCollisionObject)
779 {
780 CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
781 uint flags = (uint)collisionObject.GetCollisionFlags();
782 return (CollisionFlags) flags;
783 }
784
785 public override void SetDamping(BulletBody pBody, float pLinear, float pAngular)
786 {
787 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
788 body.SetDamping(pLinear, pAngular);
789 }
790 //PhysBody.ptr, PhysicsScene.Params.deactivationTime);
791 public override void SetDeactivationTime(BulletBody pCollisionObject, float pDeactivationTime)
792 {
793 CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
794 collisionObject.SetDeactivationTime(pDeactivationTime);
795 }
796 //SetSleepingThresholds(PhysBody.ptr, PhysicsScene.Params.linearSleepingThreshold, PhysicsScene.Params.angularSleepingThreshold);
797 public override void SetSleepingThresholds(BulletBody pBody, float plinearSleepingThreshold, float pangularSleepingThreshold)
798 {
799 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
800 body.SetSleepingThresholds(plinearSleepingThreshold, pangularSleepingThreshold);
801 }
802
803 public override CollisionObjectTypes GetBodyType(BulletBody pCollisionObject)
804 {
805 CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).body;
806 return (CollisionObjectTypes)(int) collisionObject.GetInternalType();
807 }
808
809 public override void ApplyGravity(BulletBody pBody)
810 {
811
812 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
813 body.ApplyGravity();
814 }
815
816 public override Vector3 GetGravity(BulletBody pBody)
817 {
818 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
819 IndexedVector3 gravity = body.GetGravity();
820 return new Vector3(gravity.X, gravity.Y, gravity.Z);
821 }
822
823 public override void SetLinearDamping(BulletBody pBody, float lin_damping)
824 {
825 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
826 float angularDamping = body.GetAngularDamping();
827 body.SetDamping(lin_damping, angularDamping);
828 }
829
830 public override float GetLinearDamping(BulletBody pBody)
831 {
832 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
833 return body.GetLinearDamping();
834 }
835
836 public override float GetAngularDamping(BulletBody pBody)
837 {
838 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
839 return body.GetAngularDamping();
840 }
841
842 public override float GetLinearSleepingThreshold(BulletBody pBody)
843 {
844 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
845 return body.GetLinearSleepingThreshold();
846 }
847
848 public override void ApplyDamping(BulletBody pBody, float timeStep)
849 {
850 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
851 body.ApplyDamping(timeStep);
852 }
853
854 public override Vector3 GetLinearFactor(BulletBody pBody)
855 {
856 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
857 IndexedVector3 linearFactor = body.GetLinearFactor();
858 return new Vector3(linearFactor.X, linearFactor.Y, linearFactor.Z);
859 }
860
861 public override void SetLinearFactor(BulletBody pBody, Vector3 factor)
862 {
863 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
864 body.SetLinearFactor(new IndexedVector3(factor.X, factor.Y, factor.Z));
865 }
866
867 public override void SetCenterOfMassByPosRot(BulletBody pBody, Vector3 pos, Quaternion rot)
868 {
869 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
870 IndexedQuaternion quat = new IndexedQuaternion(rot.X, rot.Y, rot.Z,rot.W);
871 IndexedMatrix mat = IndexedMatrix.CreateFromQuaternion(quat);
872 mat._origin = new IndexedVector3(pos.X, pos.Y, pos.Z);
873 body.SetCenterOfMassTransform( ref mat);
874 /* TODO: double check this */
875 }
876
877 //BulletSimAPI.ApplyCentralForce(PhysBody.ptr, fSum);
878 public override void ApplyCentralForce(BulletBody pBody, Vector3 pfSum)
879 {
880 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
881 IndexedVector3 fSum = new IndexedVector3(pfSum.X, pfSum.Y, pfSum.Z);
882 body.ApplyCentralForce(ref fSum);
883 }
884 public override void ApplyCentralImpulse(BulletBody pBody, Vector3 pfSum)
885 {
886 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
887 IndexedVector3 fSum = new IndexedVector3(pfSum.X, pfSum.Y, pfSum.Z);
888 body.ApplyCentralImpulse(ref fSum);
889 }
890 public override void ApplyTorque(BulletBody pBody, Vector3 pfSum)
891 {
892 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
893 IndexedVector3 fSum = new IndexedVector3(pfSum.X, pfSum.Y, pfSum.Z);
894 body.ApplyTorque(ref fSum);
895 }
896 public override void ApplyTorqueImpulse(BulletBody pBody, Vector3 pfSum)
897 {
898 RigidBody body = (pBody as BulletBodyXNA).rigidBody;
899 IndexedVector3 fSum = new IndexedVector3(pfSum.X, pfSum.Y, pfSum.Z);
900 body.ApplyTorqueImpulse(ref fSum);
901 }
902
903 public override void DestroyObject(BulletWorld pWorld, BulletBody pBody)
904 {
905 DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
906 CollisionObject co = (pBody as BulletBodyXNA).rigidBody;
907 RigidBody bo = co as RigidBody;
908 if (bo == null)
909 {
910
911 if (world.IsInWorld(co))
912 {
913 world.RemoveCollisionObject(co);
914 }
915 }
916 else
917 {
918
919 if (world.IsInWorld(bo))
920 {
921 world.RemoveRigidBody(bo);
922 }
923 }
924 if (co != null)
925 {
926 if (co.GetUserPointer() != null)
927 {
928 uint localId = (uint) co.GetUserPointer();
929 if (specialCollisionObjects.ContainsKey(localId))
930 {
931 specialCollisionObjects.Remove(localId);
932 }
933 }
934 }
935
936 }
937
938 public override void Shutdown(BulletWorld pWorld)
939 {
940 DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
941 world.Cleanup();
942 }
943
944 public override BulletShape DuplicateCollisionShape(BulletWorld pWorld, BulletShape pShape, uint id)
945 {
946 CollisionShape shape1 = (pShape as BulletShapeXNA).shape;
947
948 // TODO: Turn this from a reference copy to a Value Copy.
949 BulletShapeXNA shape2 = new BulletShapeXNA(shape1, BSShapeTypeFromBroadPhaseNativeType(shape1.GetShapeType()));
950
951 return shape2;
952 }
953
954 public override bool DeleteCollisionShape(BulletWorld pWorld, BulletShape pShape)
955 {
956 //TODO:
957 return false;
958 }
959 //(sim.ptr, shape.ptr, prim.LocalID, prim.RawPosition, prim.RawOrientation);
960
961 public override BulletBody CreateBodyFromShape(BulletWorld pWorld, BulletShape pShape, uint pLocalID, Vector3 pRawPosition, Quaternion pRawOrientation)
962 {
963 CollisionWorld world = (pWorld as BulletWorldXNA).world;
964 IndexedMatrix mat =
965 IndexedMatrix.CreateFromQuaternion(new IndexedQuaternion(pRawOrientation.X, pRawOrientation.Y,
966 pRawOrientation.Z, pRawOrientation.W));
967 mat._origin = new IndexedVector3(pRawPosition.X, pRawPosition.Y, pRawPosition.Z);
968 CollisionShape shape = (pShape as BulletShapeXNA).shape;
969 //UpdateSingleAabb(world, shape);
970 // TODO: Feed Update array into null
971 SimMotionState motionState = new SimMotionState(this, pLocalID, mat, null);
972 RigidBody body = new RigidBody(0,motionState,shape,IndexedVector3.Zero);
973 RigidBodyConstructionInfo constructionInfo = new RigidBodyConstructionInfo(0, motionState, shape, IndexedVector3.Zero)
974 {
975 m_mass = 0
976 };
977 /*
978 m_mass = mass;
979 m_motionState =motionState;
980 m_collisionShape = collisionShape;
981 m_localInertia = localInertia;
982 m_linearDamping = 0f;
983 m_angularDamping = 0f;
984 m_friction = 0.5f;
985 m_restitution = 0f;
986 m_linearSleepingThreshold = 0.8f;
987 m_angularSleepingThreshold = 1f;
988 m_additionalDamping = false;
989 m_additionalDampingFactor = 0.005f;
990 m_additionalLinearDampingThresholdSqr = 0.01f;
991 m_additionalAngularDampingThresholdSqr = 0.01f;
992 m_additionalAngularDampingFactor = 0.01f;
993 m_startWorldTransform = IndexedMatrix.Identity;
994 */
995 body.SetUserPointer(pLocalID);
996
997 return new BulletBodyXNA(pLocalID, body);
998 }
999
1000
1001 public override BulletBody CreateBodyWithDefaultMotionState( BulletShape pShape, uint pLocalID, Vector3 pRawPosition, Quaternion pRawOrientation)
1002 {
1003
1004 IndexedMatrix mat =
1005 IndexedMatrix.CreateFromQuaternion(new IndexedQuaternion(pRawOrientation.X, pRawOrientation.Y,
1006 pRawOrientation.Z, pRawOrientation.W));
1007 mat._origin = new IndexedVector3(pRawPosition.X, pRawPosition.Y, pRawPosition.Z);
1008
1009 CollisionShape shape = (pShape as BulletShapeXNA).shape;
1010
1011 // TODO: Feed Update array into null
1012 RigidBody body = new RigidBody(0, new DefaultMotionState( mat, IndexedMatrix.Identity), shape, IndexedVector3.Zero);
1013 body.SetWorldTransform(mat);
1014 body.SetUserPointer(pLocalID);
1015 return new BulletBodyXNA(pLocalID, body);
1016 }
1017 //(m_mapInfo.terrainBody.ptr, CollisionFlags.CF_STATIC_OBJECT);
1018 public override CollisionFlags SetCollisionFlags(BulletBody pCollisionObject, CollisionFlags collisionFlags)
1019 {
1020 CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
1021 collisionObject.SetCollisionFlags((BulletXNA.BulletCollision.CollisionFlags) (uint) collisionFlags);
1022 return (CollisionFlags)collisionObject.GetCollisionFlags();
1023 }
1024
1025 public override Vector3 GetAnisotripicFriction(BulletConstraint pconstrain)
1026 {
1027
1028 /* TODO */
1029 return Vector3.Zero;
1030 }
1031 public override Vector3 SetAnisotripicFriction(BulletConstraint pconstrain, Vector3 frict) { /* TODO */ return Vector3.Zero; }
1032 public override bool HasAnisotripicFriction(BulletConstraint pconstrain) { /* TODO */ return false; }
1033 public override float GetContactProcessingThreshold(BulletBody pBody) { /* TODO */ return 0f; }
1034 public override bool IsStaticObject(BulletBody pCollisionObject)
1035 {
1036 CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
1037 return collisionObject.IsStaticObject();
1038
1039 }
1040 public override bool IsKinematicObject(BulletBody pCollisionObject)
1041 {
1042 CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
1043 return collisionObject.IsKinematicObject();
1044 }
1045 public override bool IsStaticOrKinematicObject(BulletBody pCollisionObject)
1046 {
1047 CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
1048 return collisionObject.IsStaticOrKinematicObject();
1049 }
1050 public override bool HasContactResponse(BulletBody pCollisionObject)
1051 {
1052 CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
1053 return collisionObject.HasContactResponse();
1054 }
1055 public override int GetActivationState(BulletBody pBody) { /* TODO */ return 0; }
1056 public override void SetActivationState(BulletBody pBody, int state) { /* TODO */ }
1057 public override float GetDeactivationTime(BulletBody pBody) { /* TODO */ return 0f; }
1058 public override bool IsActive(BulletBody pBody) { /* TODO */ return false; }
1059 public override float GetRestitution(BulletBody pBody) { /* TODO */ return 0f; }
1060 public override float GetFriction(BulletBody pBody) { /* TODO */ return 0f; }
1061 public override void SetInterpolationVelocity(BulletBody pBody, Vector3 linearVel, Vector3 angularVel) { /* TODO */ }
1062 public override float GetHitFraction(BulletBody pBody) { /* TODO */ return 0f; }
1063
1064 //(m_mapInfo.terrainBody.ptr, PhysicsScene.Params.terrainHitFraction);
1065 public override void SetHitFraction(BulletBody pCollisionObject, float pHitFraction)
1066 {
1067 CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
1068 collisionObject.SetHitFraction(pHitFraction);
1069 }
1070 //BuildCapsuleShape(physicsScene.World.ptr, 1f, 1f, prim.Scale);
1071 public override BulletShape BuildCapsuleShape(BulletWorld pWorld, float pRadius, float pHeight, Vector3 pScale)
1072 {
1073 DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
1074 IndexedVector3 scale = new IndexedVector3(pScale.X, pScale.Y, pScale.Z);
1075 CapsuleShapeZ capsuleShapeZ = new CapsuleShapeZ(pRadius, pHeight);
1076 capsuleShapeZ.SetMargin(world.WorldSettings.Params.collisionMargin);
1077 capsuleShapeZ.SetLocalScaling(ref scale);
1078
1079 return new BulletShapeXNA(capsuleShapeZ, BSPhysicsShapeType.SHAPE_CAPSULE); ;
1080 }
1081
1082 public override BulletWorld Initialize(Vector3 maxPosition, ConfigurationParameters parms,
1083 int maxCollisions, ref CollisionDesc[] collisionArray,
1084 int maxUpdates, ref EntityProperties[] updateArray
1085 )
1086 {
1087
1088 UpdatedObjects = updateArray;
1089 UpdatedCollisions = collisionArray;
1090 /* TODO */
1091 ConfigurationParameters[] configparms = new ConfigurationParameters[1];
1092 configparms[0] = parms;
1093 Vector3 worldExtent = new Vector3(Constants.RegionSize, Constants.RegionSize, Constants.RegionHeight);
1094 m_maxCollisions = maxCollisions;
1095 m_maxUpdatesPerFrame = maxUpdates;
1096 specialCollisionObjects = new Dictionary<uint, GhostObject>();
1097
1098 return new BulletWorldXNA(1, PhysicsScene, BSAPIXNA.Initialize2(worldExtent, configparms, maxCollisions, ref collisionArray, maxUpdates, ref updateArray, null));
1099 }
1100
1101 private static DiscreteDynamicsWorld Initialize2(Vector3 worldExtent,
1102 ConfigurationParameters[] o,
1103 int mMaxCollisionsPerFrame, ref CollisionDesc[] collisionArray,
1104 int mMaxUpdatesPerFrame, ref EntityProperties[] updateArray,
1105 object mDebugLogCallbackHandle)
1106 {
1107 CollisionWorld.WorldData.ParamData p = new CollisionWorld.WorldData.ParamData();
1108
1109 p.angularDamping = BSParam.AngularDamping;
1110 p.defaultFriction = o[0].defaultFriction;
1111 p.defaultFriction = o[0].defaultFriction;
1112 p.defaultDensity = o[0].defaultDensity;
1113 p.defaultRestitution = o[0].defaultRestitution;
1114 p.collisionMargin = o[0].collisionMargin;
1115 p.gravity = o[0].gravity;
1116
1117 p.linearDamping = BSParam.LinearDamping;
1118 p.angularDamping = BSParam.AngularDamping;
1119 p.deactivationTime = BSParam.DeactivationTime;
1120 p.linearSleepingThreshold = BSParam.LinearSleepingThreshold;
1121 p.angularSleepingThreshold = BSParam.AngularSleepingThreshold;
1122 p.ccdMotionThreshold = BSParam.CcdMotionThreshold;
1123 p.ccdSweptSphereRadius = BSParam.CcdSweptSphereRadius;
1124 p.contactProcessingThreshold = BSParam.ContactProcessingThreshold;
1125
1126 p.terrainImplementation = BSParam.TerrainImplementation;
1127 p.terrainFriction = BSParam.TerrainFriction;
1128
1129 p.terrainHitFraction = BSParam.TerrainHitFraction;
1130 p.terrainRestitution = BSParam.TerrainRestitution;
1131 p.terrainCollisionMargin = BSParam.TerrainCollisionMargin;
1132
1133 p.avatarFriction = BSParam.AvatarFriction;
1134 p.avatarStandingFriction = BSParam.AvatarStandingFriction;
1135 p.avatarDensity = BSParam.AvatarDensity;
1136 p.avatarRestitution = BSParam.AvatarRestitution;
1137 p.avatarCapsuleWidth = BSParam.AvatarCapsuleWidth;
1138 p.avatarCapsuleDepth = BSParam.AvatarCapsuleDepth;
1139 p.avatarCapsuleHeight = BSParam.AvatarCapsuleHeight;
1140 p.avatarContactProcessingThreshold = BSParam.AvatarContactProcessingThreshold;
1141
1142 p.vehicleAngularDamping = BSParam.VehicleAngularDamping;
1143
1144 p.maxPersistantManifoldPoolSize = o[0].maxPersistantManifoldPoolSize;
1145 p.maxCollisionAlgorithmPoolSize = o[0].maxCollisionAlgorithmPoolSize;
1146 p.shouldDisableContactPoolDynamicAllocation = o[0].shouldDisableContactPoolDynamicAllocation;
1147 p.shouldForceUpdateAllAabbs = o[0].shouldForceUpdateAllAabbs;
1148 p.shouldRandomizeSolverOrder = o[0].shouldRandomizeSolverOrder;
1149 p.shouldSplitSimulationIslands = o[0].shouldSplitSimulationIslands;
1150 p.shouldEnableFrictionCaching = o[0].shouldEnableFrictionCaching;
1151 p.numberOfSolverIterations = o[0].numberOfSolverIterations;
1152
1153 p.linksetImplementation = BSParam.LinksetImplementation;
1154 p.linkConstraintUseFrameOffset = BSParam.NumericBool(BSParam.LinkConstraintUseFrameOffset);
1155 p.linkConstraintEnableTransMotor = BSParam.NumericBool(BSParam.LinkConstraintEnableTransMotor);
1156 p.linkConstraintTransMotorMaxVel = BSParam.LinkConstraintTransMotorMaxVel;
1157 p.linkConstraintTransMotorMaxForce = BSParam.LinkConstraintTransMotorMaxForce;
1158 p.linkConstraintERP = BSParam.LinkConstraintERP;
1159 p.linkConstraintCFM = BSParam.LinkConstraintCFM;
1160 p.linkConstraintSolverIterations = BSParam.LinkConstraintSolverIterations;
1161 p.physicsLoggingFrames = o[0].physicsLoggingFrames;
1162 DefaultCollisionConstructionInfo ccci = new DefaultCollisionConstructionInfo();
1163
1164 DefaultCollisionConfiguration cci = new DefaultCollisionConfiguration();
1165 CollisionDispatcher m_dispatcher = new CollisionDispatcher(cci);
1166
1167
1168 if (p.maxPersistantManifoldPoolSize > 0)
1169 cci.m_persistentManifoldPoolSize = (int)p.maxPersistantManifoldPoolSize;
1170 if (p.shouldDisableContactPoolDynamicAllocation !=0)
1171 m_dispatcher.SetDispatcherFlags(DispatcherFlags.CD_DISABLE_CONTACTPOOL_DYNAMIC_ALLOCATION);
1172 //if (p.maxCollisionAlgorithmPoolSize >0 )
1173
1174 DbvtBroadphase m_broadphase = new DbvtBroadphase();
1175 //IndexedVector3 aabbMin = new IndexedVector3(0, 0, 0);
1176 //IndexedVector3 aabbMax = new IndexedVector3(256, 256, 256);
1177
1178 //AxisSweep3Internal m_broadphase2 = new AxisSweep3Internal(ref aabbMin, ref aabbMax, Convert.ToInt32(0xfffe), 0xffff, ushort.MaxValue/2, null, true);
1179 m_broadphase.GetOverlappingPairCache().SetInternalGhostPairCallback(new GhostPairCallback());
1180
1181 SequentialImpulseConstraintSolver m_solver = new SequentialImpulseConstraintSolver();
1182
1183 DiscreteDynamicsWorld world = new DiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_solver, cci);
1184
1185 world.LastCollisionDesc = 0;
1186 world.LastEntityProperty = 0;
1187
1188 world.WorldSettings.Params = p;
1189 world.SetForceUpdateAllAabbs(p.shouldForceUpdateAllAabbs != 0);
1190 world.GetSolverInfo().m_solverMode = SolverMode.SOLVER_USE_WARMSTARTING | SolverMode.SOLVER_SIMD;
1191 if (p.shouldRandomizeSolverOrder != 0)
1192 world.GetSolverInfo().m_solverMode |= SolverMode.SOLVER_RANDMIZE_ORDER;
1193
1194 world.GetSimulationIslandManager().SetSplitIslands(p.shouldSplitSimulationIslands != 0);
1195 //world.GetDispatchInfo().m_enableSatConvex Not implemented in C# port
1196
1197 if (p.shouldEnableFrictionCaching != 0)
1198 world.GetSolverInfo().m_solverMode |= SolverMode.SOLVER_ENABLE_FRICTION_DIRECTION_CACHING;
1199
1200 if (p.numberOfSolverIterations > 0)
1201 world.GetSolverInfo().m_numIterations = (int) p.numberOfSolverIterations;
1202
1203
1204 world.GetSolverInfo().m_damping = world.WorldSettings.Params.linearDamping;
1205 world.GetSolverInfo().m_restitution = world.WorldSettings.Params.defaultRestitution;
1206 world.GetSolverInfo().m_globalCfm = 0.0f;
1207 world.GetSolverInfo().m_tau = 0.6f;
1208 world.GetSolverInfo().m_friction = 0.3f;
1209 world.GetSolverInfo().m_maxErrorReduction = 20f;
1210 world.GetSolverInfo().m_numIterations = 10;
1211 world.GetSolverInfo().m_erp = 0.2f;
1212 world.GetSolverInfo().m_erp2 = 0.1f;
1213 world.GetSolverInfo().m_sor = 1.0f;
1214 world.GetSolverInfo().m_splitImpulse = false;
1215 world.GetSolverInfo().m_splitImpulsePenetrationThreshold = -0.02f;
1216 world.GetSolverInfo().m_linearSlop = 0.0f;
1217 world.GetSolverInfo().m_warmstartingFactor = 0.85f;
1218 world.GetSolverInfo().m_restingContactRestitutionThreshold = 2;
1219 world.SetForceUpdateAllAabbs(true);
1220
1221 //BSParam.TerrainImplementation = 0;
1222 world.SetGravity(new IndexedVector3(0,0,p.gravity));
1223
1224 return world;
1225 }
1226 //m_constraint.ptr, ConstraintParams.BT_CONSTRAINT_STOP_CFM, cfm, ConstraintParamAxis.AXIS_ALL
1227 public override bool SetConstraintParam(BulletConstraint pConstraint, ConstraintParams paramIndex, float paramvalue, ConstraintParamAxis axis)
1228 {
1229 Generic6DofConstraint constrain = (pConstraint as BulletConstraintXNA).constrain as Generic6DofConstraint;
1230 if (axis == ConstraintParamAxis.AXIS_LINEAR_ALL || axis == ConstraintParamAxis.AXIS_ALL)
1231 {
1232 constrain.SetParam((BulletXNA.BulletDynamics.ConstraintParams) (int) paramIndex, paramvalue, 0);
1233 constrain.SetParam((BulletXNA.BulletDynamics.ConstraintParams) (int) paramIndex, paramvalue, 1);
1234 constrain.SetParam((BulletXNA.BulletDynamics.ConstraintParams) (int) paramIndex, paramvalue, 2);
1235 }
1236 if (axis == ConstraintParamAxis.AXIS_ANGULAR_ALL || axis == ConstraintParamAxis.AXIS_ALL)
1237 {
1238 constrain.SetParam((BulletXNA.BulletDynamics.ConstraintParams)(int)paramIndex, paramvalue, 3);
1239 constrain.SetParam((BulletXNA.BulletDynamics.ConstraintParams)(int)paramIndex, paramvalue, 4);
1240 constrain.SetParam((BulletXNA.BulletDynamics.ConstraintParams)(int)paramIndex, paramvalue, 5);
1241 }
1242 if (axis == ConstraintParamAxis.AXIS_LINEAR_ALL)
1243 {
1244 constrain.SetParam((BulletXNA.BulletDynamics.ConstraintParams)(int)paramIndex, paramvalue, (int)axis);
1245 }
1246 return true;
1247 }
1248
1249 public override bool PushUpdate(BulletBody pCollisionObject)
1250 {
1251 bool ret = false;
1252 CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
1253 RigidBody rb = collisionObject as RigidBody;
1254 if (rb != null)
1255 {
1256 SimMotionState sms = rb.GetMotionState() as SimMotionState;
1257 if (sms != null)
1258 {
1259 IndexedMatrix wt = IndexedMatrix.Identity;
1260 sms.GetWorldTransform(out wt);
1261 sms.SetWorldTransform(ref wt, true);
1262 ret = true;
1263 }
1264 }
1265 return ret;
1266
1267 }
1268
1269 public override float GetAngularMotionDisc(BulletShape pShape)
1270 {
1271 CollisionShape shape = (pShape as BulletShapeXNA).shape;
1272 return shape.GetAngularMotionDisc();
1273 }
1274 public override float GetContactBreakingThreshold(BulletShape pShape, float defaultFactor)
1275 {
1276 CollisionShape shape = (pShape as BulletShapeXNA).shape;
1277 return shape.GetContactBreakingThreshold(defaultFactor);
1278 }
1279 public override bool IsCompound(BulletShape pShape)
1280 {
1281 CollisionShape shape = (pShape as BulletShapeXNA).shape;
1282 return shape.IsCompound();
1283 }
1284 public override bool IsSoftBody(BulletShape pShape)
1285 {
1286 CollisionShape shape = (pShape as BulletShapeXNA).shape;
1287 return shape.IsSoftBody();
1288 }
1289 public override bool IsPolyhedral(BulletShape pShape)
1290 {
1291 CollisionShape shape = (pShape as BulletShapeXNA).shape;
1292 return shape.IsPolyhedral();
1293 }
1294 public override bool IsConvex2d(BulletShape pShape)
1295 {
1296 CollisionShape shape = (pShape as BulletShapeXNA).shape;
1297 return shape.IsConvex2d();
1298 }
1299 public override bool IsConvex(BulletShape pShape)
1300 {
1301 CollisionShape shape = (pShape as BulletShapeXNA).shape;
1302 return shape.IsConvex();
1303 }
1304 public override bool IsNonMoving(BulletShape pShape)
1305 {
1306 CollisionShape shape = (pShape as BulletShapeXNA).shape;
1307 return shape.IsNonMoving();
1308 }
1309 public override bool IsConcave(BulletShape pShape)
1310 {
1311 CollisionShape shape = (pShape as BulletShapeXNA).shape;
1312 return shape.IsConcave();
1313 }
1314 public override bool IsInfinite(BulletShape pShape)
1315 {
1316 CollisionShape shape = (pShape as BulletShapeXNA).shape;
1317 return shape.IsInfinite();
1318 }
1319 public override bool IsNativeShape(BulletShape pShape)
1320 {
1321 CollisionShape shape = (pShape as BulletShapeXNA).shape;
1322 bool ret;
1323 switch (shape.GetShapeType())
1324 {
1325 case BroadphaseNativeTypes.BOX_SHAPE_PROXYTYPE:
1326 case BroadphaseNativeTypes.CONE_SHAPE_PROXYTYPE:
1327 case BroadphaseNativeTypes.SPHERE_SHAPE_PROXYTYPE:
1328 case BroadphaseNativeTypes.CYLINDER_SHAPE_PROXYTYPE:
1329 ret = true;
1330 break;
1331 default:
1332 ret = false;
1333 break;
1334 }
1335 return ret;
1336 }
1337
1338 public override void SetShapeCollisionMargin(BulletShape pShape, float pMargin)
1339 {
1340 CollisionShape shape = (pShape as BulletShapeXNA).shape;
1341 shape.SetMargin(pMargin);
1342 }
1343
1344 //sim.ptr, shape.ptr,prim.LocalID, prim.RawPosition, prim.RawOrientation
1345 public override BulletBody CreateGhostFromShape(BulletWorld pWorld, BulletShape pShape, uint pLocalID, Vector3 pRawPosition, Quaternion pRawOrientation)
1346 {
1347 DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
1348 IndexedMatrix bodyTransform = new IndexedMatrix();
1349 bodyTransform._origin = new IndexedVector3(pRawPosition.X, pRawPosition.Y, pRawPosition.Z);
1350 bodyTransform.SetRotation(new IndexedQuaternion(pRawOrientation.X,pRawOrientation.Y,pRawOrientation.Z,pRawOrientation.W));
1351 GhostObject gObj = new PairCachingGhostObject();
1352 gObj.SetWorldTransform(bodyTransform);
1353 CollisionShape shape = (pShape as BulletShapeXNA).shape;
1354 gObj.SetCollisionShape(shape);
1355 gObj.SetUserPointer(pLocalID);
1356
1357 if (specialCollisionObjects.ContainsKey(pLocalID))
1358 specialCollisionObjects[pLocalID] = gObj;
1359 else
1360 specialCollisionObjects.Add(pLocalID, gObj);
1361
1362 // TODO: Add to Special CollisionObjects!
1363 return new BulletBodyXNA(pLocalID, gObj);
1364 }
1365
1366 public override void SetCollisionShape(BulletWorld pWorld, BulletBody pCollisionObject, BulletShape pShape)
1367 {
1368 DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
1369 CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).body;
1370 if (pShape == null)
1371 {
1372 collisionObject.SetCollisionShape(new EmptyShape());
1373 }
1374 else
1375 {
1376 CollisionShape shape = (pShape as BulletShapeXNA).shape;
1377 collisionObject.SetCollisionShape(shape);
1378 }
1379 }
1380 public override BulletShape GetCollisionShape(BulletBody pCollisionObject)
1381 {
1382 CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
1383 CollisionShape shape = collisionObject.GetCollisionShape();
1384 return new BulletShapeXNA(shape, BSShapeTypeFromBroadPhaseNativeType(shape.GetShapeType()));
1385 }
1386
1387 //(PhysicsScene.World.ptr, nativeShapeData)
1388 public override BulletShape BuildNativeShape(BulletWorld pWorld, ShapeData pShapeData)
1389 {
1390 DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
1391 CollisionShape shape = null;
1392 switch (pShapeData.Type)
1393 {
1394 case BSPhysicsShapeType.SHAPE_BOX:
1395 shape = new BoxShape(new IndexedVector3(0.5f,0.5f,0.5f));
1396 break;
1397 case BSPhysicsShapeType.SHAPE_CONE:
1398 shape = new ConeShapeZ(0.5f, 1.0f);
1399 break;
1400 case BSPhysicsShapeType.SHAPE_CYLINDER:
1401 shape = new CylinderShapeZ(new IndexedVector3(0.5f, 0.5f, 0.5f));
1402 break;
1403 case BSPhysicsShapeType.SHAPE_SPHERE:
1404 shape = new SphereShape(0.5f);
1405 break;
1406
1407 }
1408 if (shape != null)
1409 {
1410 IndexedVector3 scaling = new IndexedVector3(pShapeData.Scale.X, pShapeData.Scale.Y, pShapeData.Scale.Z);
1411 shape.SetMargin(world.WorldSettings.Params.collisionMargin);
1412 shape.SetLocalScaling(ref scaling);
1413
1414 }
1415 return new BulletShapeXNA(shape, pShapeData.Type);
1416 }
1417 //PhysicsScene.World.ptr, false
1418 public override BulletShape CreateCompoundShape(BulletWorld pWorld, bool enableDynamicAabbTree)
1419 {
1420 return new BulletShapeXNA(new CompoundShape(enableDynamicAabbTree), BSPhysicsShapeType.SHAPE_COMPOUND);
1421 }
1422
1423 public override int GetNumberOfCompoundChildren(BulletShape pCompoundShape)
1424 {
1425 CompoundShape compoundshape = (pCompoundShape as BulletShapeXNA).shape as CompoundShape;
1426 return compoundshape.GetNumChildShapes();
1427 }
1428 //LinksetRoot.PhysShape.ptr, newShape.ptr, displacementPos, displacementRot
1429 public override void AddChildShapeToCompoundShape(BulletShape pCShape, BulletShape paddShape, Vector3 displacementPos, Quaternion displacementRot)
1430 {
1431 IndexedMatrix relativeTransform = new IndexedMatrix();
1432 CompoundShape compoundshape = (pCShape as BulletShapeXNA).shape as CompoundShape;
1433 CollisionShape addshape = (paddShape as BulletShapeXNA).shape;
1434
1435 relativeTransform._origin = new IndexedVector3(displacementPos.X, displacementPos.Y, displacementPos.Z);
1436 relativeTransform.SetRotation(new IndexedQuaternion(displacementRot.X,displacementRot.Y,displacementRot.Z,displacementRot.W));
1437 compoundshape.AddChildShape(ref relativeTransform, addshape);
1438
1439 }
1440
1441 public override BulletShape RemoveChildShapeFromCompoundShapeIndex(BulletShape pCShape, int pii)
1442 {
1443 CompoundShape compoundshape = (pCShape as BulletShapeXNA).shape as CompoundShape;
1444 CollisionShape ret = null;
1445 ret = compoundshape.GetChildShape(pii);
1446 compoundshape.RemoveChildShapeByIndex(pii);
1447 return new BulletShapeXNA(ret, BSShapeTypeFromBroadPhaseNativeType(ret.GetShapeType()));
1448 }
1449
1450 public override BulletShape GetChildShapeFromCompoundShapeIndex(BulletShape cShape, int indx) {
1451
1452 if (cShape == null)
1453 return null;
1454 CompoundShape compoundShape = (cShape as BulletShapeXNA).shape as CompoundShape;
1455 CollisionShape shape = compoundShape.GetChildShape(indx);
1456 BulletShape retShape = new BulletShapeXNA(shape, BSShapeTypeFromBroadPhaseNativeType(shape.GetShapeType()));
1457
1458
1459 return retShape;
1460 }
1461
1462 public BSPhysicsShapeType BSShapeTypeFromBroadPhaseNativeType(BroadphaseNativeTypes pin)
1463 {
1464 BSPhysicsShapeType ret = BSPhysicsShapeType.SHAPE_UNKNOWN;
1465 switch (pin)
1466 {
1467 case BroadphaseNativeTypes.BOX_SHAPE_PROXYTYPE:
1468 ret = BSPhysicsShapeType.SHAPE_BOX;
1469 break;
1470 case BroadphaseNativeTypes.TRIANGLE_SHAPE_PROXYTYPE:
1471 ret = BSPhysicsShapeType.SHAPE_UNKNOWN;
1472 break;
1473
1474 case BroadphaseNativeTypes.TETRAHEDRAL_SHAPE_PROXYTYPE:
1475 ret = BSPhysicsShapeType.SHAPE_UNKNOWN;
1476 break;
1477 case BroadphaseNativeTypes.CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE:
1478 ret = BSPhysicsShapeType.SHAPE_MESH;
1479 break;
1480 case BroadphaseNativeTypes.CONVEX_HULL_SHAPE_PROXYTYPE:
1481 ret = BSPhysicsShapeType.SHAPE_HULL;
1482 break;
1483 case BroadphaseNativeTypes.CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE:
1484 ret = BSPhysicsShapeType.SHAPE_UNKNOWN;
1485 break;
1486 case BroadphaseNativeTypes.CUSTOM_POLYHEDRAL_SHAPE_TYPE:
1487 ret = BSPhysicsShapeType.SHAPE_UNKNOWN;
1488 break;
1489 //implicit convex shapes
1490 case BroadphaseNativeTypes.IMPLICIT_CONVEX_SHAPES_START_HERE:
1491 ret = BSPhysicsShapeType.SHAPE_UNKNOWN;
1492 break;
1493 case BroadphaseNativeTypes.SPHERE_SHAPE_PROXYTYPE:
1494 ret = BSPhysicsShapeType.SHAPE_SPHERE;
1495 break;
1496 case BroadphaseNativeTypes.MULTI_SPHERE_SHAPE_PROXYTYPE:
1497 ret = BSPhysicsShapeType.SHAPE_UNKNOWN;
1498 break;
1499 case BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE:
1500 ret = BSPhysicsShapeType.SHAPE_CAPSULE;
1501 break;
1502 case BroadphaseNativeTypes.CONE_SHAPE_PROXYTYPE:
1503 ret = BSPhysicsShapeType.SHAPE_CONE;
1504 break;
1505 case BroadphaseNativeTypes.CONVEX_SHAPE_PROXYTYPE:
1506 ret = BSPhysicsShapeType.SHAPE_UNKNOWN;
1507 break;
1508 case BroadphaseNativeTypes.CYLINDER_SHAPE_PROXYTYPE:
1509 ret = BSPhysicsShapeType.SHAPE_CYLINDER;
1510 break;
1511 case BroadphaseNativeTypes.UNIFORM_SCALING_SHAPE_PROXYTYPE:
1512 ret = BSPhysicsShapeType.SHAPE_UNKNOWN;
1513 break;
1514 case BroadphaseNativeTypes.MINKOWSKI_SUM_SHAPE_PROXYTYPE:
1515 ret = BSPhysicsShapeType.SHAPE_UNKNOWN;
1516 break;
1517 case BroadphaseNativeTypes.MINKOWSKI_DIFFERENCE_SHAPE_PROXYTYPE:
1518 ret = BSPhysicsShapeType.SHAPE_UNKNOWN;
1519 break;
1520 case BroadphaseNativeTypes.BOX_2D_SHAPE_PROXYTYPE:
1521 ret = BSPhysicsShapeType.SHAPE_UNKNOWN;
1522 break;
1523 case BroadphaseNativeTypes.CONVEX_2D_SHAPE_PROXYTYPE:
1524 ret = BSPhysicsShapeType.SHAPE_UNKNOWN;
1525 break;
1526 case BroadphaseNativeTypes.CUSTOM_CONVEX_SHAPE_TYPE:
1527 ret = BSPhysicsShapeType.SHAPE_UNKNOWN;
1528 break;
1529 //concave shape
1530 case BroadphaseNativeTypes.CONCAVE_SHAPES_START_HERE:
1531 ret = BSPhysicsShapeType.SHAPE_UNKNOWN;
1532 break;
1533 //keep all the convex shapetype below here, for the check IsConvexShape in broadphase proxy!
1534 case BroadphaseNativeTypes.TRIANGLE_MESH_SHAPE_PROXYTYPE:
1535 ret = BSPhysicsShapeType.SHAPE_MESH;
1536 break;
1537 case BroadphaseNativeTypes.SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE:
1538 ret = BSPhysicsShapeType.SHAPE_MESH;
1539 break;
1540 ///used for demo integration FAST/Swift collision library and Bullet
1541 case BroadphaseNativeTypes.FAST_CONCAVE_MESH_PROXYTYPE:
1542 ret = BSPhysicsShapeType.SHAPE_MESH;
1543 break;
1544 //terrain
1545 case BroadphaseNativeTypes.TERRAIN_SHAPE_PROXYTYPE:
1546 ret = BSPhysicsShapeType.SHAPE_HEIGHTMAP;
1547 break;
1548 ///Used for GIMPACT Trimesh integration
1549 case BroadphaseNativeTypes.GIMPACT_SHAPE_PROXYTYPE:
1550 ret = BSPhysicsShapeType.SHAPE_MESH;
1551 break;
1552 ///Multimaterial mesh
1553 case BroadphaseNativeTypes.MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE:
1554 ret = BSPhysicsShapeType.SHAPE_MESH;
1555 break;
1556
1557 case BroadphaseNativeTypes.EMPTY_SHAPE_PROXYTYPE:
1558 ret = BSPhysicsShapeType.SHAPE_UNKNOWN;
1559 break;
1560 case BroadphaseNativeTypes.STATIC_PLANE_PROXYTYPE:
1561 ret = BSPhysicsShapeType.SHAPE_GROUNDPLANE;
1562 break;
1563 case BroadphaseNativeTypes.CUSTOM_CONCAVE_SHAPE_TYPE:
1564 ret = BSPhysicsShapeType.SHAPE_UNKNOWN;
1565 break;
1566 case BroadphaseNativeTypes.CONCAVE_SHAPES_END_HERE:
1567 ret = BSPhysicsShapeType.SHAPE_UNKNOWN;
1568 break;
1569
1570 case BroadphaseNativeTypes.COMPOUND_SHAPE_PROXYTYPE:
1571 ret = BSPhysicsShapeType.SHAPE_COMPOUND;
1572 break;
1573
1574 case BroadphaseNativeTypes.SOFTBODY_SHAPE_PROXYTYPE:
1575 ret = BSPhysicsShapeType.SHAPE_MESH;
1576 break;
1577 case BroadphaseNativeTypes.HFFLUID_SHAPE_PROXYTYPE:
1578 ret = BSPhysicsShapeType.SHAPE_UNKNOWN;
1579 break;
1580 case BroadphaseNativeTypes.HFFLUID_BUOYANT_CONVEX_SHAPE_PROXYTYPE:
1581 ret = BSPhysicsShapeType.SHAPE_UNKNOWN;
1582 break;
1583 case BroadphaseNativeTypes.INVALID_SHAPE_PROXYTYPE:
1584 ret = BSPhysicsShapeType.SHAPE_UNKNOWN;
1585 break;
1586 }
1587 return ret;
1588 }
1589
1590 public override void RemoveChildShapeFromCompoundShape(BulletShape cShape, BulletShape removeShape) { /* TODO */ }
1591 public override void UpdateChildTransform(BulletShape pShape, int childIndex, Vector3 pos, Quaternion rot, bool shouldRecalculateLocalAabb) { /* TODO */ }
1592
1593 public override BulletShape CreateGroundPlaneShape(uint pLocalId, float pheight, float pcollisionMargin)
1594 {
1595 StaticPlaneShape m_planeshape = new StaticPlaneShape(new IndexedVector3(0,0,1),(int)pheight );
1596 m_planeshape.SetMargin(pcollisionMargin);
1597 m_planeshape.SetUserPointer(pLocalId);
1598 return new BulletShapeXNA(m_planeshape, BSPhysicsShapeType.SHAPE_GROUNDPLANE);
1599 }
1600
1601 public override BulletConstraint Create6DofSpringConstraint(BulletWorld pWorld, BulletBody pBody1, BulletBody pBody2,
1602 Vector3 pframe1, Quaternion pframe1rot, Vector3 pframe2, Quaternion pframe2rot,
1603 bool puseLinearReferenceFrameA, bool pdisableCollisionsBetweenLinkedBodies)
1604
1605 {
1606 Generic6DofSpringConstraint constrain = null;
1607 DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
1608 RigidBody body1 = (pBody1 as BulletBodyXNA).rigidBody;
1609 RigidBody body2 = (pBody2 as BulletBodyXNA).rigidBody;
1610 if (body1 != null && body2 != null)
1611 {
1612 IndexedVector3 frame1v = new IndexedVector3(pframe1.X, pframe1.Y, pframe1.Z);
1613 IndexedQuaternion frame1rot = new IndexedQuaternion(pframe1rot.X, pframe1rot.Y, pframe1rot.Z, pframe1rot.W);
1614 IndexedMatrix frame1 = IndexedMatrix.CreateFromQuaternion(frame1rot);
1615 frame1._origin = frame1v;
1616
1617 IndexedVector3 frame2v = new IndexedVector3(pframe2.X, pframe2.Y, pframe2.Z);
1618 IndexedQuaternion frame2rot = new IndexedQuaternion(pframe2rot.X, pframe2rot.Y, pframe2rot.Z, pframe2rot.W);
1619 IndexedMatrix frame2 = IndexedMatrix.CreateFromQuaternion(frame2rot);
1620 frame2._origin = frame1v;
1621
1622 constrain = new Generic6DofSpringConstraint(body1, body2, ref frame1, ref frame2, puseLinearReferenceFrameA);
1623 world.AddConstraint(constrain, pdisableCollisionsBetweenLinkedBodies);
1624
1625 constrain.CalculateTransforms();
1626 }
1627
1628 return new BulletConstraintXNA(constrain);
1629 }
1630
1631 public override BulletConstraint CreateHingeConstraint(BulletWorld pWorld, BulletBody pBody1, BulletBody pBody2,
1632 Vector3 ppivotInA, Vector3 ppivotInB, Vector3 paxisInA, Vector3 paxisInB,
1633 bool puseLinearReferenceFrameA, bool pdisableCollisionsBetweenLinkedBodies)
1634 {
1635 HingeConstraint constrain = null;
1636 DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
1637 RigidBody rb1 = (pBody1 as BulletBodyXNA).rigidBody;
1638 RigidBody rb2 = (pBody2 as BulletBodyXNA).rigidBody;
1639 if (rb1 != null && rb2 != null)
1640 {
1641 IndexedVector3 pivotInA = new IndexedVector3(ppivotInA.X, ppivotInA.Y, ppivotInA.Z);
1642 IndexedVector3 pivotInB = new IndexedVector3(ppivotInB.X, ppivotInB.Y, ppivotInB.Z);
1643 IndexedVector3 axisInA = new IndexedVector3(paxisInA.X, paxisInA.Y, paxisInA.Z);
1644 IndexedVector3 axisInB = new IndexedVector3(paxisInB.X, paxisInB.Y, paxisInB.Z);
1645 constrain = new HingeConstraint(rb1, rb2, ref pivotInA, ref pivotInB, ref axisInA, ref axisInB, puseLinearReferenceFrameA);
1646 world.AddConstraint(constrain, pdisableCollisionsBetweenLinkedBodies);
1647 }
1648 return new BulletConstraintXNA(constrain);
1649 }
1650
1651 public override BulletConstraint CreateSliderConstraint(BulletWorld pWorld, BulletBody pBody1, BulletBody pBody2,
1652 Vector3 pframe1, Quaternion pframe1rot,
1653 Vector3 pframe2, Quaternion pframe2rot,
1654 bool puseLinearReferenceFrameA, bool pdisableCollisionsBetweenLinkedBodies)
1655 {
1656 SliderConstraint constrain = null;
1657 DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
1658 RigidBody rb1 = (pBody1 as BulletBodyXNA).rigidBody;
1659 RigidBody rb2 = (pBody2 as BulletBodyXNA).rigidBody;
1660 if (rb1 != null && rb2 != null)
1661 {
1662 IndexedVector3 frame1v = new IndexedVector3(pframe1.X, pframe1.Y, pframe1.Z);
1663 IndexedQuaternion frame1rot = new IndexedQuaternion(pframe1rot.X, pframe1rot.Y, pframe1rot.Z, pframe1rot.W);
1664 IndexedMatrix frame1 = IndexedMatrix.CreateFromQuaternion(frame1rot);
1665 frame1._origin = frame1v;
1666
1667 IndexedVector3 frame2v = new IndexedVector3(pframe2.X, pframe2.Y, pframe2.Z);
1668 IndexedQuaternion frame2rot = new IndexedQuaternion(pframe2rot.X, pframe2rot.Y, pframe2rot.Z, pframe2rot.W);
1669 IndexedMatrix frame2 = IndexedMatrix.CreateFromQuaternion(frame2rot);
1670 frame2._origin = frame1v;
1671
1672 constrain = new SliderConstraint(rb1, rb2, ref frame1, ref frame2, puseLinearReferenceFrameA);
1673 world.AddConstraint(constrain, pdisableCollisionsBetweenLinkedBodies);
1674 }
1675 return new BulletConstraintXNA(constrain);
1676 }
1677
1678 public override BulletConstraint CreateConeTwistConstraint(BulletWorld pWorld, BulletBody pBody1, BulletBody pBody2,
1679 Vector3 pframe1, Quaternion pframe1rot,
1680 Vector3 pframe2, Quaternion pframe2rot,
1681 bool pdisableCollisionsBetweenLinkedBodies)
1682 {
1683 ConeTwistConstraint constrain = null;
1684 DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
1685 RigidBody rb1 = (pBody1 as BulletBodyXNA).rigidBody;
1686 RigidBody rb2 = (pBody2 as BulletBodyXNA).rigidBody;
1687 if (rb1 != null && rb2 != null)
1688 {
1689 IndexedVector3 frame1v = new IndexedVector3(pframe1.X, pframe1.Y, pframe1.Z);
1690 IndexedQuaternion frame1rot = new IndexedQuaternion(pframe1rot.X, pframe1rot.Y, pframe1rot.Z, pframe1rot.W);
1691 IndexedMatrix frame1 = IndexedMatrix.CreateFromQuaternion(frame1rot);
1692 frame1._origin = frame1v;
1693
1694 IndexedVector3 frame2v = new IndexedVector3(pframe2.X, pframe2.Y, pframe2.Z);
1695 IndexedQuaternion frame2rot = new IndexedQuaternion(pframe2rot.X, pframe2rot.Y, pframe2rot.Z, pframe2rot.W);
1696 IndexedMatrix frame2 = IndexedMatrix.CreateFromQuaternion(frame2rot);
1697 frame2._origin = frame1v;
1698
1699 constrain = new ConeTwistConstraint(rb1, rb2, ref frame1, ref frame2);
1700 world.AddConstraint(constrain, pdisableCollisionsBetweenLinkedBodies);
1701 }
1702 return new BulletConstraintXNA(constrain);
1703 }
1704
1705 public override BulletConstraint CreateGearConstraint(BulletWorld pWorld, BulletBody pBody1, BulletBody pBody2,
1706 Vector3 paxisInA, Vector3 paxisInB,
1707 float pratio, bool pdisableCollisionsBetweenLinkedBodies)
1708 {
1709 Generic6DofConstraint constrain = null;
1710 /* BulletXNA does not have a gear constraint
1711 GearConstraint constrain = null;
1712 DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
1713 RigidBody rb1 = (pBody1 as BulletBodyXNA).rigidBody;
1714 RigidBody rb2 = (pBody2 as BulletBodyXNA).rigidBody;
1715 if (rb1 != null && rb2 != null)
1716 {
1717 IndexedVector3 axis1 = new IndexedVector3(paxisInA.X, paxisInA.Y, paxisInA.Z);
1718 IndexedVector3 axis2 = new IndexedVector3(paxisInB.X, paxisInB.Y, paxisInB.Z);
1719 constrain = new GearConstraint(rb1, rb2, ref axis1, ref axis2, pratio);
1720 world.AddConstraint(constrain, pdisableCollisionsBetweenLinkedBodies);
1721 }
1722 */
1723 return new BulletConstraintXNA(constrain);
1724 }
1725
1726 public override BulletConstraint CreatePoint2PointConstraint(BulletWorld pWorld, BulletBody pBody1, BulletBody pBody2,
1727 Vector3 ppivotInA, Vector3 ppivotInB,
1728 bool pdisableCollisionsBetweenLinkedBodies)
1729 {
1730 Point2PointConstraint constrain = null;
1731 DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
1732 RigidBody rb1 = (pBody1 as BulletBodyXNA).rigidBody;
1733 RigidBody rb2 = (pBody2 as BulletBodyXNA).rigidBody;
1734 if (rb1 != null && rb2 != null)
1735 {
1736 IndexedVector3 pivotInA = new IndexedVector3(ppivotInA.X, ppivotInA.Y, ppivotInA.Z);
1737 IndexedVector3 pivotInB = new IndexedVector3(ppivotInB.X, ppivotInB.Y, ppivotInB.Z);
1738 constrain = new Point2PointConstraint(rb1, rb2, ref pivotInA, ref pivotInB);
1739 world.AddConstraint(constrain, pdisableCollisionsBetweenLinkedBodies);
1740 }
1741 return new BulletConstraintXNA(constrain);
1742 }
1743
1744 public override BulletShape CreateHullShape(BulletWorld pWorld, int pHullCount, float[] pConvHulls)
1745 {
1746 DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
1747 CompoundShape compoundshape = new CompoundShape(false);
1748
1749 compoundshape.SetMargin(world.WorldSettings.Params.collisionMargin);
1750 int ii = 1;
1751
1752 for (int i = 0; i < pHullCount; i++)
1753 {
1754 int vertexCount = (int) pConvHulls[ii];
1755
1756 IndexedVector3 centroid = new IndexedVector3(pConvHulls[ii + 1], pConvHulls[ii + 2], pConvHulls[ii + 3]);
1757 IndexedMatrix childTrans = IndexedMatrix.Identity;
1758 childTrans._origin = centroid;
1759
1760 List<IndexedVector3> virts = new List<IndexedVector3>();
1761 int ender = ((ii + 4) + (vertexCount*3));
1762 for (int iii = ii + 4; iii < ender; iii+=3)
1763 {
1764
1765 virts.Add(new IndexedVector3(pConvHulls[iii], pConvHulls[iii + 1], pConvHulls[iii +2]));
1766 }
1767 ConvexHullShape convexShape = new ConvexHullShape(virts, vertexCount);
1768 convexShape.SetMargin(world.WorldSettings.Params.collisionMargin);
1769 compoundshape.AddChildShape(ref childTrans, convexShape);
1770 ii += (vertexCount*3 + 4);
1771 }
1772
1773 return new BulletShapeXNA(compoundshape, BSPhysicsShapeType.SHAPE_HULL);
1774 }
1775
1776 public override BulletShape BuildHullShapeFromMesh(BulletWorld world, BulletShape meshShape)
1777 {
1778 /* TODO */ return null;
1779
1780 }
1781
1782 public override BulletShape CreateMeshShape(BulletWorld pWorld, int pIndicesCount, int[] indices, int pVerticesCount, float[] verticesAsFloats)
1783 {
1784 //DumpRaw(indices,verticesAsFloats,pIndicesCount,pVerticesCount);
1785
1786 for (int iter = 0; iter < pVerticesCount; iter++)
1787 {
1788 if (verticesAsFloats[iter] > 0 && verticesAsFloats[iter] < 0.0001) verticesAsFloats[iter] = 0;
1789 if (verticesAsFloats[iter] < 0 && verticesAsFloats[iter] > -0.0001) verticesAsFloats[iter] = 0;
1790 }
1791
1792 ObjectArray<int> indicesarr = new ObjectArray<int>(indices);
1793 ObjectArray<float> vertices = new ObjectArray<float>(verticesAsFloats);
1794 DumpRaw(indicesarr,vertices,pIndicesCount,pVerticesCount);
1795 DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
1796 IndexedMesh mesh = new IndexedMesh();
1797 mesh.m_indexType = PHY_ScalarType.PHY_INTEGER;
1798 mesh.m_numTriangles = pIndicesCount/3;
1799 mesh.m_numVertices = pVerticesCount;
1800 mesh.m_triangleIndexBase = indicesarr;
1801 mesh.m_vertexBase = vertices;
1802 mesh.m_vertexStride = 3;
1803 mesh.m_vertexType = PHY_ScalarType.PHY_FLOAT;
1804 mesh.m_triangleIndexStride = 3;
1805
1806 TriangleIndexVertexArray tribuilder = new TriangleIndexVertexArray();
1807 tribuilder.AddIndexedMesh(mesh, PHY_ScalarType.PHY_INTEGER);
1808 BvhTriangleMeshShape meshShape = new BvhTriangleMeshShape(tribuilder, true,true);
1809 meshShape.SetMargin(world.WorldSettings.Params.collisionMargin);
1810 // world.UpdateSingleAabb(meshShape);
1811 return new BulletShapeXNA(meshShape, BSPhysicsShapeType.SHAPE_MESH);
1812
1813 }
1814 public static void DumpRaw(ObjectArray<int>indices, ObjectArray<float> vertices, int pIndicesCount,int pVerticesCount )
1815 {
1816
1817 String fileName = "objTest3.raw";
1818 String completePath = System.IO.Path.Combine(Util.configDir(), fileName);
1819 StreamWriter sw = new StreamWriter(completePath);
1820 IndexedMesh mesh = new IndexedMesh();
1821
1822 mesh.m_indexType = PHY_ScalarType.PHY_INTEGER;
1823 mesh.m_numTriangles = pIndicesCount / 3;
1824 mesh.m_numVertices = pVerticesCount;
1825 mesh.m_triangleIndexBase = indices;
1826 mesh.m_vertexBase = vertices;
1827 mesh.m_vertexStride = 3;
1828 mesh.m_vertexType = PHY_ScalarType.PHY_FLOAT;
1829 mesh.m_triangleIndexStride = 3;
1830
1831 TriangleIndexVertexArray tribuilder = new TriangleIndexVertexArray();
1832 tribuilder.AddIndexedMesh(mesh, PHY_ScalarType.PHY_INTEGER);
1833
1834
1835
1836 for (int i = 0; i < pVerticesCount; i++)
1837 {
1838
1839 string s = vertices[indices[i * 3]].ToString("0.0000");
1840 s += " " + vertices[indices[i * 3 + 1]].ToString("0.0000");
1841 s += " " + vertices[indices[i * 3 + 2]].ToString("0.0000");
1842
1843 sw.Write(s + "\n");
1844 }
1845
1846 sw.Close();
1847 }
1848 public static void DumpRaw(int[] indices, float[] vertices, int pIndicesCount, int pVerticesCount)
1849 {
1850
1851 String fileName = "objTest6.raw";
1852 String completePath = System.IO.Path.Combine(Util.configDir(), fileName);
1853 StreamWriter sw = new StreamWriter(completePath);
1854 IndexedMesh mesh = new IndexedMesh();
1855
1856 mesh.m_indexType = PHY_ScalarType.PHY_INTEGER;
1857 mesh.m_numTriangles = pIndicesCount / 3;
1858 mesh.m_numVertices = pVerticesCount;
1859 mesh.m_triangleIndexBase = indices;
1860 mesh.m_vertexBase = vertices;
1861 mesh.m_vertexStride = 3;
1862 mesh.m_vertexType = PHY_ScalarType.PHY_FLOAT;
1863 mesh.m_triangleIndexStride = 3;
1864
1865 TriangleIndexVertexArray tribuilder = new TriangleIndexVertexArray();
1866 tribuilder.AddIndexedMesh(mesh, PHY_ScalarType.PHY_INTEGER);
1867
1868
1869 sw.WriteLine("Indices");
1870 sw.WriteLine(string.Format("int[] indices = new int[{0}];",pIndicesCount));
1871 for (int iter = 0; iter < indices.Length; iter++)
1872 {
1873 sw.WriteLine(string.Format("indices[{0}]={1};",iter,indices[iter]));
1874 }
1875 sw.WriteLine("VerticesFloats");
1876 sw.WriteLine(string.Format("float[] vertices = new float[{0}];", pVerticesCount));
1877 for (int iter = 0; iter < vertices.Length; iter++)
1878 {
1879 sw.WriteLine(string.Format("Vertices[{0}]={1};", iter, vertices[iter].ToString("0.0000")));
1880 }
1881
1882 // for (int i = 0; i < pVerticesCount; i++)
1883 // {
1884 //
1885 // string s = vertices[indices[i * 3]].ToString("0.0000");
1886 // s += " " + vertices[indices[i * 3 + 1]].ToString("0.0000");
1887 // s += " " + vertices[indices[i * 3 + 2]].ToString("0.0000");
1888 //
1889 // sw.Write(s + "\n");
1890 //}
1891
1892 sw.Close();
1893 }
1894
1895 public override BulletShape CreateTerrainShape(uint id, Vector3 size, float minHeight, float maxHeight, float[] heightMap,
1896 float scaleFactor, float collisionMargin)
1897 {
1898 const int upAxis = 2;
1899 HeightfieldTerrainShape terrainShape = new HeightfieldTerrainShape((int)size.X, (int)size.Y,
1900 heightMap, scaleFactor,
1901 minHeight, maxHeight, upAxis,
1902 false);
1903 terrainShape.SetMargin(collisionMargin + 0.5f);
1904 terrainShape.SetUseDiamondSubdivision(true);
1905 terrainShape.SetUserPointer(id);
1906 return new BulletShapeXNA(terrainShape, BSPhysicsShapeType.SHAPE_TERRAIN);
1907 }
1908
1909 public override bool TranslationalLimitMotor(BulletConstraint pConstraint, float ponOff, float targetVelocity, float maxMotorForce)
1910 {
1911 TypedConstraint tconstrain = (pConstraint as BulletConstraintXNA).constrain;
1912 bool onOff = ponOff != 0;
1913 bool ret = false;
1914
1915 switch (tconstrain.GetConstraintType())
1916 {
1917 case TypedConstraintType.D6_CONSTRAINT_TYPE:
1918 Generic6DofConstraint constrain = tconstrain as Generic6DofConstraint;
1919 constrain.GetTranslationalLimitMotor().m_enableMotor[0] = onOff;
1920 constrain.GetTranslationalLimitMotor().m_targetVelocity[0] = targetVelocity;
1921 constrain.GetTranslationalLimitMotor().m_maxMotorForce[0] = maxMotorForce;
1922 ret = true;
1923 break;
1924 }
1925
1926
1927 return ret;
1928
1929 }
1930
1931 public override int PhysicsStep(BulletWorld world, float timeStep, int maxSubSteps, float fixedTimeStep,
1932 out int updatedEntityCount, out int collidersCount)
1933 {
1934 /* TODO */
1935 updatedEntityCount = 0;
1936 collidersCount = 0;
1937
1938
1939 int ret = PhysicsStep2(world,timeStep,maxSubSteps,fixedTimeStep,out updatedEntityCount,out world.physicsScene.m_updateArray, out collidersCount, out world.physicsScene.m_collisionArray);
1940
1941 return ret;
1942 }
1943
1944 private int PhysicsStep2(BulletWorld pWorld, float timeStep, int m_maxSubSteps, float m_fixedTimeStep,
1945 out int updatedEntityCount, out EntityProperties[] updatedEntities,
1946 out int collidersCount, out CollisionDesc[] colliders)
1947 {
1948 int epic = PhysicsStepint(pWorld, timeStep, m_maxSubSteps, m_fixedTimeStep, out updatedEntityCount, out updatedEntities,
1949 out collidersCount, out colliders, m_maxCollisions, m_maxUpdatesPerFrame);
1950 return epic;
1951 }
1952
1953 private int PhysicsStepint(BulletWorld pWorld,float timeStep, int m_maxSubSteps, float m_fixedTimeStep, out int updatedEntityCount,
1954 out EntityProperties[] updatedEntities, out int collidersCount, out CollisionDesc[] colliders, int maxCollisions, int maxUpdates)
1955 {
1956 int numSimSteps = 0;
1957 Array.Clear(UpdatedObjects, 0, UpdatedObjects.Length);
1958 Array.Clear(UpdatedCollisions, 0, UpdatedCollisions.Length);
1959 LastEntityProperty=0;
1960
1961
1962
1963
1964
1965
1966 LastCollisionDesc=0;
1967
1968 updatedEntityCount = 0;
1969 collidersCount = 0;
1970
1971
1972 if (pWorld is BulletWorldXNA)
1973 {
1974 DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
1975
1976 world.LastCollisionDesc = 0;
1977 world.LastEntityProperty = 0;
1978 numSimSteps = world.StepSimulation(timeStep, m_maxSubSteps, m_fixedTimeStep);
1979 int updates = 0;
1980
1981 PersistentManifold contactManifold;
1982 CollisionObject objA;
1983 CollisionObject objB;
1984 ManifoldPoint manifoldPoint;
1985 PairCachingGhostObject pairCachingGhostObject;
1986
1987 m_collisionsThisFrame = 0;
1988 int numManifolds = world.GetDispatcher().GetNumManifolds();
1989 for (int j = 0; j < numManifolds; j++)
1990 {
1991 contactManifold = world.GetDispatcher().GetManifoldByIndexInternal(j);
1992 int numContacts = contactManifold.GetNumContacts();
1993 if (numContacts == 0)
1994 continue;
1995
1996 objA = contactManifold.GetBody0() as CollisionObject;
1997 objB = contactManifold.GetBody1() as CollisionObject;
1998
1999 manifoldPoint = contactManifold.GetContactPoint(0);
2000 //IndexedVector3 contactPoint = manifoldPoint.GetPositionWorldOnB();
2001 // IndexedVector3 contactNormal = -manifoldPoint.m_normalWorldOnB; // make relative to A
2002
2003 RecordCollision(this, objA, objB, manifoldPoint.GetPositionWorldOnB(), -manifoldPoint.m_normalWorldOnB, manifoldPoint.GetDistance());
2004 m_collisionsThisFrame ++;
2005 if (m_collisionsThisFrame >= 9999999)
2006 break;
2007
2008
2009 }
2010
2011 foreach (GhostObject ghostObject in specialCollisionObjects.Values)
2012 {
2013 pairCachingGhostObject = ghostObject as PairCachingGhostObject;
2014 if (pairCachingGhostObject != null)
2015 {
2016 RecordGhostCollisions(pairCachingGhostObject);
2017 }
2018
2019 }
2020
2021
2022 updatedEntityCount = LastEntityProperty;
2023 updatedEntities = UpdatedObjects;
2024
2025 collidersCount = LastCollisionDesc;
2026 colliders = UpdatedCollisions;
2027
2028
2029 }
2030 else
2031 {
2032 //if (updatedEntities is null)
2033 //updatedEntities = new List<BulletXNA.EntityProperties>();
2034 //updatedEntityCount = 0;
2035
2036
2037 //collidersCount = 0;
2038
2039 updatedEntities = new EntityProperties[0];
2040
2041
2042 colliders = new CollisionDesc[0];
2043
2044 }
2045 return numSimSteps;
2046 }
2047 public void RecordGhostCollisions(PairCachingGhostObject obj)
2048 {
2049 IOverlappingPairCache cache = obj.GetOverlappingPairCache();
2050 ObjectArray<BroadphasePair> pairs = cache.GetOverlappingPairArray();
2051
2052 DiscreteDynamicsWorld world = (PhysicsScene.World as BulletWorldXNA).world;
2053 PersistentManifoldArray manifoldArray = new PersistentManifoldArray();
2054 BroadphasePair collisionPair;
2055 PersistentManifold contactManifold;
2056
2057 CollisionObject objA;
2058 CollisionObject objB;
2059
2060 ManifoldPoint pt;
2061
2062 int numPairs = pairs.Count;
2063
2064 for (int i = 0; i < numPairs; i++)
2065 {
2066 manifoldArray.Clear();
2067 if (LastCollisionDesc < UpdatedCollisions.Length)
2068 break;
2069 collisionPair = world.GetPairCache().FindPair(pairs[i].m_pProxy0, pairs[i].m_pProxy1);
2070 if (collisionPair == null)
2071 continue;
2072
2073 collisionPair.m_algorithm.GetAllContactManifolds(manifoldArray);
2074 for (int j = 0; j < manifoldArray.Count; j++)
2075 {
2076 contactManifold = manifoldArray[j];
2077 int numContacts = contactManifold.GetNumContacts();
2078 objA = contactManifold.GetBody0() as CollisionObject;
2079 objB = contactManifold.GetBody1() as CollisionObject;
2080 for (int p = 0; p < numContacts; p++)
2081 {
2082 pt = contactManifold.GetContactPoint(p);
2083 if (pt.GetDistance() < 0.0f)
2084 {
2085 RecordCollision(this, objA, objB, pt.GetPositionWorldOnA(), -pt.m_normalWorldOnB,pt.GetDistance());
2086 break;
2087 }
2088 }
2089 }
2090 }
2091
2092 }
2093 private static void RecordCollision(BSAPIXNA world, CollisionObject objA, CollisionObject objB, IndexedVector3 contact, IndexedVector3 norm, float penetration)
2094 {
2095
2096 IndexedVector3 contactNormal = norm;
2097 if ((objA.GetCollisionFlags() & BulletXNA.BulletCollision.CollisionFlags.BS_WANTS_COLLISIONS) == 0 &&
2098 (objB.GetCollisionFlags() & BulletXNA.BulletCollision.CollisionFlags.BS_WANTS_COLLISIONS) == 0)
2099 {
2100 return;
2101 }
2102 uint idA = (uint)objA.GetUserPointer();
2103 uint idB = (uint)objB.GetUserPointer();
2104 if (idA > idB)
2105 {
2106 uint temp = idA;
2107 idA = idB;
2108 idB = temp;
2109 contactNormal = -contactNormal;
2110 }
2111
2112 //ulong collisionID = ((ulong) idA << 32) | idB;
2113
2114 CollisionDesc cDesc = new CollisionDesc()
2115 {
2116 aID = idA,
2117 bID = idB,
2118 point = new Vector3(contact.X,contact.Y,contact.Z),
2119 normal = new Vector3(contactNormal.X,contactNormal.Y,contactNormal.Z),
2120 penetration = penetration
2121
2122 };
2123 if (world.LastCollisionDesc < world.UpdatedCollisions.Length)
2124 world.UpdatedCollisions[world.LastCollisionDesc++] = (cDesc);
2125 m_collisionsThisFrame++;
2126
2127
2128 }
2129 private static EntityProperties GetDebugProperties(BulletWorld pWorld, BulletBody pCollisionObject)
2130 {
2131 EntityProperties ent = new EntityProperties();
2132 DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
2133 CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
2134 IndexedMatrix transform = collisionObject.GetWorldTransform();
2135 IndexedVector3 LinearVelocity = collisionObject.GetInterpolationLinearVelocity();
2136 IndexedVector3 AngularVelocity = collisionObject.GetInterpolationAngularVelocity();
2137 IndexedQuaternion rotation = transform.GetRotation();
2138 ent.Acceleration = Vector3.Zero;
2139 ent.ID = (uint)collisionObject.GetUserPointer();
2140 ent.Position = new Vector3(transform._origin.X,transform._origin.Y,transform._origin.Z);
2141 ent.Rotation = new Quaternion(rotation.X,rotation.Y,rotation.Z,rotation.W);
2142 ent.Velocity = new Vector3(LinearVelocity.X, LinearVelocity.Y, LinearVelocity.Z);
2143 ent.RotationalVelocity = new Vector3(AngularVelocity.X, AngularVelocity.Y, AngularVelocity.Z);
2144 return ent;
2145 }
2146
2147 public override bool UpdateParameter(BulletWorld world, uint localID, String parm, float value) { /* TODO */
2148 return false; }
2149
2150 public override Vector3 GetLocalScaling(BulletShape pShape)
2151 {
2152 CollisionShape shape = (pShape as BulletShapeXNA).shape;
2153 IndexedVector3 scale = shape.GetLocalScaling();
2154 return new Vector3(scale.X,scale.Y,scale.Z);
2155 }
2156
2157 public bool RayCastGround(BulletWorld pWorld, Vector3 _RayOrigin, float pRayHeight, BulletBody NotMe)
2158 {
2159 DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
2160 if (world != null)
2161 {
2162 if (NotMe is BulletBodyXNA && NotMe.HasPhysicalBody)
2163 {
2164 CollisionObject AvoidBody = (NotMe as BulletBodyXNA).body;
2165
2166 IndexedVector3 rOrigin = new IndexedVector3(_RayOrigin.X, _RayOrigin.Y, _RayOrigin.Z);
2167 IndexedVector3 rEnd = new IndexedVector3(_RayOrigin.X, _RayOrigin.Y, _RayOrigin.Z - pRayHeight);
2168 using (
2169 ClosestNotMeRayResultCallback rayCallback =
2170 new ClosestNotMeRayResultCallback(rOrigin, rEnd, AvoidBody)
2171 )
2172 {
2173 world.RayTest(ref rOrigin, ref rEnd, rayCallback);
2174 if (rayCallback.HasHit())
2175 {
2176 IndexedVector3 hitLocation = rayCallback.m_hitPointWorld;
2177 }
2178 return rayCallback.HasHit();
2179 }
2180 }
2181 }
2182 return false;
2183 }
2184}
2185
2186
2187
2188
2189 public class SimMotionState : DefaultMotionState
2190 {
2191 public RigidBody Rigidbody;
2192 public Vector3 ZeroVect;
2193
2194 private IndexedMatrix m_xform;
2195
2196 private EntityProperties m_properties;
2197 private EntityProperties m_lastProperties;
2198 private BSAPIXNA m_world;
2199
2200 const float POSITION_TOLERANCE = 0.05f;
2201 const float VELOCITY_TOLERANCE = 0.001f;
2202 const float ROTATION_TOLERANCE = 0.01f;
2203 const float ANGULARVELOCITY_TOLERANCE = 0.01f;
2204
2205 public SimMotionState(BSAPIXNA pWorld, uint id, IndexedMatrix starTransform, object frameUpdates)
2206 {
2207 IndexedQuaternion OrientationQuaterion = starTransform.GetRotation();
2208 m_properties = new EntityProperties()
2209 {
2210 ID = id,
2211 Position = new Vector3(starTransform._origin.X, starTransform._origin.Y,starTransform._origin.Z),
2212 Rotation = new Quaternion(OrientationQuaterion.X,OrientationQuaterion.Y,OrientationQuaterion.Z,OrientationQuaterion.W)
2213 };
2214 m_lastProperties = new EntityProperties()
2215 {
2216 ID = id,
2217 Position = new Vector3(starTransform._origin.X, starTransform._origin.Y, starTransform._origin.Z),
2218 Rotation = new Quaternion(OrientationQuaterion.X, OrientationQuaterion.Y, OrientationQuaterion.Z, OrientationQuaterion.W)
2219 };
2220 m_world = pWorld;
2221 m_xform = starTransform;
2222 }
2223
2224 public override void GetWorldTransform(out IndexedMatrix worldTrans)
2225 {
2226 worldTrans = m_xform;
2227 }
2228
2229 public override void SetWorldTransform(IndexedMatrix worldTrans)
2230 {
2231 SetWorldTransform(ref worldTrans);
2232 }
2233
2234 public override void SetWorldTransform(ref IndexedMatrix worldTrans)
2235 {
2236 SetWorldTransform(ref worldTrans, false);
2237 }
2238 public void SetWorldTransform(ref IndexedMatrix worldTrans, bool force)
2239 {
2240 m_xform = worldTrans;
2241 // Put the new transform into m_properties
2242 IndexedQuaternion OrientationQuaternion = m_xform.GetRotation();
2243 IndexedVector3 LinearVelocityVector = Rigidbody.GetLinearVelocity();
2244 IndexedVector3 AngularVelocityVector = Rigidbody.GetAngularVelocity();
2245 m_properties.Position = new Vector3(m_xform._origin.X, m_xform._origin.Y, m_xform._origin.Z);
2246 m_properties.Rotation = new Quaternion(OrientationQuaternion.X, OrientationQuaternion.Y,
2247 OrientationQuaternion.Z, OrientationQuaternion.W);
2248 // A problem with stock Bullet is that we don't get an event when an object is deactivated.
2249 // This means that the last non-zero values for linear and angular velocity
2250 // are left in the viewer who does dead reconning and the objects look like
2251 // they float off.
2252 // BulletSim ships with a patch to Bullet which creates such an event.
2253 m_properties.Velocity = new Vector3(LinearVelocityVector.X, LinearVelocityVector.Y, LinearVelocityVector.Z);
2254 m_properties.RotationalVelocity = new Vector3(AngularVelocityVector.X, AngularVelocityVector.Y, AngularVelocityVector.Z);
2255
2256 if (force
2257
2258 || !AlmostEqual(ref m_lastProperties.Position, ref m_properties.Position, POSITION_TOLERANCE)
2259 || !AlmostEqual(ref m_properties.Rotation, ref m_lastProperties.Rotation, ROTATION_TOLERANCE)
2260 // If the Velocity and AngularVelocity are zero, most likely the object has
2261 // been deactivated. If they both are zero and they have become zero recently,
2262 // make sure a property update is sent so the zeros make it to the viewer.
2263 || ((m_properties.Velocity == ZeroVect && m_properties.RotationalVelocity == ZeroVect)
2264 &&
2265 (m_properties.Velocity != m_lastProperties.Velocity ||
2266 m_properties.RotationalVelocity != m_lastProperties.RotationalVelocity))
2267 // If Velocity and AngularVelocity are non-zero but have changed, send an update.
2268 || !AlmostEqual(ref m_properties.Velocity, ref m_lastProperties.Velocity, VELOCITY_TOLERANCE)
2269 ||
2270 !AlmostEqual(ref m_properties.RotationalVelocity, ref m_lastProperties.RotationalVelocity,
2271 ANGULARVELOCITY_TOLERANCE)
2272 )
2273
2274
2275 {
2276 // Add this update to the list of updates for this frame.
2277 m_lastProperties = m_properties;
2278 if (m_world.LastEntityProperty < m_world.UpdatedObjects.Length)
2279 m_world.UpdatedObjects[m_world.LastEntityProperty++]=(m_properties);
2280
2281 //(*m_updatesThisFrame)[m_properties.ID] = &m_properties;
2282 }
2283
2284
2285
2286
2287 }
2288 public override void SetRigidBody(RigidBody body)
2289 {
2290 Rigidbody = body;
2291 }
2292 internal static bool AlmostEqual(ref Vector3 v1, ref Vector3 v2, float nEpsilon)
2293 {
2294 return
2295 (((v1.X - nEpsilon) < v2.X) && (v2.X < (v1.X + nEpsilon))) &&
2296 (((v1.Y - nEpsilon) < v2.Y) && (v2.Y < (v1.Y + nEpsilon))) &&
2297 (((v1.Z - nEpsilon) < v2.Z) && (v2.Z < (v1.Z + nEpsilon)));
2298 }
2299
2300 internal static bool AlmostEqual(ref Quaternion v1, ref Quaternion v2, float nEpsilon)
2301 {
2302 return
2303 (((v1.X - nEpsilon) < v2.X) && (v2.X < (v1.X + nEpsilon))) &&
2304 (((v1.Y - nEpsilon) < v2.Y) && (v2.Y < (v1.Y + nEpsilon))) &&
2305 (((v1.Z - nEpsilon) < v2.Z) && (v2.Z < (v1.Z + nEpsilon))) &&
2306 (((v1.W - nEpsilon) < v2.W) && (v2.W < (v1.W + nEpsilon)));
2307 }
2308
2309 }
2310}
2311
generated by cgit v1.1 at 2024-12-16 10:53:54 +0000