1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2006 Erwin Coumans https://bulletphysics.org
4
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages arising from the use of this software.
7 Permission is granted to anyone to use this software for any purpose,
8 including commercial applications, and to alter it and redistribute it freely,
9 subject to the following restrictions:
10
11 1. 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.
12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13 3. This notice may not be removed or altered from any source distribution.
14 */
15 ///btSoftBody implementation by Nathanael Presson
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
findJacobian(const btMultiBodyLinkCollider * multibodyLinkCol,btMultiBodyJacobianData & jacobianData,const btVector3 & contact_point,const btVector3 & dir)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 }
generateUnitOrthogonalVector(const btVector3 & u)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
proximityTest(const btVector3 & x1,const btVector3 & x2,const btVector3 & x3,const btVector3 & x4,const btVector3 & normal,const btScalar & mrg,btVector3 & bary)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
getSign(const btVector3 & n,const btVector3 & x)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
hasSeparatingPlane(const btSoftBody::Face * face,const btSoftBody::Node * node,const btScalar & dt)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
nearZero(const btScalar & a)142 static SIMD_FORCE_INLINE bool nearZero(const btScalar& a)
143 {
144 return (a > -SAFE_EPSILON && a < SAFE_EPSILON);
145 }
sameSign(const btScalar & a,const btScalar & b)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 }
diffSign(const btScalar & a,const btScalar & b)150 static SIMD_FORCE_INLINE bool diffSign(const btScalar& a, const btScalar& b)
151 {
152 return !sameSign(a, b);
153 }
evaluateBezier2(const btScalar & p0,const btScalar & p1,const btScalar & p2,const btScalar & t,const btScalar & s)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 }
evaluateBezier(const btScalar & p0,const btScalar & p1,const btScalar & p2,const btScalar & p3,const btScalar & t,const btScalar & s)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 }
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)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
getBernsteinCoeff(const btSoftBody::Face * face,const btSoftBody::Node * node,const btScalar & dt,btScalar & k0,btScalar & k1,btScalar & k2,btScalar & k3)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
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)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
rootFindingLemma(const btScalar & k0,const btScalar & k1,const btScalar & k2,const btScalar & k3)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
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)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
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)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
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)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
signDetermination1(const btScalar & k0,const btScalar & k1,const btScalar & k2,const btScalar & k3,const btSoftBody::Face * face,const btSoftBody::Node * node,const btScalar & dt)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
signDetermination2(const btScalar & k0,const btScalar & k1,const btScalar & k2,const btScalar & k3,const btSoftBody::Face * face,const btSoftBody::Node * node,const btScalar & dt)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
coplanarAndInsideTest(const btScalar & k0,const btScalar & k1,const btScalar & k2,const btScalar & k3,const btSoftBody::Face * face,const btSoftBody::Node * node,const btScalar & dt)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 }
conservativeCulling(const btScalar & k0,const btScalar & k1,const btScalar & k2,const btScalar & k3,const btScalar & mrg)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
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)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
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)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 }
bernsteinVFTest(const btSoftBody::Face * face,const btSoftBody::Node * node,const btScalar & dt,const btScalar & mrg)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
continuousCollisionDetection(const btSoftBody::Face * face,const btSoftBody::Node * node,const btScalar & dt,const btScalar & mrg,btVector3 & bary)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 }
bernsteinCCD(const btSoftBody::Face * face,const btSoftBody::Node * node,const btScalar & dt,const btScalar & mrg,btVector3 & bary)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 //
637 template <typename T>
638 struct btSymMatrix
639 {
btSymMatrixbtSymMatrix640 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 }
indexbtSymMatrix647 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 }
operatorbtSymMatrix653 T& operator()(int c, int r) { return (store[index(c, r)]); }
operatorbtSymMatrix654 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 {
664 public:
665 btSoftBody* m_body;
666
btSoftBodyCollisionShape(btSoftBody * backptr)667 btSoftBodyCollisionShape(btSoftBody* backptr)
668 {
669 m_shapeType = SOFTBODY_SHAPE_PROXYTYPE;
670 m_body = backptr;
671 }
672
~btSoftBodyCollisionShape()673 virtual ~btSoftBodyCollisionShape()
674 {
675 }
676
processAllTriangles(btTriangleCallback *,const btVector3 &,const btVector3 &)677 void processAllTriangles(btTriangleCallback* /*callback*/, const btVector3& /*aabbMin*/, const btVector3& /*aabbMax*/) const
678 {
679 //not yet
680 btAssert(0);
681 }
682
683 ///getAabb returns the axis aligned bounding box in the coordinate frame of the given transform t.
getAabb(const btTransform & t,btVector3 & aabbMin,btVector3 & aabbMax)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
setLocalScaling(const btVector3 &)705 virtual void setLocalScaling(const btVector3& /*scaling*/)
706 {
707 ///na
708 }
getLocalScaling()709 virtual const btVector3& getLocalScaling() const
710 {
711 static const btVector3 dummy(1, 1, 1);
712 return dummy;
713 }
calculateLocalInertia(btScalar,btVector3 &)714 virtual void calculateLocalInertia(btScalar /*mass*/, btVector3& /*inertia*/) const
715 {
716 ///not yet
717 btAssert(0);
718 }
getName()719 virtual const char* getName() const
720 {
721 return "SoftBody";
722 }
723 };
724
725 //
726 // btSoftClusterCollisionShape
727 //
728 class btSoftClusterCollisionShape : public btConvexInternalShape
729 {
730 public:
731 const btSoftBody::Cluster* m_cluster;
732
btSoftClusterCollisionShape(const btSoftBody::Cluster * cluster)733 btSoftClusterCollisionShape(const btSoftBody::Cluster* cluster) : m_cluster(cluster) { setMargin(0); }
734
localGetSupportingVertex(const btVector3 & vec)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 }
localGetSupportingVertexWithoutMargin(const btVector3 & vec)751 virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const
752 {
753 return (localGetSupportingVertex(vec));
754 }
755 //notice that the vectors should be unit length
batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3 * vectors,btVector3 * supportVerticesOut,int numVectors)756 virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const
757 {
758 }
759
calculateLocalInertia(btScalar mass,btVector3 & inertia)760 virtual void calculateLocalInertia(btScalar mass, btVector3& inertia) const
761 {
762 }
763
getAabb(const btTransform & t,btVector3 & aabbMin,btVector3 & aabbMax)764 virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const
765 {
766 }
767
getShapeType()768 virtual int getShapeType() const { return SOFTBODY_SHAPE_PROXYTYPE; }
769
770 //debugging
getName()771 virtual const char* getName() const { return "SOFTCLUSTER"; }
772
setMargin(btScalar margin)773 virtual void setMargin(btScalar margin)
774 {
775 btConvexInternalShape::setMargin(margin);
776 }
getMargin()777 virtual btScalar getMargin() const
778 {
779 return btConvexInternalShape::getMargin();
780 }
781 };
782
783 //
784 // Inline's
785 //
786
787 //
788 template <typename T>
ZeroInitialize(T & value)789 static inline void ZeroInitialize(T& value)
790 {
791 memset(&value, 0, sizeof(T));
792 }
793 //
794 template <typename T>
CompLess(const T & a,const T & b)795 static inline bool CompLess(const T& a, const T& b)
796 {
797 return (a < b);
798 }
799 //
800 template <typename T>
CompGreater(const T & a,const T & b)801 static inline bool CompGreater(const T& a, const T& b)
802 {
803 return (a > b);
804 }
805 //
806 template <typename T>
Lerp(const T & a,const T & b,btScalar t)807 static inline T Lerp(const T& a, const T& b, btScalar t)
808 {
809 return (a + (b - a) * t);
810 }
811 //
812 template <typename T>
InvLerp(const T & a,const T & b,btScalar 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 //
Lerp(const btMatrix3x3 & a,const btMatrix3x3 & b,btScalar t)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 //
Clamp(const btVector3 & v,btScalar maxlength)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 //
838 template <typename T>
Clamp(const T & x,const T & l,const T & h)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 //
844 template <typename T>
Sq(const T & x)845 static inline T Sq(const T& x)
846 {
847 return (x * x);
848 }
849 //
850 template <typename T>
Cube(const T & x)851 static inline T Cube(const T& x)
852 {
853 return (x * x * x);
854 }
855 //
856 template <typename T>
Sign(const T & x)857 static inline T Sign(const T& x)
858 {
859 return ((T)(x < 0 ? -1 : +1));
860 }
861 //
862 template <typename T>
SameSign(const T & x,const T & y)863 static inline bool SameSign(const T& x, const T& y)
864 {
865 return ((x * y) > 0);
866 }
867 //
ClusterMetric(const btVector3 & x,const btVector3 & y)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 //
ScaleAlongAxis(const btVector3 & a,btScalar s)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 //
Cross(const btVector3 & v)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 //
Diagonal(btScalar x)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
Diagonal(const btVector3 & v)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
Dot(const btScalar * a,const btScalar * b,int ndof)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
OuterProduct(const btScalar * v1,const btScalar * v2,const btScalar * v3,const btScalar * u1,const btScalar * u2,const btScalar * u3,int ndof)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
OuterProduct(const btVector3 & v1,const btVector3 & v2)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 //
Add(const btMatrix3x3 & a,const btMatrix3x3 & b)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 //
Sub(const btMatrix3x3 & a,const btMatrix3x3 & b)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 //
Mul(const btMatrix3x3 & a,btScalar b)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 //
Orthogonalize(btMatrix3x3 & m)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 //
MassMatrix(btScalar im,const btMatrix3x3 & iwi,const btVector3 & r)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 //
ImpulseMatrix(btScalar dt,btScalar ima,btScalar imb,const btMatrix3x3 & iwi,const btVector3 & r)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 //
ImpulseMatrix(btScalar dt,const btMatrix3x3 & effective_mass_inv,btScalar imb,const btMatrix3x3 & iwi,const btVector3 & r)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 //
ImpulseMatrix(btScalar ima,const btMatrix3x3 & iia,const btVector3 & ra,btScalar imb,const btMatrix3x3 & iib,const btVector3 & rb)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 //
AngularImpulseMatrix(const btMatrix3x3 & iia,const btMatrix3x3 & iib)1035 static inline btMatrix3x3 AngularImpulseMatrix(const btMatrix3x3& iia,
1036 const btMatrix3x3& iib)
1037 {
1038 return (Add(iia, iib).inverse());
1039 }
1040
1041 //
ProjectOnAxis(const btVector3 & v,const btVector3 & a)1042 static inline btVector3 ProjectOnAxis(const btVector3& v,
1043 const btVector3& a)
1044 {
1045 return (a * btDot(v, a));
1046 }
1047 //
ProjectOnPlane(const btVector3 & v,const btVector3 & a)1048 static inline btVector3 ProjectOnPlane(const btVector3& v,
1049 const btVector3& a)
1050 {
1051 return (v - ProjectOnAxis(v, a));
1052 }
1053
1054 //
ProjectOrigin(const btVector3 & a,const btVector3 & b,btVector3 & prj,btScalar & sqd)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 //
ProjectOrigin(const btVector3 & a,const btVector3 & b,const btVector3 & c,btVector3 & prj,btScalar & sqd)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 //
rayIntersectsTriangle(const btVector3 & origin,const btVector3 & dir,const btVector3 & v0,const btVector3 & v1,const btVector3 & v2,btScalar & t)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
lineIntersectsTriangle(const btVector3 & rayStart,const btVector3 & rayEnd,const btVector3 & p1,const btVector3 & p2,const btVector3 & p3,btVector3 & sect,btVector3 & normal)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 //
1177 template <typename T>
BaryEval(const T & a,const T & b,const T & c,const btVector3 & coord)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 //
BaryCoord(const btVector3 & a,const btVector3 & b,const btVector3 & c,const btVector3 & p)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
EvaluateMedium(const btSoftBodyWorldInfo * wfi,const btVector3 & x,btSoftBody::sMedium & medium)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 //
NormalizeAny(const btVector3 & v)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 //
VolumeOf(const btSoftBody::Face & f,btScalar margin)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 //
CenterOf(const btSoftBody::Face & f)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 //
AreaOf(const btVector3 & x0,const btVector3 & x1,const btVector3 & x2)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 //
VolumeOf(const btVector3 & x0,const btVector3 & x1,const btVector3 & x2,const btVector3 & x3)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 //
ApplyClampedForce(btSoftBody::Node & n,const btVector3 & f,btScalar dt)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 //
MatchEdge(const btSoftBody::Node * a,const btSoftBody::Node * b,const btSoftBody::Node * ma,const btSoftBody::Node * mb)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
1386 private:
mulTPQbtEigen1387 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 }
mulPQbtEigen1396 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 //
PolarDecompose(const btMatrix3x3 & m,btMatrix3x3 & q,btMatrix3x3 & s)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;
ClusterBasebtSoftColliders::ClusterBase1432 ClusterBase()
1433 {
1434 erp = (btScalar)1;
1435 idt = 0;
1436 m_margin = 0;
1437 friction = 0;
1438 threshold = (btScalar)0;
1439 }
SolveContactbtSoftColliders::ClusterBase1440 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
ProcessbtSoftColliders::CollideCL_RS1494 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
1501 ///don't collide an anchored cluster with a static/kinematic object
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 }
ProcessColObjbtSoftColliders::CollideCL_RS1529 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();
1535 ///Bullet rigid body uses multiply instead of minimum to determine combined friction. Some customization would be useful.
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];
ProcessbtSoftColliders::CollideCL_SS1554 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 }
ProcessSoftSoftbtSoftColliders::CollideCL_SS1592 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 {
ProcessbtSoftColliders::CollideSDF_RS1608 void Process(const btDbvtNode* leaf)
1609 {
1610 btSoftBody::Node* node = (btSoftBody::Node*)leaf->data;
1611 DoNode(*node);
1612 }
DoNodebtSoftColliders::CollideSDF_RS1613 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 {
ProcessbtSoftColliders::CollideSDF_RD1660 void Process(const btDbvtNode* leaf)
1661 {
1662 btSoftBody::Node* node = (btSoftBody::Node*)leaf->data;
1663 DoNode(*node);
1664 }
DoNodebtSoftColliders::CollideSDF_RD1665 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 static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0);
1695 const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic;
1696 const btVector3 ra = n.m_x - wtr.getOrigin();
1697
1698 c.m_c0 = ImpulseMatrix(1, n.m_effectiveMass_inv, imb, iwi, ra);
1699 // c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra);
1700 c.m_c1 = ra;
1701 }
1702 else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
1703 {
1704 btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
1705 if (multibodyLinkCol)
1706 {
1707 btVector3 normal = cti.m_normal;
1708 btVector3 t1 = generateUnitOrthogonalVector(normal);
1709 btVector3 t2 = btCross(normal, t1);
1710 btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2;
1711 findJacobian(multibodyLinkCol, jacobianData_normal, c.m_node->m_x, normal);
1712 findJacobian(multibodyLinkCol, jacobianData_t1, c.m_node->m_x, t1);
1713 findJacobian(multibodyLinkCol, jacobianData_t2, c.m_node->m_x, t2);
1714
1715 btScalar* J_n = &jacobianData_normal.m_jacobians[0];
1716 btScalar* J_t1 = &jacobianData_t1.m_jacobians[0];
1717 btScalar* J_t2 = &jacobianData_t2.m_jacobians[0];
1718
1719 btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
1720 btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
1721 btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
1722
1723 btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(),
1724 t1.getX(), t1.getY(), t1.getZ(),
1725 t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame
1726 const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
1727 btMatrix3x3 local_impulse_matrix = (n.m_effectiveMass_inv + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse();
1728 c.m_c0 = rot.transpose() * local_impulse_matrix * rot;
1729 c.jacobianData_normal = jacobianData_normal;
1730 c.jacobianData_t1 = jacobianData_t1;
1731 c.jacobianData_t2 = jacobianData_t2;
1732 c.t1 = t1;
1733 c.t2 = t2;
1734 }
1735 }
1736 psb->m_nodeRigidContacts.push_back(c);
1737 }
1738 }
1739 }
1740 }
1741 btSoftBody* psb;
1742 const btCollisionObjectWrapper* m_colObj1Wrap;
1743 btRigidBody* m_rigidBody;
1744 btScalar dynmargin;
1745 btScalar stamargin;
1746 };
1747
1748 //
1749 // CollideSDF_RDF
1750 //
1751 struct CollideSDF_RDF : btDbvt::ICollide
1752 {
ProcessbtSoftColliders::CollideSDF_RDF1753 void Process(const btDbvtNode* leaf)
1754 {
1755 btSoftBody::Face* face = (btSoftBody::Face*)leaf->data;
1756 DoNode(*face);
1757 }
DoNodebtSoftColliders::CollideSDF_RDF1758 void DoNode(btSoftBody::Face& f) const
1759 {
1760 btSoftBody::Node* n0 = f.m_n[0];
1761 btSoftBody::Node* n1 = f.m_n[1];
1762 btSoftBody::Node* n2 = f.m_n[2];
1763 const btScalar m = (n0->m_im > 0 && n1->m_im > 0 && n2->m_im > 0) ? dynmargin : stamargin;
1764 btSoftBody::DeformableFaceRigidContact c;
1765 btVector3 contact_point;
1766 btVector3 bary;
1767 if (psb->checkDeformableFaceContact(m_colObj1Wrap, f, contact_point, bary, m, c.m_cti, true))
1768 {
1769 btScalar ima = n0->m_im + n1->m_im + n2->m_im;
1770 const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f;
1771 // todo: collision between multibody and fixed deformable face will be missed.
1772 const btScalar ms = ima + imb;
1773 if (ms > 0)
1774 {
1775 // resolve contact at x_n
1776 // psb->checkDeformableFaceContact(m_colObj1Wrap, f, contact_point, bary, m, c.m_cti, /*predict = */ false);
1777 btSoftBody::sCti& cti = c.m_cti;
1778 c.m_contactPoint = contact_point;
1779 c.m_bary = bary;
1780 // todo xuchenhan@: this is assuming mass of all vertices are the same. Need to modify if mass are different for distinct vertices
1781 c.m_weights = btScalar(2) / (btScalar(1) + bary.length2()) * bary;
1782 c.m_face = &f;
1783 // friction is handled by the nodes to prevent sticking
1784 // const btScalar fc = 0;
1785 const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction();
1786
1787 // the effective inverse mass of the face as in https://graphics.stanford.edu/papers/cloth-sig02/cloth.pdf
1788 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;
1789 c.m_c2 = ima;
1790 c.m_c3 = fc;
1791 c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR;
1792 c.m_c5 = Diagonal(ima);
1793 if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
1794 {
1795 const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform();
1796 static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0);
1797 const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic;
1798 const btVector3 ra = contact_point - wtr.getOrigin();
1799
1800 // we do not scale the impulse matrix by dt
1801 c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra);
1802 c.m_c1 = ra;
1803 }
1804 else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
1805 {
1806 btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
1807 if (multibodyLinkCol)
1808 {
1809 btVector3 normal = cti.m_normal;
1810 btVector3 t1 = generateUnitOrthogonalVector(normal);
1811 btVector3 t2 = btCross(normal, t1);
1812 btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2;
1813 findJacobian(multibodyLinkCol, jacobianData_normal, contact_point, normal);
1814 findJacobian(multibodyLinkCol, jacobianData_t1, contact_point, t1);
1815 findJacobian(multibodyLinkCol, jacobianData_t2, contact_point, t2);
1816
1817 btScalar* J_n = &jacobianData_normal.m_jacobians[0];
1818 btScalar* J_t1 = &jacobianData_t1.m_jacobians[0];
1819 btScalar* J_t2 = &jacobianData_t2.m_jacobians[0];
1820
1821 btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
1822 btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
1823 btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
1824
1825 btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(),
1826 t1.getX(), t1.getY(), t1.getZ(),
1827 t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame
1828 const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
1829 btMatrix3x3 local_impulse_matrix = (Diagonal(ima) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse();
1830 c.m_c0 = rot.transpose() * local_impulse_matrix * rot;
1831 c.jacobianData_normal = jacobianData_normal;
1832 c.jacobianData_t1 = jacobianData_t1;
1833 c.jacobianData_t2 = jacobianData_t2;
1834 c.t1 = t1;
1835 c.t2 = t2;
1836 }
1837 }
1838 psb->m_faceRigidContacts.push_back(c);
1839 }
1840 }
1841 // Set caching barycenters to be false after collision detection.
1842 // Only turn on when contact is static.
1843 f.m_pcontact[3] = 0;
1844 }
1845 btSoftBody* psb;
1846 const btCollisionObjectWrapper* m_colObj1Wrap;
1847 btRigidBody* m_rigidBody;
1848 btScalar dynmargin;
1849 btScalar stamargin;
1850 };
1851
1852 //
1853 // CollideVF_SS
1854 //
1855 struct CollideVF_SS : btDbvt::ICollide
1856 {
ProcessbtSoftColliders::CollideVF_SS1857 void Process(const btDbvtNode* lnode,
1858 const btDbvtNode* lface)
1859 {
1860 btSoftBody::Node* node = (btSoftBody::Node*)lnode->data;
1861 btSoftBody::Face* face = (btSoftBody::Face*)lface->data;
1862 for (int i = 0; i < 3; ++i)
1863 {
1864 if (face->m_n[i] == node)
1865 continue;
1866 }
1867
1868 btVector3 o = node->m_x;
1869 btVector3 p;
1870 btScalar d = SIMD_INFINITY;
1871 ProjectOrigin(face->m_n[0]->m_x - o,
1872 face->m_n[1]->m_x - o,
1873 face->m_n[2]->m_x - o,
1874 p, d);
1875 const btScalar m = mrg + (o - node->m_q).length() * 2;
1876 if (d < (m * m))
1877 {
1878 const btSoftBody::Node* n[] = {face->m_n[0], face->m_n[1], face->m_n[2]};
1879 const btVector3 w = BaryCoord(n[0]->m_x, n[1]->m_x, n[2]->m_x, p + o);
1880 const btScalar ma = node->m_im;
1881 btScalar mb = BaryEval(n[0]->m_im, n[1]->m_im, n[2]->m_im, w);
1882 if ((n[0]->m_im <= 0) ||
1883 (n[1]->m_im <= 0) ||
1884 (n[2]->m_im <= 0))
1885 {
1886 mb = 0;
1887 }
1888 const btScalar ms = ma + mb;
1889 if (ms > 0)
1890 {
1891 btSoftBody::SContact c;
1892 c.m_normal = p / -btSqrt(d);
1893 c.m_margin = m;
1894 c.m_node = node;
1895 c.m_face = face;
1896 c.m_weights = w;
1897 c.m_friction = btMax(psb[0]->m_cfg.kDF, psb[1]->m_cfg.kDF);
1898 c.m_cfm[0] = ma / ms * psb[0]->m_cfg.kSHR;
1899 c.m_cfm[1] = mb / ms * psb[1]->m_cfg.kSHR;
1900 psb[0]->m_scontacts.push_back(c);
1901 }
1902 }
1903 }
1904 btSoftBody* psb[2];
1905 btScalar mrg;
1906 };
1907
1908 //
1909 // CollideVF_DD
1910 //
1911 struct CollideVF_DD : btDbvt::ICollide
1912 {
ProcessbtSoftColliders::CollideVF_DD1913 void Process(const btDbvtNode* lnode,
1914 const btDbvtNode* lface)
1915 {
1916 btSoftBody::Node* node = (btSoftBody::Node*)lnode->data;
1917 btSoftBody::Face* face = (btSoftBody::Face*)lface->data;
1918 btVector3 bary;
1919 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))
1920 {
1921 const btSoftBody::Node* n[] = {face->m_n[0], face->m_n[1], face->m_n[2]};
1922 const btVector3 w = bary;
1923 const btScalar ma = node->m_im;
1924 btScalar mb = BaryEval(n[0]->m_im, n[1]->m_im, n[2]->m_im, w);
1925 if ((n[0]->m_im <= 0) ||
1926 (n[1]->m_im <= 0) ||
1927 (n[2]->m_im <= 0))
1928 {
1929 mb = 0;
1930 }
1931 const btScalar ms = ma + mb;
1932 if (ms > 0)
1933 {
1934 btSoftBody::DeformableFaceNodeContact c;
1935 c.m_normal = face->m_normal;
1936 if (!useFaceNormal && c.m_normal.dot(node->m_x - face->m_n[2]->m_x) < 0)
1937 c.m_normal = -face->m_normal;
1938 c.m_margin = mrg;
1939 c.m_node = node;
1940 c.m_face = face;
1941 c.m_bary = w;
1942 c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF;
1943 // Initialize unused fields.
1944 c.m_weights = btVector3(0, 0, 0);
1945 c.m_imf = 0;
1946 c.m_c0 = 0;
1947 psb[0]->m_faceNodeContacts.push_back(c);
1948 }
1949 }
1950 }
1951 btSoftBody* psb[2];
1952 btScalar mrg;
1953 bool useFaceNormal;
1954 };
1955
1956 //
1957 // CollideFF_DD
1958 //
1959 struct CollideFF_DD : btDbvt::ICollide
1960 {
ProcessbtSoftColliders::CollideFF_DD1961 void Process(const btDbvntNode* lface1,
1962 const btDbvntNode* lface2)
1963 {
1964 btSoftBody::Face* f1 = (btSoftBody::Face*)lface1->data;
1965 btSoftBody::Face* f2 = (btSoftBody::Face*)lface2->data;
1966 if (f1 != f2)
1967 {
1968 Repel(f1, f2);
1969 Repel(f2, f1);
1970 }
1971 }
RepelbtSoftColliders::CollideFF_DD1972 void Repel(btSoftBody::Face* f1, btSoftBody::Face* f2)
1973 {
1974 //#define REPEL_NEIGHBOR 1
1975 #ifndef REPEL_NEIGHBOR
1976 for (int node_id = 0; node_id < 3; ++node_id)
1977 {
1978 btSoftBody::Node* node = f1->m_n[node_id];
1979 for (int i = 0; i < 3; ++i)
1980 {
1981 if (f2->m_n[i] == node)
1982 return;
1983 }
1984 }
1985 #endif
1986 bool skip = false;
1987 for (int node_id = 0; node_id < 3; ++node_id)
1988 {
1989 btSoftBody::Node* node = f1->m_n[node_id];
1990 #ifdef REPEL_NEIGHBOR
1991 for (int i = 0; i < 3; ++i)
1992 {
1993 if (f2->m_n[i] == node)
1994 {
1995 skip = true;
1996 break;
1997 }
1998 }
1999 if (skip)
2000 {
2001 skip = false;
2002 continue;
2003 }
2004 #endif
2005 btSoftBody::Face* face = f2;
2006 btVector3 bary;
2007 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))
2008 continue;
2009 btSoftBody::DeformableFaceNodeContact c;
2010 c.m_normal = face->m_normal;
2011 if (!useFaceNormal && c.m_normal.dot(node->m_x - face->m_n[2]->m_x) < 0)
2012 c.m_normal = -face->m_normal;
2013 c.m_margin = mrg;
2014 c.m_node = node;
2015 c.m_face = face;
2016 c.m_bary = bary;
2017 c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF;
2018 // Initialize unused fields.
2019 c.m_weights = btVector3(0, 0, 0);
2020 c.m_imf = 0;
2021 c.m_c0 = 0;
2022 psb[0]->m_faceNodeContacts.push_back(c);
2023 }
2024 }
2025 btSoftBody* psb[2];
2026 btScalar mrg;
2027 bool useFaceNormal;
2028 };
2029
2030 struct CollideCCD : btDbvt::ICollide
2031 {
ProcessbtSoftColliders::CollideCCD2032 void Process(const btDbvtNode* lnode,
2033 const btDbvtNode* lface)
2034 {
2035 btSoftBody::Node* node = (btSoftBody::Node*)lnode->data;
2036 btSoftBody::Face* face = (btSoftBody::Face*)lface->data;
2037 btVector3 bary;
2038 if (bernsteinCCD(face, node, dt, SAFE_EPSILON, bary))
2039 {
2040 btSoftBody::DeformableFaceNodeContact c;
2041 c.m_normal = face->m_normal;
2042 if (!useFaceNormal && c.m_normal.dot(node->m_x - face->m_n[2]->m_x) < 0)
2043 c.m_normal = -face->m_normal;
2044 c.m_node = node;
2045 c.m_face = face;
2046 c.m_bary = bary;
2047 c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF;
2048 // Initialize unused fields.
2049 c.m_weights = btVector3(0, 0, 0);
2050 c.m_margin = mrg;
2051 c.m_imf = 0;
2052 c.m_c0 = 0;
2053 psb[0]->m_faceNodeContacts.push_back(c);
2054 }
2055 }
ProcessbtSoftColliders::CollideCCD2056 void Process(const btDbvntNode* lface1,
2057 const btDbvntNode* lface2)
2058 {
2059 btSoftBody::Face* f1 = (btSoftBody::Face*)lface1->data;
2060 btSoftBody::Face* f2 = (btSoftBody::Face*)lface2->data;
2061 if (f1 != f2)
2062 {
2063 Repel(f1, f2);
2064 Repel(f2, f1);
2065 }
2066 }
RepelbtSoftColliders::CollideCCD2067 void Repel(btSoftBody::Face* f1, btSoftBody::Face* f2)
2068 {
2069 //#define REPEL_NEIGHBOR 1
2070 #ifndef REPEL_NEIGHBOR
2071 for (int node_id = 0; node_id < 3; ++node_id)
2072 {
2073 btSoftBody::Node* node = f1->m_n[node_id];
2074 for (int i = 0; i < 3; ++i)
2075 {
2076 if (f2->m_n[i] == node)
2077 return;
2078 }
2079 }
2080 #endif
2081 bool skip = false;
2082 for (int node_id = 0; node_id < 3; ++node_id)
2083 {
2084 btSoftBody::Node* node = f1->m_n[node_id];
2085 #ifdef REPEL_NEIGHBOR
2086 for (int i = 0; i < 3; ++i)
2087 {
2088 if (f2->m_n[i] == node)
2089 {
2090 skip = true;
2091 break;
2092 }
2093 }
2094 if (skip)
2095 {
2096 skip = false;
2097 continue;
2098 }
2099 #endif
2100 btSoftBody::Face* face = f2;
2101 btVector3 bary;
2102 if (bernsteinCCD(face, node, dt, SAFE_EPSILON, bary))
2103 {
2104 btSoftBody::DeformableFaceNodeContact c;
2105 c.m_normal = face->m_normal;
2106 if (!useFaceNormal && c.m_normal.dot(node->m_x - face->m_n[2]->m_x) < 0)
2107 c.m_normal = -face->m_normal;
2108 c.m_node = node;
2109 c.m_face = face;
2110 c.m_bary = bary;
2111 c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF;
2112 // Initialize unused fields.
2113 c.m_weights = btVector3(0, 0, 0);
2114 c.m_margin = mrg;
2115 c.m_imf = 0;
2116 c.m_c0 = 0;
2117 psb[0]->m_faceNodeContacts.push_back(c);
2118 }
2119 }
2120 }
2121 btSoftBody* psb[2];
2122 btScalar dt, mrg;
2123 bool useFaceNormal;
2124 };
2125 };
2126 #endif //_BT_SOFT_BODY_INTERNALS_H
2127