Bullet Collision Detection & Physics Library
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 
22 #include "LinearMath/btQuickprof.h"
30 #include <string.h> //for memset
31 #include <cmath>
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 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 // btSymMatrix
67 //
68 template <typename T>
70 {
71  btSymMatrix() : dim(0) {}
72  btSymMatrix(int n, const T& init = T()) { resize(n, init); }
73  void resize(int n, const T& init = T())
74  {
75  dim = n;
76  store.resize((n * (n + 1)) / 2, init);
77  }
78  int index(int c, int r) const
79  {
80  if (c > r) btSwap(c, r);
81  btAssert(r < dim);
82  return ((r * (r + 1)) / 2 + c);
83  }
84  T& operator()(int c, int r) { return (store[index(c, r)]); }
85  const T& operator()(int c, int r) const { return (store[index(c, r)]); }
87  int dim;
88 };
89 
90 //
91 // btSoftBodyCollisionShape
92 //
94 {
95 public:
97 
99  {
101  m_body = backptr;
102  }
103 
105  {
106  }
107 
108  void processAllTriangles(btTriangleCallback* /*callback*/, const btVector3& /*aabbMin*/, const btVector3& /*aabbMax*/) const
109  {
110  //not yet
111  btAssert(0);
112  }
113 
115  virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const
116  {
117  /* t is usually identity, except when colliding against btCompoundShape. See Issue 512 */
118  const btVector3 mins = m_body->m_bounds[0];
119  const btVector3 maxs = m_body->m_bounds[1];
120  const btVector3 crns[] = {t * btVector3(mins.x(), mins.y(), mins.z()),
121  t * btVector3(maxs.x(), mins.y(), mins.z()),
122  t * btVector3(maxs.x(), maxs.y(), mins.z()),
123  t * btVector3(mins.x(), maxs.y(), mins.z()),
124  t * btVector3(mins.x(), mins.y(), maxs.z()),
125  t * btVector3(maxs.x(), mins.y(), maxs.z()),
126  t * btVector3(maxs.x(), maxs.y(), maxs.z()),
127  t * btVector3(mins.x(), maxs.y(), maxs.z())};
128  aabbMin = aabbMax = crns[0];
129  for (int i = 1; i < 8; ++i)
130  {
131  aabbMin.setMin(crns[i]);
132  aabbMax.setMax(crns[i]);
133  }
134  }
135 
136  virtual void setLocalScaling(const btVector3& /*scaling*/)
137  {
139  }
140  virtual const btVector3& getLocalScaling() const
141  {
142  static const btVector3 dummy(1, 1, 1);
143  return dummy;
144  }
145  virtual void calculateLocalInertia(btScalar /*mass*/, btVector3& /*inertia*/) const
146  {
148  btAssert(0);
149  }
150  virtual const char* getName() const
151  {
152  return "SoftBody";
153  }
154 };
155 
156 //
157 // btSoftClusterCollisionShape
158 //
160 {
161 public:
163 
165 
166  virtual btVector3 localGetSupportingVertex(const btVector3& vec) const
167  {
168  btSoftBody::Node* const* n = &m_cluster->m_nodes[0];
169  btScalar d = btDot(vec, n[0]->m_x);
170  int j = 0;
171  for (int i = 1, ni = m_cluster->m_nodes.size(); i < ni; ++i)
172  {
173  const btScalar k = btDot(vec, n[i]->m_x);
174  if (k > d)
175  {
176  d = k;
177  j = i;
178  }
179  }
180  return (n[j]->m_x);
181  }
183  {
184  return (localGetSupportingVertex(vec));
185  }
186  //notice that the vectors should be unit length
187  virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const
188  {
189  }
190 
191  virtual void calculateLocalInertia(btScalar mass, btVector3& inertia) const
192  {
193  }
194 
195  virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const
196  {
197  }
198 
199  virtual int getShapeType() const { return SOFTBODY_SHAPE_PROXYTYPE; }
200 
201  //debugging
202  virtual const char* getName() const { return "SOFTCLUSTER"; }
203 
204  virtual void setMargin(btScalar margin)
205  {
207  }
208  virtual btScalar getMargin() const
209  {
211  }
212 };
213 
214 //
215 // Inline's
216 //
217 
218 //
219 template <typename T>
220 static inline void ZeroInitialize(T& value)
221 {
222  memset(&value, 0, sizeof(T));
223 }
224 //
225 template <typename T>
226 static inline bool CompLess(const T& a, const T& b)
227 {
228  return (a < b);
229 }
230 //
231 template <typename T>
232 static inline bool CompGreater(const T& a, const T& b)
233 {
234  return (a > b);
235 }
236 //
237 template <typename T>
238 static inline T Lerp(const T& a, const T& b, btScalar t)
239 {
240  return (a + (b - a) * t);
241 }
242 //
243 template <typename T>
244 static inline T InvLerp(const T& a, const T& b, btScalar t)
245 {
246  return ((b + a * t - b * t) / (a * b));
247 }
248 //
249 static inline btMatrix3x3 Lerp(const btMatrix3x3& a,
250  const btMatrix3x3& b,
251  btScalar t)
252 {
253  btMatrix3x3 r;
254  r[0] = Lerp(a[0], b[0], t);
255  r[1] = Lerp(a[1], b[1], t);
256  r[2] = Lerp(a[2], b[2], t);
257  return (r);
258 }
259 //
260 static inline btVector3 Clamp(const btVector3& v, btScalar maxlength)
261 {
262  const btScalar sql = v.length2();
263  if (sql > (maxlength * maxlength))
264  return ((v * maxlength) / btSqrt(sql));
265  else
266  return (v);
267 }
268 //
269 template <typename T>
270 static inline T Clamp(const T& x, const T& l, const T& h)
271 {
272  return (x < l ? l : x > h ? h : x);
273 }
274 //
275 template <typename T>
276 static inline T Sq(const T& x)
277 {
278  return (x * x);
279 }
280 //
281 template <typename T>
282 static inline T Cube(const T& x)
283 {
284  return (x * x * x);
285 }
286 //
287 template <typename T>
288 static inline T Sign(const T& x)
289 {
290  return ((T)(x < 0 ? -1 : +1));
291 }
292 //
293 template <typename T>
294 static inline bool SameSign(const T& x, const T& y)
295 {
296  return ((x * y) > 0);
297 }
298 //
299 static inline btScalar ClusterMetric(const btVector3& x, const btVector3& y)
300 {
301  const btVector3 d = x - y;
302  return (btFabs(d[0]) + btFabs(d[1]) + btFabs(d[2]));
303 }
304 //
305 static inline btMatrix3x3 ScaleAlongAxis(const btVector3& a, btScalar s)
306 {
307  const btScalar xx = a.x() * a.x();
308  const btScalar yy = a.y() * a.y();
309  const btScalar zz = a.z() * a.z();
310  const btScalar xy = a.x() * a.y();
311  const btScalar yz = a.y() * a.z();
312  const btScalar zx = a.z() * a.x();
313  btMatrix3x3 m;
314  m[0] = btVector3(1 - xx + xx * s, xy * s - xy, zx * s - zx);
315  m[1] = btVector3(xy * s - xy, 1 - yy + yy * s, yz * s - yz);
316  m[2] = btVector3(zx * s - zx, yz * s - yz, 1 - zz + zz * s);
317  return (m);
318 }
319 //
320 static inline btMatrix3x3 Cross(const btVector3& v)
321 {
322  btMatrix3x3 m;
323  m[0] = btVector3(0, -v.z(), +v.y());
324  m[1] = btVector3(+v.z(), 0, -v.x());
325  m[2] = btVector3(-v.y(), +v.x(), 0);
326  return (m);
327 }
328 //
329 static inline btMatrix3x3 Diagonal(btScalar x)
330 {
331  btMatrix3x3 m;
332  m[0] = btVector3(x, 0, 0);
333  m[1] = btVector3(0, x, 0);
334  m[2] = btVector3(0, 0, x);
335  return (m);
336 }
337 
338 static inline btMatrix3x3 Diagonal(const btVector3& v)
339 {
340  btMatrix3x3 m;
341  m[0] = btVector3(v.getX(), 0, 0);
342  m[1] = btVector3(0, v.getY(), 0);
343  m[2] = btVector3(0, 0, v.getZ());
344  return (m);
345 }
346 
347 static inline btScalar Dot(const btScalar* a,const btScalar* b, int ndof)
348 {
349  btScalar result = 0;
350  for (int i = 0; i < ndof; ++i)
351  result += a[i] * b[i];
352  return result;
353 }
354 
355 static inline btMatrix3x3 OuterProduct(const btScalar* v1,const btScalar* v2,const btScalar* v3,
356  const btScalar* u1, const btScalar* u2, const btScalar* u3, int ndof)
357 {
358  btMatrix3x3 m;
359  btScalar a11 = Dot(v1,u1,ndof);
360  btScalar a12 = Dot(v1,u2,ndof);
361  btScalar a13 = Dot(v1,u3,ndof);
362 
363  btScalar a21 = Dot(v2,u1,ndof);
364  btScalar a22 = Dot(v2,u2,ndof);
365  btScalar a23 = Dot(v2,u3,ndof);
366 
367  btScalar a31 = Dot(v3,u1,ndof);
368  btScalar a32 = Dot(v3,u2,ndof);
369  btScalar a33 = Dot(v3,u3,ndof);
370  m[0] = btVector3(a11, a12, a13);
371  m[1] = btVector3(a21, a22, a23);
372  m[2] = btVector3(a31, a32, a33);
373  return (m);
374 }
375 
376 
377 //
378 static inline btMatrix3x3 Add(const btMatrix3x3& a,
379  const btMatrix3x3& b)
380 {
381  btMatrix3x3 r;
382  for (int i = 0; i < 3; ++i) r[i] = a[i] + b[i];
383  return (r);
384 }
385 //
386 static inline btMatrix3x3 Sub(const btMatrix3x3& a,
387  const btMatrix3x3& b)
388 {
389  btMatrix3x3 r;
390  for (int i = 0; i < 3; ++i) r[i] = a[i] - b[i];
391  return (r);
392 }
393 //
394 static inline btMatrix3x3 Mul(const btMatrix3x3& a,
395  btScalar b)
396 {
397  btMatrix3x3 r;
398  for (int i = 0; i < 3; ++i) r[i] = a[i] * b;
399  return (r);
400 }
401 //
402 static inline void Orthogonalize(btMatrix3x3& m)
403 {
404  m[2] = btCross(m[0], m[1]).normalized();
405  m[1] = btCross(m[2], m[0]).normalized();
406  m[0] = btCross(m[1], m[2]).normalized();
407 }
408 //
409 static inline btMatrix3x3 MassMatrix(btScalar im, const btMatrix3x3& iwi, const btVector3& r)
410 {
411  const btMatrix3x3 cr = Cross(r);
412  return (Sub(Diagonal(im), cr * iwi * cr));
413 }
414 
415 //
417  btScalar ima,
418  btScalar imb,
419  const btMatrix3x3& iwi,
420  const btVector3& r)
421 {
422  return (Diagonal(1 / dt) * Add(Diagonal(ima), MassMatrix(imb, iwi, r)).inverse());
423 }
424 
425 //
426 static inline btMatrix3x3 ImpulseMatrix(btScalar ima, const btMatrix3x3& iia, const btVector3& ra,
427  btScalar imb, const btMatrix3x3& iib, const btVector3& rb)
428 {
429  return (Add(MassMatrix(ima, iia, ra), MassMatrix(imb, iib, rb)).inverse());
430 }
431 
432 //
434  const btMatrix3x3& iib)
435 {
436  return (Add(iia, iib).inverse());
437 }
438 
439 //
440 static inline btVector3 ProjectOnAxis(const btVector3& v,
441  const btVector3& a)
442 {
443  return (a * btDot(v, a));
444 }
445 //
446 static inline btVector3 ProjectOnPlane(const btVector3& v,
447  const btVector3& a)
448 {
449  return (v - ProjectOnAxis(v, a));
450 }
451 
452 //
453 static inline void ProjectOrigin(const btVector3& a,
454  const btVector3& b,
455  btVector3& prj,
456  btScalar& sqd)
457 {
458  const btVector3 d = b - a;
459  const btScalar m2 = d.length2();
460  if (m2 > SIMD_EPSILON)
461  {
462  const btScalar t = Clamp<btScalar>(-btDot(a, d) / m2, 0, 1);
463  const btVector3 p = a + d * t;
464  const btScalar l2 = p.length2();
465  if (l2 < sqd)
466  {
467  prj = p;
468  sqd = l2;
469  }
470  }
471 }
472 //
473 static inline void ProjectOrigin(const btVector3& a,
474  const btVector3& b,
475  const btVector3& c,
476  btVector3& prj,
477  btScalar& sqd)
478 {
479  const btVector3& q = btCross(b - a, c - a);
480  const btScalar m2 = q.length2();
481  if (m2 > SIMD_EPSILON)
482  {
483  const btVector3 n = q / btSqrt(m2);
484  const btScalar k = btDot(a, n);
485  const btScalar k2 = k * k;
486  if (k2 < sqd)
487  {
488  const btVector3 p = n * k;
489  if ((btDot(btCross(a - p, b - p), q) > 0) &&
490  (btDot(btCross(b - p, c - p), q) > 0) &&
491  (btDot(btCross(c - p, a - p), q) > 0))
492  {
493  prj = p;
494  sqd = k2;
495  }
496  else
497  {
498  ProjectOrigin(a, b, prj, sqd);
499  ProjectOrigin(b, c, prj, sqd);
500  ProjectOrigin(c, a, prj, sqd);
501  }
502  }
503  }
504 }
505 
506 //
507 static inline bool rayIntersectsTriangle(const btVector3& origin, const btVector3& dir, const btVector3& v0, const btVector3& v1, const btVector3& v2, btScalar& t)
508 {
509  btScalar a, f, u, v;
510 
511  btVector3 e1 = v1 - v0;
512  btVector3 e2 = v2 - v0;
513  btVector3 h = dir.cross(e2);
514  a = e1.dot(h);
515 
516  if (a > -0.00001 && a < 0.00001)
517  return (false);
518 
519  f = btScalar(1) / a;
520  btVector3 s = origin - v0;
521  u = f * s.dot(h);
522 
523  if (u < 0.0 || u > 1.0)
524  return (false);
525 
526  btVector3 q = s.cross(e1);
527  v = f * dir.dot(q);
528  if (v < 0.0 || u + v > 1.0)
529  return (false);
530  // at this stage we can compute t to find out where
531  // the intersection point is on the line
532  t = f * e2.dot(q);
533  if (t > 0) // ray intersection
534  return (true);
535  else // this means that there is a line intersection
536  // but not a ray intersection
537  return (false);
538 }
539 
540 static inline bool lineIntersectsTriangle(const btVector3& rayStart, const btVector3& rayEnd, const btVector3& p1, const btVector3& p2, const btVector3& p3, btVector3& sect, btVector3& normal)
541 {
542  btVector3 dir = rayEnd - rayStart;
543  btScalar dir_norm = dir.norm();
544  if (dir_norm < SIMD_EPSILON)
545  return false;
546  dir.normalize();
547 
548  btScalar t;
549 
550  bool ret = rayIntersectsTriangle(rayStart, dir, p1, p2, p3, t);
551 
552  if (ret)
553  {
554  if (t <= dir_norm)
555  {
556  sect = rayStart + dir * t;
557  }
558  else
559  {
560  ret = false;
561  }
562  }
563 
564  if (ret)
565  {
566  btVector3 n = (p3-p1).cross(p2-p1);
567  n.safeNormalize();
568  if (n.dot(dir) < 0)
569  normal = n;
570  else
571  normal = -n;
572  }
573  return ret;
574 }
575 
576 
577 //
578 template <typename T>
579 static inline T BaryEval(const T& a,
580  const T& b,
581  const T& c,
582  const btVector3& coord)
583 {
584  return (a * coord.x() + b * coord.y() + c * coord.z());
585 }
586 //
587 static inline btVector3 BaryCoord(const btVector3& a,
588  const btVector3& b,
589  const btVector3& c,
590  const btVector3& p)
591 {
592  const btScalar w[] = {btCross(a - p, b - p).length(),
593  btCross(b - p, c - p).length(),
594  btCross(c - p, a - p).length()};
595  const btScalar isum = 1 / (w[0] + w[1] + w[2]);
596  return (btVector3(w[1] * isum, w[2] * isum, w[0] * isum));
597 }
598 
599 //
601  const btVector3& a,
602  const btVector3& b,
603  const btScalar accuracy,
604  const int maxiterations = 256)
605 {
606  btScalar span[2] = {0, 1};
607  btScalar values[2] = {fn->Eval(a), fn->Eval(b)};
608  if (values[0] > values[1])
609  {
610  btSwap(span[0], span[1]);
611  btSwap(values[0], values[1]);
612  }
613  if (values[0] > -accuracy) return (-1);
614  if (values[1] < +accuracy) return (-1);
615  for (int i = 0; i < maxiterations; ++i)
616  {
617  const btScalar t = Lerp(span[0], span[1], values[0] / (values[0] - values[1]));
618  const btScalar v = fn->Eval(Lerp(a, b, t));
619  if ((t <= 0) || (t >= 1)) break;
620  if (btFabs(v) < accuracy) return (t);
621  if (v < 0)
622  {
623  span[0] = t;
624  values[0] = v;
625  }
626  else
627  {
628  span[1] = t;
629  values[1] = v;
630  }
631  }
632  return (-1);
633 }
634 
635 inline static void EvaluateMedium(const btSoftBodyWorldInfo* wfi,
636  const btVector3& x,
637  btSoftBody::sMedium& medium)
638 {
639  medium.m_velocity = btVector3(0, 0, 0);
640  medium.m_pressure = 0;
641  medium.m_density = wfi->air_density;
642  if (wfi->water_density > 0)
643  {
644  const btScalar depth = -(btDot(x, wfi->water_normal) + wfi->water_offset);
645  if (depth > 0)
646  {
647  medium.m_density = wfi->water_density;
648  medium.m_pressure = depth * wfi->water_density * wfi->m_gravity.length();
649  }
650  }
651 }
652 
653 //
654 static inline btVector3 NormalizeAny(const btVector3& v)
655 {
656  const btScalar l = v.length();
657  if (l > SIMD_EPSILON)
658  return (v / l);
659  else
660  return (btVector3(0, 0, 0));
661 }
662 
663 //
664 static inline btDbvtVolume VolumeOf(const btSoftBody::Face& f,
665  btScalar margin)
666 {
667  const btVector3* pts[] = {&f.m_n[0]->m_x,
668  &f.m_n[1]->m_x,
669  &f.m_n[2]->m_x};
671  vol.Expand(btVector3(margin, margin, margin));
672  return (vol);
673 }
674 
675 //
676 static inline btVector3 CenterOf(const btSoftBody::Face& f)
677 {
678  return ((f.m_n[0]->m_x + f.m_n[1]->m_x + f.m_n[2]->m_x) / 3);
679 }
680 
681 //
682 static inline btScalar AreaOf(const btVector3& x0,
683  const btVector3& x1,
684  const btVector3& x2)
685 {
686  const btVector3 a = x1 - x0;
687  const btVector3 b = x2 - x0;
688  const btVector3 cr = btCross(a, b);
689  const btScalar area = cr.length();
690  return (area);
691 }
692 
693 //
694 static inline btScalar VolumeOf(const btVector3& x0,
695  const btVector3& x1,
696  const btVector3& x2,
697  const btVector3& x3)
698 {
699  const btVector3 a = x1 - x0;
700  const btVector3 b = x2 - x0;
701  const btVector3 c = x3 - x0;
702  return (btDot(a, btCross(b, c)));
703 }
704 
705 //
706 
707 //
708 static inline void ApplyClampedForce(btSoftBody::Node& n,
709  const btVector3& f,
710  btScalar dt)
711 {
712  const btScalar dtim = dt * n.m_im;
713  if ((f * dtim).length2() > n.m_v.length2())
714  { /* Clamp */
715  n.m_f -= ProjectOnAxis(n.m_v, f.normalized()) / dtim;
716  }
717  else
718  { /* Apply */
719  n.m_f += f;
720  }
721 }
722 
723 //
724 static inline int MatchEdge(const btSoftBody::Node* a,
725  const btSoftBody::Node* b,
726  const btSoftBody::Node* ma,
727  const btSoftBody::Node* mb)
728 {
729  if ((a == ma) && (b == mb)) return (0);
730  if ((a == mb) && (b == ma)) return (1);
731  return (-1);
732 }
733 
734 //
735 // btEigen : Extract eigen system,
736 // straitforward implementation of http://math.fullerton.edu/mathews/n2003/JacobiMethodMod.html
737 // outputs are NOT sorted.
738 //
739 struct btEigen
740 {
741  static int system(btMatrix3x3& a, btMatrix3x3* vectors, btVector3* values = 0)
742  {
743  static const int maxiterations = 16;
744  static const btScalar accuracy = (btScalar)0.0001;
745  btMatrix3x3& v = *vectors;
746  int iterations = 0;
747  vectors->setIdentity();
748  do
749  {
750  int p = 0, q = 1;
751  if (btFabs(a[p][q]) < btFabs(a[0][2]))
752  {
753  p = 0;
754  q = 2;
755  }
756  if (btFabs(a[p][q]) < btFabs(a[1][2]))
757  {
758  p = 1;
759  q = 2;
760  }
761  if (btFabs(a[p][q]) > accuracy)
762  {
763  const btScalar w = (a[q][q] - a[p][p]) / (2 * a[p][q]);
764  const btScalar z = btFabs(w);
765  const btScalar t = w / (z * (btSqrt(1 + w * w) + z));
766  if (t == t) /* [WARNING] let hope that one does not get thrown aways by some compilers... */
767  {
768  const btScalar c = 1 / btSqrt(t * t + 1);
769  const btScalar s = c * t;
770  mulPQ(a, c, s, p, q);
771  mulTPQ(a, c, s, p, q);
772  mulPQ(v, c, s, p, q);
773  }
774  else
775  break;
776  }
777  else
778  break;
779  } while ((++iterations) < maxiterations);
780  if (values)
781  {
782  *values = btVector3(a[0][0], a[1][1], a[2][2]);
783  }
784  return (iterations);
785  }
786 
787 private:
788  static inline void mulTPQ(btMatrix3x3& a, btScalar c, btScalar s, int p, int q)
789  {
790  const btScalar m[2][3] = {{a[p][0], a[p][1], a[p][2]},
791  {a[q][0], a[q][1], a[q][2]}};
792  int i;
793 
794  for (i = 0; i < 3; ++i) a[p][i] = c * m[0][i] - s * m[1][i];
795  for (i = 0; i < 3; ++i) a[q][i] = c * m[1][i] + s * m[0][i];
796  }
797  static inline void mulPQ(btMatrix3x3& a, btScalar c, btScalar s, int p, int q)
798  {
799  const btScalar m[2][3] = {{a[0][p], a[1][p], a[2][p]},
800  {a[0][q], a[1][q], a[2][q]}};
801  int i;
802 
803  for (i = 0; i < 3; ++i) a[i][p] = c * m[0][i] - s * m[1][i];
804  for (i = 0; i < 3; ++i) a[i][q] = c * m[1][i] + s * m[0][i];
805  }
806 };
807 
808 //
809 // Polar decomposition,
810 // "Computing the Polar Decomposition with Applications", Nicholas J. Higham, 1986.
811 //
812 static inline int PolarDecompose(const btMatrix3x3& m, btMatrix3x3& q, btMatrix3x3& s)
813 {
814  static const btPolarDecomposition polar;
815  return polar.decompose(m, q, s);
816 }
817 
818 //
819 // btSoftColliders
820 //
822 {
823  //
824  // ClusterBase
825  //
827  {
834  {
835  erp = (btScalar)1;
836  idt = 0;
837  m_margin = 0;
838  friction = 0;
839  threshold = (btScalar)0;
840  }
842  btSoftBody::Body ba, const btSoftBody::Body bb,
843  btSoftBody::CJoint& joint)
844  {
845  if (res.distance < m_margin)
846  {
847  btVector3 norm = res.normal;
848  norm.normalize(); //is it necessary?
849 
850  const btVector3 ra = res.witnesses[0] - ba.xform().getOrigin();
851  const btVector3 rb = res.witnesses[1] - bb.xform().getOrigin();
852  const btVector3 va = ba.velocity(ra);
853  const btVector3 vb = bb.velocity(rb);
854  const btVector3 vrel = va - vb;
855  const btScalar rvac = btDot(vrel, norm);
856  btScalar depth = res.distance - m_margin;
857 
858  // printf("depth=%f\n",depth);
859  const btVector3 iv = norm * rvac;
860  const btVector3 fv = vrel - iv;
861  joint.m_bodies[0] = ba;
862  joint.m_bodies[1] = bb;
863  joint.m_refs[0] = ra * ba.xform().getBasis();
864  joint.m_refs[1] = rb * bb.xform().getBasis();
865  joint.m_rpos[0] = ra;
866  joint.m_rpos[1] = rb;
867  joint.m_cfm = 1;
868  joint.m_erp = 1;
869  joint.m_life = 0;
870  joint.m_maxlife = 0;
871  joint.m_split = 1;
872 
873  joint.m_drift = depth * norm;
874 
875  joint.m_normal = norm;
876  // printf("normal=%f,%f,%f\n",res.normal.getX(),res.normal.getY(),res.normal.getZ());
877  joint.m_delete = false;
878  joint.m_friction = fv.length2() < (rvac * friction * rvac * friction) ? 1 : friction;
879  joint.m_massmatrix = ImpulseMatrix(ba.invMass(), ba.invWorldInertia(), joint.m_rpos[0],
880  bb.invMass(), bb.invWorldInertia(), joint.m_rpos[1]);
881 
882  return (true);
883  }
884  return (false);
885  }
886  };
887  //
888  // CollideCL_RS
889  //
891  {
894 
895  void Process(const btDbvtNode* leaf)
896  {
897  btSoftBody::Cluster* cluster = (btSoftBody::Cluster*)leaf->data;
898  btSoftClusterCollisionShape cshape(cluster);
899 
900  const btConvexShape* rshape = (const btConvexShape*)m_colObjWrap->getCollisionShape();
901 
904  return;
905 
908  rshape, m_colObjWrap->getWorldTransform(),
909  btVector3(1, 0, 0), res))
910  {
911  btSoftBody::CJoint joint;
912  if (SolveContact(res, cluster, m_colObjWrap->getCollisionObject(), joint)) //prb,joint))
913  {
915  *pj = joint;
916  psb->m_joints.push_back(pj);
918  {
919  pj->m_erp *= psb->m_cfg.kSKHR_CL;
920  pj->m_split *= psb->m_cfg.kSK_SPLT_CL;
921  }
922  else
923  {
924  pj->m_erp *= psb->m_cfg.kSRHR_CL;
925  pj->m_split *= psb->m_cfg.kSR_SPLT_CL;
926  }
927  }
928  }
929  }
931  {
932  psb = ps;
933  m_colObjWrap = colObWrap;
934  idt = ps->m_sst.isdt;
938  btVector3 mins;
939  btVector3 maxs;
940 
942  volume;
943  colObWrap->getCollisionShape()->getAabb(colObWrap->getWorldTransform(), mins, maxs);
944  volume = btDbvtVolume::FromMM(mins, maxs);
945  volume.Expand(btVector3(1, 1, 1) * m_margin);
946  ps->m_cdbvt.collideTV(ps->m_cdbvt.m_root, volume, *this);
947  }
948  };
949  //
950  // CollideCL_SS
951  //
953  {
955  void Process(const btDbvtNode* la, const btDbvtNode* lb)
956  {
959 
960  bool connected = false;
961  if ((bodies[0] == bodies[1]) && (bodies[0]->m_clusterConnectivity.size()))
962  {
963  connected = bodies[0]->m_clusterConnectivity[cla->m_clusterIndex + bodies[0]->m_clusters.size() * clb->m_clusterIndex];
964  }
965 
966  if (!connected)
967  {
972  &csb, btTransform::getIdentity(),
973  cla->m_com - clb->m_com, res))
974  {
975  btSoftBody::CJoint joint;
976  if (SolveContact(res, cla, clb, joint))
977  {
979  *pj = joint;
980  bodies[0]->m_joints.push_back(pj);
981  pj->m_erp *= btMax(bodies[0]->m_cfg.kSSHR_CL, bodies[1]->m_cfg.kSSHR_CL);
982  pj->m_split *= (bodies[0]->m_cfg.kSS_SPLT_CL + bodies[1]->m_cfg.kSS_SPLT_CL) / 2;
983  }
984  }
985  }
986  else
987  {
988  static int count = 0;
989  count++;
990  //printf("count=%d\n",count);
991  }
992  }
994  {
995  idt = psa->m_sst.isdt;
996  //m_margin = (psa->getCollisionShape()->getMargin()+psb->getCollisionShape()->getMargin())/2;
998  friction = btMin(psa->m_cfg.kDF, psb->m_cfg.kDF);
999  bodies[0] = psa;
1000  bodies[1] = psb;
1001  psa->m_cdbvt.collideTT(psa->m_cdbvt.m_root, psb->m_cdbvt.m_root, *this);
1002  }
1003  };
1004  //
1005  // CollideSDF_RS
1006  //
1008  {
1009  void Process(const btDbvtNode* leaf)
1010  {
1011  btSoftBody::Node* node = (btSoftBody::Node*)leaf->data;
1012  DoNode(*node);
1013  }
1014  void DoNode(btSoftBody::Node& n) const
1015  {
1016  const btScalar m = n.m_im > 0 ? dynmargin : stamargin;
1018 
1019  if ((!n.m_battach) &&
1020  psb->checkContact(m_colObj1Wrap, n.m_x, m, c.m_cti))
1021  {
1022  const btScalar ima = n.m_im;
1023  const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f;
1024  const btScalar ms = ima + imb;
1025  if (ms > 0)
1026  {
1028  static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0);
1029  const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic;
1030  const btVector3 ra = n.m_x - wtr.getOrigin();
1032  const btVector3 vb = n.m_x - n.m_q;
1033  const btVector3 vr = vb - va;
1034  const btScalar dn = btDot(vr, c.m_cti.m_normal);
1035  const btVector3 fv = vr - c.m_cti.m_normal * dn;
1037  c.m_node = &n;
1038  c.m_c0 = ImpulseMatrix(psb->m_sst.sdt, ima, imb, iwi, ra);
1039  c.m_c1 = ra;
1040  c.m_c2 = ima * psb->m_sst.sdt;
1041  c.m_c3 = fv.length2() < (dn * fc * dn * fc) ? 0 : 1 - fc;
1044  if (m_rigidBody)
1045  m_rigidBody->activate();
1046  }
1047  }
1048  }
1054  };
1055 
1056  //
1057  // CollideSDF_RD
1058  //
1060  {
1061  void Process(const btDbvtNode* leaf)
1062  {
1063  btSoftBody::Node* node = (btSoftBody::Node*)leaf->data;
1064  DoNode(*node);
1065  }
1066  void DoNode(btSoftBody::Node& n) const
1067  {
1068  const btScalar m = n.m_im > 0 ? dynmargin : stamargin;
1070 
1071  if (!n.m_battach)
1072  {
1073  // check for collision at x_{n+1}^* as well at x_n
1074  if (psb->checkDeformableContact(m_colObj1Wrap, n.m_x, m, c.m_cti, /*predict = */ true) || psb->checkDeformableContact(m_colObj1Wrap, n.m_q, m, c.m_cti, /*predict = */ true))
1075  {
1076  const btScalar ima = n.m_im;
1077  // todo: collision between multibody and fixed deformable node will be missed.
1078  const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f;
1079  const btScalar ms = ima + imb;
1080  if (ms > 0)
1081  {
1082  // resolve contact at x_n
1083  psb->checkDeformableContact(m_colObj1Wrap, n.m_x, m, c.m_cti, /*predict = */ false);
1084  btSoftBody::sCti& cti = c.m_cti;
1085  c.m_node = &n;
1087  c.m_c2 = ima;
1088  c.m_c3 = fc;
1090 
1092  {
1094  static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0);
1095  const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic;
1096  const btVector3 ra = n.m_x - wtr.getOrigin();
1097 
1098  c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra);
1099  c.m_c1 = ra;
1100  }
1102  {
1104  if (multibodyLinkCol)
1105  {
1106  btVector3 normal = cti.m_normal;
1108  btVector3 t2 = btCross(normal, t1);
1109  btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2;
1110  findJacobian(multibodyLinkCol, jacobianData_normal, c.m_node->m_x, normal);
1111  findJacobian(multibodyLinkCol, jacobianData_t1, c.m_node->m_x, t1);
1112  findJacobian(multibodyLinkCol, jacobianData_t2, c.m_node->m_x, t2);
1113 
1114  btScalar* J_n = &jacobianData_normal.m_jacobians[0];
1115  btScalar* J_t1 = &jacobianData_t1.m_jacobians[0];
1116  btScalar* J_t2 = &jacobianData_t2.m_jacobians[0];
1117 
1118  btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
1119  btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
1120  btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
1121 
1122  btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(),
1123  t1.getX(), t1.getY(), t1.getZ(),
1124  t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame
1125  const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
1126  btMatrix3x3 local_impulse_matrix = (Diagonal(n.m_im) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse();
1127  c.m_c0 = rot.transpose() * local_impulse_matrix * rot;
1128  c.jacobianData_normal = jacobianData_normal;
1129  c.jacobianData_t1 = jacobianData_t1;
1130  c.jacobianData_t2 = jacobianData_t2;
1131  c.t1 = t1;
1132  c.t2 = t2;
1133  }
1134  }
1135  psb->m_nodeRigidContacts.push_back(c);
1136  }
1137  }
1138  }
1139  }
1145  };
1146 
1147  //
1148  // CollideSDF_RDF
1149  //
1151  {
1152  void Process(const btDbvtNode* leaf)
1153  {
1154  btSoftBody::Face* face = (btSoftBody::Face*)leaf->data;
1155  DoNode(*face);
1156  }
1157  void DoNode(btSoftBody::Face& f) const
1158  {
1159  btSoftBody::Node* n0 = f.m_n[0];
1160  btSoftBody::Node* n1 = f.m_n[1];
1161  btSoftBody::Node* n2 = f.m_n[2];
1162 
1163  const btScalar m = (n0->m_im > 0 && n1->m_im > 0 && n2->m_im > 0 )? dynmargin : stamargin;
1165  btVector3 contact_point;
1166  btVector3 bary;
1167  if (psb->checkDeformableFaceContact(m_colObj1Wrap, f, contact_point, bary, m, c.m_cti, true))
1168  {
1169  f.m_pcontact[3] = 1;
1170  btScalar ima = n0->m_im + n1->m_im + n2->m_im;
1171  const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f;
1172  // todo: collision between multibody and fixed deformable face will be missed.
1173  const btScalar ms = ima + imb;
1174  if (ms > 0)
1175  {
1176  // resolve contact at x_n
1177  psb->checkDeformableFaceContact(m_colObj1Wrap, f, contact_point, bary, m, c.m_cti, /*predict = */ false);
1178  btSoftBody::sCti& cti = c.m_cti;
1179  c.m_contactPoint = contact_point;
1180  c.m_bary = bary;
1181  // todo xuchenhan@: this is assuming mass of all vertices are the same. Need to modify if mass are different for distinct vertices
1182  c.m_weights = btScalar(2)/(btScalar(1) + bary.length2()) * bary;
1183  c.m_face = &f;
1185 
1186  // the effective inverse mass of the face as in https://graphics.stanford.edu/papers/cloth-sig02/cloth.pdf
1187  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;
1188 
1189  c.m_c2 = ima;
1190  c.m_c3 = fc;
1193  {
1195  static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0);
1196  const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic;
1197  const btVector3 ra = contact_point - wtr.getOrigin();
1198 
1199  // we do not scale the impulse matrix by dt
1200  c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra);
1201  c.m_c1 = ra;
1202  }
1204  {
1206  if (multibodyLinkCol)
1207  {
1208  btVector3 normal = cti.m_normal;
1210  btVector3 t2 = btCross(normal, t1);
1211  btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2;
1212  findJacobian(multibodyLinkCol, jacobianData_normal, contact_point, normal);
1213  findJacobian(multibodyLinkCol, jacobianData_t1, contact_point, t1);
1214  findJacobian(multibodyLinkCol, jacobianData_t2, contact_point, t2);
1215 
1216  btScalar* J_n = &jacobianData_normal.m_jacobians[0];
1217  btScalar* J_t1 = &jacobianData_t1.m_jacobians[0];
1218  btScalar* J_t2 = &jacobianData_t2.m_jacobians[0];
1219 
1220  btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
1221  btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
1222  btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
1223 
1224  btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(),
1225  t1.getX(), t1.getY(), t1.getZ(),
1226  t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame
1227  const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
1228  btMatrix3x3 local_impulse_matrix = (Diagonal(ima) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse();
1229  c.m_c0 = rot.transpose() * local_impulse_matrix * rot;
1230  c.jacobianData_normal = jacobianData_normal;
1231  c.jacobianData_t1 = jacobianData_t1;
1232  c.jacobianData_t2 = jacobianData_t2;
1233  c.t1 = t1;
1234  c.t2 = t2;
1235  }
1236  }
1237  psb->m_faceRigidContacts.push_back(c);
1238  }
1239  }
1240  else
1241  {
1242  f.m_pcontact[3] = 0;
1243  }
1244  }
1250  };
1251 
1252  //
1253  // CollideVF_SS
1254  //
1256  {
1257  void Process(const btDbvtNode* lnode,
1258  const btDbvtNode* lface)
1259  {
1260  btSoftBody::Node* node = (btSoftBody::Node*)lnode->data;
1261  btSoftBody::Face* face = (btSoftBody::Face*)lface->data;
1262  for (int i = 0; i < 3; ++i)
1263  {
1264  if (face->m_n[i] == node)
1265  continue;
1266  }
1267 
1268  btVector3 o = node->m_x;
1269  btVector3 p;
1270  btScalar d = SIMD_INFINITY;
1271  ProjectOrigin(face->m_n[0]->m_x - o,
1272  face->m_n[1]->m_x - o,
1273  face->m_n[2]->m_x - o,
1274  p, d);
1275  const btScalar m = mrg + (o - node->m_q).length() * 2;
1276  if (d < (m * m))
1277  {
1278  const btSoftBody::Node* n[] = {face->m_n[0], face->m_n[1], face->m_n[2]};
1279  const btVector3 w = BaryCoord(n[0]->m_x, n[1]->m_x, n[2]->m_x, p + o);
1280  const btScalar ma = node->m_im;
1281  btScalar mb = BaryEval(n[0]->m_im, n[1]->m_im, n[2]->m_im, w);
1282  if ((n[0]->m_im <= 0) ||
1283  (n[1]->m_im <= 0) ||
1284  (n[2]->m_im <= 0))
1285  {
1286  mb = 0;
1287  }
1288  const btScalar ms = ma + mb;
1289  if (ms > 0)
1290  {
1292  c.m_normal = p / -btSqrt(d);
1293  c.m_margin = m;
1294  c.m_node = node;
1295  c.m_face = face;
1296  c.m_weights = w;
1297  c.m_friction = btMax (psb[0]->m_cfg.kDF, psb[1]->m_cfg.kDF);
1298  c.m_cfm[0] = ma / ms * psb[0]->m_cfg.kSHR;
1299  c.m_cfm[1] = mb / ms * psb[1]->m_cfg.kSHR;
1300  psb[0]->m_scontacts.push_back(c);
1301  }
1302  }
1303  }
1306  };
1307 
1308 
1309  //
1310  // CollideVF_DD
1311  //
1313  {
1314  void Process(const btDbvtNode* lnode,
1315  const btDbvtNode* lface)
1316  {
1317  btSoftBody::Node* node = (btSoftBody::Node*)lnode->data;
1318  btSoftBody::Face* face = (btSoftBody::Face*)lface->data;
1319 
1320  btVector3 o = node->m_x;
1321  btVector3 p;
1322  btScalar d = SIMD_INFINITY;
1323  ProjectOrigin(face->m_n[0]->m_x - o,
1324  face->m_n[1]->m_x - o,
1325  face->m_n[2]->m_x - o,
1326  p, d);
1327  const btScalar m = mrg + (o - node->m_q).safeNorm() * 2;
1328  if (d < (m * m))
1329  {
1330  const btSoftBody::Node* n[] = {face->m_n[0], face->m_n[1], face->m_n[2]};
1331  const btVector3 w = BaryCoord(n[0]->m_x, n[1]->m_x, n[2]->m_x, p + o);
1332  const btScalar ma = node->m_im;
1333  btScalar mb = BaryEval(n[0]->m_im, n[1]->m_im, n[2]->m_im, w);
1334  if ((n[0]->m_im <= 0) ||
1335  (n[1]->m_im <= 0) ||
1336  (n[2]->m_im <= 0))
1337  {
1338  mb = 0;
1339  }
1340  const btScalar ms = ma + mb;
1341  if (ms > 0)
1342  {
1344  if (useFaceNormal)
1345  c.m_normal = face->m_normal;
1346  else
1347  c.m_normal = p / -btSqrt(d);
1348  c.m_margin = mrg;
1349  c.m_node = node;
1350  c.m_face = face;
1351  c.m_bary = w;
1352  // todo xuchenhan@: this is assuming mass of all vertices are the same. Need to modify if mass are different for distinct vertices
1353  c.m_weights = btScalar(2)/(btScalar(1) + w.length2()) * w;
1354  c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF;
1355  // the effective inverse mass of the face as in https://graphics.stanford.edu/papers/cloth-sig02/cloth.pdf
1356  c.m_imf = c.m_bary[0]*c.m_weights[0] * n[0]->m_im + c.m_bary[1]*c.m_weights[1] * n[1]->m_im + c.m_bary[2]*c.m_weights[2] * n[2]->m_im;
1357  c.m_c0 = btScalar(1)/(ma + c.m_imf);
1358  psb[0]->m_faceNodeContacts.push_back(c);
1359  }
1360  }
1361  }
1365  };
1366 
1367  //
1368  // CollideFF_DD
1369  //
1371  {
1372  void Process(const btDbvntNode* lface1,
1373  const btDbvntNode* lface2)
1374  {
1375  btSoftBody::Face* f = (btSoftBody::Face*)lface1->data;
1376  btSoftBody::Face* face = (btSoftBody::Face*)lface2->data;
1377  for (int node_id = 0; node_id < 3; ++node_id)
1378  {
1379  btSoftBody::Node* node = f->m_n[node_id];
1380  bool skip = false;
1381  for (int i = 0; i < 3; ++i)
1382  {
1383  if (face->m_n[i] == node)
1384  {
1385  skip = true;
1386  break;
1387  }
1388  }
1389  if (skip)
1390  continue;
1391  btVector3 o = node->m_x;
1392  btVector3 p;
1393  btScalar d = SIMD_INFINITY;
1394  ProjectOrigin(face->m_n[0]->m_x - o,
1395  face->m_n[1]->m_x - o,
1396  face->m_n[2]->m_x - o,
1397  p, d);
1398  const btScalar m = mrg + (o - node->m_q).safeNorm() * 2;
1399  if (d < (m * m))
1400  {
1401  const btSoftBody::Node* n[] = {face->m_n[0], face->m_n[1], face->m_n[2]};
1402  const btVector3 w = BaryCoord(n[0]->m_x, n[1]->m_x, n[2]->m_x, p + o);
1403  const btScalar ma = node->m_im;
1404  btScalar mb = BaryEval(n[0]->m_im, n[1]->m_im, n[2]->m_im, w);
1405  if ((n[0]->m_im <= 0) ||
1406  (n[1]->m_im <= 0) ||
1407  (n[2]->m_im <= 0))
1408  {
1409  mb = 0;
1410  }
1411  const btScalar ms = ma + mb;
1412  if (ms > 0)
1413  {
1415  if (useFaceNormal)
1416  c.m_normal = face->m_normal;
1417  else
1418  c.m_normal = p / -btSqrt(d);
1419  c.m_margin = mrg;
1420  c.m_node = node;
1421  c.m_face = face;
1422  c.m_bary = w;
1423  // todo xuchenhan@: this is assuming mass of all vertices are the same. Need to modify if mass are different for distinct vertices
1424  c.m_weights = btScalar(2)/(btScalar(1) + w.length2()) * w;
1425  c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF;
1426  // the effective inverse mass of the face as in https://graphics.stanford.edu/papers/cloth-sig02/cloth.pdf
1427  c.m_imf = c.m_bary[0]*c.m_weights[0] * n[0]->m_im + c.m_bary[1]*c.m_weights[1] * n[1]->m_im + c.m_bary[2]*c.m_weights[2] * n[2]->m_im;
1428  c.m_c0 = btScalar(1)/(ma + c.m_imf);
1429  psb[0]->m_faceNodeContacts.push_back(c);
1430  }
1431  }
1432  }
1433  }
1437  };
1438 };
1439 
1440 #endif //_BT_SOFT_BODY_INTERNALS_H
SIMD_EPSILON
#define SIMD_EPSILON
Definition: btScalar.h:543
btSoftClusterCollisionShape::getShapeType
virtual int getShapeType() const
Definition: btSoftBodyInternals.h:199
btSoftBody::ImplicitFn
Definition: btSoftBody.h:204
btSymMatrix::store
btAlignedObjectArray< T > store
Definition: btSoftBodyInternals.h:86
btSoftBody::RContact::m_c0
btMatrix3x3 m_c0
Definition: btSoftBody.h:322
btSoftColliders::CollideSDF_RS::m_rigidBody
btRigidBody * m_rigidBody
Definition: btSoftBodyInternals.h:1051
btSoftBody::DeformableRigidContact::jacobianData_t2
btMultiBodyJacobianData jacobianData_t2
Definition: btSoftBody.h:349
btSoftColliders::CollideFF_DD::mrg
btScalar mrg
Definition: btSoftBodyInternals.h:1435
btSoftBody::Cluster
Definition: btSoftBody.h:433
btSoftBody::Body::xform
const btTransform & xform() const
Definition: btSoftBody.h:521
btSoftBody::Joint::m_refs
btVector3 m_refs[2]
Definition: btSoftBody.h:614
btMultiBodyJacobianData::scratch_v
btAlignedObjectArray< btVector3 > scratch_v
Definition: btMultiBodyConstraint.h:34
btSoftBody::Joint::m_split
btScalar m_split
Definition: btSoftBody.h:617
btSymMatrix::operator()
T & operator()(int c, int r)
Definition: btSoftBodyInternals.h:84
btConvexInternalShape.h
CenterOf
static btVector3 CenterOf(const btSoftBody::Face &f)
Definition: btSoftBodyInternals.h:676
btSoftColliders::ClusterBase::idt
btScalar idt
Definition: btSoftBodyInternals.h:829
btRigidBody
The btRigidBody is the main class for rigid body objects.
Definition: btRigidBody.h:59
btSoftClusterCollisionShape::calculateLocalInertia
virtual void calculateLocalInertia(btScalar mass, btVector3 &inertia) const
Definition: btSoftBodyInternals.h:191
SOFTBODY_SHAPE_PROXYTYPE
Definition: btBroadphaseProxy.h:73
btDbvt::collideTT
DBVT_PREFIX void collideTT(const btDbvtNode *root0, const btDbvtNode *root1, DBVT_IPOLICY)
Definition: btDbvt.h:822
btSoftClusterCollisionShape::getAabb
virtual void getAabb(const btTransform &t, btVector3 &aabbMin, btVector3 &aabbMax) const
getAabb's default implementation is brute force, expected derived classes to implement a fast dedicat...
Definition: btSoftBodyInternals.h:195
btSoftBody::SolverState::isdt
btScalar isdt
Definition: btSoftBody.h:721
btSoftBody::DeformableFaceNodeContact::m_node
Node * m_node
Definition: btSoftBody.h:377
ImpulseMatrix
static btMatrix3x3 ImpulseMatrix(btScalar dt, btScalar ima, btScalar imb, const btMatrix3x3 &iwi, const btVector3 &r)
Definition: btSoftBodyInternals.h:416
btDbvtAabbMm::Expand
DBVT_INLINE void Expand(const btVector3 &e)
Definition: btDbvt.h:514
btSoftBody::m_clusterConnectivity
btAlignedObjectArray< bool > m_clusterConnectivity
Definition: btSoftBody.h:811
btGjkEpa2.h
btSoftColliders::CollideSDF_RDF::m_colObj1Wrap
const btCollisionObjectWrapper * m_colObj1Wrap
Definition: btSoftBodyInternals.h:1246
Sq
static T Sq(const T &x)
Definition: btSoftBodyInternals.h:276
btSoftBody::SContact::m_normal
btVector3 m_normal
Definition: btSoftBody.h:394
Sign
static T Sign(const T &x)
Definition: btSoftBodyInternals.h:288
btSoftColliders::CollideVF_DD
Definition: btSoftBodyInternals.h:1312
btMultiBodyLinkCollider::upcast
static btMultiBodyLinkCollider * upcast(btCollisionObject *colObj)
Definition: btMultiBodyLinkCollider.h:61
btSoftBody::Config::kSRHR_CL
btScalar kSRHR_CL
Definition: btSoftBody.h:698
btSoftBody::Node::m_battach
int m_battach
Definition: btSoftBody.h:267
btEigen::mulPQ
static void mulPQ(btMatrix3x3 &a, btScalar c, btScalar s, int p, int q)
Definition: btSoftBodyInternals.h:797
btSymMatrix::btSymMatrix
btSymMatrix(int n, const T &init=T())
Definition: btSoftBodyInternals.h:72
btVector3::length
btScalar length() const
Return the length of the vector.
Definition: btVector3.h:257
btSoftBody::RContact::m_c1
btVector3 m_c1
Definition: btSoftBody.h:323
Dot
static btScalar Dot(const btScalar *a, const btScalar *b, int ndof)
Definition: btSoftBodyInternals.h:347
btSoftBody::Cluster::m_com
btVector3 m_com
Definition: btSoftBody.h:443
btSoftClusterCollisionShape::getMargin
virtual btScalar getMargin() const
Definition: btSoftBodyInternals.h:208
btSoftBody::m_cfg
Config m_cfg
Definition: btSoftBody.h:771
btConvexInternalShape
The btConvexInternalShape is an internal base class, shared by most convex shape implementations.
Definition: btConvexInternalShape.h:28
btSoftColliders::CollideVF_DD::Process
void Process(const btDbvtNode *lnode, const btDbvtNode *lface)
Definition: btSoftBodyInternals.h:1314
btSoftBody::m_faceNodeContacts
btAlignedObjectArray< DeformableFaceNodeContact > m_faceNodeContacts
Definition: btSoftBody.h:789
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
btSoftBody::SContact::m_margin
btScalar m_margin
Definition: btSoftBody.h:395
btSoftColliders::ClusterBase::friction
btScalar friction
Definition: btSoftBodyInternals.h:831
btSoftBody::DeformableFaceNodeContact::m_c0
btScalar m_c0
Definition: btSoftBody.h:385
btSoftBody::CJoint
Definition: btSoftBody.h:670
btSoftColliders::CollideVF_DD::mrg
btScalar mrg
Definition: btSoftBodyInternals.h:1363
ZeroInitialize
static void ZeroInitialize(T &value)
Definition: btSoftBodyInternals.h:220
btCollisionObjectWrapper
Definition: btCollisionObjectWrapper.h:17
btSoftBody::Node::m_n
btVector3 m_n
Definition: btSoftBody.h:263
btConcaveShape
The btConcaveShape class provides an interface for non-moving (static) concave shapes.
Definition: btConcaveShape.h:37
btSoftBody::Body
Definition: btSoftBody.h:488
btSoftBodyCollisionShape
Definition: btSoftBodyInternals.h:93
btSoftBody::DeformableNodeRigidContact
Definition: btSoftBody.h:354
btSoftColliders::CollideFF_DD::useFaceNormal
bool useFaceNormal
Definition: btSoftBodyInternals.h:1436
btSoftBody::Cluster::m_clusterIndex
int m_clusterIndex
Definition: btSoftBody.h:459
btSoftClusterCollisionShape::m_cluster
const btSoftBody::Cluster * m_cluster
Definition: btSoftBodyInternals.h:162
btSoftBody::checkDeformableContact
bool checkDeformableContact(const btCollisionObjectWrapper *colObjWrap, const btVector3 &x, btScalar margin, btSoftBody::sCti &cti, bool predict=false) const
Definition: btSoftBody.cpp:2396
btMultiBodyJacobianData::m_jacobians
btAlignedObjectArray< btScalar > m_jacobians
Definition: btMultiBodyConstraint.h:30
btVector3::cross
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
Definition: btVector3.h:380
btSoftBody::DeformableRigidContact::m_c3
btScalar m_c3
Definition: btSoftBody.h:343
btSoftBody::CJoint::m_friction
btScalar m_friction
Definition: btSoftBody.h:676
btSoftClusterCollisionShape
Definition: btSoftBodyInternals.h:159
OuterProduct
static btMatrix3x3 OuterProduct(const btScalar *v1, const btScalar *v2, const btScalar *v3, const btScalar *u1, const btScalar *u2, const btScalar *u3, int ndof)
Definition: btSoftBodyInternals.h:355
btSoftBodyWorldInfo::water_normal
btVector3 water_normal
Definition: btSoftBody.h:51
btSoftColliders::CollideSDF_RD::m_colObj1Wrap
const btCollisionObjectWrapper * m_colObj1Wrap
Definition: btSoftBodyInternals.h:1141
btSoftColliders::CollideVF_SS::psb
btSoftBody * psb[2]
Definition: btSoftBodyInternals.h:1304
btDbvntNode
Definition: btDbvt.h:194
VolumeOf
static btDbvtVolume VolumeOf(const btSoftBody::Face &f, btScalar margin)
Definition: btSoftBodyInternals.h:664
ProjectOnPlane
static btVector3 ProjectOnPlane(const btVector3 &v, const btVector3 &a)
Definition: btSoftBodyInternals.h:446
btSoftColliders::ClusterBase
Definition: btSoftBodyInternals.h:826
SameSign
static bool SameSign(const T &x, const T &y)
Definition: btSoftBodyInternals.h:294
btVector3::dot
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:229
PolarDecompose
static int PolarDecompose(const btMatrix3x3 &m, btMatrix3x3 &q, btMatrix3x3 &s)
Definition: btSoftBodyInternals.h:812
btSoftBodyCollisionShape::getName
virtual const char * getName() const
Definition: btSoftBodyInternals.h:150
EvaluateMedium
static void EvaluateMedium(const btSoftBodyWorldInfo *wfi, const btVector3 &x, btSoftBody::sMedium &medium)
Definition: btSoftBodyInternals.h:635
btCross
btVector3 btCross(const btVector3 &v1, const btVector3 &v2)
Return the cross product of two vectors.
Definition: btVector3.h:918
btSoftColliders::CollideSDF_RD::dynmargin
btScalar dynmargin
Definition: btSoftBodyInternals.h:1143
btAlignedAlloc
#define btAlignedAlloc(size, alignment)
Definition: btAlignedAllocator.h:46
btSoftBodyCollisionShape::calculateLocalInertia
virtual void calculateLocalInertia(btScalar, btVector3 &) const
Definition: btSoftBodyInternals.h:145
btGjkEpaSolver2::sResults
Definition: btGjkEpa2.h:33
btSoftColliders::CollideCL_SS
Definition: btSoftBodyInternals.h:952
btCollisionObjectWrapper::getWorldTransform
const btTransform & getWorldTransform() const
Definition: btCollisionObjectWrapper.h:44
btSoftBody::DeformableRigidContact::m_c4
btScalar m_c4
Definition: btSoftBody.h:344
btEigen
Definition: btSoftBodyInternals.h:739
btSoftColliders::CollideSDF_RS::dynmargin
btScalar dynmargin
Definition: btSoftBodyInternals.h:1052
inverse
btQuaternion inverse(const btQuaternion &q)
Return the inverse of a quaternion.
Definition: btQuaternion.h:909
btSoftBody::Body::velocity
btVector3 velocity(const btVector3 &rpos) const
Definition: btSoftBody.h:546
ApplyClampedForce
static void ApplyClampedForce(btSoftBody::Node &n, const btVector3 &f, btScalar dt)
Definition: btSoftBodyInternals.h:708
Diagonal
static btMatrix3x3 Diagonal(btScalar x)
Definition: btSoftBodyInternals.h:329
btSoftBody::Joint::m_erp
btScalar m_erp
Definition: btSoftBody.h:616
BaryCoord
static btVector3 BaryCoord(const btVector3 &a, const btVector3 &b, const btVector3 &c, const btVector3 &p)
Definition: btSoftBodyInternals.h:587
btSoftColliders::CollideSDF_RS::DoNode
void DoNode(btSoftBody::Node &n) const
Definition: btSoftBodyInternals.h:1014
AreaOf
static btScalar AreaOf(const btVector3 &x0, const btVector3 &x1, const btVector3 &x2)
Definition: btSoftBodyInternals.h:682
btSoftColliders::CollideVF_DD::psb
btSoftBody * psb[2]
Definition: btSoftBodyInternals.h:1362
btSoftColliders::CollideFF_DD
Definition: btSoftBodyInternals.h:1370
btGjkEpaSolver2::SignedDistance
static btScalar SignedDistance(const btVector3 &position, btScalar margin, const btConvexShape *shape, const btTransform &wtrs, sResults &results)
Definition: btGjkEpa2.cpp:1021
btSoftBodyCollisionShape::setLocalScaling
virtual void setLocalScaling(const btVector3 &)
Definition: btSoftBodyInternals.h:136
btSoftBody::Config::kDF
btScalar kDF
Definition: btSoftBody.h:692
btEigen::system
static int system(btMatrix3x3 &a, btMatrix3x3 *vectors, btVector3 *values=0)
Definition: btSoftBodyInternals.h:741
btSoftBody::m_cdbvt
btDbvt m_cdbvt
Definition: btSoftBody.h:799
btMin
const T & btMin(const T &a, const T &b)
Definition: btMinMax.h:21
btSoftBody::Node
Definition: btSoftBody.h:255
btSoftBodyWorldInfo
Definition: btSoftBody.h:45
btSoftBody::sCti
Definition: btSoftBody.h:218
btSoftBody::Body::invWorldInertia
const btMatrix3x3 & invWorldInertia() const
Definition: btSoftBody.h:508
btDbvtNode::data
void * data
Definition: btDbvt.h:188
btSoftClusterCollisionShape::batchedUnitVectorGetSupportingVertexWithoutMargin
virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3 *vectors, btVector3 *supportVerticesOut, int numVectors) const
Definition: btSoftBodyInternals.h:187
ClusterMetric
static btScalar ClusterMetric(const btVector3 &x, const btVector3 &y)
Definition: btSoftBodyInternals.h:299
btMax
const T & btMax(const T &a, const T &b)
Definition: btMinMax.h:27
btMultiBodyJacobianData
Definition: btMultiBodyConstraint.h:28
btVector3::y
const btScalar & y() const
Return the y value.
Definition: btVector3.h:577
btCollisionObject::isStaticOrKinematicObject
bool isStaticOrKinematicObject() const
Definition: btCollisionObject.h:205
btSoftColliders::ClusterBase::erp
btScalar erp
Definition: btSoftBodyInternals.h:828
btSoftBody::DeformableFaceRigidContact::m_bary
btVector3 m_bary
Definition: btSoftBody.h:371
btVector3::getX
const btScalar & getX() const
Return the x value.
Definition: btVector3.h:561
btSoftBody::DeformableFaceNodeContact::m_normal
btVector3 m_normal
Definition: btSoftBody.h:381
btCollisionShape::m_shapeType
int m_shapeType
Definition: btCollisionShape.h:30
btCollisionObject::getWorldTransform
btTransform & getWorldTransform()
Definition: btCollisionObject.h:367
btSoftBody::m_sst
SolverState m_sst
Definition: btSoftBody.h:772
btSoftBody::Joint::m_massmatrix
btMatrix3x3 m_massmatrix
Definition: btSoftBody.h:620
btSoftBody::Node::m_q
btVector3 m_q
Definition: btSoftBody.h:258
btSoftBody::m_scontacts
tSContactArray m_scontacts
Definition: btSoftBody.h:791
btSoftBody::DeformableFaceRigidContact::m_weights
btVector3 m_weights
Definition: btSoftBody.h:372
btSoftBody::RContact::m_node
Node * m_node
Definition: btSoftBody.h:321
btCollisionObject::CO_RIGID_BODY
Definition: btCollisionObject.h:146
btSoftBody::Cluster::m_nodes
btAlignedObjectArray< Node * > m_nodes
Definition: btSoftBody.h:436
btSoftBody::DeformableFaceNodeContact::m_imf
btScalar m_imf
Definition: btSoftBody.h:384
btAssert
#define btAssert(x)
Definition: btScalar.h:153
btDbvntNode::data
void * data
Definition: btDbvt.h:202
btSoftColliders
Definition: btSoftBodyInternals.h:821
Mul
static btMatrix3x3 Mul(const btMatrix3x3 &a, btScalar b)
Definition: btSoftBodyInternals.h:394
btSoftBody::m_faceRigidContacts
btAlignedObjectArray< DeformableFaceRigidContact > m_faceRigidContacts
Definition: btSoftBody.h:790
btSoftBody::sCti::m_colObj
const btCollisionObject * m_colObj
Definition: btSoftBody.h:220
btCollisionShape::getMargin
virtual btScalar getMargin() const =0
btFabs
btScalar btFabs(btScalar x)
Definition: btScalar.h:497
btSoftColliders::CollideSDF_RDF::DoNode
void DoNode(btSoftBody::Face &f) const
Definition: btSoftBodyInternals.h:1157
btSoftBody::Node::m_x
btVector3 m_x
Definition: btSoftBody.h:257
btSoftBody::Config::kSKHR_CL
btScalar kSKHR_CL
Definition: btSoftBody.h:699
btSymMatrix::btSymMatrix
btSymMatrix()
Definition: btSoftBodyInternals.h:71
CompGreater
static bool CompGreater(const T &a, const T &b)
Definition: btSoftBodyInternals.h:232
btSoftBody::DeformableRigidContact::jacobianData_normal
btMultiBodyJacobianData jacobianData_normal
Definition: btSoftBody.h:347
btSoftBody::sMedium::m_density
btScalar m_density
Definition: btSoftBody.h:231
btSoftBody::m_bounds
btVector3 m_bounds[2]
Definition: btSoftBody.h:795
ProjectOrigin
static void ProjectOrigin(const btVector3 &a, const btVector3 &b, btVector3 &prj, btScalar &sqd)
Definition: btSoftBodyInternals.h:453
btSoftBody::SContact::m_cfm
btScalar m_cfm[2]
Definition: btSoftBody.h:397
btSoftColliders::CollideSDF_RDF::psb
btSoftBody * psb
Definition: btSoftBodyInternals.h:1245
InvLerp
static T InvLerp(const T &a, const T &b, btScalar t)
Definition: btSoftBodyInternals.h:244
btSoftBody::RContact
Definition: btSoftBody.h:318
btSoftBodyWorldInfo::m_gravity
btVector3 m_gravity
Definition: btSoftBody.h:54
btCollisionObject::activate
void activate(bool forceActivation=false) const
Definition: btCollisionObject.cpp:72
btSoftBody::CJoint::m_rpos
btVector3 m_rpos[2]
Definition: btSoftBody.h:674
btMultiBody::calcAccelerationDeltasMultiDof
void calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v) const
stepVelocitiesMultiDof is deprecated, use computeAccelerationsArticulatedBodyAlgorithmMultiDof instea...
Definition: btMultiBody.cpp:1438
btSoftColliders::CollideCL_RS::ProcessColObj
void ProcessColObj(btSoftBody *ps, const btCollisionObjectWrapper *colObWrap)
Definition: btSoftBodyInternals.h:930
btSoftBody::SContact::m_face
Face * m_face
Definition: btSoftBody.h:392
btMultiBody::getNumDofs
int getNumDofs() const
Definition: btMultiBody.h:167
btCollisionObjectWrapper::getCollisionShape
const btCollisionShape * getCollisionShape() const
Definition: btCollisionObjectWrapper.h:46
AngularImpulseMatrix
static btMatrix3x3 AngularImpulseMatrix(const btMatrix3x3 &iia, const btMatrix3x3 &iib)
Definition: btSoftBodyInternals.h:433
btAlignedObjectArray::resize
void resize(int newsize, const T &fillData=T())
Definition: btAlignedObjectArray.h:203
btSoftBody::Face
Definition: btSoftBody.h:285
btSoftBody::RContact::m_c4
btScalar m_c4
Definition: btSoftBody.h:326
btMultiBodyLinkCollider.h
btTransform::getBasis
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:108
btVector3::setMax
void setMax(const btVector3 &other)
Set each element to the max of the current values and the values of another btVector3.
Definition: btVector3.h:609
btSoftBody::DeformableFaceRigidContact::m_contactPoint
btVector3 m_contactPoint
Definition: btSoftBody.h:370
btSoftColliders::CollideSDF_RDF::m_rigidBody
btRigidBody * m_rigidBody
Definition: btSoftBodyInternals.h:1247
btSoftColliders::CollideSDF_RS
Definition: btSoftBodyInternals.h:1007
btSoftColliders::CollideCL_RS
Definition: btSoftBodyInternals.h:890
btSoftColliders::CollideSDF_RDF
Definition: btSoftBodyInternals.h:1150
btSoftBody::DeformableNodeRigidContact::m_node
Node * m_node
Definition: btSoftBody.h:357
btSoftColliders::ClusterBase::ClusterBase
ClusterBase()
Definition: btSoftBodyInternals.h:833
btCollisionShape::getAabb
virtual void getAabb(const btTransform &t, btVector3 &aabbMin, btVector3 &aabbMax) const =0
getAabb returns the axis aligned bounding box in the coordinate frame of the given transform t.
btSoftColliders::CollideSDF_RD::stamargin
btScalar stamargin
Definition: btSoftBodyInternals.h:1144
btDbvt::collideTV
DBVT_PREFIX void collideTV(const btDbvtNode *root, const btDbvtVolume &volume, DBVT_IPOLICY) const
Definition: btDbvt.h:1148
btMatrix3x3
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:46
btSoftColliders::CollideCL_SS::ProcessSoftSoft
void ProcessSoftSoft(btSoftBody *psa, btSoftBody *psb)
Definition: btSoftBodyInternals.h:993
btSymMatrix::operator()
const T & operator()(int c, int r) const
Definition: btSoftBodyInternals.h:85
btSoftColliders::ClusterBase::threshold
btScalar threshold
Definition: btSoftBodyInternals.h:832
btSoftBody::Joint::m_bodies
Body m_bodies[2]
Definition: btSoftBody.h:613
btMatrix3x3::transpose
btMatrix3x3 transpose() const
Return the transpose of the matrix.
Definition: btMatrix3x3.h:1033
btTriangleCallback
The btTriangleCallback provides a callback for each overlapping triangle when calling processAllTrian...
Definition: btTriangleCallback.h:23
findJacobian
static void findJacobian(const btMultiBodyLinkCollider *multibodyLinkCol, btMultiBodyJacobianData &jacobianData, const btVector3 &contact_point, const btVector3 &dir)
btSoftBody implementation by Nathanael Presson
Definition: btSoftBodyInternals.h:34
btRigidBody::getInvMass
btScalar getInvMass() const
Definition: btRigidBody.h:263
btGjkEpaSolver2::sResults::normal
btVector3 normal
Definition: btGjkEpa2.h:43
btMultiBodyJacobianData::scratch_r
btAlignedObjectArray< btScalar > scratch_r
Definition: btMultiBodyConstraint.h:33
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:28
btPolarDecomposition.h
btMultiBodyLinkCollider
Definition: btMultiBodyLinkCollider.h:32
btGjkEpaSolver2::sResults::witnesses
btVector3 witnesses[2]
Definition: btGjkEpa2.h:42
btCollisionObjectWrapper::getCollisionObject
const btCollisionObject * getCollisionObject() const
Definition: btCollisionObjectWrapper.h:45
Lerp
static T Lerp(const T &a, const T &b, btScalar t)
Definition: btSoftBodyInternals.h:238
btSoftBody::sMedium::m_pressure
btScalar m_pressure
Definition: btSoftBody.h:230
generateUnitOrthogonalVector
static btVector3 generateUnitOrthogonalVector(const btVector3 &u)
Definition: btSoftBodyInternals.h:47
SIMD_INFINITY
#define SIMD_INFINITY
Definition: btScalar.h:544
btSoftBody::Config::kSK_SPLT_CL
btScalar kSK_SPLT_CL
Definition: btSoftBody.h:702
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:80
btSoftBody::sCti::m_normal
btVector3 m_normal
Definition: btSoftBody.h:221
btSoftColliders::CollideSDF_RD::DoNode
void DoNode(btSoftBody::Node &n) const
Definition: btSoftBodyInternals.h:1066
btSoftColliders::CollideSDF_RD::Process
void Process(const btDbvtNode *leaf)
Definition: btSoftBodyInternals.h:1061
btSoftBody::DeformableFaceNodeContact::m_bary
btVector3 m_bary
Definition: btSoftBody.h:379
btTransform::getOrigin
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:113
btSoftColliders::CollideFF_DD::psb
btSoftBody * psb[2]
Definition: btSoftBodyInternals.h:1434
btRigidBody::getVelocityInLocalPoint
btVector3 getVelocityInLocalPoint(const btVector3 &rel_pos) const
Definition: btRigidBody.h:419
btDbvtAabbMm::FromMM
static btDbvtAabbMm FromMM(const btVector3 &mi, const btVector3 &mx)
Definition: btDbvt.h:479
btSoftBody::DeformableRigidContact::m_c0
btMatrix3x3 m_c0
Definition: btSoftBody.h:340
btSoftBody::DeformableFaceRigidContact
Definition: btSoftBody.h:366
btSoftBodyWorldInfo::water_offset
btScalar water_offset
Definition: btSoftBody.h:49
MatchEdge
static int MatchEdge(const btSoftBody::Node *a, const btSoftBody::Node *b, const btSoftBody::Node *ma, const btSoftBody::Node *mb)
Definition: btSoftBodyInternals.h:724
btSoftBodyCollisionShape::processAllTriangles
void processAllTriangles(btTriangleCallback *, const btVector3 &, const btVector3 &) const
Definition: btSoftBodyInternals.h:108
BaryEval
static T BaryEval(const T &a, const T &b, const T &c, const btVector3 &coord)
Definition: btSoftBodyInternals.h:579
btSoftColliders::ClusterBase::SolveContact
bool SolveContact(const btGjkEpaSolver2::sResults &res, btSoftBody::Body ba, const btSoftBody::Body bb, btSoftBody::CJoint &joint)
Definition: btSoftBodyInternals.h:841
ATTRIBUTE_ALIGNED16
#define ATTRIBUTE_ALIGNED16(a)
Definition: btScalar.h:99
btSoftColliders::CollideSDF_RDF::dynmargin
btScalar dynmargin
Definition: btSoftBodyInternals.h:1248
btPolarDecomposition
This class is used to compute the polar decomposition of a matrix.
Definition: btPolarDecomposition.h:14
btConvexInternalShape::getMargin
virtual btScalar getMargin() const
Definition: btConvexInternalShape.h:106
btTransform::getIdentity
static const btTransform & getIdentity()
Return an identity transform.
Definition: btTransform.h:197
btBroadphaseInterface.h
btVector3::getZ
const btScalar & getZ() const
Return the z value.
Definition: btVector3.h:565
btSoftClusterCollisionShape::setMargin
virtual void setMargin(btScalar margin)
Definition: btSoftBodyInternals.h:204
btCollisionObject::CO_FEATHERSTONE_LINK
Definition: btCollisionObject.h:153
btSoftBody::Config::kSHR
btScalar kSHR
Definition: btSoftBody.h:696
btAlignedObjectArray
The btAlignedObjectArray template class uses a subset of the stl::vector interface for its methods It...
Definition: btAlignedObjectArray.h:45
btSwap
void btSwap(T &a, T &b)
Definition: btScalar.h:643
btVector3::getY
const btScalar & getY() const
Return the y value.
Definition: btVector3.h:563
btSymMatrix::index
int index(int c, int r) const
Definition: btSoftBodyInternals.h:78
btSoftBody::ImplicitFn::Eval
virtual btScalar Eval(const btVector3 &x)=0
btSoftBody::Joint::m_drift
btVector3 m_drift
Definition: btSoftBody.h:618
btSoftBody::m_rcontacts
tRContactArray m_rcontacts
Definition: btSoftBody.h:787
Add
static btMatrix3x3 Add(const btMatrix3x3 &a, const btMatrix3x3 &b)
Definition: btSoftBodyInternals.h:378
btDbvt::m_root
btDbvtNode * m_root
Definition: btDbvt.h:302
btConvexShape
The btConvexShape is an abstract shape interface, implemented by all convex shapes such as btBoxShape...
Definition: btConvexShape.h:31
btSoftBody::DeformableRigidContact::m_c1
btVector3 m_c1
Definition: btSoftBody.h:341
btSoftBody::Config::kKHR
btScalar kKHR
Definition: btSoftBody.h:695
btSoftColliders::CollideCL_RS::psb
btSoftBody * psb
Definition: btSoftBodyInternals.h:892
btSoftColliders::CollideSDF_RS::stamargin
btScalar stamargin
Definition: btSoftBodyInternals.h:1053
btSoftBody::Face::m_pcontact
btVector4 m_pcontact
Definition: btSoftBody.h:291
Clamp
static btVector3 Clamp(const btVector3 &v, btScalar maxlength)
Definition: btSoftBodyInternals.h:260
btDbvtAabbMm::FromPoints
static btDbvtAabbMm FromPoints(const btVector3 *pts, int n)
Definition: btDbvt.h:488
btSoftClusterCollisionShape::localGetSupportingVertexWithoutMargin
virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3 &vec) const
Definition: btSoftBodyInternals.h:182
btSoftColliders::CollideCL_RS::Process
void Process(const btDbvtNode *leaf)
Definition: btSoftBodyInternals.h:895
btVector3::setMin
void setMin(const btVector3 &other)
Set each element to the min of the current values and the values of another btVector3.
Definition: btVector3.h:626
btSoftColliders::CollideVF_SS::mrg
btScalar mrg
Definition: btSoftBodyInternals.h:1305
btSoftBody::SContact::m_weights
btVector3 m_weights
Definition: btSoftBody.h:393
Cross
static btMatrix3x3 Cross(const btVector3 &v)
Definition: btSoftBodyInternals.h:320
btDot
btScalar btDot(const btVector3 &v1, const btVector3 &v2)
Return the dot product between two vectors.
Definition: btVector3.h:890
btSoftBody::RContact::m_cti
sCti m_cti
Definition: btSoftBody.h:320
btVector3::x
const btScalar & x() const
Return the x value.
Definition: btVector3.h:575
btMultiBody::fillContactJacobianMultiDof
void fillContactJacobianMultiDof(int link, const btVector3 &contact_point, const btVector3 &normal, btScalar *jac, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v, btAlignedObjectArray< btMatrix3x3 > &scratch_m) const
Definition: btMultiBody.h:456
btSoftBody
The btSoftBody is an class to simulate cloth and volumetric soft bodies.
Definition: btSoftBody.h:72
btSoftBodyCollisionShape::getLocalScaling
virtual const btVector3 & getLocalScaling() const
Definition: btSoftBodyInternals.h:140
btSoftBody::Node::m_f
btVector3 m_f
Definition: btSoftBody.h:262
btSoftBody::DeformableFaceNodeContact::m_friction
btScalar m_friction
Definition: btSoftBody.h:383
btSoftBody::SContact::m_friction
btScalar m_friction
Definition: btSoftBody.h:396
btSoftClusterCollisionShape::getName
virtual const char * getName() const
Definition: btSoftBodyInternals.h:202
btSoftBody::checkContact
bool checkContact(const btCollisionObjectWrapper *colObjWrap, const btVector3 &x, btScalar margin, btSoftBody::sCti &cti) const
Definition: btSoftBody.cpp:2367
btSymMatrix::resize
void resize(int n, const T &init=T())
Definition: btSoftBodyInternals.h:73
btSoftBody::m_nodeRigidContacts
btAlignedObjectArray< DeformableNodeRigidContact > m_nodeRigidContacts
Definition: btSoftBody.h:788
btSoftBody::sMedium
Definition: btSoftBody.h:227
CompLess
static bool CompLess(const T &a, const T &b)
Definition: btSoftBodyInternals.h:226
btSoftBody::Joint::m_delete
bool m_delete
Definition: btSoftBody.h:621
ScaleAlongAxis
static btMatrix3x3 ScaleAlongAxis(const btVector3 &a, btScalar s)
Definition: btSoftBodyInternals.h:305
btMultiBodyLinkCollider::m_link
int m_link
Definition: btMultiBodyLinkCollider.h:37
btSoftBody::Config::kCHR
btScalar kCHR
Definition: btSoftBody.h:694
btSoftColliders::CollideSDF_RDF::stamargin
btScalar stamargin
Definition: btSoftBodyInternals.h:1249
NormalizeAny
static btVector3 NormalizeAny(const btVector3 &v)
Definition: btSoftBodyInternals.h:654
btQuickprof.h
btCollisionObject::getInternalType
int getInternalType() const
reserved for Bullet internal usage
Definition: btCollisionObject.h:362
btSoftBody::CJoint::m_normal
btVector3 m_normal
Definition: btSoftBody.h:675
btMatrix3x3::setIdentity
void setIdentity()
Set the matrix to the identity.
Definition: btMatrix3x3.h:321
ProjectOnAxis
static btVector3 ProjectOnAxis(const btVector3 &v, const btVector3 &a)
Definition: btSoftBodyInternals.h:440
btSoftColliders::ClusterBase::m_margin
btScalar m_margin
Definition: btSoftBodyInternals.h:830
btSoftBody::DeformableFaceNodeContact::m_weights
btVector3 m_weights
Definition: btSoftBody.h:380
btSoftBody::DeformableFaceNodeContact
Definition: btSoftBody.h:375
btSoftBody::DeformableRigidContact::m_cti
sCti m_cti
Definition: btSoftBody.h:339
btRigidBody::getInvInertiaTensorWorld
const btMatrix3x3 & getInvInertiaTensorWorld() const
Definition: btRigidBody.h:265
btSoftColliders::CollideSDF_RD
Definition: btSoftBodyInternals.h:1059
btSoftColliders::CollideSDF_RS::Process
void Process(const btDbvtNode *leaf)
Definition: btSoftBodyInternals.h:1009
Cube
static T Cube(const T &x)
Definition: btSoftBodyInternals.h:282
btSoftBody::sMedium::m_velocity
btVector3 m_velocity
Definition: btSoftBody.h:229
btSoftBody::CJoint::m_life
int m_life
Definition: btSoftBody.h:672
btConvexInternalShape::setMargin
virtual void setMargin(btScalar margin)
Definition: btConvexInternalShape.h:102
btSoftClusterCollisionShape::btSoftClusterCollisionShape
btSoftClusterCollisionShape(const btSoftBody::Cluster *cluster)
Definition: btSoftBodyInternals.h:164
lineIntersectsTriangle
static bool lineIntersectsTriangle(const btVector3 &rayStart, const btVector3 &rayEnd, const btVector3 &p1, const btVector3 &p2, const btVector3 &p3, btVector3 &sect, btVector3 &normal)
Definition: btSoftBodyInternals.h:540
btSoftColliders::CollideCL_RS::m_colObjWrap
const btCollisionObjectWrapper * m_colObjWrap
Definition: btSoftBodyInternals.h:893
MassMatrix
static btMatrix3x3 MassMatrix(btScalar im, const btMatrix3x3 &iwi, const btVector3 &r)
Definition: btSoftBodyInternals.h:409
ImplicitSolve
static btScalar ImplicitSolve(btSoftBody::ImplicitFn *fn, const btVector3 &a, const btVector3 &b, const btScalar accuracy, const int maxiterations=256)
Definition: btSoftBodyInternals.h:600
btDbvtAabbMm
Definition: btDbvt.h:131
btMultiBodyLinkCollider::m_multiBody
btMultiBody * m_multiBody
Definition: btMultiBodyLinkCollider.h:36
btSoftColliders::CollideVF_DD::useFaceNormal
bool useFaceNormal
Definition: btSoftBodyInternals.h:1364
btSoftBody::DeformableFaceNodeContact::m_face
Face * m_face
Definition: btSoftBody.h:378
btSoftColliders::CollideSDF_RS::m_colObj1Wrap
const btCollisionObjectWrapper * m_colObj1Wrap
Definition: btSoftBodyInternals.h:1050
btSoftBody::Cluster::m_containsAnchor
bool m_containsAnchor
Definition: btSoftBody.h:457
btSoftBody::Body::invMass
btScalar invMass() const
Definition: btSoftBody.h:515
Sub
static btMatrix3x3 Sub(const btMatrix3x3 &a, const btMatrix3x3 &b)
Definition: btSoftBodyInternals.h:386
btSoftBodyCollisionShape::~btSoftBodyCollisionShape
virtual ~btSoftBodyCollisionShape()
Definition: btSoftBodyInternals.h:104
btVector3::norm
btScalar norm() const
Return the norm (length) of the vector.
Definition: btVector3.h:263
btSymMatrix
Definition: btSoftBodyInternals.h:69
btSoftBody::DeformableRigidContact::jacobianData_t1
btMultiBodyJacobianData jacobianData_t1
Definition: btSoftBody.h:348
btSoftBody::Face::m_n
Node * m_n[3]
Definition: btSoftBody.h:287
btSoftClusterCollisionShape::localGetSupportingVertex
virtual btVector3 localGetSupportingVertex(const btVector3 &vec) const
Definition: btSoftBodyInternals.h:166
btSoftBodyWorldInfo::air_density
btScalar air_density
Definition: btSoftBody.h:47
Orthogonalize
static void Orthogonalize(btMatrix3x3 &m)
Definition: btSoftBodyInternals.h:402
btDbvt::ICollide
Definition: btDbvt.h:269
btSoftBody::checkDeformableFaceContact
bool checkDeformableFaceContact(const btCollisionObjectWrapper *colObjWrap, Face &f, btVector3 &contact_point, btVector3 &bary, btScalar margin, btSoftBody::sCti &cti, bool predict=false) const
Definition: btSoftBody.cpp:2445
btMultiBodyConstraint.h
btSoftBody::m_clusters
tClusterArray m_clusters
Definition: btSoftBody.h:800
btSoftColliders::CollideVF_SS
Definition: btSoftBodyInternals.h:1255
btSoftBody::Joint::m_cfm
btScalar m_cfm
Definition: btSoftBody.h:615
btSoftBody::SContact::m_node
Node * m_node
Definition: btSoftBody.h:391
btSoftBody::DeformableRigidContact::t2
btVector3 t2
Definition: btSoftBody.h:351
btSoftBodyCollisionShape::m_body
btSoftBody * m_body
Definition: btSoftBodyInternals.h:96
btAlignedObjectArray::push_back
void push_back(const T &_Val)
Definition: btAlignedObjectArray.h:257
btSymMatrix::dim
int dim
Definition: btSoftBodyInternals.h:87
btSoftColliders::CollideSDF_RD::psb
btSoftBody * psb
Definition: btSoftBodyInternals.h:1140
btSoftColliders::CollideVF_SS::Process
void Process(const btDbvtNode *lnode, const btDbvtNode *lface)
Definition: btSoftBodyInternals.h:1257
length
btScalar length(const btQuaternion &q)
Return the length of a quaternion.
Definition: btQuaternion.h:895
btDbvtNode
Definition: btDbvt.h:180
btSqrt
btScalar btSqrt(btScalar y)
Definition: btScalar.h:466
btSoftColliders::CollideSDF_RDF::Process
void Process(const btDbvtNode *leaf)
Definition: btSoftBodyInternals.h:1152
btSoftBody::Node::m_im
btScalar m_im
Definition: btSoftBody.h:264
btSoftColliders::CollideSDF_RS::psb
btSoftBody * psb
Definition: btSoftBodyInternals.h:1049
btSoftBody::Node::m_v
btVector3 m_v
Definition: btSoftBody.h:259
btSoftBody::m_joints
tJointArray m_joints
Definition: btSoftBody.h:792
btCollisionObject::getFriction
btScalar getFriction() const
Definition: btCollisionObject.h:313
btSoftBodyCollisionShape::btSoftBodyCollisionShape
btSoftBodyCollisionShape(btSoftBody *backptr)
Definition: btSoftBodyInternals.h:98
btSoftBody::RContact::m_c3
btScalar m_c3
Definition: btSoftBody.h:325
btVector3::normalize
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition: btVector3.h:303
btVector3::normalized
btVector3 normalized() const
Return a normalized version of this vector.
Definition: btVector3.h:949
btVector3::safeNormalize
btVector3 & safeNormalize()
Definition: btVector3.h:286
btSoftBody::Config::kSS_SPLT_CL
btScalar kSS_SPLT_CL
Definition: btSoftBody.h:703
btSoftBodyWorldInfo::water_density
btScalar water_density
Definition: btSoftBody.h:48
btMultiBodyJacobianData::scratch_m
btAlignedObjectArray< btMatrix3x3 > scratch_m
Definition: btMultiBodyConstraint.h:35
btSoftBody::CJoint::m_maxlife
int m_maxlife
Definition: btSoftBody.h:673
btSoftBodyCollisionShape::getAabb
virtual void getAabb(const btTransform &t, btVector3 &aabbMin, btVector3 &aabbMax) const
getAabb returns the axis aligned bounding box in the coordinate frame of the given transform t.
Definition: btSoftBodyInternals.h:115
btMultiBodyJacobianData::m_deltaVelocitiesUnitImpulse
btAlignedObjectArray< btScalar > m_deltaVelocitiesUnitImpulse
Definition: btMultiBodyConstraint.h:31
btSoftBody::DeformableRigidContact::t1
btVector3 t1
Definition: btSoftBody.h:350
btSoftBody::Config::kSSHR_CL
btScalar kSSHR_CL
Definition: btSoftBody.h:700
btCollisionDispatcher.h
btPolarDecomposition::decompose
unsigned int decompose(const btMatrix3x3 &a, btMatrix3x3 &u, btMatrix3x3 &h) const
Decomposes a matrix into orthogonal and symmetric, positive-definite parts.
Definition: btPolarDecomposition.cpp:38
btSoftBody.h
btVector3::z
const btScalar & z() const
Return the z value.
Definition: btVector3.h:579
btSoftColliders::CollideFF_DD::Process
void Process(const btDbvntNode *lface1, const btDbvntNode *lface2)
Definition: btSoftBodyInternals.h:1372
btSoftBody::DeformableRigidContact::m_c2
btScalar m_c2
Definition: btSoftBody.h:342
rayIntersectsTriangle
static bool rayIntersectsTriangle(const btVector3 &origin, const btVector3 &dir, const btVector3 &v0, const btVector3 &v1, const btVector3 &v2, btScalar &t)
Definition: btSoftBodyInternals.h:507
btSoftBody::DeformableFaceNodeContact::m_margin
btScalar m_margin
Definition: btSoftBody.h:382
btSoftColliders::CollideCL_SS::Process
void Process(const btDbvtNode *la, const btDbvtNode *lb)
Definition: btSoftBodyInternals.h:955
btSoftBody::SContact
Definition: btSoftBody.h:389
btAlignedObjectArray::size
int size() const
return the number of elements in the array
Definition: btAlignedObjectArray.h:142
btEigen::mulTPQ
static void mulTPQ(btMatrix3x3 &a, btScalar c, btScalar s, int p, int q)
Definition: btSoftBodyInternals.h:788
btVector3::length2
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:251
btCollisionObject::getCollisionShape
const btCollisionShape * getCollisionShape() const
Definition: btCollisionObject.h:226
btSoftBody::SolverState::sdt
btScalar sdt
Definition: btSoftBody.h:720
btGjkEpaSolver2::sResults::distance
btScalar distance
Definition: btGjkEpa2.h:44
btSoftColliders::CollideCL_SS::bodies
btSoftBody * bodies[2]
Definition: btSoftBodyInternals.h:954
btSoftBody::RContact::m_c2
btScalar m_c2
Definition: btSoftBody.h:324
btSoftBody::Config::kSR_SPLT_CL
btScalar kSR_SPLT_CL
Definition: btSoftBody.h:701
btSoftBody::DeformableFaceRigidContact::m_face
Face * m_face
Definition: btSoftBody.h:369
btSoftColliders::CollideSDF_RD::m_rigidBody
btRigidBody * m_rigidBody
Definition: btSoftBodyInternals.h:1142