Blender  V3.3
btSoftBodyInternals.h
Go to the documentation of this file.
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
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 */
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"
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 }
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 
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 
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 
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 
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);
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 //
637 template <typename T>
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)]); }
656  int dim;
657 };
658 
659 //
660 // btSoftBodyCollisionShape
661 //
663 {
664 public:
666 
668  {
669  m_shapeType = SOFTBODY_SHAPE_PROXYTYPE;
670  m_body = backptr;
671  }
672 
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 //
729 {
730 public:
732 
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  }
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  {
776  }
777  virtual btScalar getMargin() const
778  {
780  }
781 };
782 
783 //
784 // Inline's
785 //
786 
787 //
788 template <typename T>
789 static inline void ZeroInitialize(T& value)
790 {
791  memset(&value, 0, sizeof(T));
792 }
793 //
794 template <typename T>
795 static inline bool CompLess(const T& a, const T& b)
796 {
797  return (a < b);
798 }
799 //
800 template <typename T>
801 static inline bool CompGreater(const T& a, const T& b)
802 {
803  return (a > b);
804 }
805 //
806 template <typename 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>
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 //
838 template <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 //
844 template <typename T>
845 static inline T Sq(const T& x)
846 {
847  return (x * x);
848 }
849 //
850 template <typename T>
851 static inline T Cube(const T& x)
852 {
853  return (x * x * x);
854 }
855 //
856 template <typename T>
857 static inline T Sign(const T& x)
858 {
859  return ((T)(x < 0 ? -1 : +1));
860 }
861 //
862 template <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 //
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 //
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 //
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 //
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 //
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 //
1177 template <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 //
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 
1386 private:
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 //
1421 {
1422  //
1423  // ClusterBase
1424  //
1426  {
1433  {
1434  erp = (btScalar)1;
1435  idt = 0;
1436  m_margin = 0;
1437  friction = 0;
1438  threshold = (btScalar)0;
1439  }
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  //
1490  {
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 
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  {
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  }
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 
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  //
1552  {
1554  void Process(const btDbvtNode* la, const btDbvtNode* lb)
1555  {
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);
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  {
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  }
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  //
1607  {
1608  void Process(const btDbvtNode* leaf)
1609  {
1611  DoNode(*node);
1612  }
1613  void DoNode(btSoftBody::Node& n) const
1614  {
1615  const btScalar m = n.m_im > 0 ? dynmargin : stamargin;
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();
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;
1643  if (m_rigidBody)
1644  m_rigidBody->activate();
1645  }
1646  }
1647  }
1653  };
1654 
1655  //
1656  // CollideSDF_RD
1657  //
1659  {
1660  void Process(const btDbvtNode* leaf)
1661  {
1663  DoNode(*node);
1664  }
1665  void DoNode(btSoftBody::Node& n) const
1666  {
1667  const btScalar m = n.m_im > 0 ? dynmargin : stamargin;
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  {
1705  if (multibodyLinkCol)
1706  {
1707  btVector3 normal = cti.m_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  }
1746  };
1747 
1748  //
1749  // CollideSDF_RDF
1750  //
1752  {
1753  void Process(const btDbvtNode* leaf)
1754  {
1755  btSoftBody::Face* face = (btSoftBody::Face*)leaf->data;
1756  DoNode(*face);
1757  }
1758  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;
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  {
1807  if (multibodyLinkCol)
1808  {
1809  btVector3 normal = cti.m_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  }
1850  };
1851 
1852  //
1853  // CollideVF_SS
1854  //
1856  {
1857  void Process(const btDbvtNode* lnode,
1858  const btDbvtNode* lface)
1859  {
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  {
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  }
1906  };
1907 
1908  //
1909  // CollideVF_DD
1910  //
1912  {
1913  void Process(const btDbvtNode* lnode,
1914  const btDbvtNode* lface)
1915  {
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  {
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  psb[0]->m_faceNodeContacts.push_back(c);
1944  }
1945  }
1946  }
1950  };
1951 
1952  //
1953  // CollideFF_DD
1954  //
1956  {
1957  void Process(const btDbvntNode* lface1,
1958  const btDbvntNode* lface2)
1959  {
1960  btSoftBody::Face* f1 = (btSoftBody::Face*)lface1->data;
1961  btSoftBody::Face* f2 = (btSoftBody::Face*)lface2->data;
1962  if (f1 != f2)
1963  {
1964  Repel(f1, f2);
1965  Repel(f2, f1);
1966  }
1967  }
1969  {
1970  //#define REPEL_NEIGHBOR 1
1971 #ifndef REPEL_NEIGHBOR
1972  for (int node_id = 0; node_id < 3; ++node_id)
1973  {
1974  btSoftBody::Node* node = f1->m_n[node_id];
1975  for (int i = 0; i < 3; ++i)
1976  {
1977  if (f2->m_n[i] == node)
1978  return;
1979  }
1980  }
1981 #endif
1982  bool skip = false;
1983  for (int node_id = 0; node_id < 3; ++node_id)
1984  {
1985  btSoftBody::Node* node = f1->m_n[node_id];
1986 #ifdef REPEL_NEIGHBOR
1987  for (int i = 0; i < 3; ++i)
1988  {
1989  if (f2->m_n[i] == node)
1990  {
1991  skip = true;
1992  break;
1993  }
1994  }
1995  if (skip)
1996  {
1997  skip = false;
1998  continue;
1999  }
2000 #endif
2001  btSoftBody::Face* face = f2;
2002  btVector3 bary;
2003  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))
2004  continue;
2006  c.m_normal = face->m_normal;
2007  if (!useFaceNormal && c.m_normal.dot(node->m_x - face->m_n[2]->m_x) < 0)
2008  c.m_normal = -face->m_normal;
2009  c.m_margin = mrg;
2010  c.m_node = node;
2011  c.m_face = face;
2012  c.m_bary = bary;
2013  c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF;
2014  psb[0]->m_faceNodeContacts.push_back(c);
2015  }
2016  }
2020  };
2021 
2023  {
2024  void Process(const btDbvtNode* lnode,
2025  const btDbvtNode* lface)
2026  {
2028  btSoftBody::Face* face = (btSoftBody::Face*)lface->data;
2029  btVector3 bary;
2030  if (bernsteinCCD(face, node, dt, SAFE_EPSILON, bary))
2031  {
2033  c.m_normal = face->m_normal;
2034  if (!useFaceNormal && c.m_normal.dot(node->m_x - face->m_n[2]->m_x) < 0)
2035  c.m_normal = -face->m_normal;
2036  c.m_node = node;
2037  c.m_face = face;
2038  c.m_bary = bary;
2039  c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF;
2040  psb[0]->m_faceNodeContacts.push_back(c);
2041  }
2042  }
2043  void Process(const btDbvntNode* lface1,
2044  const btDbvntNode* lface2)
2045  {
2046  btSoftBody::Face* f1 = (btSoftBody::Face*)lface1->data;
2047  btSoftBody::Face* f2 = (btSoftBody::Face*)lface2->data;
2048  if (f1 != f2)
2049  {
2050  Repel(f1, f2);
2051  Repel(f2, f1);
2052  }
2053  }
2055  {
2056  //#define REPEL_NEIGHBOR 1
2057 #ifndef REPEL_NEIGHBOR
2058  for (int node_id = 0; node_id < 3; ++node_id)
2059  {
2060  btSoftBody::Node* node = f1->m_n[node_id];
2061  for (int i = 0; i < 3; ++i)
2062  {
2063  if (f2->m_n[i] == node)
2064  return;
2065  }
2066  }
2067 #endif
2068  bool skip = false;
2069  for (int node_id = 0; node_id < 3; ++node_id)
2070  {
2071  btSoftBody::Node* node = f1->m_n[node_id];
2072 #ifdef REPEL_NEIGHBOR
2073  for (int i = 0; i < 3; ++i)
2074  {
2075  if (f2->m_n[i] == node)
2076  {
2077  skip = true;
2078  break;
2079  }
2080  }
2081  if (skip)
2082  {
2083  skip = false;
2084  continue;
2085  }
2086 #endif
2087  btSoftBody::Face* face = f2;
2088  btVector3 bary;
2089  if (bernsteinCCD(face, node, dt, SAFE_EPSILON, bary))
2090  {
2092  c.m_normal = face->m_normal;
2093  if (!useFaceNormal && c.m_normal.dot(node->m_x - face->m_n[2]->m_x) < 0)
2094  c.m_normal = -face->m_normal;
2095  c.m_node = node;
2096  c.m_face = face;
2097  c.m_bary = bary;
2098  c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF;
2099  psb[0]->m_faceNodeContacts.push_back(c);
2100  }
2101  }
2102  }
2106  };
2107 };
2108 #endif //_BT_SOFT_BODY_INTERNALS_H
sqrt(x)+1/max(0
#define K(key)
_GL_VOID GLfloat value _GL_VOID_RET _GL_VOID const GLuint GLboolean *residences _GL_BOOL_RET _GL_VOID GLsizei GLfloat GLfloat GLfloat GLfloat const GLubyte *bitmap _GL_VOID_RET _GL_VOID GLenum const void *lists _GL_VOID_RET _GL_VOID const GLdouble *equation _GL_VOID_RET _GL_VOID GLdouble GLdouble blue _GL_VOID_RET _GL_VOID GLfloat GLfloat blue _GL_VOID_RET _GL_VOID GLint GLint blue _GL_VOID_RET _GL_VOID GLshort GLshort blue _GL_VOID_RET _GL_VOID GLubyte GLubyte blue _GL_VOID_RET _GL_VOID GLuint GLuint blue _GL_VOID_RET _GL_VOID GLushort GLushort blue _GL_VOID_RET _GL_VOID GLbyte GLbyte GLbyte alpha _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble alpha _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat alpha _GL_VOID_RET _GL_VOID GLint GLint GLint alpha _GL_VOID_RET _GL_VOID GLshort GLshort GLshort alpha _GL_VOID_RET _GL_VOID GLubyte GLubyte GLubyte alpha _GL_VOID_RET _GL_VOID GLuint GLuint GLuint alpha _GL_VOID_RET _GL_VOID GLushort GLushort GLushort alpha _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLint GLsizei GLsizei GLenum type _GL_VOID_RET _GL_VOID GLsizei GLenum GLenum const void *pixels _GL_VOID_RET _GL_VOID const void *pointer _GL_VOID_RET _GL_VOID GLdouble v _GL_VOID_RET _GL_VOID GLfloat v _GL_VOID_RET _GL_VOID GLint GLint i2 _GL_VOID_RET _GL_VOID GLint j _GL_VOID_RET _GL_VOID GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble GLdouble GLdouble zFar _GL_VOID_RET _GL_UINT GLdouble *equation _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLenum GLfloat *v _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLfloat *values _GL_VOID_RET _GL_VOID GLushort *values _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLenum GLdouble *params _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_BOOL GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLenum GLfloat param _GL_VOID_RET _GL_VOID GLenum GLint param _GL_VOID_RET _GL_VOID GLushort pattern _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLint const GLdouble *points _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLint GLdouble GLdouble GLint GLint const GLdouble *points _GL_VOID_RET _GL_VOID GLdouble GLdouble u2 _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLdouble GLdouble v2 _GL_VOID_RET _GL_VOID GLenum GLfloat param _GL_VOID_RET _GL_VOID GLenum GLint param _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLdouble GLdouble nz _GL_VOID_RET _GL_VOID GLfloat GLfloat nz _GL_VOID_RET _GL_VOID GLint GLint nz _GL_VOID_RET _GL_VOID GLshort GLshort nz _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_VOID GLsizei const GLfloat *values _GL_VOID_RET _GL_VOID GLsizei const GLushort *values _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID const GLuint const GLclampf *priorities _GL_VOID_RET _GL_VOID GLdouble y _GL_VOID_RET _GL_VOID GLfloat y _GL_VOID_RET _GL_VOID GLint y _GL_VOID_RET _GL_VOID GLshort y _GL_VOID_RET _GL_VOID GLdouble GLdouble z _GL_VOID_RET _GL_VOID GLfloat GLfloat z _GL_VOID_RET _GL_VOID GLint GLint z _GL_VOID_RET _GL_VOID GLshort GLshort z _GL_VOID_RET _GL_VOID GLdouble GLdouble z
_GL_VOID GLfloat value _GL_VOID_RET _GL_VOID const GLuint GLboolean *residences _GL_BOOL_RET _GL_VOID GLsizei GLfloat GLfloat GLfloat GLfloat const GLubyte *bitmap _GL_VOID_RET _GL_VOID GLenum const void *lists _GL_VOID_RET _GL_VOID const GLdouble *equation _GL_VOID_RET _GL_VOID GLdouble GLdouble blue _GL_VOID_RET _GL_VOID GLfloat GLfloat blue _GL_VOID_RET _GL_VOID GLint GLint blue _GL_VOID_RET _GL_VOID GLshort GLshort blue _GL_VOID_RET _GL_VOID GLubyte GLubyte blue _GL_VOID_RET _GL_VOID GLuint GLuint blue _GL_VOID_RET _GL_VOID GLushort GLushort blue _GL_VOID_RET _GL_VOID GLbyte GLbyte GLbyte alpha _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble alpha _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat alpha _GL_VOID_RET _GL_VOID GLint GLint GLint alpha _GL_VOID_RET _GL_VOID GLshort GLshort GLshort alpha _GL_VOID_RET _GL_VOID GLubyte GLubyte GLubyte alpha _GL_VOID_RET _GL_VOID GLuint GLuint GLuint alpha _GL_VOID_RET _GL_VOID GLushort GLushort GLushort alpha _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLint GLsizei GLsizei GLenum type _GL_VOID_RET _GL_VOID GLsizei GLenum GLenum const void *pixels _GL_VOID_RET _GL_VOID const void *pointer _GL_VOID_RET _GL_VOID GLdouble v _GL_VOID_RET _GL_VOID GLfloat v _GL_VOID_RET _GL_VOID GLint GLint i2 _GL_VOID_RET _GL_VOID GLint j _GL_VOID_RET _GL_VOID GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble GLdouble GLdouble zFar _GL_VOID_RET _GL_UINT GLdouble *equation _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLenum GLfloat *v _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLfloat *values _GL_VOID_RET _GL_VOID GLushort *values _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLenum GLdouble *params _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_BOOL GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLenum GLfloat param _GL_VOID_RET _GL_VOID GLenum GLint param _GL_VOID_RET _GL_VOID GLushort pattern _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLint const GLdouble *points _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLint GLdouble GLdouble GLint GLint const GLdouble *points _GL_VOID_RET _GL_VOID GLdouble GLdouble u2 _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLdouble GLdouble v2 _GL_VOID_RET _GL_VOID GLenum GLfloat param _GL_VOID_RET _GL_VOID GLenum GLint param _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLdouble GLdouble nz _GL_VOID_RET _GL_VOID GLfloat GLfloat nz _GL_VOID_RET _GL_VOID GLint GLint nz _GL_VOID_RET _GL_VOID GLshort GLshort nz _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_VOID GLsizei const GLfloat *values _GL_VOID_RET _GL_VOID GLsizei const GLushort *values _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID const GLuint const GLclampf *priorities _GL_VOID_RET _GL_VOID GLdouble y _GL_VOID_RET _GL_VOID GLfloat y _GL_VOID_RET _GL_VOID GLint y _GL_VOID_RET _GL_VOID GLshort y _GL_VOID_RET _GL_VOID GLdouble GLdouble z _GL_VOID_RET _GL_VOID GLfloat GLfloat z _GL_VOID_RET _GL_VOID GLint GLint z _GL_VOID_RET _GL_VOID GLshort GLshort z _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble w _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat w _GL_VOID_RET _GL_VOID GLint GLint GLint w _GL_VOID_RET _GL_VOID GLshort GLshort GLshort w _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble y2 _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat y2 _GL_VOID_RET _GL_VOID GLint GLint GLint y2 _GL_VOID_RET _GL_VOID GLshort GLshort GLshort y2 _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble z _GL_VOID_RET _GL_VOID GLdouble GLdouble z _GL_VOID_RET _GL_VOID GLuint *buffer _GL_VOID_RET _GL_VOID GLdouble t _GL_VOID_RET _GL_VOID GLfloat t _GL_VOID_RET _GL_VOID GLint t _GL_VOID_RET _GL_VOID GLshort t _GL_VOID_RET _GL_VOID GLdouble GLdouble r _GL_VOID_RET _GL_VOID GLfloat GLfloat r _GL_VOID_RET _GL_VOID GLint GLint r _GL_VOID_RET _GL_VOID GLshort GLshort r _GL_VOID_RET _GL_VOID GLdouble GLdouble r
_GL_VOID GLfloat value _GL_VOID_RET _GL_VOID const GLuint GLboolean *residences _GL_BOOL_RET _GL_VOID GLsizei GLfloat GLfloat GLfloat GLfloat const GLubyte *bitmap _GL_VOID_RET _GL_VOID GLenum const void *lists _GL_VOID_RET _GL_VOID const GLdouble *equation _GL_VOID_RET _GL_VOID GLdouble GLdouble blue _GL_VOID_RET _GL_VOID GLfloat GLfloat blue _GL_VOID_RET _GL_VOID GLint GLint blue _GL_VOID_RET _GL_VOID GLshort GLshort blue _GL_VOID_RET _GL_VOID GLubyte GLubyte blue _GL_VOID_RET _GL_VOID GLuint GLuint blue _GL_VOID_RET _GL_VOID GLushort GLushort blue _GL_VOID_RET _GL_VOID GLbyte GLbyte GLbyte alpha _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble alpha _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat alpha _GL_VOID_RET _GL_VOID GLint GLint GLint alpha _GL_VOID_RET _GL_VOID GLshort GLshort GLshort alpha _GL_VOID_RET _GL_VOID GLubyte GLubyte GLubyte alpha _GL_VOID_RET _GL_VOID GLuint GLuint GLuint alpha _GL_VOID_RET _GL_VOID GLushort GLushort GLushort alpha _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLint GLsizei GLsizei GLenum type _GL_VOID_RET _GL_VOID GLsizei GLenum GLenum const void *pixels _GL_VOID_RET _GL_VOID const void *pointer _GL_VOID_RET _GL_VOID GLdouble v _GL_VOID_RET _GL_VOID GLfloat v _GL_VOID_RET _GL_VOID GLint GLint i2 _GL_VOID_RET _GL_VOID GLint j _GL_VOID_RET _GL_VOID GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble GLdouble GLdouble zFar _GL_VOID_RET _GL_UINT GLdouble *equation _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLenum GLfloat *v _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLfloat *values _GL_VOID_RET _GL_VOID GLushort *values _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLenum GLdouble *params _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_BOOL GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLenum GLfloat param _GL_VOID_RET _GL_VOID GLenum GLint param _GL_VOID_RET _GL_VOID GLushort pattern _GL_VOID_RET _GL_VOID GLdouble GLdouble u2
_GL_VOID GLfloat value _GL_VOID_RET _GL_VOID const GLuint GLboolean *residences _GL_BOOL_RET _GL_VOID GLsizei GLfloat GLfloat GLfloat GLfloat const GLubyte *bitmap _GL_VOID_RET _GL_VOID GLenum const void *lists _GL_VOID_RET _GL_VOID const GLdouble *equation _GL_VOID_RET _GL_VOID GLdouble GLdouble blue _GL_VOID_RET _GL_VOID GLfloat GLfloat blue _GL_VOID_RET _GL_VOID GLint GLint blue _GL_VOID_RET _GL_VOID GLshort GLshort blue _GL_VOID_RET _GL_VOID GLubyte GLubyte blue _GL_VOID_RET _GL_VOID GLuint GLuint blue _GL_VOID_RET _GL_VOID GLushort GLushort blue _GL_VOID_RET _GL_VOID GLbyte GLbyte GLbyte alpha _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble alpha _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat alpha _GL_VOID_RET _GL_VOID GLint GLint GLint alpha _GL_VOID_RET _GL_VOID GLshort GLshort GLshort alpha _GL_VOID_RET _GL_VOID GLubyte GLubyte GLubyte alpha _GL_VOID_RET _GL_VOID GLuint GLuint GLuint alpha _GL_VOID_RET _GL_VOID GLushort GLushort GLushort alpha _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLint GLsizei GLsizei GLenum type _GL_VOID_RET _GL_VOID GLsizei GLenum GLenum const void *pixels _GL_VOID_RET _GL_VOID const void *pointer _GL_VOID_RET _GL_VOID GLdouble v _GL_VOID_RET _GL_VOID GLfloat v _GL_VOID_RET _GL_VOID GLint GLint i2 _GL_VOID_RET _GL_VOID GLint j _GL_VOID_RET _GL_VOID GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble GLdouble GLdouble zFar _GL_VOID_RET _GL_UINT GLdouble *equation _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLenum GLfloat *v _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLfloat *values _GL_VOID_RET _GL_VOID GLushort *values _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLenum GLdouble *params _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_BOOL GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLenum GLfloat param _GL_VOID_RET _GL_VOID GLenum GLint param _GL_VOID_RET _GL_VOID GLushort pattern _GL_VOID_RET _GL_VOID GLdouble u1
_GL_VOID GLfloat value _GL_VOID_RET _GL_VOID const GLuint GLboolean *residences _GL_BOOL_RET _GL_VOID GLsizei GLfloat GLfloat GLfloat GLfloat const GLubyte *bitmap _GL_VOID_RET _GL_VOID GLenum const void *lists _GL_VOID_RET _GL_VOID const GLdouble *equation _GL_VOID_RET _GL_VOID GLdouble GLdouble blue _GL_VOID_RET _GL_VOID GLfloat GLfloat blue _GL_VOID_RET _GL_VOID GLint GLint blue _GL_VOID_RET _GL_VOID GLshort GLshort blue _GL_VOID_RET _GL_VOID GLubyte GLubyte blue _GL_VOID_RET _GL_VOID GLuint GLuint blue _GL_VOID_RET _GL_VOID GLushort GLushort blue _GL_VOID_RET _GL_VOID GLbyte GLbyte GLbyte alpha _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble alpha _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat alpha _GL_VOID_RET _GL_VOID GLint GLint GLint alpha _GL_VOID_RET _GL_VOID GLshort GLshort GLshort alpha _GL_VOID_RET _GL_VOID GLubyte GLubyte GLubyte alpha _GL_VOID_RET _GL_VOID GLuint GLuint GLuint alpha _GL_VOID_RET _GL_VOID GLushort GLushort GLushort alpha _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLint y
_GL_VOID GLfloat value _GL_VOID_RET _GL_VOID const GLuint GLboolean *residences _GL_BOOL_RET _GL_VOID GLsizei GLfloat GLfloat GLfloat GLfloat const GLubyte *bitmap _GL_VOID_RET _GL_VOID GLenum const void *lists _GL_VOID_RET _GL_VOID const GLdouble *equation _GL_VOID_RET _GL_VOID GLdouble GLdouble blue _GL_VOID_RET _GL_VOID GLfloat GLfloat blue _GL_VOID_RET _GL_VOID GLint GLint blue _GL_VOID_RET _GL_VOID GLshort GLshort blue _GL_VOID_RET _GL_VOID GLubyte GLubyte blue _GL_VOID_RET _GL_VOID GLuint GLuint blue _GL_VOID_RET _GL_VOID GLushort GLushort blue _GL_VOID_RET _GL_VOID GLbyte GLbyte GLbyte alpha _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble alpha _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat alpha _GL_VOID_RET _GL_VOID GLint GLint GLint alpha _GL_VOID_RET _GL_VOID GLshort GLshort GLshort alpha _GL_VOID_RET _GL_VOID GLubyte GLubyte GLubyte alpha _GL_VOID_RET _GL_VOID GLuint GLuint GLuint alpha _GL_VOID_RET _GL_VOID GLushort GLushort GLushort alpha _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLint GLsizei GLsizei GLenum type _GL_VOID_RET _GL_VOID GLsizei GLenum GLenum const void *pixels _GL_VOID_RET _GL_VOID const void *pointer _GL_VOID_RET _GL_VOID GLdouble v _GL_VOID_RET _GL_VOID GLfloat v _GL_VOID_RET _GL_VOID GLint GLint i2 _GL_VOID_RET _GL_VOID GLint j _GL_VOID_RET _GL_VOID GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble GLdouble GLdouble zFar _GL_VOID_RET _GL_UINT GLdouble *equation _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLenum GLfloat *v _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLfloat *values _GL_VOID_RET _GL_VOID GLushort *values _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLenum GLdouble *params _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_BOOL GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLenum GLfloat param _GL_VOID_RET _GL_VOID GLenum GLint param _GL_VOID_RET _GL_VOID GLushort pattern _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLint const GLdouble *points _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLint GLdouble GLdouble GLint GLint const GLdouble *points _GL_VOID_RET _GL_VOID GLdouble GLdouble u2 _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLdouble GLdouble v2 _GL_VOID_RET _GL_VOID GLenum GLfloat param _GL_VOID_RET _GL_VOID GLenum GLint param _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLdouble GLdouble nz _GL_VOID_RET _GL_VOID GLfloat GLfloat nz _GL_VOID_RET _GL_VOID GLint GLint nz _GL_VOID_RET _GL_VOID GLshort GLshort nz _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_VOID GLsizei const GLfloat *values _GL_VOID_RET _GL_VOID GLsizei const GLushort *values _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID const GLuint const GLclampf *priorities _GL_VOID_RET _GL_VOID GLdouble y _GL_VOID_RET _GL_VOID GLfloat y _GL_VOID_RET _GL_VOID GLint y _GL_VOID_RET _GL_VOID GLshort y _GL_VOID_RET _GL_VOID GLdouble GLdouble z _GL_VOID_RET _GL_VOID GLfloat GLfloat z _GL_VOID_RET _GL_VOID GLint GLint z _GL_VOID_RET _GL_VOID GLshort GLshort z _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble w _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat w _GL_VOID_RET _GL_VOID GLint GLint GLint w _GL_VOID_RET _GL_VOID GLshort GLshort GLshort w _GL_VOID_RET _GL_VOID GLdouble GLdouble x2
_GL_VOID GLfloat value _GL_VOID_RET _GL_VOID const GLuint GLboolean *residences _GL_BOOL_RET _GL_VOID GLsizei GLfloat GLfloat GLfloat GLfloat const GLubyte *bitmap _GL_VOID_RET _GL_VOID GLenum const void *lists _GL_VOID_RET _GL_VOID const GLdouble *equation _GL_VOID_RET _GL_VOID GLdouble GLdouble blue _GL_VOID_RET _GL_VOID GLfloat GLfloat blue _GL_VOID_RET _GL_VOID GLint GLint blue _GL_VOID_RET _GL_VOID GLshort GLshort blue _GL_VOID_RET _GL_VOID GLubyte GLubyte blue _GL_VOID_RET _GL_VOID GLuint GLuint blue _GL_VOID_RET _GL_VOID GLushort GLushort blue _GL_VOID_RET _GL_VOID GLbyte GLbyte GLbyte alpha _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble alpha _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat alpha _GL_VOID_RET _GL_VOID GLint GLint GLint alpha _GL_VOID_RET _GL_VOID GLshort GLshort GLshort alpha _GL_VOID_RET _GL_VOID GLubyte GLubyte GLubyte alpha _GL_VOID_RET _GL_VOID GLuint GLuint GLuint alpha _GL_VOID_RET _GL_VOID GLushort GLushort GLushort alpha _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLint GLsizei GLsizei GLenum type _GL_VOID_RET _GL_VOID GLsizei GLenum GLenum const void *pixels _GL_VOID_RET _GL_VOID const void *pointer _GL_VOID_RET _GL_VOID GLdouble v _GL_VOID_RET _GL_VOID GLfloat v _GL_VOID_RET _GL_VOID GLint GLint i2 _GL_VOID_RET _GL_VOID GLint j _GL_VOID_RET _GL_VOID GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble GLdouble GLdouble zFar _GL_VOID_RET _GL_UINT GLdouble *equation _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLenum GLfloat *v _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLfloat *values _GL_VOID_RET _GL_VOID GLushort *values _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLenum GLdouble *params _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_BOOL GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLenum GLfloat param _GL_VOID_RET _GL_VOID GLenum GLint param _GL_VOID_RET _GL_VOID GLushort pattern _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLint const GLdouble *points _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLint GLdouble GLdouble GLint GLint const GLdouble *points _GL_VOID_RET _GL_VOID GLdouble GLdouble u2 _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLdouble GLdouble v2 _GL_VOID_RET _GL_VOID GLenum GLfloat param _GL_VOID_RET _GL_VOID GLenum GLint param _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLdouble GLdouble nz _GL_VOID_RET _GL_VOID GLfloat GLfloat nz _GL_VOID_RET _GL_VOID GLint GLint nz _GL_VOID_RET _GL_VOID GLshort GLshort nz _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_VOID GLsizei const GLfloat *values _GL_VOID_RET _GL_VOID GLsizei const GLushort *values _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID const GLuint const GLclampf *priorities _GL_VOID_RET _GL_VOID GLdouble y _GL_VOID_RET _GL_VOID GLfloat y _GL_VOID_RET _GL_VOID GLint y _GL_VOID_RET _GL_VOID GLshort y _GL_VOID_RET _GL_VOID GLdouble GLdouble z _GL_VOID_RET _GL_VOID GLfloat GLfloat z _GL_VOID_RET _GL_VOID GLint GLint z _GL_VOID_RET _GL_VOID GLshort GLshort z _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble w _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat w _GL_VOID_RET _GL_VOID GLint GLint GLint w _GL_VOID_RET _GL_VOID GLshort GLshort GLshort w _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble y2 _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat y2 _GL_VOID_RET _GL_VOID GLint GLint GLint y2 _GL_VOID_RET _GL_VOID GLshort GLshort GLshort y2 _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble z _GL_VOID_RET _GL_VOID GLdouble GLdouble z _GL_VOID_RET _GL_VOID GLuint *buffer _GL_VOID_RET _GL_VOID GLdouble t _GL_VOID_RET _GL_VOID GLfloat t _GL_VOID_RET _GL_VOID GLint t _GL_VOID_RET _GL_VOID GLshort t _GL_VOID_RET _GL_VOID GLdouble t
_GL_VOID GLfloat value _GL_VOID_RET _GL_VOID const GLuint GLboolean *residences _GL_BOOL_RET _GL_VOID GLsizei GLfloat GLfloat GLfloat GLfloat const GLubyte *bitmap _GL_VOID_RET _GL_VOID GLenum const void *lists _GL_VOID_RET _GL_VOID const GLdouble *equation _GL_VOID_RET _GL_VOID GLdouble GLdouble blue _GL_VOID_RET _GL_VOID GLfloat GLfloat blue _GL_VOID_RET _GL_VOID GLint GLint blue _GL_VOID_RET _GL_VOID GLshort GLshort blue _GL_VOID_RET _GL_VOID GLubyte GLubyte blue _GL_VOID_RET _GL_VOID GLuint GLuint blue _GL_VOID_RET _GL_VOID GLushort GLushort blue _GL_VOID_RET _GL_VOID GLbyte GLbyte GLbyte alpha _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble alpha _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat alpha _GL_VOID_RET _GL_VOID GLint GLint GLint alpha _GL_VOID_RET _GL_VOID GLshort GLshort GLshort alpha _GL_VOID_RET _GL_VOID GLubyte GLubyte GLubyte alpha _GL_VOID_RET _GL_VOID GLuint GLuint GLuint alpha _GL_VOID_RET _GL_VOID GLushort GLushort GLushort alpha _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLint GLsizei GLsizei GLenum type _GL_VOID_RET _GL_VOID GLsizei GLenum GLenum const void *pixels _GL_VOID_RET _GL_VOID const void *pointer _GL_VOID_RET _GL_VOID GLdouble v _GL_VOID_RET _GL_VOID GLfloat v _GL_VOID_RET _GL_VOID GLint GLint i2 _GL_VOID_RET _GL_VOID GLint j _GL_VOID_RET _GL_VOID GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble GLdouble GLdouble zFar _GL_VOID_RET _GL_UINT GLdouble *equation _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLenum GLfloat *v _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLfloat *values _GL_VOID_RET _GL_VOID GLushort *values _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLenum GLdouble *params _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_BOOL GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLenum GLfloat param _GL_VOID_RET _GL_VOID GLenum GLint param _GL_VOID_RET _GL_VOID GLushort pattern _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLint const GLdouble *points _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLint GLdouble v1
ATTR_WARN_UNUSED_RESULT const BMVert * v2
ATTR_WARN_UNUSED_RESULT const BMLoop * l
ATTR_WARN_UNUSED_RESULT const BMVert const BMEdge * e
ATTR_WARN_UNUSED_RESULT const BMVert * v
#define btAlignedAlloc(size, alignment)
virtual void setMargin(btScalar collisionMargin)
Definition: btBox2dShape.h:110
@ SOFTBODY_SHAPE_PROXYTYPE
@ CO_FEATHERSTONE_LINK
@ CO_RIGID_BODY
virtual btScalar getMargin() const =0
btConcaveShape()
btConvexInternalShape()
btConvexShape()
not supported on IBM SDK, until we fix the alignment of btVector3
btMatrix3x3 inverse() const
Return the inverse of the matrix.
Definition: btTransform.h:182
btMatrix3x3
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:50
static const btMatrix3x3 & getIdentity()
Definition: btMatrix3x3.h:350
SIMD_FORCE_INLINE const T & btMin(const T &a, const T &b)
Definition: btMinMax.h:21
SIMD_FORCE_INLINE const T & btMax(const T &a, const T &b)
Definition: btMinMax.h:27
SIMD_FORCE_INLINE const btScalar & w() const
Return the w value.
Definition: btQuadWord.h:119
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
#define ATTRIBUTE_ALIGNED16(a)
Definition: btScalar.h:285
SIMD_FORCE_INLINE btScalar btFabs(btScalar x)
Definition: btScalar.h:497
SIMD_FORCE_INLINE btScalar btSqrt(btScalar y)
Definition: btScalar.h:466
#define SIMD_INFINITY
Definition: btScalar.h:544
#define SIMD_FORCE_INLINE
Definition: btScalar.h:280
SIMD_FORCE_INLINE void btSwap(T &a, T &b)
Definition: btScalar.h:643
#define SIMD_EPSILON
Definition: btScalar.h:543
#define btAssert(x)
Definition: btScalar.h:295
static btVector3 ProjectOnAxis(const btVector3 &v, const btVector3 &a)
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)
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)
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)
static bool CompGreater(const T &a, const T &b)
static btMatrix3x3 OuterProduct(const btScalar *v1, const btScalar *v2, const btScalar *v3, const btScalar *u1, const btScalar *u2, const btScalar *u3, int ndof)
static btMatrix3x3 Mul(const btMatrix3x3 &a, btScalar b)
static btScalar ImplicitSolve(btSoftBody::ImplicitFn *fn, const btVector3 &a, const btVector3 &b, const btScalar accuracy, const int maxiterations=256)
static void Orthogonalize(btMatrix3x3 &m)
static btMatrix3x3 Cross(const btVector3 &v)
static SIMD_FORCE_INLINE bool diffSign(const btScalar &a, const btScalar &b)
static btMatrix3x3 MassMatrix(btScalar im, const btMatrix3x3 &iwi, const btVector3 &r)
static btDbvtVolume VolumeOf(const btSoftBody::Face &f, btScalar margin)
static SIMD_FORCE_INLINE bool rootFindingLemma(const btScalar &k0, const btScalar &k1, const btScalar &k2, const btScalar &k3)
static T Lerp(const T &a, const T &b, btScalar t)
static btMatrix3x3 AngularImpulseMatrix(const btMatrix3x3 &iia, const btMatrix3x3 &iib)
static SIMD_FORCE_INLINE bool bernsteinCCD(const btSoftBody::Face *face, const btSoftBody::Node *node, const btScalar &dt, const btScalar &mrg, btVector3 &bary)
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)
static btScalar AreaOf(const btVector3 &x0, const btVector3 &x1, const btVector3 &x2)
static T Sq(const T &x)
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)
static btVector3 NormalizeAny(const btVector3 &v)
static int PolarDecompose(const btMatrix3x3 &m, btMatrix3x3 &q, btMatrix3x3 &s)
static bool lineIntersectsTriangle(const btVector3 &rayStart, const btVector3 &rayEnd, const btVector3 &p1, const btVector3 &p2, const btVector3 &p3, btVector3 &sect, btVector3 &normal)
static int MatchEdge(const btSoftBody::Node *a, const btSoftBody::Node *b, const btSoftBody::Node *ma, const btSoftBody::Node *mb)
static const int KDOP_COUNT
static SIMD_FORCE_INLINE btVector3 generateUnitOrthogonalVector(const btVector3 &u)
static SIMD_FORCE_INLINE bool sameSign(const btScalar &a, const btScalar &b)
static bool CompLess(const T &a, const T &b)
static T BaryEval(const T &a, const T &b, const T &c, const btVector3 &coord)
static btVector3 CenterOf(const btSoftBody::Face &f)
static int getSign(const btVector3 &n, const btVector3 &x)
static bool SameSign(const T &x, const T &y)
static void EvaluateMedium(const btSoftBodyWorldInfo *wfi, const btVector3 &x, btSoftBody::sMedium &medium)
btScalar evaluateBezier2(const btScalar &p0, const btScalar &p1, const btScalar &p2, const btScalar &t, const btScalar &s)
static btVector3 BaryCoord(const btVector3 &a, const btVector3 &b, const btVector3 &c, const btVector3 &p)
static btMatrix3x3 ScaleAlongAxis(const btVector3 &a, btScalar s)
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)
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)
static void ZeroInitialize(T &value)
static btMatrix3x3 Add(const btMatrix3x3 &a, const btMatrix3x3 &b)
static T InvLerp(const T &a, const T &b, btScalar t)
static btVector3 ProjectOnPlane(const btVector3 &v, const btVector3 &a)
static btVector3 dop[KDOP_COUNT]
static btScalar ClusterMetric(const btVector3 &x, const btVector3 &y)
static SIMD_FORCE_INLINE bool nearZero(const btScalar &a)
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)
static btMatrix3x3 ImpulseMatrix(btScalar dt, btScalar ima, btScalar imb, const btMatrix3x3 &iwi, const btVector3 &r)
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)
static void ApplyClampedForce(btSoftBody::Node &n, const btVector3 &f, btScalar dt)
static bool rayIntersectsTriangle(const btVector3 &origin, const btVector3 &dir, const btVector3 &v0, const btVector3 &v1, const btVector3 &v2, btScalar &t)
static SIMD_FORCE_INLINE void findJacobian(const btMultiBodyLinkCollider *multibodyLinkCol, btMultiBodyJacobianData &jacobianData, const btVector3 &contact_point, const btVector3 &dir)
btSoftBody implementation by Nathanael Presson
static SIMD_FORCE_INLINE bool conservativeCulling(const btScalar &k0, const btScalar &k1, const btScalar &k2, const btScalar &k3, const btScalar &mrg)
static void ProjectOrigin(const btVector3 &a, const btVector3 &b, btVector3 &prj, btScalar &sqd)
static SIMD_FORCE_INLINE bool continuousCollisionDetection(const btSoftBody::Face *face, const btSoftBody::Node *node, const btScalar &dt, const btScalar &mrg, btVector3 &bary)
static btScalar Dot(const btScalar *a, const btScalar *b, int ndof)
static btMatrix3x3 Sub(const btMatrix3x3 &a, const btMatrix3x3 &b)
static T Sign(const T &x)
static btVector3 Clamp(const btVector3 &v, btScalar maxlength)
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)
btScalar evaluateBezier(const btScalar &p0, const btScalar &p1, const btScalar &p2, const btScalar &p3, const btScalar &t, const btScalar &s)
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)
static SIMD_FORCE_INLINE bool hasSeparatingPlane(const btSoftBody::Face *face, const btSoftBody::Node *node, const btScalar &dt)
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)
static btMatrix3x3 Diagonal(btScalar x)
static T Cube(const T &x)
#define SAFE_EPSILON
Definition: btSoftBody.h:1194
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:30
SIMD_FORCE_INLINE btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:251
SIMD_FORCE_INLINE btScalar norm() const
Return the norm (length) of the vector.
Definition: btVector3.h:263
SIMD_FORCE_INLINE btScalar btDot(const btVector3 &v1, const btVector3 &v2)
Return the dot product between two vectors.
Definition: btVector3.h:890
btVector3
btVector3 can be used to represent 3D points and vectors. It has an un-used w component to suit 16-by...
Definition: btVector3.h:82
SIMD_FORCE_INLINE btVector3 btCross(const btVector3 &v1, const btVector3 &v2)
Return the cross product of two vectors.
Definition: btVector3.h:918
SIMD_FORCE_INLINE int size() const
return the number of elements in the array
SIMD_FORCE_INLINE void resize(int newsize, const T &fillData=T())
SIMD_FORCE_INLINE void push_back(const T &_Val)
static btMultiBodyLinkCollider * upcast(btCollisionObject *colObj)
unsigned int decompose(const btMatrix3x3 &a, btMatrix3x3 &u, btMatrix3x3 &h) const
btVector3 getVelocityInLocalPoint(const btVector3 &rel_pos) const
Definition: btRigidBody.h:460
btScalar getInvMass() const
Definition: btRigidBody.h:263
const btMatrix3x3 & getInvInertiaTensorWorld() const
Definition: btRigidBody.h:265
virtual void calculateLocalInertia(btScalar, btVector3 &) const
void processAllTriangles(btTriangleCallback *, const btVector3 &, const btVector3 &) const
virtual void setLocalScaling(const btVector3 &)
virtual const char * getName() const
btSoftBodyCollisionShape(btSoftBody *backptr)
virtual const btVector3 & getLocalScaling() const
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.
btDbvt m_cdbvt
Definition: btSoftBody.h:822
tJointArray m_joints
Definition: btSoftBody.h:814
btAlignedObjectArray< bool > m_clusterConnectivity
Definition: btSoftBody.h:838
SolverState m_sst
Definition: btSoftBody.h:794
btAlignedObjectArray< DeformableFaceNodeContact > m_faceNodeContacts
Definition: btSoftBody.h:811
bool checkDeformableContact(const btCollisionObjectWrapper *colObjWrap, const btVector3 &x, btScalar margin, btSoftBody::sCti &cti, bool predict=false) const
Config m_cfg
Definition: btSoftBody.h:793
tRContactArray m_rcontacts
Definition: btSoftBody.h:809
btVector3 m_bounds[2]
Definition: btSoftBody.h:817
tClusterArray m_clusters
Definition: btSoftBody.h:823
btAlignedObjectArray< DeformableFaceRigidContact > m_faceRigidContacts
Definition: btSoftBody.h:812
bool checkDeformableFaceContact(const btCollisionObjectWrapper *colObjWrap, Face &f, btVector3 &contact_point, btVector3 &bary, btScalar margin, btSoftBody::sCti &cti, bool predict=false) const
btAlignedObjectArray< DeformableNodeRigidContact > m_nodeRigidContacts
Definition: btSoftBody.h:810
tSContactArray m_scontacts
Definition: btSoftBody.h:813
bool checkContact(const btCollisionObjectWrapper *colObjWrap, const btVector3 &x, btScalar margin, btSoftBody::sCti &cti) const
virtual void setMargin(btScalar margin)
virtual void getAabb(const btTransform &t, btVector3 &aabbMin, btVector3 &aabbMax) const
btSoftClusterCollisionShape(const btSoftBody::Cluster *cluster)
virtual int getShapeType() const
virtual void calculateLocalInertia(btScalar mass, btVector3 &inertia) const
virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3 &vec) const
virtual const char * getName() const
virtual btScalar getMargin() const
const btSoftBody::Cluster * m_cluster
virtual btVector3 localGetSupportingVertex(const btVector3 &vec) const
virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3 *vectors, btVector3 *supportVerticesOut, int numVectors) const
OperationNode * node
#define rot(x, k)
IconTextureDrawCall normal
int count
#define T
#define L
static unsigned c
Definition: RandGen.cpp:83
Segment< FEdge *, Vec3r > segment
static unsigned a[3]
Definition: RandGen.cpp:78
static void area(int d1, int d2, int e1, int e2, float weights[2])
T dot(const vec_base< T, Size > &a, const vec_base< T, Size > &b)
vec_base< T, 3 > cross(const vec_base< T, 3 > &a, const vec_base< T, 3 > &b)
T length(const vec_base< T, Size > &a)
T abs(const T &a)
static const pxr::TfToken b("b", pxr::TfToken::Immortal)
const btScalar eps
Definition: poly34.cpp:11
int SolveP3(btScalar *x, btScalar a, btScalar b, btScalar c)
Definition: poly34.cpp:71
int SolveP2(btScalar *x, btScalar a, btScalar b)
Definition: poly34.cpp:52
return ret
SIMD_FORCE_INLINE const btCollisionShape * getCollisionShape() const
SIMD_FORCE_INLINE const btTransform & getWorldTransform() const
SIMD_FORCE_INLINE const btCollisionObject * getCollisionObject() const
void * data
Definition: btDbvt.h:202
static btDbvtAabbMm FromMM(const btVector3 &mi, const btVector3 &mx)
Definition: btDbvt.h:479
static btDbvtAabbMm FromPoints(const btVector3 *pts, int n)
Definition: btDbvt.h:488
DBVT_INLINE void Expand(const btVector3 &e)
Definition: btDbvt.h:514
void * data
Definition: btDbvt.h:188
DBVT_PREFIX void collideTV(const btDbvtNode *root, const btDbvtVolume &volume, DBVT_IPOLICY) const
Definition: btDbvt.h:1148
btDbvtNode * m_root
Definition: btDbvt.h:302
DBVT_PREFIX void collideTT(const btDbvtNode *root0, const btDbvtNode *root1, DBVT_IPOLICY)
Definition: btDbvt.h:822
static int system(btMatrix3x3 &a, btMatrix3x3 *vectors, btVector3 *values=0)
btVector3 witnesses[2]
Definition: btGjkEpa2.h:42
static btScalar SignedDistance(const btVector3 &position, btScalar margin, const btConvexShape *shape, const btTransform &wtrs, sResults &results)
Definition: btGjkEpa2.cpp:1021
btAlignedObjectArray< btScalar > m_deltaVelocitiesUnitImpulse
btAlignedObjectArray< btScalar > m_jacobians
btAlignedObjectArray< btScalar > scratch_r
btAlignedObjectArray< btMatrix3x3 > scratch_m
btAlignedObjectArray< btVector3 > scratch_v
btScalar air_density
Definition: btSoftBody.h:49
btScalar water_density
Definition: btSoftBody.h:50
btVector3 m_gravity
Definition: btSoftBody.h:56
btVector3 water_normal
Definition: btSoftBody.h:53
btScalar water_offset
Definition: btSoftBody.h:51
btScalar invMass() const
Definition: btSoftBody.h:528
const btTransform & xform() const
Definition: btSoftBody.h:534
btVector3 velocity(const btVector3 &rpos) const
Definition: btSoftBody.h:559
const btMatrix3x3 & invWorldInertia() const
Definition: btSoftBody.h:521
btVector3 m_rpos[2]
Definition: btSoftBody.h:687
btVector3 m_normal
Definition: btSoftBody.h:688
btScalar m_friction
Definition: btSoftBody.h:689
btAlignedObjectArray< Node * > m_nodes
Definition: btSoftBody.h:449
btScalar kSK_SPLT_CL
Definition: btSoftBody.h:715
btScalar kSS_SPLT_CL
Definition: btSoftBody.h:716
btScalar kSR_SPLT_CL
Definition: btSoftBody.h:714
btVector4 m_pcontact
Definition: btSoftBody.h:300
btVector3 m_normal
Definition: btSoftBody.h:297
btVector3 m_n0
Definition: btSoftBody.h:301
btVector3 m_n1
Definition: btSoftBody.h:301
btVector3 m_vn
Definition: btSoftBody.h:301
Node * m_n[3]
Definition: btSoftBody.h:296
virtual btScalar Eval(const btVector3 &x)=0
btVector3 m_drift
Definition: btSoftBody.h:631
btScalar m_split
Definition: btSoftBody.h:630
btMatrix3x3 m_massmatrix
Definition: btSoftBody.h:633
btVector3 m_refs[2]
Definition: btSoftBody.h:627
btVector3 m_x
Definition: btSoftBody.h:263
btVector3 m_v
Definition: btSoftBody.h:265
btVector3 m_q
Definition: btSoftBody.h:264
btVector3 m_n
Definition: btSoftBody.h:268
btVector3 m_f
Definition: btSoftBody.h:267
btMatrix3x3 m_effectiveMass_inv
Definition: btSoftBody.h:277
const btCollisionObject * m_colObj
Definition: btSoftBody.h:226
btVector3 m_normal
Definition: btSoftBody.h:227
btVector3 m_velocity
Definition: btSoftBody.h:235
bool SolveContact(const btGjkEpaSolver2::sResults &res, btSoftBody::Body ba, const btSoftBody::Body bb, btSoftBody::CJoint &joint)
void Process(const btDbvtNode *lnode, const btDbvtNode *lface)
void Repel(btSoftBody::Face *f1, btSoftBody::Face *f2)
void Process(const btDbvntNode *lface1, const btDbvntNode *lface2)
void Process(const btDbvtNode *leaf)
void ProcessColObj(btSoftBody *ps, const btCollisionObjectWrapper *colObWrap)
const btCollisionObjectWrapper * m_colObjWrap
void Process(const btDbvtNode *la, const btDbvtNode *lb)
void ProcessSoftSoft(btSoftBody *psa, btSoftBody *psb)
void Repel(btSoftBody::Face *f1, btSoftBody::Face *f2)
void Process(const btDbvntNode *lface1, const btDbvntNode *lface2)
void DoNode(btSoftBody::Face &f) const
void Process(const btDbvtNode *leaf)
const btCollisionObjectWrapper * m_colObj1Wrap
void Process(const btDbvtNode *leaf)
const btCollisionObjectWrapper * m_colObj1Wrap
void DoNode(btSoftBody::Node &n) const
void DoNode(btSoftBody::Node &n) const
void Process(const btDbvtNode *leaf)
const btCollisionObjectWrapper * m_colObj1Wrap
void Process(const btDbvtNode *lnode, const btDbvtNode *lface)
void Process(const btDbvtNode *lnode, const btDbvtNode *lface)
T & operator()(int c, int r)
int index(int c, int r) const
btAlignedObjectArray< T > store
void resize(int n, const T &init=T())
btSymMatrix(int n, const T &init=T())
const T & operator()(int c, int r) const
static const char hex[17]
Definition: thumbs.c:156
int xy[2]
Definition: wm_draw.c:135