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
#define btAlignedAlloc(size, alignment)
@ SOFTBODY_SHAPE_PROXYTYPE
const T & btMax(const T &a, const T &b)
Definition: btMinMax.h:27
const T & btMin(const T &a, const T &b)
Definition: btMinMax.h:21
btQuaternion inverse(const btQuaternion &q)
Return the inverse of a quaternion.
Definition: btQuaternion.h:909
btScalar length(const btQuaternion &q)
Return the length of a quaternion.
Definition: btQuaternion.h:895
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:99
btScalar btSqrt(btScalar y)
Definition: btScalar.h:466
btScalar btFabs(btScalar x)
Definition: btScalar.h:497
#define SIMD_INFINITY
Definition: btScalar.h:544
#define SIMD_EPSILON
Definition: btScalar.h:543
void btSwap(T &a, T &b)
Definition: btScalar.h:643
#define btAssert(x)
Definition: btScalar.h:153
static btVector3 ProjectOnAxis(const btVector3 &v, const btVector3 &a)
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 btMatrix3x3 MassMatrix(btScalar im, const btMatrix3x3 &iwi, const btVector3 &r)
static btDbvtVolume VolumeOf(const btSoftBody::Face &f, btScalar margin)
static T Lerp(const T &a, const T &b, btScalar t)
static btMatrix3x3 AngularImpulseMatrix(const btMatrix3x3 &iia, const btMatrix3x3 &iib)
static btScalar AreaOf(const btVector3 &x0, const btVector3 &x1, const btVector3 &x2)
static T Sq(const T &x)
static btVector3 generateUnitOrthogonalVector(const btVector3 &u)
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 void findJacobian(const btMultiBodyLinkCollider *multibodyLinkCol, btMultiBodyJacobianData &jacobianData, const btVector3 &contact_point, const btVector3 &dir)
btSoftBody implementation by Nathanael Presson
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 bool SameSign(const T &x, const T &y)
static void EvaluateMedium(const btSoftBodyWorldInfo *wfi, const btVector3 &x, btSoftBody::sMedium &medium)
static btVector3 BaryCoord(const btVector3 &a, const btVector3 &b, const btVector3 &c, const btVector3 &p)
static btMatrix3x3 ScaleAlongAxis(const btVector3 &a, btScalar s)
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 btScalar ClusterMetric(const btVector3 &x, const btVector3 &y)
static btMatrix3x3 ImpulseMatrix(btScalar dt, btScalar ima, btScalar imb, const btMatrix3x3 &iwi, const btVector3 &r)
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 void ProjectOrigin(const btVector3 &a, const btVector3 &b, btVector3 &prj, btScalar &sqd)
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 btMatrix3x3 Diagonal(btScalar x)
static T Cube(const T &x)
btScalar btDot(const btVector3 &v1, const btVector3 &v2)
Return the dot product between two vectors.
Definition: btVector3.h:890
btVector3 btCross(const btVector3 &v1, const btVector3 &v2)
Return the cross product of two vectors.
Definition: btVector3.h:918
The btAlignedObjectArray template class uses a subset of the stl::vector interface for its methods It...
int size() const
return the number of elements in the array
void resize(int newsize, const T &fillData=T())
void push_back(const T &_Val)
bool isStaticOrKinematicObject() const
btTransform & getWorldTransform()
int getInternalType() const
reserved for Bullet internal usage
void activate(bool forceActivation=false) const
btScalar getFriction() const
const btCollisionShape * getCollisionShape() const
virtual btScalar getMargin() const =0
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.
The btConcaveShape class provides an interface for non-moving (static) concave shapes.
The btConvexInternalShape is an internal base class, shared by most convex shape implementations.
virtual void setMargin(btScalar margin)
virtual btScalar getMargin() const
The btConvexShape is an abstract shape interface, implemented by all convex shapes such as btBoxShape...
Definition: btConvexShape.h:33
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:48
btMatrix3x3 transpose() const
Return the transpose of the matrix.
Definition: btMatrix3x3.h:1033
void setIdentity()
Set the matrix to the identity.
Definition: btMatrix3x3.h:321
static btMultiBodyLinkCollider * upcast(btCollisionObject *colObj)
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
void calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v) const
stepVelocitiesMultiDof is deprecated, use computeAccelerationsArticulatedBodyAlgorithmMultiDof instea...
int getNumDofs() const
Definition: btMultiBody.h:167
This class is used to compute the polar decomposition of a matrix.
unsigned int decompose(const btMatrix3x3 &a, btMatrix3x3 &u, btMatrix3x3 &h) const
Decomposes a matrix into orthogonal and symmetric, positive-definite parts.
The btRigidBody is the main class for rigid body objects.
Definition: btRigidBody.h:60
btVector3 getVelocityInLocalPoint(const btVector3 &rel_pos) const
Definition: btRigidBody.h:419
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.
btMultiBodyJacobianData jacobianData_t1
Definition: btSoftBody.h:348
btMultiBodyJacobianData jacobianData_normal
Definition: btSoftBody.h:347
btMultiBodyJacobianData jacobianData_t2
Definition: btSoftBody.h:349
The btSoftBody is an class to simulate cloth and volumetric soft bodies.
Definition: btSoftBody.h:73
btDbvt m_cdbvt
Definition: btSoftBody.h:799
tJointArray m_joints
Definition: btSoftBody.h:792
btAlignedObjectArray< bool > m_clusterConnectivity
Definition: btSoftBody.h:811
SolverState m_sst
Definition: btSoftBody.h:772
btAlignedObjectArray< DeformableFaceNodeContact > m_faceNodeContacts
Definition: btSoftBody.h:789
bool checkDeformableContact(const btCollisionObjectWrapper *colObjWrap, const btVector3 &x, btScalar margin, btSoftBody::sCti &cti, bool predict=false) const
Config m_cfg
Definition: btSoftBody.h:771
tRContactArray m_rcontacts
Definition: btSoftBody.h:787
btVector3 m_bounds[2]
Definition: btSoftBody.h:795
tClusterArray m_clusters
Definition: btSoftBody.h:800
btAlignedObjectArray< DeformableFaceRigidContact > m_faceRigidContacts
Definition: btSoftBody.h:790
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:788
tSContactArray m_scontacts
Definition: btSoftBody.h:791
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
getAabb's default implementation is brute force, expected derived classes to implement a fast dedicat...
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
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:30
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:108
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:113
static const btTransform & getIdentity()
Return an identity transform.
Definition: btTransform.h:197
The btTriangleCallback provides a callback for each overlapping triangle when calling processAllTrian...
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:82
btVector3 & safeNormalize()
Definition: btVector3.h:286
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
const btScalar & y() const
Return the y value.
Definition: btVector3.h:577
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition: btVector3.h:303
btScalar length() const
Return the length of the vector.
Definition: btVector3.h:257
const btScalar & z() const
Return the z value.
Definition: btVector3.h:579
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
Definition: btVector3.h:380
const btScalar & getX() const
Return the x value.
Definition: btVector3.h:561
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:229
btScalar norm() const
Return the norm (length) of the vector.
Definition: btVector3.h:263
const btScalar & getZ() const
Return the z value.
Definition: btVector3.h:565
btVector3 normalized() const
Return a normalized version of this vector.
Definition: btVector3.h:949
const btScalar & x() const
Return the x value.
Definition: btVector3.h:575
const btScalar & getY() const
Return the y value.
Definition: btVector3.h:563
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:251
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
const btCollisionShape * getCollisionShape() const
const btCollisionObject * getCollisionObject() const
const btTransform & getWorldTransform() 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 void mulPQ(btMatrix3x3 &a, btScalar c, btScalar s, int p, int q)
static int system(btMatrix3x3 &a, btMatrix3x3 *vectors, btVector3 *values=0)
static void mulTPQ(btMatrix3x3 &a, btScalar c, btScalar s, int p, int q)
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:47
btScalar water_density
Definition: btSoftBody.h:48
btVector3 m_gravity
Definition: btSoftBody.h:54
btVector3 water_normal
Definition: btSoftBody.h:51
btScalar water_offset
Definition: btSoftBody.h:49
btScalar invMass() const
Definition: btSoftBody.h:515
const btTransform & xform() const
Definition: btSoftBody.h:521
btVector3 velocity(const btVector3 &rpos) const
Definition: btSoftBody.h:546
const btMatrix3x3 & invWorldInertia() const
Definition: btSoftBody.h:508
btVector3 m_rpos[2]
Definition: btSoftBody.h:674
btVector3 m_normal
Definition: btSoftBody.h:675
btScalar m_friction
Definition: btSoftBody.h:676
btAlignedObjectArray< Node * > m_nodes
Definition: btSoftBody.h:436
btScalar kSK_SPLT_CL
Definition: btSoftBody.h:702
btScalar kSS_SPLT_CL
Definition: btSoftBody.h:703
btScalar kSR_SPLT_CL
Definition: btSoftBody.h:701
btVector4 m_pcontact
Definition: btSoftBody.h:291
Node * m_n[3]
Definition: btSoftBody.h:287
virtual btScalar Eval(const btVector3 &x)=0
btVector3 m_drift
Definition: btSoftBody.h:618
btScalar m_split
Definition: btSoftBody.h:617
btMatrix3x3 m_massmatrix
Definition: btSoftBody.h:620
btVector3 m_refs[2]
Definition: btSoftBody.h:614
btVector3 m_x
Definition: btSoftBody.h:257
btVector3 m_v
Definition: btSoftBody.h:259
btVector3 m_q
Definition: btSoftBody.h:258
btVector3 m_n
Definition: btSoftBody.h:263
btVector3 m_f
Definition: btSoftBody.h:262
btMatrix3x3 m_c0
Definition: btSoftBody.h:322
btScalar m_cfm[2]
Definition: btSoftBody.h:397
const btCollisionObject * m_colObj
Definition: btSoftBody.h:220
btVector3 m_normal
Definition: btSoftBody.h:221
btVector3 m_velocity
Definition: btSoftBody.h:229
bool SolveContact(const btGjkEpaSolver2::sResults &res, btSoftBody::Body ba, const btSoftBody::Body bb, btSoftBody::CJoint &joint)
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 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