Bullet Collision Detection & Physics Library
btSoftBodyInternals.h
Go to the documentation of this file.
1/*
2Bullet Continuous Collision Detection and Physics Library
3Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
4
5This software is provided 'as-is', without any express or implied warranty.
6In no event will the authors be held liable for any damages arising from the use of this software.
7Permission is granted to anyone to use this software for any purpose,
8including commercial applications, and to alter it and redistribute it freely,
9subject to the following restrictions:
10
111. 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.
122. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
133. 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
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
34static 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//
68template <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{
95public:
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{
161public:
163
165
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//
219template <typename T>
220static inline void ZeroInitialize(T& value)
221{
222 memset(&value, 0, sizeof(T));
223}
224//
225template <typename T>
226static inline bool CompLess(const T& a, const T& b)
227{
228 return (a < b);
229}
230//
231template <typename T>
232static inline bool CompGreater(const T& a, const T& b)
233{
234 return (a > b);
235}
236//
237template <typename T>
238static inline T Lerp(const T& a, const T& b, btScalar t)
239{
240 return (a + (b - a) * t);
241}
242//
243template <typename T>
244static inline T InvLerp(const T& a, const T& b, btScalar t)
245{
246 return ((b + a * t - b * t) / (a * b));
247}
248//
249static 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//
260static 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//
269template <typename T>
270static inline T Clamp(const T& x, const T& l, const T& h)
271{
272 return (x < l ? l : x > h ? h : x);
273}
274//
275template <typename T>
276static inline T Sq(const T& x)
277{
278 return (x * x);
279}
280//
281template <typename T>
282static inline T Cube(const T& x)
283{
284 return (x * x * x);
285}
286//
287template <typename T>
288static inline T Sign(const T& x)
289{
290 return ((T)(x < 0 ? -1 : +1));
291}
292//
293template <typename T>
294static inline bool SameSign(const T& x, const T& y)
295{
296 return ((x * y) > 0);
297}
298//
299static 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//
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//
320static 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//
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
338static 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
347static 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
355static 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//
378static 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//
386static 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//
394static 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//
402static 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//
409static 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//
426static 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//
440static inline btVector3 ProjectOnAxis(const btVector3& v,
441 const btVector3& a)
442{
443 return (a * btDot(v, a));
444}
445//
446static inline btVector3 ProjectOnPlane(const btVector3& v,
447 const btVector3& a)
448{
449 return (v - ProjectOnAxis(v, a));
450}
451
452//
453static 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//
473static 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//
507static 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
540static 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//
578template <typename T>
579static 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//
587static 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
635inline 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//
654static 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//
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//
676static 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//
682static 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//
694static 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//
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//
724static 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//
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
787private:
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//
812static 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 }
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 {
898 btSoftClusterCollisionShape cshape(cluster);
899
901
904 return;
905
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;
918 {
919 pj->m_erp *= psb->m_cfg.kSKHR_CL;
921 }
922 else
923 {
924 pj->m_erp *= psb->m_cfg.kSRHR_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 {
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 }
1015 {
1016 const btScalar m = n.m_im > 0 ? dynmargin : stamargin;
1018
1019 if ((!n.m_battach) &&
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)
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 }
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 }
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;
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;
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;
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
virtual const btVector3 & getLocalScaling() const
btSoftBodyCollisionShape(btSoftBody *backptr)
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 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
virtual const char * getName() 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
static const btTransform & getIdentity()
Return an identity transform.
Definition: btTransform.h:197
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:113
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
const btScalar & getZ() const
Return the z value.
Definition: btVector3.h:565
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 & z() const
Return the z value.
Definition: btVector3.h:579
btVector3 & safeNormalize()
Definition: btVector3.h:286
btScalar length() const
Return the length of the vector.
Definition: btVector3.h:257
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
Definition: btVector3.h:380
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
btVector3 normalized() const
Return a normalized version of this vector.
Definition: btVector3.h:949
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:251
const btScalar & getY() const
Return the y value.
Definition: btVector3.h:563
const btScalar & x() const
Return the x value.
Definition: btVector3.h:575
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
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition: btVector3.h:303
const btScalar & getX() const
Return the x value.
Definition: btVector3.h:561
const btScalar & y() const
Return the y value.
Definition: btVector3.h:577
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 btMatrix3x3 & invWorldInertia() const
Definition: btSoftBody.h:508
btVector3 velocity(const btVector3 &rpos) const
Definition: btSoftBody.h:546
const btTransform & xform() const
Definition: btSoftBody.h:521
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)
int index(int c, int r) const
T & operator()(int c, int r)
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