opensim-development – Blame information for rev 1

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