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