Bullet Collision Detection & Physics Library
btSoftBody.cpp
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#include "btSoftBodyInternals.h"
19#include "btSoftBodyData.h"
26#include <iostream>
27//
28btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo, int node_count, const btVector3* x, const btScalar* m)
29 : m_softBodySolver(0), m_worldInfo(worldInfo)
30{
31 /* Init */
33
34 /* Default material */
36 pm->m_kLST = 1;
37 pm->m_kAST = 1;
38 pm->m_kVST = 1;
40
41 /* Nodes */
42 const btScalar margin = getCollisionShape()->getMargin();
43 m_nodes.resize(node_count);
44 for (int i = 0, ni = node_count; i < ni; ++i)
45 {
46 Node& n = m_nodes[i];
48 n.m_x = x ? *x++ : btVector3(0, 0, 0);
49 n.m_q = n.m_x;
50 n.m_im = m ? *m++ : 1;
51 n.m_im = n.m_im > 0 ? 1 / n.m_im : 0;
52 n.m_leaf = m_ndbvt.insert(btDbvtVolume::FromCR(n.m_x, margin), &n);
53 n.m_material = pm;
54 }
56}
57
59 : m_worldInfo(worldInfo)
60{
62}
63
65{
68 m_cfg.kVCF = 1;
69 m_cfg.kDG = 0;
70 m_cfg.kLF = 0;
71 m_cfg.kDP = 0;
72 m_cfg.kPR = 0;
73 m_cfg.kVC = 0;
74 m_cfg.kDF = (btScalar)0.2;
75 m_cfg.kMT = 0;
76 m_cfg.kCHR = (btScalar)1.0;
77 m_cfg.kKHR = (btScalar)0.1;
78 m_cfg.kSHR = (btScalar)1.0;
79 m_cfg.kAHR = (btScalar)0.7;
80 m_cfg.kSRHR_CL = (btScalar)0.1;
82 m_cfg.kSSHR_CL = (btScalar)0.5;
87 m_cfg.timescale = 1;
92 m_cfg.drag = 0;
95 m_pose.m_bvolume = false;
96 m_pose.m_bframe = false;
97 m_pose.m_volume = 0;
98 m_pose.m_com = btVector3(0, 0, 0);
101 m_tag = 0;
102 m_timeacc = 0;
103 m_bUpdateRtCst = true;
104 m_bounds[0] = btVector3(0, 0, 0);
105 m_bounds[1] = btVector3(0, 0, 0);
108
109 /* Collision shape */
113
115
116 m_windVelocity = btVector3(0, 0, 0);
120 m_useFaceContact = true;
121 m_useSelfCollision = false;
123}
124
125//
127{
128 //for now, delete the internal shape
129 delete m_collisionShape;
130 int i;
131
133 for (i = 0; i < m_materials.size(); ++i)
135 for (i = 0; i < m_joints.size(); ++i)
137}
138
139//
140bool btSoftBody::checkLink(int node0, int node1) const
141{
142 return (checkLink(&m_nodes[node0], &m_nodes[node1]));
143}
144
145//
146bool btSoftBody::checkLink(const Node* node0, const Node* node1) const
147{
148 const Node* n[] = {node0, node1};
149 for (int i = 0, ni = m_links.size(); i < ni; ++i)
150 {
151 const Link& l = m_links[i];
152 if ((l.m_n[0] == n[0] && l.m_n[1] == n[1]) ||
153 (l.m_n[0] == n[1] && l.m_n[1] == n[0]))
154 {
155 return (true);
156 }
157 }
158 return (false);
159}
160
161//
162bool btSoftBody::checkFace(int node0, int node1, int node2) const
163{
164 const Node* n[] = {&m_nodes[node0],
165 &m_nodes[node1],
166 &m_nodes[node2]};
167 for (int i = 0, ni = m_faces.size(); i < ni; ++i)
168 {
169 const Face& f = m_faces[i];
170 int c = 0;
171 for (int j = 0; j < 3; ++j)
172 {
173 if ((f.m_n[j] == n[0]) ||
174 (f.m_n[j] == n[1]) ||
175 (f.m_n[j] == n[2]))
176 c |= 1 << j;
177 else
178 break;
179 }
180 if (c == 7) return (true);
181 }
182 return (false);
183}
184
185//
187{
188 Material* pm = new (btAlignedAlloc(sizeof(Material), 16)) Material();
189 if (m_materials.size() > 0)
190 *pm = *m_materials[0];
191 else
192 ZeroInitialize(*pm);
194 return (pm);
195}
196
197//
198void btSoftBody::appendNote(const char* text,
199 const btVector3& o,
200 const btVector4& c,
201 Node* n0,
202 Node* n1,
203 Node* n2,
204 Node* n3)
205{
206 Note n;
208 n.m_rank = 0;
209 n.m_text = text;
210 n.m_offset = o;
211 n.m_coords[0] = c.x();
212 n.m_coords[1] = c.y();
213 n.m_coords[2] = c.z();
214 n.m_coords[3] = c.w();
215 n.m_nodes[0] = n0;
216 n.m_rank += n0 ? 1 : 0;
217 n.m_nodes[1] = n1;
218 n.m_rank += n1 ? 1 : 0;
219 n.m_nodes[2] = n2;
220 n.m_rank += n2 ? 1 : 0;
221 n.m_nodes[3] = n3;
222 n.m_rank += n3 ? 1 : 0;
224}
225
226//
227void btSoftBody::appendNote(const char* text,
228 const btVector3& o,
229 Node* feature)
230{
231 appendNote(text, o, btVector4(1, 0, 0, 0), feature);
232}
233
234//
235void btSoftBody::appendNote(const char* text,
236 const btVector3& o,
237 Link* feature)
238{
239 static const btScalar w = 1 / (btScalar)2;
240 appendNote(text, o, btVector4(w, w, 0, 0), feature->m_n[0],
241 feature->m_n[1]);
242}
243
244//
245void btSoftBody::appendNote(const char* text,
246 const btVector3& o,
247 Face* feature)
248{
249 static const btScalar w = 1 / (btScalar)3;
250 appendNote(text, o, btVector4(w, w, w, 0), feature->m_n[0],
251 feature->m_n[1],
252 feature->m_n[2]);
253}
254
255//
257{
258 if (m_nodes.capacity() == m_nodes.size())
259 {
261 m_nodes.reserve(m_nodes.size() * 2 + 1);
263 }
264 const btScalar margin = getCollisionShape()->getMargin();
266 Node& n = m_nodes[m_nodes.size() - 1];
268 n.m_x = x;
269 n.m_q = n.m_x;
270 n.m_im = m > 0 ? 1 / m : 0;
271 n.m_material = m_materials[0];
272 n.m_leaf = m_ndbvt.insert(btDbvtVolume::FromCR(n.m_x, margin), &n);
273}
274
275//
276void btSoftBody::appendLink(int model, Material* mat)
277{
278 Link l;
279 if (model >= 0)
280 l = m_links[model];
281 else
282 {
284 l.m_material = mat ? mat : m_materials[0];
285 }
287}
288
289//
291 int node1,
292 Material* mat,
293 bool bcheckexist)
294{
295 appendLink(&m_nodes[node0], &m_nodes[node1], mat, bcheckexist);
296}
297
298//
300 Node* node1,
301 Material* mat,
302 bool bcheckexist)
303{
304 if ((!bcheckexist) || (!checkLink(node0, node1)))
305 {
306 appendLink(-1, mat);
307 Link& l = m_links[m_links.size() - 1];
308 l.m_n[0] = node0;
309 l.m_n[1] = node1;
310 l.m_rl = (l.m_n[0]->m_x - l.m_n[1]->m_x).length();
311 m_bUpdateRtCst = true;
312 }
313}
314
315//
316void btSoftBody::appendFace(int model, Material* mat)
317{
318 Face f;
319 if (model >= 0)
320 {
321 f = m_faces[model];
322 }
323 else
324 {
326 f.m_material = mat ? mat : m_materials[0];
327 }
329}
330
331//
332void btSoftBody::appendFace(int node0, int node1, int node2, Material* mat)
333{
334 if (node0 == node1)
335 return;
336 if (node1 == node2)
337 return;
338 if (node2 == node0)
339 return;
340
341 appendFace(-1, mat);
342 Face& f = m_faces[m_faces.size() - 1];
343 btAssert(node0 != node1);
344 btAssert(node1 != node2);
345 btAssert(node2 != node0);
346 f.m_n[0] = &m_nodes[node0];
347 f.m_n[1] = &m_nodes[node1];
348 f.m_n[2] = &m_nodes[node2];
349 f.m_ra = AreaOf(f.m_n[0]->m_x,
350 f.m_n[1]->m_x,
351 f.m_n[2]->m_x);
352 m_bUpdateRtCst = true;
353}
354
355//
357{
358 Tetra t;
359 if (model >= 0)
360 t = m_tetras[model];
361 else
362 {
364 t.m_material = mat ? mat : m_materials[0];
365 }
367}
368
369//
371 int node1,
372 int node2,
373 int node3,
374 Material* mat)
375{
376 appendTetra(-1, mat);
377 Tetra& t = m_tetras[m_tetras.size() - 1];
378 t.m_n[0] = &m_nodes[node0];
379 t.m_n[1] = &m_nodes[node1];
380 t.m_n[2] = &m_nodes[node2];
381 t.m_n[3] = &m_nodes[node3];
382 t.m_rv = VolumeOf(t.m_n[0]->m_x, t.m_n[1]->m_x, t.m_n[2]->m_x, t.m_n[3]->m_x);
383 m_bUpdateRtCst = true;
384}
385
386//
387
388void btSoftBody::appendAnchor(int node, btRigidBody* body, bool disableCollisionBetweenLinkedBodies, btScalar influence)
389{
390 btVector3 local = body->getWorldTransform().inverse() * m_nodes[node].m_x;
391 appendAnchor(node, body, local, disableCollisionBetweenLinkedBodies, influence);
392}
393
394//
395void btSoftBody::appendAnchor(int node, btRigidBody* body, const btVector3& localPivot, bool disableCollisionBetweenLinkedBodies, btScalar influence)
396{
397 if (disableCollisionBetweenLinkedBodies)
398 {
400 {
402 }
403 }
404
405 Anchor a;
406 a.m_node = &m_nodes[node];
407 a.m_body = body;
408 a.m_local = localPivot;
409 a.m_node->m_battach = 1;
410 a.m_influence = influence;
412}
413
414//
416{
418 btSoftBody::Node& n = m_nodes[node];
419 const btScalar ima = n.m_im;
420 const btScalar imb = body->getInvMass();
421 btVector3 nrm;
422 const btCollisionShape* shp = body->getCollisionShape();
423 const btTransform& wtr = body->getWorldTransform();
424 btScalar dst =
426 wtr.invXform(m_nodes[node].m_x),
427 shp,
428 nrm,
429 0);
430
431 c.m_cti.m_colObj = body;
432 c.m_cti.m_normal = wtr.getBasis() * nrm;
433 c.m_cti.m_offset = dst;
434 c.m_node = &m_nodes[node];
435 const btScalar fc = m_cfg.kDF * body->getFriction();
436 c.m_c2 = ima;
437 c.m_c3 = fc;
439 static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0);
440 const btMatrix3x3& iwi = body->getInvInertiaTensorWorld();
441 const btVector3 ra = n.m_x - wtr.getOrigin();
442
443 c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra);
444 c.m_c1 = ra;
445 c.m_local = body->getWorldTransform().inverse() * m_nodes[node].m_x;
446 c.m_node->m_battach = 1;
447 m_deformableAnchors.push_back(c);
448}
449
450//
452{
454 btSoftBody::Node& n = m_nodes[node];
455 const btScalar ima = n.m_im;
456 btVector3 nrm;
457 const btCollisionShape* shp = link->getCollisionShape();
458 const btTransform& wtr = link->getWorldTransform();
459 btScalar dst =
461 wtr.invXform(m_nodes[node].m_x),
462 shp,
463 nrm,
464 0);
465 c.m_cti.m_colObj = link;
466 c.m_cti.m_normal = wtr.getBasis() * nrm;
467 c.m_cti.m_offset = dst;
468 c.m_node = &m_nodes[node];
469 const btScalar fc = m_cfg.kDF * link->getFriction();
470 c.m_c2 = ima;
471 c.m_c3 = fc;
473 btVector3 normal = c.m_cti.m_normal;
475 btVector3 t2 = btCross(normal, t1);
476 btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2;
477 findJacobian(link, jacobianData_normal, c.m_node->m_x, normal);
478 findJacobian(link, jacobianData_t1, c.m_node->m_x, t1);
479 findJacobian(link, jacobianData_t2, c.m_node->m_x, t2);
480
481 btScalar* J_n = &jacobianData_normal.m_jacobians[0];
482 btScalar* J_t1 = &jacobianData_t1.m_jacobians[0];
483 btScalar* J_t2 = &jacobianData_t2.m_jacobians[0];
484
485 btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
486 btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
487 btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
488
489 btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(),
490 t1.getX(), t1.getY(), t1.getZ(),
491 t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame
492 const int ndof = link->m_multiBody->getNumDofs() + 6;
493 btMatrix3x3 local_impulse_matrix = (Diagonal(n.m_im) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse();
494 c.m_c0 = rot.transpose() * local_impulse_matrix * rot;
495 c.jacobianData_normal = jacobianData_normal;
496 c.jacobianData_t1 = jacobianData_t1;
497 c.jacobianData_t2 = jacobianData_t2;
498 c.t1 = t1;
499 c.t2 = t2;
500 const btVector3 ra = n.m_x - wtr.getOrigin();
501 c.m_c1 = ra;
502 c.m_local = link->getWorldTransform().inverse() * m_nodes[node].m_x;
503 c.m_node->m_battach = 1;
504 m_deformableAnchors.push_back(c);
505}
506//
508{
509 LJoint* pj = new (btAlignedAlloc(sizeof(LJoint), 16)) LJoint();
510 pj->m_bodies[0] = body0;
511 pj->m_bodies[1] = body1;
512 pj->m_refs[0] = pj->m_bodies[0].xform().inverse() * specs.position;
513 pj->m_refs[1] = pj->m_bodies[1].xform().inverse() * specs.position;
514 pj->m_cfm = specs.cfm;
515 pj->m_erp = specs.erp;
516 pj->m_split = specs.split;
518}
519
520//
522{
523 appendLinearJoint(specs, m_clusters[0], body);
524}
525
526//
528{
529 appendLinearJoint(specs, m_clusters[0], body->m_clusters[0]);
530}
531
532//
534{
535 AJoint* pj = new (btAlignedAlloc(sizeof(AJoint), 16)) AJoint();
536 pj->m_bodies[0] = body0;
537 pj->m_bodies[1] = body1;
538 pj->m_refs[0] = pj->m_bodies[0].xform().inverse().getBasis() * specs.axis;
539 pj->m_refs[1] = pj->m_bodies[1].xform().inverse().getBasis() * specs.axis;
540 pj->m_cfm = specs.cfm;
541 pj->m_erp = specs.erp;
542 pj->m_split = specs.split;
543 pj->m_icontrol = specs.icontrol;
545}
546
547//
549{
550 appendAngularJoint(specs, m_clusters[0], body);
551}
552
553//
555{
556 appendAngularJoint(specs, m_clusters[0], body->m_clusters[0]);
557}
558
559//
561{
562 for (int i = 0, ni = m_nodes.size(); i < ni; ++i) addForce(force, i);
563}
564
565//
566void btSoftBody::addForce(const btVector3& force, int node)
567{
568 Node& n = m_nodes[node];
569 if (n.m_im > 0)
570 {
571 n.m_f += force;
572 }
573}
574
575void btSoftBody::addAeroForceToNode(const btVector3& windVelocity, int nodeIndex)
576{
577 btAssert(nodeIndex >= 0 && nodeIndex < m_nodes.size());
578
579 const btScalar dt = m_sst.sdt;
580 const btScalar kLF = m_cfg.kLF;
581 const btScalar kDG = m_cfg.kDG;
582 //const btScalar kPR = m_cfg.kPR;
583 //const btScalar kVC = m_cfg.kVC;
584 const bool as_lift = kLF > 0;
585 const bool as_drag = kDG > 0;
586 const bool as_aero = as_lift || as_drag;
587 const bool as_vaero = as_aero && (m_cfg.aeromodel < btSoftBody::eAeroModel::F_TwoSided);
588
589 Node& n = m_nodes[nodeIndex];
590
591 if (n.m_im > 0)
592 {
593 btSoftBody::sMedium medium;
594
595 EvaluateMedium(m_worldInfo, n.m_x, medium);
596 medium.m_velocity = windVelocity;
598
599 /* Aerodynamics */
600 if (as_vaero)
601 {
602 const btVector3 rel_v = n.m_v - medium.m_velocity;
603 const btScalar rel_v_len = rel_v.length();
604 const btScalar rel_v2 = rel_v.length2();
605
606 if (rel_v2 > SIMD_EPSILON)
607 {
608 const btVector3 rel_v_nrm = rel_v.normalized();
609 btVector3 nrm = n.m_n;
610
612 {
613 nrm *= (btScalar)((btDot(nrm, rel_v) < 0) ? -1 : +1);
614 btVector3 fDrag(0, 0, 0);
615 btVector3 fLift(0, 0, 0);
616
617 btScalar n_dot_v = nrm.dot(rel_v_nrm);
618 btScalar tri_area = 0.5f * n.m_area;
619
620 fDrag = 0.5f * kDG * medium.m_density * rel_v2 * tri_area * n_dot_v * (-rel_v_nrm);
621
622 // Check angle of attack
623 // cos(10º) = 0.98480
624 if (0 < n_dot_v && n_dot_v < 0.98480f)
625 fLift = 0.5f * kLF * medium.m_density * rel_v_len * tri_area * btSqrt(1.0f - n_dot_v * n_dot_v) * (nrm.cross(rel_v_nrm).cross(rel_v_nrm));
626
627 // Check if the velocity change resulted by aero drag force exceeds the current velocity of the node.
628 btVector3 del_v_by_fDrag = fDrag * n.m_im * m_sst.sdt;
629 btScalar del_v_by_fDrag_len2 = del_v_by_fDrag.length2();
630 btScalar v_len2 = n.m_v.length2();
631
632 if (del_v_by_fDrag_len2 >= v_len2 && del_v_by_fDrag_len2 > 0)
633 {
634 btScalar del_v_by_fDrag_len = del_v_by_fDrag.length();
635 btScalar v_len = n.m_v.length();
636 fDrag *= btScalar(0.8) * (v_len / del_v_by_fDrag_len);
637 }
638
639 n.m_f += fDrag;
640 n.m_f += fLift;
641 }
643 {
645 nrm *= (btScalar)((btDot(nrm, rel_v) < 0) ? -1 : +1);
646
647 const btScalar dvn = btDot(rel_v, nrm);
648 /* Compute forces */
649 if (dvn > 0)
650 {
651 btVector3 force(0, 0, 0);
652 const btScalar c0 = n.m_area * dvn * rel_v2 / 2;
653 const btScalar c1 = c0 * medium.m_density;
654 force += nrm * (-c1 * kLF);
655 force += rel_v.normalized() * (-c1 * kDG);
656 ApplyClampedForce(n, force, dt);
657 }
658 }
659 }
660 }
661 }
662}
663
664void btSoftBody::addAeroForceToFace(const btVector3& windVelocity, int faceIndex)
665{
666 const btScalar dt = m_sst.sdt;
667 const btScalar kLF = m_cfg.kLF;
668 const btScalar kDG = m_cfg.kDG;
669 // const btScalar kPR = m_cfg.kPR;
670 // const btScalar kVC = m_cfg.kVC;
671 const bool as_lift = kLF > 0;
672 const bool as_drag = kDG > 0;
673 const bool as_aero = as_lift || as_drag;
674 const bool as_faero = as_aero && (m_cfg.aeromodel >= btSoftBody::eAeroModel::F_TwoSided);
675
676 if (as_faero)
677 {
678 btSoftBody::Face& f = m_faces[faceIndex];
679
680 btSoftBody::sMedium medium;
681
682 const btVector3 v = (f.m_n[0]->m_v + f.m_n[1]->m_v + f.m_n[2]->m_v) / 3;
683 const btVector3 x = (f.m_n[0]->m_x + f.m_n[1]->m_x + f.m_n[2]->m_x) / 3;
684 EvaluateMedium(m_worldInfo, x, medium);
685 medium.m_velocity = windVelocity;
687 const btVector3 rel_v = v - medium.m_velocity;
688 const btScalar rel_v_len = rel_v.length();
689 const btScalar rel_v2 = rel_v.length2();
690
691 if (rel_v2 > SIMD_EPSILON)
692 {
693 const btVector3 rel_v_nrm = rel_v.normalized();
694 btVector3 nrm = f.m_normal;
695
697 {
698 nrm *= (btScalar)((btDot(nrm, rel_v) < 0) ? -1 : +1);
699
700 btVector3 fDrag(0, 0, 0);
701 btVector3 fLift(0, 0, 0);
702
703 btScalar n_dot_v = nrm.dot(rel_v_nrm);
704 btScalar tri_area = 0.5f * f.m_ra;
705
706 fDrag = 0.5f * kDG * medium.m_density * rel_v2 * tri_area * n_dot_v * (-rel_v_nrm);
707
708 // Check angle of attack
709 // cos(10º) = 0.98480
710 if (0 < n_dot_v && n_dot_v < 0.98480f)
711 fLift = 0.5f * kLF * medium.m_density * rel_v_len * tri_area * btSqrt(1.0f - n_dot_v * n_dot_v) * (nrm.cross(rel_v_nrm).cross(rel_v_nrm));
712
713 fDrag /= 3;
714 fLift /= 3;
715
716 for (int j = 0; j < 3; ++j)
717 {
718 if (f.m_n[j]->m_im > 0)
719 {
720 // Check if the velocity change resulted by aero drag force exceeds the current velocity of the node.
721 btVector3 del_v_by_fDrag = fDrag * f.m_n[j]->m_im * m_sst.sdt;
722 btScalar del_v_by_fDrag_len2 = del_v_by_fDrag.length2();
723 btScalar v_len2 = f.m_n[j]->m_v.length2();
724
725 if (del_v_by_fDrag_len2 >= v_len2 && del_v_by_fDrag_len2 > 0)
726 {
727 btScalar del_v_by_fDrag_len = del_v_by_fDrag.length();
728 btScalar v_len = f.m_n[j]->m_v.length();
729 fDrag *= btScalar(0.8) * (v_len / del_v_by_fDrag_len);
730 }
731
732 f.m_n[j]->m_f += fDrag;
733 f.m_n[j]->m_f += fLift;
734 }
735 }
736 }
738 {
740 nrm *= (btScalar)((btDot(nrm, rel_v) < 0) ? -1 : +1);
741
742 const btScalar dvn = btDot(rel_v, nrm);
743 /* Compute forces */
744 if (dvn > 0)
745 {
746 btVector3 force(0, 0, 0);
747 const btScalar c0 = f.m_ra * dvn * rel_v2;
748 const btScalar c1 = c0 * medium.m_density;
749 force += nrm * (-c1 * kLF);
750 force += rel_v.normalized() * (-c1 * kDG);
751 force /= 3;
752 for (int j = 0; j < 3; ++j) ApplyClampedForce(*f.m_n[j], force, dt);
753 }
754 }
755 }
756 }
757}
758
759//
761{
762 for (int i = 0, ni = m_nodes.size(); i < ni; ++i) addVelocity(velocity, i);
763}
764
765/* Set velocity for the entire body */
767{
768 for (int i = 0, ni = m_nodes.size(); i < ni; ++i)
769 {
770 Node& n = m_nodes[i];
771 if (n.m_im > 0)
772 {
773 n.m_v = velocity;
774 }
775 }
776}
777
778//
779void btSoftBody::addVelocity(const btVector3& velocity, int node)
780{
781 Node& n = m_nodes[node];
782 if (n.m_im > 0)
783 {
784 n.m_v += velocity;
785 }
786}
787
788//
789void btSoftBody::setMass(int node, btScalar mass)
790{
791 m_nodes[node].m_im = mass > 0 ? 1 / mass : 0;
792 m_bUpdateRtCst = true;
793}
794
795//
797{
798 return (m_nodes[node].m_im > 0 ? 1 / m_nodes[node].m_im : 0);
799}
800
801//
803{
804 btScalar mass = 0;
805 for (int i = 0; i < m_nodes.size(); ++i)
806 {
807 mass += getMass(i);
808 }
809 return (mass);
810}
811
812//
813void btSoftBody::setTotalMass(btScalar mass, bool fromfaces)
814{
815 int i;
816
817 if (fromfaces)
818 {
819 for (i = 0; i < m_nodes.size(); ++i)
820 {
821 m_nodes[i].m_im = 0;
822 }
823 for (i = 0; i < m_faces.size(); ++i)
824 {
825 const Face& f = m_faces[i];
826 const btScalar twicearea = AreaOf(f.m_n[0]->m_x,
827 f.m_n[1]->m_x,
828 f.m_n[2]->m_x);
829 for (int j = 0; j < 3; ++j)
830 {
831 f.m_n[j]->m_im += twicearea;
832 }
833 }
834 for (i = 0; i < m_nodes.size(); ++i)
835 {
836 m_nodes[i].m_im = 1 / m_nodes[i].m_im;
837 }
838 }
839 const btScalar tm = getTotalMass();
840 const btScalar itm = 1 / tm;
841 for (i = 0; i < m_nodes.size(); ++i)
842 {
843 m_nodes[i].m_im /= itm * mass;
844 }
845 m_bUpdateRtCst = true;
846}
847
848//
850{
851 setTotalMass(getVolume() * density, true);
852}
853
854//
856{
858 ranks.resize(m_nodes.size(), 0);
859 int i;
860
861 for (i = 0; i < m_nodes.size(); ++i)
862 {
863 m_nodes[i].m_im = 0;
864 }
865 for (i = 0; i < m_tetras.size(); ++i)
866 {
867 const Tetra& t = m_tetras[i];
868 for (int j = 0; j < 4; ++j)
869 {
870 t.m_n[j]->m_im += btFabs(t.m_rv);
871 ranks[int(t.m_n[j] - &m_nodes[0])] += 1;
872 }
873 }
874 for (i = 0; i < m_nodes.size(); ++i)
875 {
876 if (m_nodes[i].m_im > 0)
877 {
878 m_nodes[i].m_im = ranks[i] / m_nodes[i].m_im;
879 }
880 }
881 setTotalMass(mass, false);
882}
883
884//
886{
887 btScalar volume = 0;
888 for (int i = 0; i < m_tetras.size(); ++i)
889 {
890 const Tetra& t = m_tetras[i];
891 for (int j = 0; j < 4; ++j)
892 {
893 volume += btFabs(t.m_rv);
894 }
895 }
896 setVolumeMass(volume * density / 6);
897}
898
899//
901{
902 const btScalar margin = getCollisionShape()->getMargin();
904 vol;
905
906 for (int i = 0, ni = m_nodes.size(); i < ni; ++i)
907 {
908 Node& n = m_nodes[i];
909 n.m_x = trs * n.m_x;
910 n.m_q = trs * n.m_q;
911 n.m_n = trs.getBasis() * n.m_n;
912 vol = btDbvtVolume::FromCR(n.m_x, margin);
913
914 m_ndbvt.update(n.m_leaf, vol);
915 }
917 updateBounds();
920}
921
922//
924{
925 btTransform t;
926 t.setIdentity();
927 t.setOrigin(trs);
928 transform(t);
929}
930
931//
933{
934 btTransform t;
935 t.setIdentity();
936 t.setRotation(rot);
937 transform(t);
938}
939
940//
942{
943 const btScalar margin = getCollisionShape()->getMargin();
945 vol;
946
947 for (int i = 0, ni = m_nodes.size(); i < ni; ++i)
948 {
949 Node& n = m_nodes[i];
950 n.m_x *= scl;
951 n.m_q *= scl;
952 vol = btDbvtVolume::FromCR(n.m_x, margin);
953 m_ndbvt.update(n.m_leaf, vol);
954 }
956 updateBounds();
959}
960
961//
963{
964 return m_restLengthScale;
965}
966
967//
969{
970 for (int i = 0, ni = m_links.size(); i < ni; ++i)
971 {
972 Link& l = m_links[i];
973 l.m_rl = l.m_rl / m_restLengthScale * restLengthScale;
974 l.m_c1 = l.m_rl * l.m_rl;
975 }
976 m_restLengthScale = restLengthScale;
977
979 activate();
980}
981
982//
983void btSoftBody::setPose(bool bvolume, bool bframe)
984{
985 m_pose.m_bvolume = bvolume;
986 m_pose.m_bframe = bframe;
987 int i, ni;
988
989 /* Weights */
990 const btScalar omass = getTotalMass();
991 const btScalar kmass = omass * m_nodes.size() * 1000;
992 btScalar tmass = omass;
994 for (i = 0, ni = m_nodes.size(); i < ni; ++i)
995 {
996 if (m_nodes[i].m_im <= 0) tmass += kmass;
997 }
998 for (i = 0, ni = m_nodes.size(); i < ni; ++i)
999 {
1000 Node& n = m_nodes[i];
1001 m_pose.m_wgh[i] = n.m_im > 0 ? 1 / (m_nodes[i].m_im * tmass) : kmass / tmass;
1002 }
1003 /* Pos */
1004 const btVector3 com = evaluateCom();
1006 for (i = 0, ni = m_nodes.size(); i < ni; ++i)
1007 {
1008 m_pose.m_pos[i] = m_nodes[i].m_x - com;
1009 }
1010 m_pose.m_volume = bvolume ? getVolume() : 0;
1011 m_pose.m_com = com;
1014 /* Aqq */
1015 m_pose.m_aqq[0] =
1016 m_pose.m_aqq[1] =
1017 m_pose.m_aqq[2] = btVector3(0, 0, 0);
1018 for (i = 0, ni = m_nodes.size(); i < ni; ++i)
1019 {
1020 const btVector3& q = m_pose.m_pos[i];
1021 const btVector3 mq = m_pose.m_wgh[i] * q;
1022 m_pose.m_aqq[0] += mq.x() * q;
1023 m_pose.m_aqq[1] += mq.y() * q;
1024 m_pose.m_aqq[2] += mq.z() * q;
1025 }
1027
1029}
1030
1032{
1033 for (int i = 0, ni = m_links.size(); i < ni; ++i)
1034 {
1035 Link& l = m_links[i];
1036 l.m_rl = (l.m_n[0]->m_x - l.m_n[1]->m_x).length();
1037 l.m_c1 = l.m_rl * l.m_rl;
1038 }
1039}
1040
1041//
1043{
1044 btScalar vol = 0;
1045 if (m_nodes.size() > 0)
1046 {
1047 int i, ni;
1048
1049 const btVector3 org = m_nodes[0].m_x;
1050 for (i = 0, ni = m_faces.size(); i < ni; ++i)
1051 {
1052 const Face& f = m_faces[i];
1053 vol += btDot(f.m_n[0]->m_x - org, btCross(f.m_n[1]->m_x - org, f.m_n[2]->m_x - org));
1054 }
1055 vol /= (btScalar)6;
1056 }
1057 return (vol);
1058}
1059
1060//
1062{
1063 return (m_clusters.size());
1064}
1065
1066//
1068{
1069 btVector3 com(0, 0, 0);
1070 for (int i = 0, ni = cluster->m_nodes.size(); i < ni; ++i)
1071 {
1072 com += cluster->m_nodes[i]->m_x * cluster->m_masses[i];
1073 }
1074 return (com * cluster->m_imass);
1075}
1076
1077//
1079{
1080 return (clusterCom(m_clusters[cluster]));
1081}
1082
1083//
1085{
1086 return (cluster->m_lv + btCross(cluster->m_av, rpos));
1087}
1088
1089//
1090void btSoftBody::clusterVImpulse(Cluster* cluster, const btVector3& rpos, const btVector3& impulse)
1091{
1092 const btVector3 li = cluster->m_imass * impulse;
1093 const btVector3 ai = cluster->m_invwi * btCross(rpos, impulse);
1094 cluster->m_vimpulses[0] += li;
1095 cluster->m_lv += li;
1096 cluster->m_vimpulses[1] += ai;
1097 cluster->m_av += ai;
1098 cluster->m_nvimpulses++;
1099}
1100
1101//
1102void btSoftBody::clusterDImpulse(Cluster* cluster, const btVector3& rpos, const btVector3& impulse)
1103{
1104 const btVector3 li = cluster->m_imass * impulse;
1105 const btVector3 ai = cluster->m_invwi * btCross(rpos, impulse);
1106 cluster->m_dimpulses[0] += li;
1107 cluster->m_dimpulses[1] += ai;
1108 cluster->m_ndimpulses++;
1109}
1110
1111//
1112void btSoftBody::clusterImpulse(Cluster* cluster, const btVector3& rpos, const Impulse& impulse)
1113{
1114 if (impulse.m_asVelocity) clusterVImpulse(cluster, rpos, impulse.m_velocity);
1115 if (impulse.m_asDrift) clusterDImpulse(cluster, rpos, impulse.m_drift);
1116}
1117
1118//
1119void btSoftBody::clusterVAImpulse(Cluster* cluster, const btVector3& impulse)
1120{
1121 const btVector3 ai = cluster->m_invwi * impulse;
1122 cluster->m_vimpulses[1] += ai;
1123 cluster->m_av += ai;
1124 cluster->m_nvimpulses++;
1125}
1126
1127//
1128void btSoftBody::clusterDAImpulse(Cluster* cluster, const btVector3& impulse)
1129{
1130 const btVector3 ai = cluster->m_invwi * impulse;
1131 cluster->m_dimpulses[1] += ai;
1132 cluster->m_ndimpulses++;
1133}
1134
1135//
1136void btSoftBody::clusterAImpulse(Cluster* cluster, const Impulse& impulse)
1137{
1138 if (impulse.m_asVelocity) clusterVAImpulse(cluster, impulse.m_velocity);
1139 if (impulse.m_asDrift) clusterDAImpulse(cluster, impulse.m_drift);
1140}
1141
1142//
1143void btSoftBody::clusterDCImpulse(Cluster* cluster, const btVector3& impulse)
1144{
1145 cluster->m_dimpulses[0] += impulse * cluster->m_imass;
1146 cluster->m_ndimpulses++;
1147}
1148
1150{
1152};
1153
1154//
1156{
1157 int i, j;
1158
1159 if (distance > 1)
1160 {
1161 /* Build graph */
1162 const int n = m_nodes.size();
1163 const unsigned inf = (~(unsigned)0) >> 1;
1164 unsigned* adj = new unsigned[n * n];
1165
1166#define IDX(_x_, _y_) ((_y_)*n + (_x_))
1167 for (j = 0; j < n; ++j)
1168 {
1169 for (i = 0; i < n; ++i)
1170 {
1171 if (i != j)
1172 {
1173 adj[IDX(i, j)] = adj[IDX(j, i)] = inf;
1174 }
1175 else
1176 {
1177 adj[IDX(i, j)] = adj[IDX(j, i)] = 0;
1178 }
1179 }
1180 }
1181 for (i = 0; i < m_links.size(); ++i)
1182 {
1183 const int ia = (int)(m_links[i].m_n[0] - &m_nodes[0]);
1184 const int ib = (int)(m_links[i].m_n[1] - &m_nodes[0]);
1185 adj[IDX(ia, ib)] = 1;
1186 adj[IDX(ib, ia)] = 1;
1187 }
1188
1189 //special optimized case for distance == 2
1190 if (distance == 2)
1191 {
1193
1194 /* Build node links */
1195 nodeLinks.resize(m_nodes.size());
1196
1197 for (i = 0; i < m_links.size(); ++i)
1198 {
1199 const int ia = (int)(m_links[i].m_n[0] - &m_nodes[0]);
1200 const int ib = (int)(m_links[i].m_n[1] - &m_nodes[0]);
1201 if (nodeLinks[ia].m_links.findLinearSearch(ib) == nodeLinks[ia].m_links.size())
1202 nodeLinks[ia].m_links.push_back(ib);
1203
1204 if (nodeLinks[ib].m_links.findLinearSearch(ia) == nodeLinks[ib].m_links.size())
1205 nodeLinks[ib].m_links.push_back(ia);
1206 }
1207 for (int ii = 0; ii < nodeLinks.size(); ii++)
1208 {
1209 int i = ii;
1210
1211 for (int jj = 0; jj < nodeLinks[ii].m_links.size(); jj++)
1212 {
1213 int k = nodeLinks[ii].m_links[jj];
1214 for (int kk = 0; kk < nodeLinks[k].m_links.size(); kk++)
1215 {
1216 int j = nodeLinks[k].m_links[kk];
1217 if (i != j)
1218 {
1219 const unsigned sum = adj[IDX(i, k)] + adj[IDX(k, j)];
1220 btAssert(sum == 2);
1221 if (adj[IDX(i, j)] > sum)
1222 {
1223 adj[IDX(i, j)] = adj[IDX(j, i)] = sum;
1224 }
1225 }
1226 }
1227 }
1228 }
1229 }
1230 else
1231 {
1233 for (int k = 0; k < n; ++k)
1234 {
1235 for (j = 0; j < n; ++j)
1236 {
1237 for (i = j + 1; i < n; ++i)
1238 {
1239 const unsigned sum = adj[IDX(i, k)] + adj[IDX(k, j)];
1240 if (adj[IDX(i, j)] > sum)
1241 {
1242 adj[IDX(i, j)] = adj[IDX(j, i)] = sum;
1243 }
1244 }
1245 }
1246 }
1247 }
1248
1249 /* Build links */
1250 int nlinks = 0;
1251 for (j = 0; j < n; ++j)
1252 {
1253 for (i = j + 1; i < n; ++i)
1254 {
1255 if (adj[IDX(i, j)] == (unsigned)distance)
1256 {
1257 appendLink(i, j, mat);
1258 m_links[m_links.size() - 1].m_bbending = 1;
1259 ++nlinks;
1260 }
1261 }
1262 }
1263 delete[] adj;
1264 return (nlinks);
1265 }
1266 return (0);
1267}
1268
1269//
1271{
1272 unsigned long seed = 243703;
1273#define NEXTRAND (seed = (1664525L * seed + 1013904223L) & 0xffffffff)
1274 int i, ni;
1275
1276 for (i = 0, ni = m_links.size(); i < ni; ++i)
1277 {
1278 btSwap(m_links[i], m_links[NEXTRAND % ni]);
1279 }
1280 for (i = 0, ni = m_faces.size(); i < ni; ++i)
1281 {
1282 btSwap(m_faces[i], m_faces[NEXTRAND % ni]);
1283 }
1284#undef NEXTRAND
1285}
1286
1287//
1289{
1290 Cluster* c = m_clusters[index];
1291 if (c->m_leaf) m_cdbvt.remove(c->m_leaf);
1292 c->~Cluster();
1293 btAlignedFree(c);
1294 m_clusters.remove(c);
1295}
1296
1297//
1299{
1300 while (m_clusters.size() > 0) releaseCluster(0);
1301}
1302
1303//
1304int btSoftBody::generateClusters(int k, int maxiterations)
1305{
1306 int i;
1309 for (i = 0; i < m_clusters.size(); ++i)
1310 {
1311 m_clusters[i] = new (btAlignedAlloc(sizeof(Cluster), 16)) Cluster();
1312 m_clusters[i]->m_collide = true;
1313 }
1314 k = m_clusters.size();
1315 if (k > 0)
1316 {
1317 /* Initialize */
1319 btVector3 cog(0, 0, 0);
1320 int i;
1321 for (i = 0; i < m_nodes.size(); ++i)
1322 {
1323 cog += m_nodes[i].m_x;
1324 m_clusters[(i * 29873) % m_clusters.size()]->m_nodes.push_back(&m_nodes[i]);
1325 }
1326 cog /= (btScalar)m_nodes.size();
1327 centers.resize(k, cog);
1328 /* Iterate */
1329 const btScalar slope = 16;
1330 bool changed;
1331 int iterations = 0;
1332 do
1333 {
1334 const btScalar w = 2 - btMin<btScalar>(1, iterations / slope);
1335 changed = false;
1336 iterations++;
1337 int i;
1338
1339 for (i = 0; i < k; ++i)
1340 {
1341 btVector3 c(0, 0, 0);
1342 for (int j = 0; j < m_clusters[i]->m_nodes.size(); ++j)
1343 {
1344 c += m_clusters[i]->m_nodes[j]->m_x;
1345 }
1346 if (m_clusters[i]->m_nodes.size())
1347 {
1348 c /= (btScalar)m_clusters[i]->m_nodes.size();
1349 c = centers[i] + (c - centers[i]) * w;
1350 changed |= ((c - centers[i]).length2() > SIMD_EPSILON);
1351 centers[i] = c;
1352 m_clusters[i]->m_nodes.resize(0);
1353 }
1354 }
1355 for (i = 0; i < m_nodes.size(); ++i)
1356 {
1357 const btVector3 nx = m_nodes[i].m_x;
1358 int kbest = 0;
1359 btScalar kdist = ClusterMetric(centers[0], nx);
1360 for (int j = 1; j < k; ++j)
1361 {
1362 const btScalar d = ClusterMetric(centers[j], nx);
1363 if (d < kdist)
1364 {
1365 kbest = j;
1366 kdist = d;
1367 }
1368 }
1369 m_clusters[kbest]->m_nodes.push_back(&m_nodes[i]);
1370 }
1371 } while (changed && (iterations < maxiterations));
1372 /* Merge */
1374 cids.resize(m_nodes.size(), -1);
1375 for (i = 0; i < m_clusters.size(); ++i)
1376 {
1377 for (int j = 0; j < m_clusters[i]->m_nodes.size(); ++j)
1378 {
1379 cids[int(m_clusters[i]->m_nodes[j] - &m_nodes[0])] = i;
1380 }
1381 }
1382 for (i = 0; i < m_faces.size(); ++i)
1383 {
1384 const int idx[] = {int(m_faces[i].m_n[0] - &m_nodes[0]),
1385 int(m_faces[i].m_n[1] - &m_nodes[0]),
1386 int(m_faces[i].m_n[2] - &m_nodes[0])};
1387 for (int j = 0; j < 3; ++j)
1388 {
1389 const int cid = cids[idx[j]];
1390 for (int q = 1; q < 3; ++q)
1391 {
1392 const int kid = idx[(j + q) % 3];
1393 if (cids[kid] != cid)
1394 {
1395 if (m_clusters[cid]->m_nodes.findLinearSearch(&m_nodes[kid]) == m_clusters[cid]->m_nodes.size())
1396 {
1397 m_clusters[cid]->m_nodes.push_back(&m_nodes[kid]);
1398 }
1399 }
1400 }
1401 }
1402 }
1403 /* Master */
1404 if (m_clusters.size() > 1)
1405 {
1406 Cluster* pmaster = new (btAlignedAlloc(sizeof(Cluster), 16)) Cluster();
1407 pmaster->m_collide = false;
1408 pmaster->m_nodes.reserve(m_nodes.size());
1409 for (int i = 0; i < m_nodes.size(); ++i) pmaster->m_nodes.push_back(&m_nodes[i]);
1410 m_clusters.push_back(pmaster);
1412 }
1413 /* Terminate */
1414 for (i = 0; i < m_clusters.size(); ++i)
1415 {
1416 if (m_clusters[i]->m_nodes.size() == 0)
1417 {
1418 releaseCluster(i--);
1419 }
1420 }
1421 }
1422 else
1423 {
1424 //create a cluster for each tetrahedron (if tetrahedra exist) or each face
1425 if (m_tetras.size())
1426 {
1428 for (i = 0; i < m_clusters.size(); ++i)
1429 {
1430 m_clusters[i] = new (btAlignedAlloc(sizeof(Cluster), 16)) Cluster();
1431 m_clusters[i]->m_collide = true;
1432 }
1433 for (i = 0; i < m_tetras.size(); i++)
1434 {
1435 for (int j = 0; j < 4; j++)
1436 {
1437 m_clusters[i]->m_nodes.push_back(m_tetras[i].m_n[j]);
1438 }
1439 }
1440 }
1441 else
1442 {
1444 for (i = 0; i < m_clusters.size(); ++i)
1445 {
1446 m_clusters[i] = new (btAlignedAlloc(sizeof(Cluster), 16)) Cluster();
1447 m_clusters[i]->m_collide = true;
1448 }
1449
1450 for (i = 0; i < m_faces.size(); ++i)
1451 {
1452 for (int j = 0; j < 3; ++j)
1453 {
1454 m_clusters[i]->m_nodes.push_back(m_faces[i].m_n[j]);
1455 }
1456 }
1457 }
1458 }
1459
1460 if (m_clusters.size())
1461 {
1464
1465 //for self-collision
1467 {
1468 for (int c0 = 0; c0 < m_clusters.size(); c0++)
1469 {
1470 m_clusters[c0]->m_clusterIndex = c0;
1471 for (int c1 = 0; c1 < m_clusters.size(); c1++)
1472 {
1473 bool connected = false;
1474 Cluster* cla = m_clusters[c0];
1475 Cluster* clb = m_clusters[c1];
1476 for (int i = 0; !connected && i < cla->m_nodes.size(); i++)
1477 {
1478 for (int j = 0; j < clb->m_nodes.size(); j++)
1479 {
1480 if (cla->m_nodes[i] == clb->m_nodes[j])
1481 {
1482 connected = true;
1483 break;
1484 }
1485 }
1486 }
1487 m_clusterConnectivity[c0 + c1 * m_clusters.size()] = connected;
1488 }
1489 }
1490 }
1491 }
1492
1493 return (m_clusters.size());
1494}
1495
1496//
1497void btSoftBody::refine(ImplicitFn* ifn, btScalar accurary, bool cut)
1498{
1499 const Node* nbase = &m_nodes[0];
1500 int ncount = m_nodes.size();
1501 btSymMatrix<int> edges(ncount, -2);
1502 int newnodes = 0;
1503 int i, j, k, ni;
1504
1505 /* Filter out */
1506 for (i = 0; i < m_links.size(); ++i)
1507 {
1508 Link& l = m_links[i];
1509 if (l.m_bbending)
1510 {
1511 if (!SameSign(ifn->Eval(l.m_n[0]->m_x), ifn->Eval(l.m_n[1]->m_x)))
1512 {
1513 btSwap(m_links[i], m_links[m_links.size() - 1]);
1514 m_links.pop_back();
1515 --i;
1516 }
1517 }
1518 }
1519 /* Fill edges */
1520 for (i = 0; i < m_links.size(); ++i)
1521 {
1522 Link& l = m_links[i];
1523 edges(int(l.m_n[0] - nbase), int(l.m_n[1] - nbase)) = -1;
1524 }
1525 for (i = 0; i < m_faces.size(); ++i)
1526 {
1527 Face& f = m_faces[i];
1528 edges(int(f.m_n[0] - nbase), int(f.m_n[1] - nbase)) = -1;
1529 edges(int(f.m_n[1] - nbase), int(f.m_n[2] - nbase)) = -1;
1530 edges(int(f.m_n[2] - nbase), int(f.m_n[0] - nbase)) = -1;
1531 }
1532 /* Intersect */
1533 for (i = 0; i < ncount; ++i)
1534 {
1535 for (j = i + 1; j < ncount; ++j)
1536 {
1537 if (edges(i, j) == -1)
1538 {
1539 Node& a = m_nodes[i];
1540 Node& b = m_nodes[j];
1541 const btScalar t = ImplicitSolve(ifn, a.m_x, b.m_x, accurary);
1542 if (t > 0)
1543 {
1544 const btVector3 x = Lerp(a.m_x, b.m_x, t);
1545 const btVector3 v = Lerp(a.m_v, b.m_v, t);
1546 btScalar m = 0;
1547 if (a.m_im > 0)
1548 {
1549 if (b.m_im > 0)
1550 {
1551 const btScalar ma = 1 / a.m_im;
1552 const btScalar mb = 1 / b.m_im;
1553 const btScalar mc = Lerp(ma, mb, t);
1554 const btScalar f = (ma + mb) / (ma + mb + mc);
1555 a.m_im = 1 / (ma * f);
1556 b.m_im = 1 / (mb * f);
1557 m = mc * f;
1558 }
1559 else
1560 {
1561 a.m_im /= 0.5f;
1562 m = 1 / a.m_im;
1563 }
1564 }
1565 else
1566 {
1567 if (b.m_im > 0)
1568 {
1569 b.m_im /= 0.5f;
1570 m = 1 / b.m_im;
1571 }
1572 else
1573 m = 0;
1574 }
1575 appendNode(x, m);
1576 edges(i, j) = m_nodes.size() - 1;
1577 m_nodes[edges(i, j)].m_v = v;
1578 ++newnodes;
1579 }
1580 }
1581 }
1582 }
1583 nbase = &m_nodes[0];
1584 /* Refine links */
1585 for (i = 0, ni = m_links.size(); i < ni; ++i)
1586 {
1587 Link& feat = m_links[i];
1588 const int idx[] = {int(feat.m_n[0] - nbase),
1589 int(feat.m_n[1] - nbase)};
1590 if ((idx[0] < ncount) && (idx[1] < ncount))
1591 {
1592 const int ni = edges(idx[0], idx[1]);
1593 if (ni > 0)
1594 {
1595 appendLink(i);
1596 Link* pft[] = {&m_links[i],
1597 &m_links[m_links.size() - 1]};
1598 pft[0]->m_n[0] = &m_nodes[idx[0]];
1599 pft[0]->m_n[1] = &m_nodes[ni];
1600 pft[1]->m_n[0] = &m_nodes[ni];
1601 pft[1]->m_n[1] = &m_nodes[idx[1]];
1602 }
1603 }
1604 }
1605 /* Refine faces */
1606 for (i = 0; i < m_faces.size(); ++i)
1607 {
1608 const Face& feat = m_faces[i];
1609 const int idx[] = {int(feat.m_n[0] - nbase),
1610 int(feat.m_n[1] - nbase),
1611 int(feat.m_n[2] - nbase)};
1612 for (j = 2, k = 0; k < 3; j = k++)
1613 {
1614 if ((idx[j] < ncount) && (idx[k] < ncount))
1615 {
1616 const int ni = edges(idx[j], idx[k]);
1617 if (ni > 0)
1618 {
1619 appendFace(i);
1620 const int l = (k + 1) % 3;
1621 Face* pft[] = {&m_faces[i],
1622 &m_faces[m_faces.size() - 1]};
1623 pft[0]->m_n[0] = &m_nodes[idx[l]];
1624 pft[0]->m_n[1] = &m_nodes[idx[j]];
1625 pft[0]->m_n[2] = &m_nodes[ni];
1626 pft[1]->m_n[0] = &m_nodes[ni];
1627 pft[1]->m_n[1] = &m_nodes[idx[k]];
1628 pft[1]->m_n[2] = &m_nodes[idx[l]];
1629 appendLink(ni, idx[l], pft[0]->m_material);
1630 --i;
1631 break;
1632 }
1633 }
1634 }
1635 }
1636 /* Cut */
1637 if (cut)
1638 {
1640 const int pcount = ncount;
1641 int i;
1642 ncount = m_nodes.size();
1643 cnodes.resize(ncount, 0);
1644 /* Nodes */
1645 for (i = 0; i < ncount; ++i)
1646 {
1647 const btVector3 x = m_nodes[i].m_x;
1648 if ((i >= pcount) || (btFabs(ifn->Eval(x)) < accurary))
1649 {
1650 const btVector3 v = m_nodes[i].m_v;
1651 btScalar m = getMass(i);
1652 if (m > 0)
1653 {
1654 m *= 0.5f;
1655 m_nodes[i].m_im /= 0.5f;
1656 }
1657 appendNode(x, m);
1658 cnodes[i] = m_nodes.size() - 1;
1659 m_nodes[cnodes[i]].m_v = v;
1660 }
1661 }
1662 nbase = &m_nodes[0];
1663 /* Links */
1664 for (i = 0, ni = m_links.size(); i < ni; ++i)
1665 {
1666 const int id[] = {int(m_links[i].m_n[0] - nbase),
1667 int(m_links[i].m_n[1] - nbase)};
1668 int todetach = 0;
1669 if (cnodes[id[0]] && cnodes[id[1]])
1670 {
1671 appendLink(i);
1672 todetach = m_links.size() - 1;
1673 }
1674 else
1675 {
1676 if (((ifn->Eval(m_nodes[id[0]].m_x) < accurary) &&
1677 (ifn->Eval(m_nodes[id[1]].m_x) < accurary)))
1678 todetach = i;
1679 }
1680 if (todetach)
1681 {
1682 Link& l = m_links[todetach];
1683 for (int j = 0; j < 2; ++j)
1684 {
1685 int cn = cnodes[int(l.m_n[j] - nbase)];
1686 if (cn) l.m_n[j] = &m_nodes[cn];
1687 }
1688 }
1689 }
1690 /* Faces */
1691 for (i = 0, ni = m_faces.size(); i < ni; ++i)
1692 {
1693 Node** n = m_faces[i].m_n;
1694 if ((ifn->Eval(n[0]->m_x) < accurary) &&
1695 (ifn->Eval(n[1]->m_x) < accurary) &&
1696 (ifn->Eval(n[2]->m_x) < accurary))
1697 {
1698 for (int j = 0; j < 3; ++j)
1699 {
1700 int cn = cnodes[int(n[j] - nbase)];
1701 if (cn) n[j] = &m_nodes[cn];
1702 }
1703 }
1704 }
1705 /* Clean orphans */
1706 int nnodes = m_nodes.size();
1709 ranks.resize(nnodes, 0);
1710 for (i = 0, ni = m_links.size(); i < ni; ++i)
1711 {
1712 for (int j = 0; j < 2; ++j) ranks[int(m_links[i].m_n[j] - nbase)]++;
1713 }
1714 for (i = 0, ni = m_faces.size(); i < ni; ++i)
1715 {
1716 for (int j = 0; j < 3; ++j) ranks[int(m_faces[i].m_n[j] - nbase)]++;
1717 }
1718 for (i = 0; i < m_links.size(); ++i)
1719 {
1720 const int id[] = {int(m_links[i].m_n[0] - nbase),
1721 int(m_links[i].m_n[1] - nbase)};
1722 const bool sg[] = {ranks[id[0]] == 1,
1723 ranks[id[1]] == 1};
1724 if (sg[0] || sg[1])
1725 {
1726 --ranks[id[0]];
1727 --ranks[id[1]];
1728 btSwap(m_links[i], m_links[m_links.size() - 1]);
1729 m_links.pop_back();
1730 --i;
1731 }
1732 }
1733#if 0
1734 for(i=nnodes-1;i>=0;--i)
1735 {
1736 if(!ranks[i]) todelete.push_back(i);
1737 }
1738 if(todelete.size())
1739 {
1740 btAlignedObjectArray<int>& map=ranks;
1741 for(int i=0;i<nnodes;++i) map[i]=i;
1742 PointersToIndices(this);
1743 for(int i=0,ni=todelete.size();i<ni;++i)
1744 {
1745 int j=todelete[i];
1746 int& a=map[j];
1747 int& b=map[--nnodes];
1748 m_ndbvt.remove(m_nodes[a].m_leaf);m_nodes[a].m_leaf=0;
1749 btSwap(m_nodes[a],m_nodes[b]);
1750 j=a;a=b;b=j;
1751 }
1752 IndicesToPointers(this,&map[0]);
1753 m_nodes.resize(nnodes);
1754 }
1755#endif
1756 }
1757 m_bUpdateRtCst = true;
1758}
1759
1760//
1761bool btSoftBody::cutLink(const Node* node0, const Node* node1, btScalar position)
1762{
1763 return (cutLink(int(node0 - &m_nodes[0]), int(node1 - &m_nodes[0]), position));
1764}
1765
1766//
1767bool btSoftBody::cutLink(int node0, int node1, btScalar position)
1768{
1769 bool done = false;
1770 int i, ni;
1771 // const btVector3 d=m_nodes[node0].m_x-m_nodes[node1].m_x;
1772 const btVector3 x = Lerp(m_nodes[node0].m_x, m_nodes[node1].m_x, position);
1773 const btVector3 v = Lerp(m_nodes[node0].m_v, m_nodes[node1].m_v, position);
1774 const btScalar m = 1;
1775 appendNode(x, m);
1776 appendNode(x, m);
1777 Node* pa = &m_nodes[node0];
1778 Node* pb = &m_nodes[node1];
1779 Node* pn[2] = {&m_nodes[m_nodes.size() - 2],
1780 &m_nodes[m_nodes.size() - 1]};
1781 pn[0]->m_v = v;
1782 pn[1]->m_v = v;
1783 for (i = 0, ni = m_links.size(); i < ni; ++i)
1784 {
1785 const int mtch = MatchEdge(m_links[i].m_n[0], m_links[i].m_n[1], pa, pb);
1786 if (mtch != -1)
1787 {
1788 appendLink(i);
1789 Link* pft[] = {&m_links[i], &m_links[m_links.size() - 1]};
1790 pft[0]->m_n[1] = pn[mtch];
1791 pft[1]->m_n[0] = pn[1 - mtch];
1792 done = true;
1793 }
1794 }
1795 for (i = 0, ni = m_faces.size(); i < ni; ++i)
1796 {
1797 for (int k = 2, l = 0; l < 3; k = l++)
1798 {
1799 const int mtch = MatchEdge(m_faces[i].m_n[k], m_faces[i].m_n[l], pa, pb);
1800 if (mtch != -1)
1801 {
1802 appendFace(i);
1803 Face* pft[] = {&m_faces[i], &m_faces[m_faces.size() - 1]};
1804 pft[0]->m_n[l] = pn[mtch];
1805 pft[1]->m_n[k] = pn[1 - mtch];
1806 appendLink(pn[0], pft[0]->m_n[(l + 1) % 3], pft[0]->m_material, true);
1807 appendLink(pn[1], pft[0]->m_n[(l + 1) % 3], pft[0]->m_material, true);
1808 }
1809 }
1810 }
1811 if (!done)
1812 {
1813 m_ndbvt.remove(pn[0]->m_leaf);
1814 m_ndbvt.remove(pn[1]->m_leaf);
1815 m_nodes.pop_back();
1816 m_nodes.pop_back();
1817 }
1818 return (done);
1819}
1820
1821//
1822bool btSoftBody::rayTest(const btVector3& rayFrom,
1823 const btVector3& rayTo,
1824 sRayCast& results)
1825{
1826 if (m_faces.size() && m_fdbvt.empty())
1828
1829 results.body = this;
1830 results.fraction = 1.f;
1831 results.feature = eFeature::None;
1832 results.index = -1;
1833
1834 return (rayTest(rayFrom, rayTo, results.fraction, results.feature, results.index, false) != 0);
1835}
1836
1837//
1839{
1843 switch (preset)
1844 {
1850 break;
1853
1857
1859 break;
1860 }
1861}
1862
1864{
1865 int i, ni;
1866
1867 /* Update */
1868 if (m_bUpdateRtCst)
1869 {
1870 m_bUpdateRtCst = false;
1872 m_fdbvt.clear();
1874 {
1876 }
1877 }
1878
1879 /* Prepare */
1880 m_sst.sdt = dt * m_cfg.timescale;
1881 m_sst.isdt = 1 / m_sst.sdt;
1882 m_sst.velmrg = m_sst.sdt * 3;
1884 m_sst.updmrg = m_sst.radmrg * (btScalar)0.25;
1885 /* Forces */
1887 applyForces();
1888 /* Integrate */
1889 for (i = 0, ni = m_nodes.size(); i < ni; ++i)
1890 {
1891 Node& n = m_nodes[i];
1892 n.m_q = n.m_x;
1893 btVector3 deltaV = n.m_f * n.m_im * m_sst.sdt;
1894 {
1895 btScalar maxDisplacement = m_worldInfo->m_maxDisplacement;
1896 btScalar clampDeltaV = maxDisplacement / m_sst.sdt;
1897 for (int c = 0; c < 3; c++)
1898 {
1899 if (deltaV[c] > clampDeltaV)
1900 {
1901 deltaV[c] = clampDeltaV;
1902 }
1903 if (deltaV[c] < -clampDeltaV)
1904 {
1905 deltaV[c] = -clampDeltaV;
1906 }
1907 }
1908 }
1909 n.m_v += deltaV;
1910 n.m_x += n.m_v * m_sst.sdt;
1911 n.m_f = btVector3(0, 0, 0);
1912 }
1913 /* Clusters */
1915 /* Bounds */
1916 updateBounds();
1917 /* Nodes */
1919 vol;
1920 for (i = 0, ni = m_nodes.size(); i < ni; ++i)
1921 {
1922 Node& n = m_nodes[i];
1925 vol,
1926 n.m_v * m_sst.velmrg,
1927 m_sst.updmrg);
1928 }
1929 /* Faces */
1930 if (!m_fdbvt.empty())
1931 {
1932 for (int i = 0; i < m_faces.size(); ++i)
1933 {
1934 Face& f = m_faces[i];
1935 const btVector3 v = (f.m_n[0]->m_v +
1936 f.m_n[1]->m_v +
1937 f.m_n[2]->m_v) /
1938 3;
1939 vol = VolumeOf(f, m_sst.radmrg);
1941 vol,
1942 v * m_sst.velmrg,
1943 m_sst.updmrg);
1944 }
1945 }
1946 /* Pose */
1947 updatePose();
1948 /* Match */
1949 if (m_pose.m_bframe && (m_cfg.kMT > 0))
1950 {
1951 const btMatrix3x3 posetrs = m_pose.m_rot;
1952 for (int i = 0, ni = m_nodes.size(); i < ni; ++i)
1953 {
1954 Node& n = m_nodes[i];
1955 if (n.m_im > 0)
1956 {
1957 const btVector3 x = posetrs * m_pose.m_pos[i] + m_pose.m_com;
1958 n.m_x = Lerp(n.m_x, x, m_cfg.kMT);
1959 }
1960 }
1961 }
1962 /* Clear contacts */
1965 /* Optimize dbvt's */
1969}
1970
1971
1972//
1974{
1975 /* Apply clusters */
1976 applyClusters(false);
1977 /* Prepare links */
1978
1979 int i, ni;
1980
1981 for (i = 0, ni = m_links.size(); i < ni; ++i)
1982 {
1983 Link& l = m_links[i];
1984 l.m_c3 = l.m_n[1]->m_q - l.m_n[0]->m_q;
1985 l.m_c2 = 1 / (l.m_c3.length2() * l.m_c0);
1986 }
1987 /* Prepare anchors */
1988 for (i = 0, ni = m_anchors.size(); i < ni; ++i)
1989 {
1990 Anchor& a = m_anchors[i];
1991 const btVector3 ra = a.m_body->getWorldTransform().getBasis() * a.m_local;
1993 a.m_node->m_im,
1994 a.m_body->getInvMass(),
1996 ra);
1997 a.m_c1 = ra;
1998 a.m_c2 = m_sst.sdt * a.m_node->m_im;
1999 a.m_body->activate();
2000 }
2001 /* Solve velocities */
2002 if (m_cfg.viterations > 0)
2003 {
2004 /* Solve */
2005 for (int isolve = 0; isolve < m_cfg.viterations; ++isolve)
2006 {
2007 for (int iseq = 0; iseq < m_cfg.m_vsequence.size(); ++iseq)
2008 {
2009 getSolver(m_cfg.m_vsequence[iseq])(this, 1);
2010 }
2011 }
2012 /* Update */
2013 for (i = 0, ni = m_nodes.size(); i < ni; ++i)
2014 {
2015 Node& n = m_nodes[i];
2016 n.m_x = n.m_q + n.m_v * m_sst.sdt;
2017 }
2018 }
2019 /* Solve positions */
2020 if (m_cfg.piterations > 0)
2021 {
2022 for (int isolve = 0; isolve < m_cfg.piterations; ++isolve)
2023 {
2024 const btScalar ti = isolve / (btScalar)m_cfg.piterations;
2025 for (int iseq = 0; iseq < m_cfg.m_psequence.size(); ++iseq)
2026 {
2027 getSolver(m_cfg.m_psequence[iseq])(this, 1, ti);
2028 }
2029 }
2030 const btScalar vc = m_sst.isdt * (1 - m_cfg.kDP);
2031 for (i = 0, ni = m_nodes.size(); i < ni; ++i)
2032 {
2033 Node& n = m_nodes[i];
2034 n.m_v = (n.m_x - n.m_q) * vc;
2035 n.m_f = btVector3(0, 0, 0);
2036 }
2037 }
2038 /* Solve drift */
2039 if (m_cfg.diterations > 0)
2040 {
2041 const btScalar vcf = m_cfg.kVCF * m_sst.isdt;
2042 for (i = 0, ni = m_nodes.size(); i < ni; ++i)
2043 {
2044 Node& n = m_nodes[i];
2045 n.m_q = n.m_x;
2046 }
2047 for (int idrift = 0; idrift < m_cfg.diterations; ++idrift)
2048 {
2049 for (int iseq = 0; iseq < m_cfg.m_dsequence.size(); ++iseq)
2050 {
2051 getSolver(m_cfg.m_dsequence[iseq])(this, 1, 0);
2052 }
2053 }
2054 for (int i = 0, ni = m_nodes.size(); i < ni; ++i)
2055 {
2056 Node& n = m_nodes[i];
2057 n.m_v += (n.m_x - n.m_q) * vcf;
2058 }
2059 }
2060 /* Apply clusters */
2061 dampClusters();
2062 applyClusters(true);
2063}
2064
2065//
2066void btSoftBody::staticSolve(int iterations)
2067{
2068 for (int isolve = 0; isolve < iterations; ++isolve)
2069 {
2070 for (int iseq = 0; iseq < m_cfg.m_psequence.size(); ++iseq)
2071 {
2072 getSolver(m_cfg.m_psequence[iseq])(this, 1, 0);
2073 }
2074 }
2075}
2076
2077//
2078void btSoftBody::solveCommonConstraints(btSoftBody** /*bodies*/, int /*count*/, int /*iterations*/)
2079{
2081}
2082
2083//
2085{
2086 const int nb = bodies.size();
2087 int iterations = 0;
2088 int i;
2089
2090 for (i = 0; i < nb; ++i)
2091 {
2092 iterations = btMax(iterations, bodies[i]->m_cfg.citerations);
2093 }
2094 for (i = 0; i < nb; ++i)
2095 {
2096 bodies[i]->prepareClusters(iterations);
2097 }
2098 for (i = 0; i < iterations; ++i)
2099 {
2100 const btScalar sor = 1;
2101 for (int j = 0; j < nb; ++j)
2102 {
2103 bodies[j]->solveClusters(sor);
2104 }
2105 }
2106 for (i = 0; i < nb; ++i)
2107 {
2108 bodies[i]->cleanupClusters();
2109 }
2110}
2111
2112//
2114{
2115 /* Update */
2116 updateNormals();
2117}
2118
2119//
2121{
2122 m_rayFrom = rayFrom;
2123 m_rayNormalizedDirection = (rayTo - rayFrom);
2124 m_rayTo = rayTo;
2125 m_mint = mxt;
2126 m_face = 0;
2127 m_tests = 0;
2128}
2129
2130//
2132{
2134 const btScalar t = rayFromToTriangle(m_rayFrom, m_rayTo, m_rayNormalizedDirection,
2135 f.m_n[0]->m_x,
2136 f.m_n[1]->m_x,
2137 f.m_n[2]->m_x,
2138 m_mint);
2139 if ((t > 0) && (t < m_mint))
2140 {
2141 m_mint = t;
2142 m_face = &f;
2143 }
2144 ++m_tests;
2145}
2146
2147//
2149 const btVector3& rayTo,
2150 const btVector3& rayNormalizedDirection,
2151 const btVector3& a,
2152 const btVector3& b,
2153 const btVector3& c,
2154 btScalar maxt)
2155{
2156 static const btScalar ceps = -SIMD_EPSILON * 10;
2157 static const btScalar teps = SIMD_EPSILON * 10;
2158
2159 const btVector3 n = btCross(b - a, c - a);
2160 const btScalar d = btDot(a, n);
2161 const btScalar den = btDot(rayNormalizedDirection, n);
2162 if (!btFuzzyZero(den))
2163 {
2164 const btScalar num = btDot(rayFrom, n) - d;
2165 const btScalar t = -num / den;
2166 if ((t > teps) && (t < maxt))
2167 {
2168 const btVector3 hit = rayFrom + rayNormalizedDirection * t;
2169 if ((btDot(n, btCross(a - hit, b - hit)) > ceps) &&
2170 (btDot(n, btCross(b - hit, c - hit)) > ceps) &&
2171 (btDot(n, btCross(c - hit, a - hit)) > ceps))
2172 {
2173 return (t);
2174 }
2175 }
2176 }
2177 return (-1);
2178}
2179
2180//
2182{
2183#define PTR2IDX(_p_, _b_) reinterpret_cast<btSoftBody::Node*>((_p_) - (_b_))
2184 btSoftBody::Node* base = m_nodes.size() ? &m_nodes[0] : 0;
2185 int i, ni;
2186
2187 for (i = 0, ni = m_nodes.size(); i < ni; ++i)
2188 {
2189 if (m_nodes[i].m_leaf)
2190 {
2191 m_nodes[i].m_leaf->data = *(void**)&i;
2192 }
2193 }
2194 for (i = 0, ni = m_links.size(); i < ni; ++i)
2195 {
2196 m_links[i].m_n[0] = PTR2IDX(m_links[i].m_n[0], base);
2197 m_links[i].m_n[1] = PTR2IDX(m_links[i].m_n[1], base);
2198 }
2199 for (i = 0, ni = m_faces.size(); i < ni; ++i)
2200 {
2201 m_faces[i].m_n[0] = PTR2IDX(m_faces[i].m_n[0], base);
2202 m_faces[i].m_n[1] = PTR2IDX(m_faces[i].m_n[1], base);
2203 m_faces[i].m_n[2] = PTR2IDX(m_faces[i].m_n[2], base);
2204 if (m_faces[i].m_leaf)
2205 {
2206 m_faces[i].m_leaf->data = *(void**)&i;
2207 }
2208 }
2209 for (i = 0, ni = m_anchors.size(); i < ni; ++i)
2210 {
2211 m_anchors[i].m_node = PTR2IDX(m_anchors[i].m_node, base);
2212 }
2213 for (i = 0, ni = m_notes.size(); i < ni; ++i)
2214 {
2215 for (int j = 0; j < m_notes[i].m_rank; ++j)
2216 {
2217 m_notes[i].m_nodes[j] = PTR2IDX(m_notes[i].m_nodes[j], base);
2218 }
2219 }
2220#undef PTR2IDX
2221}
2222
2223//
2225{
2226#define IDX2PTR(_p_, _b_) map ? (&(_b_)[map[(((char*)_p_) - (char*)0)]]) : (&(_b_)[(((char*)_p_) - (char*)0)])
2227 btSoftBody::Node* base = m_nodes.size() ? &m_nodes[0] : 0;
2228 int i, ni;
2229
2230 for (i = 0, ni = m_nodes.size(); i < ni; ++i)
2231 {
2232 if (m_nodes[i].m_leaf)
2233 {
2234 m_nodes[i].m_leaf->data = &m_nodes[i];
2235 }
2236 }
2237 for (i = 0, ni = m_links.size(); i < ni; ++i)
2238 {
2239 m_links[i].m_n[0] = IDX2PTR(m_links[i].m_n[0], base);
2240 m_links[i].m_n[1] = IDX2PTR(m_links[i].m_n[1], base);
2241 }
2242 for (i = 0, ni = m_faces.size(); i < ni; ++i)
2243 {
2244 m_faces[i].m_n[0] = IDX2PTR(m_faces[i].m_n[0], base);
2245 m_faces[i].m_n[1] = IDX2PTR(m_faces[i].m_n[1], base);
2246 m_faces[i].m_n[2] = IDX2PTR(m_faces[i].m_n[2], base);
2247 if (m_faces[i].m_leaf)
2248 {
2249 m_faces[i].m_leaf->data = &m_faces[i];
2250 }
2251 }
2252 for (i = 0, ni = m_anchors.size(); i < ni; ++i)
2253 {
2254 m_anchors[i].m_node = IDX2PTR(m_anchors[i].m_node, base);
2255 }
2256 for (i = 0, ni = m_notes.size(); i < ni; ++i)
2257 {
2258 for (int j = 0; j < m_notes[i].m_rank; ++j)
2259 {
2260 m_notes[i].m_nodes[j] = IDX2PTR(m_notes[i].m_nodes[j], base);
2261 }
2262 }
2263#undef IDX2PTR
2264}
2265
2266//
2267int btSoftBody::rayTest(const btVector3& rayFrom, const btVector3& rayTo,
2268 btScalar& mint, eFeature::_& feature, int& index, bool bcountonly) const
2269{
2270 int cnt = 0;
2271 btVector3 dir = rayTo - rayFrom;
2272
2273 if (bcountonly || m_fdbvt.empty())
2274 { /* Full search */
2275
2276 for (int i = 0, ni = m_faces.size(); i < ni; ++i)
2277 {
2278 const btSoftBody::Face& f = m_faces[i];
2279
2280 const btScalar t = RayFromToCaster::rayFromToTriangle(rayFrom, rayTo, dir,
2281 f.m_n[0]->m_x,
2282 f.m_n[1]->m_x,
2283 f.m_n[2]->m_x,
2284 mint);
2285 if (t > 0)
2286 {
2287 ++cnt;
2288 if (!bcountonly)
2289 {
2291 index = i;
2292 mint = t;
2293 }
2294 }
2295 }
2296 }
2297 else
2298 { /* Use dbvt */
2299 RayFromToCaster collider(rayFrom, rayTo, mint);
2300
2301 btDbvt::rayTest(m_fdbvt.m_root, rayFrom, rayTo, collider);
2302 if (collider.m_face)
2303 {
2304 mint = collider.m_mint;
2306 index = (int)(collider.m_face - &m_faces[0]);
2307 cnt = 1;
2308 }
2309 }
2310
2311 for (int i = 0; i < m_tetras.size(); i++)
2312 {
2313 const btSoftBody::Tetra& tet = m_tetras[i];
2314 int tetfaces[4][3] = {{0, 1, 2}, {0, 1, 3}, {1, 2, 3}, {0, 2, 3}};
2315 for (int f = 0; f < 4; f++)
2316 {
2317 int index0 = tetfaces[f][0];
2318 int index1 = tetfaces[f][1];
2319 int index2 = tetfaces[f][2];
2320 btVector3 v0 = tet.m_n[index0]->m_x;
2321 btVector3 v1 = tet.m_n[index1]->m_x;
2322 btVector3 v2 = tet.m_n[index2]->m_x;
2323
2324 const btScalar t = RayFromToCaster::rayFromToTriangle(rayFrom, rayTo, dir,
2325 v0, v1, v2,
2326 mint);
2327 if (t > 0)
2328 {
2329 ++cnt;
2330 if (!bcountonly)
2331 {
2333 index = i;
2334 mint = t;
2335 }
2336 }
2337 }
2338 }
2339 return (cnt);
2340}
2341
2342//
2344{
2345 m_fdbvt.clear();
2346 for (int i = 0; i < m_faces.size(); ++i)
2347 {
2348 Face& f = m_faces[i];
2349 f.m_leaf = m_fdbvt.insert(VolumeOf(f, 0), &f);
2350 }
2351}
2352
2353//
2355{
2356 btVector3 com(0, 0, 0);
2357 if (m_pose.m_bframe)
2358 {
2359 for (int i = 0, ni = m_nodes.size(); i < ni; ++i)
2360 {
2361 com += m_nodes[i].m_x * m_pose.m_wgh[i];
2362 }
2363 }
2364 return (com);
2365}
2366
2368 const btVector3& x,
2369 btScalar margin,
2370 btSoftBody::sCti& cti) const
2371{
2372 btVector3 nrm;
2373 const btCollisionShape* shp = colObjWrap->getCollisionShape();
2374 // const btRigidBody *tmpRigid = btRigidBody::upcast(colObjWrap->getCollisionObject());
2375 //const btTransform &wtr = tmpRigid ? tmpRigid->getWorldTransform() : colObjWrap->getWorldTransform();
2376 const btTransform& wtr = colObjWrap->getWorldTransform();
2377 //todo: check which transform is needed here
2378
2379 btScalar dst =
2381 wtr.invXform(x),
2382 shp,
2383 nrm,
2384 margin);
2385 if (dst < 0)
2386 {
2387 cti.m_colObj = colObjWrap->getCollisionObject();
2388 cti.m_normal = wtr.getBasis() * nrm;
2389 cti.m_offset = -btDot(cti.m_normal, x - cti.m_normal * dst);
2390 return (true);
2391 }
2392 return (false);
2393}
2394
2395//
2397 const btVector3& x,
2398 btScalar margin,
2399 btSoftBody::sCti& cti, bool predict) const
2400{
2401 btVector3 nrm;
2402 const btCollisionShape* shp = colObjWrap->getCollisionShape();
2403 const btCollisionObject* tmpCollisionObj = colObjWrap->getCollisionObject();
2404 // use the position x_{n+1}^* = x_n + dt * v_{n+1}^* where v_{n+1}^* = v_n + dtg for collision detect
2405 // but resolve contact at x_n
2406// btTransform wtr = (predict) ?
2407// (colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform()*(*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform())
2408// : colObjWrap->getWorldTransform();
2409 const btTransform& wtr = colObjWrap->getWorldTransform();
2410 btScalar dst =
2412 wtr.invXform(x),
2413 shp,
2414 nrm,
2415 margin);
2416 if (!predict)
2417 {
2418 cti.m_colObj = colObjWrap->getCollisionObject();
2419 cti.m_normal = wtr.getBasis() * nrm;
2420 cti.m_offset = dst;
2421 }
2422 if (dst < 0)
2423 return true;
2424 return (false);
2425}
2426
2427//
2428// Compute barycentric coordinates (u, v, w) for
2429// point p with respect to triangle (a, b, c)
2430static void getBarycentric(const btVector3& p, btVector3& a, btVector3& b, btVector3& c, btVector3& bary)
2431{
2432 btVector3 v0 = b - a, v1 = c - a, v2 = p - a;
2433 btScalar d00 = v0.dot(v0);
2434 btScalar d01 = v0.dot(v1);
2435 btScalar d11 = v1.dot(v1);
2436 btScalar d20 = v2.dot(v0);
2437 btScalar d21 = v2.dot(v1);
2438 btScalar denom = d00 * d11 - d01 * d01;
2439 bary.setY((d11 * d20 - d01 * d21) / denom);
2440 bary.setZ((d00 * d21 - d01 * d20) / denom);
2441 bary.setX(btScalar(1) - bary.getY() - bary.getZ());
2442}
2443
2444//
2446 Face& f,
2447 btVector3& contact_point,
2448 btVector3& bary,
2449 btScalar margin,
2450 btSoftBody::sCti& cti, bool predict) const
2451{
2452 btVector3 nrm;
2453 const btCollisionShape* shp = colObjWrap->getCollisionShape();
2454 const btCollisionObject* tmpCollisionObj = colObjWrap->getCollisionObject();
2455 // use the position x_{n+1}^* = x_n + dt * v_{n+1}^* where v_{n+1}^* = v_n + dtg for collision detect
2456 // but resolve contact at x_n
2457 btTransform wtr = (predict) ?
2458 (colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform()*(*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform())
2459 : colObjWrap->getWorldTransform();
2460// const btTransform& wtr = colObjWrap->getWorldTransform();
2461 btScalar dst;
2462
2463//#define USE_QUADRATURE 1
2464//#define CACHE_PREV_COLLISION
2465
2466 // use the contact position of the previous collision
2467#ifdef CACHE_PREV_COLLISION
2468 if (f.m_pcontact[3] != 0)
2469 {
2470 for (int i = 0; i < 3; ++i)
2471 bary[i] = f.m_pcontact[i];
2472 contact_point = BaryEval(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary);
2474 wtr.invXform(contact_point),
2475 shp,
2476 nrm,
2477 margin);
2478 nrm = wtr.getBasis() * nrm;
2479 // use cached contact point
2480 }
2481 else
2482 {
2484 btTransform triangle_transform;
2485 triangle_transform.setIdentity();
2486 triangle_transform.setOrigin(f.m_n[0]->m_x);
2487 btTriangleShape triangle(btVector3(0,0,0), f.m_n[1]->m_x-f.m_n[0]->m_x, f.m_n[2]->m_x-f.m_n[0]->m_x);
2488 btVector3 guess(0,0,0);
2489 const btConvexShape* csh = static_cast<const btConvexShape*>(shp);
2490 btGjkEpaSolver2::SignedDistance(&triangle, triangle_transform, csh, wtr, guess, results);
2491 dst = results.distance - margin;
2492 contact_point = results.witnesses[0];
2493 getBarycentric(contact_point, f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary);
2494 nrm = results.normal;
2495 for (int i = 0; i < 3; ++i)
2496 f.m_pcontact[i] = bary[i];
2497 }
2498
2499#endif
2500
2501 // use collision quadrature point
2502#ifdef USE_QUADRATURE
2503 {
2504 dst = SIMD_INFINITY;
2505 btVector3 local_nrm;
2506 for (int q = 0; q < m_quads.size(); ++q)
2507 {
2508 btVector3 p = BaryEval(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, m_quads[q]);
2510 wtr.invXform(p),
2511 shp,
2512 local_nrm,
2513 margin);
2514 if (local_dst < dst)
2515 {
2516 dst = local_dst;
2517 contact_point = p;
2518 bary = m_quads[q];
2519 nrm = wtr.getBasis() * local_nrm;
2520 }
2521 }
2522 }
2523#endif
2524
2525 // regular face contact
2526 {
2528 btTransform triangle_transform;
2529 triangle_transform.setIdentity();
2530 triangle_transform.setOrigin(f.m_n[0]->m_x);
2531 btTriangleShape triangle(btVector3(0,0,0), f.m_n[1]->m_x-f.m_n[0]->m_x, f.m_n[2]->m_x-f.m_n[0]->m_x);
2532 btVector3 guess(0,0,0);
2533 const btConvexShape* csh = static_cast<const btConvexShape*>(shp);
2534 btGjkEpaSolver2::SignedDistance(&triangle, triangle_transform, csh, wtr, guess, results);
2535 dst = results.distance - margin;
2536 contact_point = results.witnesses[0];
2537 getBarycentric(contact_point, f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary);
2538 nrm = results.normal;
2539 for (int i = 0; i < 3; ++i)
2540 f.m_pcontact[i] = bary[i];
2541 }
2542
2543 if (!predict)
2544 {
2545 cti.m_colObj = colObjWrap->getCollisionObject();
2546 cti.m_normal = nrm;
2547 cti.m_offset = dst;
2548 }
2549
2550 if (dst < 0)
2551 return true;
2552 return (false);
2553}
2554
2555//
2557{
2558 const btVector3 zv(0, 0, 0);
2559 int i, ni;
2560
2561 for (i = 0, ni = m_nodes.size(); i < ni; ++i)
2562 {
2563 m_nodes[i].m_n = zv;
2564 }
2565 for (i = 0, ni = m_faces.size(); i < ni; ++i)
2566 {
2567 btSoftBody::Face& f = m_faces[i];
2568 const btVector3 n = btCross(f.m_n[1]->m_x - f.m_n[0]->m_x,
2569 f.m_n[2]->m_x - f.m_n[0]->m_x);
2570 f.m_normal = n;
2572 f.m_n[0]->m_n += n;
2573 f.m_n[1]->m_n += n;
2574 f.m_n[2]->m_n += n;
2575 }
2576 for (i = 0, ni = m_nodes.size(); i < ni; ++i)
2577 {
2578 btScalar len = m_nodes[i].m_n.length();
2579 if (len > SIMD_EPSILON)
2580 m_nodes[i].m_n /= len;
2581 }
2582}
2583
2584//
2586{
2587 /*if( m_acceleratedSoftBody )
2588 {
2589 // If we have an accelerated softbody we need to obtain the bounds correctly
2590 // For now (slightly hackily) just have a very large AABB
2591 // TODO: Write get bounds kernel
2592 // If that is updating in place, atomic collisions might be low (when the cloth isn't perfectly aligned to an axis) and we could
2593 // probably do a test and exchange reasonably efficiently.
2594
2595 m_bounds[0] = btVector3(-1000, -1000, -1000);
2596 m_bounds[1] = btVector3(1000, 1000, 1000);
2597
2598 } else {*/
2599// if (m_ndbvt.m_root)
2600// {
2601// const btVector3& mins = m_ndbvt.m_root->volume.Mins();
2602// const btVector3& maxs = m_ndbvt.m_root->volume.Maxs();
2603// const btScalar csm = getCollisionShape()->getMargin();
2604// const btVector3 mrg = btVector3(csm,
2605// csm,
2606// csm) *
2607// 1; // ??? to investigate...
2608// m_bounds[0] = mins - mrg;
2609// m_bounds[1] = maxs + mrg;
2610// if (0 != getBroadphaseHandle())
2611// {
2612// m_worldInfo->m_broadphase->setAabb(getBroadphaseHandle(),
2613// m_bounds[0],
2614// m_bounds[1],
2615// m_worldInfo->m_dispatcher);
2616// }
2617// }
2618// else
2619// {
2620// m_bounds[0] =
2621// m_bounds[1] = btVector3(0, 0, 0);
2622// }
2623 if (m_nodes.size())
2624 {
2625 btVector3 mins = m_nodes[0].m_x;
2626 btVector3 maxs = m_nodes[0].m_x;
2627 for (int i = 1; i < m_nodes.size(); ++i)
2628 {
2629 for (int d = 0; d < 3; ++d)
2630 {
2631 if (m_nodes[i].m_x[d] > maxs[d])
2632 maxs[d] = m_nodes[i].m_x[d];
2633 if (m_nodes[i].m_x[d] < mins[d])
2634 mins[d] = m_nodes[i].m_x[d];
2635 }
2636 }
2637 const btScalar csm = getCollisionShape()->getMargin();
2638 const btVector3 mrg = btVector3(csm,
2639 csm,
2640 csm);
2641 m_bounds[0] = mins - mrg;
2642 m_bounds[1] = maxs + mrg;
2643 if (0 != getBroadphaseHandle())
2644 {
2646 m_bounds[0],
2647 m_bounds[1],
2649 }
2650 }
2651 else
2652 {
2653 m_bounds[0] =
2654 m_bounds[1] = btVector3(0, 0, 0);
2655 }
2656}
2657
2658//
2660{
2661 if (m_pose.m_bframe)
2662 {
2663 btSoftBody::Pose& pose = m_pose;
2664 const btVector3 com = evaluateCom();
2665 /* Com */
2666 pose.m_com = com;
2667 /* Rotation */
2668 btMatrix3x3 Apq;
2669 const btScalar eps = SIMD_EPSILON;
2670 Apq[0] = Apq[1] = Apq[2] = btVector3(0, 0, 0);
2671 Apq[0].setX(eps);
2672 Apq[1].setY(eps * 2);
2673 Apq[2].setZ(eps * 3);
2674 for (int i = 0, ni = m_nodes.size(); i < ni; ++i)
2675 {
2676 const btVector3 a = pose.m_wgh[i] * (m_nodes[i].m_x - com);
2677 const btVector3& b = pose.m_pos[i];
2678 Apq[0] += a.x() * b;
2679 Apq[1] += a.y() * b;
2680 Apq[2] += a.z() * b;
2681 }
2682 btMatrix3x3 r, s;
2683 PolarDecompose(Apq, r, s);
2684 pose.m_rot = r;
2685 pose.m_scl = pose.m_aqq * r.transpose() * Apq;
2686 if (m_cfg.maxvolume > 1)
2687 {
2688 const btScalar idet = Clamp<btScalar>(1 / pose.m_scl.determinant(),
2689 1, m_cfg.maxvolume);
2690 pose.m_scl = Mul(pose.m_scl, idet);
2691 }
2692 }
2693}
2694
2695//
2696void btSoftBody::updateArea(bool averageArea)
2697{
2698 int i, ni;
2699
2700 /* Face area */
2701 for (i = 0, ni = m_faces.size(); i < ni; ++i)
2702 {
2703 Face& f = m_faces[i];
2704 f.m_ra = AreaOf(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x);
2705 }
2706
2707 /* Node area */
2708
2709 if (averageArea)
2710 {
2712 counts.resize(m_nodes.size(), 0);
2713 for (i = 0, ni = m_nodes.size(); i < ni; ++i)
2714 {
2715 m_nodes[i].m_area = 0;
2716 }
2717 for (i = 0, ni = m_faces.size(); i < ni; ++i)
2718 {
2719 btSoftBody::Face& f = m_faces[i];
2720 for (int j = 0; j < 3; ++j)
2721 {
2722 const int index = (int)(f.m_n[j] - &m_nodes[0]);
2723 counts[index]++;
2724 f.m_n[j]->m_area += btFabs(f.m_ra);
2725 }
2726 }
2727 for (i = 0, ni = m_nodes.size(); i < ni; ++i)
2728 {
2729 if (counts[i] > 0)
2730 m_nodes[i].m_area /= (btScalar)counts[i];
2731 else
2732 m_nodes[i].m_area = 0;
2733 }
2734 }
2735 else
2736 {
2737 // initialize node area as zero
2738 for (i = 0, ni = m_nodes.size(); i < ni; ++i)
2739 {
2740 m_nodes[i].m_area = 0;
2741 }
2742
2743 for (i = 0, ni = m_faces.size(); i < ni; ++i)
2744 {
2745 btSoftBody::Face& f = m_faces[i];
2746
2747 for (int j = 0; j < 3; ++j)
2748 {
2749 f.m_n[j]->m_area += f.m_ra;
2750 }
2751 }
2752
2753 for (i = 0, ni = m_nodes.size(); i < ni; ++i)
2754 {
2755 m_nodes[i].m_area *= 0.3333333f;
2756 }
2757 }
2758}
2759
2761{
2762 int i, ni;
2763
2764 /* Links */
2765 for (i = 0, ni = m_links.size(); i < ni; ++i)
2766 {
2767 Link& l = m_links[i];
2768 Material& m = *l.m_material;
2769 l.m_c0 = (l.m_n[0]->m_im + l.m_n[1]->m_im) / m.m_kLST;
2770 }
2771}
2772
2774{
2777 updateArea();
2778}
2779
2780//
2782{
2783 int i;
2784
2785 for (i = 0; i < m_clusters.size(); ++i)
2786 {
2787 Cluster& c = *m_clusters[i];
2788 c.m_imass = 0;
2789 c.m_masses.resize(c.m_nodes.size());
2790 for (int j = 0; j < c.m_nodes.size(); ++j)
2791 {
2792 if (c.m_nodes[j]->m_im == 0)
2793 {
2794 c.m_containsAnchor = true;
2795 c.m_masses[j] = BT_LARGE_FLOAT;
2796 }
2797 else
2798 {
2799 c.m_masses[j] = btScalar(1.) / c.m_nodes[j]->m_im;
2800 }
2801 c.m_imass += c.m_masses[j];
2802 }
2803 c.m_imass = btScalar(1.) / c.m_imass;
2805 c.m_lv = btVector3(0, 0, 0);
2806 c.m_av = btVector3(0, 0, 0);
2807 c.m_leaf = 0;
2808 /* Inertia */
2809 btMatrix3x3& ii = c.m_locii;
2810 ii[0] = ii[1] = ii[2] = btVector3(0, 0, 0);
2811 {
2812 int i, ni;
2813
2814 for (i = 0, ni = c.m_nodes.size(); i < ni; ++i)
2815 {
2816 const btVector3 k = c.m_nodes[i]->m_x - c.m_com;
2817 const btVector3 q = k * k;
2818 const btScalar m = c.m_masses[i];
2819 ii[0][0] += m * (q[1] + q[2]);
2820 ii[1][1] += m * (q[0] + q[2]);
2821 ii[2][2] += m * (q[0] + q[1]);
2822 ii[0][1] -= m * k[0] * k[1];
2823 ii[0][2] -= m * k[0] * k[2];
2824 ii[1][2] -= m * k[1] * k[2];
2825 }
2826 }
2827 ii[1][0] = ii[0][1];
2828 ii[2][0] = ii[0][2];
2829 ii[2][1] = ii[1][2];
2830
2831 ii = ii.inverse();
2832
2833 /* Frame */
2836 c.m_framerefs.resize(c.m_nodes.size());
2837 {
2838 int i;
2839 for (i = 0; i < c.m_framerefs.size(); ++i)
2840 {
2841 c.m_framerefs[i] = c.m_nodes[i]->m_x - c.m_com;
2842 }
2843 }
2844 }
2845}
2846
2847//
2849{
2850 BT_PROFILE("UpdateClusters");
2851 int i;
2852
2853 for (i = 0; i < m_clusters.size(); ++i)
2854 {
2856 const int n = c.m_nodes.size();
2857 //const btScalar invn=1/(btScalar)n;
2858 if (n)
2859 {
2860 /* Frame */
2861 const btScalar eps = btScalar(0.0001);
2862 btMatrix3x3 m, r, s;
2863 m[0] = m[1] = m[2] = btVector3(0, 0, 0);
2864 m[0][0] = eps * 1;
2865 m[1][1] = eps * 2;
2866 m[2][2] = eps * 3;
2867 c.m_com = clusterCom(&c);
2868 for (int i = 0; i < c.m_nodes.size(); ++i)
2869 {
2870 const btVector3 a = c.m_nodes[i]->m_x - c.m_com;
2871 const btVector3& b = c.m_framerefs[i];
2872 m[0] += a[0] * b;
2873 m[1] += a[1] * b;
2874 m[2] += a[2] * b;
2875 }
2876 PolarDecompose(m, r, s);
2879 /* Inertia */
2880#if 1 /* Constant */
2882#else
2883#if 0 /* Sphere */
2884 const btScalar rk=(2*c.m_extents.length2())/(5*c.m_imass);
2885 const btVector3 inertia(rk,rk,rk);
2886 const btVector3 iin(btFabs(inertia[0])>SIMD_EPSILON?1/inertia[0]:0,
2887 btFabs(inertia[1])>SIMD_EPSILON?1/inertia[1]:0,
2888 btFabs(inertia[2])>SIMD_EPSILON?1/inertia[2]:0);
2889
2890 c.m_invwi=c.m_xform.getBasis().scaled(iin)*c.m_xform.getBasis().transpose();
2891#else /* Actual */
2892 c.m_invwi[0] = c.m_invwi[1] = c.m_invwi[2] = btVector3(0, 0, 0);
2893 for (int i = 0; i < n; ++i)
2894 {
2895 const btVector3 k = c.m_nodes[i]->m_x - c.m_com;
2896 const btVector3 q = k * k;
2897 const btScalar m = 1 / c.m_nodes[i]->m_im;
2898 c.m_invwi[0][0] += m * (q[1] + q[2]);
2899 c.m_invwi[1][1] += m * (q[0] + q[2]);
2900 c.m_invwi[2][2] += m * (q[0] + q[1]);
2901 c.m_invwi[0][1] -= m * k[0] * k[1];
2902 c.m_invwi[0][2] -= m * k[0] * k[2];
2903 c.m_invwi[1][2] -= m * k[1] * k[2];
2904 }
2905 c.m_invwi[1][0] = c.m_invwi[0][1];
2906 c.m_invwi[2][0] = c.m_invwi[0][2];
2907 c.m_invwi[2][1] = c.m_invwi[1][2];
2908 c.m_invwi = c.m_invwi.inverse();
2909#endif
2910#endif
2911 /* Velocities */
2912 c.m_lv = btVector3(0, 0, 0);
2913 c.m_av = btVector3(0, 0, 0);
2914 {
2915 int i;
2916
2917 for (i = 0; i < n; ++i)
2918 {
2919 const btVector3 v = c.m_nodes[i]->m_v * c.m_masses[i];
2920 c.m_lv += v;
2921 c.m_av += btCross(c.m_nodes[i]->m_x - c.m_com, v);
2922 }
2923 }
2924 c.m_lv = c.m_imass * c.m_lv * (1 - c.m_ldamping);
2925 c.m_av = c.m_invwi * c.m_av * (1 - c.m_adamping);
2926 c.m_vimpulses[0] =
2927 c.m_vimpulses[1] = btVector3(0, 0, 0);
2928 c.m_dimpulses[0] =
2929 c.m_dimpulses[1] = btVector3(0, 0, 0);
2930 c.m_nvimpulses = 0;
2931 c.m_ndimpulses = 0;
2932 /* Matching */
2933 if (c.m_matching > 0)
2934 {
2935 for (int j = 0; j < c.m_nodes.size(); ++j)
2936 {
2937 Node& n = *c.m_nodes[j];
2938 const btVector3 x = c.m_framexform * c.m_framerefs[j];
2939 n.m_x = Lerp(n.m_x, x, c.m_matching);
2940 }
2941 }
2942 /* Dbvt */
2943 if (c.m_collide)
2944 {
2945 btVector3 mi = c.m_nodes[0]->m_x;
2946 btVector3 mx = mi;
2947 for (int j = 1; j < n; ++j)
2948 {
2949 mi.setMin(c.m_nodes[j]->m_x);
2950 mx.setMax(c.m_nodes[j]->m_x);
2951 }
2953 bounds = btDbvtVolume::FromMM(mi, mx);
2954 if (c.m_leaf)
2956 else
2957 c.m_leaf = m_cdbvt.insert(bounds, &c);
2958 }
2959 }
2960 }
2961}
2962
2963//
2965{
2966 for (int i = 0; i < m_joints.size(); ++i)
2967 {
2968 m_joints[i]->Terminate(m_sst.sdt);
2969 if (m_joints[i]->m_delete)
2970 {
2972 m_joints.remove(m_joints[i--]);
2973 }
2974 }
2975}
2976
2977//
2979{
2980 for (int i = 0; i < m_joints.size(); ++i)
2981 {
2982 m_joints[i]->Prepare(m_sst.sdt, iterations);
2983 }
2984}
2985
2986//
2988{
2989 for (int i = 0, ni = m_joints.size(); i < ni; ++i)
2990 {
2991 m_joints[i]->Solve(m_sst.sdt, sor);
2992 }
2993}
2994
2995//
2997{
2998 BT_PROFILE("ApplyClusters");
2999 // const btScalar f0=m_sst.sdt;
3000 //const btScalar f1=f0/2;
3003 deltas.resize(m_nodes.size(), btVector3(0, 0, 0));
3004 weights.resize(m_nodes.size(), 0);
3005 int i;
3006
3007 if (drift)
3008 {
3009 for (i = 0; i < m_clusters.size(); ++i)
3010 {
3011 Cluster& c = *m_clusters[i];
3012 if (c.m_ndimpulses)
3013 {
3016 }
3017 }
3018 }
3019
3020 for (i = 0; i < m_clusters.size(); ++i)
3021 {
3022 Cluster& c = *m_clusters[i];
3023 if (0 < (drift ? c.m_ndimpulses : c.m_nvimpulses))
3024 {
3025 const btVector3 v = (drift ? c.m_dimpulses[0] : c.m_vimpulses[0]) * m_sst.sdt;
3026 const btVector3 w = (drift ? c.m_dimpulses[1] : c.m_vimpulses[1]) * m_sst.sdt;
3027 for (int j = 0; j < c.m_nodes.size(); ++j)
3028 {
3029 const int idx = int(c.m_nodes[j] - &m_nodes[0]);
3030 const btVector3& x = c.m_nodes[j]->m_x;
3031 const btScalar q = c.m_masses[j];
3032 deltas[idx] += (v + btCross(w, x - c.m_com)) * q;
3033 weights[idx] += q;
3034 }
3035 }
3036 }
3037 for (i = 0; i < deltas.size(); ++i)
3038 {
3039 if (weights[i] > 0)
3040 {
3041 m_nodes[i].m_x += deltas[i] / weights[i];
3042 }
3043 }
3044}
3045
3046//
3048{
3049 int i;
3050
3051 for (i = 0; i < m_clusters.size(); ++i)
3052 {
3053 Cluster& c = *m_clusters[i];
3054 if (c.m_ndamping > 0)
3055 {
3056 for (int j = 0; j < c.m_nodes.size(); ++j)
3057 {
3058 Node& n = *c.m_nodes[j];
3059 if (n.m_im > 0)
3060 {
3061 const btVector3 vx = c.m_lv + btCross(c.m_av, c.m_nodes[j]->m_q - c.m_com);
3062 if (vx.length2() <= n.m_v.length2())
3063 {
3064 n.m_v += c.m_ndamping * (vx - n.m_v);
3065 }
3066 }
3067 }
3068 }
3069 }
3070}
3071
3073{
3074 for (int i = 0; i < m_links.size(); ++i)
3075 {
3076 m_links[i].Feature::m_material->m_kLST = k;
3077 }
3078}
3079
3081{
3082 btScalar unit_simplex_measure = 1./6.;
3083
3084 for (int i = 0; i < m_tetras.size(); ++i)
3085 {
3086 Tetra &t = m_tetras[i];
3087 btVector3 c1 = t.m_n[1]->m_x - t.m_n[0]->m_x;
3088 btVector3 c2 = t.m_n[2]->m_x - t.m_n[0]->m_x;
3089 btVector3 c3 = t.m_n[3]->m_x - t.m_n[0]->m_x;
3090 btMatrix3x3 Dm(c1.getX(), c2.getX(), c3.getX(),
3091 c1.getY(), c2.getY(), c3.getY(),
3092 c1.getZ(), c2.getZ(), c3.getZ());
3093 t.m_element_measure = Dm.determinant() * unit_simplex_measure;
3094 t.m_Dm_inverse = Dm.inverse();
3095 }
3096}
3097
3099{
3100 for (int i = 0; i < m_tetras.size(); ++i)
3101 {
3103 btVector3 c1 = t.m_n[1]->m_q - t.m_n[0]->m_q;
3104 btVector3 c2 = t.m_n[2]->m_q - t.m_n[0]->m_q;
3105 btVector3 c3 = t.m_n[3]->m_q - t.m_n[0]->m_q;
3106 btMatrix3x3 Ds(c1.getX(), c2.getX(), c3.getX(),
3107 c1.getY(), c2.getY(), c3.getY(),
3108 c1.getZ(), c2.getZ(), c3.getZ());
3109 t.m_F = Ds * t.m_Dm_inverse;
3110
3112 s.m_F = t.m_F;
3113 s.m_J = t.m_F.determinant();
3114 btMatrix3x3 C = t.m_F.transpose()*t.m_F;
3115 s.m_trace = C[0].getX() + C[1].getY() + C[2].getZ();
3116 s.m_cofF = t.m_F.adjoint().transpose();
3117 }
3118}
3119
3121{
3123 for (int i = 0; i < m_tetras.size(); ++i)
3124 {
3126 }
3127}
3128//
3130{
3131 m_bodies[0].activate();
3132 m_bodies[1].activate();
3133}
3134
3135//
3137{
3138 static const btScalar maxdrift = 4;
3139 Joint::Prepare(dt, iterations);
3140 m_rpos[0] = m_bodies[0].xform() * m_refs[0];
3141 m_rpos[1] = m_bodies[1].xform() * m_refs[1];
3142 m_drift = Clamp(m_rpos[0] - m_rpos[1], maxdrift) * m_erp / dt;
3143 m_rpos[0] -= m_bodies[0].xform().getOrigin();
3144 m_rpos[1] -= m_bodies[1].xform().getOrigin();
3145 m_massmatrix = ImpulseMatrix(m_bodies[0].invMass(), m_bodies[0].invWorldInertia(), m_rpos[0],
3146 m_bodies[1].invMass(), m_bodies[1].invWorldInertia(), m_rpos[1]);
3147 if (m_split > 0)
3148 {
3149 m_sdrift = m_massmatrix * (m_drift * m_split);
3150 m_drift *= 1 - m_split;
3151 }
3152 m_drift /= (btScalar)iterations;
3153}
3154
3155//
3157{
3158 const btVector3 va = m_bodies[0].velocity(m_rpos[0]);
3159 const btVector3 vb = m_bodies[1].velocity(m_rpos[1]);
3160 const btVector3 vr = va - vb;
3161 btSoftBody::Impulse impulse;
3162 impulse.m_asVelocity = 1;
3163 impulse.m_velocity = m_massmatrix * (m_drift + vr * m_cfm) * sor;
3164 m_bodies[0].applyImpulse(-impulse, m_rpos[0]);
3165 m_bodies[1].applyImpulse(impulse, m_rpos[1]);
3166}
3167
3168//
3170{
3171 if (m_split > 0)
3172 {
3173 m_bodies[0].applyDImpulse(-m_sdrift, m_rpos[0]);
3174 m_bodies[1].applyDImpulse(m_sdrift, m_rpos[1]);
3175 }
3176}
3177
3178//
3180{
3181 static const btScalar maxdrift = SIMD_PI / 16;
3182 m_icontrol->Prepare(this);
3183 Joint::Prepare(dt, iterations);
3184 m_axis[0] = m_bodies[0].xform().getBasis() * m_refs[0];
3185 m_axis[1] = m_bodies[1].xform().getBasis() * m_refs[1];
3186 m_drift = NormalizeAny(btCross(m_axis[1], m_axis[0]));
3187 m_drift *= btMin(maxdrift, btAcos(Clamp<btScalar>(btDot(m_axis[0], m_axis[1]), -1, +1)));
3188 m_drift *= m_erp / dt;
3189 m_massmatrix = AngularImpulseMatrix(m_bodies[0].invWorldInertia(), m_bodies[1].invWorldInertia());
3190 if (m_split > 0)
3191 {
3192 m_sdrift = m_massmatrix * (m_drift * m_split);
3193 m_drift *= 1 - m_split;
3194 }
3195 m_drift /= (btScalar)iterations;
3196}
3197
3198//
3200{
3201 const btVector3 va = m_bodies[0].angularVelocity();
3202 const btVector3 vb = m_bodies[1].angularVelocity();
3203 const btVector3 vr = va - vb;
3204 const btScalar sp = btDot(vr, m_axis[0]);
3205 const btVector3 vc = vr - m_axis[0] * m_icontrol->Speed(this, sp);
3206 btSoftBody::Impulse impulse;
3207 impulse.m_asVelocity = 1;
3208 impulse.m_velocity = m_massmatrix * (m_drift + vc * m_cfm) * sor;
3209 m_bodies[0].applyAImpulse(-impulse);
3210 m_bodies[1].applyAImpulse(impulse);
3211}
3212
3213//
3215{
3216 if (m_split > 0)
3217 {
3218 m_bodies[0].applyDAImpulse(-m_sdrift);
3219 m_bodies[1].applyDAImpulse(m_sdrift);
3220 }
3221}
3222
3223//
3225{
3226 Joint::Prepare(dt, iterations);
3227 const bool dodrift = (m_life == 0);
3228 m_delete = (++m_life) > m_maxlife;
3229 if (dodrift)
3230 {
3231 m_drift = m_drift * m_erp / dt;
3232 if (m_split > 0)
3233 {
3234 m_sdrift = m_massmatrix * (m_drift * m_split);
3235 m_drift *= 1 - m_split;
3236 }
3237 m_drift /= (btScalar)iterations;
3238 }
3239 else
3240 {
3241 m_drift = m_sdrift = btVector3(0, 0, 0);
3242 }
3243}
3244
3245//
3247{
3248 const btVector3 va = m_bodies[0].velocity(m_rpos[0]);
3249 const btVector3 vb = m_bodies[1].velocity(m_rpos[1]);
3250 const btVector3 vrel = va - vb;
3251 const btScalar rvac = btDot(vrel, m_normal);
3252 btSoftBody::Impulse impulse;
3253 impulse.m_asVelocity = 1;
3254 impulse.m_velocity = m_drift;
3255 if (rvac < 0)
3256 {
3257 const btVector3 iv = m_normal * rvac;
3258 const btVector3 fv = vrel - iv;
3259 impulse.m_velocity += iv + fv * m_friction;
3260 }
3261 impulse.m_velocity = m_massmatrix * impulse.m_velocity * sor;
3262
3263 if (m_bodies[0].m_soft == m_bodies[1].m_soft)
3264 {
3265 if ((impulse.m_velocity.getX() == impulse.m_velocity.getX()) && (impulse.m_velocity.getY() == impulse.m_velocity.getY()) &&
3266 (impulse.m_velocity.getZ() == impulse.m_velocity.getZ()))
3267 {
3268 if (impulse.m_asVelocity)
3269 {
3270 if (impulse.m_velocity.length() < m_bodies[0].m_soft->m_maxSelfCollisionImpulse)
3271 {
3272 }
3273 else
3274 {
3275 m_bodies[0].applyImpulse(-impulse * m_bodies[0].m_soft->m_selfCollisionImpulseFactor, m_rpos[0]);
3276 m_bodies[1].applyImpulse(impulse * m_bodies[0].m_soft->m_selfCollisionImpulseFactor, m_rpos[1]);
3277 }
3278 }
3279 }
3280 }
3281 else
3282 {
3283 m_bodies[0].applyImpulse(-impulse, m_rpos[0]);
3284 m_bodies[1].applyImpulse(impulse, m_rpos[1]);
3285 }
3286}
3287
3288//
3290{
3291 if (m_split > 0)
3292 {
3293 m_bodies[0].applyDImpulse(-m_sdrift, m_rpos[0]);
3294 m_bodies[1].applyDImpulse(m_sdrift, m_rpos[1]);
3295 }
3296}
3297
3298//
3300{
3301 BT_PROFILE("SoftBody applyForces");
3302 // const btScalar dt = m_sst.sdt;
3303 const btScalar kLF = m_cfg.kLF;
3304 const btScalar kDG = m_cfg.kDG;
3305 const btScalar kPR = m_cfg.kPR;
3306 const btScalar kVC = m_cfg.kVC;
3307 const bool as_lift = kLF > 0;
3308 const bool as_drag = kDG > 0;
3309 const bool as_pressure = kPR != 0;
3310 const bool as_volume = kVC > 0;
3311 const bool as_aero = as_lift ||
3312 as_drag;
3313 //const bool as_vaero = as_aero &&
3314 // (m_cfg.aeromodel < btSoftBody::eAeroModel::F_TwoSided);
3315 //const bool as_faero = as_aero &&
3316 // (m_cfg.aeromodel >= btSoftBody::eAeroModel::F_TwoSided);
3317 const bool use_medium = as_aero;
3318 const bool use_volume = as_pressure ||
3319 as_volume;
3320 btScalar volume = 0;
3321 btScalar ivolumetp = 0;
3322 btScalar dvolumetv = 0;
3323 btSoftBody::sMedium medium;
3324 if (use_volume)
3325 {
3326 volume = getVolume();
3327 ivolumetp = 1 / btFabs(volume) * kPR;
3328 dvolumetv = (m_pose.m_volume - volume) * kVC;
3329 }
3330 /* Per vertex forces */
3331 int i, ni;
3332
3333 for (i = 0, ni = m_nodes.size(); i < ni; ++i)
3334 {
3335 btSoftBody::Node& n = m_nodes[i];
3336 if (n.m_im > 0)
3337 {
3338 if (use_medium)
3339 {
3340 /* Aerodynamics */
3342 }
3343 /* Pressure */
3344 if (as_pressure)
3345 {
3346 n.m_f += n.m_n * (n.m_area * ivolumetp);
3347 }
3348 /* Volume */
3349 if (as_volume)
3350 {
3351 n.m_f += n.m_n * (n.m_area * dvolumetv);
3352 }
3353 }
3354 }
3355
3356 /* Per face forces */
3357 for (i = 0, ni = m_faces.size(); i < ni; ++i)
3358 {
3359 // btSoftBody::Face& f=m_faces[i];
3360
3361 /* Aerodynamics */
3363 }
3364}
3365
3366//
3368{
3369 m_cfg.m_maxStress = maxStress;
3370}
3371
3372//
3374{
3375 for (int i = 0; i < m_renderNodes.size(); ++i)
3376 {
3377 Node& n = m_renderNodes[i];
3378 n.m_x.setZero();
3379 for (int j = 0; j < 4; ++j)
3380 {
3381 if (m_renderNodesParents[i].size())
3382 {
3384 }
3385 }
3386 }
3387}
3388
3390{
3391 for (int i = 0; i <= N; ++i)
3392 {
3393 for (int j = 0; i+j <= N; ++j)
3394 {
3396 }
3397 }
3398}
3399
3400//
3402{
3403 BT_PROFILE("PSolve_Anchors");
3404 const btScalar kAHR = psb->m_cfg.kAHR * kst;
3405 const btScalar dt = psb->m_sst.sdt;
3406 for (int i = 0, ni = psb->m_anchors.size(); i < ni; ++i)
3407 {
3408 const Anchor& a = psb->m_anchors[i];
3409 const btTransform& t = a.m_body->getWorldTransform();
3410 Node& n = *a.m_node;
3411 const btVector3 wa = t * a.m_local;
3412 const btVector3 va = a.m_body->getVelocityInLocalPoint(a.m_c1) * dt;
3413 const btVector3 vb = n.m_x - n.m_q;
3414 const btVector3 vr = (va - vb) + (wa - n.m_x) * kAHR;
3415 const btVector3 impulse = a.m_c0 * vr * a.m_influence;
3416 n.m_x += impulse * a.m_c2;
3417 a.m_body->applyImpulse(-impulse, a.m_c1);
3418 }
3419}
3420
3421//
3423{
3424 BT_PROFILE("PSolve_RContacts");
3425 const btScalar dt = psb->m_sst.sdt;
3426 const btScalar mrg = psb->getCollisionShape()->getMargin();
3427 btMultiBodyJacobianData jacobianData;
3428 for (int i = 0, ni = psb->m_rcontacts.size(); i < ni; ++i)
3429 {
3430 const RContact& c = psb->m_rcontacts[i];
3431 const sCti& cti = c.m_cti;
3432 if (cti.m_colObj->hasContactResponse())
3433 {
3434 btVector3 va(0, 0, 0);
3435 btRigidBody* rigidCol = 0;
3436 btMultiBodyLinkCollider* multibodyLinkCol = 0;
3437 btScalar* deltaV;
3438
3440 {
3441 rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
3442 va = rigidCol ? rigidCol->getVelocityInLocalPoint(c.m_c1) * dt : btVector3(0, 0, 0);
3443 }
3445 {
3447 if (multibodyLinkCol)
3448 {
3449 const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
3450 jacobianData.m_jacobians.resize(ndof);
3451 jacobianData.m_deltaVelocitiesUnitImpulse.resize(ndof);
3452 btScalar* jac = &jacobianData.m_jacobians[0];
3453
3454 multibodyLinkCol->m_multiBody->fillContactJacobianMultiDof(multibodyLinkCol->m_link, c.m_node->m_x, cti.m_normal, jac, jacobianData.scratch_r, jacobianData.scratch_v, jacobianData.scratch_m);
3455 deltaV = &jacobianData.m_deltaVelocitiesUnitImpulse[0];
3456 multibodyLinkCol->m_multiBody->calcAccelerationDeltasMultiDof(&jacobianData.m_jacobians[0], deltaV, jacobianData.scratch_r, jacobianData.scratch_v);
3457
3458 btScalar vel = 0.0;
3459 for (int j = 0; j < ndof; ++j)
3460 {
3461 vel += multibodyLinkCol->m_multiBody->getVelocityVector()[j] * jac[j];
3462 }
3463 va = cti.m_normal * vel * dt;
3464 }
3465 }
3466
3467 const btVector3 vb = c.m_node->m_x - c.m_node->m_q;
3468 const btVector3 vr = vb - va;
3469 const btScalar dn = btDot(vr, cti.m_normal);
3470 if (dn <= SIMD_EPSILON)
3471 {
3472 const btScalar dp = btMin((btDot(c.m_node->m_x, cti.m_normal) + cti.m_offset), mrg);
3473 const btVector3 fv = vr - (cti.m_normal * dn);
3474 // c0 is the impulse matrix, c3 is 1 - the friction coefficient or 0, c4 is the contact hardness coefficient
3475 const btVector3 impulse = c.m_c0 * ((vr - (fv * c.m_c3) + (cti.m_normal * (dp * c.m_c4))) * kst);
3476 c.m_node->m_x -= impulse * c.m_c2;
3477
3479 {
3480 if (rigidCol)
3481 rigidCol->applyImpulse(impulse, c.m_c1);
3482 }
3484 {
3485 if (multibodyLinkCol)
3486 {
3487 double multiplier = 0.5;
3488 multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV, -impulse.length() * multiplier);
3489 }
3490 }
3491 }
3492 }
3493 }
3494}
3495
3496//
3498{
3499 BT_PROFILE("PSolve_SContacts");
3500
3501 for (int i = 0, ni = psb->m_scontacts.size(); i < ni; ++i)
3502 {
3503 const SContact& c = psb->m_scontacts[i];
3504 const btVector3& nr = c.m_normal;
3505 Node& n = *c.m_node;
3506 Face& f = *c.m_face;
3507 const btVector3 p = BaryEval(f.m_n[0]->m_x,
3508 f.m_n[1]->m_x,
3509 f.m_n[2]->m_x,
3510 c.m_weights);
3511 const btVector3 q = BaryEval(f.m_n[0]->m_q,
3512 f.m_n[1]->m_q,
3513 f.m_n[2]->m_q,
3514 c.m_weights);
3515 const btVector3 vr = (n.m_x - n.m_q) - (p - q);
3516 btVector3 corr(0, 0, 0);
3517 btScalar dot = btDot(vr, nr);
3518 if (dot < 0)
3519 {
3520 const btScalar j = c.m_margin - (btDot(nr, n.m_x) - btDot(nr, p));
3521 corr += c.m_normal * j;
3522 }
3523 corr -= ProjectOnPlane(vr, nr) * c.m_friction;
3524 n.m_x += corr * c.m_cfm[0];
3525 f.m_n[0]->m_x -= corr * (c.m_cfm[1] * c.m_weights.x());
3526 f.m_n[1]->m_x -= corr * (c.m_cfm[1] * c.m_weights.y());
3527 f.m_n[2]->m_x -= corr * (c.m_cfm[1] * c.m_weights.z());
3528 }
3529}
3530
3531//
3533{
3534 BT_PROFILE("PSolve_Links");
3535 for (int i = 0, ni = psb->m_links.size(); i < ni; ++i)
3536 {
3537 Link& l = psb->m_links[i];
3538 if (l.m_c0 > 0)
3539 {
3540 Node& a = *l.m_n[0];
3541 Node& b = *l.m_n[1];
3542 const btVector3 del = b.m_x - a.m_x;
3543 const btScalar len = del.length2();
3544 if (l.m_c1 + len > SIMD_EPSILON)
3545 {
3546 const btScalar k = ((l.m_c1 - len) / (l.m_c0 * (l.m_c1 + len))) * kst;
3547 a.m_x -= del * (k * a.m_im);
3548 b.m_x += del * (k * b.m_im);
3549 }
3550 }
3551 }
3552}
3553
3554//
3556{
3557 BT_PROFILE("VSolve_Links");
3558 for (int i = 0, ni = psb->m_links.size(); i < ni; ++i)
3559 {
3560 Link& l = psb->m_links[i];
3561 Node** n = l.m_n;
3562 const btScalar j = -btDot(l.m_c3, n[0]->m_v - n[1]->m_v) * l.m_c2 * kst;
3563 n[0]->m_v += l.m_c3 * (j * n[0]->m_im);
3564 n[1]->m_v -= l.m_c3 * (j * n[1]->m_im);
3565 }
3566}
3567
3568//
3570{
3571 switch (solver)
3572 {
3573 case ePSolver::Anchors:
3575 case ePSolver::Linear:
3576 return (&btSoftBody::PSolve_Links);
3581 default:
3582 {
3583 }
3584 }
3585 return (0);
3586}
3587
3588//
3590{
3591 switch (solver)
3592 {
3593 case eVSolver::Linear:
3594 return (&btSoftBody::VSolve_Links);
3595 default:
3596 {
3597 }
3598 }
3599 return (0);
3600}
3601
3603{
3605}
3606
3608{
3609 return m_useSelfCollision;
3610}
3611
3612//
3614{
3616 {
3617 case fCollision::SDF_RS:
3618 {
3621 btTransform wtr = pcoWrap->getWorldTransform();
3622
3623 const btTransform ctr = pcoWrap->getWorldTransform();
3624 const btScalar timemargin = (wtr.getOrigin() - ctr.getOrigin()).length();
3625 const btScalar basemargin = getCollisionShape()->getMargin();
3626 btVector3 mins;
3627 btVector3 maxs;
3629 volume;
3630 pcoWrap->getCollisionShape()->getAabb(pcoWrap->getWorldTransform(),
3631 mins,
3632 maxs);
3633 volume = btDbvtVolume::FromMM(mins, maxs);
3634 volume.Expand(btVector3(basemargin, basemargin, basemargin));
3635 docollide.psb = this;
3636 docollide.m_colObj1Wrap = pcoWrap;
3637 docollide.m_rigidBody = prb1;
3638
3639 docollide.dynmargin = basemargin + timemargin;
3640 docollide.stamargin = basemargin;
3641 m_ndbvt.collideTV(m_ndbvt.m_root, volume, docollide);
3642 }
3643 break;
3644 case fCollision::CL_RS:
3645 {
3647 collider.ProcessColObj(this, pcoWrap);
3648 }
3649 break;
3650 case fCollision::SDF_RD:
3651 {
3652
3654 if (pcoWrap->getCollisionObject()->isActive() || this->isActive())
3655 {
3656 const btTransform wtr = pcoWrap->getWorldTransform();
3657// const btTransform ctr = pcoWrap->getWorldTransform();
3658// const btScalar timemargin = (wtr.getOrigin() - ctr.getOrigin()).length();
3659 const btScalar timemargin = 0;
3660 const btScalar basemargin = getCollisionShape()->getMargin();
3661 btVector3 mins;
3662 btVector3 maxs;
3664 volume;
3665 pcoWrap->getCollisionShape()->getAabb(wtr,
3666 mins,
3667 maxs);
3668 volume = btDbvtVolume::FromMM(mins, maxs);
3669 volume.Expand(btVector3(basemargin, basemargin, basemargin));
3670 btSoftColliders::CollideSDF_RD docollideNode;
3671 docollideNode.psb = this;
3672 docollideNode.m_colObj1Wrap = pcoWrap;
3673 docollideNode.m_rigidBody = prb1;
3674 docollideNode.dynmargin = basemargin + timemargin;
3675 docollideNode.stamargin = basemargin;
3676 m_ndbvt.collideTV(m_ndbvt.m_root, volume, docollideNode);
3677
3678 if (this->m_useFaceContact)
3679 {
3680 btSoftColliders::CollideSDF_RDF docollideFace;
3681 docollideFace.psb = this;
3682 docollideFace.m_colObj1Wrap = pcoWrap;
3683 docollideFace.m_rigidBody = prb1;
3684 docollideFace.dynmargin = basemargin + timemargin;
3685 docollideFace.stamargin = basemargin;
3686 m_fdbvt.collideTV(m_fdbvt.m_root, volume, docollideFace);
3687 }
3688 }
3689 }
3690 break;
3691 }
3692}
3693
3694static inline btDbvntNode* copyToDbvnt(const btDbvtNode* n)
3695{
3696 if (n == 0)
3697 return 0;
3698 btDbvntNode* root = new btDbvntNode(n);
3699 if (n->isinternal())
3700 {
3701 btDbvntNode* c0 = copyToDbvnt(n->childs[0]);
3702 root->childs[0] = c0;
3703 btDbvntNode* c1 = copyToDbvnt(n->childs[1]);
3704 root->childs[1] = c1;
3705 }
3706 return root;
3707}
3708
3709static inline void calculateNormalCone(btDbvntNode* root)
3710{
3711 if (!root)
3712 return;
3713 if (root->isleaf())
3714 {
3715 const btSoftBody::Face* face = (btSoftBody::Face*)root->data;
3716 root->normal = face->m_normal;
3717 root->angle = 0;
3718 }
3719 else
3720 {
3721 btVector3 n0(0,0,0), n1(0,0,0);
3722 btScalar a0 = 0, a1 = 0;
3723 if (root->childs[0])
3724 {
3725 calculateNormalCone(root->childs[0]);
3726 n0 = root->childs[0]->normal;
3727 a0 = root->childs[0]->angle;
3728 }
3729 if (root->childs[1])
3730 {
3731 calculateNormalCone(root->childs[1]);
3732 n1 = root->childs[1]->normal;
3733 a1 = root->childs[1]->angle;
3734 }
3735 root->normal = (n0+n1).safeNormalize();
3736 root->angle = btMax(a0,a1) + btAngle(n0, n1)*0.5;
3737 }
3738}
3739//
3741{
3742 BT_PROFILE("Deformable Collision");
3743 const int cf = m_cfg.collisions & psb->m_cfg.collisions;
3744 switch (cf & fCollision::SVSmask)
3745 {
3746 case fCollision::CL_SS:
3747 {
3748 //support self-collision if CL_SELF flag set
3749 if (this != psb || psb->m_cfg.collisions & fCollision::CL_SELF)
3750 {
3752 docollide.ProcessSoftSoft(this, psb);
3753 }
3754 }
3755 break;
3756 case fCollision::VF_SS:
3757 {
3758 //only self-collision for Cluster, not Vertex-Face yet
3759 if (this != psb)
3760 {
3762 /* common */
3763 docollide.mrg = getCollisionShape()->getMargin() +
3764 psb->getCollisionShape()->getMargin();
3765 /* psb0 nodes vs psb1 faces */
3766 docollide.psb[0] = this;
3767 docollide.psb[1] = psb;
3768 docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root,
3769 docollide.psb[1]->m_fdbvt.m_root,
3770 docollide);
3771 /* psb1 nodes vs psb0 faces */
3772 docollide.psb[0] = psb;
3773 docollide.psb[1] = this;
3774 docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root,
3775 docollide.psb[1]->m_fdbvt.m_root,
3776 docollide);
3777 }
3778 }
3779 break;
3780 case fCollision::VF_DD:
3781 {
3782 if (psb->isActive() || this->isActive())
3783 {
3784 if (this != psb)
3785 {
3787 /* common */
3788 docollide.mrg = getCollisionShape()->getMargin() +
3789 psb->getCollisionShape()->getMargin();
3790 /* psb0 nodes vs psb1 faces */
3791 if (psb->m_tetras.size() > 0)
3792 docollide.useFaceNormal = true;
3793 else
3794 docollide.useFaceNormal = false;
3795 docollide.psb[0] = this;
3796 docollide.psb[1] = psb;
3797 docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root,
3798 docollide.psb[1]->m_fdbvt.m_root,
3799 docollide);
3800 /* psb1 nodes vs psb0 faces */
3801 if (this->m_tetras.size() > 0)
3802 docollide.useFaceNormal = true;
3803 else
3804 docollide.useFaceNormal = false;
3805 docollide.psb[0] = psb;
3806 docollide.psb[1] = this;
3807 docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root,
3808 docollide.psb[1]->m_fdbvt.m_root,
3809 docollide);
3810 }
3811 else
3812 {
3813 if (psb->useSelfCollision())
3814 {
3816 docollide.mrg = getCollisionShape()->getMargin() +
3817 psb->getCollisionShape()->getMargin();
3818 docollide.psb[0] = this;
3819 docollide.psb[1] = psb;
3820 if (this->m_tetras.size() > 0)
3821 docollide.useFaceNormal = true;
3822 else
3823 docollide.useFaceNormal = false;
3824 /* psb0 faces vs psb0 faces */
3825 btDbvntNode* root = copyToDbvnt(this->m_fdbvt.m_root);
3826 calculateNormalCone(root);
3827 this->m_fdbvt.selfCollideT(root,docollide);
3828 delete root;
3829 }
3830 }
3831 }
3832 }
3833 break;
3834 default:
3835 {
3836 }
3837 }
3838}
3839
3841{
3842 m_windVelocity = velocity;
3843}
3844
3846{
3847 return m_windVelocity;
3848}
3849
3851{
3852 int sz = sizeof(btSoftBodyData);
3853 return sz;
3854}
3855
3857const char* btSoftBody::serialize(void* dataBuffer, class btSerializer* serializer) const
3858{
3859 btSoftBodyData* sbd = (btSoftBodyData*)dataBuffer;
3860
3861 btCollisionObject::serialize(&sbd->m_collisionObjectData, serializer);
3862
3863 btHashMap<btHashPtr, int> m_nodeIndexMap;
3864
3865 sbd->m_numMaterials = m_materials.size();
3866 sbd->m_materials = sbd->m_numMaterials ? (SoftBodyMaterialData**)serializer->getUniquePointer((void*)&m_materials) : 0;
3867
3868 if (sbd->m_materials)
3869 {
3870 int sz = sizeof(SoftBodyMaterialData*);
3871 int numElem = sbd->m_numMaterials;
3872 btChunk* chunk = serializer->allocate(sz, numElem);
3873 //SoftBodyMaterialData** memPtr = chunk->m_oldPtr;
3875 for (int i = 0; i < numElem; i++, memPtr++)
3876 {
3878 *memPtr = mat ? (SoftBodyMaterialData*)serializer->getUniquePointer((void*)mat) : 0;
3879 if (!serializer->findPointer(mat))
3880 {
3881 //serialize it here
3882 btChunk* chunk = serializer->allocate(sizeof(SoftBodyMaterialData), 1);
3884 memPtr->m_flags = mat->m_flags;
3885 memPtr->m_angularStiffness = mat->m_kAST;
3886 memPtr->m_linearStiffness = mat->m_kLST;
3887 memPtr->m_volumeStiffness = mat->m_kVST;
3888 serializer->finalizeChunk(chunk, "SoftBodyMaterialData", BT_SBMATERIAL_CODE, mat);
3889 }
3890 }
3891 serializer->finalizeChunk(chunk, "SoftBodyMaterialData", BT_ARRAY_CODE, (void*)&m_materials);
3892 }
3893
3894 sbd->m_numNodes = m_nodes.size();
3895 sbd->m_nodes = sbd->m_numNodes ? (SoftBodyNodeData*)serializer->getUniquePointer((void*)&m_nodes) : 0;
3896 if (sbd->m_nodes)
3897 {
3898 int sz = sizeof(SoftBodyNodeData);
3899 int numElem = sbd->m_numNodes;
3900 btChunk* chunk = serializer->allocate(sz, numElem);
3901 SoftBodyNodeData* memPtr = (SoftBodyNodeData*)chunk->m_oldPtr;
3902 for (int i = 0; i < numElem; i++, memPtr++)
3903 {
3904 m_nodes[i].m_f.serializeFloat(memPtr->m_accumulatedForce);
3905 memPtr->m_area = m_nodes[i].m_area;
3906 memPtr->m_attach = m_nodes[i].m_battach;
3907 memPtr->m_inverseMass = m_nodes[i].m_im;
3908 memPtr->m_material = m_nodes[i].m_material ? (SoftBodyMaterialData*)serializer->getUniquePointer((void*)m_nodes[i].m_material) : 0;
3909 m_nodes[i].m_n.serializeFloat(memPtr->m_normal);
3910 m_nodes[i].m_x.serializeFloat(memPtr->m_position);
3911 m_nodes[i].m_q.serializeFloat(memPtr->m_previousPosition);
3912 m_nodes[i].m_v.serializeFloat(memPtr->m_velocity);
3913 m_nodeIndexMap.insert(&m_nodes[i], i);
3914 }
3915 serializer->finalizeChunk(chunk, "SoftBodyNodeData", BT_SBNODE_CODE, (void*)&m_nodes);
3916 }
3917
3918 sbd->m_numLinks = m_links.size();
3919 sbd->m_links = sbd->m_numLinks ? (SoftBodyLinkData*)serializer->getUniquePointer((void*)&m_links[0]) : 0;
3920 if (sbd->m_links)
3921 {
3922 int sz = sizeof(SoftBodyLinkData);
3923 int numElem = sbd->m_numLinks;
3924 btChunk* chunk = serializer->allocate(sz, numElem);
3925 SoftBodyLinkData* memPtr = (SoftBodyLinkData*)chunk->m_oldPtr;
3926 for (int i = 0; i < numElem; i++, memPtr++)
3927 {
3928 memPtr->m_bbending = m_links[i].m_bbending;
3929 memPtr->m_material = m_links[i].m_material ? (SoftBodyMaterialData*)serializer->getUniquePointer((void*)m_links[i].m_material) : 0;
3930 memPtr->m_nodeIndices[0] = m_links[i].m_n[0] ? m_links[i].m_n[0] - &m_nodes[0] : -1;
3931 memPtr->m_nodeIndices[1] = m_links[i].m_n[1] ? m_links[i].m_n[1] - &m_nodes[0] : -1;
3932 btAssert(memPtr->m_nodeIndices[0] < m_nodes.size());
3933 btAssert(memPtr->m_nodeIndices[1] < m_nodes.size());
3934 memPtr->m_restLength = m_links[i].m_rl;
3935 }
3936 serializer->finalizeChunk(chunk, "SoftBodyLinkData", BT_ARRAY_CODE, (void*)&m_links[0]);
3937 }
3938
3939 sbd->m_numFaces = m_faces.size();
3940 sbd->m_faces = sbd->m_numFaces ? (SoftBodyFaceData*)serializer->getUniquePointer((void*)&m_faces[0]) : 0;
3941 if (sbd->m_faces)
3942 {
3943 int sz = sizeof(SoftBodyFaceData);
3944 int numElem = sbd->m_numFaces;
3945 btChunk* chunk = serializer->allocate(sz, numElem);
3946 SoftBodyFaceData* memPtr = (SoftBodyFaceData*)chunk->m_oldPtr;
3947 for (int i = 0; i < numElem; i++, memPtr++)
3948 {
3949 memPtr->m_material = m_faces[i].m_material ? (SoftBodyMaterialData*)serializer->getUniquePointer((void*)m_faces[i].m_material) : 0;
3950 m_faces[i].m_normal.serializeFloat(memPtr->m_normal);
3951 for (int j = 0; j < 3; j++)
3952 {
3953 memPtr->m_nodeIndices[j] = m_faces[i].m_n[j] ? m_faces[i].m_n[j] - &m_nodes[0] : -1;
3954 }
3955 memPtr->m_restArea = m_faces[i].m_ra;
3956 }
3957 serializer->finalizeChunk(chunk, "SoftBodyFaceData", BT_ARRAY_CODE, (void*)&m_faces[0]);
3958 }
3959
3960 sbd->m_numTetrahedra = m_tetras.size();
3961 sbd->m_tetrahedra = sbd->m_numTetrahedra ? (SoftBodyTetraData*)serializer->getUniquePointer((void*)&m_tetras[0]) : 0;
3962 if (sbd->m_tetrahedra)
3963 {
3964 int sz = sizeof(SoftBodyTetraData);
3965 int numElem = sbd->m_numTetrahedra;
3966 btChunk* chunk = serializer->allocate(sz, numElem);
3967 SoftBodyTetraData* memPtr = (SoftBodyTetraData*)chunk->m_oldPtr;
3968 for (int i = 0; i < numElem; i++, memPtr++)
3969 {
3970 for (int j = 0; j < 4; j++)
3971 {
3972 m_tetras[i].m_c0[j].serializeFloat(memPtr->m_c0[j]);
3973 memPtr->m_nodeIndices[j] = m_tetras[i].m_n[j] ? m_tetras[i].m_n[j] - &m_nodes[0] : -1;
3974 }
3975 memPtr->m_c1 = m_tetras[i].m_c1;
3976 memPtr->m_c2 = m_tetras[i].m_c2;
3977 memPtr->m_material = m_tetras[i].m_material ? (SoftBodyMaterialData*)serializer->getUniquePointer((void*)m_tetras[i].m_material) : 0;
3978 memPtr->m_restVolume = m_tetras[i].m_rv;
3979 }
3980 serializer->finalizeChunk(chunk, "SoftBodyTetraData", BT_ARRAY_CODE, (void*)&m_tetras[0]);
3981 }
3982
3983 sbd->m_numAnchors = m_anchors.size();
3984 sbd->m_anchors = sbd->m_numAnchors ? (SoftRigidAnchorData*)serializer->getUniquePointer((void*)&m_anchors[0]) : 0;
3985 if (sbd->m_anchors)
3986 {
3987 int sz = sizeof(SoftRigidAnchorData);
3988 int numElem = sbd->m_numAnchors;
3989 btChunk* chunk = serializer->allocate(sz, numElem);
3991 for (int i = 0; i < numElem; i++, memPtr++)
3992 {
3993 m_anchors[i].m_c0.serializeFloat(memPtr->m_c0);
3994 m_anchors[i].m_c1.serializeFloat(memPtr->m_c1);
3995 memPtr->m_c2 = m_anchors[i].m_c2;
3996 m_anchors[i].m_local.serializeFloat(memPtr->m_localFrame);
3997 memPtr->m_nodeIndex = m_anchors[i].m_node ? m_anchors[i].m_node - &m_nodes[0] : -1;
3998
3999 memPtr->m_rigidBody = m_anchors[i].m_body ? (btRigidBodyData*)serializer->getUniquePointer((void*)m_anchors[i].m_body) : 0;
4000 btAssert(memPtr->m_nodeIndex < m_nodes.size());
4001 }
4002 serializer->finalizeChunk(chunk, "SoftRigidAnchorData", BT_ARRAY_CODE, (void*)&m_anchors[0]);
4003 }
4004
4005 sbd->m_config.m_dynamicFriction = m_cfg.kDF;
4006 sbd->m_config.m_baumgarte = m_cfg.kVCF;
4007 sbd->m_config.m_pressure = m_cfg.kPR;
4008 sbd->m_config.m_aeroModel = this->m_cfg.aeromodel;
4009 sbd->m_config.m_lift = m_cfg.kLF;
4010 sbd->m_config.m_drag = m_cfg.kDG;
4011 sbd->m_config.m_positionIterations = m_cfg.piterations;
4012 sbd->m_config.m_driftIterations = m_cfg.diterations;
4013 sbd->m_config.m_clusterIterations = m_cfg.citerations;
4014 sbd->m_config.m_velocityIterations = m_cfg.viterations;
4015 sbd->m_config.m_maxVolume = m_cfg.maxvolume;
4016 sbd->m_config.m_damping = m_cfg.kDP;
4017 sbd->m_config.m_poseMatch = m_cfg.kMT;
4018 sbd->m_config.m_collisionFlags = m_cfg.collisions;
4019 sbd->m_config.m_volume = m_cfg.kVC;
4020 sbd->m_config.m_rigidContactHardness = m_cfg.kCHR;
4021 sbd->m_config.m_kineticContactHardness = m_cfg.kKHR;
4022 sbd->m_config.m_softContactHardness = m_cfg.kSHR;
4023 sbd->m_config.m_anchorHardness = m_cfg.kAHR;
4024 sbd->m_config.m_timeScale = m_cfg.timescale;
4025 sbd->m_config.m_maxVolume = m_cfg.maxvolume;
4026 sbd->m_config.m_softRigidClusterHardness = m_cfg.kSRHR_CL;
4027 sbd->m_config.m_softKineticClusterHardness = m_cfg.kSKHR_CL;
4028 sbd->m_config.m_softSoftClusterHardness = m_cfg.kSSHR_CL;
4029 sbd->m_config.m_softRigidClusterImpulseSplit = m_cfg.kSR_SPLT_CL;
4030 sbd->m_config.m_softKineticClusterImpulseSplit = m_cfg.kSK_SPLT_CL;
4031 sbd->m_config.m_softSoftClusterImpulseSplit = m_cfg.kSS_SPLT_CL;
4032
4033 //pose for shape matching
4034 {
4035 sbd->m_pose = (SoftBodyPoseData*)serializer->getUniquePointer((void*)&m_pose);
4036
4037 int sz = sizeof(SoftBodyPoseData);
4038 btChunk* chunk = serializer->allocate(sz, 1);
4039 SoftBodyPoseData* memPtr = (SoftBodyPoseData*)chunk->m_oldPtr;
4040
4042 memPtr->m_bframe = m_pose.m_bframe;
4043 memPtr->m_bvolume = m_pose.m_bvolume;
4045
4046 memPtr->m_numPositions = m_pose.m_pos.size();
4047 memPtr->m_positions = memPtr->m_numPositions ? (btVector3FloatData*)serializer->getUniquePointer((void*)&m_pose.m_pos[0]) : 0;
4048 if (memPtr->m_numPositions)
4049 {
4050 int numElem = memPtr->m_numPositions;
4051 int sz = sizeof(btVector3Data);
4052 btChunk* chunk = serializer->allocate(sz, numElem);
4054 for (int i = 0; i < numElem; i++, memPtr++)
4055 {
4056 m_pose.m_pos[i].serializeFloat(*memPtr);
4057 }
4058 serializer->finalizeChunk(chunk, "btVector3FloatData", BT_ARRAY_CODE, (void*)&m_pose.m_pos[0]);
4059 }
4060 memPtr->m_restVolume = m_pose.m_volume;
4063
4064 memPtr->m_numWeigts = m_pose.m_wgh.size();
4065 memPtr->m_weights = memPtr->m_numWeigts ? (float*)serializer->getUniquePointer((void*)&m_pose.m_wgh[0]) : 0;
4066 if (memPtr->m_numWeigts)
4067 {
4068 int numElem = memPtr->m_numWeigts;
4069 int sz = sizeof(float);
4070 btChunk* chunk = serializer->allocate(sz, numElem);
4071 float* memPtr = (float*)chunk->m_oldPtr;
4072 for (int i = 0; i < numElem; i++, memPtr++)
4073 {
4074 *memPtr = m_pose.m_wgh[i];
4075 }
4076 serializer->finalizeChunk(chunk, "float", BT_ARRAY_CODE, (void*)&m_pose.m_wgh[0]);
4077 }
4078
4079 serializer->finalizeChunk(chunk, "SoftBodyPoseData", BT_ARRAY_CODE, (void*)&m_pose);
4080 }
4081
4082 //clusters for convex-cluster collision detection
4083
4084 sbd->m_numClusters = m_clusters.size();
4085 sbd->m_clusters = sbd->m_numClusters ? (SoftBodyClusterData*)serializer->getUniquePointer((void*)m_clusters[0]) : 0;
4086 if (sbd->m_numClusters)
4087 {
4088 int numElem = sbd->m_numClusters;
4089 int sz = sizeof(SoftBodyClusterData);
4090 btChunk* chunk = serializer->allocate(sz, numElem);
4092 for (int i = 0; i < numElem; i++, memPtr++)
4093 {
4094 memPtr->m_adamping = m_clusters[i]->m_adamping;
4095 m_clusters[i]->m_av.serializeFloat(memPtr->m_av);
4096 memPtr->m_clusterIndex = m_clusters[i]->m_clusterIndex;
4097 memPtr->m_collide = m_clusters[i]->m_collide;
4098 m_clusters[i]->m_com.serializeFloat(memPtr->m_com);
4099 memPtr->m_containsAnchor = m_clusters[i]->m_containsAnchor;
4100 m_clusters[i]->m_dimpulses[0].serializeFloat(memPtr->m_dimpulses[0]);
4101 m_clusters[i]->m_dimpulses[1].serializeFloat(memPtr->m_dimpulses[1]);
4102 m_clusters[i]->m_framexform.serializeFloat(memPtr->m_framexform);
4103 memPtr->m_idmass = m_clusters[i]->m_idmass;
4104 memPtr->m_imass = m_clusters[i]->m_imass;
4105 m_clusters[i]->m_invwi.serializeFloat(memPtr->m_invwi);
4106 memPtr->m_ldamping = m_clusters[i]->m_ldamping;
4107 m_clusters[i]->m_locii.serializeFloat(memPtr->m_locii);
4108 m_clusters[i]->m_lv.serializeFloat(memPtr->m_lv);
4109 memPtr->m_matching = m_clusters[i]->m_matching;
4110 memPtr->m_maxSelfCollisionImpulse = m_clusters[i]->m_maxSelfCollisionImpulse;
4111 memPtr->m_ndamping = m_clusters[i]->m_ndamping;
4112 memPtr->m_ldamping = m_clusters[i]->m_ldamping;
4113 memPtr->m_adamping = m_clusters[i]->m_adamping;
4114 memPtr->m_selfCollisionImpulseFactor = m_clusters[i]->m_selfCollisionImpulseFactor;
4115
4116 memPtr->m_numFrameRefs = m_clusters[i]->m_framerefs.size();
4117 memPtr->m_numMasses = m_clusters[i]->m_masses.size();
4118 memPtr->m_numNodes = m_clusters[i]->m_nodes.size();
4119
4120 memPtr->m_nvimpulses = m_clusters[i]->m_nvimpulses;
4121 m_clusters[i]->m_vimpulses[0].serializeFloat(memPtr->m_vimpulses[0]);
4122 m_clusters[i]->m_vimpulses[1].serializeFloat(memPtr->m_vimpulses[1]);
4123 memPtr->m_ndimpulses = m_clusters[i]->m_ndimpulses;
4124
4125 memPtr->m_framerefs = memPtr->m_numFrameRefs ? (btVector3FloatData*)serializer->getUniquePointer((void*)&m_clusters[i]->m_framerefs[0]) : 0;
4126 if (memPtr->m_framerefs)
4127 {
4128 int numElem = memPtr->m_numFrameRefs;
4129 int sz = sizeof(btVector3FloatData);
4130 btChunk* chunk = serializer->allocate(sz, numElem);
4132 for (int j = 0; j < numElem; j++, memPtr++)
4133 {
4134 m_clusters[i]->m_framerefs[j].serializeFloat(*memPtr);
4135 }
4136 serializer->finalizeChunk(chunk, "btVector3FloatData", BT_ARRAY_CODE, (void*)&m_clusters[i]->m_framerefs[0]);
4137 }
4138
4139 memPtr->m_masses = memPtr->m_numMasses ? (float*)serializer->getUniquePointer((void*)&m_clusters[i]->m_masses[0]) : 0;
4140 if (memPtr->m_masses)
4141 {
4142 int numElem = memPtr->m_numMasses;
4143 int sz = sizeof(float);
4144 btChunk* chunk = serializer->allocate(sz, numElem);
4145 float* memPtr = (float*)chunk->m_oldPtr;
4146 for (int j = 0; j < numElem; j++, memPtr++)
4147 {
4148 *memPtr = m_clusters[i]->m_masses[j];
4149 }
4150 serializer->finalizeChunk(chunk, "float", BT_ARRAY_CODE, (void*)&m_clusters[i]->m_masses[0]);
4151 }
4152
4153 memPtr->m_nodeIndices = memPtr->m_numNodes ? (int*)serializer->getUniquePointer((void*)&m_clusters[i]->m_nodes) : 0;
4154 if (memPtr->m_nodeIndices)
4155 {
4156 int numElem = memPtr->m_numMasses;
4157 int sz = sizeof(int);
4158 btChunk* chunk = serializer->allocate(sz, numElem);
4159 int* memPtr = (int*)chunk->m_oldPtr;
4160 for (int j = 0; j < numElem; j++, memPtr++)
4161 {
4162 int* indexPtr = m_nodeIndexMap.find(m_clusters[i]->m_nodes[j]);
4163 btAssert(indexPtr);
4164 *memPtr = *indexPtr;
4165 }
4166 serializer->finalizeChunk(chunk, "int", BT_ARRAY_CODE, (void*)&m_clusters[i]->m_nodes);
4167 }
4168 }
4169 serializer->finalizeChunk(chunk, "SoftBodyClusterData", BT_ARRAY_CODE, (void*)m_clusters[0]);
4170 }
4171
4172 sbd->m_numJoints = m_joints.size();
4173 sbd->m_joints = m_joints.size() ? (btSoftBodyJointData*)serializer->getUniquePointer((void*)&m_joints[0]) : 0;
4174
4175 if (sbd->m_joints)
4176 {
4177 int sz = sizeof(btSoftBodyJointData);
4178 int numElem = m_joints.size();
4179 btChunk* chunk = serializer->allocate(sz, numElem);
4181
4182 for (int i = 0; i < numElem; i++, memPtr++)
4183 {
4184 memPtr->m_jointType = (int)m_joints[i]->Type();
4185 m_joints[i]->m_refs[0].serializeFloat(memPtr->m_refs[0]);
4186 m_joints[i]->m_refs[1].serializeFloat(memPtr->m_refs[1]);
4187 memPtr->m_cfm = m_joints[i]->m_cfm;
4188 memPtr->m_erp = float(m_joints[i]->m_erp);
4189 memPtr->m_split = float(m_joints[i]->m_split);
4190 memPtr->m_delete = m_joints[i]->m_delete;
4191
4192 for (int j = 0; j < 4; j++)
4193 {
4194 memPtr->m_relPosition[0].m_floats[j] = 0.f;
4195 memPtr->m_relPosition[1].m_floats[j] = 0.f;
4196 }
4197 memPtr->m_bodyA = 0;
4198 memPtr->m_bodyB = 0;
4199 if (m_joints[i]->m_bodies[0].m_soft)
4200 {
4202 memPtr->m_bodyA = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[0].m_soft);
4203 }
4204 if (m_joints[i]->m_bodies[0].m_collisionObject)
4205 {
4207 memPtr->m_bodyA = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[0].m_collisionObject);
4208 }
4209 if (m_joints[i]->m_bodies[0].m_rigid)
4210 {
4212 memPtr->m_bodyA = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[0].m_rigid);
4213 }
4214
4215 if (m_joints[i]->m_bodies[1].m_soft)
4216 {
4218 memPtr->m_bodyB = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[1].m_soft);
4219 }
4220 if (m_joints[i]->m_bodies[1].m_collisionObject)
4221 {
4223 memPtr->m_bodyB = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[1].m_collisionObject);
4224 }
4225 if (m_joints[i]->m_bodies[1].m_rigid)
4226 {
4228 memPtr->m_bodyB = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[1].m_rigid);
4229 }
4230 }
4231 serializer->finalizeChunk(chunk, "btSoftBodyJointData", BT_ARRAY_CODE, (void*)&m_joints[0]);
4232 }
4233
4234 return btSoftBodyDataName;
4235}
4236
4238{
4240 return;
4241
4243 {
4244 m_deactivationTime += timeStep;
4245 }
4246 else
4247 {
4250 }
4251}
4252
4253
4255{
4256 for (int i = 0; i < m_nodes.size(); ++i)
4257 {
4258 m_nodes[i].m_v.setZero();
4259 }
4260}
4261
4263{
4265 return false;
4266
4267 //disable deactivation
4269 return false;
4270
4272 return true;
4273
4275 {
4276 return true;
4277 }
4278 return false;
4279}
#define btAlignedFree(ptr)
#define btAlignedAlloc(size, alignment)
#define DISABLE_DEACTIVATION
#define WANTS_DEACTIVATION
#define ISLAND_SLEEPING
static DBVT_INLINE btScalar size(const btDbvtVolume &a)
Definition: btDbvt.cpp:52
static btDbvtVolume bounds(btDbvtNode **leaves, int count)
Definition: btDbvt.cpp:299
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
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
Definition: btQuaternion.h:888
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
btScalar btAngle(const btQuaternion &q1, const btQuaternion &q2)
Return the angle between two quaternions.
Definition: btQuaternion.h:902
#define BT_PROFILE(name)
Definition: btQuickprof.h:198
bool gDisableDeactivation
Definition: btRigidBody.cpp:26
btScalar gDeactivationTime
Definition: btRigidBody.cpp:25
#define btRigidBodyData
Definition: btRigidBody.h:35
#define SIMD_PI
Definition: btScalar.h:526
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
#define BT_LARGE_FLOAT
Definition: btScalar.h:316
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
btScalar btAcos(btScalar x)
Definition: btScalar.h:501
void btSwap(T &a, T &b)
Definition: btScalar.h:643
bool btFuzzyZero(btScalar x)
Definition: btScalar.h:572
#define btAssert(x)
Definition: btScalar.h:153
#define BT_SBMATERIAL_CODE
Definition: btSerializer.h:119
#define BT_ARRAY_CODE
Definition: btSerializer.h:118
#define BT_SBNODE_CODE
Definition: btSerializer.h:120
@ BT_JOINT_RIGID_BODY
@ BT_JOINT_SOFT_BODY_CLUSTER
@ BT_JOINT_COLLISION_OBJECT
static T sum(const btAlignedObjectArray< T > &items)
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 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 btVector3 generateUnitOrthogonalVector(const btVector3 &u)
static btVector3 NormalizeAny(const btVector3 &v)
static int PolarDecompose(const btMatrix3x3 &m, btMatrix3x3 &q, btMatrix3x3 &s)
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 T BaryEval(const T &a, const T &b, const T &c, const btVector3 &coord)
static bool SameSign(const T &x, const T &y)
static void EvaluateMedium(const btSoftBodyWorldInfo *wfi, const btVector3 &x, btSoftBody::sMedium &medium)
static void ZeroInitialize(T &value)
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 btVector3 Clamp(const btVector3 &v, btScalar maxlength)
static btMatrix3x3 Diagonal(btScalar x)
static void calculateNormalCone(btDbvntNode *root)
#define IDX(_x_, _y_)
#define IDX2PTR(_p_, _b_)
#define PTR2IDX(_p_, _b_)
static void getBarycentric(const btVector3 &p, btVector3 &a, btVector3 &b, btVector3 &c, btVector3 &bary)
#define NEXTRAND
static btDbvntNode * copyToDbvnt(const btDbvtNode *n)
#define btSoftBodyDataName
Definition: btSoftBody.h:37
#define btSoftBodyData
btSoftBody implementation by Nathanael Presson
Definition: btSoftBody.h:36
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
#define btVector3Data
Definition: btVector3.h:27
int size() const
return the number of elements in the array
int findLinearSearch(const T &key) const
void resize(int newsize, const T &fillData=T())
void clear()
clear the array, deallocated memory. Generally it is better to use array.resize(0),...
void remove(const T &key)
void push_back(const T &_Val)
int capacity() const
return the pre-allocated (reserved) elements, this is at least as large as the total number of elemen...
virtual void setAabb(btBroadphaseProxy *proxy, const btVector3 &aabbMin, const btVector3 &aabbMax, btDispatcher *dispatcher)=0
void * m_oldPtr
Definition: btSerializer.h:52
btCollisionObject can be used to manage collision detection objects.
bool isStaticOrKinematicObject() const
virtual const char * serialize(void *dataBuffer, class btSerializer *serializer) const
fills the dataBuffer and returns the struct name (and 0 on failure)
btTransform & getWorldTransform()
btBroadphaseProxy * getBroadphaseHandle()
btTransform m_worldTransform
btCollisionShape * m_collisionShape
int getInternalType() const
reserved for Bullet internal usage
bool hasContactResponse() const
void activate(bool forceActivation=false) const
void setActivationState(int newState) const
int m_internalType
m_internalType is reserved to distinguish Bullet's btCollisionObject, btRigidBody,...
btScalar getFriction() const
const btTransform & getInterpolationWorldTransform() const
const btCollisionShape * getCollisionShape() const
int getActivationState() const
The btCollisionShape class provides an interface for collision shapes that can be shared among btColl...
virtual void setMargin(btScalar margin)=0
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 btConvexShape is an abstract shape interface, implemented by all convex shapes such as btBoxShape...
Definition: btConvexShape.h:33
The btHashMap template class implements a generic and lightweight hashmap.
Definition: btHashMap.h:220
void insert(const Key &key, const Value &value)
Definition: btHashMap.h:264
const Value * find(const Key &key) const
Definition: btHashMap.h:424
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:48
btMatrix3x3 adjoint() const
Return the adjoint of the matrix.
Definition: btMatrix3x3.h:1069
btMatrix3x3 inverse() const
Return the inverse of the matrix.
Definition: btMatrix3x3.h:1077
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
btScalar determinant() const
Return the determinant of the matrix.
Definition: btMatrix3x3.h:1006
void serializeFloat(struct btMatrix3x3FloatData &dataOut) const
Definition: btMatrix3x3.h:1391
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
void applyDeltaVeeMultiDof(const btScalar *delta_vee, btScalar multiplier)
Definition: btMultiBody.h:417
const btScalar * getVelocityVector() const
Definition: btMultiBody.h:272
The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatr...
Definition: btQuaternion.h:50
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 btCollisionShape * getCollisionShape() const
Definition: btRigidBody.h:242
void applyImpulse(const btVector3 &impulse, const btVector3 &rel_pos)
Definition: btRigidBody.h:326
static const btRigidBody * upcast(const btCollisionObject *colObj)
to keep collision detection and dynamics separate we don't store a rigidbody pointer but a rigidbody ...
Definition: btRigidBody.h:189
const btMatrix3x3 & getInvInertiaTensorWorld() const
Definition: btRigidBody.h:265
virtual btChunk * allocate(size_t size, int numElements)=0
virtual void * getUniquePointer(void *oldPtr)=0
virtual void finalizeChunk(btChunk *chunk, const char *structType, int chunkCode, void *oldPtr)=0
virtual void * findPointer(void *oldPtr)=0
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
static void PSolve_Links(btSoftBody *psb, btScalar kst, btScalar ti)
static void PSolve_SContacts(btSoftBody *psb, btScalar, btScalar ti)
bool checkLink(int node0, int node1) const
Definition: btSoftBody.cpp:140
bool m_bUpdateRtCst
Definition: btSoftBody.h:796
btScalar m_sleepingThreshold
Definition: btSoftBody.h:802
bool checkFace(int node0, int node1, int node2) const
Definition: btSoftBody.cpp:162
void advanceDeformation()
void updateClusters()
btDbvt m_cdbvt
Definition: btSoftBody.h:799
void setPose(bool bvolume, bool bframe)
Definition: btSoftBody.cpp:983
bool cutLink(int node0, int node1, btScalar position)
void appendFace(int model=-1, Material *mat=0)
Definition: btSoftBody.cpp:316
void setMass(int node, btScalar mass)
Definition: btSoftBody.cpp:789
void interpolateRenderMesh()
tJointArray m_joints
Definition: btSoftBody.h:792
btScalar m_dampingCoefficient
Definition: btSoftBody.h:801
btAlignedObjectArray< TetraScratch > m_tetraScratchesTn
Definition: btSoftBody.h:784
void integrateMotion()
void appendLinearJoint(const LJoint::Specs &specs, Cluster *body0, Body body1)
Definition: btSoftBody.cpp:507
void scale(const btVector3 &scl)
Definition: btSoftBody.cpp:941
btAlignedObjectArray< bool > m_clusterConnectivity
Definition: btSoftBody.h:811
void defaultCollisionHandler(const btCollisionObjectWrapper *pcoWrap)
btScalar getVolume() const
bool rayTest(const btVector3 &rayFrom, const btVector3 &rayTo, sRayCast &results)
Ray casting using rayFrom and rayTo in worldspace, (not direction!)
SolverState m_sst
Definition: btSoftBody.h:772
void addVelocity(const btVector3 &velocity)
Definition: btSoftBody.cpp:760
void predictMotion(btScalar dt)
void setSelfCollision(bool useSelfCollision)
btScalar m_timeacc
Definition: btSoftBody.h:794
Pose m_pose
Definition: btSoftBody.h:773
void appendTetra(int model, Material *mat)
Definition: btSoftBody.cpp:356
void setRestLengthScale(btScalar restLength)
Definition: btSoftBody.cpp:968
void rotate(const btQuaternion &rot)
Definition: btSoftBody.cpp:932
void applyClusters(bool drift)
void setZeroVelocity()
static void PSolve_Anchors(btSoftBody *psb, btScalar kst, btScalar ti)
btSoftBodyWorldInfo * m_worldInfo
Definition: btSoftBody.h:775
void updateArea(bool averageArea=true)
void addForce(const btVector3 &force)
Definition: btSoftBody.cpp:560
bool wantsSleeping()
void prepareClusters(int iterations)
void setCollisionQuadrature(int N)
static void clusterVImpulse(Cluster *cluster, const btVector3 &rpos, const btVector3 &impulse)
virtual const char * serialize(void *dataBuffer, class btSerializer *serializer) const
fills the dataBuffer and returns the struct name (and 0 on failure)
static void VSolve_Links(btSoftBody *psb, btScalar kst)
tTetraArray m_tetras
Definition: btSoftBody.h:782
bool useSelfCollision()
btVector3 evaluateCom() const
void setTotalDensity(btScalar density)
Definition: btSoftBody.cpp:849
btAlignedObjectArray< const class btCollisionObject * > m_collisionDisabledObjects
Definition: btSoftBody.h:75
static void clusterDAImpulse(Cluster *cluster, const btVector3 &impulse)
void appendNode(const btVector3 &x, btScalar m)
Definition: btSoftBody.cpp:256
btTransform m_initialWorldTransform
Definition: btSoftBody.h:813
void staticSolve(int iterations)
void setVolumeMass(btScalar mass)
Definition: btSoftBody.cpp:855
btScalar m_restLengthScale
Definition: btSoftBody.h:817
bool checkDeformableContact(const btCollisionObjectWrapper *colObjWrap, const btVector3 &x, btScalar margin, btSoftBody::sCti &cti, bool predict=false) const
void cleanupClusters()
Config m_cfg
Definition: btSoftBody.h:771
void updateDeactivation(btScalar timeStep)
btAlignedObjectArray< TetraScratch > m_tetraScratches
Definition: btSoftBody.h:783
const btVector3 & getWindVelocity()
Return the wind velocity for interaction with the air.
void addAeroForceToFace(const btVector3 &windVelocity, int faceIndex)
Definition: btSoftBody.cpp:664
btAlignedObjectArray< btVector4 > m_renderNodesInterpolationWeights
Definition: btSoftBody.h:807
void appendAngularJoint(const AJoint::Specs &specs, Cluster *body0, Body body1)
Definition: btSoftBody.cpp:533
tFaceArray m_faces
Definition: btSoftBody.h:780
void setVolumeDensity(btScalar density)
Definition: btSoftBody.cpp:885
static void clusterDCImpulse(Cluster *cluster, const btVector3 &impulse)
void transform(const btTransform &trs)
Definition: btSoftBody.cpp:900
static void clusterVAImpulse(Cluster *cluster, const btVector3 &impulse)
btScalar getMass(int node) const
Definition: btSoftBody.cpp:796
tMaterialArray m_materials
Definition: btSoftBody.h:793
void setMaxStress(btScalar maxStress)
void dampClusters()
btSoftBody(btSoftBodyWorldInfo *worldInfo, int node_count, const btVector3 *x, const btScalar *m)
btSoftBody implementation by Nathanael Presson
Definition: btSoftBody.cpp:28
void updateDeformation()
void addAeroForceToNode(const btVector3 &windVelocity, int nodeIndex)
Definition: btSoftBody.cpp:575
static btVector3 clusterCom(const Cluster *cluster)
tRContactArray m_rcontacts
Definition: btSoftBody.h:787
void appendAnchor(int node, btRigidBody *body, bool disableCollisionBetweenLinkedBodies=false, btScalar influence=1)
Definition: btSoftBody.cpp:388
btVector3 m_bounds[2]
Definition: btSoftBody.h:795
btScalar m_maxSpeedSquared
Definition: btSoftBody.h:803
void releaseCluster(int index)
void setVelocity(const btVector3 &velocity)
Definition: btSoftBody.cpp:766
tClusterArray m_clusters
Definition: btSoftBody.h:800
void solveConstraints()
int generateClusters(int k, int maxiterations=8192)
generateClusters with k=0 will create a convex cluster for each tetrahedron or triangle otherwise an ...
void releaseClusters()
void refine(ImplicitFn *ifn, btScalar accurary, bool cut)
void setSolver(eSolverPresets::_ preset)
Material * appendMaterial()
Definition: btSoftBody.cpp:186
btAlignedObjectArray< DeformableNodeRigidAnchor > m_deformableAnchors
Definition: btSoftBody.h:786
static void solveClusters(const btAlignedObjectArray< btSoftBody * > &bodies)
void appendNote(const char *text, const btVector3 &o, const btVector4 &c=btVector4(1, 0, 0, 0), Node *n0=0, Node *n1=0, Node *n2=0, Node *n3=0)
Definition: btSoftBody.cpp:198
bool checkDeformableFaceContact(const btCollisionObjectWrapper *colObjWrap, Face &f, btVector3 &contact_point, btVector3 &bary, btScalar margin, btSoftBody::sCti &cti, bool predict=false) const
virtual int calculateSerializeBufferSize() const
static void PSolve_RContacts(btSoftBody *psb, btScalar kst, btScalar ti)
static void clusterImpulse(Cluster *cluster, const btVector3 &rpos, const Impulse &impulse)
btAlignedObjectArray< btAlignedObjectArray< const btSoftBody::Node * > > m_renderNodesParents
Definition: btSoftBody.h:808
void pointersToIndices()
tNoteArray m_notes
Definition: btSoftBody.h:776
void updateNormals()
static void clusterDImpulse(Cluster *cluster, const btVector3 &rpos, const btVector3 &impulse)
btDbvt m_fdbvt
Definition: btSoftBody.h:798
tLinkArray m_links
Definition: btSoftBody.h:779
void applyForces()
static btVector3 clusterVelocity(const Cluster *cluster, const btVector3 &rpos)
tSContactArray m_scontacts
Definition: btSoftBody.h:791
bool m_useSelfCollision
Definition: btSoftBody.h:809
void * m_tag
Definition: btSoftBody.h:774
void updatePose()
void(* psolver_t)(btSoftBody *, btScalar, btScalar)
Definition: btSoftBody.h:751
void initializeClusters()
tAnchorArray m_anchors
Definition: btSoftBody.h:785
btScalar getRestLengthScale()
Definition: btSoftBody.cpp:962
void randomizeConstraints()
btVector3 m_windVelocity
Definition: btSoftBody.h:815
btScalar getTotalMass() const
Definition: btSoftBody.cpp:802
bool m_useFaceContact
Definition: btSoftBody.h:804
tNodeArray m_nodes
Definition: btSoftBody.h:777
void appendLink(int model=-1, Material *mat=0)
Definition: btSoftBody.cpp:276
void setSpringStiffness(btScalar k)
void initializeDmInverse()
void updateConstants()
void setTotalMass(btScalar mass, bool fromfaces=false)
Definition: btSoftBody.cpp:813
virtual ~btSoftBody()
Definition: btSoftBody.cpp:126
void appendDeformableAnchor(int node, btRigidBody *body)
Definition: btSoftBody.cpp:415
void updateLinkConstants()
void(* vsolver_t)(btSoftBody *, btScalar)
Definition: btSoftBody.h:752
tNodeArray m_renderNodes
Definition: btSoftBody.h:778
void initDefaults()
Definition: btSoftBody.cpp:64
btAlignedObjectArray< btVector3 > m_quads
Definition: btSoftBody.h:805
static psolver_t getSolver(ePSolver::_ solver)
bool checkContact(const btCollisionObjectWrapper *colObjWrap, const btVector3 &x, btScalar margin, btSoftBody::sCti &cti) const
void indicesToPointers(const int *map=0)
static void solveCommonConstraints(btSoftBody **bodies, int count, int iterations)
void updateBounds()
void setWindVelocity(const btVector3 &velocity)
Set a wind velocity for interaction with the air.
int generateBendingConstraints(int distance, Material *mat=0)
void translate(const btVector3 &trs)
Definition: btSoftBody.cpp:923
void initializeFaceTree()
void resetLinkRestLengths()
int clusterCount() const
btDbvt m_ndbvt
Definition: btSoftBody.h:797
static void clusterAImpulse(Cluster *cluster, const Impulse &impulse)
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:30
btTransform inverse() const
Return the inverse of this transform.
Definition: btTransform.h:182
btVector3 invXform(const btVector3 &inVec) const
Definition: btTransform.h:215
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:108
void setRotation(const btQuaternion &q)
Set the rotational element by btQuaternion.
Definition: btTransform.h:160
void setIdentity()
Set this transformation to the identity.
Definition: btTransform.h:166
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:113
void setOrigin(const btVector3 &origin)
Set the translational element.
Definition: btTransform.h:146
void setBasis(const btMatrix3x3 &basis)
Set the rotational element by btMatrix3x3.
Definition: btTransform.h:154
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
void setZ(btScalar _z)
Set the z value.
Definition: btVector3.h:571
const btScalar & w() const
Return the w value.
Definition: btVector3.h:581
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
void serializeFloat(struct btVector3FloatData &dataOut) const
Definition: btVector3.h:1291
btVector3 normalized() const
Return a normalized version of this vector.
Definition: btVector3.h:949
void setY(btScalar _y)
Set the y value.
Definition: btVector3.h:569
void setX(btScalar _x)
Set the x value.
Definition: btVector3.h:567
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 setZero()
Definition: btVector3.h:671
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 btScalar & getX() const
Return the x value.
Definition: btVector3.h:561
const btScalar & y() const
Return the y value.
Definition: btVector3.h:577
btVector3FloatData m_av
btMatrix3x3FloatData m_invwi
btVector3FloatData m_vimpulses[2]
btVector3FloatData * m_framerefs
btVector3FloatData m_dimpulses[2]
btMatrix3x3FloatData m_locii
btVector3FloatData m_lv
btTransformFloatData m_framexform
float m_selfCollisionImpulseFactor
btVector3FloatData m_com
btVector3FloatData m_normal
SoftBodyMaterialData * m_material
SoftBodyMaterialData * m_material
btVector3FloatData m_accumulatedForce
btVector3FloatData m_normal
btVector3FloatData m_previousPosition
btVector3FloatData m_velocity
btVector3FloatData m_position
SoftBodyMaterialData * m_material
btMatrix3x3FloatData m_rot
btVector3FloatData m_com
btVector3FloatData * m_positions
btMatrix3x3FloatData m_aqq
btMatrix3x3FloatData m_scale
btVector3FloatData m_c0[4]
SoftBodyMaterialData * m_material
btMatrix3x3FloatData m_c0
btVector3FloatData m_localFrame
btVector3FloatData m_c1
btRigidBodyData * m_rigidBody
const btCollisionShape * getCollisionShape() const
const btTransform * m_preTransform
const btCollisionObject * getCollisionObject() const
const btTransform & getWorldTransform() const
void * data
Definition: btDbvt.h:202
btVector3 normal
Definition: btDbvt.h:197
btDbvntNode * childs[2]
Definition: btDbvt.h:201
btScalar angle
Definition: btDbvt.h:198
DBVT_INLINE bool isleaf() const
Definition: btDbvt.h:199
static btDbvtAabbMm FromMM(const btVector3 &mi, const btVector3 &mx)
Definition: btDbvt.h:479
static btDbvtAabbMm FromCR(const btVector3 &c, btScalar r)
Definition: btDbvt.h:473
DBVT_INLINE bool isinternal() const
Definition: btDbvt.h:185
btDbvtNode * childs[2]
Definition: btDbvt.h:187
void * data
Definition: btDbvt.h:188
btDbvtNode * insert(const btDbvtVolume &box, void *data)
Definition: btDbvt.cpp:535
void optimizeIncremental(int passes)
Definition: btDbvt.cpp:514
void update(btDbvtNode *leaf, int lookahead=-1)
Definition: btDbvt.cpp:544
DBVT_PREFIX void collideTV(const btDbvtNode *root, const btDbvtVolume &volume, DBVT_IPOLICY) const
Definition: btDbvt.h:1148
bool empty() const
Definition: btDbvt.h:314
static DBVT_PREFIX void rayTest(const btDbvtNode *root, const btVector3 &rayFrom, const btVector3 &rayTo, DBVT_IPOLICY)
rayTest is a re-entrant ray test, and can be called in parallel as long as the btAlignedAlloc is thre...
Definition: btDbvt.h:1276
void clear()
Definition: btDbvt.cpp:477
btDbvtNode * m_root
Definition: btDbvt.h:302
void remove(btDbvtNode *leaf)
Definition: btDbvt.cpp:611
DBVT_PREFIX void collideTT(const btDbvtNode *root0, const btDbvtNode *root1, DBVT_IPOLICY)
Definition: btDbvt.h:822
DBVT_PREFIX void selfCollideT(const btDbvntNode *root, DBVT_IPOLICY)
Definition: btDbvt.h:887
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
btVector3FloatData m_refs[2]
btVector3FloatData m_relPosition[2]
btScalar air_density
Definition: btSoftBody.h:47
btDispatcher * m_dispatcher
Definition: btSoftBody.h:53
btSparseSdf< 3 > m_sparsesdf
Definition: btSoftBody.h:55
btVector3 m_gravity
Definition: btSoftBody.h:54
btScalar m_maxDisplacement
Definition: btSoftBody.h:50
btBroadphaseInterface * m_broadphase
Definition: btSoftBody.h:52
void Prepare(btScalar dt, int iterations)
void Solve(btScalar dt, btScalar sor)
IControl * m_icontrol
Definition: btSoftBody.h:663
void Terminate(btScalar dt)
btScalar m_influence
Definition: btSoftBody.h:405
btVector3 m_local
Definition: btSoftBody.h:403
btRigidBody * m_body
Definition: btSoftBody.h:404
btMatrix3x3 m_c0
Definition: btSoftBody.h:406
const btTransform & xform() const
Definition: btSoftBody.h:521
void Terminate(btScalar dt)
void Prepare(btScalar dt, int iterations)
void Solve(btScalar dt, btScalar sor)
btVector3 m_dimpulses[2]
Definition: btSoftBody.h:445
tVector3Array m_framerefs
Definition: btSoftBody.h:437
btMatrix3x3 m_invwi
Definition: btSoftBody.h:442
btMatrix3x3 m_locii
Definition: btSoftBody.h:441
btAlignedObjectArray< Node * > m_nodes
Definition: btSoftBody.h:436
btDbvtNode * m_leaf
Definition: btSoftBody.h:450
btVector3 m_vimpulses[2]
Definition: btSoftBody.h:444
tScalarArray m_masses
Definition: btSoftBody.h:435
btTransform m_framexform
Definition: btSoftBody.h:438
btScalar maxvolume
Definition: btSoftBody.h:704
tPSolverArray m_psequence
Definition: btSoftBody.h:712
tPSolverArray m_dsequence
Definition: btSoftBody.h:713
btScalar kSK_SPLT_CL
Definition: btSoftBody.h:702
btScalar kSS_SPLT_CL
Definition: btSoftBody.h:703
btScalar m_maxStress
Definition: btSoftBody.h:715
eAeroModel::_ aeromodel
Definition: btSoftBody.h:685
btScalar kSR_SPLT_CL
Definition: btSoftBody.h:701
tVSolverArray m_vsequence
Definition: btSoftBody.h:711
btScalar timescale
Definition: btSoftBody.h:705
btVector4 m_pcontact
Definition: btSoftBody.h:291
btVector3 m_normal
Definition: btSoftBody.h:288
Node * m_n[3]
Definition: btSoftBody.h:287
btDbvtNode * m_leaf
Definition: btSoftBody.h:290
Material * m_material
Definition: btSoftBody.h:252
virtual btScalar Eval(const btVector3 &x)=0
btVector3 m_velocity
Definition: btSoftBody.h:467
btScalar m_split
Definition: btSoftBody.h:617
btVector3 m_refs[2]
Definition: btSoftBody.h:614
virtual void Prepare(btScalar dt, int iterations)
void Solve(btScalar dt, btScalar sor)
void Prepare(btScalar dt, int iterations)
void Terminate(btScalar dt)
btScalar m_area
Definition: btSoftBody.h:265
btVector3 m_x
Definition: btSoftBody.h:257
btVector3 m_v
Definition: btSoftBody.h:259
btVector3 m_q
Definition: btSoftBody.h:258
btDbvtNode * m_leaf
Definition: btSoftBody.h:266
btVector3 m_n
Definition: btSoftBody.h:263
btVector3 m_f
Definition: btSoftBody.h:262
btScalar m_coords[4]
Definition: btSoftBody.h:417
btVector3 m_offset
Definition: btSoftBody.h:414
Node * m_nodes[4]
Definition: btSoftBody.h:416
const char * m_text
Definition: btSoftBody.h:413
btMatrix3x3 m_scl
Definition: btSoftBody.h:429
btScalar m_volume
Definition: btSoftBody.h:424
btVector3 m_com
Definition: btSoftBody.h:427
tVector3Array m_pos
Definition: btSoftBody.h:425
btMatrix3x3 m_aqq
Definition: btSoftBody.h:430
btMatrix3x3 m_rot
Definition: btSoftBody.h:428
tScalarArray m_wgh
Definition: btSoftBody.h:426
btMatrix3x3 m_c0
Definition: btSoftBody.h:322
RayFromToCaster takes a ray from, ray to (instead of direction!)
Definition: btSoftBody.h:728
RayFromToCaster(const btVector3 &rayFrom, const btVector3 &rayTo, btScalar mxt)
void Process(const btDbvtNode *leaf)
static btScalar rayFromToTriangle(const btVector3 &rayFrom, const btVector3 &rayTo, const btVector3 &rayNormalizedDirection, const btVector3 &a, const btVector3 &b, const btVector3 &c, btScalar maxt=SIMD_INFINITY)
btVector3 m_rayNormalizedDirection
Definition: btSoftBody.h:731
btScalar m_cfm[2]
Definition: btSoftBody.h:397
btScalar m_element_measure
Definition: btSoftBody.h:305
btMatrix3x3 m_Dm_inverse
Definition: btSoftBody.h:303
btMatrix3x3 m_F
Definition: btSoftBody.h:304
@ V_TwoSided
Vertex normals are oriented toward velocity.
Definition: btSoftBody.h:90
@ V_OneSided
Vertex normals are flipped to match velocity and lift and drag forces are applied.
Definition: btSoftBody.h:92
@ V_TwoSidedLiftDrag
Vertex normals are flipped to match velocity.
Definition: btSoftBody.h:91
@ F_OneSided
Face normals are flipped to match velocity and lift and drag forces are applied.
Definition: btSoftBody.h:95
@ F_TwoSided
Vertex normals are taken as it is.
Definition: btSoftBody.h:93
@ F_TwoSidedLiftDrag
Face normals are flipped to match velocity.
Definition: btSoftBody.h:94
@ RContacts
Anchor solver.
Definition: btSoftBody.h:117
@ SContacts
Rigid contacts solver.
Definition: btSoftBody.h:118
@ Anchors
Linear solver.
Definition: btSoftBody.h:116
@ VF_SS
Rigid versus soft mask.
Definition: btSoftBody.h:168
@ Default
Vertex vs face soft vs soft handling.
Definition: btSoftBody.h:173
@ VF_DD
Cluster soft body self collision.
Definition: btSoftBody.h:171
@ CL_SS
Vertex vs face soft vs soft handling.
Definition: btSoftBody.h:169
@ CL_SELF
Cluster vs cluster soft vs soft handling.
Definition: btSoftBody.h:170
@ SVSmask
DF based rigid vs deformable faces.
Definition: btSoftBody.h:167
@ SDF_RS
Rigid versus soft mask.
Definition: btSoftBody.h:162
@ SDF_RD
Cluster vs convex rigid vs soft.
Definition: btSoftBody.h:164
@ CL_RS
SDF based rigid vs soft.
Definition: btSoftBody.h:163
@ Default
Enable debug draw.
Definition: btSoftBody.h:185
const btCollisionObject * m_colObj
Definition: btSoftBody.h:220
btScalar m_offset
Definition: btSoftBody.h:222
btVector3 m_normal
Definition: btSoftBody.h:221
btVector3 m_velocity
Definition: btSoftBody.h:229
eFeature::_ feature
soft body
Definition: btSoftBody.h:198
btScalar fraction
feature index
Definition: btSoftBody.h:200
int index
feature type
Definition: btSoftBody.h:199
btSoftBody * body
Definition: btSoftBody.h:197
void ProcessColObj(btSoftBody *ps, const btCollisionObjectWrapper *colObWrap)
void ProcessSoftSoft(btSoftBody *psa, btSoftBody *psb)
const btCollisionObjectWrapper * m_colObj1Wrap
const btCollisionObjectWrapper * m_colObj1Wrap
const btCollisionObjectWrapper * m_colObj1Wrap
btScalar Evaluate(const btVector3 &x, const btCollisionShape *shape, btVector3 &normal, btScalar margin)
Definition: btSparseSDF.h:196