diff options
Diffstat (limited to '')
-rw-r--r-- | libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/ContactConstraint.cs | 488 |
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace 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 | } | ||