aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/BulletSNPlugin/BulletSimAPI.cs
diff options
context:
space:
mode:
authorteravus2012-12-23 15:21:25 -0500
committerteravus2012-12-23 15:21:25 -0500
commit92e4f9f412046f8f7926c99c9e56c3a8b6b2edbf (patch)
treeeabcbb758a7512222e84cb51b51b6822cdbf561b /OpenSim/Region/Physics/BulletSNPlugin/BulletSimAPI.cs
parentRevert "Whitespace change to trigger bot" (diff)
downloadopensim-SC_OLD-92e4f9f412046f8f7926c99c9e56c3a8b6b2edbf.zip
opensim-SC_OLD-92e4f9f412046f8f7926c99c9e56c3a8b6b2edbf.tar.gz
opensim-SC_OLD-92e4f9f412046f8f7926c99c9e56c3a8b6b2edbf.tar.bz2
opensim-SC_OLD-92e4f9f412046f8f7926c99c9e56c3a8b6b2edbf.tar.xz
* Initial commit of BulletSimN (BulletSNPlugin). Purely C# implementation of BulletSim. This is designed to be /as close as possible/ to the BulletSim plugin while still being entirely in the managed space to make keeping it up to date easy as possible (no thinking work). This implementation is /slower/ then the c++ version just because it's fully managed, so it's not appropriate for huge sims, but it will run small ones OK. At the moment, it supports all known features of BulletSim. Think of it like.. POS but everything works. To use this plugin, set the physics plugin to BulletSimN.
Diffstat (limited to 'OpenSim/Region/Physics/BulletSNPlugin/BulletSimAPI.cs')
-rw-r--r--OpenSim/Region/Physics/BulletSNPlugin/BulletSimAPI.cs1604
1 files changed, 1604 insertions, 0 deletions
diff --git a/OpenSim/Region/Physics/BulletSNPlugin/BulletSimAPI.cs b/OpenSim/Region/Physics/BulletSNPlugin/BulletSimAPI.cs
new file mode 100644
index 0000000..6af59d6
--- /dev/null
+++ b/OpenSim/Region/Physics/BulletSNPlugin/BulletSimAPI.cs
@@ -0,0 +1,1604 @@
1/*
2 * Copyright (c) Contributors, http://opensimulator.org/
3 * See CONTRIBUTORS.TXT for a full list of copyright holders.
4 *
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions are met:
7 * * Redistributions of source code must retain the above copyright
8 * notice, this list of conditions and the following disclaimer.
9 * * Redistributions in binary form must reproduce the above copyrightD
10 * notice, this list of conditions and the following disclaimer in the
11 * documentation and/or other materials provided with the distribution.
12 * * Neither the name of the OpenSimulator Project nor the
13 * names of its contributors may be used to endorse or promote products
14 * derived from this software without specific prior written permission.
15 *
16 * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
17 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
20 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27using System;
28using System.Collections.Generic;
29using System.IO;
30using System.Runtime.InteropServices;
31using System.Security;
32using System.Text;
33using BulletXNA;
34using OpenMetaverse;
35using BulletXNA.LinearMath;
36using BulletXNA.BulletCollision;
37using BulletXNA.BulletDynamics;
38using BulletXNA.BulletCollision.CollisionDispatch;
39using OpenSim.Framework;
40
41namespace OpenSim.Region.Physics.BulletSNPlugin {
42
43// Classes to allow some type checking for the API
44// These hold pointers to allocated objects in the unmanaged space.
45
46
47
48 // Constraint type values as defined by Bullet
49public enum ConstraintType : int
50{
51 POINT2POINT_CONSTRAINT_TYPE = 3,
52 HINGE_CONSTRAINT_TYPE,
53 CONETWIST_CONSTRAINT_TYPE,
54 D6_CONSTRAINT_TYPE,
55 SLIDER_CONSTRAINT_TYPE,
56 CONTACT_CONSTRAINT_TYPE,
57 D6_SPRING_CONSTRAINT_TYPE,
58 MAX_CONSTRAINT_TYPE
59}
60
61
62// ===============================================================================
63[StructLayout(LayoutKind.Sequential)]
64public struct ConvexHull
65{
66 Vector3 Offset;
67 int VertexCount;
68 Vector3[] Vertices;
69}
70public enum BSPhysicsShapeType
71{
72 SHAPE_UNKNOWN = 0,
73 SHAPE_CAPSULE = 1,
74 SHAPE_BOX = 2,
75 SHAPE_CONE = 3,
76 SHAPE_CYLINDER = 4,
77 SHAPE_SPHERE = 5,
78 SHAPE_MESH = 6,
79 SHAPE_HULL = 7,
80 // following defined by BulletSim
81 SHAPE_GROUNDPLANE = 20,
82 SHAPE_TERRAIN = 21,
83 SHAPE_COMPOUND = 22,
84 SHAPE_HEIGHTMAP = 23,
85};
86
87// The native shapes have predefined shape hash keys
88public enum FixedShapeKey : ulong
89{
90 KEY_NONE = 0,
91 KEY_BOX = 1,
92 KEY_SPHERE = 2,
93 KEY_CONE = 3,
94 KEY_CYLINDER = 4,
95 KEY_CAPSULE = 5,
96}
97
98[StructLayout(LayoutKind.Sequential)]
99public struct ShapeData
100{
101 public uint ID;
102 public BSPhysicsShapeType Type;
103 public Vector3 Position;
104 public Quaternion Rotation;
105 public Vector3 Velocity;
106 public Vector3 Scale;
107 public float Mass;
108 public float Buoyancy;
109 public System.UInt64 HullKey;
110 public System.UInt64 MeshKey;
111 public float Friction;
112 public float Restitution;
113 public float Collidable; // true of things bump into this
114 public float Static; // true if a static object. Otherwise gravity, etc.
115 public float Solid; // true if object cannot be passed through
116 public Vector3 Size;
117
118 // note that bools are passed as floats since bool size changes by language and architecture
119 public const float numericTrue = 1f;
120 public const float numericFalse = 0f;
121}
122[StructLayout(LayoutKind.Sequential)]
123public struct SweepHit
124{
125 public uint ID;
126 public float Fraction;
127 public Vector3 Normal;
128 public Vector3 Point;
129}
130[StructLayout(LayoutKind.Sequential)]
131public struct RaycastHit
132{
133 public uint ID;
134 public float Fraction;
135 public Vector3 Normal;
136}
137[StructLayout(LayoutKind.Sequential)]
138public struct CollisionDesc
139{
140 public uint aID;
141 public uint bID;
142 public Vector3 point;
143 public Vector3 normal;
144}
145[StructLayout(LayoutKind.Sequential)]
146public struct EntityProperties
147{
148 public uint ID;
149 public Vector3 Position;
150 public Quaternion Rotation;
151 public Vector3 Velocity;
152 public Vector3 Acceleration;
153 public Vector3 RotationalVelocity;
154 public override string ToString()
155 {
156 return string.Format("ID:{0}, Pos:<{1:F},{2:F},{3:F}>, Rot:<{4:F},{5:F},{6:F},{7:F}>, LVel:<{8:F},{9:F},{10:F}>, AVel:<{11:F},{12:F},{13:F}>",
157 ID.ToString(),
158 Position.X,Position.Y,Position.Z,
159 Rotation.X,Rotation.Y,Rotation.Z,Rotation.W,
160 Velocity.X,Velocity.Y,Velocity.Z,
161 RotationalVelocity.X,RotationalVelocity.Y,RotationalVelocity.Z
162 );
163 }
164}
165
166// Format of this structure must match the definition in the C++ code
167// NOTE: adding the X causes compile breaks if used. These are unused symbols
168// that can be removed from both here and the unmanaged definition of this structure.
169[StructLayout(LayoutKind.Sequential)]
170public struct ConfigurationParameters
171{
172 public float defaultFriction;
173 public float defaultDensity;
174 public float defaultRestitution;
175 public float collisionMargin;
176 public float gravity;
177
178 public float XlinearDamping;
179 public float XangularDamping;
180 public float XdeactivationTime;
181 public float XlinearSleepingThreshold;
182 public float XangularSleepingThreshold;
183 public float XccdMotionThreshold;
184 public float XccdSweptSphereRadius;
185 public float XcontactProcessingThreshold;
186
187 public float XterrainImplementation;
188 public float XterrainFriction;
189 public float XterrainHitFraction;
190 public float XterrainRestitution;
191 public float XterrainCollisionMargin;
192
193 public float XavatarFriction;
194 public float XavatarStandingFriction;
195 public float XavatarDensity;
196 public float XavatarRestitution;
197 public float XavatarCapsuleWidth;
198 public float XavatarCapsuleDepth;
199 public float XavatarCapsuleHeight;
200 public float XavatarContactProcessingThreshold;
201
202 public float XvehicleAngularDamping;
203
204 public float maxPersistantManifoldPoolSize;
205 public float maxCollisionAlgorithmPoolSize;
206 public float shouldDisableContactPoolDynamicAllocation;
207 public float shouldForceUpdateAllAabbs;
208 public float shouldRandomizeSolverOrder;
209 public float shouldSplitSimulationIslands;
210 public float shouldEnableFrictionCaching;
211 public float numberOfSolverIterations;
212
213 public float XlinksetImplementation;
214 public float XlinkConstraintUseFrameOffset;
215 public float XlinkConstraintEnableTransMotor;
216 public float XlinkConstraintTransMotorMaxVel;
217 public float XlinkConstraintTransMotorMaxForce;
218 public float XlinkConstraintERP;
219 public float XlinkConstraintCFM;
220 public float XlinkConstraintSolverIterations;
221
222 public float physicsLoggingFrames;
223
224 public const float numericTrue = 1f;
225 public const float numericFalse = 0f;
226}
227
228
229// The states a bullet collision object can have
230
231public enum ActivationState : uint
232{
233 UNDEFINED = 0,
234 ACTIVE_TAG = 1,
235 ISLAND_SLEEPING = 2,
236 WANTS_DEACTIVATION = 3,
237 DISABLE_DEACTIVATION = 4,
238 DISABLE_SIMULATION = 5,
239}
240
241public enum CollisionObjectTypes : int
242{
243 CO_COLLISION_OBJECT = 1 << 0,
244 CO_RIGID_BODY = 1 << 1,
245 CO_GHOST_OBJECT = 1 << 2,
246 CO_SOFT_BODY = 1 << 3,
247 CO_HF_FLUID = 1 << 4,
248 CO_USER_TYPE = 1 << 5,
249}
250
251// Values used by Bullet and BulletSim to control object properties.
252// Bullet's "CollisionFlags" has more to do with operations on the
253// object (if collisions happen, if gravity effects it, ...).
254 [Flags]
255public enum CollisionFlags : uint
256{
257 CF_STATIC_OBJECT = 1 << 0,
258 CF_KINEMATIC_OBJECT = 1 << 1,
259 CF_NO_CONTACT_RESPONSE = 1 << 2,
260 CF_CUSTOM_MATERIAL_CALLBACK = 1 << 3,
261 CF_CHARACTER_OBJECT = 1 << 4,
262 CF_DISABLE_VISUALIZE_OBJECT = 1 << 5,
263 CF_DISABLE_SPU_COLLISION_PROCESS = 1 << 6,
264 // Following used by BulletSim to control collisions and updates
265 BS_SUBSCRIBE_COLLISION_EVENTS = 1 << 10,
266 BS_FLOATS_ON_WATER = 1 << 11,
267 BS_VEHICLE_COLLISIONS = 1 << 12,
268 BS_NONE = 0,
269 BS_ALL = 0xFFFFFFFF,
270
271 // These are the collision flags switched depending on physical state.
272 // The other flags are used for other things and should not be fooled with.
273 BS_ACTIVE = CF_STATIC_OBJECT
274 | CF_KINEMATIC_OBJECT
275 | CF_NO_CONTACT_RESPONSE
276};
277
278// Values for collisions groups and masks
279public enum CollisionFilterGroups : uint
280{
281 // Don't use the bit definitions!! Define the use in a
282 // filter/mask definition below. This way collision interactions
283 // are more easily debugged.
284 BNoneGroup = 0,
285 BDefaultGroup = 1 << 0,
286 BStaticGroup = 1 << 1,
287 BKinematicGroup = 1 << 2,
288 BDebrisGroup = 1 << 3,
289 BSensorTrigger = 1 << 4,
290 BCharacterGroup = 1 << 5,
291 BAllGroup = 0xFFFFFFFF,
292 // Filter groups defined by BulletSim
293 BGroundPlaneGroup = 1 << 10,
294 BTerrainGroup = 1 << 11,
295 BRaycastGroup = 1 << 12,
296 BSolidGroup = 1 << 13,
297 // BLinksetGroup = xx // a linkset proper is either static or dynamic
298 BLinksetChildGroup = 1 << 14,
299 // The collsion filters and masked are defined in one place -- don't want them scattered
300 AvatarGroup = BCharacterGroup,
301 AvatarMask = BAllGroup,
302 ObjectGroup = BSolidGroup,
303 ObjectMask = BAllGroup,
304 StaticObjectGroup = BStaticGroup,
305 StaticObjectMask = AvatarGroup | ObjectGroup, // static things don't interact with much
306 LinksetGroup = BLinksetChildGroup,
307 LinksetMask = BAllGroup & ~BLinksetChildGroup, // linkset objects don't collide with each other
308 VolumeDetectGroup = BSensorTrigger,
309 VolumeDetectMask = ~BSensorTrigger,
310 TerrainGroup = BTerrainGroup,
311 TerrainMask = BAllGroup & ~BStaticGroup, // static objects on the ground don't collide
312 GroundPlaneGroup = BGroundPlaneGroup,
313 GroundPlaneMask = BAllGroup
314
315};
316
317// CFM controls the 'hardness' of the constraint. 0=fixed, 0..1=violatable. Default=0
318// ERP controls amount of correction per tick. Usable range=0.1..0.8. Default=0.2.
319public enum ConstraintParams : int
320{
321 BT_CONSTRAINT_ERP = 1, // this one is not used in Bullet as of 20120730
322 BT_CONSTRAINT_STOP_ERP,
323 BT_CONSTRAINT_CFM,
324 BT_CONSTRAINT_STOP_CFM,
325};
326public enum ConstraintParamAxis : int
327{
328 AXIS_LINEAR_X = 0,
329 AXIS_LINEAR_Y,
330 AXIS_LINEAR_Z,
331 AXIS_ANGULAR_X,
332 AXIS_ANGULAR_Y,
333 AXIS_ANGULAR_Z,
334 AXIS_LINEAR_ALL = 20, // these last three added by BulletSim so we don't have to do zillions of calls
335 AXIS_ANGULAR_ALL,
336 AXIS_ALL
337};
338
339// ===============================================================================
340static class BulletSimAPI {
341 private static int m_collisionsThisFrame;
342 public delegate void DebugLogCallback(string msg);
343 /// <summary>
344 ///
345 /// </summary>
346 /// <param name="p"></param>
347 /// <param name="p_2"></param>
348 internal static bool RemoveObjectFromWorld2(object pWorld, object pBody)
349 {
350 DiscreteDynamicsWorld world = pWorld as DiscreteDynamicsWorld;
351 RigidBody body = pBody as RigidBody;
352 world.RemoveRigidBody(body);
353 return true;
354 }
355
356 internal static void SetRestitution2(object pBody, float pRestitution)
357 {
358 RigidBody body = pBody as RigidBody;
359 body.SetRestitution(pRestitution);
360 }
361
362 internal static void SetMargin2(object pShape, float pMargin)
363 {
364 CollisionShape shape = pShape as CollisionShape;
365 shape.SetMargin(pMargin);
366 }
367
368 internal static void SetLocalScaling2(object pShape, Vector3 pScale)
369 {
370 CollisionShape shape = pShape as CollisionShape;
371 IndexedVector3 vec = new IndexedVector3(pScale.X, pScale.Y, pScale.Z);
372 shape.SetLocalScaling(ref vec);
373
374 }
375
376 internal static void SetContactProcessingThreshold2(object pBody, float contactprocessingthreshold)
377 {
378 RigidBody body = pBody as RigidBody;
379 body.SetContactProcessingThreshold(contactprocessingthreshold);
380 }
381
382 internal static void SetCcdMotionThreshold2(object pBody, float pccdMotionThreashold)
383 {
384 RigidBody body = pBody as RigidBody;
385 body.SetCcdMotionThreshold(pccdMotionThreashold);
386 }
387
388 internal static void SetCcdSweptSphereRadius2(object pBody, float pCcdSweptSphereRadius)
389 {
390 RigidBody body = pBody as RigidBody;
391 body.SetCcdSweptSphereRadius(pCcdSweptSphereRadius);
392 }
393
394 internal static void SetAngularFactorV2(object pBody, Vector3 pAngularFactor)
395 {
396 RigidBody body = pBody as RigidBody;
397 body.SetAngularFactor(new IndexedVector3(pAngularFactor.X, pAngularFactor.Y, pAngularFactor.Z));
398 }
399
400 internal static CollisionFlags AddToCollisionFlags2(object pBody, CollisionFlags pcollisionFlags)
401 {
402 CollisionObject body = pBody as CollisionObject;
403 CollisionFlags existingcollisionFlags = (CollisionFlags)(uint)body.GetCollisionFlags();
404 existingcollisionFlags |= pcollisionFlags;
405 body.SetCollisionFlags((BulletXNA.BulletCollision.CollisionFlags)(uint)existingcollisionFlags);
406 return (CollisionFlags) (uint) existingcollisionFlags;
407 }
408
409 internal static void AddObjectToWorld2(object pWorld, object pBody)
410 {
411 RigidBody body = pBody as RigidBody;
412 DiscreteDynamicsWorld world = pWorld as DiscreteDynamicsWorld;
413 //if (!(body.GetCollisionShape().GetShapeType() == BroadphaseNativeTypes.STATIC_PLANE_PROXYTYPE && body.GetCollisionShape().GetShapeType() == BroadphaseNativeTypes.TERRAIN_SHAPE_PROXYTYPE))
414
415 world.AddRigidBody(body);
416
417 //if (body.GetBroadphaseHandle() != null)
418 // world.UpdateSingleAabb(body);
419 }
420
421 internal static void AddObjectToWorld2(object pWorld, object pBody, Vector3 _position, Quaternion _orientation)
422 {
423 RigidBody body = pBody as RigidBody;
424 DiscreteDynamicsWorld world = pWorld as DiscreteDynamicsWorld;
425 //if (!(body.GetCollisionShape().GetShapeType() == BroadphaseNativeTypes.STATIC_PLANE_PROXYTYPE && body.GetCollisionShape().GetShapeType() == BroadphaseNativeTypes.TERRAIN_SHAPE_PROXYTYPE))
426
427 world.AddRigidBody(body);
428 IndexedVector3 vposition = new IndexedVector3(_position.X, _position.Y, _position.Z);
429 IndexedQuaternion vquaternion = new IndexedQuaternion(_orientation.X, _orientation.Y, _orientation.Z,
430 _orientation.W);
431 IndexedMatrix mat = IndexedMatrix.CreateFromQuaternion(vquaternion);
432 mat._origin = vposition;
433 body.SetWorldTransform(mat);
434 //if (body.GetBroadphaseHandle() != null)
435 // world.UpdateSingleAabb(body);
436 }
437
438 internal static void ForceActivationState2(object pBody, ActivationState pActivationState)
439 {
440 CollisionObject body = pBody as CollisionObject;
441 body.ForceActivationState((BulletXNA.BulletCollision.ActivationState)(uint)pActivationState);
442 }
443
444 internal static void UpdateSingleAabb2(object pWorld, object pBody)
445 {
446 CollisionObject body = pBody as CollisionObject;
447 DiscreteDynamicsWorld world = pWorld as DiscreteDynamicsWorld;
448 world.UpdateSingleAabb(body);
449 }
450
451 internal static bool SetCollisionGroupMask2(object pBody, uint pGroup, uint pMask)
452 {
453 RigidBody body = pBody as RigidBody;
454 body.GetBroadphaseHandle().m_collisionFilterGroup = (BulletXNA.BulletCollision.CollisionFilterGroups) pGroup;
455 body.GetBroadphaseHandle().m_collisionFilterGroup = (BulletXNA.BulletCollision.CollisionFilterGroups) pGroup;
456 if ((uint) body.GetBroadphaseHandle().m_collisionFilterGroup == 0)
457 return false;
458 return true;
459 }
460
461 internal static void ClearAllForces2(object pBody)
462 {
463 CollisionObject body = pBody as CollisionObject;
464 IndexedVector3 zeroVector = new IndexedVector3(0, 0, 0);
465 body.SetInterpolationLinearVelocity(ref zeroVector);
466 body.SetInterpolationAngularVelocity(ref zeroVector);
467 IndexedMatrix bodytransform = body.GetWorldTransform();
468
469 body.SetInterpolationWorldTransform(ref bodytransform);
470
471 if (body is RigidBody)
472 {
473 RigidBody rigidbody = body as RigidBody;
474 rigidbody.SetLinearVelocity(zeroVector);
475 rigidbody.SetAngularVelocity(zeroVector);
476 rigidbody.ClearForces();
477 }
478 }
479
480 internal static void SetInterpolationAngularVelocity2(object pBody, Vector3 pVector3)
481 {
482 RigidBody body = pBody as RigidBody;
483 IndexedVector3 vec = new IndexedVector3(pVector3.X, pVector3.Y, pVector3.Z);
484 body.SetInterpolationAngularVelocity(ref vec);
485 }
486
487 internal static void SetAngularVelocity2(object pBody, Vector3 pVector3)
488 {
489 RigidBody body = pBody as RigidBody;
490 IndexedVector3 vec = new IndexedVector3(pVector3.X, pVector3.Y, pVector3.Z);
491 body.SetAngularVelocity(ref vec);
492 }
493
494 internal static void ClearForces2(object pBody)
495 {
496 RigidBody body = pBody as RigidBody;
497 body.ClearForces();
498 }
499
500 internal static void SetTranslation2(object pBody, Vector3 _position, Quaternion _orientation)
501 {
502 RigidBody body = pBody as RigidBody;
503 IndexedVector3 vposition = new IndexedVector3(_position.X, _position.Y, _position.Z);
504 IndexedQuaternion vquaternion = new IndexedQuaternion(_orientation.X, _orientation.Y, _orientation.Z,
505 _orientation.W);
506 IndexedMatrix mat = IndexedMatrix.CreateFromQuaternion(vquaternion);
507 mat._origin = vposition;
508 body.SetWorldTransform(mat);
509
510 }
511
512 internal static Vector3 GetPosition2(object pBody)
513 {
514 RigidBody body = pBody as RigidBody;
515 IndexedVector3 pos = body.GetInterpolationWorldTransform()._origin;
516 return new Vector3(pos.X, pos.Y, pos.Z);
517 }
518
519 internal static Vector3 CalculateLocalInertia2(object pShape, float pphysMass)
520 {
521 CollisionShape shape = pShape as CollisionShape;
522 IndexedVector3 inertia = IndexedVector3.Zero;
523 shape.CalculateLocalInertia(pphysMass, out inertia);
524 return new Vector3(inertia.X, inertia.Y, inertia.Z);
525 }
526
527 internal static void SetMassProps2(object pBody, float pphysMass, Vector3 plocalInertia)
528 {
529 RigidBody body = pBody as RigidBody;
530 IndexedVector3 inertia = new IndexedVector3(plocalInertia.X, plocalInertia.Y, plocalInertia.Z);
531 body.SetMassProps(pphysMass, inertia);
532 }
533
534
535 internal static void SetObjectForce2(object pBody, Vector3 _force)
536 {
537 RigidBody body = pBody as RigidBody;
538 IndexedVector3 force = new IndexedVector3(_force.X, _force.Y, _force.Z);
539 body.SetTotalForce(ref force);
540 }
541
542 internal static void SetFriction2(object pBody, float _currentFriction)
543 {
544 RigidBody body = pBody as RigidBody;
545 body.SetFriction(_currentFriction);
546 }
547
548 internal static void SetLinearVelocity2(object pBody, Vector3 _velocity)
549 {
550 RigidBody body = pBody as RigidBody;
551 IndexedVector3 velocity = new IndexedVector3(_velocity.X, _velocity.Y, _velocity.Z);
552 body.SetLinearVelocity(velocity);
553 }
554
555 internal static void Activate2(object pBody, bool pforceactivation)
556 {
557 RigidBody body = pBody as RigidBody;
558 body.Activate(pforceactivation);
559
560 }
561
562 internal static Quaternion GetOrientation2(object pBody)
563 {
564 RigidBody body = pBody as RigidBody;
565 IndexedQuaternion mat = body.GetInterpolationWorldTransform().GetRotation();
566 return new Quaternion(mat.X, mat.Y, mat.Z, mat.W);
567 }
568
569 internal static CollisionFlags RemoveFromCollisionFlags2(object pBody, CollisionFlags pcollisionFlags)
570 {
571 RigidBody body = pBody as RigidBody;
572 CollisionFlags existingcollisionFlags = (CollisionFlags)(uint)body.GetCollisionFlags();
573 existingcollisionFlags &= ~pcollisionFlags;
574 body.SetCollisionFlags((BulletXNA.BulletCollision.CollisionFlags)(uint)existingcollisionFlags);
575 return (CollisionFlags)(uint)existingcollisionFlags;
576 }
577
578 internal static void SetGravity2(object pBody, Vector3 pGravity)
579 {
580 RigidBody body = pBody as RigidBody;
581 IndexedVector3 gravity = new IndexedVector3(pGravity.X, pGravity.Y, pGravity.Z);
582 body.SetGravity(gravity);
583 }
584
585 internal static bool DestroyConstraint2(object pBody, object pConstraint)
586 {
587 RigidBody body = pBody as RigidBody;
588 TypedConstraint constraint = pConstraint as TypedConstraint;
589 body.RemoveConstraintRef(constraint);
590 return true;
591 }
592
593 internal static bool SetLinearLimits2(object pConstraint, Vector3 low, Vector3 high)
594 {
595 Generic6DofConstraint constraint = pConstraint as Generic6DofConstraint;
596 IndexedVector3 lowlimit = new IndexedVector3(low.X, low.Y, low.Z);
597 IndexedVector3 highlimit = new IndexedVector3(high.X, high.Y, high.Z);
598 constraint.SetLinearLowerLimit(lowlimit);
599 constraint.SetLinearUpperLimit(highlimit);
600 return true;
601 }
602
603 internal static bool SetAngularLimits2(object pConstraint, Vector3 low, Vector3 high)
604 {
605 Generic6DofConstraint constraint = pConstraint as Generic6DofConstraint;
606 IndexedVector3 lowlimit = new IndexedVector3(low.X, low.Y, low.Z);
607 IndexedVector3 highlimit = new IndexedVector3(high.X, high.Y, high.Z);
608 constraint.SetAngularLowerLimit(lowlimit);
609 constraint.SetAngularUpperLimit(highlimit);
610 return true;
611 }
612
613 internal static void SetConstraintNumSolverIterations2(object pConstraint, float cnt)
614 {
615 Generic6DofConstraint constraint = pConstraint as Generic6DofConstraint;
616 constraint.SetOverrideNumSolverIterations((int)cnt);
617 }
618
619 internal static void CalculateTransforms2(object pConstraint)
620 {
621 Generic6DofConstraint constraint = pConstraint as Generic6DofConstraint;
622 constraint.CalculateTransforms();
623 }
624
625 internal static void SetConstraintEnable2(object pConstraint, float p_2)
626 {
627 Generic6DofConstraint constraint = pConstraint as Generic6DofConstraint;
628 constraint.SetEnabled((p_2 == 0) ? false : true);
629 }
630
631
632 //BulletSimAPI.Create6DofConstraint2(m_world.ptr, m_body1.ptr, m_body2.ptr,frame1, frame1rot,frame2, frame2rot,useLinearReferenceFrameA, disableCollisionsBetweenLinkedBodies));
633 internal static object Create6DofConstraint2(object pWorld, object pBody1, object pBody2, Vector3 pframe1, Quaternion pframe1rot, Vector3 pframe2, Quaternion pframe2rot, bool puseLinearReferenceFrameA, bool pdisableCollisionsBetweenLinkedBodies)
634
635 {
636 DiscreteDynamicsWorld world = pWorld as DiscreteDynamicsWorld;
637 RigidBody body1 = pBody1 as RigidBody;
638 RigidBody body2 = pBody2 as RigidBody;
639 IndexedVector3 frame1v = new IndexedVector3(pframe1.X, pframe1.Y, pframe1.Z);
640 IndexedQuaternion frame1rot = new IndexedQuaternion(pframe1rot.X, pframe1rot.Y, pframe1rot.Z, pframe1rot.W);
641 IndexedMatrix frame1 = IndexedMatrix.CreateFromQuaternion(frame1rot);
642 frame1._origin = frame1v;
643
644 IndexedVector3 frame2v = new IndexedVector3(pframe2.X, pframe2.Y, pframe2.Z);
645 IndexedQuaternion frame2rot = new IndexedQuaternion(pframe2rot.X, pframe2rot.Y, pframe2rot.Z, pframe2rot.W);
646 IndexedMatrix frame2 = IndexedMatrix.CreateFromQuaternion(frame2rot);
647 frame2._origin = frame1v;
648
649 Generic6DofConstraint consttr = new Generic6DofConstraint(body1, body2, ref frame1, ref frame2,
650 puseLinearReferenceFrameA);
651 consttr.CalculateTransforms();
652 world.AddConstraint(consttr,pdisableCollisionsBetweenLinkedBodies);
653
654 return consttr;
655 }
656
657
658 /// <summary>
659 ///
660 /// </summary>
661 /// <param name="pWorld"></param>
662 /// <param name="pBody1"></param>
663 /// <param name="pBody2"></param>
664 /// <param name="pjoinPoint"></param>
665 /// <param name="puseLinearReferenceFrameA"></param>
666 /// <param name="pdisableCollisionsBetweenLinkedBodies"></param>
667 /// <returns></returns>
668 internal static object Create6DofConstraintToPoint2(object pWorld, object pBody1, object pBody2, Vector3 pjoinPoint, bool puseLinearReferenceFrameA, bool pdisableCollisionsBetweenLinkedBodies)
669 {
670 DiscreteDynamicsWorld world = pWorld as DiscreteDynamicsWorld;
671 RigidBody body1 = pBody1 as RigidBody;
672 RigidBody body2 = pBody2 as RigidBody;
673 IndexedMatrix frame1 = new IndexedMatrix(IndexedBasisMatrix.Identity, new IndexedVector3(0, 0, 0));
674 IndexedMatrix frame2 = new IndexedMatrix(IndexedBasisMatrix.Identity, new IndexedVector3(0, 0, 0));
675
676 IndexedVector3 joinPoint = new IndexedVector3(pjoinPoint.X, pjoinPoint.Y, pjoinPoint.Z);
677 IndexedMatrix mat = IndexedMatrix.Identity;
678 mat._origin = new IndexedVector3(pjoinPoint.X, pjoinPoint.Y, pjoinPoint.Z);
679 frame1._origin = body1.GetWorldTransform().Inverse()*joinPoint;
680 frame2._origin = body2.GetWorldTransform().Inverse()*joinPoint;
681
682 Generic6DofConstraint consttr = new Generic6DofConstraint(body1, body2, ref frame1, ref frame2, puseLinearReferenceFrameA);
683 consttr.CalculateTransforms();
684 world.AddConstraint(consttr, pdisableCollisionsBetweenLinkedBodies);
685
686 return consttr;
687 }
688 //SetFrames2(m_constraint.ptr, frameA, frameArot, frameB, frameBrot);
689 internal static void SetFrames2(object pConstraint, Vector3 pframe1, Quaternion pframe1rot, Vector3 pframe2, Quaternion pframe2rot)
690 {
691 Generic6DofConstraint constraint = pConstraint as Generic6DofConstraint;
692 IndexedVector3 frame1v = new IndexedVector3(pframe1.X, pframe1.Y, pframe1.Z);
693 IndexedQuaternion frame1rot = new IndexedQuaternion(pframe1rot.X, pframe1rot.Y, pframe1rot.Z, pframe1rot.W);
694 IndexedMatrix frame1 = IndexedMatrix.CreateFromQuaternion(frame1rot);
695 frame1._origin = frame1v;
696
697 IndexedVector3 frame2v = new IndexedVector3(pframe2.X, pframe2.Y, pframe2.Z);
698 IndexedQuaternion frame2rot = new IndexedQuaternion(pframe2rot.X, pframe2rot.Y, pframe2rot.Z, pframe2rot.W);
699 IndexedMatrix frame2 = IndexedMatrix.CreateFromQuaternion(frame2rot);
700 frame2._origin = frame1v;
701 constraint.SetFrames(ref frame1, ref frame2);
702 }
703
704
705
706
707 internal static bool IsInWorld2(object pWorld, object pShapeObj)
708 {
709 DiscreteDynamicsWorld world = pWorld as DiscreteDynamicsWorld;
710 CollisionObject shape = pShapeObj as CollisionObject;
711 return world.IsInWorld(shape);
712 }
713
714 internal static void SetInterpolationLinearVelocity2(object pBody, Vector3 VehicleVelocity)
715 {
716 RigidBody body = pBody as RigidBody;
717 IndexedVector3 velocity = new IndexedVector3(VehicleVelocity.X, VehicleVelocity.Y, VehicleVelocity.Z);
718 body.SetInterpolationLinearVelocity(ref velocity);
719 }
720
721 internal static bool UseFrameOffset2(object pConstraint, float onOff)
722 {
723 Generic6DofConstraint constraint = pConstraint as Generic6DofConstraint;
724 constraint.SetUseFrameOffset((onOff == 0) ? false : true);
725 return true;
726 }
727 //SetBreakingImpulseThreshold2(m_constraint.ptr, threshold);
728 internal static bool SetBreakingImpulseThreshold2(object pConstraint, float threshold)
729 {
730 Generic6DofConstraint constraint = pConstraint as Generic6DofConstraint;
731 constraint.SetBreakingImpulseThreshold(threshold);
732 return true;
733 }
734 //BulletSimAPI.SetAngularDamping2(Prim.PhysBody.ptr, angularDamping);
735 internal static void SetAngularDamping2(object pBody, float angularDamping)
736 {
737 RigidBody body = pBody as RigidBody;
738 float lineardamping = body.GetLinearDamping();
739 body.SetDamping(lineardamping, angularDamping);
740
741 }
742
743 internal static void UpdateInertiaTensor2(object pBody)
744 {
745 RigidBody body = pBody as RigidBody;
746 body.UpdateInertiaTensor();
747 }
748
749 internal static void RecalculateCompoundShapeLocalAabb2( object pCompoundShape)
750 {
751
752 CompoundShape shape = pCompoundShape as CompoundShape;
753 shape.RecalculateLocalAabb();
754 }
755
756 //BulletSimAPI.GetCollisionFlags2(PhysBody.ptr)
757 internal static CollisionFlags GetCollisionFlags2(object pBody)
758 {
759 RigidBody body = pBody as RigidBody;
760 uint flags = (uint)body.GetCollisionFlags();
761 return (CollisionFlags) flags;
762 }
763
764 internal static void SetDamping2(object pBody, float pLinear, float pAngular)
765 {
766 RigidBody body = pBody as RigidBody;
767 body.SetDamping(pLinear, pAngular);
768 }
769 //PhysBody.ptr, PhysicsScene.Params.deactivationTime);
770 internal static void SetDeactivationTime2(object pBody, float pDeactivationTime)
771 {
772 RigidBody body = pBody as RigidBody;
773 body.SetDeactivationTime(pDeactivationTime);
774 }
775 //SetSleepingThresholds2(PhysBody.ptr, PhysicsScene.Params.linearSleepingThreshold, PhysicsScene.Params.angularSleepingThreshold);
776 internal static void SetSleepingThresholds2(object pBody, float plinearSleepingThreshold, float pangularSleepingThreshold)
777 {
778 RigidBody body = pBody as RigidBody;
779 body.SetSleepingThresholds(plinearSleepingThreshold, pangularSleepingThreshold);
780 }
781
782 internal static CollisionObjectTypes GetBodyType2(object pBody)
783 {
784 RigidBody body = pBody as RigidBody;
785 return (CollisionObjectTypes)(int) body.GetInternalType();
786 }
787
788 //BulletSimAPI.ApplyCentralForce2(PhysBody.ptr, fSum);
789 internal static void ApplyCentralForce2(object pBody, Vector3 pfSum)
790 {
791 RigidBody body = pBody as RigidBody;
792 IndexedVector3 fSum = new IndexedVector3(pfSum.X, pfSum.Y, pfSum.Z);
793 body.ApplyCentralForce(ref fSum);
794 }
795 internal static void ApplyCentralImpulse2(object pBody, Vector3 pfSum)
796 {
797 RigidBody body = pBody as RigidBody;
798 IndexedVector3 fSum = new IndexedVector3(pfSum.X, pfSum.Y, pfSum.Z);
799 body.ApplyCentralImpulse(ref fSum);
800 }
801 internal static void ApplyTorque2(object pBody, Vector3 pfSum)
802 {
803 RigidBody body = pBody as RigidBody;
804 IndexedVector3 fSum = new IndexedVector3(pfSum.X, pfSum.Y, pfSum.Z);
805 body.ApplyTorque(ref fSum);
806 }
807 internal static void ApplyTorqueImpulse2(object pBody, Vector3 pfSum)
808 {
809 RigidBody body = pBody as RigidBody;
810 IndexedVector3 fSum = new IndexedVector3(pfSum.X, pfSum.Y, pfSum.Z);
811 body.ApplyTorqueImpulse(ref fSum);
812 }
813
814 internal static void DumpRigidBody2(object p, object p_2)
815 {
816 //TODO:
817 }
818
819 internal static void DumpCollisionShape2(object p, object p_2)
820 {
821 //TODO:
822 }
823
824 internal static void DestroyObject2(object p, object p_2)
825 {
826 //TODO:
827 }
828
829 internal static void Shutdown2(object pWorld)
830 {
831 DiscreteDynamicsWorld world = pWorld as DiscreteDynamicsWorld;
832 world.Cleanup();
833 }
834
835 internal static void DeleteCollisionShape2(object p, object p_2)
836 {
837 //TODO:
838 }
839 //(sim.ptr, shape.ptr, prim.LocalID, prim.RawPosition, prim.RawOrientation);
840
841 internal static object CreateBodyFromShape2(object pWorld, object pShape, uint pLocalID, Vector3 pRawPosition, Quaternion pRawOrientation)
842 {
843 CollisionWorld world = pWorld as CollisionWorld;
844 IndexedMatrix mat =
845 IndexedMatrix.CreateFromQuaternion(new IndexedQuaternion(pRawOrientation.X, pRawOrientation.Y,
846 pRawOrientation.Z, pRawOrientation.W));
847 mat._origin = new IndexedVector3(pRawPosition.X, pRawPosition.Y, pRawPosition.Z);
848 CollisionShape shape = pShape as CollisionShape;
849 //UpdateSingleAabb2(world, shape);
850 // TODO: Feed Update array into null
851 RigidBody body = new RigidBody(0,new SimMotionState(world,pLocalID,mat,null),shape,IndexedVector3.Zero);
852
853 body.SetUserPointer(pLocalID);
854 return body;
855 }
856
857
858 internal static object CreateBodyWithDefaultMotionState2( object pShape, uint pLocalID, Vector3 pRawPosition, Quaternion pRawOrientation)
859 {
860
861 IndexedMatrix mat =
862 IndexedMatrix.CreateFromQuaternion(new IndexedQuaternion(pRawOrientation.X, pRawOrientation.Y,
863 pRawOrientation.Z, pRawOrientation.W));
864 mat._origin = new IndexedVector3(pRawPosition.X, pRawPosition.Y, pRawPosition.Z);
865
866 CollisionShape shape = pShape as CollisionShape;
867
868 // TODO: Feed Update array into null
869 RigidBody body = new RigidBody(0, new DefaultMotionState( mat, IndexedMatrix.Identity), shape, IndexedVector3.Zero);
870 body.SetWorldTransform(mat);
871 body.SetUserPointer(pLocalID);
872 return body;
873 }
874 //(m_mapInfo.terrainBody.ptr, CollisionFlags.CF_STATIC_OBJECT);
875 internal static void SetCollisionFlags2(object pBody, CollisionFlags collisionFlags)
876 {
877 RigidBody body = pBody as RigidBody;
878 body.SetCollisionFlags((BulletXNA.BulletCollision.CollisionFlags) (uint) collisionFlags);
879 }
880 //(m_mapInfo.terrainBody.ptr, PhysicsScene.Params.terrainHitFraction);
881 internal static void SetHitFraction2(object pBody, float pHitFraction)
882 {
883 RigidBody body = pBody as RigidBody;
884 body.SetHitFraction(pHitFraction);
885 }
886 //BuildCapsuleShape2(physicsScene.World.ptr, 1f, 1f, prim.Scale);
887 internal static object BuildCapsuleShape2(object pWorld, float pRadius, float pHeight, Vector3 pScale)
888 {
889 DiscreteDynamicsWorld world = pWorld as DiscreteDynamicsWorld;
890 IndexedVector3 scale = new IndexedVector3(pScale.X, pScale.Y, pScale.Z);
891 CapsuleShapeZ capsuleShapeZ = new CapsuleShapeZ(pRadius, pHeight);
892 capsuleShapeZ.SetMargin(world.WorldSettings.Params.collisionMargin);
893 capsuleShapeZ.SetLocalScaling(ref scale);
894
895 return capsuleShapeZ;
896 }
897
898 public static object Initialize2(Vector3 worldExtent, ConfigurationParameters[] o, int mMaxCollisionsPerFrame, ref List<BulletXNA.CollisionDesc> collisionArray, int mMaxUpdatesPerFrame, ref List<BulletXNA.EntityProperties> updateArray, object mDebugLogCallbackHandle)
899 {
900 CollisionWorld.WorldData.ParamData p = new CollisionWorld.WorldData.ParamData();
901
902 p.angularDamping = o[0].XangularDamping;
903 p.defaultFriction = o[0].defaultFriction;
904 p.defaultFriction = o[0].defaultFriction;
905 p.defaultDensity = o[0].defaultDensity;
906 p.defaultRestitution = o[0].defaultRestitution;
907 p.collisionMargin = o[0].collisionMargin;
908 p.gravity = o[0].gravity;
909
910 p.linearDamping = o[0].XlinearDamping;
911 p.angularDamping = o[0].XangularDamping;
912 p.deactivationTime = o[0].XdeactivationTime;
913 p.linearSleepingThreshold = o[0].XlinearSleepingThreshold;
914 p.angularSleepingThreshold = o[0].XangularSleepingThreshold;
915 p.ccdMotionThreshold = o[0].XccdMotionThreshold;
916 p.ccdSweptSphereRadius = o[0].XccdSweptSphereRadius;
917 p.contactProcessingThreshold = o[0].XcontactProcessingThreshold;
918
919 p.terrainImplementation = o[0].XterrainImplementation;
920 p.terrainFriction = o[0].XterrainFriction;
921
922 p.terrainHitFraction = o[0].XterrainHitFraction;
923 p.terrainRestitution = o[0].XterrainRestitution;
924 p.terrainCollisionMargin = o[0].XterrainCollisionMargin;
925
926 p.avatarFriction = o[0].XavatarFriction;
927 p.avatarStandingFriction = o[0].XavatarStandingFriction;
928 p.avatarDensity = o[0].XavatarDensity;
929 p.avatarRestitution = o[0].XavatarRestitution;
930 p.avatarCapsuleWidth = o[0].XavatarCapsuleWidth;
931 p.avatarCapsuleDepth = o[0].XavatarCapsuleDepth;
932 p.avatarCapsuleHeight = o[0].XavatarCapsuleHeight;
933 p.avatarContactProcessingThreshold = o[0].XavatarContactProcessingThreshold;
934
935 p.vehicleAngularDamping = o[0].XvehicleAngularDamping;
936
937 p.maxPersistantManifoldPoolSize = o[0].maxPersistantManifoldPoolSize;
938 p.maxCollisionAlgorithmPoolSize = o[0].maxCollisionAlgorithmPoolSize;
939 p.shouldDisableContactPoolDynamicAllocation = o[0].shouldDisableContactPoolDynamicAllocation;
940 p.shouldForceUpdateAllAabbs = o[0].shouldForceUpdateAllAabbs;
941 p.shouldRandomizeSolverOrder = o[0].shouldRandomizeSolverOrder;
942 p.shouldSplitSimulationIslands = o[0].shouldSplitSimulationIslands;
943 p.shouldEnableFrictionCaching = o[0].shouldEnableFrictionCaching;
944 p.numberOfSolverIterations = o[0].numberOfSolverIterations;
945
946 p.linksetImplementation = o[0].XlinksetImplementation;
947 p.linkConstraintUseFrameOffset = o[0].XlinkConstraintUseFrameOffset;
948 p.linkConstraintEnableTransMotor = o[0].XlinkConstraintEnableTransMotor;
949 p.linkConstraintTransMotorMaxVel = o[0].XlinkConstraintTransMotorMaxVel;
950 p.linkConstraintTransMotorMaxForce = o[0].XlinkConstraintTransMotorMaxForce;
951 p.linkConstraintERP = o[0].XlinkConstraintERP;
952 p.linkConstraintCFM = o[0].XlinkConstraintCFM;
953 p.linkConstraintSolverIterations = o[0].XlinkConstraintSolverIterations;
954 p.physicsLoggingFrames = o[0].physicsLoggingFrames;
955 DefaultCollisionConstructionInfo ccci = new DefaultCollisionConstructionInfo();
956
957 DefaultCollisionConfiguration cci = new DefaultCollisionConfiguration();
958 CollisionDispatcher m_dispatcher = new CollisionDispatcher(cci);
959
960
961 if (p.maxPersistantManifoldPoolSize > 0)
962 cci.m_persistentManifoldPoolSize = (int)p.maxPersistantManifoldPoolSize;
963 if (p.shouldDisableContactPoolDynamicAllocation !=0)
964 m_dispatcher.SetDispatcherFlags(DispatcherFlags.CD_DISABLE_CONTACTPOOL_DYNAMIC_ALLOCATION);
965 //if (p.maxCollisionAlgorithmPoolSize >0 )
966
967 DbvtBroadphase m_broadphase = new DbvtBroadphase();
968 //IndexedVector3 aabbMin = new IndexedVector3(0, 0, 0);
969 //IndexedVector3 aabbMax = new IndexedVector3(256, 256, 256);
970
971 //AxisSweep3Internal m_broadphase2 = new AxisSweep3Internal(ref aabbMin, ref aabbMax, Convert.ToInt32(0xfffe), 0xffff, ushort.MaxValue/2, null, true);
972 m_broadphase.GetOverlappingPairCache().SetInternalGhostPairCallback(new GhostPairCallback());
973
974 SequentialImpulseConstraintSolver m_solver = new SequentialImpulseConstraintSolver();
975
976 DiscreteDynamicsWorld world = new DiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_solver, cci);
977 world.UpdatedObjects = updateArray;
978 world.UpdatedCollisions = collisionArray;
979 world.WorldSettings.Params = p;
980 world.SetForceUpdateAllAabbs(p.shouldForceUpdateAllAabbs != 0);
981 world.GetSolverInfo().m_solverMode = SolverMode.SOLVER_USE_WARMSTARTING | SolverMode.SOLVER_SIMD;
982 if (p.shouldRandomizeSolverOrder != 0)
983 world.GetSolverInfo().m_solverMode |= SolverMode.SOLVER_RANDMIZE_ORDER;
984
985 world.GetSimulationIslandManager().SetSplitIslands(p.shouldSplitSimulationIslands != 0);
986 //world.GetDispatchInfo().m_enableSatConvex Not implemented in C# port
987
988 if (p.shouldEnableFrictionCaching != 0)
989 world.GetSolverInfo().m_solverMode |= SolverMode.SOLVER_ENABLE_FRICTION_DIRECTION_CACHING;
990
991 if (p.numberOfSolverIterations > 0)
992 world.GetSolverInfo().m_numIterations = (int) p.numberOfSolverIterations;
993
994
995 world.GetSolverInfo().m_damping = world.WorldSettings.Params.linearDamping;
996 world.GetSolverInfo().m_restitution = world.WorldSettings.Params.defaultRestitution;
997 world.GetSolverInfo().m_globalCfm = 0.0f;
998 world.GetSolverInfo().m_tau = 0.6f;
999 world.GetSolverInfo().m_friction = 0.3f;
1000 world.GetSolverInfo().m_maxErrorReduction = 20f;
1001 world.GetSolverInfo().m_numIterations = 10;
1002 world.GetSolverInfo().m_erp = 0.2f;
1003 world.GetSolverInfo().m_erp2 = 0.1f;
1004 world.GetSolverInfo().m_sor = 1.0f;
1005 world.GetSolverInfo().m_splitImpulse = false;
1006 world.GetSolverInfo().m_splitImpulsePenetrationThreshold = -0.02f;
1007 world.GetSolverInfo().m_linearSlop = 0.0f;
1008 world.GetSolverInfo().m_warmstartingFactor = 0.85f;
1009 world.GetSolverInfo().m_restingContactRestitutionThreshold = 2;
1010 world.SetForceUpdateAllAabbs(true);
1011
1012
1013 world.SetGravity(new IndexedVector3(0,0,p.gravity));
1014
1015 return world;
1016 }
1017 //m_constraint.ptr, ConstraintParams.BT_CONSTRAINT_STOP_CFM, cfm, ConstraintParamAxis.AXIS_ALL
1018 internal static bool SetConstraintParam2(object pConstraint, ConstraintParams paramIndex, float paramvalue, ConstraintParamAxis axis)
1019 {
1020 Generic6DofConstraint constrain = pConstraint as Generic6DofConstraint;
1021 if (axis == ConstraintParamAxis.AXIS_LINEAR_ALL || axis == ConstraintParamAxis.AXIS_ALL)
1022 {
1023 constrain.SetParam((BulletXNA.BulletDynamics.ConstraintParams) (int) paramIndex, paramvalue, 0);
1024 constrain.SetParam((BulletXNA.BulletDynamics.ConstraintParams) (int) paramIndex, paramvalue, 1);
1025 constrain.SetParam((BulletXNA.BulletDynamics.ConstraintParams) (int) paramIndex, paramvalue, 2);
1026 }
1027 if (axis == ConstraintParamAxis.AXIS_ANGULAR_ALL || axis == ConstraintParamAxis.AXIS_ALL)
1028 {
1029 constrain.SetParam((BulletXNA.BulletDynamics.ConstraintParams)(int)paramIndex, paramvalue, 3);
1030 constrain.SetParam((BulletXNA.BulletDynamics.ConstraintParams)(int)paramIndex, paramvalue, 4);
1031 constrain.SetParam((BulletXNA.BulletDynamics.ConstraintParams)(int)paramIndex, paramvalue, 5);
1032 }
1033 if (axis == ConstraintParamAxis.AXIS_LINEAR_ALL)
1034 {
1035 constrain.SetParam((BulletXNA.BulletDynamics.ConstraintParams)(int)paramIndex, paramvalue, (int)axis);
1036 }
1037 return true;
1038 }
1039
1040 internal static bool PushUpdate2(object pCollisionObject)
1041 {
1042 bool ret = false;
1043 RigidBody rb = pCollisionObject as RigidBody;
1044 if (rb != null)
1045 {
1046 SimMotionState sms = rb.GetMotionState() as SimMotionState;
1047 if (sms != null)
1048 {
1049 IndexedMatrix wt = IndexedMatrix.Identity;
1050 sms.GetWorldTransform(out wt);
1051 sms.SetWorldTransform(ref wt, true);
1052 ret = true;
1053 }
1054 }
1055 return ret;
1056
1057 }
1058
1059 internal static bool IsCompound2(object pShape)
1060 {
1061 CollisionShape shape = pShape as CollisionShape;
1062 return shape.IsCompound();
1063 }
1064 internal static bool IsPloyhedral2(object pShape)
1065 {
1066 CollisionShape shape = pShape as CollisionShape;
1067 return shape.IsPolyhedral();
1068 }
1069 internal static bool IsConvex2d2(object pShape)
1070 {
1071 CollisionShape shape = pShape as CollisionShape;
1072 return shape.IsConvex2d();
1073 }
1074 internal static bool IsConvex2(object pShape)
1075 {
1076 CollisionShape shape = pShape as CollisionShape;
1077 return shape.IsConvex();
1078 }
1079 internal static bool IsNonMoving2(object pShape)
1080 {
1081 CollisionShape shape = pShape as CollisionShape;
1082 return shape.IsNonMoving();
1083 }
1084 internal static bool IsConcave2(object pShape)
1085 {
1086 CollisionShape shape = pShape as CollisionShape;
1087 return shape.IsConcave();
1088 }
1089 internal static bool IsInfinite2(object pShape)
1090 {
1091 CollisionShape shape = pShape as CollisionShape;
1092 return shape.IsInfinite();
1093 }
1094 internal static bool IsNativeShape2(object pShape)
1095 {
1096 CollisionShape shape = pShape as CollisionShape;
1097 bool ret;
1098 switch (shape.GetShapeType())
1099 {
1100 case BroadphaseNativeTypes.BOX_SHAPE_PROXYTYPE:
1101 case BroadphaseNativeTypes.CONE_SHAPE_PROXYTYPE:
1102 case BroadphaseNativeTypes.SPHERE_SHAPE_PROXYTYPE:
1103 case BroadphaseNativeTypes.CYLINDER_SHAPE_PROXYTYPE:
1104 ret = true;
1105 break;
1106 default:
1107 ret = false;
1108 break;
1109 }
1110 return ret;
1111 }
1112 //sim.ptr, shape.ptr,prim.LocalID, prim.RawPosition, prim.RawOrientation
1113 internal static object CreateGhostFromShape2(object pWorld, object pShape, uint pLocalID, Vector3 pRawPosition, Quaternion pRawOrientation)
1114 {
1115 IndexedMatrix bodyTransform = new IndexedMatrix();
1116 bodyTransform._origin = new IndexedVector3(pRawPosition.X, pRawPosition.Y, pRawPosition.Z);
1117 bodyTransform.SetRotation(new IndexedQuaternion(pRawOrientation.X,pRawOrientation.Y,pRawOrientation.Z,pRawOrientation.W));
1118 GhostObject gObj = new PairCachingGhostObject();
1119 gObj.SetWorldTransform(bodyTransform);
1120 CollisionShape shape = pShape as CollisionShape;
1121 gObj.SetCollisionShape(shape);
1122 gObj.SetUserPointer(pLocalID);
1123 // TODO: Add to Special CollisionObjects!
1124 return gObj;
1125 }
1126
1127 public static void SetCollisionShape2(object pWorld, object pObj, object pShape)
1128 {
1129 var world = pWorld as DiscreteDynamicsWorld;
1130 var obj = pObj as CollisionObject;
1131 var shape = pShape as CollisionShape;
1132 obj.SetCollisionShape(shape);
1133
1134 }
1135 //(PhysicsScene.World.ptr, nativeShapeData)
1136 internal static object BuildNativeShape2(object pWorld, ShapeData pShapeData)
1137 {
1138 var world = pWorld as DiscreteDynamicsWorld;
1139 CollisionShape shape = null;
1140 switch (pShapeData.Type)
1141 {
1142 case BSPhysicsShapeType.SHAPE_BOX:
1143 shape = new BoxShape(new IndexedVector3(0.5f,0.5f,0.5f));
1144 break;
1145 case BSPhysicsShapeType.SHAPE_CONE:
1146 shape = new ConeShapeZ(0.5f, 1.0f);
1147 break;
1148 case BSPhysicsShapeType.SHAPE_CYLINDER:
1149 shape = new CylinderShapeZ(new IndexedVector3(0.5f, 0.5f, 0.5f));
1150 break;
1151 case BSPhysicsShapeType.SHAPE_SPHERE:
1152 shape = new SphereShape(0.5f);
1153 break;
1154
1155 }
1156 if (shape != null)
1157 {
1158 IndexedVector3 scaling = new IndexedVector3(pShapeData.Scale.X, pShapeData.Scale.Y, pShapeData.Scale.Z);
1159 shape.SetMargin(world.WorldSettings.Params.collisionMargin);
1160 shape.SetLocalScaling(ref scaling);
1161
1162 }
1163 return shape;
1164 }
1165 //PhysicsScene.World.ptr, false
1166 internal static object CreateCompoundShape2(object pWorld, bool enableDynamicAabbTree)
1167 {
1168 return new CompoundShape(enableDynamicAabbTree);
1169 }
1170
1171 internal static int GetNumberOfCompoundChildren2(object pCompoundShape)
1172 {
1173 var compoundshape = pCompoundShape as CompoundShape;
1174 return compoundshape.GetNumChildShapes();
1175 }
1176 //LinksetRoot.PhysShape.ptr, newShape.ptr, displacementPos, displacementRot
1177 internal static void AddChildShapeToCompoundShape2(object pCShape, object paddShape, Vector3 displacementPos, Quaternion displacementRot)
1178 {
1179 IndexedMatrix relativeTransform = new IndexedMatrix();
1180 var compoundshape = pCShape as CompoundShape;
1181 var addshape = paddShape as CollisionShape;
1182
1183 relativeTransform._origin = new IndexedVector3(displacementPos.X, displacementPos.Y, displacementPos.Z);
1184 relativeTransform.SetRotation(new IndexedQuaternion(displacementRot.X,displacementRot.Y,displacementRot.Z,displacementRot.W));
1185 compoundshape.AddChildShape(ref relativeTransform, addshape);
1186
1187 }
1188
1189 internal static object RemoveChildShapeFromCompoundShapeIndex2(object pCShape, int pii)
1190 {
1191 var compoundshape = pCShape as CompoundShape;
1192 CollisionShape ret = null;
1193 ret = compoundshape.GetChildShape(pii);
1194 compoundshape.RemoveChildShapeByIndex(pii);
1195 return ret;
1196 }
1197
1198 internal static object CreateGroundPlaneShape2(uint pLocalId, float pheight, float pcollisionMargin)
1199 {
1200 StaticPlaneShape m_planeshape = new StaticPlaneShape(new IndexedVector3(0,0,1),(int)pheight );
1201 m_planeshape.SetMargin(pcollisionMargin);
1202 m_planeshape.SetUserPointer(pLocalId);
1203 return m_planeshape;
1204 }
1205
1206 internal static object CreateHingeConstraint2(object pWorld, object pBody1, object ppBody2, Vector3 ppivotInA, Vector3 ppivotInB, Vector3 paxisInA, Vector3 paxisInB, bool puseLinearReferenceFrameA, bool pdisableCollisionsBetweenLinkedBodies)
1207 {
1208 HingeConstraint constrain = null;
1209 var rb1 = pBody1 as RigidBody;
1210 var rb2 = ppBody2 as RigidBody;
1211 if (rb1 != null && rb2 != null)
1212 {
1213 IndexedVector3 pivotInA = new IndexedVector3(ppivotInA.X, ppivotInA.Y, ppivotInA.Z);
1214 IndexedVector3 pivotInB = new IndexedVector3(ppivotInB.X, ppivotInB.Y, ppivotInB.Z);
1215 IndexedVector3 axisInA = new IndexedVector3(paxisInA.X, paxisInA.Y, paxisInA.Z);
1216 IndexedVector3 axisInB = new IndexedVector3(paxisInB.X, paxisInB.Y, paxisInB.Z);
1217 var world = pWorld as DiscreteDynamicsWorld;
1218 world.AddConstraint(constrain, pdisableCollisionsBetweenLinkedBodies);
1219 }
1220 return constrain;
1221 }
1222
1223 internal static bool ReleaseHeightMapInfo2(object pMapInfo)
1224 {
1225 if (pMapInfo != null)
1226 {
1227 BulletHeightMapInfo mapinfo = pMapInfo as BulletHeightMapInfo;
1228 if (mapinfo.heightMap != null)
1229 mapinfo.heightMap = null;
1230
1231
1232 }
1233 return true;
1234 }
1235
1236 internal static object CreateHullShape2(object pWorld, int pHullCount, float[] pConvHulls)
1237 {
1238 CompoundShape compoundshape = new CompoundShape(false);
1239 var world = pWorld as DiscreteDynamicsWorld;
1240
1241
1242 compoundshape.SetMargin(world.WorldSettings.Params.collisionMargin);
1243 int ii = 1;
1244
1245 for (int i = 0; i < pHullCount; i++)
1246 {
1247 int vertexCount = (int) pConvHulls[ii];
1248
1249 IndexedVector3 centroid = new IndexedVector3(pConvHulls[ii + 1], pConvHulls[ii + 2], pConvHulls[ii + 3]);
1250 IndexedMatrix childTrans = IndexedMatrix.Identity;
1251 childTrans._origin = centroid;
1252
1253 List<IndexedVector3> virts = new List<IndexedVector3>();
1254 int ender = ((ii + 4) + (vertexCount*3));
1255 for (int iii = ii + 4; iii < ender; iii+=3)
1256 {
1257
1258 virts.Add(new IndexedVector3(pConvHulls[iii], pConvHulls[iii + 1], pConvHulls[iii +2]));
1259 }
1260 ConvexHullShape convexShape = new ConvexHullShape(virts, vertexCount);
1261 convexShape.SetMargin(world.WorldSettings.Params.collisionMargin);
1262 compoundshape.AddChildShape(ref childTrans, convexShape);
1263 ii += (vertexCount*3 + 4);
1264 }
1265
1266
1267 return compoundshape;
1268 }
1269
1270 internal static object CreateMeshShape2(object pWorld, int pIndicesCount, int[] indices, int pVerticesCount, float[] verticesAsFloats)
1271 {
1272 //DumpRaw(indices,verticesAsFloats,pIndicesCount,pVerticesCount);
1273
1274 for (int iter = 0; iter < pVerticesCount; iter++)
1275 {
1276 if (verticesAsFloats[iter] > 0 && verticesAsFloats[iter] < 0.0001) verticesAsFloats[iter] = 0;
1277 if (verticesAsFloats[iter] < 0 && verticesAsFloats[iter] > -0.0001) verticesAsFloats[iter] = 0;
1278 }
1279
1280 ObjectArray<int> indicesarr = new ObjectArray<int>(indices);
1281 ObjectArray<float> vertices = new ObjectArray<float>(verticesAsFloats);
1282 DumpRaw(indicesarr,vertices,pIndicesCount,pVerticesCount);
1283 var world = pWorld as DiscreteDynamicsWorld;
1284 IndexedMesh mesh = new IndexedMesh();
1285 mesh.m_indexType = PHY_ScalarType.PHY_INTEGER;
1286 mesh.m_numTriangles = pIndicesCount/3;
1287 mesh.m_numVertices = pVerticesCount;
1288 mesh.m_triangleIndexBase = indicesarr;
1289 mesh.m_vertexBase = vertices;
1290 mesh.m_vertexStride = 3;
1291 mesh.m_vertexType = PHY_ScalarType.PHY_FLOAT;
1292 mesh.m_triangleIndexStride = 3;
1293
1294 TriangleIndexVertexArray tribuilder = new TriangleIndexVertexArray();
1295 tribuilder.AddIndexedMesh(mesh, PHY_ScalarType.PHY_INTEGER);
1296 BvhTriangleMeshShape meshShape = new BvhTriangleMeshShape(tribuilder, true,true);
1297 meshShape.SetMargin(world.WorldSettings.Params.collisionMargin);
1298 // world.UpdateSingleAabb(meshShape);
1299 return meshShape;
1300
1301 }
1302 public static void DumpRaw(ObjectArray<int>indices, ObjectArray<float> vertices, int pIndicesCount,int pVerticesCount )
1303 {
1304
1305 String fileName = "objTest3.raw";
1306 String completePath = System.IO.Path.Combine(Util.configDir(), fileName);
1307 StreamWriter sw = new StreamWriter(completePath);
1308 IndexedMesh mesh = new IndexedMesh();
1309
1310 mesh.m_indexType = PHY_ScalarType.PHY_INTEGER;
1311 mesh.m_numTriangles = pIndicesCount / 3;
1312 mesh.m_numVertices = pVerticesCount;
1313 mesh.m_triangleIndexBase = indices;
1314 mesh.m_vertexBase = vertices;
1315 mesh.m_vertexStride = 3;
1316 mesh.m_vertexType = PHY_ScalarType.PHY_FLOAT;
1317 mesh.m_triangleIndexStride = 3;
1318
1319 TriangleIndexVertexArray tribuilder = new TriangleIndexVertexArray();
1320 tribuilder.AddIndexedMesh(mesh, PHY_ScalarType.PHY_INTEGER);
1321
1322
1323
1324 for (int i = 0; i < pVerticesCount; i++)
1325 {
1326
1327 string s = vertices[indices[i * 3]].ToString("0.0000");
1328 s += " " + vertices[indices[i * 3 + 1]].ToString("0.0000");
1329 s += " " + vertices[indices[i * 3 + 2]].ToString("0.0000");
1330
1331 sw.Write(s + "\n");
1332 }
1333
1334 sw.Close();
1335 }
1336 public static void DumpRaw(int[] indices, float[] vertices, int pIndicesCount, int pVerticesCount)
1337 {
1338
1339 String fileName = "objTest6.raw";
1340 String completePath = System.IO.Path.Combine(Util.configDir(), fileName);
1341 StreamWriter sw = new StreamWriter(completePath);
1342 IndexedMesh mesh = new IndexedMesh();
1343
1344 mesh.m_indexType = PHY_ScalarType.PHY_INTEGER;
1345 mesh.m_numTriangles = pIndicesCount / 3;
1346 mesh.m_numVertices = pVerticesCount;
1347 mesh.m_triangleIndexBase = indices;
1348 mesh.m_vertexBase = vertices;
1349 mesh.m_vertexStride = 3;
1350 mesh.m_vertexType = PHY_ScalarType.PHY_FLOAT;
1351 mesh.m_triangleIndexStride = 3;
1352
1353 TriangleIndexVertexArray tribuilder = new TriangleIndexVertexArray();
1354 tribuilder.AddIndexedMesh(mesh, PHY_ScalarType.PHY_INTEGER);
1355
1356
1357 sw.WriteLine("Indices");
1358 sw.WriteLine(string.Format("int[] indices = new int[{0}];",pIndicesCount));
1359 for (int iter = 0; iter < indices.Length; iter++)
1360 {
1361 sw.WriteLine(string.Format("indices[{0}]={1};",iter,indices[iter]));
1362 }
1363 sw.WriteLine("VerticesFloats");
1364 sw.WriteLine(string.Format("float[] vertices = new float[{0}];", pVerticesCount));
1365 for (int iter = 0; iter < vertices.Length; iter++)
1366 {
1367 sw.WriteLine(string.Format("Vertices[{0}]={1};", iter, vertices[iter].ToString("0.0000")));
1368 }
1369
1370 // for (int i = 0; i < pVerticesCount; i++)
1371 // {
1372 //
1373 // string s = vertices[indices[i * 3]].ToString("0.0000");
1374 // s += " " + vertices[indices[i * 3 + 1]].ToString("0.0000");
1375 // s += " " + vertices[indices[i * 3 + 2]].ToString("0.0000");
1376 //
1377 // sw.Write(s + "\n");
1378 //}
1379
1380 sw.Close();
1381 }
1382 //PhysicsScene.World.ptr, m_mapInfo.ID, m_mapInfo.minCoords, m_mapInfo.maxCoords, m_mapInfo.heightMap, PhysicsScene.Params.terrainCollisionMargin
1383 internal static object CreateHeightMapInfo2(object pWorld, uint pId, Vector3 pminCoords, Vector3 pmaxCoords, float[] pheightMap, float pCollisionMargin)
1384 {
1385 BulletHeightMapInfo mapInfo = new BulletHeightMapInfo(pId, pheightMap, null);
1386 mapInfo.heightMap = null;
1387 mapInfo.minCoords = pminCoords;
1388 mapInfo.maxCoords = pmaxCoords;
1389 mapInfo.sizeX = (int) (pmaxCoords.X - pminCoords.X);
1390 mapInfo.sizeY = (int) (pmaxCoords.Y - pminCoords.Y);
1391 mapInfo.ID = pId;
1392 mapInfo.minZ = pminCoords.Z;
1393 mapInfo.maxZ = pmaxCoords.Z;
1394 mapInfo.collisionMargin = pCollisionMargin;
1395 if (mapInfo.minZ == mapInfo.maxZ)
1396 mapInfo.minZ -= 0.2f;
1397 mapInfo.heightMap = pheightMap;
1398
1399 return mapInfo;
1400
1401 }
1402
1403 internal static object CreateTerrainShape2(object pMapInfo)
1404 {
1405 BulletHeightMapInfo mapinfo = pMapInfo as BulletHeightMapInfo;
1406 const int upAxis = 2;
1407 const float scaleFactor = 1.0f;
1408 HeightfieldTerrainShape terrainShape = new HeightfieldTerrainShape((int)mapinfo.sizeX, (int)mapinfo.sizeY,
1409 mapinfo.heightMap, scaleFactor,
1410 mapinfo.minZ, mapinfo.maxZ, upAxis,
1411 false);
1412 terrainShape.SetMargin(mapinfo.collisionMargin + 0.5f);
1413 terrainShape.SetUseDiamondSubdivision(true);
1414 terrainShape.SetUserPointer(mapinfo.ID);
1415 return terrainShape;
1416 }
1417
1418 internal static bool TranslationalLimitMotor2(object pConstraint, float ponOff, float targetVelocity, float maxMotorForce)
1419 {
1420 TypedConstraint tconstrain = pConstraint as TypedConstraint;
1421 bool onOff = ponOff != 0;
1422 bool ret = false;
1423
1424 switch (tconstrain.GetConstraintType())
1425 {
1426 case TypedConstraintType.D6_CONSTRAINT_TYPE:
1427 Generic6DofConstraint constrain = pConstraint as Generic6DofConstraint;
1428 constrain.GetTranslationalLimitMotor().m_enableMotor[0] = onOff;
1429 constrain.GetTranslationalLimitMotor().m_targetVelocity[0] = targetVelocity;
1430 constrain.GetTranslationalLimitMotor().m_maxMotorForce[0] = maxMotorForce;
1431 ret = true;
1432 break;
1433 }
1434
1435
1436 return ret;
1437
1438 }
1439
1440 internal static int PhysicsStep2(object pWorld, float timeStep, int m_maxSubSteps, float m_fixedTimeStep, out int updatedEntityCount, out List<BulletXNA.EntityProperties> updatedEntities, out int collidersCount, out List<BulletXNA.CollisionDesc>colliders)
1441 {
1442 int epic = PhysicsStepint2(pWorld, timeStep, m_maxSubSteps, m_fixedTimeStep, out updatedEntityCount, out updatedEntities,
1443 out collidersCount, out colliders);
1444 return epic;
1445 }
1446
1447 private static int PhysicsStepint2(object pWorld,float timeStep, int m_maxSubSteps, float m_fixedTimeStep, out int updatedEntityCount, out List<BulletXNA.EntityProperties> updatedEntities, out int collidersCount, out List<BulletXNA.CollisionDesc> colliders)
1448 {
1449 int numSimSteps = 0;
1450
1451
1452 //if (updatedEntities is null)
1453 // updatedEntities = new List<BulletXNA.EntityProperties>();
1454
1455 //if (colliders is null)
1456 // colliders = new List<BulletXNA.CollisionDesc>();
1457
1458
1459 if (pWorld is DiscreteDynamicsWorld)
1460 {
1461 DiscreteDynamicsWorld world = pWorld as DiscreteDynamicsWorld;
1462
1463 numSimSteps = world.StepSimulation(timeStep, m_maxSubSteps, m_fixedTimeStep);
1464 int updates = 0;
1465
1466 updatedEntityCount = world.UpdatedObjects.Count;
1467 updatedEntities = new List<BulletXNA.EntityProperties>(world.UpdatedObjects);
1468 updatedEntityCount = updatedEntities.Count;
1469 world.UpdatedObjects.Clear();
1470
1471
1472 collidersCount = world.UpdatedCollisions.Count;
1473 colliders = new List<BulletXNA.CollisionDesc>(world.UpdatedCollisions);
1474
1475 world.UpdatedCollisions.Clear();
1476 m_collisionsThisFrame = 0;
1477 int numManifolds = world.GetDispatcher().GetNumManifolds();
1478 for (int j = 0; j < numManifolds; j++)
1479 {
1480 PersistentManifold contactManifold = world.GetDispatcher().GetManifoldByIndexInternal(j);
1481 int numContacts = contactManifold.GetNumContacts();
1482 if (numContacts == 0)
1483 continue;
1484
1485 CollisionObject objA = contactManifold.GetBody0() as CollisionObject;
1486 CollisionObject objB = contactManifold.GetBody1() as CollisionObject;
1487
1488 ManifoldPoint manifoldPoint = contactManifold.GetContactPoint(0);
1489 IndexedVector3 contactPoint = manifoldPoint.GetPositionWorldOnB();
1490 IndexedVector3 contactNormal = -manifoldPoint.m_normalWorldOnB; // make relative to A
1491
1492 RecordCollision(world, objA, objB, contactPoint, contactNormal);
1493 m_collisionsThisFrame ++;
1494 if (m_collisionsThisFrame >= 9999999)
1495 break;
1496
1497
1498 }
1499
1500
1501 }
1502 else
1503 {
1504 //if (updatedEntities is null)
1505 updatedEntities = new List<BulletXNA.EntityProperties>();
1506 updatedEntityCount = 0;
1507 //if (colliders is null)
1508 colliders = new List<BulletXNA.CollisionDesc>();
1509 collidersCount = 0;
1510 }
1511 return numSimSteps;
1512 }
1513
1514 private static void RecordCollision(CollisionWorld world,CollisionObject objA, CollisionObject objB, IndexedVector3 contact, IndexedVector3 norm)
1515 {
1516
1517 IndexedVector3 contactNormal = norm;
1518 if ((objA.GetCollisionFlags() & BulletXNA.BulletCollision.CollisionFlags.BS_WANTS_COLLISIONS) == 0 &&
1519 (objB.GetCollisionFlags() & BulletXNA.BulletCollision.CollisionFlags.BS_WANTS_COLLISIONS) == 0)
1520 {
1521 return;
1522 }
1523 uint idA = (uint)objA.GetUserPointer();
1524 uint idB = (uint)objB.GetUserPointer();
1525 if (idA > idB)
1526 {
1527 uint temp = idA;
1528 idA = idB;
1529 idB = temp;
1530 contactNormal = -contactNormal;
1531 }
1532
1533 ulong collisionID = ((ulong) idA << 32) | idB;
1534
1535 BulletXNA.CollisionDesc cDesc = new BulletXNA.CollisionDesc()
1536 {
1537 aID = idA,
1538 bID = idB,
1539 point = contact,
1540 normal = contactNormal
1541 };
1542 world.UpdatedCollisions.Add(cDesc);
1543 m_collisionsThisFrame++;
1544
1545
1546 }
1547 private static EntityProperties GetDebugProperties(object pWorld, object pBody)
1548 {
1549 EntityProperties ent = new EntityProperties();
1550 DiscreteDynamicsWorld world = pWorld as DiscreteDynamicsWorld;
1551 RigidBody body = pBody as RigidBody;
1552 IndexedMatrix transform = body.GetWorldTransform();
1553 IndexedVector3 LinearVelocity = body.GetInterpolationLinearVelocity();
1554 IndexedVector3 AngularVelocity = body.GetInterpolationAngularVelocity();
1555 IndexedQuaternion rotation = transform.GetRotation();
1556 ent.Acceleration = Vector3.Zero;
1557 ent.ID = (uint)body.GetUserPointer();
1558 ent.Position = new Vector3(transform._origin.X,transform._origin.Y,transform._origin.Z);
1559 ent.Rotation = new Quaternion(rotation.X,rotation.Y,rotation.Z,rotation.W);
1560 ent.Velocity = new Vector3(LinearVelocity.X, LinearVelocity.Y, LinearVelocity.Z);
1561 ent.RotationalVelocity = new Vector3(AngularVelocity.X, AngularVelocity.Y, AngularVelocity.Z);
1562 return ent;
1563
1564
1565 }
1566
1567
1568 internal static Vector3 GetLocalScaling2(object pBody)
1569 {
1570 CollisionShape shape = pBody as CollisionShape;
1571 IndexedVector3 scale = shape.GetLocalScaling();
1572 return new Vector3(scale.X,scale.Y,scale.Z);
1573 }
1574
1575 internal static bool RayCastGround(object pWorld, Vector3 _RayOrigin, float pRayHeight, object NotMe)
1576 {
1577 DynamicsWorld world = pWorld as DynamicsWorld;
1578 if (world != null)
1579 {
1580 if (NotMe is CollisionObject || NotMe is RigidBody)
1581 {
1582 CollisionObject AvoidBody = NotMe as CollisionObject;
1583
1584 IndexedVector3 rOrigin = new IndexedVector3(_RayOrigin.X, _RayOrigin.Y, _RayOrigin.Z);
1585 IndexedVector3 rEnd = new IndexedVector3(_RayOrigin.X, _RayOrigin.Y, _RayOrigin.Z - pRayHeight);
1586 using (
1587 ClosestNotMeRayResultCallback rayCallback = new ClosestNotMeRayResultCallback(rOrigin,
1588 rEnd, AvoidBody)
1589 )
1590 {
1591 world.RayTest(ref rOrigin, ref rEnd, rayCallback);
1592 if (rayCallback.HasHit())
1593 {
1594 IndexedVector3 hitLocation = rayCallback.m_hitPointWorld;
1595
1596 }
1597 return rayCallback.HasHit();
1598 }
1599 }
1600 }
1601 return false;
1602 }
1603}
1604}
generated by cgit v1.1 at 2024-11-18 17:21:13 +0000