diff options
author | Robert Adams | 2013-01-01 09:30:49 -0800 |
---|---|---|
committer | Robert Adams | 2013-01-01 17:27:32 -0800 |
commit | 04132d3af4c8f44639ea842091f86274513e2dfd (patch) | |
tree | 8afd4aa11cb7bcf15ffd7156204f73859e762f31 /OpenSim/Region/Physics/BulletSPlugin/BSAPIXNA.cs | |
parent | Fixed bugs when getting the root folder. (diff) | |
download | opensim-SC_OLD-04132d3af4c8f44639ea842091f86274513e2dfd.zip opensim-SC_OLD-04132d3af4c8f44639ea842091f86274513e2dfd.tar.gz opensim-SC_OLD-04132d3af4c8f44639ea842091f86274513e2dfd.tar.bz2 opensim-SC_OLD-04132d3af4c8f44639ea842091f86274513e2dfd.tar.xz |
BulletSim: subclass Bullet[World|Body|Shape|Constraint] for unmanaged
to have pointers and managed to have objects.
Initial paste of XNA code. Commented out.
Diffstat (limited to '')
-rwxr-xr-x | OpenSim/Region/Physics/BulletSPlugin/BSAPIXNA.cs | 1278 |
1 files changed, 1277 insertions, 1 deletions
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSAPIXNA.cs b/OpenSim/Region/Physics/BulletSPlugin/BSAPIXNA.cs index 8ed791e..f70ad30 100755 --- a/OpenSim/Region/Physics/BulletSPlugin/BSAPIXNA.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BSAPIXNA.cs | |||
@@ -29,11 +29,1287 @@ using System.Collections.Generic; | |||
29 | using System.Linq; | 29 | using System.Linq; |
30 | using System.Text; | 30 | using System.Text; |
31 | 31 | ||
32 | using BulletXNA; | ||
33 | using BulletXNA.LinearMath; | ||
34 | using BulletXNA.BulletCollision; | ||
35 | using BulletXNA.BulletDynamics; | ||
36 | using BulletXNA.BulletCollision.CollisionDispatch; | ||
37 | |||
38 | using OpenMetaverse; | ||
39 | |||
32 | namespace OpenSim.Region.Physics.BulletSPlugin | 40 | namespace OpenSim.Region.Physics.BulletSPlugin |
33 | { | 41 | { |
34 | /* | 42 | /* |
35 | public sealed class BSAPIXNA : BSAPITemplate | 43 | public sealed class BSAPIXNA : BSAPITemplate |
36 | { | 44 | { |
45 | private static int m_collisionsThisFrame; | ||
46 | private BSScene PhysicsScene { get; set; } | ||
47 | |||
48 | public override string BulletEngineName { get { return "BulletXNA"; } } | ||
49 | public override string BulletEngineVersion { get; protected set; } | ||
50 | |||
51 | public BSAPIXNA(string paramName, BSScene physScene) | ||
52 | { | ||
53 | PhysicsScene = physScene; | ||
54 | } | ||
55 | |||
56 | /// <summary> | ||
57 | /// | ||
58 | /// </summary> | ||
59 | /// <param name="p"></param> | ||
60 | /// <param name="p_2"></param> | ||
61 | public override bool RemoveObjectFromWorld2(object pWorld, object pBody) | ||
62 | { | ||
63 | DiscreteDynamicsWorld world = pWorld as DiscreteDynamicsWorld; | ||
64 | RigidBody body = pBody as RigidBody; | ||
65 | world.RemoveRigidBody(body); | ||
66 | return true; | ||
67 | } | ||
68 | |||
69 | public override void SetRestitution2(object pBody, float pRestitution) | ||
70 | { | ||
71 | RigidBody body = pBody as RigidBody; | ||
72 | body.SetRestitution(pRestitution); | ||
73 | } | ||
74 | |||
75 | public override void SetMargin2(object pShape, float pMargin) | ||
76 | { | ||
77 | CollisionShape shape = pShape as CollisionShape; | ||
78 | shape.SetMargin(pMargin); | ||
79 | } | ||
80 | |||
81 | public override void SetLocalScaling2(object pShape, Vector3 pScale) | ||
82 | { | ||
83 | CollisionShape shape = pShape as CollisionShape; | ||
84 | IndexedVector3 vec = new IndexedVector3(pScale.X, pScale.Y, pScale.Z); | ||
85 | shape.SetLocalScaling(ref vec); | ||
86 | |||
87 | } | ||
88 | |||
89 | public override void SetContactProcessingThreshold2(object pBody, float contactprocessingthreshold) | ||
90 | { | ||
91 | RigidBody body = pBody as RigidBody; | ||
92 | body.SetContactProcessingThreshold(contactprocessingthreshold); | ||
93 | } | ||
94 | |||
95 | public override void SetCcdMotionThreshold2(object pBody, float pccdMotionThreashold) | ||
96 | { | ||
97 | RigidBody body = pBody as RigidBody; | ||
98 | body.SetCcdMotionThreshold(pccdMotionThreashold); | ||
99 | } | ||
100 | |||
101 | public override void SetCcdSweptSphereRadius2(object pBody, float pCcdSweptSphereRadius) | ||
102 | { | ||
103 | RigidBody body = pBody as RigidBody; | ||
104 | body.SetCcdSweptSphereRadius(pCcdSweptSphereRadius); | ||
105 | } | ||
106 | |||
107 | public override void SetAngularFactorV2(object pBody, Vector3 pAngularFactor) | ||
108 | { | ||
109 | RigidBody body = pBody as RigidBody; | ||
110 | body.SetAngularFactor(new IndexedVector3(pAngularFactor.X, pAngularFactor.Y, pAngularFactor.Z)); | ||
111 | } | ||
112 | |||
113 | public override CollisionFlags AddToCollisionFlags2(object pBody, CollisionFlags pcollisionFlags) | ||
114 | { | ||
115 | CollisionObject body = pBody as CollisionObject; | ||
116 | CollisionFlags existingcollisionFlags = (CollisionFlags)(uint)body.GetCollisionFlags(); | ||
117 | existingcollisionFlags |= pcollisionFlags; | ||
118 | body.SetCollisionFlags((BulletXNA.BulletCollision.CollisionFlags)(uint)existingcollisionFlags); | ||
119 | return (CollisionFlags) (uint) existingcollisionFlags; | ||
120 | } | ||
121 | |||
122 | public override void AddObjectToWorld2(object pWorld, object pBody) | ||
123 | { | ||
124 | RigidBody body = pBody as RigidBody; | ||
125 | DiscreteDynamicsWorld world = pWorld as DiscreteDynamicsWorld; | ||
126 | //if (!(body.GetCollisionShape().GetShapeType() == BroadphaseNativeTypes.STATIC_PLANE_PROXYTYPE && body.GetCollisionShape().GetShapeType() == BroadphaseNativeTypes.TERRAIN_SHAPE_PROXYTYPE)) | ||
127 | |||
128 | world.AddRigidBody(body); | ||
129 | |||
130 | //if (body.GetBroadphaseHandle() != null) | ||
131 | // world.UpdateSingleAabb(body); | ||
132 | } | ||
133 | |||
134 | public override void AddObjectToWorld2(object pWorld, object pBody, Vector3 _position, Quaternion _orientation) | ||
135 | { | ||
136 | RigidBody body = pBody as RigidBody; | ||
137 | DiscreteDynamicsWorld world = pWorld as DiscreteDynamicsWorld; | ||
138 | //if (!(body.GetCollisionShape().GetShapeType() == BroadphaseNativeTypes.STATIC_PLANE_PROXYTYPE && body.GetCollisionShape().GetShapeType() == BroadphaseNativeTypes.TERRAIN_SHAPE_PROXYTYPE)) | ||
139 | |||
140 | world.AddRigidBody(body); | ||
141 | IndexedVector3 vposition = new IndexedVector3(_position.X, _position.Y, _position.Z); | ||
142 | IndexedQuaternion vquaternion = new IndexedQuaternion(_orientation.X, _orientation.Y, _orientation.Z, | ||
143 | _orientation.W); | ||
144 | IndexedMatrix mat = IndexedMatrix.CreateFromQuaternion(vquaternion); | ||
145 | mat._origin = vposition; | ||
146 | body.SetWorldTransform(mat); | ||
147 | //if (body.GetBroadphaseHandle() != null) | ||
148 | // world.UpdateSingleAabb(body); | ||
149 | } | ||
150 | |||
151 | public override void ForceActivationState2(object pBody, ActivationState pActivationState) | ||
152 | { | ||
153 | CollisionObject body = pBody as CollisionObject; | ||
154 | body.ForceActivationState((BulletXNA.BulletCollision.ActivationState)(uint)pActivationState); | ||
155 | } | ||
156 | |||
157 | public override void UpdateSingleAabb2(object pWorld, object pBody) | ||
158 | { | ||
159 | CollisionObject body = pBody as CollisionObject; | ||
160 | DiscreteDynamicsWorld world = pWorld as DiscreteDynamicsWorld; | ||
161 | world.UpdateSingleAabb(body); | ||
162 | } | ||
163 | |||
164 | public override bool SetCollisionGroupMask2(object pBody, uint pGroup, uint pMask) | ||
165 | { | ||
166 | RigidBody body = pBody as RigidBody; | ||
167 | body.GetBroadphaseHandle().m_collisionFilterGroup = (BulletXNA.BulletCollision.CollisionFilterGroups) pGroup; | ||
168 | body.GetBroadphaseHandle().m_collisionFilterGroup = (BulletXNA.BulletCollision.CollisionFilterGroups) pGroup; | ||
169 | if ((uint) body.GetBroadphaseHandle().m_collisionFilterGroup == 0) | ||
170 | return false; | ||
171 | return true; | ||
172 | } | ||
173 | |||
174 | public override void ClearAllForces2(object pBody) | ||
175 | { | ||
176 | CollisionObject body = pBody as CollisionObject; | ||
177 | IndexedVector3 zeroVector = new IndexedVector3(0, 0, 0); | ||
178 | body.SetInterpolationLinearVelocity(ref zeroVector); | ||
179 | body.SetInterpolationAngularVelocity(ref zeroVector); | ||
180 | IndexedMatrix bodytransform = body.GetWorldTransform(); | ||
181 | |||
182 | body.SetInterpolationWorldTransform(ref bodytransform); | ||
183 | |||
184 | if (body is RigidBody) | ||
185 | { | ||
186 | RigidBody rigidbody = body as RigidBody; | ||
187 | rigidbody.SetLinearVelocity(zeroVector); | ||
188 | rigidbody.SetAngularVelocity(zeroVector); | ||
189 | rigidbody.ClearForces(); | ||
190 | } | ||
191 | } | ||
192 | |||
193 | public override void SetInterpolationAngularVelocity2(object pBody, Vector3 pVector3) | ||
194 | { | ||
195 | RigidBody body = pBody as RigidBody; | ||
196 | IndexedVector3 vec = new IndexedVector3(pVector3.X, pVector3.Y, pVector3.Z); | ||
197 | body.SetInterpolationAngularVelocity(ref vec); | ||
198 | } | ||
199 | |||
200 | public override void SetAngularVelocity2(object pBody, Vector3 pVector3) | ||
201 | { | ||
202 | RigidBody body = pBody as RigidBody; | ||
203 | IndexedVector3 vec = new IndexedVector3(pVector3.X, pVector3.Y, pVector3.Z); | ||
204 | body.SetAngularVelocity(ref vec); | ||
205 | } | ||
206 | |||
207 | public override void ClearForces2(object pBody) | ||
208 | { | ||
209 | RigidBody body = pBody as RigidBody; | ||
210 | body.ClearForces(); | ||
211 | } | ||
212 | |||
213 | public override void SetTranslation2(object pBody, Vector3 _position, Quaternion _orientation) | ||
214 | { | ||
215 | RigidBody body = pBody as RigidBody; | ||
216 | IndexedVector3 vposition = new IndexedVector3(_position.X, _position.Y, _position.Z); | ||
217 | IndexedQuaternion vquaternion = new IndexedQuaternion(_orientation.X, _orientation.Y, _orientation.Z, | ||
218 | _orientation.W); | ||
219 | IndexedMatrix mat = IndexedMatrix.CreateFromQuaternion(vquaternion); | ||
220 | mat._origin = vposition; | ||
221 | body.SetWorldTransform(mat); | ||
222 | |||
223 | } | ||
224 | |||
225 | public override Vector3 GetPosition2(object pBody) | ||
226 | { | ||
227 | RigidBody body = pBody as RigidBody; | ||
228 | IndexedVector3 pos = body.GetInterpolationWorldTransform()._origin; | ||
229 | return new Vector3(pos.X, pos.Y, pos.Z); | ||
230 | } | ||
231 | |||
232 | public override Vector3 CalculateLocalInertia2(object pShape, float pphysMass) | ||
233 | { | ||
234 | CollisionShape shape = pShape as CollisionShape; | ||
235 | IndexedVector3 inertia = IndexedVector3.Zero; | ||
236 | shape.CalculateLocalInertia(pphysMass, out inertia); | ||
237 | return new Vector3(inertia.X, inertia.Y, inertia.Z); | ||
238 | } | ||
239 | |||
240 | public override void SetMassProps2(object pBody, float pphysMass, Vector3 plocalInertia) | ||
241 | { | ||
242 | RigidBody body = pBody as RigidBody; | ||
243 | IndexedVector3 inertia = new IndexedVector3(plocalInertia.X, plocalInertia.Y, plocalInertia.Z); | ||
244 | body.SetMassProps(pphysMass, inertia); | ||
245 | } | ||
246 | |||
247 | |||
248 | public override void SetObjectForce2(object pBody, Vector3 _force) | ||
249 | { | ||
250 | RigidBody body = pBody as RigidBody; | ||
251 | IndexedVector3 force = new IndexedVector3(_force.X, _force.Y, _force.Z); | ||
252 | body.SetTotalForce(ref force); | ||
253 | } | ||
254 | |||
255 | public override void SetFriction2(object pBody, float _currentFriction) | ||
256 | { | ||
257 | RigidBody body = pBody as RigidBody; | ||
258 | body.SetFriction(_currentFriction); | ||
259 | } | ||
260 | |||
261 | public override void SetLinearVelocity2(object pBody, Vector3 _velocity) | ||
262 | { | ||
263 | RigidBody body = pBody as RigidBody; | ||
264 | IndexedVector3 velocity = new IndexedVector3(_velocity.X, _velocity.Y, _velocity.Z); | ||
265 | body.SetLinearVelocity(velocity); | ||
266 | } | ||
267 | |||
268 | public override void Activate2(object pBody, bool pforceactivation) | ||
269 | { | ||
270 | RigidBody body = pBody as RigidBody; | ||
271 | body.Activate(pforceactivation); | ||
272 | |||
273 | } | ||
274 | |||
275 | public override Quaternion GetOrientation2(object pBody) | ||
276 | { | ||
277 | RigidBody body = pBody as RigidBody; | ||
278 | IndexedQuaternion mat = body.GetInterpolationWorldTransform().GetRotation(); | ||
279 | return new Quaternion(mat.X, mat.Y, mat.Z, mat.W); | ||
280 | } | ||
281 | |||
282 | public override CollisionFlags RemoveFromCollisionFlags2(object pBody, CollisionFlags pcollisionFlags) | ||
283 | { | ||
284 | RigidBody body = pBody as RigidBody; | ||
285 | CollisionFlags existingcollisionFlags = (CollisionFlags)(uint)body.GetCollisionFlags(); | ||
286 | existingcollisionFlags &= ~pcollisionFlags; | ||
287 | body.SetCollisionFlags((BulletXNA.BulletCollision.CollisionFlags)(uint)existingcollisionFlags); | ||
288 | return (CollisionFlags)(uint)existingcollisionFlags; | ||
289 | } | ||
290 | |||
291 | public override void SetGravity2(object pBody, Vector3 pGravity) | ||
292 | { | ||
293 | RigidBody body = pBody as RigidBody; | ||
294 | IndexedVector3 gravity = new IndexedVector3(pGravity.X, pGravity.Y, pGravity.Z); | ||
295 | body.SetGravity(gravity); | ||
296 | } | ||
297 | |||
298 | public override bool DestroyConstraint2(object pBody, object pConstraint) | ||
299 | { | ||
300 | RigidBody body = pBody as RigidBody; | ||
301 | TypedConstraint constraint = pConstraint as TypedConstraint; | ||
302 | body.RemoveConstraintRef(constraint); | ||
303 | return true; | ||
304 | } | ||
305 | |||
306 | public override bool SetLinearLimits2(object pConstraint, Vector3 low, Vector3 high) | ||
307 | { | ||
308 | Generic6DofConstraint constraint = pConstraint as Generic6DofConstraint; | ||
309 | IndexedVector3 lowlimit = new IndexedVector3(low.X, low.Y, low.Z); | ||
310 | IndexedVector3 highlimit = new IndexedVector3(high.X, high.Y, high.Z); | ||
311 | constraint.SetLinearLowerLimit(lowlimit); | ||
312 | constraint.SetLinearUpperLimit(highlimit); | ||
313 | return true; | ||
314 | } | ||
315 | |||
316 | public override bool SetAngularLimits2(object pConstraint, Vector3 low, Vector3 high) | ||
317 | { | ||
318 | Generic6DofConstraint constraint = pConstraint as Generic6DofConstraint; | ||
319 | IndexedVector3 lowlimit = new IndexedVector3(low.X, low.Y, low.Z); | ||
320 | IndexedVector3 highlimit = new IndexedVector3(high.X, high.Y, high.Z); | ||
321 | constraint.SetAngularLowerLimit(lowlimit); | ||
322 | constraint.SetAngularUpperLimit(highlimit); | ||
323 | return true; | ||
324 | } | ||
325 | |||
326 | public override void SetConstraintNumSolverIterations2(object pConstraint, float cnt) | ||
327 | { | ||
328 | Generic6DofConstraint constraint = pConstraint as Generic6DofConstraint; | ||
329 | constraint.SetOverrideNumSolverIterations((int)cnt); | ||
330 | } | ||
331 | |||
332 | public override void CalculateTransforms2(object pConstraint) | ||
333 | { | ||
334 | Generic6DofConstraint constraint = pConstraint as Generic6DofConstraint; | ||
335 | constraint.CalculateTransforms(); | ||
336 | } | ||
337 | |||
338 | public override void SetConstraintEnable2(object pConstraint, float p_2) | ||
339 | { | ||
340 | Generic6DofConstraint constraint = pConstraint as Generic6DofConstraint; | ||
341 | constraint.SetEnabled((p_2 == 0) ? false : true); | ||
342 | } | ||
343 | |||
344 | |||
345 | //BulletSimAPI.Create6DofConstraint2(m_world.ptr, m_body1.ptr, m_body2.ptr,frame1, frame1rot,frame2, frame2rot,useLinearReferenceFrameA, disableCollisionsBetweenLinkedBodies)); | ||
346 | public override object Create6DofConstraint2(object pWorld, object pBody1, object pBody2, Vector3 pframe1, Quaternion pframe1rot, Vector3 pframe2, Quaternion pframe2rot, bool puseLinearReferenceFrameA, bool pdisableCollisionsBetweenLinkedBodies) | ||
347 | |||
348 | { | ||
349 | DiscreteDynamicsWorld world = pWorld as DiscreteDynamicsWorld; | ||
350 | RigidBody body1 = pBody1 as RigidBody; | ||
351 | RigidBody body2 = pBody2 as RigidBody; | ||
352 | IndexedVector3 frame1v = new IndexedVector3(pframe1.X, pframe1.Y, pframe1.Z); | ||
353 | IndexedQuaternion frame1rot = new IndexedQuaternion(pframe1rot.X, pframe1rot.Y, pframe1rot.Z, pframe1rot.W); | ||
354 | IndexedMatrix frame1 = IndexedMatrix.CreateFromQuaternion(frame1rot); | ||
355 | frame1._origin = frame1v; | ||
356 | |||
357 | IndexedVector3 frame2v = new IndexedVector3(pframe2.X, pframe2.Y, pframe2.Z); | ||
358 | IndexedQuaternion frame2rot = new IndexedQuaternion(pframe2rot.X, pframe2rot.Y, pframe2rot.Z, pframe2rot.W); | ||
359 | IndexedMatrix frame2 = IndexedMatrix.CreateFromQuaternion(frame2rot); | ||
360 | frame2._origin = frame1v; | ||
361 | |||
362 | Generic6DofConstraint consttr = new Generic6DofConstraint(body1, body2, ref frame1, ref frame2, | ||
363 | puseLinearReferenceFrameA); | ||
364 | consttr.CalculateTransforms(); | ||
365 | world.AddConstraint(consttr,pdisableCollisionsBetweenLinkedBodies); | ||
366 | |||
367 | return consttr; | ||
368 | } | ||
369 | |||
370 | |||
371 | /// <summary> | ||
372 | /// | ||
373 | /// </summary> | ||
374 | /// <param name="pWorld"></param> | ||
375 | /// <param name="pBody1"></param> | ||
376 | /// <param name="pBody2"></param> | ||
377 | /// <param name="pjoinPoint"></param> | ||
378 | /// <param name="puseLinearReferenceFrameA"></param> | ||
379 | /// <param name="pdisableCollisionsBetweenLinkedBodies"></param> | ||
380 | /// <returns></returns> | ||
381 | public override object Create6DofConstraintToPoint2(object pWorld, object pBody1, object pBody2, Vector3 pjoinPoint, bool puseLinearReferenceFrameA, bool pdisableCollisionsBetweenLinkedBodies) | ||
382 | { | ||
383 | DiscreteDynamicsWorld world = pWorld as DiscreteDynamicsWorld; | ||
384 | RigidBody body1 = pBody1 as RigidBody; | ||
385 | RigidBody body2 = pBody2 as RigidBody; | ||
386 | IndexedMatrix frame1 = new IndexedMatrix(IndexedBasisMatrix.Identity, new IndexedVector3(0, 0, 0)); | ||
387 | IndexedMatrix frame2 = new IndexedMatrix(IndexedBasisMatrix.Identity, new IndexedVector3(0, 0, 0)); | ||
388 | |||
389 | IndexedVector3 joinPoint = new IndexedVector3(pjoinPoint.X, pjoinPoint.Y, pjoinPoint.Z); | ||
390 | IndexedMatrix mat = IndexedMatrix.Identity; | ||
391 | mat._origin = new IndexedVector3(pjoinPoint.X, pjoinPoint.Y, pjoinPoint.Z); | ||
392 | frame1._origin = body1.GetWorldTransform().Inverse()*joinPoint; | ||
393 | frame2._origin = body2.GetWorldTransform().Inverse()*joinPoint; | ||
394 | |||
395 | Generic6DofConstraint consttr = new Generic6DofConstraint(body1, body2, ref frame1, ref frame2, puseLinearReferenceFrameA); | ||
396 | consttr.CalculateTransforms(); | ||
397 | world.AddConstraint(consttr, pdisableCollisionsBetweenLinkedBodies); | ||
398 | |||
399 | return consttr; | ||
400 | } | ||
401 | //SetFrames2(m_constraint.ptr, frameA, frameArot, frameB, frameBrot); | ||
402 | public override void SetFrames2(object pConstraint, Vector3 pframe1, Quaternion pframe1rot, Vector3 pframe2, Quaternion pframe2rot) | ||
403 | { | ||
404 | Generic6DofConstraint constraint = pConstraint as Generic6DofConstraint; | ||
405 | IndexedVector3 frame1v = new IndexedVector3(pframe1.X, pframe1.Y, pframe1.Z); | ||
406 | IndexedQuaternion frame1rot = new IndexedQuaternion(pframe1rot.X, pframe1rot.Y, pframe1rot.Z, pframe1rot.W); | ||
407 | IndexedMatrix frame1 = IndexedMatrix.CreateFromQuaternion(frame1rot); | ||
408 | frame1._origin = frame1v; | ||
409 | |||
410 | IndexedVector3 frame2v = new IndexedVector3(pframe2.X, pframe2.Y, pframe2.Z); | ||
411 | IndexedQuaternion frame2rot = new IndexedQuaternion(pframe2rot.X, pframe2rot.Y, pframe2rot.Z, pframe2rot.W); | ||
412 | IndexedMatrix frame2 = IndexedMatrix.CreateFromQuaternion(frame2rot); | ||
413 | frame2._origin = frame1v; | ||
414 | constraint.SetFrames(ref frame1, ref frame2); | ||
415 | } | ||
416 | |||
417 | |||
418 | |||
419 | |||
420 | public override bool IsInWorld2(object pWorld, object pShapeObj) | ||
421 | { | ||
422 | DiscreteDynamicsWorld world = pWorld as DiscreteDynamicsWorld; | ||
423 | CollisionObject shape = pShapeObj as CollisionObject; | ||
424 | return world.IsInWorld(shape); | ||
425 | } | ||
426 | |||
427 | public override void SetInterpolationLinearVelocity2(object pBody, Vector3 VehicleVelocity) | ||
428 | { | ||
429 | RigidBody body = pBody as RigidBody; | ||
430 | IndexedVector3 velocity = new IndexedVector3(VehicleVelocity.X, VehicleVelocity.Y, VehicleVelocity.Z); | ||
431 | body.SetInterpolationLinearVelocity(ref velocity); | ||
432 | } | ||
433 | |||
434 | public override bool UseFrameOffset2(object pConstraint, float onOff) | ||
435 | { | ||
436 | Generic6DofConstraint constraint = pConstraint as Generic6DofConstraint; | ||
437 | constraint.SetUseFrameOffset((onOff == 0) ? false : true); | ||
438 | return true; | ||
439 | } | ||
440 | //SetBreakingImpulseThreshold2(m_constraint.ptr, threshold); | ||
441 | public override bool SetBreakingImpulseThreshold2(object pConstraint, float threshold) | ||
442 | { | ||
443 | Generic6DofConstraint constraint = pConstraint as Generic6DofConstraint; | ||
444 | constraint.SetBreakingImpulseThreshold(threshold); | ||
445 | return true; | ||
446 | } | ||
447 | //BulletSimAPI.SetAngularDamping2(Prim.PhysBody.ptr, angularDamping); | ||
448 | public override void SetAngularDamping2(object pBody, float angularDamping) | ||
449 | { | ||
450 | RigidBody body = pBody as RigidBody; | ||
451 | float lineardamping = body.GetLinearDamping(); | ||
452 | body.SetDamping(lineardamping, angularDamping); | ||
453 | |||
454 | } | ||
455 | |||
456 | public override void UpdateInertiaTensor2(object pBody) | ||
457 | { | ||
458 | RigidBody body = pBody as RigidBody; | ||
459 | body.UpdateInertiaTensor(); | ||
460 | } | ||
461 | |||
462 | public override void RecalculateCompoundShapeLocalAabb2( object pCompoundShape) | ||
463 | { | ||
464 | |||
465 | CompoundShape shape = pCompoundShape as CompoundShape; | ||
466 | shape.RecalculateLocalAabb(); | ||
467 | } | ||
468 | |||
469 | //BulletSimAPI.GetCollisionFlags2(PhysBody.ptr) | ||
470 | public override CollisionFlags GetCollisionFlags2(object pBody) | ||
471 | { | ||
472 | RigidBody body = pBody as RigidBody; | ||
473 | uint flags = (uint)body.GetCollisionFlags(); | ||
474 | return (CollisionFlags) flags; | ||
475 | } | ||
476 | |||
477 | public override void SetDamping2(object pBody, float pLinear, float pAngular) | ||
478 | { | ||
479 | RigidBody body = pBody as RigidBody; | ||
480 | body.SetDamping(pLinear, pAngular); | ||
481 | } | ||
482 | //PhysBody.ptr, PhysicsScene.Params.deactivationTime); | ||
483 | public override void SetDeactivationTime2(object pBody, float pDeactivationTime) | ||
484 | { | ||
485 | RigidBody body = pBody as RigidBody; | ||
486 | body.SetDeactivationTime(pDeactivationTime); | ||
487 | } | ||
488 | //SetSleepingThresholds2(PhysBody.ptr, PhysicsScene.Params.linearSleepingThreshold, PhysicsScene.Params.angularSleepingThreshold); | ||
489 | public override void SetSleepingThresholds2(object pBody, float plinearSleepingThreshold, float pangularSleepingThreshold) | ||
490 | { | ||
491 | RigidBody body = pBody as RigidBody; | ||
492 | body.SetSleepingThresholds(plinearSleepingThreshold, pangularSleepingThreshold); | ||
493 | } | ||
494 | |||
495 | public override CollisionObjectTypes GetBodyType2(object pBody) | ||
496 | { | ||
497 | RigidBody body = pBody as RigidBody; | ||
498 | return (CollisionObjectTypes)(int) body.GetInternalType(); | ||
499 | } | ||
500 | |||
501 | //BulletSimAPI.ApplyCentralForce2(PhysBody.ptr, fSum); | ||
502 | public override void ApplyCentralForce2(object pBody, Vector3 pfSum) | ||
503 | { | ||
504 | RigidBody body = pBody as RigidBody; | ||
505 | IndexedVector3 fSum = new IndexedVector3(pfSum.X, pfSum.Y, pfSum.Z); | ||
506 | body.ApplyCentralForce(ref fSum); | ||
507 | } | ||
508 | public override void ApplyCentralImpulse2(object pBody, Vector3 pfSum) | ||
509 | { | ||
510 | RigidBody body = pBody as RigidBody; | ||
511 | IndexedVector3 fSum = new IndexedVector3(pfSum.X, pfSum.Y, pfSum.Z); | ||
512 | body.ApplyCentralImpulse(ref fSum); | ||
513 | } | ||
514 | public override void ApplyTorque2(object pBody, Vector3 pfSum) | ||
515 | { | ||
516 | RigidBody body = pBody as RigidBody; | ||
517 | IndexedVector3 fSum = new IndexedVector3(pfSum.X, pfSum.Y, pfSum.Z); | ||
518 | body.ApplyTorque(ref fSum); | ||
519 | } | ||
520 | public override void ApplyTorqueImpulse2(object pBody, Vector3 pfSum) | ||
521 | { | ||
522 | RigidBody body = pBody as RigidBody; | ||
523 | IndexedVector3 fSum = new IndexedVector3(pfSum.X, pfSum.Y, pfSum.Z); | ||
524 | body.ApplyTorqueImpulse(ref fSum); | ||
525 | } | ||
526 | |||
527 | public override void DumpRigidBody2(object p, object p_2) | ||
528 | { | ||
529 | //TODO: | ||
530 | } | ||
531 | |||
532 | public override void DumpCollisionShape2(object p, object p_2) | ||
533 | { | ||
534 | //TODO: | ||
535 | } | ||
536 | |||
537 | public override void DestroyObject2(object p, object p_2) | ||
538 | { | ||
539 | //TODO: | ||
540 | } | ||
541 | |||
542 | public override void Shutdown2(object pWorld) | ||
543 | { | ||
544 | DiscreteDynamicsWorld world = pWorld as DiscreteDynamicsWorld; | ||
545 | world.Cleanup(); | ||
546 | } | ||
547 | |||
548 | public override void DeleteCollisionShape2(object p, object p_2) | ||
549 | { | ||
550 | //TODO: | ||
551 | } | ||
552 | //(sim.ptr, shape.ptr, prim.LocalID, prim.RawPosition, prim.RawOrientation); | ||
553 | |||
554 | public override object CreateBodyFromShape2(object pWorld, object pShape, uint pLocalID, Vector3 pRawPosition, Quaternion pRawOrientation) | ||
555 | { | ||
556 | CollisionWorld world = pWorld as CollisionWorld; | ||
557 | IndexedMatrix mat = | ||
558 | IndexedMatrix.CreateFromQuaternion(new IndexedQuaternion(pRawOrientation.X, pRawOrientation.Y, | ||
559 | pRawOrientation.Z, pRawOrientation.W)); | ||
560 | mat._origin = new IndexedVector3(pRawPosition.X, pRawPosition.Y, pRawPosition.Z); | ||
561 | CollisionShape shape = pShape as CollisionShape; | ||
562 | //UpdateSingleAabb2(world, shape); | ||
563 | // TODO: Feed Update array into null | ||
564 | RigidBody body = new RigidBody(0,new SimMotionState(world,pLocalID,mat,null),shape,IndexedVector3.Zero); | ||
565 | |||
566 | body.SetUserPointer(pLocalID); | ||
567 | return body; | ||
568 | } | ||
569 | |||
570 | |||
571 | public override object CreateBodyWithDefaultMotionState2( object pShape, uint pLocalID, Vector3 pRawPosition, Quaternion pRawOrientation) | ||
572 | { | ||
573 | |||
574 | IndexedMatrix mat = | ||
575 | IndexedMatrix.CreateFromQuaternion(new IndexedQuaternion(pRawOrientation.X, pRawOrientation.Y, | ||
576 | pRawOrientation.Z, pRawOrientation.W)); | ||
577 | mat._origin = new IndexedVector3(pRawPosition.X, pRawPosition.Y, pRawPosition.Z); | ||
578 | |||
579 | CollisionShape shape = pShape as CollisionShape; | ||
580 | |||
581 | // TODO: Feed Update array into null | ||
582 | RigidBody body = new RigidBody(0, new DefaultMotionState( mat, IndexedMatrix.Identity), shape, IndexedVector3.Zero); | ||
583 | body.SetWorldTransform(mat); | ||
584 | body.SetUserPointer(pLocalID); | ||
585 | return body; | ||
586 | } | ||
587 | //(m_mapInfo.terrainBody.ptr, CollisionFlags.CF_STATIC_OBJECT); | ||
588 | public override void SetCollisionFlags2(object pBody, CollisionFlags collisionFlags) | ||
589 | { | ||
590 | RigidBody body = pBody as RigidBody; | ||
591 | body.SetCollisionFlags((BulletXNA.BulletCollision.CollisionFlags) (uint) collisionFlags); | ||
592 | } | ||
593 | //(m_mapInfo.terrainBody.ptr, PhysicsScene.Params.terrainHitFraction); | ||
594 | public override void SetHitFraction2(object pBody, float pHitFraction) | ||
595 | { | ||
596 | RigidBody body = pBody as RigidBody; | ||
597 | body.SetHitFraction(pHitFraction); | ||
598 | } | ||
599 | //BuildCapsuleShape2(physicsScene.World.ptr, 1f, 1f, prim.Scale); | ||
600 | public override object BuildCapsuleShape2(object pWorld, float pRadius, float pHeight, Vector3 pScale) | ||
601 | { | ||
602 | DiscreteDynamicsWorld world = pWorld as DiscreteDynamicsWorld; | ||
603 | IndexedVector3 scale = new IndexedVector3(pScale.X, pScale.Y, pScale.Z); | ||
604 | CapsuleShapeZ capsuleShapeZ = new CapsuleShapeZ(pRadius, pHeight); | ||
605 | capsuleShapeZ.SetMargin(world.WorldSettings.Params.collisionMargin); | ||
606 | capsuleShapeZ.SetLocalScaling(ref scale); | ||
607 | |||
608 | return capsuleShapeZ; | ||
609 | } | ||
610 | |||
611 | public static object Initialize2(Vector3 worldExtent, ConfigurationParameters[] o, int mMaxCollisionsPerFrame, ref List<BulletXNA.CollisionDesc> collisionArray, int mMaxUpdatesPerFrame, ref List<BulletXNA.EntityProperties> updateArray, object mDebugLogCallbackHandle) | ||
612 | { | ||
613 | CollisionWorld.WorldData.ParamData p = new CollisionWorld.WorldData.ParamData(); | ||
614 | |||
615 | p.angularDamping = o[0].XangularDamping; | ||
616 | p.defaultFriction = o[0].defaultFriction; | ||
617 | p.defaultFriction = o[0].defaultFriction; | ||
618 | p.defaultDensity = o[0].defaultDensity; | ||
619 | p.defaultRestitution = o[0].defaultRestitution; | ||
620 | p.collisionMargin = o[0].collisionMargin; | ||
621 | p.gravity = o[0].gravity; | ||
622 | |||
623 | p.linearDamping = o[0].XlinearDamping; | ||
624 | p.angularDamping = o[0].XangularDamping; | ||
625 | p.deactivationTime = o[0].XdeactivationTime; | ||
626 | p.linearSleepingThreshold = o[0].XlinearSleepingThreshold; | ||
627 | p.angularSleepingThreshold = o[0].XangularSleepingThreshold; | ||
628 | p.ccdMotionThreshold = o[0].XccdMotionThreshold; | ||
629 | p.ccdSweptSphereRadius = o[0].XccdSweptSphereRadius; | ||
630 | p.contactProcessingThreshold = o[0].XcontactProcessingThreshold; | ||
631 | |||
632 | p.terrainImplementation = o[0].XterrainImplementation; | ||
633 | p.terrainFriction = o[0].XterrainFriction; | ||
634 | |||
635 | p.terrainHitFraction = o[0].XterrainHitFraction; | ||
636 | p.terrainRestitution = o[0].XterrainRestitution; | ||
637 | p.terrainCollisionMargin = o[0].XterrainCollisionMargin; | ||
638 | |||
639 | p.avatarFriction = o[0].XavatarFriction; | ||
640 | p.avatarStandingFriction = o[0].XavatarStandingFriction; | ||
641 | p.avatarDensity = o[0].XavatarDensity; | ||
642 | p.avatarRestitution = o[0].XavatarRestitution; | ||
643 | p.avatarCapsuleWidth = o[0].XavatarCapsuleWidth; | ||
644 | p.avatarCapsuleDepth = o[0].XavatarCapsuleDepth; | ||
645 | p.avatarCapsuleHeight = o[0].XavatarCapsuleHeight; | ||
646 | p.avatarContactProcessingThreshold = o[0].XavatarContactProcessingThreshold; | ||
647 | |||
648 | p.vehicleAngularDamping = o[0].XvehicleAngularDamping; | ||
649 | |||
650 | p.maxPersistantManifoldPoolSize = o[0].maxPersistantManifoldPoolSize; | ||
651 | p.maxCollisionAlgorithmPoolSize = o[0].maxCollisionAlgorithmPoolSize; | ||
652 | p.shouldDisableContactPoolDynamicAllocation = o[0].shouldDisableContactPoolDynamicAllocation; | ||
653 | p.shouldForceUpdateAllAabbs = o[0].shouldForceUpdateAllAabbs; | ||
654 | p.shouldRandomizeSolverOrder = o[0].shouldRandomizeSolverOrder; | ||
655 | p.shouldSplitSimulationIslands = o[0].shouldSplitSimulationIslands; | ||
656 | p.shouldEnableFrictionCaching = o[0].shouldEnableFrictionCaching; | ||
657 | p.numberOfSolverIterations = o[0].numberOfSolverIterations; | ||
658 | |||
659 | p.linksetImplementation = o[0].XlinksetImplementation; | ||
660 | p.linkConstraintUseFrameOffset = o[0].XlinkConstraintUseFrameOffset; | ||
661 | p.linkConstraintEnableTransMotor = o[0].XlinkConstraintEnableTransMotor; | ||
662 | p.linkConstraintTransMotorMaxVel = o[0].XlinkConstraintTransMotorMaxVel; | ||
663 | p.linkConstraintTransMotorMaxForce = o[0].XlinkConstraintTransMotorMaxForce; | ||
664 | p.linkConstraintERP = o[0].XlinkConstraintERP; | ||
665 | p.linkConstraintCFM = o[0].XlinkConstraintCFM; | ||
666 | p.linkConstraintSolverIterations = o[0].XlinkConstraintSolverIterations; | ||
667 | p.physicsLoggingFrames = o[0].physicsLoggingFrames; | ||
668 | DefaultCollisionConstructionInfo ccci = new DefaultCollisionConstructionInfo(); | ||
669 | |||
670 | DefaultCollisionConfiguration cci = new DefaultCollisionConfiguration(); | ||
671 | CollisionDispatcher m_dispatcher = new CollisionDispatcher(cci); | ||
672 | |||
673 | |||
674 | if (p.maxPersistantManifoldPoolSize > 0) | ||
675 | cci.m_persistentManifoldPoolSize = (int)p.maxPersistantManifoldPoolSize; | ||
676 | if (p.shouldDisableContactPoolDynamicAllocation !=0) | ||
677 | m_dispatcher.SetDispatcherFlags(DispatcherFlags.CD_DISABLE_CONTACTPOOL_DYNAMIC_ALLOCATION); | ||
678 | //if (p.maxCollisionAlgorithmPoolSize >0 ) | ||
679 | |||
680 | DbvtBroadphase m_broadphase = new DbvtBroadphase(); | ||
681 | //IndexedVector3 aabbMin = new IndexedVector3(0, 0, 0); | ||
682 | //IndexedVector3 aabbMax = new IndexedVector3(256, 256, 256); | ||
683 | |||
684 | //AxisSweep3Internal m_broadphase2 = new AxisSweep3Internal(ref aabbMin, ref aabbMax, Convert.ToInt32(0xfffe), 0xffff, ushort.MaxValue/2, null, true); | ||
685 | m_broadphase.GetOverlappingPairCache().SetInternalGhostPairCallback(new GhostPairCallback()); | ||
686 | |||
687 | SequentialImpulseConstraintSolver m_solver = new SequentialImpulseConstraintSolver(); | ||
688 | |||
689 | DiscreteDynamicsWorld world = new DiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_solver, cci); | ||
690 | world.UpdatedObjects = updateArray; | ||
691 | world.UpdatedCollisions = collisionArray; | ||
692 | world.WorldSettings.Params = p; | ||
693 | world.SetForceUpdateAllAabbs(p.shouldForceUpdateAllAabbs != 0); | ||
694 | world.GetSolverInfo().m_solverMode = SolverMode.SOLVER_USE_WARMSTARTING | SolverMode.SOLVER_SIMD; | ||
695 | if (p.shouldRandomizeSolverOrder != 0) | ||
696 | world.GetSolverInfo().m_solverMode |= SolverMode.SOLVER_RANDMIZE_ORDER; | ||
697 | |||
698 | world.GetSimulationIslandManager().SetSplitIslands(p.shouldSplitSimulationIslands != 0); | ||
699 | //world.GetDispatchInfo().m_enableSatConvex Not implemented in C# port | ||
700 | |||
701 | if (p.shouldEnableFrictionCaching != 0) | ||
702 | world.GetSolverInfo().m_solverMode |= SolverMode.SOLVER_ENABLE_FRICTION_DIRECTION_CACHING; | ||
703 | |||
704 | if (p.numberOfSolverIterations > 0) | ||
705 | world.GetSolverInfo().m_numIterations = (int) p.numberOfSolverIterations; | ||
706 | |||
707 | |||
708 | world.GetSolverInfo().m_damping = world.WorldSettings.Params.linearDamping; | ||
709 | world.GetSolverInfo().m_restitution = world.WorldSettings.Params.defaultRestitution; | ||
710 | world.GetSolverInfo().m_globalCfm = 0.0f; | ||
711 | world.GetSolverInfo().m_tau = 0.6f; | ||
712 | world.GetSolverInfo().m_friction = 0.3f; | ||
713 | world.GetSolverInfo().m_maxErrorReduction = 20f; | ||
714 | world.GetSolverInfo().m_numIterations = 10; | ||
715 | world.GetSolverInfo().m_erp = 0.2f; | ||
716 | world.GetSolverInfo().m_erp2 = 0.1f; | ||
717 | world.GetSolverInfo().m_sor = 1.0f; | ||
718 | world.GetSolverInfo().m_splitImpulse = false; | ||
719 | world.GetSolverInfo().m_splitImpulsePenetrationThreshold = -0.02f; | ||
720 | world.GetSolverInfo().m_linearSlop = 0.0f; | ||
721 | world.GetSolverInfo().m_warmstartingFactor = 0.85f; | ||
722 | world.GetSolverInfo().m_restingContactRestitutionThreshold = 2; | ||
723 | world.SetForceUpdateAllAabbs(true); | ||
724 | |||
725 | |||
726 | world.SetGravity(new IndexedVector3(0,0,p.gravity)); | ||
727 | |||
728 | return world; | ||
729 | } | ||
730 | //m_constraint.ptr, ConstraintParams.BT_CONSTRAINT_STOP_CFM, cfm, ConstraintParamAxis.AXIS_ALL | ||
731 | public override bool SetConstraintParam2(object pConstraint, ConstraintParams paramIndex, float paramvalue, ConstraintParamAxis axis) | ||
732 | { | ||
733 | Generic6DofConstraint constrain = pConstraint as Generic6DofConstraint; | ||
734 | if (axis == ConstraintParamAxis.AXIS_LINEAR_ALL || axis == ConstraintParamAxis.AXIS_ALL) | ||
735 | { | ||
736 | constrain.SetParam((BulletXNA.BulletDynamics.ConstraintParams) (int) paramIndex, paramvalue, 0); | ||
737 | constrain.SetParam((BulletXNA.BulletDynamics.ConstraintParams) (int) paramIndex, paramvalue, 1); | ||
738 | constrain.SetParam((BulletXNA.BulletDynamics.ConstraintParams) (int) paramIndex, paramvalue, 2); | ||
739 | } | ||
740 | if (axis == ConstraintParamAxis.AXIS_ANGULAR_ALL || axis == ConstraintParamAxis.AXIS_ALL) | ||
741 | { | ||
742 | constrain.SetParam((BulletXNA.BulletDynamics.ConstraintParams)(int)paramIndex, paramvalue, 3); | ||
743 | constrain.SetParam((BulletXNA.BulletDynamics.ConstraintParams)(int)paramIndex, paramvalue, 4); | ||
744 | constrain.SetParam((BulletXNA.BulletDynamics.ConstraintParams)(int)paramIndex, paramvalue, 5); | ||
745 | } | ||
746 | if (axis == ConstraintParamAxis.AXIS_LINEAR_ALL) | ||
747 | { | ||
748 | constrain.SetParam((BulletXNA.BulletDynamics.ConstraintParams)(int)paramIndex, paramvalue, (int)axis); | ||
749 | } | ||
750 | return true; | ||
751 | } | ||
752 | |||
753 | public override bool PushUpdate2(object pCollisionObject) | ||
754 | { | ||
755 | bool ret = false; | ||
756 | RigidBody rb = pCollisionObject as RigidBody; | ||
757 | if (rb != null) | ||
758 | { | ||
759 | SimMotionState sms = rb.GetMotionState() as SimMotionState; | ||
760 | if (sms != null) | ||
761 | { | ||
762 | IndexedMatrix wt = IndexedMatrix.Identity; | ||
763 | sms.GetWorldTransform(out wt); | ||
764 | sms.SetWorldTransform(ref wt, true); | ||
765 | ret = true; | ||
766 | } | ||
767 | } | ||
768 | return ret; | ||
769 | |||
770 | } | ||
771 | |||
772 | public override bool IsCompound2(object pShape) | ||
773 | { | ||
774 | CollisionShape shape = pShape as CollisionShape; | ||
775 | return shape.IsCompound(); | ||
776 | } | ||
777 | public override bool IsPloyhedral2(object pShape) | ||
778 | { | ||
779 | CollisionShape shape = pShape as CollisionShape; | ||
780 | return shape.IsPolyhedral(); | ||
781 | } | ||
782 | public override bool IsConvex2d2(object pShape) | ||
783 | { | ||
784 | CollisionShape shape = pShape as CollisionShape; | ||
785 | return shape.IsConvex2d(); | ||
786 | } | ||
787 | public override bool IsConvex2(object pShape) | ||
788 | { | ||
789 | CollisionShape shape = pShape as CollisionShape; | ||
790 | return shape.IsConvex(); | ||
791 | } | ||
792 | public override bool IsNonMoving2(object pShape) | ||
793 | { | ||
794 | CollisionShape shape = pShape as CollisionShape; | ||
795 | return shape.IsNonMoving(); | ||
796 | } | ||
797 | public override bool IsConcave2(object pShape) | ||
798 | { | ||
799 | CollisionShape shape = pShape as CollisionShape; | ||
800 | return shape.IsConcave(); | ||
801 | } | ||
802 | public override bool IsInfinite2(object pShape) | ||
803 | { | ||
804 | CollisionShape shape = pShape as CollisionShape; | ||
805 | return shape.IsInfinite(); | ||
806 | } | ||
807 | public override bool IsNativeShape2(object pShape) | ||
808 | { | ||
809 | CollisionShape shape = pShape as CollisionShape; | ||
810 | bool ret; | ||
811 | switch (shape.GetShapeType()) | ||
812 | { | ||
813 | case BroadphaseNativeTypes.BOX_SHAPE_PROXYTYPE: | ||
814 | case BroadphaseNativeTypes.CONE_SHAPE_PROXYTYPE: | ||
815 | case BroadphaseNativeTypes.SPHERE_SHAPE_PROXYTYPE: | ||
816 | case BroadphaseNativeTypes.CYLINDER_SHAPE_PROXYTYPE: | ||
817 | ret = true; | ||
818 | break; | ||
819 | default: | ||
820 | ret = false; | ||
821 | break; | ||
822 | } | ||
823 | return ret; | ||
824 | } | ||
825 | //sim.ptr, shape.ptr,prim.LocalID, prim.RawPosition, prim.RawOrientation | ||
826 | public override object CreateGhostFromShape2(object pWorld, object pShape, uint pLocalID, Vector3 pRawPosition, Quaternion pRawOrientation) | ||
827 | { | ||
828 | IndexedMatrix bodyTransform = new IndexedMatrix(); | ||
829 | bodyTransform._origin = new IndexedVector3(pRawPosition.X, pRawPosition.Y, pRawPosition.Z); | ||
830 | bodyTransform.SetRotation(new IndexedQuaternion(pRawOrientation.X,pRawOrientation.Y,pRawOrientation.Z,pRawOrientation.W)); | ||
831 | GhostObject gObj = new PairCachingGhostObject(); | ||
832 | gObj.SetWorldTransform(bodyTransform); | ||
833 | CollisionShape shape = pShape as CollisionShape; | ||
834 | gObj.SetCollisionShape(shape); | ||
835 | gObj.SetUserPointer(pLocalID); | ||
836 | // TODO: Add to Special CollisionObjects! | ||
837 | return gObj; | ||
838 | } | ||
839 | |||
840 | public static void SetCollisionShape2(object pWorld, object pObj, object pShape) | ||
841 | { | ||
842 | var world = pWorld as DiscreteDynamicsWorld; | ||
843 | var obj = pObj as CollisionObject; | ||
844 | var shape = pShape as CollisionShape; | ||
845 | obj.SetCollisionShape(shape); | ||
846 | |||
847 | } | ||
848 | //(PhysicsScene.World.ptr, nativeShapeData) | ||
849 | public override object BuildNativeShape2(object pWorld, ShapeData pShapeData) | ||
850 | { | ||
851 | var world = pWorld as DiscreteDynamicsWorld; | ||
852 | CollisionShape shape = null; | ||
853 | switch (pShapeData.Type) | ||
854 | { | ||
855 | case BSPhysicsShapeType.SHAPE_BOX: | ||
856 | shape = new BoxShape(new IndexedVector3(0.5f,0.5f,0.5f)); | ||
857 | break; | ||
858 | case BSPhysicsShapeType.SHAPE_CONE: | ||
859 | shape = new ConeShapeZ(0.5f, 1.0f); | ||
860 | break; | ||
861 | case BSPhysicsShapeType.SHAPE_CYLINDER: | ||
862 | shape = new CylinderShapeZ(new IndexedVector3(0.5f, 0.5f, 0.5f)); | ||
863 | break; | ||
864 | case BSPhysicsShapeType.SHAPE_SPHERE: | ||
865 | shape = new SphereShape(0.5f); | ||
866 | break; | ||
867 | |||
868 | } | ||
869 | if (shape != null) | ||
870 | { | ||
871 | IndexedVector3 scaling = new IndexedVector3(pShapeData.Scale.X, pShapeData.Scale.Y, pShapeData.Scale.Z); | ||
872 | shape.SetMargin(world.WorldSettings.Params.collisionMargin); | ||
873 | shape.SetLocalScaling(ref scaling); | ||
874 | |||
875 | } | ||
876 | return shape; | ||
877 | } | ||
878 | //PhysicsScene.World.ptr, false | ||
879 | public override object CreateCompoundShape2(object pWorld, bool enableDynamicAabbTree) | ||
880 | { | ||
881 | return new CompoundShape(enableDynamicAabbTree); | ||
882 | } | ||
883 | |||
884 | public override int GetNumberOfCompoundChildren2(object pCompoundShape) | ||
885 | { | ||
886 | var compoundshape = pCompoundShape as CompoundShape; | ||
887 | return compoundshape.GetNumChildShapes(); | ||
888 | } | ||
889 | //LinksetRoot.PhysShape.ptr, newShape.ptr, displacementPos, displacementRot | ||
890 | public override void AddChildShapeToCompoundShape2(object pCShape, object paddShape, Vector3 displacementPos, Quaternion displacementRot) | ||
891 | { | ||
892 | IndexedMatrix relativeTransform = new IndexedMatrix(); | ||
893 | var compoundshape = pCShape as CompoundShape; | ||
894 | var addshape = paddShape as CollisionShape; | ||
895 | |||
896 | relativeTransform._origin = new IndexedVector3(displacementPos.X, displacementPos.Y, displacementPos.Z); | ||
897 | relativeTransform.SetRotation(new IndexedQuaternion(displacementRot.X,displacementRot.Y,displacementRot.Z,displacementRot.W)); | ||
898 | compoundshape.AddChildShape(ref relativeTransform, addshape); | ||
899 | |||
900 | } | ||
901 | |||
902 | public override object RemoveChildShapeFromCompoundShapeIndex2(object pCShape, int pii) | ||
903 | { | ||
904 | var compoundshape = pCShape as CompoundShape; | ||
905 | CollisionShape ret = null; | ||
906 | ret = compoundshape.GetChildShape(pii); | ||
907 | compoundshape.RemoveChildShapeByIndex(pii); | ||
908 | return ret; | ||
909 | } | ||
910 | |||
911 | public override object CreateGroundPlaneShape2(uint pLocalId, float pheight, float pcollisionMargin) | ||
912 | { | ||
913 | StaticPlaneShape m_planeshape = new StaticPlaneShape(new IndexedVector3(0,0,1),(int)pheight ); | ||
914 | m_planeshape.SetMargin(pcollisionMargin); | ||
915 | m_planeshape.SetUserPointer(pLocalId); | ||
916 | return m_planeshape; | ||
917 | } | ||
918 | |||
919 | public override object CreateHingeConstraint2(object pWorld, object pBody1, object ppBody2, Vector3 ppivotInA, Vector3 ppivotInB, Vector3 paxisInA, Vector3 paxisInB, bool puseLinearReferenceFrameA, bool pdisableCollisionsBetweenLinkedBodies) | ||
920 | { | ||
921 | HingeConstraint constrain = null; | ||
922 | var rb1 = pBody1 as RigidBody; | ||
923 | var rb2 = ppBody2 as RigidBody; | ||
924 | if (rb1 != null && rb2 != null) | ||
925 | { | ||
926 | IndexedVector3 pivotInA = new IndexedVector3(ppivotInA.X, ppivotInA.Y, ppivotInA.Z); | ||
927 | IndexedVector3 pivotInB = new IndexedVector3(ppivotInB.X, ppivotInB.Y, ppivotInB.Z); | ||
928 | IndexedVector3 axisInA = new IndexedVector3(paxisInA.X, paxisInA.Y, paxisInA.Z); | ||
929 | IndexedVector3 axisInB = new IndexedVector3(paxisInB.X, paxisInB.Y, paxisInB.Z); | ||
930 | var world = pWorld as DiscreteDynamicsWorld; | ||
931 | world.AddConstraint(constrain, pdisableCollisionsBetweenLinkedBodies); | ||
932 | } | ||
933 | return constrain; | ||
934 | } | ||
935 | |||
936 | public override bool ReleaseHeightMapInfo2(object pMapInfo) | ||
937 | { | ||
938 | if (pMapInfo != null) | ||
939 | { | ||
940 | BulletHeightMapInfo mapinfo = pMapInfo as BulletHeightMapInfo; | ||
941 | if (mapinfo.heightMap != null) | ||
942 | mapinfo.heightMap = null; | ||
943 | |||
944 | |||
945 | } | ||
946 | return true; | ||
947 | } | ||
948 | |||
949 | public override object CreateHullShape2(object pWorld, int pHullCount, float[] pConvHulls) | ||
950 | { | ||
951 | CompoundShape compoundshape = new CompoundShape(false); | ||
952 | var world = pWorld as DiscreteDynamicsWorld; | ||
953 | |||
954 | |||
955 | compoundshape.SetMargin(world.WorldSettings.Params.collisionMargin); | ||
956 | int ii = 1; | ||
957 | |||
958 | for (int i = 0; i < pHullCount; i++) | ||
959 | { | ||
960 | int vertexCount = (int) pConvHulls[ii]; | ||
961 | |||
962 | IndexedVector3 centroid = new IndexedVector3(pConvHulls[ii + 1], pConvHulls[ii + 2], pConvHulls[ii + 3]); | ||
963 | IndexedMatrix childTrans = IndexedMatrix.Identity; | ||
964 | childTrans._origin = centroid; | ||
965 | |||
966 | List<IndexedVector3> virts = new List<IndexedVector3>(); | ||
967 | int ender = ((ii + 4) + (vertexCount*3)); | ||
968 | for (int iii = ii + 4; iii < ender; iii+=3) | ||
969 | { | ||
970 | |||
971 | virts.Add(new IndexedVector3(pConvHulls[iii], pConvHulls[iii + 1], pConvHulls[iii +2])); | ||
972 | } | ||
973 | ConvexHullShape convexShape = new ConvexHullShape(virts, vertexCount); | ||
974 | convexShape.SetMargin(world.WorldSettings.Params.collisionMargin); | ||
975 | compoundshape.AddChildShape(ref childTrans, convexShape); | ||
976 | ii += (vertexCount*3 + 4); | ||
977 | } | ||
978 | |||
979 | |||
980 | return compoundshape; | ||
981 | } | ||
982 | |||
983 | public override object CreateMeshShape2(object pWorld, int pIndicesCount, int[] indices, int pVerticesCount, float[] verticesAsFloats) | ||
984 | { | ||
985 | //DumpRaw(indices,verticesAsFloats,pIndicesCount,pVerticesCount); | ||
986 | |||
987 | for (int iter = 0; iter < pVerticesCount; iter++) | ||
988 | { | ||
989 | if (verticesAsFloats[iter] > 0 && verticesAsFloats[iter] < 0.0001) verticesAsFloats[iter] = 0; | ||
990 | if (verticesAsFloats[iter] < 0 && verticesAsFloats[iter] > -0.0001) verticesAsFloats[iter] = 0; | ||
991 | } | ||
992 | |||
993 | ObjectArray<int> indicesarr = new ObjectArray<int>(indices); | ||
994 | ObjectArray<float> vertices = new ObjectArray<float>(verticesAsFloats); | ||
995 | DumpRaw(indicesarr,vertices,pIndicesCount,pVerticesCount); | ||
996 | var world = pWorld as DiscreteDynamicsWorld; | ||
997 | IndexedMesh mesh = new IndexedMesh(); | ||
998 | mesh.m_indexType = PHY_ScalarType.PHY_INTEGER; | ||
999 | mesh.m_numTriangles = pIndicesCount/3; | ||
1000 | mesh.m_numVertices = pVerticesCount; | ||
1001 | mesh.m_triangleIndexBase = indicesarr; | ||
1002 | mesh.m_vertexBase = vertices; | ||
1003 | mesh.m_vertexStride = 3; | ||
1004 | mesh.m_vertexType = PHY_ScalarType.PHY_FLOAT; | ||
1005 | mesh.m_triangleIndexStride = 3; | ||
1006 | |||
1007 | TriangleIndexVertexArray tribuilder = new TriangleIndexVertexArray(); | ||
1008 | tribuilder.AddIndexedMesh(mesh, PHY_ScalarType.PHY_INTEGER); | ||
1009 | BvhTriangleMeshShape meshShape = new BvhTriangleMeshShape(tribuilder, true,true); | ||
1010 | meshShape.SetMargin(world.WorldSettings.Params.collisionMargin); | ||
1011 | // world.UpdateSingleAabb(meshShape); | ||
1012 | return meshShape; | ||
1013 | |||
1014 | } | ||
1015 | public static void DumpRaw(ObjectArray<int>indices, ObjectArray<float> vertices, int pIndicesCount,int pVerticesCount ) | ||
1016 | { | ||
1017 | |||
1018 | String fileName = "objTest3.raw"; | ||
1019 | String completePath = System.IO.Path.Combine(Util.configDir(), fileName); | ||
1020 | StreamWriter sw = new StreamWriter(completePath); | ||
1021 | IndexedMesh mesh = new IndexedMesh(); | ||
1022 | |||
1023 | mesh.m_indexType = PHY_ScalarType.PHY_INTEGER; | ||
1024 | mesh.m_numTriangles = pIndicesCount / 3; | ||
1025 | mesh.m_numVertices = pVerticesCount; | ||
1026 | mesh.m_triangleIndexBase = indices; | ||
1027 | mesh.m_vertexBase = vertices; | ||
1028 | mesh.m_vertexStride = 3; | ||
1029 | mesh.m_vertexType = PHY_ScalarType.PHY_FLOAT; | ||
1030 | mesh.m_triangleIndexStride = 3; | ||
1031 | |||
1032 | TriangleIndexVertexArray tribuilder = new TriangleIndexVertexArray(); | ||
1033 | tribuilder.AddIndexedMesh(mesh, PHY_ScalarType.PHY_INTEGER); | ||
1034 | |||
1035 | |||
1036 | |||
1037 | for (int i = 0; i < pVerticesCount; i++) | ||
1038 | { | ||
1039 | |||
1040 | string s = vertices[indices[i * 3]].ToString("0.0000"); | ||
1041 | s += " " + vertices[indices[i * 3 + 1]].ToString("0.0000"); | ||
1042 | s += " " + vertices[indices[i * 3 + 2]].ToString("0.0000"); | ||
1043 | |||
1044 | sw.Write(s + "\n"); | ||
1045 | } | ||
1046 | |||
1047 | sw.Close(); | ||
1048 | } | ||
1049 | public static void DumpRaw(int[] indices, float[] vertices, int pIndicesCount, int pVerticesCount) | ||
1050 | { | ||
1051 | |||
1052 | String fileName = "objTest6.raw"; | ||
1053 | String completePath = System.IO.Path.Combine(Util.configDir(), fileName); | ||
1054 | StreamWriter sw = new StreamWriter(completePath); | ||
1055 | IndexedMesh mesh = new IndexedMesh(); | ||
1056 | |||
1057 | mesh.m_indexType = PHY_ScalarType.PHY_INTEGER; | ||
1058 | mesh.m_numTriangles = pIndicesCount / 3; | ||
1059 | mesh.m_numVertices = pVerticesCount; | ||
1060 | mesh.m_triangleIndexBase = indices; | ||
1061 | mesh.m_vertexBase = vertices; | ||
1062 | mesh.m_vertexStride = 3; | ||
1063 | mesh.m_vertexType = PHY_ScalarType.PHY_FLOAT; | ||
1064 | mesh.m_triangleIndexStride = 3; | ||
1065 | |||
1066 | TriangleIndexVertexArray tribuilder = new TriangleIndexVertexArray(); | ||
1067 | tribuilder.AddIndexedMesh(mesh, PHY_ScalarType.PHY_INTEGER); | ||
1068 | |||
1069 | |||
1070 | sw.WriteLine("Indices"); | ||
1071 | sw.WriteLine(string.Format("int[] indices = new int[{0}];",pIndicesCount)); | ||
1072 | for (int iter = 0; iter < indices.Length; iter++) | ||
1073 | { | ||
1074 | sw.WriteLine(string.Format("indices[{0}]={1};",iter,indices[iter])); | ||
1075 | } | ||
1076 | sw.WriteLine("VerticesFloats"); | ||
1077 | sw.WriteLine(string.Format("float[] vertices = new float[{0}];", pVerticesCount)); | ||
1078 | for (int iter = 0; iter < vertices.Length; iter++) | ||
1079 | { | ||
1080 | sw.WriteLine(string.Format("Vertices[{0}]={1};", iter, vertices[iter].ToString("0.0000"))); | ||
1081 | } | ||
1082 | |||
1083 | // for (int i = 0; i < pVerticesCount; i++) | ||
1084 | // { | ||
1085 | // | ||
1086 | // string s = vertices[indices[i * 3]].ToString("0.0000"); | ||
1087 | // s += " " + vertices[indices[i * 3 + 1]].ToString("0.0000"); | ||
1088 | // s += " " + vertices[indices[i * 3 + 2]].ToString("0.0000"); | ||
1089 | // | ||
1090 | // sw.Write(s + "\n"); | ||
1091 | //} | ||
1092 | |||
1093 | sw.Close(); | ||
1094 | } | ||
1095 | //PhysicsScene.World.ptr, m_mapInfo.ID, m_mapInfo.minCoords, m_mapInfo.maxCoords, m_mapInfo.heightMap, PhysicsScene.Params.terrainCollisionMargin | ||
1096 | public override object CreateHeightMapInfo2(object pWorld, uint pId, Vector3 pminCoords, Vector3 pmaxCoords, float[] pheightMap, float pCollisionMargin) | ||
1097 | { | ||
1098 | BulletHeightMapInfo mapInfo = new BulletHeightMapInfo(pId, pheightMap, null); | ||
1099 | mapInfo.heightMap = null; | ||
1100 | mapInfo.minCoords = pminCoords; | ||
1101 | mapInfo.maxCoords = pmaxCoords; | ||
1102 | mapInfo.sizeX = (int) (pmaxCoords.X - pminCoords.X); | ||
1103 | mapInfo.sizeY = (int) (pmaxCoords.Y - pminCoords.Y); | ||
1104 | mapInfo.ID = pId; | ||
1105 | mapInfo.minZ = pminCoords.Z; | ||
1106 | mapInfo.maxZ = pmaxCoords.Z; | ||
1107 | mapInfo.collisionMargin = pCollisionMargin; | ||
1108 | if (mapInfo.minZ == mapInfo.maxZ) | ||
1109 | mapInfo.minZ -= 0.2f; | ||
1110 | mapInfo.heightMap = pheightMap; | ||
1111 | |||
1112 | return mapInfo; | ||
1113 | |||
1114 | } | ||
1115 | |||
1116 | public override object CreateTerrainShape2(object pMapInfo) | ||
1117 | { | ||
1118 | BulletHeightMapInfo mapinfo = pMapInfo as BulletHeightMapInfo; | ||
1119 | const int upAxis = 2; | ||
1120 | const float scaleFactor = 1.0f; | ||
1121 | HeightfieldTerrainShape terrainShape = new HeightfieldTerrainShape((int)mapinfo.sizeX, (int)mapinfo.sizeY, | ||
1122 | mapinfo.heightMap, scaleFactor, | ||
1123 | mapinfo.minZ, mapinfo.maxZ, upAxis, | ||
1124 | false); | ||
1125 | terrainShape.SetMargin(mapinfo.collisionMargin + 0.5f); | ||
1126 | terrainShape.SetUseDiamondSubdivision(true); | ||
1127 | terrainShape.SetUserPointer(mapinfo.ID); | ||
1128 | return terrainShape; | ||
1129 | } | ||
1130 | |||
1131 | public override bool TranslationalLimitMotor2(object pConstraint, float ponOff, float targetVelocity, float maxMotorForce) | ||
1132 | { | ||
1133 | TypedConstraint tconstrain = pConstraint as TypedConstraint; | ||
1134 | bool onOff = ponOff != 0; | ||
1135 | bool ret = false; | ||
1136 | |||
1137 | switch (tconstrain.GetConstraintType()) | ||
1138 | { | ||
1139 | case TypedConstraintType.D6_CONSTRAINT_TYPE: | ||
1140 | Generic6DofConstraint constrain = pConstraint as Generic6DofConstraint; | ||
1141 | constrain.GetTranslationalLimitMotor().m_enableMotor[0] = onOff; | ||
1142 | constrain.GetTranslationalLimitMotor().m_targetVelocity[0] = targetVelocity; | ||
1143 | constrain.GetTranslationalLimitMotor().m_maxMotorForce[0] = maxMotorForce; | ||
1144 | ret = true; | ||
1145 | break; | ||
1146 | } | ||
1147 | |||
1148 | |||
1149 | return ret; | ||
1150 | |||
1151 | } | ||
1152 | |||
1153 | public override int PhysicsStep2(object pWorld, float timeStep, int m_maxSubSteps, float m_fixedTimeStep, out int updatedEntityCount, out List<BulletXNA.EntityProperties> updatedEntities, out int collidersCount, out List<BulletXNA.CollisionDesc>colliders) | ||
1154 | { | ||
1155 | int epic = PhysicsStepint2(pWorld, timeStep, m_maxSubSteps, m_fixedTimeStep, out updatedEntityCount, out updatedEntities, | ||
1156 | out collidersCount, out colliders); | ||
1157 | return epic; | ||
1158 | } | ||
1159 | |||
1160 | private static int PhysicsStepint2(object pWorld,float timeStep, int m_maxSubSteps, float m_fixedTimeStep, out int updatedEntityCount, out List<BulletXNA.EntityProperties> updatedEntities, out int collidersCount, out List<BulletXNA.CollisionDesc> colliders) | ||
1161 | { | ||
1162 | int numSimSteps = 0; | ||
1163 | |||
1164 | |||
1165 | //if (updatedEntities is null) | ||
1166 | // updatedEntities = new List<BulletXNA.EntityProperties>(); | ||
1167 | |||
1168 | //if (colliders is null) | ||
1169 | // colliders = new List<BulletXNA.CollisionDesc>(); | ||
1170 | |||
1171 | |||
1172 | if (pWorld is DiscreteDynamicsWorld) | ||
1173 | { | ||
1174 | DiscreteDynamicsWorld world = pWorld as DiscreteDynamicsWorld; | ||
1175 | |||
1176 | numSimSteps = world.StepSimulation(timeStep, m_maxSubSteps, m_fixedTimeStep); | ||
1177 | int updates = 0; | ||
1178 | |||
1179 | updatedEntityCount = world.UpdatedObjects.Count; | ||
1180 | updatedEntities = new List<BulletXNA.EntityProperties>(world.UpdatedObjects); | ||
1181 | updatedEntityCount = updatedEntities.Count; | ||
1182 | world.UpdatedObjects.Clear(); | ||
1183 | |||
1184 | |||
1185 | collidersCount = world.UpdatedCollisions.Count; | ||
1186 | colliders = new List<BulletXNA.CollisionDesc>(world.UpdatedCollisions); | ||
1187 | |||
1188 | world.UpdatedCollisions.Clear(); | ||
1189 | m_collisionsThisFrame = 0; | ||
1190 | int numManifolds = world.GetDispatcher().GetNumManifolds(); | ||
1191 | for (int j = 0; j < numManifolds; j++) | ||
1192 | { | ||
1193 | PersistentManifold contactManifold = world.GetDispatcher().GetManifoldByIndexInternal(j); | ||
1194 | int numContacts = contactManifold.GetNumContacts(); | ||
1195 | if (numContacts == 0) | ||
1196 | continue; | ||
1197 | |||
1198 | CollisionObject objA = contactManifold.GetBody0() as CollisionObject; | ||
1199 | CollisionObject objB = contactManifold.GetBody1() as CollisionObject; | ||
1200 | |||
1201 | ManifoldPoint manifoldPoint = contactManifold.GetContactPoint(0); | ||
1202 | IndexedVector3 contactPoint = manifoldPoint.GetPositionWorldOnB(); | ||
1203 | IndexedVector3 contactNormal = -manifoldPoint.m_normalWorldOnB; // make relative to A | ||
1204 | |||
1205 | RecordCollision(world, objA, objB, contactPoint, contactNormal); | ||
1206 | m_collisionsThisFrame ++; | ||
1207 | if (m_collisionsThisFrame >= 9999999) | ||
1208 | break; | ||
1209 | |||
1210 | |||
1211 | } | ||
1212 | |||
1213 | |||
1214 | } | ||
1215 | else | ||
1216 | { | ||
1217 | //if (updatedEntities is null) | ||
1218 | updatedEntities = new List<BulletXNA.EntityProperties>(); | ||
1219 | updatedEntityCount = 0; | ||
1220 | //if (colliders is null) | ||
1221 | colliders = new List<BulletXNA.CollisionDesc>(); | ||
1222 | collidersCount = 0; | ||
1223 | } | ||
1224 | return numSimSteps; | ||
1225 | } | ||
1226 | |||
1227 | private static void RecordCollision(CollisionWorld world,CollisionObject objA, CollisionObject objB, IndexedVector3 contact, IndexedVector3 norm) | ||
1228 | { | ||
1229 | |||
1230 | IndexedVector3 contactNormal = norm; | ||
1231 | if ((objA.GetCollisionFlags() & BulletXNA.BulletCollision.CollisionFlags.BS_WANTS_COLLISIONS) == 0 && | ||
1232 | (objB.GetCollisionFlags() & BulletXNA.BulletCollision.CollisionFlags.BS_WANTS_COLLISIONS) == 0) | ||
1233 | { | ||
1234 | return; | ||
1235 | } | ||
1236 | uint idA = (uint)objA.GetUserPointer(); | ||
1237 | uint idB = (uint)objB.GetUserPointer(); | ||
1238 | if (idA > idB) | ||
1239 | { | ||
1240 | uint temp = idA; | ||
1241 | idA = idB; | ||
1242 | idB = temp; | ||
1243 | contactNormal = -contactNormal; | ||
1244 | } | ||
1245 | |||
1246 | ulong collisionID = ((ulong) idA << 32) | idB; | ||
1247 | |||
1248 | BulletXNA.CollisionDesc cDesc = new BulletXNA.CollisionDesc() | ||
1249 | { | ||
1250 | aID = idA, | ||
1251 | bID = idB, | ||
1252 | point = contact, | ||
1253 | normal = contactNormal | ||
1254 | }; | ||
1255 | world.UpdatedCollisions.Add(cDesc); | ||
1256 | m_collisionsThisFrame++; | ||
1257 | |||
1258 | |||
1259 | } | ||
1260 | private static EntityProperties GetDebugProperties(object pWorld, object pBody) | ||
1261 | { | ||
1262 | EntityProperties ent = new EntityProperties(); | ||
1263 | DiscreteDynamicsWorld world = pWorld as DiscreteDynamicsWorld; | ||
1264 | RigidBody body = pBody as RigidBody; | ||
1265 | IndexedMatrix transform = body.GetWorldTransform(); | ||
1266 | IndexedVector3 LinearVelocity = body.GetInterpolationLinearVelocity(); | ||
1267 | IndexedVector3 AngularVelocity = body.GetInterpolationAngularVelocity(); | ||
1268 | IndexedQuaternion rotation = transform.GetRotation(); | ||
1269 | ent.Acceleration = Vector3.Zero; | ||
1270 | ent.ID = (uint)body.GetUserPointer(); | ||
1271 | ent.Position = new Vector3(transform._origin.X,transform._origin.Y,transform._origin.Z); | ||
1272 | ent.Rotation = new Quaternion(rotation.X,rotation.Y,rotation.Z,rotation.W); | ||
1273 | ent.Velocity = new Vector3(LinearVelocity.X, LinearVelocity.Y, LinearVelocity.Z); | ||
1274 | ent.RotationalVelocity = new Vector3(AngularVelocity.X, AngularVelocity.Y, AngularVelocity.Z); | ||
1275 | return ent; | ||
1276 | } | ||
1277 | |||
1278 | public override Vector3 GetLocalScaling2(object pBody) | ||
1279 | { | ||
1280 | CollisionShape shape = pBody as CollisionShape; | ||
1281 | IndexedVector3 scale = shape.GetLocalScaling(); | ||
1282 | return new Vector3(scale.X,scale.Y,scale.Z); | ||
1283 | } | ||
1284 | |||
1285 | public override bool RayCastGround(object pWorld, Vector3 _RayOrigin, float pRayHeight, object NotMe) | ||
1286 | { | ||
1287 | DynamicsWorld world = pWorld as DynamicsWorld; | ||
1288 | if (world != null) | ||
1289 | { | ||
1290 | if (NotMe is CollisionObject || NotMe is RigidBody) | ||
1291 | { | ||
1292 | CollisionObject AvoidBody = NotMe as CollisionObject; | ||
1293 | |||
1294 | IndexedVector3 rOrigin = new IndexedVector3(_RayOrigin.X, _RayOrigin.Y, _RayOrigin.Z); | ||
1295 | IndexedVector3 rEnd = new IndexedVector3(_RayOrigin.X, _RayOrigin.Y, _RayOrigin.Z - pRayHeight); | ||
1296 | using ( | ||
1297 | ClosestNotMeRayResultCallback rayCallback = new ClosestNotMeRayResultCallback(rOrigin, | ||
1298 | rEnd, AvoidBody) | ||
1299 | ) | ||
1300 | { | ||
1301 | world.RayTest(ref rOrigin, ref rEnd, rayCallback); | ||
1302 | if (rayCallback.HasHit()) | ||
1303 | { | ||
1304 | IndexedVector3 hitLocation = rayCallback.m_hitPointWorld; | ||
1305 | |||
1306 | } | ||
1307 | return rayCallback.HasHit(); | ||
1308 | } | ||
1309 | } | ||
1310 | } | ||
1311 | return false; | ||
1312 | } | ||
37 | } | 1313 | } |
38 | */ | 1314 | */ |
39 | } | 1315 | } |