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"
28 #include <string.h> //for memset
29 //
30 // btSymMatrix
31 //
32 template <typename T>
34 {
35  btSymMatrix() : dim(0) {}
36  btSymMatrix(int n, const T& init = T()) { resize(n, init); }
37  void resize(int n, const T& init = T())
38  {
39  dim = n;
40  store.resize((n * (n + 1)) / 2, init);
41  }
42  int index(int c, int r) const
43  {
44  if (c > r) btSwap(c, r);
45  btAssert(r < dim);
46  return ((r * (r + 1)) / 2 + c);
47  }
48  T& operator()(int c, int r) { return (store[index(c, r)]); }
49  const T& operator()(int c, int r) const { return (store[index(c, r)]); }
51  int dim;
52 };
53 
54 //
55 // btSoftBodyCollisionShape
56 //
58 {
59 public:
61 
63  {
65  m_body = backptr;
66  }
67 
69  {
70  }
71 
72  void processAllTriangles(btTriangleCallback* /*callback*/, const btVector3& /*aabbMin*/, const btVector3& /*aabbMax*/) const
73  {
74  //not yet
75  btAssert(0);
76  }
77 
79  virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const
80  {
81  /* t is usually identity, except when colliding against btCompoundShape. See Issue 512 */
82  const btVector3 mins = m_body->m_bounds[0];
83  const btVector3 maxs = m_body->m_bounds[1];
84  const btVector3 crns[] = {t * btVector3(mins.x(), mins.y(), mins.z()),
85  t * btVector3(maxs.x(), mins.y(), mins.z()),
86  t * btVector3(maxs.x(), maxs.y(), mins.z()),
87  t * btVector3(mins.x(), maxs.y(), mins.z()),
88  t * btVector3(mins.x(), mins.y(), maxs.z()),
89  t * btVector3(maxs.x(), mins.y(), maxs.z()),
90  t * btVector3(maxs.x(), maxs.y(), maxs.z()),
91  t * btVector3(mins.x(), maxs.y(), maxs.z())};
92  aabbMin = aabbMax = crns[0];
93  for (int i = 1; i < 8; ++i)
94  {
95  aabbMin.setMin(crns[i]);
96  aabbMax.setMax(crns[i]);
97  }
98  }
99 
100  virtual void setLocalScaling(const btVector3& /*scaling*/)
101  {
103  }
104  virtual const btVector3& getLocalScaling() const
105  {
106  static const btVector3 dummy(1, 1, 1);
107  return dummy;
108  }
109  virtual void calculateLocalInertia(btScalar /*mass*/, btVector3& /*inertia*/) const
110  {
112  btAssert(0);
113  }
114  virtual const char* getName() const
115  {
116  return "SoftBody";
117  }
118 };
119 
120 //
121 // btSoftClusterCollisionShape
122 //
124 {
125 public:
127 
129 
130  virtual btVector3 localGetSupportingVertex(const btVector3& vec) const
131  {
132  btSoftBody::Node* const* n = &m_cluster->m_nodes[0];
133  btScalar d = btDot(vec, n[0]->m_x);
134  int j = 0;
135  for (int i = 1, ni = m_cluster->m_nodes.size(); i < ni; ++i)
136  {
137  const btScalar k = btDot(vec, n[i]->m_x);
138  if (k > d)
139  {
140  d = k;
141  j = i;
142  }
143  }
144  return (n[j]->m_x);
145  }
147  {
148  return (localGetSupportingVertex(vec));
149  }
150  //notice that the vectors should be unit length
151  virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const
152  {
153  }
154 
155  virtual void calculateLocalInertia(btScalar mass, btVector3& inertia) const
156  {
157  }
158 
159  virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const
160  {
161  }
162 
163  virtual int getShapeType() const { return SOFTBODY_SHAPE_PROXYTYPE; }
164 
165  //debugging
166  virtual const char* getName() const { return "SOFTCLUSTER"; }
167 
168  virtual void setMargin(btScalar margin)
169  {
171  }
172  virtual btScalar getMargin() const
173  {
175  }
176 };
177 
178 //
179 // Inline's
180 //
181 
182 //
183 template <typename T>
184 static inline void ZeroInitialize(T& value)
185 {
186  memset(&value, 0, sizeof(T));
187 }
188 //
189 template <typename T>
190 static inline bool CompLess(const T& a, const T& b)
191 {
192  return (a < b);
193 }
194 //
195 template <typename T>
196 static inline bool CompGreater(const T& a, const T& b)
197 {
198  return (a > b);
199 }
200 //
201 template <typename T>
202 static inline T Lerp(const T& a, const T& b, btScalar t)
203 {
204  return (a + (b - a) * t);
205 }
206 //
207 template <typename T>
208 static inline T InvLerp(const T& a, const T& b, btScalar t)
209 {
210  return ((b + a * t - b * t) / (a * b));
211 }
212 //
213 static inline btMatrix3x3 Lerp(const btMatrix3x3& a,
214  const btMatrix3x3& b,
215  btScalar t)
216 {
217  btMatrix3x3 r;
218  r[0] = Lerp(a[0], b[0], t);
219  r[1] = Lerp(a[1], b[1], t);
220  r[2] = Lerp(a[2], b[2], t);
221  return (r);
222 }
223 //
224 static inline btVector3 Clamp(const btVector3& v, btScalar maxlength)
225 {
226  const btScalar sql = v.length2();
227  if (sql > (maxlength * maxlength))
228  return ((v * maxlength) / btSqrt(sql));
229  else
230  return (v);
231 }
232 //
233 template <typename T>
234 static inline T Clamp(const T& x, const T& l, const T& h)
235 {
236  return (x < l ? l : x > h ? h : x);
237 }
238 //
239 template <typename T>
240 static inline T Sq(const T& x)
241 {
242  return (x * x);
243 }
244 //
245 template <typename T>
246 static inline T Cube(const T& x)
247 {
248  return (x * x * x);
249 }
250 //
251 template <typename T>
252 static inline T Sign(const T& x)
253 {
254  return ((T)(x < 0 ? -1 : +1));
255 }
256 //
257 template <typename T>
258 static inline bool SameSign(const T& x, const T& y)
259 {
260  return ((x * y) > 0);
261 }
262 //
263 static inline btScalar ClusterMetric(const btVector3& x, const btVector3& y)
264 {
265  const btVector3 d = x - y;
266  return (btFabs(d[0]) + btFabs(d[1]) + btFabs(d[2]));
267 }
268 //
269 static inline btMatrix3x3 ScaleAlongAxis(const btVector3& a, btScalar s)
270 {
271  const btScalar xx = a.x() * a.x();
272  const btScalar yy = a.y() * a.y();
273  const btScalar zz = a.z() * a.z();
274  const btScalar xy = a.x() * a.y();
275  const btScalar yz = a.y() * a.z();
276  const btScalar zx = a.z() * a.x();
277  btMatrix3x3 m;
278  m[0] = btVector3(1 - xx + xx * s, xy * s - xy, zx * s - zx);
279  m[1] = btVector3(xy * s - xy, 1 - yy + yy * s, yz * s - yz);
280  m[2] = btVector3(zx * s - zx, yz * s - yz, 1 - zz + zz * s);
281  return (m);
282 }
283 //
284 static inline btMatrix3x3 Cross(const btVector3& v)
285 {
286  btMatrix3x3 m;
287  m[0] = btVector3(0, -v.z(), +v.y());
288  m[1] = btVector3(+v.z(), 0, -v.x());
289  m[2] = btVector3(-v.y(), +v.x(), 0);
290  return (m);
291 }
292 //
293 static inline btMatrix3x3 Diagonal(btScalar x)
294 {
295  btMatrix3x3 m;
296  m[0] = btVector3(x, 0, 0);
297  m[1] = btVector3(0, x, 0);
298  m[2] = btVector3(0, 0, x);
299  return (m);
300 }
301 //
302 static inline btMatrix3x3 Add(const btMatrix3x3& a,
303  const btMatrix3x3& b)
304 {
305  btMatrix3x3 r;
306  for (int i = 0; i < 3; ++i) r[i] = a[i] + b[i];
307  return (r);
308 }
309 //
310 static inline btMatrix3x3 Sub(const btMatrix3x3& a,
311  const btMatrix3x3& b)
312 {
313  btMatrix3x3 r;
314  for (int i = 0; i < 3; ++i) r[i] = a[i] - b[i];
315  return (r);
316 }
317 //
318 static inline btMatrix3x3 Mul(const btMatrix3x3& a,
319  btScalar b)
320 {
321  btMatrix3x3 r;
322  for (int i = 0; i < 3; ++i) r[i] = a[i] * b;
323  return (r);
324 }
325 //
326 static inline void Orthogonalize(btMatrix3x3& m)
327 {
328  m[2] = btCross(m[0], m[1]).normalized();
329  m[1] = btCross(m[2], m[0]).normalized();
330  m[0] = btCross(m[1], m[2]).normalized();
331 }
332 //
333 static inline btMatrix3x3 MassMatrix(btScalar im, const btMatrix3x3& iwi, const btVector3& r)
334 {
335  const btMatrix3x3 cr = Cross(r);
336  return (Sub(Diagonal(im), cr * iwi * cr));
337 }
338 
339 //
341  btScalar ima,
342  btScalar imb,
343  const btMatrix3x3& iwi,
344  const btVector3& r)
345 {
346  return (Diagonal(1 / dt) * Add(Diagonal(ima), MassMatrix(imb, iwi, r)).inverse());
347 }
348 
349 //
350 static inline btMatrix3x3 ImpulseMatrix(btScalar ima, const btMatrix3x3& iia, const btVector3& ra,
351  btScalar imb, const btMatrix3x3& iib, const btVector3& rb)
352 {
353  return (Add(MassMatrix(ima, iia, ra), MassMatrix(imb, iib, rb)).inverse());
354 }
355 
356 //
358  const btMatrix3x3& iib)
359 {
360  return (Add(iia, iib).inverse());
361 }
362 
363 //
364 static inline btVector3 ProjectOnAxis(const btVector3& v,
365  const btVector3& a)
366 {
367  return (a * btDot(v, a));
368 }
369 //
370 static inline btVector3 ProjectOnPlane(const btVector3& v,
371  const btVector3& a)
372 {
373  return (v - ProjectOnAxis(v, a));
374 }
375 
376 //
377 static inline void ProjectOrigin(const btVector3& a,
378  const btVector3& b,
379  btVector3& prj,
380  btScalar& sqd)
381 {
382  const btVector3 d = b - a;
383  const btScalar m2 = d.length2();
384  if (m2 > SIMD_EPSILON)
385  {
386  const btScalar t = Clamp<btScalar>(-btDot(a, d) / m2, 0, 1);
387  const btVector3 p = a + d * t;
388  const btScalar l2 = p.length2();
389  if (l2 < sqd)
390  {
391  prj = p;
392  sqd = l2;
393  }
394  }
395 }
396 //
397 static inline void ProjectOrigin(const btVector3& a,
398  const btVector3& b,
399  const btVector3& c,
400  btVector3& prj,
401  btScalar& sqd)
402 {
403  const btVector3& q = btCross(b - a, c - a);
404  const btScalar m2 = q.length2();
405  if (m2 > SIMD_EPSILON)
406  {
407  const btVector3 n = q / btSqrt(m2);
408  const btScalar k = btDot(a, n);
409  const btScalar k2 = k * k;
410  if (k2 < sqd)
411  {
412  const btVector3 p = n * k;
413  if ((btDot(btCross(a - p, b - p), q) > 0) &&
414  (btDot(btCross(b - p, c - p), q) > 0) &&
415  (btDot(btCross(c - p, a - p), q) > 0))
416  {
417  prj = p;
418  sqd = k2;
419  }
420  else
421  {
422  ProjectOrigin(a, b, prj, sqd);
423  ProjectOrigin(b, c, prj, sqd);
424  ProjectOrigin(c, a, prj, sqd);
425  }
426  }
427  }
428 }
429 
430 //
431 template <typename T>
432 static inline T BaryEval(const T& a,
433  const T& b,
434  const T& c,
435  const btVector3& coord)
436 {
437  return (a * coord.x() + b * coord.y() + c * coord.z());
438 }
439 //
440 static inline btVector3 BaryCoord(const btVector3& a,
441  const btVector3& b,
442  const btVector3& c,
443  const btVector3& p)
444 {
445  const btScalar w[] = {btCross(a - p, b - p).length(),
446  btCross(b - p, c - p).length(),
447  btCross(c - p, a - p).length()};
448  const btScalar isum = 1 / (w[0] + w[1] + w[2]);
449  return (btVector3(w[1] * isum, w[2] * isum, w[0] * isum));
450 }
451 
452 //
454  const btVector3& a,
455  const btVector3& b,
456  const btScalar accuracy,
457  const int maxiterations = 256)
458 {
459  btScalar span[2] = {0, 1};
460  btScalar values[2] = {fn->Eval(a), fn->Eval(b)};
461  if (values[0] > values[1])
462  {
463  btSwap(span[0], span[1]);
464  btSwap(values[0], values[1]);
465  }
466  if (values[0] > -accuracy) return (-1);
467  if (values[1] < +accuracy) return (-1);
468  for (int i = 0; i < maxiterations; ++i)
469  {
470  const btScalar t = Lerp(span[0], span[1], values[0] / (values[0] - values[1]));
471  const btScalar v = fn->Eval(Lerp(a, b, t));
472  if ((t <= 0) || (t >= 1)) break;
473  if (btFabs(v) < accuracy) return (t);
474  if (v < 0)
475  {
476  span[0] = t;
477  values[0] = v;
478  }
479  else
480  {
481  span[1] = t;
482  values[1] = v;
483  }
484  }
485  return (-1);
486 }
487 
488 inline static void EvaluateMedium(const btSoftBodyWorldInfo* wfi,
489  const btVector3& x,
490  btSoftBody::sMedium& medium)
491 {
492  medium.m_velocity = btVector3(0, 0, 0);
493  medium.m_pressure = 0;
494  medium.m_density = wfi->air_density;
495  if (wfi->water_density > 0)
496  {
497  const btScalar depth = -(btDot(x, wfi->water_normal) + wfi->water_offset);
498  if (depth > 0)
499  {
500  medium.m_density = wfi->water_density;
501  medium.m_pressure = depth * wfi->water_density * wfi->m_gravity.length();
502  }
503  }
504 }
505 
506 //
507 static inline btVector3 NormalizeAny(const btVector3& v)
508 {
509  const btScalar l = v.length();
510  if (l > SIMD_EPSILON)
511  return (v / l);
512  else
513  return (btVector3(0, 0, 0));
514 }
515 
516 //
517 static inline btDbvtVolume VolumeOf(const btSoftBody::Face& f,
518  btScalar margin)
519 {
520  const btVector3* pts[] = {&f.m_n[0]->m_x,
521  &f.m_n[1]->m_x,
522  &f.m_n[2]->m_x};
524  vol.Expand(btVector3(margin, margin, margin));
525  return (vol);
526 }
527 
528 //
529 static inline btVector3 CenterOf(const btSoftBody::Face& f)
530 {
531  return ((f.m_n[0]->m_x + f.m_n[1]->m_x + f.m_n[2]->m_x) / 3);
532 }
533 
534 //
535 static inline btScalar AreaOf(const btVector3& x0,
536  const btVector3& x1,
537  const btVector3& x2)
538 {
539  const btVector3 a = x1 - x0;
540  const btVector3 b = x2 - x0;
541  const btVector3 cr = btCross(a, b);
542  const btScalar area = cr.length();
543  return (area);
544 }
545 
546 //
547 static inline btScalar VolumeOf(const btVector3& x0,
548  const btVector3& x1,
549  const btVector3& x2,
550  const btVector3& x3)
551 {
552  const btVector3 a = x1 - x0;
553  const btVector3 b = x2 - x0;
554  const btVector3 c = x3 - x0;
555  return (btDot(a, btCross(b, c)));
556 }
557 
558 //
559 
560 //
561 static inline void ApplyClampedForce(btSoftBody::Node& n,
562  const btVector3& f,
563  btScalar dt)
564 {
565  const btScalar dtim = dt * n.m_im;
566  if ((f * dtim).length2() > n.m_v.length2())
567  { /* Clamp */
568  n.m_f -= ProjectOnAxis(n.m_v, f.normalized()) / dtim;
569  }
570  else
571  { /* Apply */
572  n.m_f += f;
573  }
574 }
575 
576 //
577 static inline int MatchEdge(const btSoftBody::Node* a,
578  const btSoftBody::Node* b,
579  const btSoftBody::Node* ma,
580  const btSoftBody::Node* mb)
581 {
582  if ((a == ma) && (b == mb)) return (0);
583  if ((a == mb) && (b == ma)) return (1);
584  return (-1);
585 }
586 
587 //
588 // btEigen : Extract eigen system,
589 // straitforward implementation of http://math.fullerton.edu/mathews/n2003/JacobiMethodMod.html
590 // outputs are NOT sorted.
591 //
592 struct btEigen
593 {
594  static int system(btMatrix3x3& a, btMatrix3x3* vectors, btVector3* values = 0)
595  {
596  static const int maxiterations = 16;
597  static const btScalar accuracy = (btScalar)0.0001;
598  btMatrix3x3& v = *vectors;
599  int iterations = 0;
600  vectors->setIdentity();
601  do
602  {
603  int p = 0, q = 1;
604  if (btFabs(a[p][q]) < btFabs(a[0][2]))
605  {
606  p = 0;
607  q = 2;
608  }
609  if (btFabs(a[p][q]) < btFabs(a[1][2]))
610  {
611  p = 1;
612  q = 2;
613  }
614  if (btFabs(a[p][q]) > accuracy)
615  {
616  const btScalar w = (a[q][q] - a[p][p]) / (2 * a[p][q]);
617  const btScalar z = btFabs(w);
618  const btScalar t = w / (z * (btSqrt(1 + w * w) + z));
619  if (t == t) /* [WARNING] let hope that one does not get thrown aways by some compilers... */
620  {
621  const btScalar c = 1 / btSqrt(t * t + 1);
622  const btScalar s = c * t;
623  mulPQ(a, c, s, p, q);
624  mulTPQ(a, c, s, p, q);
625  mulPQ(v, c, s, p, q);
626  }
627  else
628  break;
629  }
630  else
631  break;
632  } while ((++iterations) < maxiterations);
633  if (values)
634  {
635  *values = btVector3(a[0][0], a[1][1], a[2][2]);
636  }
637  return (iterations);
638  }
639 
640 private:
641  static inline void mulTPQ(btMatrix3x3& a, btScalar c, btScalar s, int p, int q)
642  {
643  const btScalar m[2][3] = {{a[p][0], a[p][1], a[p][2]},
644  {a[q][0], a[q][1], a[q][2]}};
645  int i;
646 
647  for (i = 0; i < 3; ++i) a[p][i] = c * m[0][i] - s * m[1][i];
648  for (i = 0; i < 3; ++i) a[q][i] = c * m[1][i] + s * m[0][i];
649  }
650  static inline void mulPQ(btMatrix3x3& a, btScalar c, btScalar s, int p, int q)
651  {
652  const btScalar m[2][3] = {{a[0][p], a[1][p], a[2][p]},
653  {a[0][q], a[1][q], a[2][q]}};
654  int i;
655 
656  for (i = 0; i < 3; ++i) a[i][p] = c * m[0][i] - s * m[1][i];
657  for (i = 0; i < 3; ++i) a[i][q] = c * m[1][i] + s * m[0][i];
658  }
659 };
660 
661 //
662 // Polar decomposition,
663 // "Computing the Polar Decomposition with Applications", Nicholas J. Higham, 1986.
664 //
665 static inline int PolarDecompose(const btMatrix3x3& m, btMatrix3x3& q, btMatrix3x3& s)
666 {
667  static const btPolarDecomposition polar;
668  return polar.decompose(m, q, s);
669 }
670 
671 //
672 // btSoftColliders
673 //
675 {
676  //
677  // ClusterBase
678  //
680  {
687  {
688  erp = (btScalar)1;
689  idt = 0;
690  m_margin = 0;
691  friction = 0;
692  threshold = (btScalar)0;
693  }
695  btSoftBody::Body ba, const btSoftBody::Body bb,
696  btSoftBody::CJoint& joint)
697  {
698  if (res.distance < m_margin)
699  {
700  btVector3 norm = res.normal;
701  norm.normalize(); //is it necessary?
702 
703  const btVector3 ra = res.witnesses[0] - ba.xform().getOrigin();
704  const btVector3 rb = res.witnesses[1] - bb.xform().getOrigin();
705  const btVector3 va = ba.velocity(ra);
706  const btVector3 vb = bb.velocity(rb);
707  const btVector3 vrel = va - vb;
708  const btScalar rvac = btDot(vrel, norm);
709  btScalar depth = res.distance - m_margin;
710 
711  // printf("depth=%f\n",depth);
712  const btVector3 iv = norm * rvac;
713  const btVector3 fv = vrel - iv;
714  joint.m_bodies[0] = ba;
715  joint.m_bodies[1] = bb;
716  joint.m_refs[0] = ra * ba.xform().getBasis();
717  joint.m_refs[1] = rb * bb.xform().getBasis();
718  joint.m_rpos[0] = ra;
719  joint.m_rpos[1] = rb;
720  joint.m_cfm = 1;
721  joint.m_erp = 1;
722  joint.m_life = 0;
723  joint.m_maxlife = 0;
724  joint.m_split = 1;
725 
726  joint.m_drift = depth * norm;
727 
728  joint.m_normal = norm;
729  // printf("normal=%f,%f,%f\n",res.normal.getX(),res.normal.getY(),res.normal.getZ());
730  joint.m_delete = false;
731  joint.m_friction = fv.length2() < (rvac * friction * rvac * friction) ? 1 : friction;
732  joint.m_massmatrix = ImpulseMatrix(ba.invMass(), ba.invWorldInertia(), joint.m_rpos[0],
733  bb.invMass(), bb.invWorldInertia(), joint.m_rpos[1]);
734 
735  return (true);
736  }
737  return (false);
738  }
739  };
740  //
741  // CollideCL_RS
742  //
744  {
747 
748  void Process(const btDbvtNode* leaf)
749  {
750  btSoftBody::Cluster* cluster = (btSoftBody::Cluster*)leaf->data;
751  btSoftClusterCollisionShape cshape(cluster);
752 
753  const btConvexShape* rshape = (const btConvexShape*)m_colObjWrap->getCollisionShape();
754 
757  return;
758 
761  rshape, m_colObjWrap->getWorldTransform(),
762  btVector3(1, 0, 0), res))
763  {
764  btSoftBody::CJoint joint;
765  if (SolveContact(res, cluster, m_colObjWrap->getCollisionObject(), joint)) //prb,joint))
766  {
768  *pj = joint;
769  psb->m_joints.push_back(pj);
771  {
772  pj->m_erp *= psb->m_cfg.kSKHR_CL;
773  pj->m_split *= psb->m_cfg.kSK_SPLT_CL;
774  }
775  else
776  {
777  pj->m_erp *= psb->m_cfg.kSRHR_CL;
778  pj->m_split *= psb->m_cfg.kSR_SPLT_CL;
779  }
780  }
781  }
782  }
784  {
785  psb = ps;
786  m_colObjWrap = colObWrap;
787  idt = ps->m_sst.isdt;
791  btVector3 mins;
792  btVector3 maxs;
793 
795  volume;
796  colObWrap->getCollisionShape()->getAabb(colObWrap->getWorldTransform(), mins, maxs);
797  volume = btDbvtVolume::FromMM(mins, maxs);
798  volume.Expand(btVector3(1, 1, 1) * m_margin);
799  ps->m_cdbvt.collideTV(ps->m_cdbvt.m_root, volume, *this);
800  }
801  };
802  //
803  // CollideCL_SS
804  //
806  {
808  void Process(const btDbvtNode* la, const btDbvtNode* lb)
809  {
812 
813  bool connected = false;
814  if ((bodies[0] == bodies[1]) && (bodies[0]->m_clusterConnectivity.size()))
815  {
816  connected = bodies[0]->m_clusterConnectivity[cla->m_clusterIndex + bodies[0]->m_clusters.size() * clb->m_clusterIndex];
817  }
818 
819  if (!connected)
820  {
825  &csb, btTransform::getIdentity(),
826  cla->m_com - clb->m_com, res))
827  {
828  btSoftBody::CJoint joint;
829  if (SolveContact(res, cla, clb, joint))
830  {
832  *pj = joint;
833  bodies[0]->m_joints.push_back(pj);
834  pj->m_erp *= btMax(bodies[0]->m_cfg.kSSHR_CL, bodies[1]->m_cfg.kSSHR_CL);
835  pj->m_split *= (bodies[0]->m_cfg.kSS_SPLT_CL + bodies[1]->m_cfg.kSS_SPLT_CL) / 2;
836  }
837  }
838  }
839  else
840  {
841  static int count = 0;
842  count++;
843  //printf("count=%d\n",count);
844  }
845  }
847  {
848  idt = psa->m_sst.isdt;
849  //m_margin = (psa->getCollisionShape()->getMargin()+psb->getCollisionShape()->getMargin())/2;
851  friction = btMin(psa->m_cfg.kDF, psb->m_cfg.kDF);
852  bodies[0] = psa;
853  bodies[1] = psb;
854  psa->m_cdbvt.collideTT(psa->m_cdbvt.m_root, psb->m_cdbvt.m_root, *this);
855  }
856  };
857  //
858  // CollideSDF_RS
859  //
861  {
862  void Process(const btDbvtNode* leaf)
863  {
864  btSoftBody::Node* node = (btSoftBody::Node*)leaf->data;
865  DoNode(*node);
866  }
867  void DoNode(btSoftBody::Node& n) const
868  {
869  const btScalar m = n.m_im > 0 ? dynmargin : stamargin;
871 
872  if ((!n.m_battach) &&
874  {
875  const btScalar ima = n.m_im;
876  const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f;
877  const btScalar ms = ima + imb;
878  if (ms > 0)
879  {
881  static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0);
882  const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic;
883  const btVector3 ra = n.m_x - wtr.getOrigin();
885  const btVector3 vb = n.m_x - n.m_q;
886  const btVector3 vr = vb - va;
887  const btScalar dn = btDot(vr, c.m_cti.m_normal);
888  const btVector3 fv = vr - c.m_cti.m_normal * dn;
890  c.m_node = &n;
891  c.m_c0 = ImpulseMatrix(psb->m_sst.sdt, ima, imb, iwi, ra);
892  c.m_c1 = ra;
893  c.m_c2 = ima * psb->m_sst.sdt;
894  c.m_c3 = fv.length2() < (dn * fc * dn * fc) ? 0 : 1 - fc;
897  if (m_rigidBody)
899  }
900  }
901  }
907  };
908  //
909  // CollideVF_SS
910  //
912  {
913  void Process(const btDbvtNode* lnode,
914  const btDbvtNode* lface)
915  {
916  btSoftBody::Node* node = (btSoftBody::Node*)lnode->data;
917  btSoftBody::Face* face = (btSoftBody::Face*)lface->data;
918  btVector3 o = node->m_x;
919  btVector3 p;
921  ProjectOrigin(face->m_n[0]->m_x - o,
922  face->m_n[1]->m_x - o,
923  face->m_n[2]->m_x - o,
924  p, d);
925  const btScalar m = mrg + (o - node->m_q).length() * 2;
926  if (d < (m * m))
927  {
928  const btSoftBody::Node* n[] = {face->m_n[0], face->m_n[1], face->m_n[2]};
929  const btVector3 w = BaryCoord(n[0]->m_x, n[1]->m_x, n[2]->m_x, p + o);
930  const btScalar ma = node->m_im;
931  btScalar mb = BaryEval(n[0]->m_im, n[1]->m_im, n[2]->m_im, w);
932  if ((n[0]->m_im <= 0) ||
933  (n[1]->m_im <= 0) ||
934  (n[2]->m_im <= 0))
935  {
936  mb = 0;
937  }
938  const btScalar ms = ma + mb;
939  if (ms > 0)
940  {
942  c.m_normal = p / -btSqrt(d);
943  c.m_margin = m;
944  c.m_node = node;
945  c.m_face = face;
946  c.m_weights = w;
947  c.m_friction = btMax(psb[0]->m_cfg.kDF, psb[1]->m_cfg.kDF);
948  c.m_cfm[0] = ma / ms * psb[0]->m_cfg.kSHR;
949  c.m_cfm[1] = mb / ms * psb[1]->m_cfg.kSHR;
950  psb[0]->m_scontacts.push_back(c);
951  }
952  }
953  }
956  };
957 };
958 
959 #endif //_BT_SOFT_BODY_INTERNALS_H
static void Orthogonalize(btMatrix3x3 &m)
#define SIMD_EPSILON
Definition: btScalar.h:523
static T Sq(const T &x)
btScalar length(const btQuaternion &q)
Return the length of a quaternion.
Definition: btQuaternion.h:895
int index(int c, int r) const
btScalar kSS_SPLT_CL
Definition: btSoftBody.h:619
void push_back(const T &_Val)
Config m_cfg
Definition: btSoftBody.h:685
btMatrix3x3 m_c0
Definition: btSoftBody.h:298
static void ZeroInitialize(T &value)
static btVector3 CenterOf(const btSoftBody::Face &f)
btVector3 m_normal
Definition: btSoftBody.h:216
btScalar water_offset
Definition: btSoftBody.h:47
btVector3 m_rpos[2]
Definition: btSoftBody.h:590
btSoftBody implementation by Nathanael Presson
static btMatrix3x3 ImpulseMatrix(btScalar dt, btScalar ima, btScalar imb, const btMatrix3x3 &iwi, const btVector3 &r)
The btAlignedObjectArray template class uses a subset of the stl::vector interface for its methods It...
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:251
btScalar kSK_SPLT_CL
Definition: btSoftBody.h:618
void * data
Definition: btDbvt.h:188
btSymMatrix(int n, const T &init=T())
The btConvexInternalShape is an internal base class, shared by most convex shape implementations.
const btCollisionObjectWrapper * m_colObj1Wrap
T & operator()(int c, int r)
tJointArray m_joints
Definition: btSoftBody.h:698
btAlignedObjectArray< bool > m_clusterConnectivity
Definition: btSoftBody.h:708
virtual btVector3 localGetSupportingVertex(const btVector3 &vec) const
static btDbvtAabbMm FromPoints(const btVector3 *pts, int n)
Definition: btDbvt.h:443
virtual const btVector3 & getLocalScaling() const
btScalar m_split
Definition: btSoftBody.h:533
btScalar btSqrt(btScalar y)
Definition: btScalar.h:446
#define btAssert(x)
Definition: btScalar.h:133
btAlignedObjectArray< Node * > m_nodes
Definition: btSoftBody.h:352
bool SolveContact(const btGjkEpaSolver2::sResults &res, btSoftBody::Body ba, const btSoftBody::Body bb, btSoftBody::CJoint &joint)
virtual int getShapeType() const
static T Sign(const T &x)
static btMatrix3x3 Mul(const btMatrix3x3 &a, btScalar b)
unsigned int decompose(const btMatrix3x3 &a, btMatrix3x3 &u, btMatrix3x3 &h) const
Decomposes a matrix into orthogonal and symmetric, positive-definite parts.
static btScalar AreaOf(const btVector3 &x0, const btVector3 &x1, const btVector3 &x2)
tSContactArray m_scontacts
Definition: btSoftBody.h:697
btDbvtNode * m_root
Definition: btDbvt.h:263
static bool CompGreater(const T &a, const T &b)
btVector3 water_normal
Definition: btSoftBody.h:49
static void ProjectOrigin(const btVector3 &a, const btVector3 &b, btVector3 &prj, btScalar &sqd)
static bool SameSign(const T &x, const T &y)
static T InvLerp(const T &a, const T &b, btScalar t)
virtual void getAabb(const btTransform &t, btVector3 &aabbMin, btVector3 &aabbMax) const
getAabb&#39;s default implementation is brute force, expected derived classes to implement a fast dedicat...
static int PolarDecompose(const btMatrix3x3 &m, btMatrix3x3 &q, btMatrix3x3 &s)
static void EvaluateMedium(const btSoftBodyWorldInfo *wfi, const btVector3 &x, btSoftBody::sMedium &medium)
const btCollisionShape * getCollisionShape() const
void ProcessColObj(btSoftBody *ps, const btCollisionObjectWrapper *colObWrap)
virtual void calculateLocalInertia(btScalar, btVector3 &) const
btQuaternion inverse(const btQuaternion &q)
Return the inverse of a quaternion.
Definition: btQuaternion.h:909
bool isStaticOrKinematicObject() const
void DoNode(btSoftBody::Node &n) const
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...
static btMatrix3x3 Diagonal(btScalar x)
Node * m_n[3]
Definition: btSoftBody.h:278
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition: btVector3.h:303
btVector3 normalized() const
Return a normalized version of this vector.
Definition: btVector3.h:949
The btConvexShape is an abstract shape interface, implemented by all convex shapes such as btBoxShape...
Definition: btConvexShape.h:31
static btDbvtVolume VolumeOf(const btSoftBody::Face &f, btScalar margin)
static btVector3 ProjectOnPlane(const btVector3 &v, const btVector3 &a)
btVector3 btCross(const btVector3 &v1, const btVector3 &v2)
Return the cross product of two vectors.
Definition: btVector3.h:918
virtual const char * getName() const
void Process(const btDbvtNode *lnode, const btDbvtNode *lface)
void activate(bool forceActivation=false) const
#define SIMD_INFINITY
Definition: btScalar.h:524
btTransform & getWorldTransform()
btScalar kSR_SPLT_CL
Definition: btSoftBody.h:617
virtual void setMargin(btScalar margin)
virtual void setLocalScaling(const btVector3 &)
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:113
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...
static void ApplyClampedForce(btSoftBody::Node &n, const btVector3 &f, btScalar dt)
btVector3 m_normal
Definition: btSoftBody.h:591
btSoftBodyCollisionShape(btSoftBody *backptr)
btVector3 m_velocity
Definition: btSoftBody.h:223
static btVector3 BaryCoord(const btVector3 &a, const btVector3 &b, const btVector3 &c, const btVector3 &p)
const btScalar & x() const
Return the x value.
Definition: btVector3.h:575
The btTriangleCallback provides a callback for each overlapping triangle when calling processAllTrian...
void btSwap(T &a, T &b)
Definition: btScalar.h:623
btScalar getInvMass() const
Definition: btRigidBody.h:261
static btDbvtAabbMm FromMM(const btVector3 &mi, const btVector3 &mx)
Definition: btDbvt.h:434
virtual btScalar getMargin() const
const btCollisionObject * getCollisionObject() const
static btScalar ClusterMetric(const btVector3 &x, const btVector3 &y)
void processAllTriangles(btTriangleCallback *, const btVector3 &, const btVector3 &) const
virtual void calculateLocalInertia(btScalar mass, btVector3 &inertia) const
const btScalar & y() const
Return the y value.
Definition: btVector3.h:577
const btScalar & z() const
Return the z value.
Definition: btVector3.h:579
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:108
DBVT_PREFIX void collideTV(const btDbvtNode *root, const btDbvtVolume &volume, DBVT_IPOLICY) const
Definition: btDbvt.h:974
static btVector3 Clamp(const btVector3 &v, btScalar maxlength)
The btRigidBody is the main class for rigid body objects.
Definition: btRigidBody.h:59
static btMatrix3x3 Cross(const btVector3 &v)
DBVT_INLINE void Expand(const btVector3 &e)
Definition: btDbvt.h:469
static int MatchEdge(const btSoftBody::Node *a, const btSoftBody::Node *b, const btSoftBody::Node *ma, const btSoftBody::Node *mb)
btScalar m_cfm[2]
Definition: btSoftBody.h:313
const btMatrix3x3 & invWorldInertia() const
Definition: btSoftBody.h:424
static T BaryEval(const T &a, const T &b, const T &c, const btVector3 &coord)
static bool CompLess(const T &a, const T &b)
static btMatrix3x3 ScaleAlongAxis(const btVector3 &a, btScalar s)
static btMatrix3x3 AngularImpulseMatrix(const btMatrix3x3 &iia, const btMatrix3x3 &iib)
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:80
#define ATTRIBUTE_ALIGNED16(a)
Definition: btScalar.h:84
const btSoftBody::Cluster * m_cluster
virtual btScalar Eval(const btVector3 &x)=0
int size() const
return the number of elements in the array
btVector3 getVelocityInLocalPoint(const btVector3 &rel_pos) const
Definition: btRigidBody.h:374
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:28
virtual btScalar getMargin() const
static T Cube(const T &x)
btVector3 m_refs[2]
Definition: btSoftBody.h:530
btVector3 velocity(const btVector3 &rpos) const
Definition: btSoftBody.h:462
static btMatrix3x3 MassMatrix(btScalar im, const btMatrix3x3 &iwi, const btVector3 &r)
static btScalar ImplicitSolve(btSoftBody::ImplicitFn *fn, const btVector3 &a, const btVector3 &b, const btScalar accuracy, const int maxiterations=256)
const btCollisionObjectWrapper * m_colObjWrap
btDbvt m_cdbvt
Definition: btSoftBody.h:705
static void mulPQ(btMatrix3x3 &a, btScalar c, btScalar s, int p, int q)
The btConcaveShape class provides an interface for non-moving (static) concave shapes.
void resize(int n, const T &init=T())
static const btTransform & getIdentity()
Return an identity transform.
Definition: btTransform.h:197
btVector3 witnesses[2]
Definition: btGjkEpa2.h:42
static T Lerp(const T &a, const T &b, btScalar t)
btVector3 m_n
Definition: btSoftBody.h:255
void ProcessSoftSoft(btSoftBody *psa, btSoftBody *psb)
static void mulTPQ(btMatrix3x3 &a, btScalar c, btScalar s, int p, int q)
SolverState m_sst
Definition: btSoftBody.h:686
btAlignedObjectArray< T > store
tClusterArray m_clusters
Definition: btSoftBody.h:706
virtual btScalar getMargin() const =0
const btTransform & getWorldTransform() const
virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3 *vectors, btVector3 *supportVerticesOut, int numVectors) const
void Process(const btDbvtNode *leaf)
bool checkContact(const btCollisionObjectWrapper *colObjWrap, const btVector3 &x, btScalar margin, btSoftBody::sCti &cti) const
const btTransform & xform() const
Definition: btSoftBody.h:437
const T & btMax(const T &a, const T &b)
Definition: btMinMax.h:27
static btScalar SignedDistance(const btVector3 &position, btScalar margin, const btConvexShape *shape, const btTransform &wtrs, sResults &results)
Definition: btGjkEpa2.cpp:1021
static btMatrix3x3 Sub(const btMatrix3x3 &a, const btMatrix3x3 &b)
void Process(const btDbvtNode *leaf)
btMatrix3x3 m_massmatrix
Definition: btSoftBody.h:536
btScalar water_density
Definition: btSoftBody.h:46
btVector3 m_bounds[2]
Definition: btSoftBody.h:701
btVector3 m_v
Definition: btSoftBody.h:253
#define btAlignedAlloc(size, alignment)
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:46
static btVector3 NormalizeAny(const btVector3 &v)
static btMatrix3x3 Add(const btMatrix3x3 &a, const btMatrix3x3 &b)
static btVector3 ProjectOnAxis(const btVector3 &v, const btVector3 &a)
DBVT_PREFIX void collideTT(const btDbvtNode *root0, const btDbvtNode *root1, DBVT_IPOLICY)
Definition: btDbvt.h:777
btScalar air_density
Definition: btSoftBody.h:45
btSoftClusterCollisionShape(const btSoftBody::Cluster *cluster)
const btMatrix3x3 & getInvInertiaTensorWorld() const
Definition: btRigidBody.h:262
btScalar btDot(const btVector3 &v1, const btVector3 &v2)
Return the dot product between two vectors.
Definition: btVector3.h:890
btVector3 m_q
Definition: btSoftBody.h:252
void setMax(const btVector3 &other)
Set each element to the max of the current values and the values of another btVector3.
Definition: btVector3.h:609
btVector3 m_f
Definition: btSoftBody.h:254
btVector3 m_gravity
Definition: btSoftBody.h:52
void Process(const btDbvtNode *la, const btDbvtNode *lb)
btScalar invMass() const
Definition: btSoftBody.h:431
virtual void setMargin(btScalar margin)
This class is used to compute the polar decomposition of a matrix.
The btSoftBody is an class to simulate cloth and volumetric soft bodies.
Definition: btSoftBody.h:70
const T & btMin(const T &a, const T &b)
Definition: btMinMax.h:21
const T & operator()(int c, int r) const
btScalar getFriction() const
btVector3 m_drift
Definition: btSoftBody.h:534
btVector3 m_x
Definition: btSoftBody.h:251
virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3 &vec) const
tRContactArray m_rcontacts
Definition: btSoftBody.h:696
void setIdentity()
Set the matrix to the identity.
Definition: btMatrix3x3.h:314
static int system(btMatrix3x3 &a, btMatrix3x3 *vectors, btVector3 *values=0)
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
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:294
const btCollisionShape * getCollisionShape() const
btScalar m_friction
Definition: btSoftBody.h:592
virtual const char * getName() const
btScalar length() const
Return the length of the vector.
Definition: btVector3.h:257
btScalar btFabs(btScalar x)
Definition: btScalar.h:477