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