aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/ContactConstraint.cs
diff options
context:
space:
mode:
Diffstat (limited to '')
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/ContactConstraint.cs488
1 files changed, 0 insertions, 488 deletions
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/ContactConstraint.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/ContactConstraint.cs
deleted file mode 100644
index e731643..0000000
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/ContactConstraint.cs
+++ /dev/null
@@ -1,488 +0,0 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX.Dynamics
28{
29 public delegate float ContactSolverFunc (RigidBody bodyA, RigidBody bodyB, ManifoldPoint contactPoint, ContactSolverInfo info);
30
31 public enum ContactSolverType
32 {
33 Default = 0,
34 TypeA,
35 TypeB,
36 User,
37 MaxContactSolverType,
38 }
39
40 public class ConstraintPersistentData
41 {
42 // total applied impulse during most recent frame
43 private float _appliedImpulse;
44 private float _previousAppliedImpulse;
45 private float _accumulatedTangentImpulse0;
46 private float _accumulatedTangentImpulse1;
47
48 private float _jacDiagABInv;
49 private float _jacDiagABInvTangentA;
50 private float _jacDiagABInvTangentB;
51 private int _persistentLifeTime;
52 private float _restitution;
53 private float _friction;
54 private float _penetration;
55 private Vector3 _frictionWorldTangentialA;
56 private Vector3 _frictionWorldTangentialB;
57
58 private Vector3 _frictionAngularComponent0A;
59 private Vector3 _frictionAngularComponent0B;
60 private Vector3 _frictionAngularComponent1A;
61 private Vector3 _frictionAngularComponent1B;
62
63 //some data doesn't need to be persistent over frames: todo: clean/reuse this
64 private Vector3 _angularComponentA;
65 private Vector3 _angularComponentB;
66
67 private ContactSolverFunc _contactSolverFunc;
68 private ContactSolverFunc _frictionSolverFunc;
69
70 public float AppliedImpulse { get { return _appliedImpulse; } set { _appliedImpulse = value; } }
71 public float PreviousAppliedImpulse { get { return _previousAppliedImpulse; } set { _previousAppliedImpulse = value; } }
72 public float AccumulatedTangentImpulseA { get { return _accumulatedTangentImpulse0; } set { _accumulatedTangentImpulse0 = value; } }
73 public float AccumulatedTangentImpulseB { get { return _accumulatedTangentImpulse1; } set { _accumulatedTangentImpulse1 = value; } }
74
75 public float JacDiagABInv { get { return _jacDiagABInv; } set { _jacDiagABInv = value; } }
76 public float JacDiagABInvTangentA { get { return _jacDiagABInvTangentA; } set { _jacDiagABInvTangentA = value; } }
77 public float JacDiagABInvTangentB { get { return _jacDiagABInvTangentB; } set { _jacDiagABInvTangentB = value; } }
78 public int PersistentLifeTime { get { return _persistentLifeTime; } set { _persistentLifeTime = value; } }
79 public float Restitution { get { return _restitution; } set { _restitution = value; } }
80 public float Friction { get { return _friction; } set { _friction = value; } }
81 public float Penetration { get { return _penetration; } set { _penetration = value; } }
82 public Vector3 FrictionWorldTangentialA { get { return _frictionWorldTangentialA; } set { _frictionWorldTangentialA = value; } }
83 public Vector3 FrictionWorldTangentialB { get { return _frictionWorldTangentialB; } set { _frictionWorldTangentialB = value; } }
84
85 public Vector3 FrictionAngularComponent0A { get { return _frictionAngularComponent0A; } set { _frictionAngularComponent0A = value; } }
86 public Vector3 FrictionAngularComponent0B { get { return _frictionAngularComponent0B; } set { _frictionAngularComponent0B = value; } }
87 public Vector3 FrictionAngularComponent1A { get { return _frictionAngularComponent1A; } set { _frictionAngularComponent1A = value; } }
88 public Vector3 FrictionAngularComponent1B { get { return _frictionAngularComponent1B; } set { _frictionAngularComponent1B = value; } }
89
90 public Vector3 AngularComponentA { get { return _angularComponentA; } set { _angularComponentA = value; } }
91 public Vector3 AngularComponentB { get { return _angularComponentB; } set { _angularComponentB = value; } }
92
93 public ContactSolverFunc ContactSolverFunc { get { return _contactSolverFunc; } set { _contactSolverFunc = value; } }
94 public ContactSolverFunc FrictionSolverFunc { get { return _frictionSolverFunc; } set { _frictionSolverFunc = value; } }
95 }
96
97 public static class ContactConstraint
98 {
99 private const int UseInternalApplyImpulse = 1;
100
101 /// <summary>
102 /// bilateral constraint between two dynamic objects
103 /// positive distance = separation, negative distance = penetration
104 /// </summary>
105 /// <param name="body1"></param>
106 /// <param name="pos1"></param>
107 /// <param name="body2"></param>
108 /// <param name="pos2"></param>
109 /// <param name="distance"></param>
110 /// <param name="normal"></param>
111 /// <param name="impulse"></param>
112 /// <param name="timeStep"></param>
113 public static void ResolveSingleBilateral(RigidBody bodyA, Vector3 posA,
114 RigidBody bodyB, Vector3 posB,
115 float distance, Vector3 normal, out float impulse, float timeStep)
116 {
117 float normalLenSqr = normal.LengthSquared();
118
119 if (Math.Abs(normalLenSqr) >= 1.1f)
120 throw new BulletException();
121
122 /*if (normalLenSqr > 1.1f)
123 {
124 impulse = 0f;
125 return;
126 }*/
127 Vector3 rel_pos1 = posA - bodyA.CenterOfMassPosition;
128 Vector3 rel_pos2 = posB - bodyB.CenterOfMassPosition;
129 //this jacobian entry could be re-used for all iterations
130
131 Vector3 vel1 = bodyA.GetVelocityInLocalPoint(rel_pos1);
132 Vector3 vel2 = bodyB.GetVelocityInLocalPoint(rel_pos2);
133 Vector3 vel = vel1 - vel2;
134
135
136 JacobianEntry jac = new JacobianEntry(Matrix.Transpose(bodyA.CenterOfMassTransform),
137 Matrix.Transpose(bodyB.CenterOfMassTransform),
138 rel_pos1, rel_pos2, normal, bodyA.InvInertiaDiagLocal, bodyA.InverseMass,
139 bodyB.InvInertiaDiagLocal, bodyB.InverseMass);
140
141 float jacDiagAB = jac.Diagonal;
142 float jacDiagABInv = 1f / jacDiagAB;
143
144 float rel_vel = jac.GetRelativeVelocity(
145 bodyA.LinearVelocity,
146 Vector3.TransformNormal(bodyA.AngularVelocity, Matrix.Transpose(bodyA.CenterOfMassTransform)),
147 bodyB.LinearVelocity,
148 Vector3.TransformNormal(bodyB.AngularVelocity, Matrix.Transpose(bodyB.CenterOfMassTransform)));
149 float a;
150 a = jacDiagABInv;
151
152
153 rel_vel = Vector3.Dot(normal, vel);
154
155 float contactDamping = 0.2f;
156
157 float velocityImpulse = -contactDamping * rel_vel * jacDiagABInv;
158 impulse = velocityImpulse;
159 }
160
161
162 /// <summary>
163 /// contact constraint resolution:
164 /// calculate and apply impulse to satisfy non-penetration and non-negative relative velocity constraint
165 /// positive distance = separation, negative distance = penetration
166 /// </summary>
167 /// <param name="body1"></param>
168 /// <param name="body2"></param>
169 /// <param name="contactPoint"></param>
170 /// <param name="info"></param>
171 /// <returns></returns>
172 public static float ResolveSingleCollision(RigidBody bodyA, RigidBody bodyB,
173 ManifoldPoint contactPoint, ContactSolverInfo solverInfo)
174 {
175 Vector3 pos1 = contactPoint.PositionWorldOnA;
176 Vector3 pos2 = contactPoint.PositionWorldOnB;
177
178
179 // printf("distance=%f\n",distance);
180
181 Vector3 normal = contactPoint.NormalWorldOnB;
182
183 Vector3 rel_pos1 = pos1 - bodyA.CenterOfMassPosition;
184 Vector3 rel_pos2 = pos2 - bodyB.CenterOfMassPosition;
185
186 Vector3 vel1 = bodyA.GetVelocityInLocalPoint(rel_pos1);
187 Vector3 vel2 = bodyB.GetVelocityInLocalPoint(rel_pos2);
188 Vector3 vel = vel1 - vel2;
189 float rel_vel;
190 rel_vel = Vector3.Dot(normal, vel);
191
192
193 float Kfps = 1f / solverInfo.TimeStep;
194
195 //float damping = solverInfo.m_damping;
196 float Kerp = solverInfo.Erp;
197
198 float Kcor = Kerp * Kfps;
199
200 //printf("dist=%f\n",distance);
201
202 ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData;
203 if (cpd == null)
204 throw new BulletException();
205
206 float distance = cpd.Penetration;//contactPoint.getDistance();
207
208
209 //distance = 0.f;
210 float positionalError = Kcor * -distance;
211 //jacDiagABInv;
212 float velocityError = cpd.Restitution - rel_vel;// * damping;
213
214
215 float penetrationImpulse = positionalError * cpd.JacDiagABInv;
216 float velocityImpulse = velocityError * cpd.JacDiagABInv;
217 float normalImpulse = penetrationImpulse + velocityImpulse;
218
219 // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
220 float oldNormalImpulse = cpd.AppliedImpulse;
221 float sum = oldNormalImpulse + normalImpulse;
222 cpd.AppliedImpulse = 0f > sum ? 0f : sum;
223
224 normalImpulse = cpd.AppliedImpulse - oldNormalImpulse;
225
226 if (bodyA.InverseMass != 0)
227 {
228 bodyA.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyA.InverseMass, cpd.AngularComponentA, normalImpulse);
229 }
230 if (bodyB.InverseMass != 0)
231 {
232 bodyB.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyB.InverseMass, cpd.AngularComponentB, -normalImpulse);
233 }
234
235 /*body1.applyImpulse(normal * (normalImpulse), rel_pos1);
236 body2.applyImpulse(-normal * (normalImpulse), rel_pos2);*/
237
238 return normalImpulse;
239 }
240
241 public static float ResolveSingleFriction(RigidBody bodyA, RigidBody bodyB,
242 ManifoldPoint contactPoint, ContactSolverInfo solverInfo)
243 {
244
245 Vector3 pos1 = contactPoint.PositionWorldOnA;
246 Vector3 pos2 = contactPoint.PositionWorldOnB;
247
248 Vector3 rel_pos1 = pos1 - bodyA.CenterOfMassPosition;
249 Vector3 rel_pos2 = pos2 - bodyB.CenterOfMassPosition;
250
251 ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData;
252 if (cpd == null)
253 throw new BulletException();
254
255 float combinedFriction = cpd.Friction;
256
257 float limit = cpd.AppliedImpulse * combinedFriction;
258
259 //friction
260 if (cpd.AppliedImpulse > 0)
261 {
262 //apply friction in the 2 tangential directions
263
264 // 1st tangent
265 Vector3 vel1 = bodyA.GetVelocityInLocalPoint(rel_pos1);
266 Vector3 vel2 = bodyB.GetVelocityInLocalPoint(rel_pos2);
267 Vector3 vel = vel1 - vel2;
268
269 float j1, j2;
270
271 {
272
273 float vrel = Vector3.Dot(cpd.FrictionWorldTangentialA, vel);
274
275 // calculate j that moves us to zero relative velocity
276 j1 = -vrel * cpd.JacDiagABInvTangentA;
277 float oldTangentImpulse = cpd.AccumulatedTangentImpulseA;
278 cpd.AccumulatedTangentImpulseA = oldTangentImpulse + j1;
279 float atia = cpd.AccumulatedTangentImpulseA;
280 MathHelper.SetMin(ref atia, limit);
281 MathHelper.SetMax(ref atia, -limit);
282 cpd.AccumulatedTangentImpulseA = atia;
283 j1 = cpd.AccumulatedTangentImpulseA - oldTangentImpulse;
284
285 }
286 {
287 // 2nd tangent
288
289 float vrel = Vector3.Dot(cpd.FrictionWorldTangentialB, vel);
290
291 // calculate j that moves us to zero relative velocity
292 j2 = -vrel * cpd.JacDiagABInvTangentB;
293 float oldTangentImpulse = cpd.AccumulatedTangentImpulseB;
294 cpd.AccumulatedTangentImpulseB = oldTangentImpulse + j2;
295 float atib = cpd.AccumulatedTangentImpulseB;
296 MathHelper.SetMin(ref atib, limit);
297 MathHelper.SetMax(ref atib, -limit);
298 cpd.AccumulatedTangentImpulseB = atib;
299 j2 = cpd.AccumulatedTangentImpulseB - oldTangentImpulse;
300 }
301
302 if (bodyA.InverseMass != 0)
303 {
304 bodyA.InternalApplyImpulse(cpd.FrictionWorldTangentialA * bodyA.InverseMass, cpd.FrictionAngularComponent0A, j1);
305 bodyA.InternalApplyImpulse(cpd.FrictionWorldTangentialB * bodyA.InverseMass, cpd.FrictionAngularComponent1A, j2);
306 }
307 if (bodyB.InverseMass != 0)
308 {
309 bodyB.InternalApplyImpulse(cpd.FrictionWorldTangentialA * bodyB.InverseMass, cpd.FrictionAngularComponent0B, -j1);
310 bodyB.InternalApplyImpulse(cpd.FrictionWorldTangentialB * bodyB.InverseMass, cpd.FrictionAngularComponent1B, -j2);
311 }
312
313 }
314 return cpd.AppliedImpulse;
315 }
316
317 public static float ResolveSingleFrictionOriginal(
318 RigidBody bodyA,
319 RigidBody bodyB,
320 ManifoldPoint contactPoint,
321 ContactSolverInfo solverInfo)
322 {
323 Vector3 posA = contactPoint.PositionWorldOnA;
324 Vector3 posB = contactPoint.PositionWorldOnB;
325
326 Vector3 relPosA = posA - bodyA.CenterOfMassPosition;
327 Vector3 relPosB = posB - bodyB.CenterOfMassPosition;
328
329 ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData;
330 if (cpd == null)
331 throw new BulletException();
332
333 float combinedFriction = cpd.Friction;
334
335 float limit = cpd.AppliedImpulse * combinedFriction;
336 //if (contactPoint.m_appliedImpulse>0.f)
337 //friction
338 {
339 //apply friction in the 2 tangential directions
340
341 {
342 // 1st tangent
343 Vector3 velA = bodyA.GetVelocityInLocalPoint(relPosA);
344 Vector3 velB = bodyB.GetVelocityInLocalPoint(relPosB);
345 Vector3 vel = velA - velB;
346
347 float vrel = Vector3.Dot(cpd.FrictionWorldTangentialA, vel);
348
349 // calculate j that moves us to zero relative velocity
350 float j = -vrel * cpd.JacDiagABInvTangentA;
351 float total = cpd.AccumulatedTangentImpulseA + j;
352 if (limit < total)
353 total = limit;
354 if (total < -limit)
355 total = -limit;
356 j = total - cpd.AccumulatedTangentImpulseA;
357 cpd.AccumulatedTangentImpulseA = total;
358 bodyA.ApplyImpulse(j * cpd.FrictionWorldTangentialA, relPosA);
359 bodyB.ApplyImpulse(j * -cpd.FrictionWorldTangentialA, relPosB);
360 }
361
362
363 {
364 // 2nd tangent
365 Vector3 velA = bodyA.GetVelocityInLocalPoint(relPosA);
366 Vector3 velB = bodyB.GetVelocityInLocalPoint(relPosB);
367 Vector3 vel = velA - velB;
368
369 float vrel = Vector3.Dot(cpd.FrictionWorldTangentialB, vel);
370
371 // calculate j that moves us to zero relative velocity
372 float j = -vrel * cpd.JacDiagABInvTangentB;
373 float total = cpd.AccumulatedTangentImpulseB + j;
374 if (limit < total)
375 total = limit;
376 if (total < -limit)
377 total = -limit;
378 j = total - cpd.AccumulatedTangentImpulseB;
379 cpd.AccumulatedTangentImpulseB = total;
380 bodyA.ApplyImpulse(j * cpd.FrictionWorldTangentialB, relPosA);
381 bodyB.ApplyImpulse(j * -cpd.FrictionWorldTangentialB, relPosB);
382 }
383 }
384
385 return cpd.AppliedImpulse;
386 }
387
388 //velocity + friction
389 //response between two dynamic objects with friction
390 public static float ResolveSingleCollisionCombined(
391 RigidBody bodyA,
392 RigidBody bodyB,
393 ManifoldPoint contactPoint,
394 ContactSolverInfo solverInfo)
395 {
396
397 Vector3 posA = contactPoint.PositionWorldOnA;
398 Vector3 posB = contactPoint.PositionWorldOnB;
399 Vector3 normal = contactPoint.NormalWorldOnB;
400
401 Vector3 relPosA = posA - bodyA.CenterOfMassPosition;
402 Vector3 relPosB = posB - bodyB.CenterOfMassPosition;
403
404 Vector3 velA = bodyA.GetVelocityInLocalPoint(relPosA);
405 Vector3 velB = bodyB.GetVelocityInLocalPoint(relPosB);
406 Vector3 vel = velA - velB;
407 float relVel;
408 relVel = Vector3.Dot(normal, vel);
409
410 float Kfps = 1f / solverInfo.TimeStep;
411
412 //float damping = solverInfo.m_damping;
413 float Kerp = solverInfo.Erp;
414 float Kcor = Kerp * Kfps;
415
416 ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData;
417 if (cpd == null)
418 throw new BulletException();
419
420 float distance = cpd.Penetration;
421 float positionalError = Kcor * -distance;
422 float velocityError = cpd.Restitution - relVel;// * damping;
423
424 float penetrationImpulse = positionalError * cpd.JacDiagABInv;
425
426 float velocityImpulse = velocityError * cpd.JacDiagABInv;
427
428 float normalImpulse = penetrationImpulse + velocityImpulse;
429
430 // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
431 float oldNormalImpulse = cpd.AppliedImpulse;
432 float sum = oldNormalImpulse + normalImpulse;
433 cpd.AppliedImpulse = 0 > sum ? 0 : sum;
434
435 normalImpulse = cpd.AppliedImpulse - oldNormalImpulse;
436
437 if (bodyA.InverseMass != 0)
438 {
439 bodyA.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyA.InverseMass, cpd.AngularComponentA, normalImpulse);
440 }
441 if (bodyB.InverseMass != 0)
442 {
443 bodyB.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyB.InverseMass, cpd.AngularComponentB, -normalImpulse);
444 }
445
446 {
447 //friction
448 Vector3 vel12 = bodyA.GetVelocityInLocalPoint(relPosA);
449 Vector3 vel22 = bodyB.GetVelocityInLocalPoint(relPosB);
450 Vector3 vel3 = vel12 - vel22;
451
452 relVel = Vector3.Dot(normal, vel3);
453
454
455 Vector3 latVel = vel3 - normal * relVel;
456 float lat_rel_vel = latVel.Length();
457
458 float combinedFriction = cpd.Friction;
459
460 if (cpd.AppliedImpulse > 0)
461 if (lat_rel_vel > float.Epsilon)
462 {
463 latVel /= lat_rel_vel;
464 Vector3 temp1 = Vector3.TransformNormal(Vector3.Cross(relPosA, latVel), bodyA.InvInertiaTensorWorld);
465 Vector3 temp2 = Vector3.TransformNormal(Vector3.Cross(relPosB, latVel), bodyB.InvInertiaTensorWorld);
466 float friction_impulse = lat_rel_vel /
467 (bodyA.InverseMass + bodyB.InverseMass + Vector3.Dot(latVel, Vector3.Cross(temp1, relPosA) + Vector3.Cross(temp2, relPosB)));
468 float normal_impulse = cpd.AppliedImpulse * combinedFriction;
469
470 MathHelper.SetMin(ref friction_impulse, normal_impulse);
471 MathHelper.SetMin(ref friction_impulse, -normal_impulse);
472 bodyA.ApplyImpulse(latVel * -friction_impulse, relPosA);
473 bodyB.ApplyImpulse(latVel * friction_impulse, relPosB);
474 }
475 }
476 return normalImpulse;
477 }
478
479 public static float ResolveSingleFrictionEmpty(
480 RigidBody bodyA,
481 RigidBody bodyB,
482 ManifoldPoint contactPoint,
483 ContactSolverInfo solverInfo)
484 {
485 return 0;
486 }
487 }
488}
generated by cgit v1.1 at 2024-08-26 15:36:16 +0000