Bullet Collision Detection & Physics Library: btSoftBodyInternals.h Source File

Bullet Collision Detection & Physics Library

btSoftBodyInternals.h
Go to the documentation of this file.
1/*
2Bullet Continuous Collision Detection and Physics Library
3Copyright (c) 2003-2006 Erwin Coumans https://bulletphysics.org
4
5This software is provided 'as-is', without any express or implied warranty.
6In no event will the authors be held liable for any damages arising from the use of this software.
7Permission is granted to anyone to use this software for any purpose,
8including commercial applications, and to alter it and redistribute it freely,
9subject to the following restrictions:
10
111. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
122. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
133. This notice may not be removed or altered from any source distribution.
14*/
16
17#ifndef _BT_SOFT_BODY_INTERNALS_H
18#define _BT_SOFT_BODY_INTERNALS_H
19
20#include "btSoftBody.h"
21#include "LinearMath/btQuickprof.h"
22#include "LinearMath/btPolarDecomposition.h"
23#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
24#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
25#include "BulletCollision/CollisionShapes/btConvexInternalShape.h"
26#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
27#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
28#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
29#include <string.h> //for memset
30#include <cmath>
31#include "poly34.h"
32
33// Given a multibody link, a contact point and a contact direction, fill in the jacobian data needed to calculate the velocity change given an impulse in the contact direction
34 static SIMD_FORCE_INLINE void findJacobian(const btMultiBodyLinkCollider* multibodyLinkCol,
35 btMultiBodyJacobianData& jacobianData,
36 const btVector3& contact_point,
37 const btVector3& dir)
38{
39 const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
40 jacobianData.m_jacobians.resize(ndof);
41 jacobianData.m_deltaVelocitiesUnitImpulse.resize(ndof);
42 btScalar* jac = &jacobianData.m_jacobians[0];
43
44 multibodyLinkCol->m_multiBody->fillContactJacobianMultiDof(multibodyLinkCol->m_link, contact_point, dir, jac, jacobianData.scratch_r, jacobianData.scratch_v, jacobianData.scratch_m);
45 multibodyLinkCol->m_multiBody->calcAccelerationDeltasMultiDof(&jacobianData.m_jacobians[0], &jacobianData.m_deltaVelocitiesUnitImpulse[0], jacobianData.scratch_r, jacobianData.scratch_v);
46}
47 static SIMD_FORCE_INLINE btVector3 generateUnitOrthogonalVector(const btVector3& u)
48{
49 btScalar ux = u.getX();
50 btScalar uy = u.getY();
51 btScalar uz = u.getZ();
52 btScalar ax = std::abs(ux);
53 btScalar ay = std::abs(uy);
54 btScalar az = std::abs(uz);
55 btVector3 v;
56 if (ax <= ay && ax <= az)
57 v = btVector3(0, -uz, uy);
58 else if (ay <= ax && ay <= az)
59 v = btVector3(-uz, 0, ux);
60 else
61 v = btVector3(-uy, ux, 0);
62 v.normalize();
63 return v;
64}
65
66 static SIMD_FORCE_INLINE bool proximityTest(const btVector3& x1, const btVector3& x2, const btVector3& x3, const btVector3& x4, const btVector3& normal, const btScalar& mrg, btVector3& bary)
67{
68 btVector3 x43 = x4 - x3;
69 if (std::abs(x43.dot(normal)) > mrg)
70 return false;
71 btVector3 x13 = x1 - x3;
72 btVector3 x23 = x2 - x3;
73 btScalar a11 = x13.length2();
74 btScalar a22 = x23.length2();
75 btScalar a12 = x13.dot(x23);
76 btScalar b1 = x13.dot(x43);
77 btScalar b2 = x23.dot(x43);
78 btScalar det = a11 * a22 - a12 * a12;
79 if (det < SIMD_EPSILON)
80 return false;
81 btScalar w1 = (b1 * a22 - b2 * a12) / det;
82 btScalar w2 = (b2 * a11 - b1 * a12) / det;
83 btScalar w3 = 1 - w1 - w2;
84 btScalar delta = mrg / std::sqrt(0.5 * std::abs(x13.cross(x23).safeNorm()));
85 bary = btVector3(w1, w2, w3);
86 for (int i = 0; i < 3; ++i)
87 {
88 if (bary[i] < -delta || bary[i] > 1 + delta)
89 return false;
90 }
91 return true;
92}
93 static const int KDOP_COUNT = 13;
94 static btVector3 dop[KDOP_COUNT] = {btVector3(1, 0, 0),
95 btVector3(0, 1, 0),
96 btVector3(0, 0, 1),
97 btVector3(1, 1, 0),
98 btVector3(1, 0, 1),
99 btVector3(0, 1, 1),
100 btVector3(1, -1, 0),
101 btVector3(1, 0, -1),
102 btVector3(0, 1, -1),
103 btVector3(1, 1, 1),
104 btVector3(1, -1, 1),
105 btVector3(1, 1, -1),
106 btVector3(1, -1, -1)};
107
108 static inline int getSign(const btVector3& n, const btVector3& x)
109{
110 btScalar d = n.dot(x);
111 if (d > SIMD_EPSILON)
112 return 1;
113 if (d < -SIMD_EPSILON)
114 return -1;
115 return 0;
116}
117
118 static SIMD_FORCE_INLINE bool hasSeparatingPlane(const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt)
119{
120 btVector3 hex[6] = {face->m_n[0]->m_x - node->m_x,
121 face->m_n[1]->m_x - node->m_x,
122 face->m_n[2]->m_x - node->m_x,
123 face->m_n[0]->m_x + dt * face->m_n[0]->m_v - node->m_x,
124 face->m_n[1]->m_x + dt * face->m_n[1]->m_v - node->m_x,
125 face->m_n[2]->m_x + dt * face->m_n[2]->m_v - node->m_x};
126 btVector3 segment = dt * node->m_v;
127 for (int i = 0; i < KDOP_COUNT; ++i)
128 {
129 int s = getSign(dop[i], segment);
130 int j = 0;
131 for (; j < 6; ++j)
132 {
133 if (getSign(dop[i], hex[j]) == s)
134 break;
135 }
136 if (j == 6)
137 return true;
138 }
139 return false;
140}
141
142 static SIMD_FORCE_INLINE bool nearZero(const btScalar& a)
143{
144 return (a > -SAFE_EPSILON && a < SAFE_EPSILON);
145}
146 static SIMD_FORCE_INLINE bool sameSign(const btScalar& a, const btScalar& b)
147{
148 return (nearZero(a) || nearZero(b) || (a > SAFE_EPSILON && b > SAFE_EPSILON) || (a < -SAFE_EPSILON && b < -SAFE_EPSILON));
149}
150 static SIMD_FORCE_INLINE bool diffSign(const btScalar& a, const btScalar& b)
151{
152 return !sameSign(a, b);
153}
154 inline btScalar evaluateBezier2(const btScalar& p0, const btScalar& p1, const btScalar& p2, const btScalar& t, const btScalar& s)
155{
156 btScalar s2 = s * s;
157 btScalar t2 = t * t;
158
159 return p0 * s2 + p1 * btScalar(2.0) * s * t + p2 * t2;
160}
161 inline btScalar evaluateBezier(const btScalar& p0, const btScalar& p1, const btScalar& p2, const btScalar& p3, const btScalar& t, const btScalar& s)
162{
163 btScalar s2 = s * s;
164 btScalar s3 = s2 * s;
165 btScalar t2 = t * t;
166 btScalar t3 = t2 * t;
167
168 return p0 * s3 + p1 * btScalar(3.0) * s2 * t + p2 * btScalar(3.0) * s * t2 + p3 * t3;
169}
170 static SIMD_FORCE_INLINE bool getSigns(bool type_c, const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& t0, const btScalar& t1, btScalar& lt0, btScalar& lt1)
171{
172 if (sameSign(t0, t1))
173 {
174 lt0 = t0;
175 lt1 = t0;
176 return true;
177 }
178
179 if (type_c || diffSign(k0, k3))
180 {
181 btScalar ft = evaluateBezier(k0, k1, k2, k3, t0, -t1);
182 if (t0 < -0)
183 ft = -ft;
184
185 if (sameSign(ft, k0))
186 {
187 lt0 = t1;
188 lt1 = t1;
189 }
190 else
191 {
192 lt0 = t0;
193 lt1 = t0;
194 }
195 return true;
196 }
197
198 if (!type_c)
199 {
200 btScalar ft = evaluateBezier(k0, k1, k2, k3, t0, -t1);
201 if (t0 < -0)
202 ft = -ft;
203
204 if (diffSign(ft, k0))
205 {
206 lt0 = t0;
207 lt1 = t1;
208 return true;
209 }
210
211 btScalar fk = evaluateBezier2(k1 - k0, k2 - k1, k3 - k2, t0, -t1);
212
213 if (sameSign(fk, k1 - k0))
214 lt0 = lt1 = t1;
215 else
216 lt0 = lt1 = t0;
217
218 return true;
219 }
220 return false;
221}
222
223 static SIMD_FORCE_INLINE void getBernsteinCoeff(const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt, btScalar& k0, btScalar& k1, btScalar& k2, btScalar& k3)
224{
225 const btVector3& n0 = face->m_n0;
226 const btVector3& n1 = face->m_n1;
227 btVector3 n_hat = n0 + n1 - face->m_vn;
228 btVector3 p0ma0 = node->m_x - face->m_n[0]->m_x;
229 btVector3 p1ma1 = node->m_q - face->m_n[0]->m_q;
230 k0 = (p0ma0).dot(n0) * 3.0;
231 k1 = (p0ma0).dot(n_hat) + (p1ma1).dot(n0);
232 k2 = (p1ma1).dot(n_hat) + (p0ma0).dot(n1);
233 k3 = (p1ma1).dot(n1) * 3.0;
234}
235
236 static SIMD_FORCE_INLINE void polyDecomposition(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& j0, const btScalar& j1, const btScalar& j2, btScalar& u0, btScalar& u1, btScalar& v0, btScalar& v1)
237{
238 btScalar denom = 4.0 * (j1 - j2) * (j1 - j0) + (j2 - j0) * (j2 - j0);
239 u0 = (2.0 * (j1 - j2) * (3.0 * k1 - 2.0 * k0 - k3) - (j0 - j2) * (3.0 * k2 - 2.0 * k3 - k0)) / denom;
240 u1 = (2.0 * (j1 - j0) * (3.0 * k2 - 2.0 * k3 - k0) - (j2 - j0) * (3.0 * k1 - 2.0 * k0 - k3)) / denom;
241 v0 = k0 - u0 * j0;
242 v1 = k3 - u1 * j2;
243}
244
245 static SIMD_FORCE_INLINE bool rootFindingLemma(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3)
246{
247 btScalar u0, u1, v0, v1;
248 btScalar j0 = 3.0 * (k1 - k0);
249 btScalar j1 = 3.0 * (k2 - k1);
250 btScalar j2 = 3.0 * (k3 - k2);
251 polyDecomposition(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1);
252 if (sameSign(v0, v1))
253 {
254 btScalar Ypa = j0 * (1.0 - v0) * (1.0 - v0) + 2.0 * j1 * v0 * (1.0 - v0) + j2 * v0 * v0; // Y'(v0)
255 if (sameSign(Ypa, j0))
256 {
257 return (diffSign(k0, v1));
258 }
259 }
260 return diffSign(k0, v0);
261}
262
263 static SIMD_FORCE_INLINE void getJs(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btSoftBody::Node* a, const btSoftBody::Node* b, const btSoftBody::Node* c, const btSoftBody::Node* p, const btScalar& dt, btScalar& j0, btScalar& j1, btScalar& j2)
264{
265 const btVector3& a0 = a->m_x;
266 const btVector3& b0 = b->m_x;
267 const btVector3& c0 = c->m_x;
268 const btVector3& va = a->m_v;
269 const btVector3& vb = b->m_v;
270 const btVector3& vc = c->m_v;
271 const btVector3 a1 = a0 + dt * va;
272 const btVector3 b1 = b0 + dt * vb;
273 const btVector3 c1 = c0 + dt * vc;
274 btVector3 n0 = (b0 - a0).cross(c0 - a0);
275 btVector3 n1 = (b1 - a1).cross(c1 - a1);
276 btVector3 n_hat = n0 + n1 - dt * dt * (vb - va).cross(vc - va);
277 const btVector3& p0 = p->m_x;
278 const btVector3& vp = p->m_v;
279 btVector3 p1 = p0 + dt * vp;
280 btVector3 m0 = (b0 - p0).cross(c0 - p0);
281 btVector3 m1 = (b1 - p1).cross(c1 - p1);
282 btVector3 m_hat = m0 + m1 - dt * dt * (vb - vp).cross(vc - vp);
283 btScalar l0 = m0.dot(n0);
284 btScalar l1 = 0.25 * (m0.dot(n_hat) + m_hat.dot(n0));
285 btScalar l2 = btScalar(1) / btScalar(6) * (m0.dot(n1) + m_hat.dot(n_hat) + m1.dot(n0));
286 btScalar l3 = 0.25 * (m_hat.dot(n1) + m1.dot(n_hat));
287 btScalar l4 = m1.dot(n1);
288
289 btScalar k1p = 0.25 * k0 + 0.75 * k1;
290 btScalar k2p = 0.5 * k1 + 0.5 * k2;
291 btScalar k3p = 0.75 * k2 + 0.25 * k3;
292
293 btScalar s0 = (l1 * k0 - l0 * k1p) * 4.0;
294 btScalar s1 = (l2 * k0 - l0 * k2p) * 2.0;
295 btScalar s2 = (l3 * k0 - l0 * k3p) * btScalar(4) / btScalar(3);
296 btScalar s3 = l4 * k0 - l0 * k3;
297
298 j0 = (s1 * k0 - s0 * k1) * 3.0;
299 j1 = (s2 * k0 - s0 * k2) * 1.5;
300 j2 = (s3 * k0 - s0 * k3);
301}
302
303 static SIMD_FORCE_INLINE bool signDetermination1Internal(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& u0, const btScalar& u1, const btScalar& v0, const btScalar& v1)
304{
305 btScalar Yu0 = k0 * (1.0 - u0) * (1.0 - u0) * (1.0 - u0) + 3.0 * k1 * u0 * (1.0 - u0) * (1.0 - u0) + 3.0 * k2 * u0 * u0 * (1.0 - u0) + k3 * u0 * u0 * u0; // Y(u0)
306 btScalar Yv0 = k0 * (1.0 - v0) * (1.0 - v0) * (1.0 - v0) + 3.0 * k1 * v0 * (1.0 - v0) * (1.0 - v0) + 3.0 * k2 * v0 * v0 * (1.0 - v0) + k3 * v0 * v0 * v0; // Y(v0)
307
308 btScalar sign_Ytp = (u0 > u1) ? Yu0 : -Yu0;
309 btScalar L = sameSign(sign_Ytp, k0) ? u1 : u0;
310 sign_Ytp = (v0 > v1) ? Yv0 : -Yv0;
311 btScalar K = (sameSign(sign_Ytp, k0)) ? v1 : v0;
312 return diffSign(L, K);
313}
314
315 static SIMD_FORCE_INLINE bool signDetermination2Internal(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& j0, const btScalar& j1, const btScalar& j2, const btScalar& u0, const btScalar& u1, const btScalar& v0, const btScalar& v1)
316{
317 btScalar Yu0 = k0 * (1.0 - u0) * (1.0 - u0) * (1.0 - u0) + 3.0 * k1 * u0 * (1.0 - u0) * (1.0 - u0) + 3.0 * k2 * u0 * u0 * (1.0 - u0) + k3 * u0 * u0 * u0; // Y(u0)
318 btScalar sign_Ytp = (u0 > u1) ? Yu0 : -Yu0, L1, L2;
319 if (diffSign(sign_Ytp, k0))
320 {
321 L1 = u0;
322 L2 = u1;
323 }
324 else
325 {
326 btScalar Yp_u0 = j0 * (1.0 - u0) * (1.0 - u0) + 2.0 * j1 * (1.0 - u0) * u0 + j2 * u0 * u0;
327 if (sameSign(Yp_u0, j0))
328 {
329 L1 = u1;
330 L2 = u1;
331 }
332 else
333 {
334 L1 = u0;
335 L2 = u0;
336 }
337 }
338 btScalar Yv0 = k0 * (1.0 - v0) * (1.0 - v0) * (1.0 - v0) + 3.0 * k1 * v0 * (1.0 - v0) * (1.0 - v0) + 3.0 * k2 * v0 * v0 * (1.0 - v0) + k3 * v0 * v0 * v0; // Y(uv0)
339 sign_Ytp = (v0 > v1) ? Yv0 : -Yv0;
340 btScalar K1, K2;
341 if (diffSign(sign_Ytp, k0))
342 {
343 K1 = v0;
344 K2 = v1;
345 }
346 else
347 {
348 btScalar Yp_v0 = j0 * (1.0 - v0) * (1.0 - v0) + 2.0 * j1 * (1.0 - v0) * v0 + j2 * v0 * v0;
349 if (sameSign(Yp_v0, j0))
350 {
351 K1 = v1;
352 K2 = v1;
353 }
354 else
355 {
356 K1 = v0;
357 K2 = v0;
358 }
359 }
360 return (diffSign(K1, L1) || diffSign(L2, K2));
361}
362
363 static SIMD_FORCE_INLINE bool signDetermination1(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt)
364{
365 btScalar j0, j1, j2, u0, u1, v0, v1;
366 // p1
367 getJs(k0, k1, k2, k3, face->m_n[0], face->m_n[1], face->m_n[2], node, dt, j0, j1, j2);
368 if (nearZero(j0 + j2 - j1 * 2.0))
369 {
370 btScalar lt0, lt1;
371 getSigns(true, k0, k1, k2, k3, j0, j2, lt0, lt1);
372 if (lt0 < -SAFE_EPSILON)
373 return false;
374 }
375 else
376 {
377 polyDecomposition(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1);
378 if (!signDetermination1Internal(k0, k1, k2, k3, u0, u1, v0, v1))
379 return false;
380 }
381 // p2
382 getJs(k0, k1, k2, k3, face->m_n[1], face->m_n[2], face->m_n[0], node, dt, j0, j1, j2);
383 if (nearZero(j0 + j2 - j1 * 2.0))
384 {
385 btScalar lt0, lt1;
386 getSigns(true, k0, k1, k2, k3, j0, j2, lt0, lt1);
387 if (lt0 < -SAFE_EPSILON)
388 return false;
389 }
390 else
391 {
392 polyDecomposition(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1);
393 if (!signDetermination1Internal(k0, k1, k2, k3, u0, u1, v0, v1))
394 return false;
395 }
396 // p3
397 getJs(k0, k1, k2, k3, face->m_n[2], face->m_n[0], face->m_n[1], node, dt, j0, j1, j2);
398 if (nearZero(j0 + j2 - j1 * 2.0))
399 {
400 btScalar lt0, lt1;
401 getSigns(true, k0, k1, k2, k3, j0, j2, lt0, lt1);
402 if (lt0 < -SAFE_EPSILON)
403 return false;
404 }
405 else
406 {
407 polyDecomposition(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1);
408 if (!signDetermination1Internal(k0, k1, k2, k3, u0, u1, v0, v1))
409 return false;
410 }
411 return true;
412}
413
414 static SIMD_FORCE_INLINE bool signDetermination2(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt)
415{
416 btScalar j0, j1, j2, u0, u1, v0, v1;
417 // p1
418 getJs(k0, k1, k2, k3, face->m_n[0], face->m_n[1], face->m_n[2], node, dt, j0, j1, j2);
419 if (nearZero(j0 + j2 - j1 * 2.0))
420 {
421 btScalar lt0, lt1;
422 bool bt0 = true, bt1 = true;
423 getSigns(false, k0, k1, k2, k3, j0, j2, lt0, lt1);
424 if (lt0 < -SAFE_EPSILON)
425 bt0 = false;
426 if (lt1 < -SAFE_EPSILON)
427 bt1 = false;
428 if (!bt0 && !bt1)
429 return false;
430 }
431 else
432 {
433 polyDecomposition(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1);
434 if (!signDetermination2Internal(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1))
435 return false;
436 }
437 // p2
438 getJs(k0, k1, k2, k3, face->m_n[1], face->m_n[2], face->m_n[0], node, dt, j0, j1, j2);
439 if (nearZero(j0 + j2 - j1 * 2.0))
440 {
441 btScalar lt0, lt1;
442 bool bt0 = true, bt1 = true;
443 getSigns(false, k0, k1, k2, k3, j0, j2, lt0, lt1);
444 if (lt0 < -SAFE_EPSILON)
445 bt0 = false;
446 if (lt1 < -SAFE_EPSILON)
447 bt1 = false;
448 if (!bt0 && !bt1)
449 return false;
450 }
451 else
452 {
453 polyDecomposition(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1);
454 if (!signDetermination2Internal(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1))
455 return false;
456 }
457 // p3
458 getJs(k0, k1, k2, k3, face->m_n[2], face->m_n[0], face->m_n[1], node, dt, j0, j1, j2);
459 if (nearZero(j0 + j2 - j1 * 2.0))
460 {
461 btScalar lt0, lt1;
462 bool bt0 = true, bt1 = true;
463 getSigns(false, k0, k1, k2, k3, j0, j2, lt0, lt1);
464 if (lt0 < -SAFE_EPSILON)
465 bt0 = false;
466 if (lt1 < -SAFE_EPSILON)
467 bt1 = false;
468 if (!bt0 && !bt1)
469 return false;
470 }
471 else
472 {
473 polyDecomposition(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1);
474 if (!signDetermination2Internal(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1))
475 return false;
476 }
477 return true;
478}
479
480 static SIMD_FORCE_INLINE bool coplanarAndInsideTest(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt)
481{
482 // Coplanar test
483 if (diffSign(k1 - k0, k3 - k2))
484 {
485 // Case b:
486 if (sameSign(k0, k3) && !rootFindingLemma(k0, k1, k2, k3))
487 return false;
488 // inside test
489 return signDetermination2(k0, k1, k2, k3, face, node, dt);
490 }
491 else
492 {
493 // Case c:
494 if (sameSign(k0, k3))
495 return false;
496 // inside test
497 return signDetermination1(k0, k1, k2, k3, face, node, dt);
498 }
499 return false;
500}
501 static SIMD_FORCE_INLINE bool conservativeCulling(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& mrg)
502{
503 if (k0 > mrg && k1 > mrg && k2 > mrg && k3 > mrg)
504 return true;
505 if (k0 < -mrg && k1 < -mrg && k2 < -mrg && k3 < -mrg)
506 return true;
507 return false;
508}
509
510 static SIMD_FORCE_INLINE bool bernsteinVFTest(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& mrg, const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt)
511{
512 if (conservativeCulling(k0, k1, k2, k3, mrg))
513 return false;
514 return coplanarAndInsideTest(k0, k1, k2, k3, face, node, dt);
515}
516
517 static SIMD_FORCE_INLINE void deCasteljau(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& t0, btScalar& k10, btScalar& k20, btScalar& k30, btScalar& k21, btScalar& k12)
518{
519 k10 = k0 * (1.0 - t0) + k1 * t0;
520 btScalar k11 = k1 * (1.0 - t0) + k2 * t0;
521 k12 = k2 * (1.0 - t0) + k3 * t0;
522 k20 = k10 * (1.0 - t0) + k11 * t0;
523 k21 = k11 * (1.0 - t0) + k12 * t0;
524 k30 = k20 * (1.0 - t0) + k21 * t0;
525}
526 static SIMD_FORCE_INLINE bool bernsteinVFTest(const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt, const btScalar& mrg)
527{
528 btScalar k0, k1, k2, k3;
529 getBernsteinCoeff(face, node, dt, k0, k1, k2, k3);
530 if (conservativeCulling(k0, k1, k2, k3, mrg))
531 return false;
532 return true;
533 if (diffSign(k2 - 2.0 * k1 + k0, k3 - 2.0 * k2 + k1))
534 {
535 btScalar k10, k20, k30, k21, k12;
536 btScalar t0 = (k2 - 2.0 * k1 + k0) / (k0 - 3.0 * k1 + 3.0 * k2 - k3);
537 deCasteljau(k0, k1, k2, k3, t0, k10, k20, k30, k21, k12);
538 return bernsteinVFTest(k0, k10, k20, k30, mrg, face, node, dt) || bernsteinVFTest(k30, k21, k12, k3, mrg, face, node, dt);
539 }
540 return coplanarAndInsideTest(k0, k1, k2, k3, face, node, dt);
541}
542
543 static SIMD_FORCE_INLINE bool continuousCollisionDetection(const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt, const btScalar& mrg, btVector3& bary)
544{
545 if (hasSeparatingPlane(face, node, dt))
546 return false;
547 btVector3 x21 = face->m_n[1]->m_x - face->m_n[0]->m_x;
548 btVector3 x31 = face->m_n[2]->m_x - face->m_n[0]->m_x;
549 btVector3 x41 = node->m_x - face->m_n[0]->m_x;
550 btVector3 v21 = face->m_n[1]->m_v - face->m_n[0]->m_v;
551 btVector3 v31 = face->m_n[2]->m_v - face->m_n[0]->m_v;
552 btVector3 v41 = node->m_v - face->m_n[0]->m_v;
553 btVector3 a = x21.cross(x31);
554 btVector3 b = x21.cross(v31) + v21.cross(x31);
555 btVector3 c = v21.cross(v31);
556 btVector3 d = x41;
557 btVector3 e = v41;
558 btScalar a0 = a.dot(d);
559 btScalar a1 = a.dot(e) + b.dot(d);
560 btScalar a2 = c.dot(d) + b.dot(e);
561 btScalar a3 = c.dot(e);
562 btScalar eps = SAFE_EPSILON;
563 int num_roots = 0;
564 btScalar roots[3];
565 if (std::abs(a3) < eps)
566 {
567 // cubic term is zero
568 if (std::abs(a2) < eps)
569 {
570 if (std::abs(a1) < eps)
571 {
572 if (std::abs(a0) < eps)
573 {
574 num_roots = 2;
575 roots[0] = 0;
576 roots[1] = dt;
577 }
578 }
579 else
580 {
581 num_roots = 1;
582 roots[0] = -a0 / a1;
583 }
584 }
585 else
586 {
587 num_roots = SolveP2(roots, a1 / a2, a0 / a2);
588 }
589 }
590 else
591 {
592 num_roots = SolveP3(roots, a2 / a3, a1 / a3, a0 / a3);
593 }
594 // std::sort(roots, roots+num_roots);
595 if (num_roots > 1)
596 {
597 if (roots[0] > roots[1])
598 btSwap(roots[0], roots[1]);
599 }
600 if (num_roots > 2)
601 {
602 if (roots[0] > roots[2])
603 btSwap(roots[0], roots[2]);
604 if (roots[1] > roots[2])
605 btSwap(roots[1], roots[2]);
606 }
607 for (int r = 0; r < num_roots; ++r)
608 {
609 double root = roots[r];
610 if (root <= 0)
611 continue;
612 if (root > dt + SIMD_EPSILON)
613 return false;
614 btVector3 x1 = face->m_n[0]->m_x + root * face->m_n[0]->m_v;
615 btVector3 x2 = face->m_n[1]->m_x + root * face->m_n[1]->m_v;
616 btVector3 x3 = face->m_n[2]->m_x + root * face->m_n[2]->m_v;
617 btVector3 x4 = node->m_x + root * node->m_v;
618 btVector3 normal = (x2 - x1).cross(x3 - x1);
619 normal.safeNormalize();
620 if (proximityTest(x1, x2, x3, x4, normal, mrg, bary))
621 return true;
622 }
623 return false;
624}
625 static SIMD_FORCE_INLINE bool bernsteinCCD(const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt, const btScalar& mrg, btVector3& bary)
626{
627 if (!bernsteinVFTest(face, node, dt, mrg))
628 return false;
629 if (!continuousCollisionDetection(face, node, dt, 1e-6, bary))
630 return false;
631 return true;
632}
633
634//
635// btSymMatrix
636//
637template <typename T>
638 struct btSymMatrix
639{
640 btSymMatrix() : dim(0) {}
641 btSymMatrix(int n, const T& init = T()) { resize(n, init); }
642 void resize(int n, const T& init = T())
643 {
644 dim = n;
645 store.resize((n * (n + 1)) / 2, init);
646 }
647 int index(int c, int r) const
648 {
649 if (c > r) btSwap(c, r);
650 btAssert(r < dim);
651 return ((r * (r + 1)) / 2 + c);
652 }
653 T& operator()(int c, int r) { return (store[index(c, r)]); }
654 const T& operator()(int c, int r) const { return (store[index(c, r)]); }
655 btAlignedObjectArray<T> store;
656 int dim;
657};
658
659//
660// btSoftBodyCollisionShape
661//
662 class btSoftBodyCollisionShape : public btConcaveShape
663{
664public:
665 btSoftBody* m_body;
666
667 btSoftBodyCollisionShape(btSoftBody* backptr)
668 {
669 m_shapeType = SOFTBODY_SHAPE_PROXYTYPE;
670 m_body = backptr;
671 }
672
673 virtual ~btSoftBodyCollisionShape()
674 {
675 }
676
677 void processAllTriangles(btTriangleCallback* /*callback*/, const btVector3& /*aabbMin*/, const btVector3& /*aabbMax*/) const
678 {
679 //not yet
680 btAssert(0);
681 }
682
684 virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const
685 {
686 /* t is usually identity, except when colliding against btCompoundShape. See Issue 512 */
687 const btVector3 mins = m_body->m_bounds[0];
688 const btVector3 maxs = m_body->m_bounds[1];
689 const btVector3 crns[] = {t * btVector3(mins.x(), mins.y(), mins.z()),
690 t * btVector3(maxs.x(), mins.y(), mins.z()),
691 t * btVector3(maxs.x(), maxs.y(), mins.z()),
692 t * btVector3(mins.x(), maxs.y(), mins.z()),
693 t * btVector3(mins.x(), mins.y(), maxs.z()),
694 t * btVector3(maxs.x(), mins.y(), maxs.z()),
695 t * btVector3(maxs.x(), maxs.y(), maxs.z()),
696 t * btVector3(mins.x(), maxs.y(), maxs.z())};
697 aabbMin = aabbMax = crns[0];
698 for (int i = 1; i < 8; ++i)
699 {
700 aabbMin.setMin(crns[i]);
701 aabbMax.setMax(crns[i]);
702 }
703 }
704
705 virtual void setLocalScaling(const btVector3& /*scaling*/)
706 {
708 }
709 virtual const btVector3& getLocalScaling() const
710 {
711 static const btVector3 dummy(1, 1, 1);
712 return dummy;
713 }
714 virtual void calculateLocalInertia(btScalar /*mass*/, btVector3& /*inertia*/) const
715 {
717 btAssert(0);
718 }
719 virtual const char* getName() const
720 {
721 return "SoftBody";
722 }
723};
724
725//
726// btSoftClusterCollisionShape
727//
728 class btSoftClusterCollisionShape : public btConvexInternalShape
729{
730public:
731 const btSoftBody::Cluster* m_cluster;
732
733 btSoftClusterCollisionShape(const btSoftBody::Cluster* cluster) : m_cluster(cluster) { setMargin(0); }
734
735 virtual btVector3 localGetSupportingVertex(const btVector3& vec) const
736 {
737 btSoftBody::Node* const* n = &m_cluster->m_nodes[0];
738 btScalar d = btDot(vec, n[0]->m_x);
739 int j = 0;
740 for (int i = 1, ni = m_cluster->m_nodes.size(); i < ni; ++i)
741 {
742 const btScalar k = btDot(vec, n[i]->m_x);
743 if (k > d)
744 {
745 d = k;
746 j = i;
747 }
748 }
749 return (n[j]->m_x);
750 }
751 virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const
752 {
753 return (localGetSupportingVertex(vec));
754 }
755 //notice that the vectors should be unit length
756 virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const
757 {
758 }
759
760 virtual void calculateLocalInertia(btScalar mass, btVector3& inertia) const
761 {
762 }
763
764 virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const
765 {
766 }
767
768 virtual int getShapeType() const { return SOFTBODY_SHAPE_PROXYTYPE; }
769
770 //debugging
771 virtual const char* getName() const { return "SOFTCLUSTER"; }
772
773 virtual void setMargin(btScalar margin)
774 {
775 btConvexInternalShape::setMargin(margin);
776 }
777 virtual btScalar getMargin() const
778 {
779 return btConvexInternalShape::getMargin();
780 }
781};
782
783//
784// Inline's
785//
786
787//
788template <typename T>
789 static inline void ZeroInitialize(T& value)
790{
791 memset(&value, 0, sizeof(T));
792}
793//
794template <typename T>
795 static inline bool CompLess(const T& a, const T& b)
796{
797 return (a < b);
798}
799//
800template <typename T>
801 static inline bool CompGreater(const T& a, const T& b)
802{
803 return (a > b);
804}
805//
806template <typename T>
807 static inline T Lerp(const T& a, const T& b, btScalar t)
808{
809 return (a + (b - a) * t);
810}
811//
812template <typename T>
813 static inline T InvLerp(const T& a, const T& b, btScalar t)
814{
815 return ((b + a * t - b * t) / (a * b));
816}
817//
818 static inline btMatrix3x3 Lerp(const btMatrix3x3& a,
819 const btMatrix3x3& b,
820 btScalar t)
821{
822 btMatrix3x3 r;
823 r[0] = Lerp(a[0], b[0], t);
824 r[1] = Lerp(a[1], b[1], t);
825 r[2] = Lerp(a[2], b[2], t);
826 return (r);
827}
828//
829 static inline btVector3 Clamp(const btVector3& v, btScalar maxlength)
830{
831 const btScalar sql = v.length2();
832 if (sql > (maxlength * maxlength))
833 return ((v * maxlength) / btSqrt(sql));
834 else
835 return (v);
836}
837//
838template <typename T>
839 static inline T Clamp(const T& x, const T& l, const T& h)
840{
841 return (x < l ? l : x > h ? h : x);
842}
843//
844template <typename T>
845 static inline T Sq(const T& x)
846{
847 return (x * x);
848}
849//
850template <typename T>
851 static inline T Cube(const T& x)
852{
853 return (x * x * x);
854}
855//
856template <typename T>
857 static inline T Sign(const T& x)
858{
859 return ((T)(x < 0 ? -1 : +1));
860}
861//
862template <typename T>
863 static inline bool SameSign(const T& x, const T& y)
864{
865 return ((x * y) > 0);
866}
867//
868 static inline btScalar ClusterMetric(const btVector3& x, const btVector3& y)
869{
870 const btVector3 d = x - y;
871 return (btFabs(d[0]) + btFabs(d[1]) + btFabs(d[2]));
872}
873//
874 static inline btMatrix3x3 ScaleAlongAxis(const btVector3& a, btScalar s)
875{
876 const btScalar xx = a.x() * a.x();
877 const btScalar yy = a.y() * a.y();
878 const btScalar zz = a.z() * a.z();
879 const btScalar xy = a.x() * a.y();
880 const btScalar yz = a.y() * a.z();
881 const btScalar zx = a.z() * a.x();
882 btMatrix3x3 m;
883 m[0] = btVector3(1 - xx + xx * s, xy * s - xy, zx * s - zx);
884 m[1] = btVector3(xy * s - xy, 1 - yy + yy * s, yz * s - yz);
885 m[2] = btVector3(zx * s - zx, yz * s - yz, 1 - zz + zz * s);
886 return (m);
887}
888//
889 static inline btMatrix3x3 Cross(const btVector3& v)
890{
891 btMatrix3x3 m;
892 m[0] = btVector3(0, -v.z(), +v.y());
893 m[1] = btVector3(+v.z(), 0, -v.x());
894 m[2] = btVector3(-v.y(), +v.x(), 0);
895 return (m);
896}
897//
898 static inline btMatrix3x3 Diagonal(btScalar x)
899{
900 btMatrix3x3 m;
901 m[0] = btVector3(x, 0, 0);
902 m[1] = btVector3(0, x, 0);
903 m[2] = btVector3(0, 0, x);
904 return (m);
905}
906
907 static inline btMatrix3x3 Diagonal(const btVector3& v)
908{
909 btMatrix3x3 m;
910 m[0] = btVector3(v.getX(), 0, 0);
911 m[1] = btVector3(0, v.getY(), 0);
912 m[2] = btVector3(0, 0, v.getZ());
913 return (m);
914}
915
916 static inline btScalar Dot(const btScalar* a, const btScalar* b, int ndof)
917{
918 btScalar result = 0;
919 for (int i = 0; i < ndof; ++i)
920 result += a[i] * b[i];
921 return result;
922}
923
924 static inline btMatrix3x3 OuterProduct(const btScalar* v1, const btScalar* v2, const btScalar* v3,
925 const btScalar* u1, const btScalar* u2, const btScalar* u3, int ndof)
926{
927 btMatrix3x3 m;
928 btScalar a11 = Dot(v1, u1, ndof);
929 btScalar a12 = Dot(v1, u2, ndof);
930 btScalar a13 = Dot(v1, u3, ndof);
931
932 btScalar a21 = Dot(v2, u1, ndof);
933 btScalar a22 = Dot(v2, u2, ndof);
934 btScalar a23 = Dot(v2, u3, ndof);
935
936 btScalar a31 = Dot(v3, u1, ndof);
937 btScalar a32 = Dot(v3, u2, ndof);
938 btScalar a33 = Dot(v3, u3, ndof);
939 m[0] = btVector3(a11, a12, a13);
940 m[1] = btVector3(a21, a22, a23);
941 m[2] = btVector3(a31, a32, a33);
942 return (m);
943}
944
945 static inline btMatrix3x3 OuterProduct(const btVector3& v1, const btVector3& v2)
946{
947 btMatrix3x3 m;
948 btScalar a11 = v1[0] * v2[0];
949 btScalar a12 = v1[0] * v2[1];
950 btScalar a13 = v1[0] * v2[2];
951
952 btScalar a21 = v1[1] * v2[0];
953 btScalar a22 = v1[1] * v2[1];
954 btScalar a23 = v1[1] * v2[2];
955
956 btScalar a31 = v1[2] * v2[0];
957 btScalar a32 = v1[2] * v2[1];
958 btScalar a33 = v1[2] * v2[2];
959 m[0] = btVector3(a11, a12, a13);
960 m[1] = btVector3(a21, a22, a23);
961 m[2] = btVector3(a31, a32, a33);
962 return (m);
963}
964
965//
966 static inline btMatrix3x3 Add(const btMatrix3x3& a,
967 const btMatrix3x3& b)
968{
969 btMatrix3x3 r;
970 for (int i = 0; i < 3; ++i) r[i] = a[i] + b[i];
971 return (r);
972}
973//
974 static inline btMatrix3x3 Sub(const btMatrix3x3& a,
975 const btMatrix3x3& b)
976{
977 btMatrix3x3 r;
978 for (int i = 0; i < 3; ++i) r[i] = a[i] - b[i];
979 return (r);
980}
981//
982 static inline btMatrix3x3 Mul(const btMatrix3x3& a,
983 btScalar b)
984{
985 btMatrix3x3 r;
986 for (int i = 0; i < 3; ++i) r[i] = a[i] * b;
987 return (r);
988}
989//
990 static inline void Orthogonalize(btMatrix3x3& m)
991{
992 m[2] = btCross(m[0], m[1]).normalized();
993 m[1] = btCross(m[2], m[0]).normalized();
994 m[0] = btCross(m[1], m[2]).normalized();
995}
996//
997 static inline btMatrix3x3 MassMatrix(btScalar im, const btMatrix3x3& iwi, const btVector3& r)
998{
999 const btMatrix3x3 cr = Cross(r);
1000 return (Sub(Diagonal(im), cr * iwi * cr));
1001}
1002
1003//
1004 static inline btMatrix3x3 ImpulseMatrix(btScalar dt,
1005 btScalar ima,
1006 btScalar imb,
1007 const btMatrix3x3& iwi,
1008 const btVector3& r)
1009{
1010 return (Diagonal(1 / dt) * Add(Diagonal(ima), MassMatrix(imb, iwi, r)).inverse());
1011}
1012
1013//
1014 static inline btMatrix3x3 ImpulseMatrix(btScalar dt,
1015 const btMatrix3x3& effective_mass_inv,
1016 btScalar imb,
1017 const btMatrix3x3& iwi,
1018 const btVector3& r)
1019{
1020 return (Diagonal(1 / dt) * Add(effective_mass_inv, MassMatrix(imb, iwi, r)).inverse());
1021 // btMatrix3x3 iimb = MassMatrix(imb, iwi, r);
1022 // if (iimb.determinant() == 0)
1023 // return effective_mass_inv.inverse();
1024 // return effective_mass_inv.inverse() * Add(effective_mass_inv.inverse(), iimb.inverse()).inverse() * iimb.inverse();
1025}
1026
1027//
1028 static inline btMatrix3x3 ImpulseMatrix(btScalar ima, const btMatrix3x3& iia, const btVector3& ra,
1029 btScalar imb, const btMatrix3x3& iib, const btVector3& rb)
1030{
1031 return (Add(MassMatrix(ima, iia, ra), MassMatrix(imb, iib, rb)).inverse());
1032}
1033
1034//
1035 static inline btMatrix3x3 AngularImpulseMatrix(const btMatrix3x3& iia,
1036 const btMatrix3x3& iib)
1037{
1038 return (Add(iia, iib).inverse());
1039}
1040
1041//
1042 static inline btVector3 ProjectOnAxis(const btVector3& v,
1043 const btVector3& a)
1044{
1045 return (a * btDot(v, a));
1046}
1047//
1048 static inline btVector3 ProjectOnPlane(const btVector3& v,
1049 const btVector3& a)
1050{
1051 return (v - ProjectOnAxis(v, a));
1052}
1053
1054//
1055 static inline void ProjectOrigin(const btVector3& a,
1056 const btVector3& b,
1057 btVector3& prj,
1058 btScalar& sqd)
1059{
1060 const btVector3 d = b - a;
1061 const btScalar m2 = d.length2();
1062 if (m2 > SIMD_EPSILON)
1063 {
1064 const btScalar t = Clamp<btScalar>(-btDot(a, d) / m2, 0, 1);
1065 const btVector3 p = a + d * t;
1066 const btScalar l2 = p.length2();
1067 if (l2 < sqd)
1068 {
1069 prj = p;
1070 sqd = l2;
1071 }
1072 }
1073}
1074//
1075 static inline void ProjectOrigin(const btVector3& a,
1076 const btVector3& b,
1077 const btVector3& c,
1078 btVector3& prj,
1079 btScalar& sqd)
1080{
1081 const btVector3& q = btCross(b - a, c - a);
1082 const btScalar m2 = q.length2();
1083 if (m2 > SIMD_EPSILON)
1084 {
1085 const btVector3 n = q / btSqrt(m2);
1086 const btScalar k = btDot(a, n);
1087 const btScalar k2 = k * k;
1088 if (k2 < sqd)
1089 {
1090 const btVector3 p = n * k;
1091 if ((btDot(btCross(a - p, b - p), q) > 0) &&
1092 (btDot(btCross(b - p, c - p), q) > 0) &&
1093 (btDot(btCross(c - p, a - p), q) > 0))
1094 {
1095 prj = p;
1096 sqd = k2;
1097 }
1098 else
1099 {
1100 ProjectOrigin(a, b, prj, sqd);
1101 ProjectOrigin(b, c, prj, sqd);
1102 ProjectOrigin(c, a, prj, sqd);
1103 }
1104 }
1105 }
1106}
1107
1108//
1109 static inline bool rayIntersectsTriangle(const btVector3& origin, const btVector3& dir, const btVector3& v0, const btVector3& v1, const btVector3& v2, btScalar& t)
1110{
1111 btScalar a, f, u, v;
1112
1113 btVector3 e1 = v1 - v0;
1114 btVector3 e2 = v2 - v0;
1115 btVector3 h = dir.cross(e2);
1116 a = e1.dot(h);
1117
1118 if (a > -0.00001 && a < 0.00001)
1119 return (false);
1120
1121 f = btScalar(1) / a;
1122 btVector3 s = origin - v0;
1123 u = f * s.dot(h);
1124
1125 if (u < 0.0 || u > 1.0)
1126 return (false);
1127
1128 btVector3 q = s.cross(e1);
1129 v = f * dir.dot(q);
1130 if (v < 0.0 || u + v > 1.0)
1131 return (false);
1132 // at this stage we can compute t to find out where
1133 // the intersection point is on the line
1134 t = f * e2.dot(q);
1135 if (t > 0) // ray intersection
1136 return (true);
1137 else // this means that there is a line intersection
1138 // but not a ray intersection
1139 return (false);
1140}
1141
1142 static inline bool lineIntersectsTriangle(const btVector3& rayStart, const btVector3& rayEnd, const btVector3& p1, const btVector3& p2, const btVector3& p3, btVector3& sect, btVector3& normal)
1143{
1144 btVector3 dir = rayEnd - rayStart;
1145 btScalar dir_norm = dir.norm();
1146 if (dir_norm < SIMD_EPSILON)
1147 return false;
1148 dir.normalize();
1149 btScalar t;
1150 bool ret = rayIntersectsTriangle(rayStart, dir, p1, p2, p3, t);
1151
1152 if (ret)
1153 {
1154 if (t <= dir_norm)
1155 {
1156 sect = rayStart + dir * t;
1157 }
1158 else
1159 {
1160 ret = false;
1161 }
1162 }
1163
1164 if (ret)
1165 {
1166 btVector3 n = (p3 - p1).cross(p2 - p1);
1167 n.safeNormalize();
1168 if (n.dot(dir) < 0)
1169 normal = n;
1170 else
1171 normal = -n;
1172 }
1173 return ret;
1174}
1175
1176//
1177template <typename T>
1178 static inline T BaryEval(const T& a,
1179 const T& b,
1180 const T& c,
1181 const btVector3& coord)
1182{
1183 return (a * coord.x() + b * coord.y() + c * coord.z());
1184}
1185//
1186 static inline btVector3 BaryCoord(const btVector3& a,
1187 const btVector3& b,
1188 const btVector3& c,
1189 const btVector3& p)
1190{
1191 const btScalar w[] = {btCross(a - p, b - p).length(),
1192 btCross(b - p, c - p).length(),
1193 btCross(c - p, a - p).length()};
1194 const btScalar isum = 1 / (w[0] + w[1] + w[2]);
1195 return (btVector3(w[1] * isum, w[2] * isum, w[0] * isum));
1196}
1197
1198//
1199 inline static btScalar ImplicitSolve(btSoftBody::ImplicitFn* fn,
1200 const btVector3& a,
1201 const btVector3& b,
1202 const btScalar accuracy,
1203 const int maxiterations = 256)
1204{
1205 btScalar span[2] = {0, 1};
1206 btScalar values[2] = {fn->Eval(a), fn->Eval(b)};
1207 if (values[0] > values[1])
1208 {
1209 btSwap(span[0], span[1]);
1210 btSwap(values[0], values[1]);
1211 }
1212 if (values[0] > -accuracy) return (-1);
1213 if (values[1] < +accuracy) return (-1);
1214 for (int i = 0; i < maxiterations; ++i)
1215 {
1216 const btScalar t = Lerp(span[0], span[1], values[0] / (values[0] - values[1]));
1217 const btScalar v = fn->Eval(Lerp(a, b, t));
1218 if ((t <= 0) || (t >= 1)) break;
1219 if (btFabs(v) < accuracy) return (t);
1220 if (v < 0)
1221 {
1222 span[0] = t;
1223 values[0] = v;
1224 }
1225 else
1226 {
1227 span[1] = t;
1228 values[1] = v;
1229 }
1230 }
1231 return (-1);
1232}
1233
1234 inline static void EvaluateMedium(const btSoftBodyWorldInfo* wfi,
1235 const btVector3& x,
1236 btSoftBody::sMedium& medium)
1237{
1238 medium.m_velocity = btVector3(0, 0, 0);
1239 medium.m_pressure = 0;
1240 medium.m_density = wfi->air_density;
1241 if (wfi->water_density > 0)
1242 {
1243 const btScalar depth = -(btDot(x, wfi->water_normal) + wfi->water_offset);
1244 if (depth > 0)
1245 {
1246 medium.m_density = wfi->water_density;
1247 medium.m_pressure = depth * wfi->water_density * wfi->m_gravity.length();
1248 }
1249 }
1250}
1251
1252//
1253 static inline btVector3 NormalizeAny(const btVector3& v)
1254{
1255 const btScalar l = v.length();
1256 if (l > SIMD_EPSILON)
1257 return (v / l);
1258 else
1259 return (btVector3(0, 0, 0));
1260}
1261
1262//
1263 static inline btDbvtVolume VolumeOf(const btSoftBody::Face& f,
1264 btScalar margin)
1265{
1266 const btVector3* pts[] = {&f.m_n[0]->m_x,
1267 &f.m_n[1]->m_x,
1268 &f.m_n[2]->m_x};
1269 btDbvtVolume vol = btDbvtVolume::FromPoints(pts, 3);
1270 vol.Expand(btVector3(margin, margin, margin));
1271 return (vol);
1272}
1273
1274//
1275 static inline btVector3 CenterOf(const btSoftBody::Face& f)
1276{
1277 return ((f.m_n[0]->m_x + f.m_n[1]->m_x + f.m_n[2]->m_x) / 3);
1278}
1279
1280//
1281 static inline btScalar AreaOf(const btVector3& x0,
1282 const btVector3& x1,
1283 const btVector3& x2)
1284{
1285 const btVector3 a = x1 - x0;
1286 const btVector3 b = x2 - x0;
1287 const btVector3 cr = btCross(a, b);
1288 const btScalar area = cr.length();
1289 return (area);
1290}
1291
1292//
1293 static inline btScalar VolumeOf(const btVector3& x0,
1294 const btVector3& x1,
1295 const btVector3& x2,
1296 const btVector3& x3)
1297{
1298 const btVector3 a = x1 - x0;
1299 const btVector3 b = x2 - x0;
1300 const btVector3 c = x3 - x0;
1301 return (btDot(a, btCross(b, c)));
1302}
1303
1304//
1305
1306//
1307 static inline void ApplyClampedForce(btSoftBody::Node& n,
1308 const btVector3& f,
1309 btScalar dt)
1310{
1311 const btScalar dtim = dt * n.m_im;
1312 if ((f * dtim).length2() > n.m_v.length2())
1313 { /* Clamp */
1314 n.m_f -= ProjectOnAxis(n.m_v, f.normalized()) / dtim;
1315 }
1316 else
1317 { /* Apply */
1318 n.m_f += f;
1319 }
1320}
1321
1322//
1323 static inline int MatchEdge(const btSoftBody::Node* a,
1324 const btSoftBody::Node* b,
1325 const btSoftBody::Node* ma,
1326 const btSoftBody::Node* mb)
1327{
1328 if ((a == ma) && (b == mb)) return (0);
1329 if ((a == mb) && (b == ma)) return (1);
1330 return (-1);
1331}
1332
1333//
1334// btEigen : Extract eigen system,
1335// straitforward implementation of http://math.fullerton.edu/mathews/n2003/JacobiMethodMod.html
1336// outputs are NOT sorted.
1337//
1338 struct btEigen
1339{
1340 static int system(btMatrix3x3& a, btMatrix3x3* vectors, btVector3* values = 0)
1341 {
1342 static const int maxiterations = 16;
1343 static const btScalar accuracy = (btScalar)0.0001;
1344 btMatrix3x3& v = *vectors;
1345 int iterations = 0;
1346 vectors->setIdentity();
1347 do
1348 {
1349 int p = 0, q = 1;
1350 if (btFabs(a[p][q]) < btFabs(a[0][2]))
1351 {
1352 p = 0;
1353 q = 2;
1354 }
1355 if (btFabs(a[p][q]) < btFabs(a[1][2]))
1356 {
1357 p = 1;
1358 q = 2;
1359 }
1360 if (btFabs(a[p][q]) > accuracy)
1361 {
1362 const btScalar w = (a[q][q] - a[p][p]) / (2 * a[p][q]);
1363 const btScalar z = btFabs(w);
1364 const btScalar t = w / (z * (btSqrt(1 + w * w) + z));
1365 if (t == t) /* [WARNING] let hope that one does not get thrown aways by some compilers... */
1366 {
1367 const btScalar c = 1 / btSqrt(t * t + 1);
1368 const btScalar s = c * t;
1369 mulPQ(a, c, s, p, q);
1370 mulTPQ(a, c, s, p, q);
1371 mulPQ(v, c, s, p, q);
1372 }
1373 else
1374 break;
1375 }
1376 else
1377 break;
1378 } while ((++iterations) < maxiterations);
1379 if (values)
1380 {
1381 *values = btVector3(a[0][0], a[1][1], a[2][2]);
1382 }
1383 return (iterations);
1384 }
1385
1386private:
1387 static inline void mulTPQ(btMatrix3x3& a, btScalar c, btScalar s, int p, int q)
1388 {
1389 const btScalar m[2][3] = {{a[p][0], a[p][1], a[p][2]},
1390 {a[q][0], a[q][1], a[q][2]}};
1391 int i;
1392
1393 for (i = 0; i < 3; ++i) a[p][i] = c * m[0][i] - s * m[1][i];
1394 for (i = 0; i < 3; ++i) a[q][i] = c * m[1][i] + s * m[0][i];
1395 }
1396 static inline void mulPQ(btMatrix3x3& a, btScalar c, btScalar s, int p, int q)
1397 {
1398 const btScalar m[2][3] = {{a[0][p], a[1][p], a[2][p]},
1399 {a[0][q], a[1][q], a[2][q]}};
1400 int i;
1401
1402 for (i = 0; i < 3; ++i) a[i][p] = c * m[0][i] - s * m[1][i];
1403 for (i = 0; i < 3; ++i) a[i][q] = c * m[1][i] + s * m[0][i];
1404 }
1405};
1406
1407//
1408// Polar decomposition,
1409// "Computing the Polar Decomposition with Applications", Nicholas J. Higham, 1986.
1410//
1411 static inline int PolarDecompose(const btMatrix3x3& m, btMatrix3x3& q, btMatrix3x3& s)
1412{
1413 static const btPolarDecomposition polar;
1414 return polar.decompose(m, q, s);
1415}
1416
1417//
1418// btSoftColliders
1419//
1420 struct btSoftColliders
1421{
1422 //
1423 // ClusterBase
1424 //
1425 struct ClusterBase : btDbvt::ICollide
1426 {
1427 btScalar erp;
1428 btScalar idt;
1429 btScalar m_margin;
1430 btScalar friction;
1431 btScalar threshold;
1432 ClusterBase()
1433 {
1434 erp = (btScalar)1;
1435 idt = 0;
1436 m_margin = 0;
1437 friction = 0;
1438 threshold = (btScalar)0;
1439 }
1440 bool SolveContact(const btGjkEpaSolver2::sResults& res,
1441 btSoftBody::Body ba, const btSoftBody::Body bb,
1442 btSoftBody::CJoint& joint)
1443 {
1444 if (res.distance < m_margin)
1445 {
1446 btVector3 norm = res.normal;
1447 norm.normalize(); //is it necessary?
1448
1449 const btVector3 ra = res.witnesses[0] - ba.xform().getOrigin();
1450 const btVector3 rb = res.witnesses[1] - bb.xform().getOrigin();
1451 const btVector3 va = ba.velocity(ra);
1452 const btVector3 vb = bb.velocity(rb);
1453 const btVector3 vrel = va - vb;
1454 const btScalar rvac = btDot(vrel, norm);
1455 btScalar depth = res.distance - m_margin;
1456
1457 // printf("depth=%f\n",depth);
1458 const btVector3 iv = norm * rvac;
1459 const btVector3 fv = vrel - iv;
1460 joint.m_bodies[0] = ba;
1461 joint.m_bodies[1] = bb;
1462 joint.m_refs[0] = ra * ba.xform().getBasis();
1463 joint.m_refs[1] = rb * bb.xform().getBasis();
1464 joint.m_rpos[0] = ra;
1465 joint.m_rpos[1] = rb;
1466 joint.m_cfm = 1;
1467 joint.m_erp = 1;
1468 joint.m_life = 0;
1469 joint.m_maxlife = 0;
1470 joint.m_split = 1;
1471
1472 joint.m_drift = depth * norm;
1473
1474 joint.m_normal = norm;
1475 // printf("normal=%f,%f,%f\n",res.normal.getX(),res.normal.getY(),res.normal.getZ());
1476 joint.m_delete = false;
1477 joint.m_friction = fv.length2() < (rvac * friction * rvac * friction) ? 1 : friction;
1478 joint.m_massmatrix = ImpulseMatrix(ba.invMass(), ba.invWorldInertia(), joint.m_rpos[0],
1479 bb.invMass(), bb.invWorldInertia(), joint.m_rpos[1]);
1480
1481 return (true);
1482 }
1483 return (false);
1484 }
1485 };
1486 //
1487 // CollideCL_RS
1488 //
1489 struct CollideCL_RS : ClusterBase
1490 {
1491 btSoftBody* psb;
1492 const btCollisionObjectWrapper* m_colObjWrap;
1493
1494 void Process(const btDbvtNode* leaf)
1495 {
1496 btSoftBody::Cluster* cluster = (btSoftBody::Cluster*)leaf->data;
1497 btSoftClusterCollisionShape cshape(cluster);
1498
1499 const btConvexShape* rshape = (const btConvexShape*)m_colObjWrap->getCollisionShape();
1500
1502 if (m_colObjWrap->getCollisionObject()->isStaticOrKinematicObject() && cluster->m_containsAnchor)
1503 return;
1504
1505 btGjkEpaSolver2::sResults res;
1506 if (btGjkEpaSolver2::SignedDistance(&cshape, btTransform::getIdentity(),
1507 rshape, m_colObjWrap->getWorldTransform(),
1508 btVector3(1, 0, 0), res))
1509 {
1510 btSoftBody::CJoint joint;
1511 if (SolveContact(res, cluster, m_colObjWrap->getCollisionObject(), joint)) //prb,joint))
1512 {
1513 btSoftBody::CJoint* pj = new (btAlignedAlloc(sizeof(btSoftBody::CJoint), 16)) btSoftBody::CJoint();
1514 *pj = joint;
1515 psb->m_joints.push_back(pj);
1516 if (m_colObjWrap->getCollisionObject()->isStaticOrKinematicObject())
1517 {
1518 pj->m_erp *= psb->m_cfg.kSKHR_CL;
1519 pj->m_split *= psb->m_cfg.kSK_SPLT_CL;
1520 }
1521 else
1522 {
1523 pj->m_erp *= psb->m_cfg.kSRHR_CL;
1524 pj->m_split *= psb->m_cfg.kSR_SPLT_CL;
1525 }
1526 }
1527 }
1528 }
1529 void ProcessColObj(btSoftBody* ps, const btCollisionObjectWrapper* colObWrap)
1530 {
1531 psb = ps;
1532 m_colObjWrap = colObWrap;
1533 idt = ps->m_sst.isdt;
1534 m_margin = m_colObjWrap->getCollisionShape()->getMargin() + psb->getCollisionShape()->getMargin();
1536 friction = btMin(psb->m_cfg.kDF, m_colObjWrap->getCollisionObject()->getFriction());
1537 btVector3 mins;
1538 btVector3 maxs;
1539
1540 ATTRIBUTE_ALIGNED16(btDbvtVolume)
1541 volume;
1542 colObWrap->getCollisionShape()->getAabb(colObWrap->getWorldTransform(), mins, maxs);
1543 volume = btDbvtVolume::FromMM(mins, maxs);
1544 volume.Expand(btVector3(1, 1, 1) * m_margin);
1545 ps->m_cdbvt.collideTV(ps->m_cdbvt.m_root, volume, *this);
1546 }
1547 };
1548 //
1549 // CollideCL_SS
1550 //
1551 struct CollideCL_SS : ClusterBase
1552 {
1553 btSoftBody* bodies[2];
1554 void Process(const btDbvtNode* la, const btDbvtNode* lb)
1555 {
1556 btSoftBody::Cluster* cla = (btSoftBody::Cluster*)la->data;
1557 btSoftBody::Cluster* clb = (btSoftBody::Cluster*)lb->data;
1558
1559 bool connected = false;
1560 if ((bodies[0] == bodies[1]) && (bodies[0]->m_clusterConnectivity.size()))
1561 {
1562 connected = bodies[0]->m_clusterConnectivity[cla->m_clusterIndex + bodies[0]->m_clusters.size() * clb->m_clusterIndex];
1563 }
1564
1565 if (!connected)
1566 {
1567 btSoftClusterCollisionShape csa(cla);
1568 btSoftClusterCollisionShape csb(clb);
1569 btGjkEpaSolver2::sResults res;
1570 if (btGjkEpaSolver2::SignedDistance(&csa, btTransform::getIdentity(),
1571 &csb, btTransform::getIdentity(),
1572 cla->m_com - clb->m_com, res))
1573 {
1574 btSoftBody::CJoint joint;
1575 if (SolveContact(res, cla, clb, joint))
1576 {
1577 btSoftBody::CJoint* pj = new (btAlignedAlloc(sizeof(btSoftBody::CJoint), 16)) btSoftBody::CJoint();
1578 *pj = joint;
1579 bodies[0]->m_joints.push_back(pj);
1580 pj->m_erp *= btMax(bodies[0]->m_cfg.kSSHR_CL, bodies[1]->m_cfg.kSSHR_CL);
1581 pj->m_split *= (bodies[0]->m_cfg.kSS_SPLT_CL + bodies[1]->m_cfg.kSS_SPLT_CL) / 2;
1582 }
1583 }
1584 }
1585 else
1586 {
1587 static int count = 0;
1588 count++;
1589 //printf("count=%d\n",count);
1590 }
1591 }
1592 void ProcessSoftSoft(btSoftBody* psa, btSoftBody* psb)
1593 {
1594 idt = psa->m_sst.isdt;
1595 //m_margin = (psa->getCollisionShape()->getMargin()+psb->getCollisionShape()->getMargin())/2;
1596 m_margin = (psa->getCollisionShape()->getMargin() + psb->getCollisionShape()->getMargin());
1597 friction = btMin(psa->m_cfg.kDF, psb->m_cfg.kDF);
1598 bodies[0] = psa;
1599 bodies[1] = psb;
1600 psa->m_cdbvt.collideTT(psa->m_cdbvt.m_root, psb->m_cdbvt.m_root, *this);
1601 }
1602 };
1603 //
1604 // CollideSDF_RS
1605 //
1606 struct CollideSDF_RS : btDbvt::ICollide
1607 {
1608 void Process(const btDbvtNode* leaf)
1609 {
1610 btSoftBody::Node* node = (btSoftBody::Node*)leaf->data;
1611 DoNode(*node);
1612 }
1613 void DoNode(btSoftBody::Node& n) const
1614 {
1615 const btScalar m = n.m_im > 0 ? dynmargin : stamargin;
1616 btSoftBody::RContact c;
1617
1618 if ((!n.m_battach) &&
1619 psb->checkContact(m_colObj1Wrap, n.m_x, m, c.m_cti))
1620 {
1621 const btScalar ima = n.m_im;
1622 const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f;
1623 const btScalar ms = ima + imb;
1624 if (ms > 0)
1625 {
1626 const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform();
1627 static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0);
1628 const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic;
1629 const btVector3 ra = n.m_x - wtr.getOrigin();
1630 const btVector3 va = m_rigidBody ? m_rigidBody->getVelocityInLocalPoint(ra) * psb->m_sst.sdt : btVector3(0, 0, 0);
1631 const btVector3 vb = n.m_x - n.m_q;
1632 const btVector3 vr = vb - va;
1633 const btScalar dn = btDot(vr, c.m_cti.m_normal);
1634 const btVector3 fv = vr - c.m_cti.m_normal * dn;
1635 const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction();
1636 c.m_node = &n;
1637 c.m_c0 = ImpulseMatrix(psb->m_sst.sdt, ima, imb, iwi, ra);
1638 c.m_c1 = ra;
1639 c.m_c2 = ima * psb->m_sst.sdt;
1640 c.m_c3 = fv.length2() < (dn * fc * dn * fc) ? 0 : 1 - fc;
1641 c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR;
1642 psb->m_rcontacts.push_back(c);
1643 if (m_rigidBody)
1644 m_rigidBody->activate();
1645 }
1646 }
1647 }
1648 btSoftBody* psb;
1649 const btCollisionObjectWrapper* m_colObj1Wrap;
1650 btRigidBody* m_rigidBody;
1651 btScalar dynmargin;
1652 btScalar stamargin;
1653 };
1654
1655 //
1656 // CollideSDF_RD
1657 //
1658 struct CollideSDF_RD : btDbvt::ICollide
1659 {
1660 void Process(const btDbvtNode* leaf)
1661 {
1662 btSoftBody::Node* node = (btSoftBody::Node*)leaf->data;
1663 DoNode(*node);
1664 }
1665 void DoNode(btSoftBody::Node& n) const
1666 {
1667 const btScalar m = n.m_im > 0 ? dynmargin : stamargin;
1668 btSoftBody::DeformableNodeRigidContact c;
1669
1670 if (!n.m_battach)
1671 {
1672 // check for collision at x_{n+1}^*
1673 if (psb->checkDeformableContact(m_colObj1Wrap, n.m_q, m, c.m_cti, /*predict = */ true))
1674 {
1675 const btScalar ima = n.m_im;
1676 // todo: collision between multibody and fixed deformable node will be missed.
1677 const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f;
1678 const btScalar ms = ima + imb;
1679 if (ms > 0)
1680 {
1681 // resolve contact at x_n
1682 psb->checkDeformableContact(m_colObj1Wrap, n.m_x, m, c.m_cti, /*predict = */ false);
1683 btSoftBody::sCti& cti = c.m_cti;
1684 c.m_node = &n;
1685 const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction();
1686 c.m_c2 = ima;
1687 c.m_c3 = fc;
1688 c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR;
1689 c.m_c5 = n.m_effectiveMass_inv;
1690
1691 if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
1692 {
1693 const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform();
1694 const btVector3 ra = n.m_x - wtr.getOrigin();
1695
1696 static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0);
1697 const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic;
1698 if (psb->m_reducedModel)
1699 {
1700 c.m_c0 = MassMatrix(imb, iwi, ra); //impulse factor K of the rigid body only (not the inverse)
1701 }
1702 else
1703 {
1704 c.m_c0 = ImpulseMatrix(1, n.m_effectiveMass_inv, imb, iwi, ra);
1705 // c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra);
1706 }
1707 c.m_c1 = ra;
1708 }
1709 else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
1710 {
1711 btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
1712 if (multibodyLinkCol)
1713 {
1714 btVector3 normal = cti.m_normal;
1715 btVector3 t1 = generateUnitOrthogonalVector(normal);
1716 btVector3 t2 = btCross(normal, t1);
1717 btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2;
1718 findJacobian(multibodyLinkCol, jacobianData_normal, c.m_node->m_x, normal);
1719 findJacobian(multibodyLinkCol, jacobianData_t1, c.m_node->m_x, t1);
1720 findJacobian(multibodyLinkCol, jacobianData_t2, c.m_node->m_x, t2);
1721
1722 btScalar* J_n = &jacobianData_normal.m_jacobians[0];
1723 btScalar* J_t1 = &jacobianData_t1.m_jacobians[0];
1724 btScalar* J_t2 = &jacobianData_t2.m_jacobians[0];
1725
1726 btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
1727 btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
1728 btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
1729
1730 btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(),
1731 t1.getX(), t1.getY(), t1.getZ(),
1732 t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame
1733 const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
1734
1735 btMatrix3x3 local_impulse_matrix;
1736 if (psb->m_reducedModel)
1737 {
1738 local_impulse_matrix = OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof);
1739 }
1740 else
1741 {
1742 local_impulse_matrix = (n.m_effectiveMass_inv + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse();
1743 }
1744 c.m_c0 = rot.transpose() * local_impulse_matrix * rot;
1745 c.jacobianData_normal = jacobianData_normal;
1746 c.jacobianData_t1 = jacobianData_t1;
1747 c.jacobianData_t2 = jacobianData_t2;
1748 c.t1 = t1;
1749 c.t2 = t2;
1750 }
1751 }
1752 psb->m_nodeRigidContacts.push_back(c);
1753 }
1754 }
1755 }
1756 }
1757 btSoftBody* psb;
1758 const btCollisionObjectWrapper* m_colObj1Wrap;
1759 btRigidBody* m_rigidBody;
1760 btScalar dynmargin;
1761 btScalar stamargin;
1762 };
1763
1764 //
1765 // CollideSDF_RDF
1766 //
1767 struct CollideSDF_RDF : btDbvt::ICollide
1768 {
1769 void Process(const btDbvtNode* leaf)
1770 {
1771 btSoftBody::Face* face = (btSoftBody::Face*)leaf->data;
1772 DoNode(*face);
1773 }
1774 void DoNode(btSoftBody::Face& f) const
1775 {
1776 btSoftBody::Node* n0 = f.m_n[0];
1777 btSoftBody::Node* n1 = f.m_n[1];
1778 btSoftBody::Node* n2 = f.m_n[2];
1779 const btScalar m = (n0->m_im > 0 && n1->m_im > 0 && n2->m_im > 0) ? dynmargin : stamargin;
1780 btSoftBody::DeformableFaceRigidContact c;
1781 btVector3 contact_point;
1782 btVector3 bary;
1783 if (psb->checkDeformableFaceContact(m_colObj1Wrap, f, contact_point, bary, m, c.m_cti, true))
1784 {
1785 btScalar ima = n0->m_im + n1->m_im + n2->m_im;
1786 const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f;
1787 // todo: collision between multibody and fixed deformable face will be missed.
1788 const btScalar ms = ima + imb;
1789 if (ms > 0)
1790 {
1791 // resolve contact at x_n
1792 // psb->checkDeformableFaceContact(m_colObj1Wrap, f, contact_point, bary, m, c.m_cti, /*predict = */ false);
1793 btSoftBody::sCti& cti = c.m_cti;
1794 c.m_contactPoint = contact_point;
1795 c.m_bary = bary;
1796 // todo xuchenhan@: this is assuming mass of all vertices are the same. Need to modify if mass are different for distinct vertices
1797 c.m_weights = btScalar(2) / (btScalar(1) + bary.length2()) * bary;
1798 c.m_face = &f;
1799 // friction is handled by the nodes to prevent sticking
1800 // const btScalar fc = 0;
1801 const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction();
1802
1803 // the effective inverse mass of the face as in https://graphics.stanford.edu/papers/cloth-sig02/cloth.pdf
1804 ima = bary.getX() * c.m_weights.getX() * n0->m_im + bary.getY() * c.m_weights.getY() * n1->m_im + bary.getZ() * c.m_weights.getZ() * n2->m_im;
1805 c.m_c2 = ima;
1806 c.m_c3 = fc;
1807 c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR;
1808 c.m_c5 = Diagonal(ima);
1809 if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
1810 {
1811 const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform();
1812 static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0);
1813 const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic;
1814 const btVector3 ra = contact_point - wtr.getOrigin();
1815
1816 // we do not scale the impulse matrix by dt
1817 c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra);
1818 c.m_c1 = ra;
1819 }
1820 else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
1821 {
1822 btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
1823 if (multibodyLinkCol)
1824 {
1825 btVector3 normal = cti.m_normal;
1826 btVector3 t1 = generateUnitOrthogonalVector(normal);
1827 btVector3 t2 = btCross(normal, t1);
1828 btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2;
1829 findJacobian(multibodyLinkCol, jacobianData_normal, contact_point, normal);
1830 findJacobian(multibodyLinkCol, jacobianData_t1, contact_point, t1);
1831 findJacobian(multibodyLinkCol, jacobianData_t2, contact_point, t2);
1832
1833 btScalar* J_n = &jacobianData_normal.m_jacobians[0];
1834 btScalar* J_t1 = &jacobianData_t1.m_jacobians[0];
1835 btScalar* J_t2 = &jacobianData_t2.m_jacobians[0];
1836
1837 btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
1838 btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
1839 btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
1840
1841 btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(),
1842 t1.getX(), t1.getY(), t1.getZ(),
1843 t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame
1844 const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
1845 btMatrix3x3 local_impulse_matrix = (Diagonal(ima) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse();
1846 c.m_c0 = rot.transpose() * local_impulse_matrix * rot;
1847 c.jacobianData_normal = jacobianData_normal;
1848 c.jacobianData_t1 = jacobianData_t1;
1849 c.jacobianData_t2 = jacobianData_t2;
1850 c.t1 = t1;
1851 c.t2 = t2;
1852 }
1853 }
1854 psb->m_faceRigidContacts.push_back(c);
1855 }
1856 }
1857 // Set caching barycenters to be false after collision detection.
1858 // Only turn on when contact is static.
1859 f.m_pcontact[3] = 0;
1860 }
1861 btSoftBody* psb;
1862 const btCollisionObjectWrapper* m_colObj1Wrap;
1863 btRigidBody* m_rigidBody;
1864 btScalar dynmargin;
1865 btScalar stamargin;
1866 };
1867
1868 //
1869 // CollideVF_SS
1870 //
1871 struct CollideVF_SS : btDbvt::ICollide
1872 {
1873 void Process(const btDbvtNode* lnode,
1874 const btDbvtNode* lface)
1875 {
1876 btSoftBody::Node* node = (btSoftBody::Node*)lnode->data;
1877 btSoftBody::Face* face = (btSoftBody::Face*)lface->data;
1878 for (int i = 0; i < 3; ++i)
1879 {
1880 if (face->m_n[i] == node)
1881 continue;
1882 }
1883
1884 btVector3 o = node->m_x;
1885 btVector3 p;
1886 btScalar d = SIMD_INFINITY;
1887 ProjectOrigin(face->m_n[0]->m_x - o,
1888 face->m_n[1]->m_x - o,
1889 face->m_n[2]->m_x - o,
1890 p, d);
1891 const btScalar m = mrg + (o - node->m_q).length() * 2;
1892 if (d < (m * m))
1893 {
1894 const btSoftBody::Node* n[] = {face->m_n[0], face->m_n[1], face->m_n[2]};
1895 const btVector3 w = BaryCoord(n[0]->m_x, n[1]->m_x, n[2]->m_x, p + o);
1896 const btScalar ma = node->m_im;
1897 btScalar mb = BaryEval(n[0]->m_im, n[1]->m_im, n[2]->m_im, w);
1898 if ((n[0]->m_im <= 0) ||
1899 (n[1]->m_im <= 0) ||
1900 (n[2]->m_im <= 0))
1901 {
1902 mb = 0;
1903 }
1904 const btScalar ms = ma + mb;
1905 if (ms > 0)
1906 {
1907 btSoftBody::SContact c;
1908 c.m_normal = p / -btSqrt(d);
1909 c.m_margin = m;
1910 c.m_node = node;
1911 c.m_face = face;
1912 c.m_weights = w;
1913 c.m_friction = btMax(psb[0]->m_cfg.kDF, psb[1]->m_cfg.kDF);
1914 c.m_cfm[0] = ma / ms * psb[0]->m_cfg.kSHR;
1915 c.m_cfm[1] = mb / ms * psb[1]->m_cfg.kSHR;
1916 psb[0]->m_scontacts.push_back(c);
1917 }
1918 }
1919 }
1920 btSoftBody* psb[2];
1921 btScalar mrg;
1922 };
1923
1924 //
1925 // CollideVF_DD
1926 //
1927 struct CollideVF_DD : btDbvt::ICollide
1928 {
1929 void Process(const btDbvtNode* lnode,
1930 const btDbvtNode* lface)
1931 {
1932 btSoftBody::Node* node = (btSoftBody::Node*)lnode->data;
1933 btSoftBody::Face* face = (btSoftBody::Face*)lface->data;
1934 btVector3 bary;
1935 if (proximityTest(face->m_n[0]->m_x, face->m_n[1]->m_x, face->m_n[2]->m_x, node->m_x, face->m_normal, mrg, bary))
1936 {
1937 const btSoftBody::Node* n[] = {face->m_n[0], face->m_n[1], face->m_n[2]};
1938 const btVector3 w = bary;
1939 const btScalar ma = node->m_im;
1940 btScalar mb = BaryEval(n[0]->m_im, n[1]->m_im, n[2]->m_im, w);
1941 if ((n[0]->m_im <= 0) ||
1942 (n[1]->m_im <= 0) ||
1943 (n[2]->m_im <= 0))
1944 {
1945 mb = 0;
1946 }
1947 const btScalar ms = ma + mb;
1948 if (ms > 0)
1949 {
1950 btSoftBody::DeformableFaceNodeContact c;
1951 c.m_normal = face->m_normal;
1952 if (!useFaceNormal && c.m_normal.dot(node->m_x - face->m_n[2]->m_x) < 0)
1953 c.m_normal = -face->m_normal;
1954 c.m_margin = mrg;
1955 c.m_node = node;
1956 c.m_face = face;
1957 c.m_bary = w;
1958 c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF;
1959 // Initialize unused fields.
1960 c.m_weights = btVector3(0, 0, 0);
1961 c.m_imf = 0;
1962 c.m_c0 = 0;
1963 c.m_colObj = psb[1];
1964 psb[0]->m_faceNodeContacts.push_back(c);
1965 }
1966 }
1967 }
1968 btSoftBody* psb[2];
1969 btScalar mrg;
1970 bool useFaceNormal;
1971 };
1972
1973 //
1974 // CollideFF_DD
1975 //
1976 struct CollideFF_DD : btDbvt::ICollide
1977 {
1978 void Process(const btDbvntNode* lface1,
1979 const btDbvntNode* lface2)
1980 {
1981 btSoftBody::Face* f1 = (btSoftBody::Face*)lface1->data;
1982 btSoftBody::Face* f2 = (btSoftBody::Face*)lface2->data;
1983 if (f1 != f2)
1984 {
1985 Repel(f1, f2);
1986 Repel(f2, f1);
1987 }
1988 }
1989 void Repel(btSoftBody::Face* f1, btSoftBody::Face* f2)
1990 {
1991 //#define REPEL_NEIGHBOR 1
1992#ifndef REPEL_NEIGHBOR
1993 for (int node_id = 0; node_id < 3; ++node_id)
1994 {
1995 btSoftBody::Node* node = f1->m_n[node_id];
1996 for (int i = 0; i < 3; ++i)
1997 {
1998 if (f2->m_n[i] == node)
1999 return;
2000 }
2001 }
2002#endif
2003 bool skip = false;
2004 for (int node_id = 0; node_id < 3; ++node_id)
2005 {
2006 btSoftBody::Node* node = f1->m_n[node_id];
2007#ifdef REPEL_NEIGHBOR
2008 for (int i = 0; i < 3; ++i)
2009 {
2010 if (f2->m_n[i] == node)
2011 {
2012 skip = true;
2013 break;
2014 }
2015 }
2016 if (skip)
2017 {
2018 skip = false;
2019 continue;
2020 }
2021#endif
2022 btSoftBody::Face* face = f2;
2023 btVector3 bary;
2024 if (!proximityTest(face->m_n[0]->m_x, face->m_n[1]->m_x, face->m_n[2]->m_x, node->m_x, face->m_normal, mrg, bary))
2025 continue;
2026 btSoftBody::DeformableFaceNodeContact c;
2027 c.m_normal = face->m_normal;
2028 if (!useFaceNormal && c.m_normal.dot(node->m_x - face->m_n[2]->m_x) < 0)
2029 c.m_normal = -face->m_normal;
2030 c.m_margin = mrg;
2031 c.m_node = node;
2032 c.m_face = face;
2033 c.m_bary = bary;
2034 c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF;
2035 // Initialize unused fields.
2036 c.m_weights = btVector3(0, 0, 0);
2037 c.m_imf = 0;
2038 c.m_c0 = 0;
2039 c.m_colObj = psb[1];
2040 psb[0]->m_faceNodeContacts.push_back(c);
2041 }
2042 }
2043 btSoftBody* psb[2];
2044 btScalar mrg;
2045 bool useFaceNormal;
2046 };
2047
2048 struct CollideCCD : btDbvt::ICollide
2049 {
2050 void Process(const btDbvtNode* lnode,
2051 const btDbvtNode* lface)
2052 {
2053 btSoftBody::Node* node = (btSoftBody::Node*)lnode->data;
2054 btSoftBody::Face* face = (btSoftBody::Face*)lface->data;
2055 btVector3 bary;
2056 if (bernsteinCCD(face, node, dt, SAFE_EPSILON, bary))
2057 {
2058 btSoftBody::DeformableFaceNodeContact c;
2059 c.m_normal = face->m_normal;
2060 if (!useFaceNormal && c.m_normal.dot(node->m_x - face->m_n[2]->m_x) < 0)
2061 c.m_normal = -face->m_normal;
2062 c.m_node = node;
2063 c.m_face = face;
2064 c.m_bary = bary;
2065 c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF;
2066 // Initialize unused fields.
2067 c.m_weights = btVector3(0, 0, 0);
2068 c.m_margin = mrg;
2069 c.m_imf = 0;
2070 c.m_c0 = 0;
2071 c.m_colObj = psb[1];
2072 psb[0]->m_faceNodeContactsCCD.push_back(c);
2073 }
2074 }
2075 void Process(const btDbvntNode* lface1,
2076 const btDbvntNode* lface2)
2077 {
2078 btSoftBody::Face* f1 = (btSoftBody::Face*)lface1->data;
2079 btSoftBody::Face* f2 = (btSoftBody::Face*)lface2->data;
2080 if (f1 != f2)
2081 {
2082 Repel(f1, f2);
2083 Repel(f2, f1);
2084 }
2085 }
2086 void Repel(btSoftBody::Face* f1, btSoftBody::Face* f2)
2087 {
2088 //#define REPEL_NEIGHBOR 1
2089#ifndef REPEL_NEIGHBOR
2090 for (int node_id = 0; node_id < 3; ++node_id)
2091 {
2092 btSoftBody::Node* node = f1->m_n[node_id];
2093 for (int i = 0; i < 3; ++i)
2094 {
2095 if (f2->m_n[i] == node)
2096 return;
2097 }
2098 }
2099#endif
2100 bool skip = false;
2101 for (int node_id = 0; node_id < 3; ++node_id)
2102 {
2103 btSoftBody::Node* node = f1->m_n[node_id];
2104#ifdef REPEL_NEIGHBOR
2105 for (int i = 0; i < 3; ++i)
2106 {
2107 if (f2->m_n[i] == node)
2108 {
2109 skip = true;
2110 break;
2111 }
2112 }
2113 if (skip)
2114 {
2115 skip = false;
2116 continue;
2117 }
2118#endif
2119 btSoftBody::Face* face = f2;
2120 btVector3 bary;
2121 if (bernsteinCCD(face, node, dt, SAFE_EPSILON, bary))
2122 {
2123 btSoftBody::DeformableFaceNodeContact c;
2124 c.m_normal = face->m_normal;
2125 if (!useFaceNormal && c.m_normal.dot(node->m_x - face->m_n[2]->m_x) < 0)
2126 c.m_normal = -face->m_normal;
2127 c.m_node = node;
2128 c.m_face = face;
2129 c.m_bary = bary;
2130 c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF;
2131 // Initialize unused fields.
2132 c.m_weights = btVector3(0, 0, 0);
2133 c.m_margin = mrg;
2134 c.m_imf = 0;
2135 c.m_c0 = 0;
2136 c.m_colObj = psb[1];
2137 psb[0]->m_faceNodeContactsCCD.push_back(c);
2138 }
2139 }
2140 }
2141 btSoftBody* psb[2];
2142 btScalar dt, mrg;
2143 bool useFaceNormal;
2144 };
2145};
2146#endif //_BT_SOFT_BODY_INTERNALS_H
btAlignedAlloc
#define btAlignedAlloc(size, alignment)
Definition: btAlignedAllocator.h:46
btBroadphaseInterface.h
SOFTBODY_SHAPE_PROXYTYPE
@ SOFTBODY_SHAPE_PROXYTYPE
Definition: btBroadphaseProxy.h:73
btCollisionDispatcher.h
btConvexInternalShape.h
btGjkEpa2.h
btMax
const T & btMax(const T &a, const T &b)
Definition: btMinMax.h:27
btMin
const T & btMin(const T &a, const T &b)
Definition: btMinMax.h:21
btMultiBodyConstraint.h
btMultiBodyLinkCollider.h
btPolarDecomposition.h
dot
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
Definition: btQuaternion.h:888
inverse
btQuaternion inverse(const btQuaternion &q)
Return the inverse of a quaternion.
Definition: btQuaternion.h:909
length
btScalar length(const btQuaternion &q)
Return the length of a quaternion.
Definition: btQuaternion.h:895
btQuickprof.h
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
ATTRIBUTE_ALIGNED16
#define ATTRIBUTE_ALIGNED16(a)
Definition: btScalar.h:99
btSqrt
btScalar btSqrt(btScalar y)
Definition: btScalar.h:466
btFabs
btScalar btFabs(btScalar x)
Definition: btScalar.h:497
SIMD_INFINITY
#define SIMD_INFINITY
Definition: btScalar.h:544
SIMD_FORCE_INLINE
#define SIMD_FORCE_INLINE
Definition: btScalar.h:98
SIMD_EPSILON
#define SIMD_EPSILON
Definition: btScalar.h:543
btSwap
void btSwap(T &a, T &b)
Definition: btScalar.h:643
btAssert
#define btAssert(x)
Definition: btScalar.h:153
ProjectOnAxis
static btVector3 ProjectOnAxis(const btVector3 &v, const btVector3 &a)
Definition: btSoftBodyInternals.h:1042
bernsteinCCD
static bool bernsteinCCD(const btSoftBody::Face *face, const btSoftBody::Node *node, const btScalar &dt, const btScalar &mrg, btVector3 &bary)
Definition: btSoftBodyInternals.h:625
nearZero
static bool nearZero(const btScalar &a)
Definition: btSoftBodyInternals.h:142
deCasteljau
static void deCasteljau(const btScalar &k0, const btScalar &k1, const btScalar &k2, const btScalar &k3, const btScalar &t0, btScalar &k10, btScalar &k20, btScalar &k30, btScalar &k21, btScalar &k12)
Definition: btSoftBodyInternals.h:517
CompGreater
static bool CompGreater(const T &a, const T &b)
Definition: btSoftBodyInternals.h:801
getSigns
static bool getSigns(bool type_c, const btScalar &k0, const btScalar &k1, const btScalar &k2, const btScalar &k3, const btScalar &t0, const btScalar &t1, btScalar &lt0, btScalar &lt1)
Definition: btSoftBodyInternals.h:170
OuterProduct
static btMatrix3x3 OuterProduct(const btScalar *v1, const btScalar *v2, const btScalar *v3, const btScalar *u1, const btScalar *u2, const btScalar *u3, int ndof)
Definition: btSoftBodyInternals.h:924
Mul
static btMatrix3x3 Mul(const btMatrix3x3 &a, btScalar b)
Definition: btSoftBodyInternals.h:982
ImplicitSolve
static btScalar ImplicitSolve(btSoftBody::ImplicitFn *fn, const btVector3 &a, const btVector3 &b, const btScalar accuracy, const int maxiterations=256)
Definition: btSoftBodyInternals.h:1199
Orthogonalize
static void Orthogonalize(btMatrix3x3 &m)
Definition: btSoftBodyInternals.h:990
sameSign
static bool sameSign(const btScalar &a, const btScalar &b)
Definition: btSoftBodyInternals.h:146
Cross
static btMatrix3x3 Cross(const btVector3 &v)
Definition: btSoftBodyInternals.h:889
MassMatrix
static btMatrix3x3 MassMatrix(btScalar im, const btMatrix3x3 &iwi, const btVector3 &r)
Definition: btSoftBodyInternals.h:997
VolumeOf
static btDbvtVolume VolumeOf(const btSoftBody::Face &f, btScalar margin)
Definition: btSoftBodyInternals.h:1263
Lerp
static T Lerp(const T &a, const T &b, btScalar t)
Definition: btSoftBodyInternals.h:807
AngularImpulseMatrix
static btMatrix3x3 AngularImpulseMatrix(const btMatrix3x3 &iia, const btMatrix3x3 &iib)
Definition: btSoftBodyInternals.h:1035
continuousCollisionDetection
static bool continuousCollisionDetection(const btSoftBody::Face *face, const btSoftBody::Node *node, const btScalar &dt, const btScalar &mrg, btVector3 &bary)
Definition: btSoftBodyInternals.h:543
hasSeparatingPlane
static bool hasSeparatingPlane(const btSoftBody::Face *face, const btSoftBody::Node *node, const btScalar &dt)
Definition: btSoftBodyInternals.h:118
AreaOf
static btScalar AreaOf(const btVector3 &x0, const btVector3 &x1, const btVector3 &x2)
Definition: btSoftBodyInternals.h:1281
Sq
static T Sq(const T &x)
Definition: btSoftBodyInternals.h:845
generateUnitOrthogonalVector
static btVector3 generateUnitOrthogonalVector(const btVector3 &u)
Definition: btSoftBodyInternals.h:47
NormalizeAny
static btVector3 NormalizeAny(const btVector3 &v)
Definition: btSoftBodyInternals.h:1253
PolarDecompose
static int PolarDecompose(const btMatrix3x3 &m, btMatrix3x3 &q, btMatrix3x3 &s)
Definition: btSoftBodyInternals.h:1411
lineIntersectsTriangle
static bool lineIntersectsTriangle(const btVector3 &rayStart, const btVector3 &rayEnd, const btVector3 &p1, const btVector3 &p2, const btVector3 &p3, btVector3 &sect, btVector3 &normal)
Definition: btSoftBodyInternals.h:1142
MatchEdge
static int MatchEdge(const btSoftBody::Node *a, const btSoftBody::Node *b, const btSoftBody::Node *ma, const btSoftBody::Node *mb)
Definition: btSoftBodyInternals.h:1323
KDOP_COUNT
static const int KDOP_COUNT
Definition: btSoftBodyInternals.h:93
findJacobian
static void findJacobian(const btMultiBodyLinkCollider *multibodyLinkCol, btMultiBodyJacobianData &jacobianData, const btVector3 &contact_point, const btVector3 &dir)
btSoftBody implementation by Nathanael Presson
Definition: btSoftBodyInternals.h:34
CompLess
static bool CompLess(const T &a, const T &b)
Definition: btSoftBodyInternals.h:795
BaryEval
static T BaryEval(const T &a, const T &b, const T &c, const btVector3 &coord)
Definition: btSoftBodyInternals.h:1178
CenterOf
static btVector3 CenterOf(const btSoftBody::Face &f)
Definition: btSoftBodyInternals.h:1275
getSign
static int getSign(const btVector3 &n, const btVector3 &x)
Definition: btSoftBodyInternals.h:108
SameSign
static bool SameSign(const T &x, const T &y)
Definition: btSoftBodyInternals.h:863
EvaluateMedium
static void EvaluateMedium(const btSoftBodyWorldInfo *wfi, const btVector3 &x, btSoftBody::sMedium &medium)
Definition: btSoftBodyInternals.h:1234
evaluateBezier2
btScalar evaluateBezier2(const btScalar &p0, const btScalar &p1, const btScalar &p2, const btScalar &t, const btScalar &s)
Definition: btSoftBodyInternals.h:154
BaryCoord
static btVector3 BaryCoord(const btVector3 &a, const btVector3 &b, const btVector3 &c, const btVector3 &p)
Definition: btSoftBodyInternals.h:1186
rootFindingLemma
static bool rootFindingLemma(const btScalar &k0, const btScalar &k1, const btScalar &k2, const btScalar &k3)
Definition: btSoftBodyInternals.h:245
signDetermination1Internal
static bool signDetermination1Internal(const btScalar &k0, const btScalar &k1, const btScalar &k2, const btScalar &k3, const btScalar &u0, const btScalar &u1, const btScalar &v0, const btScalar &v1)
Definition: btSoftBodyInternals.h:303
diffSign
static bool diffSign(const btScalar &a, const btScalar &b)
Definition: btSoftBodyInternals.h:150
signDetermination1
static bool signDetermination1(const btScalar &k0, const btScalar &k1, const btScalar &k2, const btScalar &k3, const btSoftBody::Face *face, const btSoftBody::Node *node, const btScalar &dt)
Definition: btSoftBodyInternals.h:363
ScaleAlongAxis
static btMatrix3x3 ScaleAlongAxis(const btVector3 &a, btScalar s)
Definition: btSoftBodyInternals.h:874
proximityTest
static bool proximityTest(const btVector3 &x1, const btVector3 &x2, const btVector3 &x3, const btVector3 &x4, const btVector3 &normal, const btScalar &mrg, btVector3 &bary)
Definition: btSoftBodyInternals.h:66
ZeroInitialize
static void ZeroInitialize(T &value)
Definition: btSoftBodyInternals.h:789
Add
static btMatrix3x3 Add(const btMatrix3x3 &a, const btMatrix3x3 &b)
Definition: btSoftBodyInternals.h:966
InvLerp
static T InvLerp(const T &a, const T &b, btScalar t)
Definition: btSoftBodyInternals.h:813
coplanarAndInsideTest
static bool coplanarAndInsideTest(const btScalar &k0, const btScalar &k1, const btScalar &k2, const btScalar &k3, const btSoftBody::Face *face, const btSoftBody::Node *node, const btScalar &dt)
Definition: btSoftBodyInternals.h:480
ProjectOnPlane
static btVector3 ProjectOnPlane(const btVector3 &v, const btVector3 &a)
Definition: btSoftBodyInternals.h:1048
dop
static btVector3 dop[KDOP_COUNT]
Definition: btSoftBodyInternals.h:94
ClusterMetric
static btScalar ClusterMetric(const btVector3 &x, const btVector3 &y)
Definition: btSoftBodyInternals.h:868
signDetermination2
static bool signDetermination2(const btScalar &k0, const btScalar &k1, const btScalar &k2, const btScalar &k3, const btSoftBody::Face *face, const btSoftBody::Node *node, const btScalar &dt)
Definition: btSoftBodyInternals.h:414
getBernsteinCoeff
static void getBernsteinCoeff(const btSoftBody::Face *face, const btSoftBody::Node *node, const btScalar &dt, btScalar &k0, btScalar &k1, btScalar &k2, btScalar &k3)
Definition: btSoftBodyInternals.h:223
polyDecomposition
static void polyDecomposition(const btScalar &k0, const btScalar &k1, const btScalar &k2, const btScalar &k3, const btScalar &j0, const btScalar &j1, const btScalar &j2, btScalar &u0, btScalar &u1, btScalar &v0, btScalar &v1)
Definition: btSoftBodyInternals.h:236
conservativeCulling
static bool conservativeCulling(const btScalar &k0, const btScalar &k1, const btScalar &k2, const btScalar &k3, const btScalar &mrg)
Definition: btSoftBodyInternals.h:501
ImpulseMatrix
static btMatrix3x3 ImpulseMatrix(btScalar dt, btScalar ima, btScalar imb, const btMatrix3x3 &iwi, const btVector3 &r)
Definition: btSoftBodyInternals.h:1004
ApplyClampedForce
static void ApplyClampedForce(btSoftBody::Node &n, const btVector3 &f, btScalar dt)
Definition: btSoftBodyInternals.h:1307
rayIntersectsTriangle
static bool rayIntersectsTriangle(const btVector3 &origin, const btVector3 &dir, const btVector3 &v0, const btVector3 &v1, const btVector3 &v2, btScalar &t)
Definition: btSoftBodyInternals.h:1109
ProjectOrigin
static void ProjectOrigin(const btVector3 &a, const btVector3 &b, btVector3 &prj, btScalar &sqd)
Definition: btSoftBodyInternals.h:1055
Dot
static btScalar Dot(const btScalar *a, const btScalar *b, int ndof)
Definition: btSoftBodyInternals.h:916
Sub
static btMatrix3x3 Sub(const btMatrix3x3 &a, const btMatrix3x3 &b)
Definition: btSoftBodyInternals.h:974
Sign
static T Sign(const T &x)
Definition: btSoftBodyInternals.h:857
Clamp
static btVector3 Clamp(const btVector3 &v, btScalar maxlength)
Definition: btSoftBodyInternals.h:829
evaluateBezier
btScalar evaluateBezier(const btScalar &p0, const btScalar &p1, const btScalar &p2, const btScalar &p3, const btScalar &t, const btScalar &s)
Definition: btSoftBodyInternals.h:161
signDetermination2Internal
static bool signDetermination2Internal(const btScalar &k0, const btScalar &k1, const btScalar &k2, const btScalar &k3, const btScalar &j0, const btScalar &j1, const btScalar &j2, const btScalar &u0, const btScalar &u1, const btScalar &v0, const btScalar &v1)
Definition: btSoftBodyInternals.h:315
getJs
static void getJs(const btScalar &k0, const btScalar &k1, const btScalar &k2, const btScalar &k3, const btSoftBody::Node *a, const btSoftBody::Node *b, const btSoftBody::Node *c, const btSoftBody::Node *p, const btScalar &dt, btScalar &j0, btScalar &j1, btScalar &j2)
Definition: btSoftBodyInternals.h:263
bernsteinVFTest
static bool bernsteinVFTest(const btScalar &k0, const btScalar &k1, const btScalar &k2, const btScalar &k3, const btScalar &mrg, const btSoftBody::Face *face, const btSoftBody::Node *node, const btScalar &dt)
Definition: btSoftBodyInternals.h:510
Diagonal
static btMatrix3x3 Diagonal(btScalar x)
Definition: btSoftBodyInternals.h:898
Cube
static T Cube(const T &x)
Definition: btSoftBodyInternals.h:851
btSoftBody.h
SAFE_EPSILON
#define SAFE_EPSILON
Definition: btSoftBody.h:1224
btDot
btScalar btDot(const btVector3 &v1, const btVector3 &v2)
Return the dot product between two vectors.
Definition: btVector3.h:890
btCross
btVector3 btCross(const btVector3 &v1, const btVector3 &v2)
Return the cross product of two vectors.
Definition: btVector3.h:918
btAlignedObjectArray
The btAlignedObjectArray template class uses a subset of the stl::vector interface for its methods It...
Definition: btAlignedObjectArray.h:46
btAlignedObjectArray::size
int size() const
return the number of elements in the array
Definition: btAlignedObjectArray.h:142
btAlignedObjectArray::resize
void resize(int newsize, const T &fillData=T())
Definition: btAlignedObjectArray.h:203
btAlignedObjectArray::push_back
void push_back(const T &_Val)
Definition: btAlignedObjectArray.h:257
btCollisionObject::isStaticOrKinematicObject
bool isStaticOrKinematicObject() const
Definition: btCollisionObject.h:207
btCollisionObject::CO_FEATHERSTONE_LINK
@ CO_FEATHERSTONE_LINK
Definition: btCollisionObject.h:155
btCollisionObject::CO_RIGID_BODY
@ CO_RIGID_BODY
Definition: btCollisionObject.h:148
btCollisionObject::getWorldTransform
btTransform & getWorldTransform()
Definition: btCollisionObject.h:379
btCollisionObject::getInternalType
int getInternalType() const
reserved for Bullet internal usage
Definition: btCollisionObject.h:374
btCollisionObject::activate
void activate(bool forceActivation=false) const
Definition: btCollisionObject.cpp:72
btCollisionObject::getFriction
btScalar getFriction() const
Definition: btCollisionObject.h:325
btCollisionObject::getCollisionShape
const btCollisionShape * getCollisionShape() const
Definition: btCollisionObject.h:228
btCollisionShape::m_shapeType
int m_shapeType
Definition: btCollisionShape.h:30
btCollisionShape::getMargin
virtual btScalar getMargin() const =0
btCollisionShape::getAabb
virtual void getAabb(const btTransform &t, btVector3 &aabbMin, btVector3 &aabbMax) const =0
getAabb returns the axis aligned bounding box in the coordinate frame of the given transform t.
btConcaveShape
The btConcaveShape class provides an interface for non-moving (static) concave shapes.
Definition: btConcaveShape.h:39
btConvexInternalShape
The btConvexInternalShape is an internal base class, shared by most convex shape implementations.
Definition: btConvexInternalShape.h:30
btConvexInternalShape::setMargin
virtual void setMargin(btScalar margin)
Definition: btConvexInternalShape.h:102
btConvexInternalShape::getMargin
virtual btScalar getMargin() const
Definition: btConvexInternalShape.h:106
btConvexShape
The btConvexShape is an abstract shape interface, implemented by all convex shapes such as btBoxShape...
Definition: btConvexShape.h:33
btMatrix3x3
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:50
btMatrix3x3::transpose
btMatrix3x3 transpose() const
Return the transpose of the matrix.
Definition: btMatrix3x3.h:1049
btMatrix3x3::setIdentity
void setIdentity()
Set the matrix to the identity.
Definition: btMatrix3x3.h:323
btMultiBodyLinkCollider
Definition: btMultiBodyLinkCollider.h:33
btMultiBodyLinkCollider::upcast
static btMultiBodyLinkCollider * upcast(btCollisionObject *colObj)
Definition: btMultiBodyLinkCollider.h:61
btMultiBodyLinkCollider::m_link
int m_link
Definition: btMultiBodyLinkCollider.h:37
btMultiBodyLinkCollider::m_multiBody
btMultiBody * m_multiBody
Definition: btMultiBodyLinkCollider.h:36
btMultiBody::fillContactJacobianMultiDof
void fillContactJacobianMultiDof(int link, const btVector3 &contact_point, const btVector3 &normal, btScalar *jac, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v, btAlignedObjectArray< btMatrix3x3 > &scratch_m) const
Definition: btMultiBody.h:504
btMultiBody::calcAccelerationDeltasMultiDof
void calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v) const
stepVelocitiesMultiDof is deprecated, use computeAccelerationsArticulatedBodyAlgorithmMultiDof instea...
Definition: btMultiBody.cpp:1432
btMultiBody::getNumDofs
int getNumDofs() const
Definition: btMultiBody.h:167
btPolarDecomposition
This class is used to compute the polar decomposition of a matrix.
Definition: btPolarDecomposition.h:15
btPolarDecomposition::decompose
unsigned int decompose(const btMatrix3x3 &a, btMatrix3x3 &u, btMatrix3x3 &h) const
Decomposes a matrix into orthogonal and symmetric, positive-definite parts.
Definition: btPolarDecomposition.cpp:38
btRigidBody
The btRigidBody is the main class for rigid body objects.
Definition: btRigidBody.h:60
btRigidBody::getVelocityInLocalPoint
btVector3 getVelocityInLocalPoint(const btVector3 &rel_pos) const
Definition: btRigidBody.h:460
btRigidBody::getInvMass
btScalar getInvMass() const
Definition: btRigidBody.h:263
btRigidBody::getInvInertiaTensorWorld
const btMatrix3x3 & getInvInertiaTensorWorld() const
Definition: btRigidBody.h:265
btSoftBodyCollisionShape
Definition: btSoftBodyInternals.h:663
btSoftBodyCollisionShape::calculateLocalInertia
virtual void calculateLocalInertia(btScalar, btVector3 &) const
Definition: btSoftBodyInternals.h:714
btSoftBodyCollisionShape::processAllTriangles
void processAllTriangles(btTriangleCallback *, const btVector3 &, const btVector3 &) const
Definition: btSoftBodyInternals.h:677
btSoftBodyCollisionShape::setLocalScaling
virtual void setLocalScaling(const btVector3 &)
Definition: btSoftBodyInternals.h:705
btSoftBodyCollisionShape::getName
virtual const char * getName() const
Definition: btSoftBodyInternals.h:719
btSoftBodyCollisionShape::m_body
btSoftBody * m_body
Definition: btSoftBodyInternals.h:665
btSoftBodyCollisionShape::getLocalScaling
virtual const btVector3 & getLocalScaling() const
Definition: btSoftBodyInternals.h:709
btSoftBodyCollisionShape::btSoftBodyCollisionShape
btSoftBodyCollisionShape(btSoftBody *backptr)
Definition: btSoftBodyInternals.h:667
btSoftBodyCollisionShape::getAabb
virtual void getAabb(const btTransform &t, btVector3 &aabbMin, btVector3 &aabbMax) const
getAabb returns the axis aligned bounding box in the coordinate frame of the given transform t.
Definition: btSoftBodyInternals.h:684
btSoftBodyCollisionShape::~btSoftBodyCollisionShape
virtual ~btSoftBodyCollisionShape()
Definition: btSoftBodyInternals.h:673
btSoftBody::DeformableFaceRigidContact
Definition: btSoftBody.h:391
btSoftBody::DeformableFaceRigidContact::m_contactPoint
btVector3 m_contactPoint
Definition: btSoftBody.h:394
btSoftBody::DeformableFaceRigidContact::m_bary
btVector3 m_bary
Definition: btSoftBody.h:395
btSoftBody::DeformableFaceRigidContact::m_face
Face * m_face
Definition: btSoftBody.h:393
btSoftBody::DeformableFaceRigidContact::m_weights
btVector3 m_weights
Definition: btSoftBody.h:396
btSoftBody::DeformableNodeRigidContact
Definition: btSoftBody.h:379
btSoftBody::DeformableNodeRigidContact::m_node
Node * m_node
Definition: btSoftBody.h:381
btSoftBody::DeformableRigidContact::m_c2
btScalar m_c2
Definition: btSoftBody.h:365
btSoftBody::DeformableRigidContact::m_c1
btVector3 m_c1
Definition: btSoftBody.h:364
btSoftBody::DeformableRigidContact::t1
btVector3 t1
Definition: btSoftBody.h:374
btSoftBody::DeformableRigidContact::m_c5
btMatrix3x3 m_c5
Definition: btSoftBody.h:368
btSoftBody::DeformableRigidContact::m_c3
btScalar m_c3
Definition: btSoftBody.h:366
btSoftBody::DeformableRigidContact::m_cti
sCti m_cti
Definition: btSoftBody.h:362
btSoftBody::DeformableRigidContact::m_c0
btMatrix3x3 m_c0
Definition: btSoftBody.h:363
btSoftBody::DeformableRigidContact::t2
btVector3 t2
Definition: btSoftBody.h:375
btSoftBody::DeformableRigidContact::jacobianData_t1
btMultiBodyJacobianData jacobianData_t1
Definition: btSoftBody.h:372
btSoftBody::DeformableRigidContact::jacobianData_normal
btMultiBodyJacobianData jacobianData_normal
Definition: btSoftBody.h:371
btSoftBody::DeformableRigidContact::jacobianData_t2
btMultiBodyJacobianData jacobianData_t2
Definition: btSoftBody.h:373
btSoftBody::DeformableRigidContact::m_c4
btScalar m_c4
Definition: btSoftBody.h:367
btSoftBody
The btSoftBody is an class to simulate cloth and volumetric soft bodies.
Definition: btSoftBody.h:75
btSoftBody::m_cdbvt
btDbvt m_cdbvt
Definition: btSoftBody.h:838
btSoftBody::m_joints
tJointArray m_joints
Definition: btSoftBody.h:830
btSoftBody::m_clusterConnectivity
btAlignedObjectArray< bool > m_clusterConnectivity
Definition: btSoftBody.h:855
btSoftBody::m_sst
SolverState m_sst
Definition: btSoftBody.h:809
btSoftBody::m_faceNodeContactsCCD
btAlignedObjectArray< DeformableFaceNodeContact > m_faceNodeContactsCCD
Definition: btSoftBody.h:828
btSoftBody::m_faceNodeContacts
btAlignedObjectArray< DeformableFaceNodeContact > m_faceNodeContacts
Definition: btSoftBody.h:826
btSoftBody::checkDeformableContact
bool checkDeformableContact(const btCollisionObjectWrapper *colObjWrap, const btVector3 &x, btScalar margin, btSoftBody::sCti &cti, bool predict=false) const
Definition: btSoftBody.cpp:2777
btSoftBody::m_reducedModel
bool m_reducedModel
Definition: btSoftBody.h:861
btSoftBody::m_cfg
Config m_cfg
Definition: btSoftBody.h:808
btSoftBody::m_rcontacts
tRContactArray m_rcontacts
Definition: btSoftBody.h:824
btSoftBody::m_bounds
btVector3 m_bounds[2]
Definition: btSoftBody.h:833
btSoftBody::m_clusters
tClusterArray m_clusters
Definition: btSoftBody.h:839
btSoftBody::m_faceRigidContacts
btAlignedObjectArray< DeformableFaceRigidContact > m_faceRigidContacts
Definition: btSoftBody.h:827
btSoftBody::checkDeformableFaceContact
bool checkDeformableFaceContact(const btCollisionObjectWrapper *colObjWrap, Face &f, btVector3 &contact_point, btVector3 &bary, btScalar margin, btSoftBody::sCti &cti, bool predict=false) const
Definition: btSoftBody.cpp:2825
btSoftBody::m_nodeRigidContacts
btAlignedObjectArray< DeformableNodeRigidContact > m_nodeRigidContacts
Definition: btSoftBody.h:825
btSoftBody::m_scontacts
tSContactArray m_scontacts
Definition: btSoftBody.h:829
btSoftBody::checkContact
bool checkContact(const btCollisionObjectWrapper *colObjWrap, const btVector3 &x, btScalar margin, btSoftBody::sCti &cti) const
Definition: btSoftBody.cpp:2748
btSoftClusterCollisionShape
Definition: btSoftBodyInternals.h:729
btSoftClusterCollisionShape::setMargin
virtual void setMargin(btScalar margin)
Definition: btSoftBodyInternals.h:773
btSoftClusterCollisionShape::getAabb
virtual void getAabb(const btTransform &t, btVector3 &aabbMin, btVector3 &aabbMax) const
getAabb's default implementation is brute force, expected derived classes to implement a fast dedicat...
Definition: btSoftBodyInternals.h:764
btSoftClusterCollisionShape::btSoftClusterCollisionShape
btSoftClusterCollisionShape(const btSoftBody::Cluster *cluster)
Definition: btSoftBodyInternals.h:733
btSoftClusterCollisionShape::getShapeType
virtual int getShapeType() const
Definition: btSoftBodyInternals.h:768
btSoftClusterCollisionShape::calculateLocalInertia
virtual void calculateLocalInertia(btScalar mass, btVector3 &inertia) const
Definition: btSoftBodyInternals.h:760
btSoftClusterCollisionShape::localGetSupportingVertexWithoutMargin
virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3 &vec) const
Definition: btSoftBodyInternals.h:751
btSoftClusterCollisionShape::getMargin
virtual btScalar getMargin() const
Definition: btSoftBodyInternals.h:777
btSoftClusterCollisionShape::m_cluster
const btSoftBody::Cluster * m_cluster
Definition: btSoftBodyInternals.h:731
btSoftClusterCollisionShape::localGetSupportingVertex
virtual btVector3 localGetSupportingVertex(const btVector3 &vec) const
Definition: btSoftBodyInternals.h:735
btSoftClusterCollisionShape::batchedUnitVectorGetSupportingVertexWithoutMargin
virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3 *vectors, btVector3 *supportVerticesOut, int numVectors) const
Definition: btSoftBodyInternals.h:756
btSoftClusterCollisionShape::getName
virtual const char * getName() const
Definition: btSoftBodyInternals.h:771
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:30
btTransform::getBasis
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:109
btTransform::getIdentity
static const btTransform & getIdentity()
Return an identity transform.
Definition: btTransform.h:198
btTransform::getOrigin
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:114
btTriangleCallback
The btTriangleCallback provides a callback for each overlapping triangle when calling processAllTrian...
Definition: btTriangleCallback.h:24
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:82
btVector3::getZ
const btScalar & getZ() const
Return the z value.
Definition: btVector3.h:565
btVector3::safeNorm
btScalar safeNorm() const
Return the norm (length) of the vector.
Definition: btVector3.h:269
btVector3::setMax
void setMax(const btVector3 &other)
Set each element to the max of the current values and the values of another btVector3.
Definition: btVector3.h:609
btVector3::z
const btScalar & z() const
Return the z value.
Definition: btVector3.h:579
btVector3::safeNormalize
btVector3 & safeNormalize()
Definition: btVector3.h:286
btVector3::length
btScalar length() const
Return the length of the vector.
Definition: btVector3.h:257
btVector3::cross
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
Definition: btVector3.h:380
btVector3::dot
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:229
btVector3::norm
btScalar norm() const
Return the norm (length) of the vector.
Definition: btVector3.h:263
btVector3::normalized
btVector3 normalized() const
Return a normalized version of this vector.
Definition: btVector3.h:949
btVector3::length2
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:251
btVector3::getY
const btScalar & getY() const
Return the y value.
Definition: btVector3.h:563
btVector3::x
const btScalar & x() const
Return the x value.
Definition: btVector3.h:575
btVector3::setMin
void setMin(const btVector3 &other)
Set each element to the min of the current values and the values of another btVector3.
Definition: btVector3.h:626
btVector3::normalize
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition: btVector3.h:303
btVector3::getX
const btScalar & getX() const
Return the x value.
Definition: btVector3.h:561
btVector3::y
const btScalar & y() const
Return the y value.
Definition: btVector3.h:577
eps
const btScalar eps
Definition: poly34.cpp:11
SolveP3
int SolveP3(btScalar *x, btScalar a, btScalar b, btScalar c)
Definition: poly34.cpp:71
SolveP2
int SolveP2(btScalar *x, btScalar a, btScalar b)
Definition: poly34.cpp:52
poly34.h
btCollisionObjectWrapper
Definition: btCollisionObjectWrapper.h:18
btCollisionObjectWrapper::getCollisionShape
const btCollisionShape * getCollisionShape() const
Definition: btCollisionObjectWrapper.h:46
btCollisionObjectWrapper::getCollisionObject
const btCollisionObject * getCollisionObject() const
Definition: btCollisionObjectWrapper.h:45
btCollisionObjectWrapper::getWorldTransform
const btTransform & getWorldTransform() const
Definition: btCollisionObjectWrapper.h:44
btDbvntNode
Definition: btDbvt.h:195
btDbvntNode::data
void * data
Definition: btDbvt.h:202
btDbvtAabbMm
Definition: btDbvt.h:132
btDbvtAabbMm::FromMM
static btDbvtAabbMm FromMM(const btVector3 &mi, const btVector3 &mx)
Definition: btDbvt.h:479
btDbvtAabbMm::FromPoints
static btDbvtAabbMm FromPoints(const btVector3 *pts, int n)
Definition: btDbvt.h:488
btDbvtAabbMm::Expand
DBVT_INLINE void Expand(const btVector3 &e)
Definition: btDbvt.h:514
btDbvtNode
Definition: btDbvt.h:181
btDbvtNode::data
void * data
Definition: btDbvt.h:188
btDbvt::ICollide
Definition: btDbvt.h:270
btDbvt::collideTV
DBVT_PREFIX void collideTV(const btDbvtNode *root, const btDbvtVolume &volume, DBVT_IPOLICY) const
Definition: btDbvt.h:1148
btDbvt::m_root
btDbvtNode * m_root
Definition: btDbvt.h:302
btDbvt::collideTT
DBVT_PREFIX void collideTT(const btDbvtNode *root0, const btDbvtNode *root1, DBVT_IPOLICY)
Definition: btDbvt.h:822
btEigen
Definition: btSoftBodyInternals.h:1339
btEigen::mulPQ
static void mulPQ(btMatrix3x3 &a, btScalar c, btScalar s, int p, int q)
Definition: btSoftBodyInternals.h:1396
btEigen::system
static int system(btMatrix3x3 &a, btMatrix3x3 *vectors, btVector3 *values=0)
Definition: btSoftBodyInternals.h:1340
btEigen::mulTPQ
static void mulTPQ(btMatrix3x3 &a, btScalar c, btScalar s, int p, int q)
Definition: btSoftBodyInternals.h:1387
btGjkEpaSolver2::sResults
Definition: btGjkEpa2.h:34
btGjkEpaSolver2::sResults::distance
btScalar distance
Definition: btGjkEpa2.h:44
btGjkEpaSolver2::sResults::witnesses
btVector3 witnesses[2]
Definition: btGjkEpa2.h:42
btGjkEpaSolver2::sResults::normal
btVector3 normal
Definition: btGjkEpa2.h:43
btGjkEpaSolver2::SignedDistance
static btScalar SignedDistance(const btVector3 &position, btScalar margin, const btConvexShape *shape, const btTransform &wtrs, sResults &results)
Definition: btGjkEpa2.cpp:1021
btMultiBodyJacobianData
Definition: btMultiBodyConstraint.h:44
btMultiBodyJacobianData::m_deltaVelocitiesUnitImpulse
btAlignedObjectArray< btScalar > m_deltaVelocitiesUnitImpulse
Definition: btMultiBodyConstraint.h:46
btMultiBodyJacobianData::m_jacobians
btAlignedObjectArray< btScalar > m_jacobians
Definition: btMultiBodyConstraint.h:45
btMultiBodyJacobianData::scratch_r
btAlignedObjectArray< btScalar > scratch_r
Definition: btMultiBodyConstraint.h:48
btMultiBodyJacobianData::scratch_m
btAlignedObjectArray< btMatrix3x3 > scratch_m
Definition: btMultiBodyConstraint.h:50
btMultiBodyJacobianData::scratch_v
btAlignedObjectArray< btVector3 > scratch_v
Definition: btMultiBodyConstraint.h:49
btSoftBodyWorldInfo
Definition: btSoftBody.h:48
btSoftBodyWorldInfo::air_density
btScalar air_density
Definition: btSoftBody.h:49
btSoftBodyWorldInfo::water_density
btScalar water_density
Definition: btSoftBody.h:50
btSoftBodyWorldInfo::m_gravity
btVector3 m_gravity
Definition: btSoftBody.h:56
btSoftBodyWorldInfo::water_normal
btVector3 water_normal
Definition: btSoftBody.h:53
btSoftBodyWorldInfo::water_offset
btScalar water_offset
Definition: btSoftBody.h:51
btSoftBody::Body
Definition: btSoftBody.h:514
btSoftBody::Body::invMass
btScalar invMass() const
Definition: btSoftBody.h:540
btSoftBody::Body::invWorldInertia
const btMatrix3x3 & invWorldInertia() const
Definition: btSoftBody.h:533
btSoftBody::Body::velocity
btVector3 velocity(const btVector3 &rpos) const
Definition: btSoftBody.h:571
btSoftBody::Body::xform
const btTransform & xform() const
Definition: btSoftBody.h:546
btSoftBody::CJoint
Definition: btSoftBody.h:696
btSoftBody::CJoint::m_life
int m_life
Definition: btSoftBody.h:697
btSoftBody::CJoint::m_rpos
btVector3 m_rpos[2]
Definition: btSoftBody.h:699
btSoftBody::CJoint::m_normal
btVector3 m_normal
Definition: btSoftBody.h:700
btSoftBody::CJoint::m_maxlife
int m_maxlife
Definition: btSoftBody.h:698
btSoftBody::CJoint::m_friction
btScalar m_friction
Definition: btSoftBody.h:701
btSoftBody::Cluster
Definition: btSoftBody.h:459
btSoftBody::Cluster::m_nodes
btAlignedObjectArray< Node * > m_nodes
Definition: btSoftBody.h:461
btSoftBody::Cluster::m_com
btVector3 m_com
Definition: btSoftBody.h:468
btSoftBody::Cluster::m_clusterIndex
int m_clusterIndex
Definition: btSoftBody.h:484
btSoftBody::Cluster::m_containsAnchor
bool m_containsAnchor
Definition: btSoftBody.h:482
btSoftBody::Config::kSKHR_CL
btScalar kSKHR_CL
Definition: btSoftBody.h:724
btSoftBody::Config::kKHR
btScalar kKHR
Definition: btSoftBody.h:720
btSoftBody::Config::kCHR
btScalar kCHR
Definition: btSoftBody.h:719
btSoftBody::Config::kDF
btScalar kDF
Definition: btSoftBody.h:717
btSoftBody::Config::kSSHR_CL
btScalar kSSHR_CL
Definition: btSoftBody.h:725
btSoftBody::Config::kSRHR_CL
btScalar kSRHR_CL
Definition: btSoftBody.h:723
btSoftBody::Config::kSK_SPLT_CL
btScalar kSK_SPLT_CL
Definition: btSoftBody.h:727
btSoftBody::Config::kSS_SPLT_CL
btScalar kSS_SPLT_CL
Definition: btSoftBody.h:728
btSoftBody::Config::kSR_SPLT_CL
btScalar kSR_SPLT_CL
Definition: btSoftBody.h:726
btSoftBody::Config::kSHR
btScalar kSHR
Definition: btSoftBody.h:721
btSoftBody::DeformableFaceNodeContact
Definition: btSoftBody.h:400
btSoftBody::DeformableFaceNodeContact::m_bary
btVector3 m_bary
Definition: btSoftBody.h:403
btSoftBody::DeformableFaceNodeContact::m_weights
btVector3 m_weights
Definition: btSoftBody.h:404
btSoftBody::DeformableFaceNodeContact::m_normal
btVector3 m_normal
Definition: btSoftBody.h:405
btSoftBody::DeformableFaceNodeContact::m_node
Node * m_node
Definition: btSoftBody.h:401
btSoftBody::DeformableFaceNodeContact::m_margin
btScalar m_margin
Definition: btSoftBody.h:406
btSoftBody::DeformableFaceNodeContact::m_face
Face * m_face
Definition: btSoftBody.h:402
btSoftBody::DeformableFaceNodeContact::m_c0
btScalar m_c0
Definition: btSoftBody.h:409
btSoftBody::DeformableFaceNodeContact::m_imf
btScalar m_imf
Definition: btSoftBody.h:408
btSoftBody::DeformableFaceNodeContact::m_colObj
const btCollisionObject * m_colObj
Definition: btSoftBody.h:410
btSoftBody::DeformableFaceNodeContact::m_friction
btScalar m_friction
Definition: btSoftBody.h:407
btSoftBody::Face
Definition: btSoftBody.h:306
btSoftBody::Face::m_pcontact
btVector4 m_pcontact
Definition: btSoftBody.h:311
btSoftBody::Face::m_normal
btVector3 m_normal
Definition: btSoftBody.h:308
btSoftBody::Face::m_n0
btVector3 m_n0
Definition: btSoftBody.h:312
btSoftBody::Face::m_n1
btVector3 m_n1
Definition: btSoftBody.h:312
btSoftBody::Face::m_vn
btVector3 m_vn
Definition: btSoftBody.h:312
btSoftBody::Face::m_n
Node * m_n[3]
Definition: btSoftBody.h:307
btSoftBody::ImplicitFn
Definition: btSoftBody.h:211
btSoftBody::ImplicitFn::Eval
virtual btScalar Eval(const btVector3 &x)=0
btSoftBody::Joint::m_drift
btVector3 m_drift
Definition: btSoftBody.h:643
btSoftBody::Joint::m_split
btScalar m_split
Definition: btSoftBody.h:642
btSoftBody::Joint::m_cfm
btScalar m_cfm
Definition: btSoftBody.h:640
btSoftBody::Joint::m_delete
bool m_delete
Definition: btSoftBody.h:646
btSoftBody::Joint::m_bodies
Body m_bodies[2]
Definition: btSoftBody.h:638
btSoftBody::Joint::m_erp
btScalar m_erp
Definition: btSoftBody.h:641
btSoftBody::Joint::m_massmatrix
btMatrix3x3 m_massmatrix
Definition: btSoftBody.h:645
btSoftBody::Joint::m_refs
btVector3 m_refs[2]
Definition: btSoftBody.h:639
btSoftBody::Node
Definition: btSoftBody.h:268
btSoftBody::Node::m_im
btScalar m_im
Definition: btSoftBody.h:275
btSoftBody::Node::m_x
btVector3 m_x
Definition: btSoftBody.h:269
btSoftBody::Node::m_battach
int m_battach
Definition: btSoftBody.h:279
btSoftBody::Node::m_v
btVector3 m_v
Definition: btSoftBody.h:271
btSoftBody::Node::m_q
btVector3 m_q
Definition: btSoftBody.h:270
btSoftBody::Node::m_n
btVector3 m_n
Definition: btSoftBody.h:274
btSoftBody::Node::m_f
btVector3 m_f
Definition: btSoftBody.h:273
btSoftBody::Node::m_effectiveMass_inv
btMatrix3x3 m_effectiveMass_inv
Definition: btSoftBody.h:283
btSoftBody::RContact
Definition: btSoftBody.h:342
btSoftBody::RContact::m_c2
btScalar m_c2
Definition: btSoftBody.h:347
btSoftBody::RContact::m_cti
sCti m_cti
Definition: btSoftBody.h:343
btSoftBody::RContact::m_c4
btScalar m_c4
Definition: btSoftBody.h:349
btSoftBody::RContact::m_c1
btVector3 m_c1
Definition: btSoftBody.h:346
btSoftBody::RContact::m_node
Node * m_node
Definition: btSoftBody.h:344
btSoftBody::RContact::m_c0
btMatrix3x3 m_c0
Definition: btSoftBody.h:345
btSoftBody::RContact::m_c3
btScalar m_c3
Definition: btSoftBody.h:348
btSoftBody::SContact
Definition: btSoftBody.h:415
btSoftBody::SContact::m_normal
btVector3 m_normal
Definition: btSoftBody.h:419
btSoftBody::SContact::m_node
Node * m_node
Definition: btSoftBody.h:416
btSoftBody::SContact::m_margin
btScalar m_margin
Definition: btSoftBody.h:420
btSoftBody::SContact::m_weights
btVector3 m_weights
Definition: btSoftBody.h:418
btSoftBody::SContact::m_cfm
btScalar m_cfm[2]
Definition: btSoftBody.h:422
btSoftBody::SContact::m_friction
btScalar m_friction
Definition: btSoftBody.h:421
btSoftBody::SContact::m_face
Face * m_face
Definition: btSoftBody.h:417
btSoftBody::SolverState::isdt
btScalar isdt
Definition: btSoftBody.h:755
btSoftBody::SolverState::sdt
btScalar sdt
Definition: btSoftBody.h:754
btSoftBody::sCti
Definition: btSoftBody.h:225
btSoftBody::sCti::m_colObj
const btCollisionObject * m_colObj
Definition: btSoftBody.h:226
btSoftBody::sCti::m_normal
btVector3 m_normal
Definition: btSoftBody.h:227
btSoftBody::sMedium
Definition: btSoftBody.h:234
btSoftBody::sMedium::m_density
btScalar m_density
Definition: btSoftBody.h:237
btSoftBody::sMedium::m_pressure
btScalar m_pressure
Definition: btSoftBody.h:236
btSoftBody::sMedium::m_velocity
btVector3 m_velocity
Definition: btSoftBody.h:235
btSoftColliders::ClusterBase
Definition: btSoftBodyInternals.h:1426
btSoftColliders::ClusterBase::m_margin
btScalar m_margin
Definition: btSoftBodyInternals.h:1429
btSoftColliders::ClusterBase::erp
btScalar erp
Definition: btSoftBodyInternals.h:1427
btSoftColliders::ClusterBase::threshold
btScalar threshold
Definition: btSoftBodyInternals.h:1431
btSoftColliders::ClusterBase::idt
btScalar idt
Definition: btSoftBodyInternals.h:1428
btSoftColliders::ClusterBase::ClusterBase
ClusterBase()
Definition: btSoftBodyInternals.h:1432
btSoftColliders::ClusterBase::SolveContact
bool SolveContact(const btGjkEpaSolver2::sResults &res, btSoftBody::Body ba, const btSoftBody::Body bb, btSoftBody::CJoint &joint)
Definition: btSoftBodyInternals.h:1440
btSoftColliders::ClusterBase::friction
btScalar friction
Definition: btSoftBodyInternals.h:1430
btSoftColliders::CollideCCD
Definition: btSoftBodyInternals.h:2049
btSoftColliders::CollideCCD::dt
btScalar dt
Definition: btSoftBodyInternals.h:2142
btSoftColliders::CollideCCD::Process
void Process(const btDbvtNode *lnode, const btDbvtNode *lface)
Definition: btSoftBodyInternals.h:2050
btSoftColliders::CollideCCD::useFaceNormal
bool useFaceNormal
Definition: btSoftBodyInternals.h:2143
btSoftColliders::CollideCCD::mrg
btScalar mrg
Definition: btSoftBodyInternals.h:2142
btSoftColliders::CollideCCD::Repel
void Repel(btSoftBody::Face *f1, btSoftBody::Face *f2)
Definition: btSoftBodyInternals.h:2086
btSoftColliders::CollideCCD::psb
btSoftBody * psb[2]
Definition: btSoftBodyInternals.h:2141
btSoftColliders::CollideCCD::Process
void Process(const btDbvntNode *lface1, const btDbvntNode *lface2)
Definition: btSoftBodyInternals.h:2075
btSoftColliders::CollideCL_RS
Definition: btSoftBodyInternals.h:1490
btSoftColliders::CollideCL_RS::Process
void Process(const btDbvtNode *leaf)
Definition: btSoftBodyInternals.h:1494
btSoftColliders::CollideCL_RS::psb
btSoftBody * psb
Definition: btSoftBodyInternals.h:1491
btSoftColliders::CollideCL_RS::ProcessColObj
void ProcessColObj(btSoftBody *ps, const btCollisionObjectWrapper *colObWrap)
Definition: btSoftBodyInternals.h:1529
btSoftColliders::CollideCL_RS::m_colObjWrap
const btCollisionObjectWrapper * m_colObjWrap
Definition: btSoftBodyInternals.h:1492
btSoftColliders::CollideCL_SS
Definition: btSoftBodyInternals.h:1552
btSoftColliders::CollideCL_SS::Process
void Process(const btDbvtNode *la, const btDbvtNode *lb)
Definition: btSoftBodyInternals.h:1554
btSoftColliders::CollideCL_SS::bodies
btSoftBody * bodies[2]
Definition: btSoftBodyInternals.h:1553
btSoftColliders::CollideCL_SS::ProcessSoftSoft
void ProcessSoftSoft(btSoftBody *psa, btSoftBody *psb)
Definition: btSoftBodyInternals.h:1592
btSoftColliders::CollideFF_DD
Definition: btSoftBodyInternals.h:1977
btSoftColliders::CollideFF_DD::useFaceNormal
bool useFaceNormal
Definition: btSoftBodyInternals.h:2045
btSoftColliders::CollideFF_DD::Repel
void Repel(btSoftBody::Face *f1, btSoftBody::Face *f2)
Definition: btSoftBodyInternals.h:1989
btSoftColliders::CollideFF_DD::mrg
btScalar mrg
Definition: btSoftBodyInternals.h:2044
btSoftColliders::CollideFF_DD::psb
btSoftBody * psb[2]
Definition: btSoftBodyInternals.h:2043
btSoftColliders::CollideFF_DD::Process
void Process(const btDbvntNode *lface1, const btDbvntNode *lface2)
Definition: btSoftBodyInternals.h:1978
btSoftColliders::CollideSDF_RDF
Definition: btSoftBodyInternals.h:1768
btSoftColliders::CollideSDF_RDF::psb
btSoftBody * psb
Definition: btSoftBodyInternals.h:1861
btSoftColliders::CollideSDF_RDF::DoNode
void DoNode(btSoftBody::Face &f) const
Definition: btSoftBodyInternals.h:1774
btSoftColliders::CollideSDF_RDF::Process
void Process(const btDbvtNode *leaf)
Definition: btSoftBodyInternals.h:1769
btSoftColliders::CollideSDF_RDF::m_rigidBody
btRigidBody * m_rigidBody
Definition: btSoftBodyInternals.h:1863
btSoftColliders::CollideSDF_RDF::stamargin
btScalar stamargin
Definition: btSoftBodyInternals.h:1865
btSoftColliders::CollideSDF_RDF::m_colObj1Wrap
const btCollisionObjectWrapper * m_colObj1Wrap
Definition: btSoftBodyInternals.h:1862
btSoftColliders::CollideSDF_RDF::dynmargin
btScalar dynmargin
Definition: btSoftBodyInternals.h:1864
btSoftColliders::CollideSDF_RD
Definition: btSoftBodyInternals.h:1659
btSoftColliders::CollideSDF_RD::stamargin
btScalar stamargin
Definition: btSoftBodyInternals.h:1761
btSoftColliders::CollideSDF_RD::m_rigidBody
btRigidBody * m_rigidBody
Definition: btSoftBodyInternals.h:1759
btSoftColliders::CollideSDF_RD::psb
btSoftBody * psb
Definition: btSoftBodyInternals.h:1757
btSoftColliders::CollideSDF_RD::Process
void Process(const btDbvtNode *leaf)
Definition: btSoftBodyInternals.h:1660
btSoftColliders::CollideSDF_RD::m_colObj1Wrap
const btCollisionObjectWrapper * m_colObj1Wrap
Definition: btSoftBodyInternals.h:1758
btSoftColliders::CollideSDF_RD::DoNode
void DoNode(btSoftBody::Node &n) const
Definition: btSoftBodyInternals.h:1665
btSoftColliders::CollideSDF_RD::dynmargin
btScalar dynmargin
Definition: btSoftBodyInternals.h:1760
btSoftColliders::CollideSDF_RS
Definition: btSoftBodyInternals.h:1607
btSoftColliders::CollideSDF_RS::dynmargin
btScalar dynmargin
Definition: btSoftBodyInternals.h:1651
btSoftColliders::CollideSDF_RS::DoNode
void DoNode(btSoftBody::Node &n) const
Definition: btSoftBodyInternals.h:1613
btSoftColliders::CollideSDF_RS::Process
void Process(const btDbvtNode *leaf)
Definition: btSoftBodyInternals.h:1608
btSoftColliders::CollideSDF_RS::m_rigidBody
btRigidBody * m_rigidBody
Definition: btSoftBodyInternals.h:1650
btSoftColliders::CollideSDF_RS::psb
btSoftBody * psb
Definition: btSoftBodyInternals.h:1648
btSoftColliders::CollideSDF_RS::stamargin
btScalar stamargin
Definition: btSoftBodyInternals.h:1652
btSoftColliders::CollideSDF_RS::m_colObj1Wrap
const btCollisionObjectWrapper * m_colObj1Wrap
Definition: btSoftBodyInternals.h:1649
btSoftColliders::CollideVF_DD
Definition: btSoftBodyInternals.h:1928
btSoftColliders::CollideVF_DD::Process
void Process(const btDbvtNode *lnode, const btDbvtNode *lface)
Definition: btSoftBodyInternals.h:1929
btSoftColliders::CollideVF_DD::useFaceNormal
bool useFaceNormal
Definition: btSoftBodyInternals.h:1970
btSoftColliders::CollideVF_DD::mrg
btScalar mrg
Definition: btSoftBodyInternals.h:1969
btSoftColliders::CollideVF_DD::psb
btSoftBody * psb[2]
Definition: btSoftBodyInternals.h:1968
btSoftColliders::CollideVF_SS
Definition: btSoftBodyInternals.h:1872
btSoftColliders::CollideVF_SS::Process
void Process(const btDbvtNode *lnode, const btDbvtNode *lface)
Definition: btSoftBodyInternals.h:1873
btSoftColliders::CollideVF_SS::psb
btSoftBody * psb[2]
Definition: btSoftBodyInternals.h:1920
btSoftColliders::CollideVF_SS::mrg
btScalar mrg
Definition: btSoftBodyInternals.h:1921
btSoftColliders
Definition: btSoftBodyInternals.h:1421
btSymMatrix
Definition: btSoftBodyInternals.h:639
btSymMatrix::index
int index(int c, int r) const
Definition: btSoftBodyInternals.h:647
btSymMatrix::operator()
T & operator()(int c, int r)
Definition: btSoftBodyInternals.h:653
btSymMatrix::btSymMatrix
btSymMatrix()
Definition: btSoftBodyInternals.h:640
btSymMatrix::dim
int dim
Definition: btSoftBodyInternals.h:656
btSymMatrix::store
btAlignedObjectArray< T > store
Definition: btSoftBodyInternals.h:655
btSymMatrix::resize
void resize(int n, const T &init=T())
Definition: btSoftBodyInternals.h:642
btSymMatrix::btSymMatrix
btSymMatrix(int n, const T &init=T())
Definition: btSoftBodyInternals.h:641
btSymMatrix::operator()
const T & operator()(int c, int r) const
Definition: btSoftBodyInternals.h:654