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 https://bulletphysics.org
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"
27#include <iostream>
28//
30{
31 int N = leafNodes.size();
32 if (N == 0)
33 {
34 return NULL;
35 }
36 while (N > 1)
37 {
42 marked.resize(N);
43 for (int i = 0; i < N; ++i)
44 marked[i] = false;
45
46 // pair adjacent nodes into new(parent) node
47 for (int i = 0; i < N; ++i)
48 {
49 if (marked[i])
50 continue;
51 bool merged = false;
52 for (int j = 0; j < adj[i].size(); ++j)
53 {
54 int n = adj[i][j];
55 if (!marked[adj[i][j]])
56 {
57 btDbvtNode* node = new (btAlignedAlloc(sizeof(btDbvtNode), 16)) btDbvtNode();
58 node->parent = NULL;
59 node->childs[0] = leafNodes[i];
60 node->childs[1] = leafNodes[n];
61 leafNodes[i]->parent = node;
62 leafNodes[n]->parent = node;
63 newLeafNodes.push_back(node);
64 childIds.push_back(std::make_pair(i, n));
65 merged = true;
66 marked[n] = true;
67 break;
68 }
69 }
70 if (!merged)
71 {
72 newLeafNodes.push_back(leafNodes[i]);
73 childIds.push_back(std::make_pair(i, -1));
74 }
75 marked[i] = true;
76 }
77 // update adjacency matrix
78 newAdj.resize(newLeafNodes.size());
79 for (int i = 0; i < newLeafNodes.size(); ++i)
80 {
81 for (int j = i + 1; j < newLeafNodes.size(); ++j)
82 {
83 bool neighbor = false;
84 const btAlignedObjectArray<int>& leftChildNeighbors = adj[childIds[i].first];
85 for (int k = 0; k < leftChildNeighbors.size(); ++k)
86 {
87 if (leftChildNeighbors[k] == childIds[j].first || leftChildNeighbors[k] == childIds[j].second)
88 {
89 neighbor = true;
90 break;
91 }
92 }
93 if (!neighbor && childIds[i].second != -1)
94 {
95 const btAlignedObjectArray<int>& rightChildNeighbors = adj[childIds[i].second];
96 for (int k = 0; k < rightChildNeighbors.size(); ++k)
97 {
98 if (rightChildNeighbors[k] == childIds[j].first || rightChildNeighbors[k] == childIds[j].second)
99 {
100 neighbor = true;
101 break;
102 }
103 }
104 }
105 if (neighbor)
106 {
107 newAdj[i].push_back(j);
108 newAdj[j].push_back(i);
109 }
110 }
111 }
112 leafNodes = newLeafNodes;
113 //this assignment leaks memory, the assignment doesn't do a deep copy, for now a manual copy
114 //adj = newAdj;
115 adj.clear();
116 adj.resize(newAdj.size());
117 for (int i = 0; i < newAdj.size(); i++)
118 {
119 for (int j = 0; j < newAdj[i].size(); j++)
120 {
121 adj[i].push_back(newAdj[i][j]);
122 }
123 }
124 N = leafNodes.size();
125 }
126 return leafNodes[0];
127}
128
129//
130btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo, int node_count, const btVector3* x, const btScalar* m)
131 : m_softBodySolver(0), m_worldInfo(worldInfo)
132{
133 /* Init */
134 initDefaults();
135
136 /* Default material */
137 Material* pm = appendMaterial();
138 pm->m_kLST = 1;
139 pm->m_kAST = 1;
140 pm->m_kVST = 1;
142
143 /* Nodes */
144 const btScalar margin = getCollisionShape()->getMargin();
145 m_nodes.resize(node_count);
146 m_X.resize(node_count);
147 for (int i = 0, ni = node_count; i < ni; ++i)
148 {
149 Node& n = m_nodes[i];
151 n.m_x = x ? *x++ : btVector3(0, 0, 0);
152 n.m_q = n.m_x;
153 n.m_im = m ? *m++ : 1;
154 n.m_im = n.m_im > 0 ? 1 / n.m_im : 0;
155 n.m_leaf = m_ndbvt.insert(btDbvtVolume::FromCR(n.m_x, margin), &n);
156 n.m_material = pm;
157 m_X[i] = n.m_x;
158 }
159 updateBounds();
161 m_fdbvnt = 0;
162}
163
165 : m_worldInfo(worldInfo)
166{
167 initDefaults();
168}
169
171{
174 m_cfg.kVCF = 1;
175 m_cfg.kDG = 0;
176 m_cfg.kLF = 0;
177 m_cfg.kDP = 0;
178 m_cfg.kPR = 0;
179 m_cfg.kVC = 0;
180 m_cfg.kDF = (btScalar)0.2;
181 m_cfg.kMT = 0;
182 m_cfg.kCHR = (btScalar)1.0;
183 m_cfg.kKHR = (btScalar)0.1;
184 m_cfg.kSHR = (btScalar)1.0;
185 m_cfg.kAHR = (btScalar)0.7;
186 m_cfg.kSRHR_CL = (btScalar)0.1;
188 m_cfg.kSSHR_CL = (btScalar)0.5;
193 m_cfg.timescale = 1;
194 m_cfg.viterations = 0;
195 m_cfg.piterations = 1;
196 m_cfg.diterations = 0;
197 m_cfg.citerations = 4;
198 m_cfg.drag = 0;
199 m_cfg.m_maxStress = 0;
201 m_pose.m_bvolume = false;
202 m_pose.m_bframe = false;
203 m_pose.m_volume = 0;
204 m_pose.m_com = btVector3(0, 0, 0);
207 m_tag = 0;
208 m_timeacc = 0;
209 m_bUpdateRtCst = true;
210 m_bounds[0] = btVector3(0, 0, 0);
211 m_bounds[1] = btVector3(0, 0, 0);
214
215 /* Collision shape */
219
221
222 m_windVelocity = btVector3(0, 0, 0);
226 m_useSelfCollision = false;
228 m_softSoftCollision = false;
231 m_gravityFactor = 1;
232 m_cacheBarycenter = false;
233 m_fdbvnt = 0;
234
235 // reduced flag
236 m_reducedModel = false;
237}
238
239//
241{
242 //for now, delete the internal shape
243 delete m_collisionShape;
244 int i;
245
247 for (i = 0; i < m_materials.size(); ++i)
249 for (i = 0; i < m_joints.size(); ++i)
251 if (m_fdbvnt)
252 delete m_fdbvnt;
253}
254
255//
256bool btSoftBody::checkLink(int node0, int node1) const
257{
258 return (checkLink(&m_nodes[node0], &m_nodes[node1]));
259}
260
261//
262bool btSoftBody::checkLink(const Node* node0, const Node* node1) const
263{
264 const Node* n[] = {node0, node1};
265 for (int i = 0, ni = m_links.size(); i < ni; ++i)
266 {
267 const Link& l = m_links[i];
268 if ((l.m_n[0] == n[0] && l.m_n[1] == n[1]) ||
269 (l.m_n[0] == n[1] && l.m_n[1] == n[0]))
270 {
271 return (true);
272 }
273 }
274 return (false);
275}
276
277//
278bool btSoftBody::checkFace(int node0, int node1, int node2) const
279{
280 const Node* n[] = {&m_nodes[node0],
281 &m_nodes[node1],
282 &m_nodes[node2]};
283 for (int i = 0, ni = m_faces.size(); i < ni; ++i)
284 {
285 const Face& f = m_faces[i];
286 int c = 0;
287 for (int j = 0; j < 3; ++j)
288 {
289 if ((f.m_n[j] == n[0]) ||
290 (f.m_n[j] == n[1]) ||
291 (f.m_n[j] == n[2]))
292 c |= 1 << j;
293 else
294 break;
295 }
296 if (c == 7) return (true);
297 }
298 return (false);
299}
300
301//
303{
304 Material* pm = new (btAlignedAlloc(sizeof(Material), 16)) Material();
305 if (m_materials.size() > 0)
306 *pm = *m_materials[0];
307 else
308 ZeroInitialize(*pm);
310 return (pm);
311}
312
313//
314void btSoftBody::appendNote(const char* text,
315 const btVector3& o,
316 const btVector4& c,
317 Node* n0,
318 Node* n1,
319 Node* n2,
320 Node* n3)
321{
322 Note n;
324 n.m_rank = 0;
325 n.m_text = text;
326 n.m_offset = o;
327 n.m_coords[0] = c.x();
328 n.m_coords[1] = c.y();
329 n.m_coords[2] = c.z();
330 n.m_coords[3] = c.w();
331 n.m_nodes[0] = n0;
332 n.m_rank += n0 ? 1 : 0;
333 n.m_nodes[1] = n1;
334 n.m_rank += n1 ? 1 : 0;
335 n.m_nodes[2] = n2;
336 n.m_rank += n2 ? 1 : 0;
337 n.m_nodes[3] = n3;
338 n.m_rank += n3 ? 1 : 0;
340}
341
342//
343void btSoftBody::appendNote(const char* text,
344 const btVector3& o,
345 Node* feature)
346{
347 appendNote(text, o, btVector4(1, 0, 0, 0), feature);
348}
349
350//
351void btSoftBody::appendNote(const char* text,
352 const btVector3& o,
353 Link* feature)
354{
355 static const btScalar w = 1 / (btScalar)2;
356 appendNote(text, o, btVector4(w, w, 0, 0), feature->m_n[0],
357 feature->m_n[1]);
358}
359
360//
361void btSoftBody::appendNote(const char* text,
362 const btVector3& o,
363 Face* feature)
364{
365 static const btScalar w = 1 / (btScalar)3;
366 appendNote(text, o, btVector4(w, w, w, 0), feature->m_n[0],
367 feature->m_n[1],
368 feature->m_n[2]);
369}
370
371//
373{
374 if (m_nodes.capacity() == m_nodes.size())
375 {
377 m_nodes.reserve(m_nodes.size() * 2 + 1);
379 }
380 const btScalar margin = getCollisionShape()->getMargin();
382 Node& n = m_nodes[m_nodes.size() - 1];
384 n.m_x = x;
385 n.m_q = n.m_x;
386 n.m_im = m > 0 ? 1 / m : 0;
387 n.m_material = m_materials[0];
388 n.m_leaf = m_ndbvt.insert(btDbvtVolume::FromCR(n.m_x, margin), &n);
389}
390
391//
392void btSoftBody::appendLink(int model, Material* mat)
393{
394 Link l;
395 if (model >= 0)
396 l = m_links[model];
397 else
398 {
400 l.m_material = mat ? mat : m_materials[0];
401 }
403}
404
405//
407 int node1,
408 Material* mat,
409 bool bcheckexist)
410{
411 appendLink(&m_nodes[node0], &m_nodes[node1], mat, bcheckexist);
412}
413
414//
416 Node* node1,
417 Material* mat,
418 bool bcheckexist)
419{
420 if ((!bcheckexist) || (!checkLink(node0, node1)))
421 {
422 appendLink(-1, mat);
423 Link& l = m_links[m_links.size() - 1];
424 l.m_n[0] = node0;
425 l.m_n[1] = node1;
426 l.m_rl = (l.m_n[0]->m_x - l.m_n[1]->m_x).length();
427 m_bUpdateRtCst = true;
428 }
429}
430
431//
432void btSoftBody::appendFace(int model, Material* mat)
433{
434 Face f;
435 if (model >= 0)
436 {
437 f = m_faces[model];
438 }
439 else
440 {
442 f.m_material = mat ? mat : m_materials[0];
443 }
445}
446
447//
448void btSoftBody::appendFace(int node0, int node1, int node2, Material* mat)
449{
450 if (node0 == node1)
451 return;
452 if (node1 == node2)
453 return;
454 if (node2 == node0)
455 return;
456
457 appendFace(-1, mat);
458 Face& f = m_faces[m_faces.size() - 1];
459 btAssert(node0 != node1);
460 btAssert(node1 != node2);
461 btAssert(node2 != node0);
462 f.m_n[0] = &m_nodes[node0];
463 f.m_n[1] = &m_nodes[node1];
464 f.m_n[2] = &m_nodes[node2];
465 f.m_ra = AreaOf(f.m_n[0]->m_x,
466 f.m_n[1]->m_x,
467 f.m_n[2]->m_x);
468 m_bUpdateRtCst = true;
469}
470
471//
473{
474 Tetra t;
475 if (model >= 0)
476 t = m_tetras[model];
477 else
478 {
480 t.m_material = mat ? mat : m_materials[0];
481 }
483}
484
485//
487 int node1,
488 int node2,
489 int node3,
490 Material* mat)
491{
492 appendTetra(-1, mat);
493 Tetra& t = m_tetras[m_tetras.size() - 1];
494 t.m_n[0] = &m_nodes[node0];
495 t.m_n[1] = &m_nodes[node1];
496 t.m_n[2] = &m_nodes[node2];
497 t.m_n[3] = &m_nodes[node3];
498 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);
499 m_bUpdateRtCst = true;
500}
501
502//
503
504void btSoftBody::appendAnchor(int node, btRigidBody* body, bool disableCollisionBetweenLinkedBodies, btScalar influence)
505{
506 btVector3 local = body->getWorldTransform().inverse() * m_nodes[node].m_x;
507 appendAnchor(node, body, local, disableCollisionBetweenLinkedBodies, influence);
508}
509
510//
511void btSoftBody::appendAnchor(int node, btRigidBody* body, const btVector3& localPivot, bool disableCollisionBetweenLinkedBodies, btScalar influence)
512{
513 if (disableCollisionBetweenLinkedBodies)
514 {
516 {
518 }
519 }
520
521 Anchor a;
522 a.m_node = &m_nodes[node];
523 a.m_body = body;
524 a.m_local = localPivot;
525 a.m_node->m_battach = 1;
526 a.m_influence = influence;
528}
529
530//
532{
534 btSoftBody::Node& n = m_nodes[node];
535 const btScalar ima = n.m_im;
536 const btScalar imb = body->getInvMass();
537 btVector3 nrm;
538 const btCollisionShape* shp = body->getCollisionShape();
539 const btTransform& wtr = body->getWorldTransform();
540 btScalar dst =
542 wtr.invXform(m_nodes[node].m_x),
543 shp,
544 nrm,
545 0);
546
547 c.m_cti.m_colObj = body;
548 c.m_cti.m_normal = wtr.getBasis() * nrm;
549 c.m_cti.m_offset = dst;
550 c.m_node = &m_nodes[node];
551 const btScalar fc = m_cfg.kDF * body->getFriction();
552 c.m_c2 = ima;
553 c.m_c3 = fc;
555 static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0);
556 const btMatrix3x3& iwi = body->getInvInertiaTensorWorld();
557 const btVector3 ra = n.m_x - wtr.getOrigin();
558
559 c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra);
560 c.m_c1 = ra;
561 c.m_local = body->getWorldTransform().inverse() * m_nodes[node].m_x;
562 c.m_node->m_battach = 1;
563 m_deformableAnchors.push_back(c);
564}
565
567{
568 const btSoftBody::Node& n = m_nodes[node];
569 for (int i = 0; i < m_deformableAnchors.size();)
570 {
572 if (c.m_node == &n)
573 {
574 m_deformableAnchors.removeAtIndex(i);
575 }
576 else
577 {
578 i++;
579 }
580 }
581}
582
583//
585{
587 btSoftBody::Node& n = m_nodes[node];
588 const btScalar ima = n.m_im;
589 btVector3 nrm;
590 const btCollisionShape* shp = link->getCollisionShape();
591 const btTransform& wtr = link->getWorldTransform();
592 btScalar dst =
594 wtr.invXform(m_nodes[node].m_x),
595 shp,
596 nrm,
597 0);
598 c.m_cti.m_colObj = link;
599 c.m_cti.m_normal = wtr.getBasis() * nrm;
600 c.m_cti.m_offset = dst;
601 c.m_node = &m_nodes[node];
602 const btScalar fc = m_cfg.kDF * link->getFriction();
603 c.m_c2 = ima;
604 c.m_c3 = fc;
606 btVector3 normal = c.m_cti.m_normal;
608 btVector3 t2 = btCross(normal, t1);
609 btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2;
610 findJacobian(link, jacobianData_normal, c.m_node->m_x, normal);
611 findJacobian(link, jacobianData_t1, c.m_node->m_x, t1);
612 findJacobian(link, jacobianData_t2, c.m_node->m_x, t2);
613
614 btScalar* J_n = &jacobianData_normal.m_jacobians[0];
615 btScalar* J_t1 = &jacobianData_t1.m_jacobians[0];
616 btScalar* J_t2 = &jacobianData_t2.m_jacobians[0];
617
618 btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
619 btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
620 btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
621
622 btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(),
623 t1.getX(), t1.getY(), t1.getZ(),
624 t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame
625 const int ndof = link->m_multiBody->getNumDofs() + 6;
626 btMatrix3x3 local_impulse_matrix = (Diagonal(n.m_im) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse();
627 c.m_c0 = rot.transpose() * local_impulse_matrix * rot;
628 c.jacobianData_normal = jacobianData_normal;
629 c.jacobianData_t1 = jacobianData_t1;
630 c.jacobianData_t2 = jacobianData_t2;
631 c.t1 = t1;
632 c.t2 = t2;
633 const btVector3 ra = n.m_x - wtr.getOrigin();
634 c.m_c1 = ra;
635 c.m_local = link->getWorldTransform().inverse() * m_nodes[node].m_x;
636 c.m_node->m_battach = 1;
637 m_deformableAnchors.push_back(c);
638}
639//
641{
642 LJoint* pj = new (btAlignedAlloc(sizeof(LJoint), 16)) LJoint();
643 pj->m_bodies[0] = body0;
644 pj->m_bodies[1] = body1;
645 pj->m_refs[0] = pj->m_bodies[0].xform().inverse() * specs.position;
646 pj->m_refs[1] = pj->m_bodies[1].xform().inverse() * specs.position;
647 pj->m_cfm = specs.cfm;
648 pj->m_erp = specs.erp;
649 pj->m_split = specs.split;
651}
652
653//
655{
656 appendLinearJoint(specs, m_clusters[0], body);
657}
658
659//
661{
662 appendLinearJoint(specs, m_clusters[0], body->m_clusters[0]);
663}
664
665//
667{
668 AJoint* pj = new (btAlignedAlloc(sizeof(AJoint), 16)) AJoint();
669 pj->m_bodies[0] = body0;
670 pj->m_bodies[1] = body1;
671 pj->m_refs[0] = pj->m_bodies[0].xform().inverse().getBasis() * specs.axis;
672 pj->m_refs[1] = pj->m_bodies[1].xform().inverse().getBasis() * specs.axis;
673 pj->m_cfm = specs.cfm;
674 pj->m_erp = specs.erp;
675 pj->m_split = specs.split;
676 pj->m_icontrol = specs.icontrol;
678}
679
680//
682{
683 appendAngularJoint(specs, m_clusters[0], body);
684}
685
686//
688{
689 appendAngularJoint(specs, m_clusters[0], body->m_clusters[0]);
690}
691
692//
694{
695 for (int i = 0, ni = m_nodes.size(); i < ni; ++i) addForce(force, i);
696}
697
698//
699void btSoftBody::addForce(const btVector3& force, int node)
700{
701 Node& n = m_nodes[node];
702 if (n.m_im > 0)
703 {
704 n.m_f += force;
705 }
706}
707
708void btSoftBody::addAeroForceToNode(const btVector3& windVelocity, int nodeIndex)
709{
710 btAssert(nodeIndex >= 0 && nodeIndex < m_nodes.size());
711
712 const btScalar dt = m_sst.sdt;
713 const btScalar kLF = m_cfg.kLF;
714 const btScalar kDG = m_cfg.kDG;
715 //const btScalar kPR = m_cfg.kPR;
716 //const btScalar kVC = m_cfg.kVC;
717 const bool as_lift = kLF > 0;
718 const bool as_drag = kDG > 0;
719 const bool as_aero = as_lift || as_drag;
720 const bool as_vaero = as_aero && (m_cfg.aeromodel < btSoftBody::eAeroModel::F_TwoSided);
721
722 Node& n = m_nodes[nodeIndex];
723
724 if (n.m_im > 0)
725 {
726 btSoftBody::sMedium medium;
727
728 EvaluateMedium(m_worldInfo, n.m_x, medium);
729 medium.m_velocity = windVelocity;
731
732 /* Aerodynamics */
733 if (as_vaero)
734 {
735 const btVector3 rel_v = n.m_v - medium.m_velocity;
736 const btScalar rel_v_len = rel_v.length();
737 const btScalar rel_v2 = rel_v.length2();
738
739 if (rel_v2 > SIMD_EPSILON)
740 {
741 const btVector3 rel_v_nrm = rel_v.normalized();
742 btVector3 nrm = n.m_n;
743
745 {
746 nrm *= (btScalar)((btDot(nrm, rel_v) < 0) ? -1 : +1);
747 btVector3 fDrag(0, 0, 0);
748 btVector3 fLift(0, 0, 0);
749
750 btScalar n_dot_v = nrm.dot(rel_v_nrm);
751 btScalar tri_area = 0.5f * n.m_area;
752
753 fDrag = 0.5f * kDG * medium.m_density * rel_v2 * tri_area * n_dot_v * (-rel_v_nrm);
754
755 // Check angle of attack
756 // cos(10º) = 0.98480
757 if (0 < n_dot_v && n_dot_v < 0.98480f)
758 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));
759
760 // Check if the velocity change resulted by aero drag force exceeds the current velocity of the node.
761 btVector3 del_v_by_fDrag = fDrag * n.m_im * m_sst.sdt;
762 btScalar del_v_by_fDrag_len2 = del_v_by_fDrag.length2();
763 btScalar v_len2 = n.m_v.length2();
764
765 if (del_v_by_fDrag_len2 >= v_len2 && del_v_by_fDrag_len2 > 0)
766 {
767 btScalar del_v_by_fDrag_len = del_v_by_fDrag.length();
768 btScalar v_len = n.m_v.length();
769 fDrag *= btScalar(0.8) * (v_len / del_v_by_fDrag_len);
770 }
771
772 n.m_f += fDrag;
773 n.m_f += fLift;
774 }
776 {
778 nrm *= (btScalar)((btDot(nrm, rel_v) < 0) ? -1 : +1);
779
780 const btScalar dvn = btDot(rel_v, nrm);
781 /* Compute forces */
782 if (dvn > 0)
783 {
784 btVector3 force(0, 0, 0);
785 const btScalar c0 = n.m_area * dvn * rel_v2 / 2;
786 const btScalar c1 = c0 * medium.m_density;
787 force += nrm * (-c1 * kLF);
788 force += rel_v.normalized() * (-c1 * kDG);
789 ApplyClampedForce(n, force, dt);
790 }
791 }
792 }
793 }
794 }
795}
796
797void btSoftBody::addAeroForceToFace(const btVector3& windVelocity, int faceIndex)
798{
799 const btScalar dt = m_sst.sdt;
800 const btScalar kLF = m_cfg.kLF;
801 const btScalar kDG = m_cfg.kDG;
802 // const btScalar kPR = m_cfg.kPR;
803 // const btScalar kVC = m_cfg.kVC;
804 const bool as_lift = kLF > 0;
805 const bool as_drag = kDG > 0;
806 const bool as_aero = as_lift || as_drag;
807 const bool as_faero = as_aero && (m_cfg.aeromodel >= btSoftBody::eAeroModel::F_TwoSided);
808
809 if (as_faero)
810 {
811 btSoftBody::Face& f = m_faces[faceIndex];
812
813 btSoftBody::sMedium medium;
814
815 const btVector3 v = (f.m_n[0]->m_v + f.m_n[1]->m_v + f.m_n[2]->m_v) / 3;
816 const btVector3 x = (f.m_n[0]->m_x + f.m_n[1]->m_x + f.m_n[2]->m_x) / 3;
817 EvaluateMedium(m_worldInfo, x, medium);
818 medium.m_velocity = windVelocity;
820 const btVector3 rel_v = v - medium.m_velocity;
821 const btScalar rel_v_len = rel_v.length();
822 const btScalar rel_v2 = rel_v.length2();
823
824 if (rel_v2 > SIMD_EPSILON)
825 {
826 const btVector3 rel_v_nrm = rel_v.normalized();
827 btVector3 nrm = f.m_normal;
828
830 {
831 nrm *= (btScalar)((btDot(nrm, rel_v) < 0) ? -1 : +1);
832
833 btVector3 fDrag(0, 0, 0);
834 btVector3 fLift(0, 0, 0);
835
836 btScalar n_dot_v = nrm.dot(rel_v_nrm);
837 btScalar tri_area = 0.5f * f.m_ra;
838
839 fDrag = 0.5f * kDG * medium.m_density * rel_v2 * tri_area * n_dot_v * (-rel_v_nrm);
840
841 // Check angle of attack
842 // cos(10º) = 0.98480
843 if (0 < n_dot_v && n_dot_v < 0.98480f)
844 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));
845
846 fDrag /= 3;
847 fLift /= 3;
848
849 for (int j = 0; j < 3; ++j)
850 {
851 if (f.m_n[j]->m_im > 0)
852 {
853 // Check if the velocity change resulted by aero drag force exceeds the current velocity of the node.
854 btVector3 del_v_by_fDrag = fDrag * f.m_n[j]->m_im * m_sst.sdt;
855 btScalar del_v_by_fDrag_len2 = del_v_by_fDrag.length2();
856 btScalar v_len2 = f.m_n[j]->m_v.length2();
857
858 if (del_v_by_fDrag_len2 >= v_len2 && del_v_by_fDrag_len2 > 0)
859 {
860 btScalar del_v_by_fDrag_len = del_v_by_fDrag.length();
861 btScalar v_len = f.m_n[j]->m_v.length();
862 fDrag *= btScalar(0.8) * (v_len / del_v_by_fDrag_len);
863 }
864
865 f.m_n[j]->m_f += fDrag;
866 f.m_n[j]->m_f += fLift;
867 }
868 }
869 }
871 {
873 nrm *= (btScalar)((btDot(nrm, rel_v) < 0) ? -1 : +1);
874
875 const btScalar dvn = btDot(rel_v, nrm);
876 /* Compute forces */
877 if (dvn > 0)
878 {
879 btVector3 force(0, 0, 0);
880 const btScalar c0 = f.m_ra * dvn * rel_v2;
881 const btScalar c1 = c0 * medium.m_density;
882 force += nrm * (-c1 * kLF);
883 force += rel_v.normalized() * (-c1 * kDG);
884 force /= 3;
885 for (int j = 0; j < 3; ++j) ApplyClampedForce(*f.m_n[j], force, dt);
886 }
887 }
888 }
889 }
890}
891
892//
894{
895 for (int i = 0, ni = m_nodes.size(); i < ni; ++i) addVelocity(velocity, i);
896}
897
898/* Set velocity for the entire body */
900{
901 for (int i = 0, ni = m_nodes.size(); i < ni; ++i)
902 {
903 Node& n = m_nodes[i];
904 if (n.m_im > 0)
905 {
906 n.m_v = velocity;
907 n.m_vn = velocity;
908 }
909 }
910}
911
912//
913void btSoftBody::addVelocity(const btVector3& velocity, int node)
914{
915 Node& n = m_nodes[node];
916 if (n.m_im > 0)
917 {
918 n.m_v += velocity;
919 }
920}
921
922//
923void btSoftBody::setMass(int node, btScalar mass)
924{
925 m_nodes[node].m_im = mass > 0 ? 1 / mass : 0;
926 m_bUpdateRtCst = true;
927}
928
929//
931{
932 return (m_nodes[node].m_im > 0 ? 1 / m_nodes[node].m_im : 0);
933}
934
935//
937{
938 btScalar mass = 0;
939 for (int i = 0; i < m_nodes.size(); ++i)
940 {
941 mass += getMass(i);
942 }
943 return (mass);
944}
945
946//
947void btSoftBody::setTotalMass(btScalar mass, bool fromfaces)
948{
949 int i;
950
951 if (fromfaces)
952 {
953 for (i = 0; i < m_nodes.size(); ++i)
954 {
955 m_nodes[i].m_im = 0;
956 }
957 for (i = 0; i < m_faces.size(); ++i)
958 {
959 const Face& f = m_faces[i];
960 const btScalar twicearea = AreaOf(f.m_n[0]->m_x,
961 f.m_n[1]->m_x,
962 f.m_n[2]->m_x);
963 for (int j = 0; j < 3; ++j)
964 {
965 f.m_n[j]->m_im += twicearea;
966 }
967 }
968 for (i = 0; i < m_nodes.size(); ++i)
969 {
970 m_nodes[i].m_im = 1 / m_nodes[i].m_im;
971 }
972 }
973 const btScalar tm = getTotalMass();
974 const btScalar itm = 1 / tm;
975 for (i = 0; i < m_nodes.size(); ++i)
976 {
977 m_nodes[i].m_im /= itm * mass;
978 }
979 m_bUpdateRtCst = true;
980}
981
982//
984{
985 setTotalMass(getVolume() * density, true);
986}
987
988//
990{
992 ranks.resize(m_nodes.size(), 0);
993 int i;
994
995 for (i = 0; i < m_nodes.size(); ++i)
996 {
997 m_nodes[i].m_im = 0;
998 }
999 for (i = 0; i < m_tetras.size(); ++i)
1000 {
1001 const Tetra& t = m_tetras[i];
1002 for (int j = 0; j < 4; ++j)
1003 {
1004 t.m_n[j]->m_im += btFabs(t.m_rv);
1005 ranks[int(t.m_n[j] - &m_nodes[0])] += 1;
1006 }
1007 }
1008 for (i = 0; i < m_nodes.size(); ++i)
1009 {
1010 if (m_nodes[i].m_im > 0)
1011 {
1012 m_nodes[i].m_im = ranks[i] / m_nodes[i].m_im;
1013 }
1014 }
1015 setTotalMass(mass, false);
1016}
1017
1018//
1020{
1021 btScalar volume = 0;
1022 for (int i = 0; i < m_tetras.size(); ++i)
1023 {
1024 const Tetra& t = m_tetras[i];
1025 for (int j = 0; j < 4; ++j)
1026 {
1027 volume += btFabs(t.m_rv);
1028 }
1029 }
1030 setVolumeMass(volume * density / 6);
1031}
1032
1033//
1035{
1036 btVector3 total_momentum = btVector3(0, 0, 0);
1037 for (int i = 0; i < m_nodes.size(); ++i)
1038 {
1039 btScalar mass = m_nodes[i].m_im == 0 ? 0 : 1.0 / m_nodes[i].m_im;
1040 total_momentum += mass * m_nodes[i].m_v;
1041 }
1042 btScalar total_mass = getTotalMass();
1043 return total_mass == 0 ? total_momentum : total_momentum / total_mass;
1044}
1045
1046//
1048{
1049 btVector3 old_vel = getLinearVelocity();
1050 btVector3 diff = linVel - old_vel;
1051 for (int i = 0; i < m_nodes.size(); ++i)
1052 m_nodes[i].m_v += diff;
1053}
1054
1055//
1057{
1058 btVector3 old_vel = getLinearVelocity();
1059 btVector3 com = getCenterOfMass();
1060 for (int i = 0; i < m_nodes.size(); ++i)
1061 {
1062 m_nodes[i].m_v = angVel.cross(m_nodes[i].m_x - com) + old_vel;
1063 }
1064}
1065
1066//
1068{
1070 btMatrix3x3 S;
1071 S.setZero();
1072 // Get rotation that minimizes L2 difference: \sum_i || RX_i + t - x_i ||
1073 // It's important to make sure that S has the correct signs.
1074 // SVD is only unique up to the ordering of singular values.
1075 // SVD will manipulate U and V to ensure the ordering of singular values. If all three singular
1076 // vaues are negative, SVD will permute colums of U to make two of them positive.
1077 for (int i = 0; i < m_nodes.size(); ++i)
1078 {
1079 S -= OuterProduct(m_X[i], t - m_nodes[i].m_x);
1080 }
1081 btVector3 sigma;
1082 btMatrix3x3 U, V;
1083 singularValueDecomposition(S, U, sigma, V);
1084 btMatrix3x3 R = V * U.transpose();
1085 btTransform trs;
1086 trs.setIdentity();
1087 trs.setOrigin(t);
1088 trs.setBasis(R);
1089 return trs;
1090}
1091
1092//
1094{
1095 // get the current best rigid fit
1096 btTransform current_transform = getRigidTransform();
1097 // apply transform in material space
1098 btTransform new_transform = trs * current_transform.inverse();
1099 transform(new_transform);
1100}
1101
1102//
1104{
1105 const btScalar margin = getCollisionShape()->getMargin();
1107 vol;
1108
1109 for (int i = 0, ni = m_nodes.size(); i < ni; ++i)
1110 {
1111 Node& n = m_nodes[i];
1112 n.m_x = trs * n.m_x;
1113 n.m_q = trs * n.m_q;
1114 n.m_n = trs.getBasis() * n.m_n;
1115 vol = btDbvtVolume::FromCR(n.m_x, margin);
1116
1117 m_ndbvt.update(n.m_leaf, vol);
1118 }
1119 updateNormals();
1120 updateBounds();
1122}
1123
1124//
1126{
1127 btTransform t;
1128 t.setIdentity();
1129 t.setOrigin(trs);
1130 transform(t);
1131}
1132
1133//
1135{
1136 btTransform t;
1137 t.setIdentity();
1138 t.setRotation(rot);
1139 transform(t);
1140}
1141
1142//
1144{
1145 const btScalar margin = getCollisionShape()->getMargin();
1147 vol;
1148
1149 for (int i = 0, ni = m_nodes.size(); i < ni; ++i)
1150 {
1151 Node& n = m_nodes[i];
1152 n.m_x *= scl;
1153 n.m_q *= scl;
1154 vol = btDbvtVolume::FromCR(n.m_x, margin);
1155 m_ndbvt.update(n.m_leaf, vol);
1156 }
1157 updateNormals();
1158 updateBounds();
1161}
1162
1163//
1168
1169//
1171{
1172 for (int i = 0, ni = m_links.size(); i < ni; ++i)
1173 {
1174 Link& l = m_links[i];
1175 l.m_rl = l.m_rl / m_restLengthScale * restLengthScale;
1176 l.m_c1 = l.m_rl * l.m_rl;
1177 }
1178 m_restLengthScale = restLengthScale;
1179
1181 activate();
1182}
1183
1184//
1185void btSoftBody::setPose(bool bvolume, bool bframe)
1186{
1187 m_pose.m_bvolume = bvolume;
1188 m_pose.m_bframe = bframe;
1189 int i, ni;
1190
1191 /* Weights */
1192 const btScalar omass = getTotalMass();
1193 const btScalar kmass = omass * m_nodes.size() * 1000;
1194 btScalar tmass = omass;
1196 for (i = 0, ni = m_nodes.size(); i < ni; ++i)
1197 {
1198 if (m_nodes[i].m_im <= 0) tmass += kmass;
1199 }
1200 for (i = 0, ni = m_nodes.size(); i < ni; ++i)
1201 {
1202 Node& n = m_nodes[i];
1203 m_pose.m_wgh[i] = n.m_im > 0 ? 1 / (m_nodes[i].m_im * tmass) : kmass / tmass;
1204 }
1205 /* Pos */
1206 const btVector3 com = evaluateCom();
1208 for (i = 0, ni = m_nodes.size(); i < ni; ++i)
1209 {
1210 m_pose.m_pos[i] = m_nodes[i].m_x - com;
1211 }
1212 m_pose.m_volume = bvolume ? getVolume() : 0;
1213 m_pose.m_com = com;
1216 /* Aqq */
1217 m_pose.m_aqq[0] =
1218 m_pose.m_aqq[1] =
1219 m_pose.m_aqq[2] = btVector3(0, 0, 0);
1220 for (i = 0, ni = m_nodes.size(); i < ni; ++i)
1221 {
1222 const btVector3& q = m_pose.m_pos[i];
1223 const btVector3 mq = m_pose.m_wgh[i] * q;
1224 m_pose.m_aqq[0] += mq.x() * q;
1225 m_pose.m_aqq[1] += mq.y() * q;
1226 m_pose.m_aqq[2] += mq.z() * q;
1227 }
1229
1231}
1232
1234{
1235 for (int i = 0, ni = m_links.size(); i < ni; ++i)
1236 {
1237 Link& l = m_links[i];
1238 l.m_rl = (l.m_n[0]->m_x - l.m_n[1]->m_x).length();
1239 l.m_c1 = l.m_rl * l.m_rl;
1240 }
1241}
1242
1243//
1245{
1246 btScalar vol = 0;
1247 if (m_nodes.size() > 0)
1248 {
1249 int i, ni;
1250
1251 const btVector3 org = m_nodes[0].m_x;
1252 for (i = 0, ni = m_faces.size(); i < ni; ++i)
1253 {
1254 const Face& f = m_faces[i];
1255 vol += btDot(f.m_n[0]->m_x - org, btCross(f.m_n[1]->m_x - org, f.m_n[2]->m_x - org));
1256 }
1257 vol /= (btScalar)6;
1258 }
1259 return (vol);
1260}
1261
1262//
1264{
1265 return (m_clusters.size());
1266}
1267
1268//
1270{
1271 btVector3 com(0, 0, 0);
1272 for (int i = 0, ni = cluster->m_nodes.size(); i < ni; ++i)
1273 {
1274 com += cluster->m_nodes[i]->m_x * cluster->m_masses[i];
1275 }
1276 return (com * cluster->m_imass);
1277}
1278
1279//
1281{
1282 return (clusterCom(m_clusters[cluster]));
1283}
1284
1285//
1287{
1288 return (cluster->m_lv + btCross(cluster->m_av, rpos));
1289}
1290
1291//
1292void btSoftBody::clusterVImpulse(Cluster* cluster, const btVector3& rpos, const btVector3& impulse)
1293{
1294 const btVector3 li = cluster->m_imass * impulse;
1295 const btVector3 ai = cluster->m_invwi * btCross(rpos, impulse);
1296 cluster->m_vimpulses[0] += li;
1297 cluster->m_lv += li;
1298 cluster->m_vimpulses[1] += ai;
1299 cluster->m_av += ai;
1300 cluster->m_nvimpulses++;
1301}
1302
1303//
1304void btSoftBody::clusterDImpulse(Cluster* cluster, const btVector3& rpos, const btVector3& impulse)
1305{
1306 const btVector3 li = cluster->m_imass * impulse;
1307 const btVector3 ai = cluster->m_invwi * btCross(rpos, impulse);
1308 cluster->m_dimpulses[0] += li;
1309 cluster->m_dimpulses[1] += ai;
1310 cluster->m_ndimpulses++;
1311}
1312
1313//
1314void btSoftBody::clusterImpulse(Cluster* cluster, const btVector3& rpos, const Impulse& impulse)
1315{
1316 if (impulse.m_asVelocity) clusterVImpulse(cluster, rpos, impulse.m_velocity);
1317 if (impulse.m_asDrift) clusterDImpulse(cluster, rpos, impulse.m_drift);
1318}
1319
1320//
1321void btSoftBody::clusterVAImpulse(Cluster* cluster, const btVector3& impulse)
1322{
1323 const btVector3 ai = cluster->m_invwi * impulse;
1324 cluster->m_vimpulses[1] += ai;
1325 cluster->m_av += ai;
1326 cluster->m_nvimpulses++;
1327}
1328
1329//
1330void btSoftBody::clusterDAImpulse(Cluster* cluster, const btVector3& impulse)
1331{
1332 const btVector3 ai = cluster->m_invwi * impulse;
1333 cluster->m_dimpulses[1] += ai;
1334 cluster->m_ndimpulses++;
1335}
1336
1337//
1338void btSoftBody::clusterAImpulse(Cluster* cluster, const Impulse& impulse)
1339{
1340 if (impulse.m_asVelocity) clusterVAImpulse(cluster, impulse.m_velocity);
1341 if (impulse.m_asDrift) clusterDAImpulse(cluster, impulse.m_drift);
1342}
1343
1344//
1345void btSoftBody::clusterDCImpulse(Cluster* cluster, const btVector3& impulse)
1346{
1347 cluster->m_dimpulses[0] += impulse * cluster->m_imass;
1348 cluster->m_ndimpulses++;
1349}
1350
1355
1356//
1358{
1359 int i, j;
1360
1361 if (distance > 1)
1362 {
1363 /* Build graph */
1364 const int n = m_nodes.size();
1365 const unsigned inf = (~(unsigned)0) >> 1;
1366 unsigned* adj = new unsigned[n * n];
1367
1368#define IDX(_x_, _y_) ((_y_)*n + (_x_))
1369 for (j = 0; j < n; ++j)
1370 {
1371 for (i = 0; i < n; ++i)
1372 {
1373 if (i != j)
1374 {
1375 adj[IDX(i, j)] = adj[IDX(j, i)] = inf;
1376 }
1377 else
1378 {
1379 adj[IDX(i, j)] = adj[IDX(j, i)] = 0;
1380 }
1381 }
1382 }
1383 for (i = 0; i < m_links.size(); ++i)
1384 {
1385 const int ia = (int)(m_links[i].m_n[0] - &m_nodes[0]);
1386 const int ib = (int)(m_links[i].m_n[1] - &m_nodes[0]);
1387 adj[IDX(ia, ib)] = 1;
1388 adj[IDX(ib, ia)] = 1;
1389 }
1390
1391 //special optimized case for distance == 2
1392 if (distance == 2)
1393 {
1395
1396 /* Build node links */
1397 nodeLinks.resize(m_nodes.size());
1398
1399 for (i = 0; i < m_links.size(); ++i)
1400 {
1401 const int ia = (int)(m_links[i].m_n[0] - &m_nodes[0]);
1402 const int ib = (int)(m_links[i].m_n[1] - &m_nodes[0]);
1403 if (nodeLinks[ia].m_links.findLinearSearch(ib) == nodeLinks[ia].m_links.size())
1404 nodeLinks[ia].m_links.push_back(ib);
1405
1406 if (nodeLinks[ib].m_links.findLinearSearch(ia) == nodeLinks[ib].m_links.size())
1407 nodeLinks[ib].m_links.push_back(ia);
1408 }
1409 for (int ii = 0; ii < nodeLinks.size(); ii++)
1410 {
1411 int i = ii;
1412
1413 for (int jj = 0; jj < nodeLinks[ii].m_links.size(); jj++)
1414 {
1415 int k = nodeLinks[ii].m_links[jj];
1416 for (int kk = 0; kk < nodeLinks[k].m_links.size(); kk++)
1417 {
1418 int j = nodeLinks[k].m_links[kk];
1419 if (i != j)
1420 {
1421 const unsigned sum = adj[IDX(i, k)] + adj[IDX(k, j)];
1422 btAssert(sum == 2);
1423 if (adj[IDX(i, j)] > sum)
1424 {
1425 adj[IDX(i, j)] = adj[IDX(j, i)] = sum;
1426 }
1427 }
1428 }
1429 }
1430 }
1431 }
1432 else
1433 {
1435 for (int k = 0; k < n; ++k)
1436 {
1437 for (j = 0; j < n; ++j)
1438 {
1439 for (i = j + 1; i < n; ++i)
1440 {
1441 const unsigned sum = adj[IDX(i, k)] + adj[IDX(k, j)];
1442 if (adj[IDX(i, j)] > sum)
1443 {
1444 adj[IDX(i, j)] = adj[IDX(j, i)] = sum;
1445 }
1446 }
1447 }
1448 }
1449 }
1450
1451 /* Build links */
1452 int nlinks = 0;
1453 for (j = 0; j < n; ++j)
1454 {
1455 for (i = j + 1; i < n; ++i)
1456 {
1457 if (adj[IDX(i, j)] == (unsigned)distance)
1458 {
1459 appendLink(i, j, mat);
1460 m_links[m_links.size() - 1].m_bbending = 1;
1461 ++nlinks;
1462 }
1463 }
1464 }
1465 delete[] adj;
1466 return (nlinks);
1467 }
1468 return (0);
1469}
1470
1471//
1473{
1474 unsigned long seed = 243703;
1475#define NEXTRAND (seed = (1664525L * seed + 1013904223L) & 0xffffffff)
1476 int i, ni;
1477
1478 for (i = 0, ni = m_links.size(); i < ni; ++i)
1479 {
1480 btSwap(m_links[i], m_links[NEXTRAND % ni]);
1481 }
1482 for (i = 0, ni = m_faces.size(); i < ni; ++i)
1483 {
1484 btSwap(m_faces[i], m_faces[NEXTRAND % ni]);
1485 }
1486#undef NEXTRAND
1487}
1488
1490{
1491 int node_count = m_nodes.size();
1492 btAssert(node_count == q.size());
1493 btAssert(node_count == v.size());
1494 for (int i = 0; i < node_count; i++)
1495 {
1496 Node& n = m_nodes[i];
1497 n.m_x = q[i];
1498 n.m_q = q[i];
1499 n.m_v = v[i];
1500 n.m_vn = v[i];
1501 }
1502}
1503
1504//
1506{
1507 Cluster* c = m_clusters[index];
1508 if (c->m_leaf) m_cdbvt.remove(c->m_leaf);
1509 c->~Cluster();
1510 btAlignedFree(c);
1511 m_clusters.remove(c);
1512}
1513
1514//
1516{
1517 while (m_clusters.size() > 0) releaseCluster(0);
1518}
1519
1520//
1521int btSoftBody::generateClusters(int k, int maxiterations)
1522{
1523 int i;
1526 for (i = 0; i < m_clusters.size(); ++i)
1527 {
1528 m_clusters[i] = new (btAlignedAlloc(sizeof(Cluster), 16)) Cluster();
1529 m_clusters[i]->m_collide = true;
1530 }
1531 k = m_clusters.size();
1532 if (k > 0)
1533 {
1534 /* Initialize */
1536 btVector3 cog(0, 0, 0);
1537 int i;
1538 for (i = 0; i < m_nodes.size(); ++i)
1539 {
1540 cog += m_nodes[i].m_x;
1541 m_clusters[(i * 29873) % m_clusters.size()]->m_nodes.push_back(&m_nodes[i]);
1542 }
1543 cog /= (btScalar)m_nodes.size();
1544 centers.resize(k, cog);
1545 /* Iterate */
1546 const btScalar slope = 16;
1547 bool changed;
1548 int iterations = 0;
1549 do
1550 {
1551 const btScalar w = 2 - btMin<btScalar>(1, iterations / slope);
1552 changed = false;
1553 iterations++;
1554 int i;
1555
1556 for (i = 0; i < k; ++i)
1557 {
1558 btVector3 c(0, 0, 0);
1559 for (int j = 0; j < m_clusters[i]->m_nodes.size(); ++j)
1560 {
1561 c += m_clusters[i]->m_nodes[j]->m_x;
1562 }
1563 if (m_clusters[i]->m_nodes.size())
1564 {
1565 c /= (btScalar)m_clusters[i]->m_nodes.size();
1566 c = centers[i] + (c - centers[i]) * w;
1567 changed |= ((c - centers[i]).length2() > SIMD_EPSILON);
1568 centers[i] = c;
1569 m_clusters[i]->m_nodes.resize(0);
1570 }
1571 }
1572 for (i = 0; i < m_nodes.size(); ++i)
1573 {
1574 const btVector3 nx = m_nodes[i].m_x;
1575 int kbest = 0;
1576 btScalar kdist = ClusterMetric(centers[0], nx);
1577 for (int j = 1; j < k; ++j)
1578 {
1579 const btScalar d = ClusterMetric(centers[j], nx);
1580 if (d < kdist)
1581 {
1582 kbest = j;
1583 kdist = d;
1584 }
1585 }
1586 m_clusters[kbest]->m_nodes.push_back(&m_nodes[i]);
1587 }
1588 } while (changed && (iterations < maxiterations));
1589 /* Merge */
1591 cids.resize(m_nodes.size(), -1);
1592 for (i = 0; i < m_clusters.size(); ++i)
1593 {
1594 for (int j = 0; j < m_clusters[i]->m_nodes.size(); ++j)
1595 {
1596 cids[int(m_clusters[i]->m_nodes[j] - &m_nodes[0])] = i;
1597 }
1598 }
1599 for (i = 0; i < m_faces.size(); ++i)
1600 {
1601 const int idx[] = {int(m_faces[i].m_n[0] - &m_nodes[0]),
1602 int(m_faces[i].m_n[1] - &m_nodes[0]),
1603 int(m_faces[i].m_n[2] - &m_nodes[0])};
1604 for (int j = 0; j < 3; ++j)
1605 {
1606 const int cid = cids[idx[j]];
1607 for (int q = 1; q < 3; ++q)
1608 {
1609 const int kid = idx[(j + q) % 3];
1610 if (cids[kid] != cid)
1611 {
1612 if (m_clusters[cid]->m_nodes.findLinearSearch(&m_nodes[kid]) == m_clusters[cid]->m_nodes.size())
1613 {
1614 m_clusters[cid]->m_nodes.push_back(&m_nodes[kid]);
1615 }
1616 }
1617 }
1618 }
1619 }
1620 /* Master */
1621 if (m_clusters.size() > 1)
1622 {
1623 Cluster* pmaster = new (btAlignedAlloc(sizeof(Cluster), 16)) Cluster();
1624 pmaster->m_collide = false;
1625 pmaster->m_nodes.reserve(m_nodes.size());
1626 for (int i = 0; i < m_nodes.size(); ++i) pmaster->m_nodes.push_back(&m_nodes[i]);
1627 m_clusters.push_back(pmaster);
1629 }
1630 /* Terminate */
1631 for (i = 0; i < m_clusters.size(); ++i)
1632 {
1633 if (m_clusters[i]->m_nodes.size() == 0)
1634 {
1635 releaseCluster(i--);
1636 }
1637 }
1638 }
1639 else
1640 {
1641 //create a cluster for each tetrahedron (if tetrahedra exist) or each face
1642 if (m_tetras.size())
1643 {
1645 for (i = 0; i < m_clusters.size(); ++i)
1646 {
1647 m_clusters[i] = new (btAlignedAlloc(sizeof(Cluster), 16)) Cluster();
1648 m_clusters[i]->m_collide = true;
1649 }
1650 for (i = 0; i < m_tetras.size(); i++)
1651 {
1652 for (int j = 0; j < 4; j++)
1653 {
1654 m_clusters[i]->m_nodes.push_back(m_tetras[i].m_n[j]);
1655 }
1656 }
1657 }
1658 else
1659 {
1661 for (i = 0; i < m_clusters.size(); ++i)
1662 {
1663 m_clusters[i] = new (btAlignedAlloc(sizeof(Cluster), 16)) Cluster();
1664 m_clusters[i]->m_collide = true;
1665 }
1666
1667 for (i = 0; i < m_faces.size(); ++i)
1668 {
1669 for (int j = 0; j < 3; ++j)
1670 {
1671 m_clusters[i]->m_nodes.push_back(m_faces[i].m_n[j]);
1672 }
1673 }
1674 }
1675 }
1676
1677 if (m_clusters.size())
1678 {
1681
1682 //for self-collision
1684 {
1685 for (int c0 = 0; c0 < m_clusters.size(); c0++)
1686 {
1687 m_clusters[c0]->m_clusterIndex = c0;
1688 for (int c1 = 0; c1 < m_clusters.size(); c1++)
1689 {
1690 bool connected = false;
1691 Cluster* cla = m_clusters[c0];
1692 Cluster* clb = m_clusters[c1];
1693 for (int i = 0; !connected && i < cla->m_nodes.size(); i++)
1694 {
1695 for (int j = 0; j < clb->m_nodes.size(); j++)
1696 {
1697 if (cla->m_nodes[i] == clb->m_nodes[j])
1698 {
1699 connected = true;
1700 break;
1701 }
1702 }
1703 }
1704 m_clusterConnectivity[c0 + c1 * m_clusters.size()] = connected;
1705 }
1706 }
1707 }
1708 }
1709
1710 return (m_clusters.size());
1711}
1712
1713//
1714void btSoftBody::refine(ImplicitFn* ifn, btScalar accurary, bool cut)
1715{
1716 const Node* nbase = &m_nodes[0];
1717 int ncount = m_nodes.size();
1718 btSymMatrix<int> edges(ncount, -2);
1719 int newnodes = 0;
1720 int i, j, k, ni;
1721
1722 /* Filter out */
1723 for (i = 0; i < m_links.size(); ++i)
1724 {
1725 Link& l = m_links[i];
1726 if (l.m_bbending)
1727 {
1728 if (!SameSign(ifn->Eval(l.m_n[0]->m_x), ifn->Eval(l.m_n[1]->m_x)))
1729 {
1730 btSwap(m_links[i], m_links[m_links.size() - 1]);
1731 m_links.pop_back();
1732 --i;
1733 }
1734 }
1735 }
1736 /* Fill edges */
1737 for (i = 0; i < m_links.size(); ++i)
1738 {
1739 Link& l = m_links[i];
1740 edges(int(l.m_n[0] - nbase), int(l.m_n[1] - nbase)) = -1;
1741 }
1742 for (i = 0; i < m_faces.size(); ++i)
1743 {
1744 Face& f = m_faces[i];
1745 edges(int(f.m_n[0] - nbase), int(f.m_n[1] - nbase)) = -1;
1746 edges(int(f.m_n[1] - nbase), int(f.m_n[2] - nbase)) = -1;
1747 edges(int(f.m_n[2] - nbase), int(f.m_n[0] - nbase)) = -1;
1748 }
1749 /* Intersect */
1750 for (i = 0; i < ncount; ++i)
1751 {
1752 for (j = i + 1; j < ncount; ++j)
1753 {
1754 if (edges(i, j) == -1)
1755 {
1756 Node& a = m_nodes[i];
1757 Node& b = m_nodes[j];
1758 const btScalar t = ImplicitSolve(ifn, a.m_x, b.m_x, accurary);
1759 if (t > 0)
1760 {
1761 const btVector3 x = Lerp(a.m_x, b.m_x, t);
1762 const btVector3 v = Lerp(a.m_v, b.m_v, t);
1763 btScalar m = 0;
1764 if (a.m_im > 0)
1765 {
1766 if (b.m_im > 0)
1767 {
1768 const btScalar ma = 1 / a.m_im;
1769 const btScalar mb = 1 / b.m_im;
1770 const btScalar mc = Lerp(ma, mb, t);
1771 const btScalar f = (ma + mb) / (ma + mb + mc);
1772 a.m_im = 1 / (ma * f);
1773 b.m_im = 1 / (mb * f);
1774 m = mc * f;
1775 }
1776 else
1777 {
1778 a.m_im /= 0.5f;
1779 m = 1 / a.m_im;
1780 }
1781 }
1782 else
1783 {
1784 if (b.m_im > 0)
1785 {
1786 b.m_im /= 0.5f;
1787 m = 1 / b.m_im;
1788 }
1789 else
1790 m = 0;
1791 }
1792 appendNode(x, m);
1793 edges(i, j) = m_nodes.size() - 1;
1794 m_nodes[edges(i, j)].m_v = v;
1795 ++newnodes;
1796 }
1797 }
1798 }
1799 }
1800 nbase = &m_nodes[0];
1801 /* Refine links */
1802 for (i = 0, ni = m_links.size(); i < ni; ++i)
1803 {
1804 Link& feat = m_links[i];
1805 const int idx[] = {int(feat.m_n[0] - nbase),
1806 int(feat.m_n[1] - nbase)};
1807 if ((idx[0] < ncount) && (idx[1] < ncount))
1808 {
1809 const int ni = edges(idx[0], idx[1]);
1810 if (ni > 0)
1811 {
1812 appendLink(i);
1813 Link* pft[] = {&m_links[i],
1814 &m_links[m_links.size() - 1]};
1815 pft[0]->m_n[0] = &m_nodes[idx[0]];
1816 pft[0]->m_n[1] = &m_nodes[ni];
1817 pft[1]->m_n[0] = &m_nodes[ni];
1818 pft[1]->m_n[1] = &m_nodes[idx[1]];
1819 }
1820 }
1821 }
1822 /* Refine faces */
1823 for (i = 0; i < m_faces.size(); ++i)
1824 {
1825 const Face& feat = m_faces[i];
1826 const int idx[] = {int(feat.m_n[0] - nbase),
1827 int(feat.m_n[1] - nbase),
1828 int(feat.m_n[2] - nbase)};
1829 for (j = 2, k = 0; k < 3; j = k++)
1830 {
1831 if ((idx[j] < ncount) && (idx[k] < ncount))
1832 {
1833 const int ni = edges(idx[j], idx[k]);
1834 if (ni > 0)
1835 {
1836 appendFace(i);
1837 const int l = (k + 1) % 3;
1838 Face* pft[] = {&m_faces[i],
1839 &m_faces[m_faces.size() - 1]};
1840 pft[0]->m_n[0] = &m_nodes[idx[l]];
1841 pft[0]->m_n[1] = &m_nodes[idx[j]];
1842 pft[0]->m_n[2] = &m_nodes[ni];
1843 pft[1]->m_n[0] = &m_nodes[ni];
1844 pft[1]->m_n[1] = &m_nodes[idx[k]];
1845 pft[1]->m_n[2] = &m_nodes[idx[l]];
1846 appendLink(ni, idx[l], pft[0]->m_material);
1847 --i;
1848 break;
1849 }
1850 }
1851 }
1852 }
1853 /* Cut */
1854 if (cut)
1855 {
1857 const int pcount = ncount;
1858 int i;
1859 ncount = m_nodes.size();
1860 cnodes.resize(ncount, 0);
1861 /* Nodes */
1862 for (i = 0; i < ncount; ++i)
1863 {
1864 const btVector3 x = m_nodes[i].m_x;
1865 if ((i >= pcount) || (btFabs(ifn->Eval(x)) < accurary))
1866 {
1867 const btVector3 v = m_nodes[i].m_v;
1868 btScalar m = getMass(i);
1869 if (m > 0)
1870 {
1871 m *= 0.5f;
1872 m_nodes[i].m_im /= 0.5f;
1873 }
1874 appendNode(x, m);
1875 cnodes[i] = m_nodes.size() - 1;
1876 m_nodes[cnodes[i]].m_v = v;
1877 }
1878 }
1879 nbase = &m_nodes[0];
1880 /* Links */
1881 for (i = 0, ni = m_links.size(); i < ni; ++i)
1882 {
1883 const int id[] = {int(m_links[i].m_n[0] - nbase),
1884 int(m_links[i].m_n[1] - nbase)};
1885 int todetach = 0;
1886 if (cnodes[id[0]] && cnodes[id[1]])
1887 {
1888 appendLink(i);
1889 todetach = m_links.size() - 1;
1890 }
1891 else
1892 {
1893 if (((ifn->Eval(m_nodes[id[0]].m_x) < accurary) &&
1894 (ifn->Eval(m_nodes[id[1]].m_x) < accurary)))
1895 todetach = i;
1896 }
1897 if (todetach)
1898 {
1899 Link& l = m_links[todetach];
1900 for (int j = 0; j < 2; ++j)
1901 {
1902 int cn = cnodes[int(l.m_n[j] - nbase)];
1903 if (cn) l.m_n[j] = &m_nodes[cn];
1904 }
1905 }
1906 }
1907 /* Faces */
1908 for (i = 0, ni = m_faces.size(); i < ni; ++i)
1909 {
1910 Node** n = m_faces[i].m_n;
1911 if ((ifn->Eval(n[0]->m_x) < accurary) &&
1912 (ifn->Eval(n[1]->m_x) < accurary) &&
1913 (ifn->Eval(n[2]->m_x) < accurary))
1914 {
1915 for (int j = 0; j < 3; ++j)
1916 {
1917 int cn = cnodes[int(n[j] - nbase)];
1918 if (cn) n[j] = &m_nodes[cn];
1919 }
1920 }
1921 }
1922 /* Clean orphans */
1923 int nnodes = m_nodes.size();
1926 ranks.resize(nnodes, 0);
1927 for (i = 0, ni = m_links.size(); i < ni; ++i)
1928 {
1929 for (int j = 0; j < 2; ++j) ranks[int(m_links[i].m_n[j] - nbase)]++;
1930 }
1931 for (i = 0, ni = m_faces.size(); i < ni; ++i)
1932 {
1933 for (int j = 0; j < 3; ++j) ranks[int(m_faces[i].m_n[j] - nbase)]++;
1934 }
1935 for (i = 0; i < m_links.size(); ++i)
1936 {
1937 const int id[] = {int(m_links[i].m_n[0] - nbase),
1938 int(m_links[i].m_n[1] - nbase)};
1939 const bool sg[] = {ranks[id[0]] == 1,
1940 ranks[id[1]] == 1};
1941 if (sg[0] || sg[1])
1942 {
1943 --ranks[id[0]];
1944 --ranks[id[1]];
1945 btSwap(m_links[i], m_links[m_links.size() - 1]);
1946 m_links.pop_back();
1947 --i;
1948 }
1949 }
1950#if 0
1951 for(i=nnodes-1;i>=0;--i)
1952 {
1953 if(!ranks[i]) todelete.push_back(i);
1954 }
1955 if(todelete.size())
1956 {
1957 btAlignedObjectArray<int>& map=ranks;
1958 for(int i=0;i<nnodes;++i) map[i]=i;
1959 PointersToIndices(this);
1960 for(int i=0,ni=todelete.size();i<ni;++i)
1961 {
1962 int j=todelete[i];
1963 int& a=map[j];
1964 int& b=map[--nnodes];
1965 m_ndbvt.remove(m_nodes[a].m_leaf);m_nodes[a].m_leaf=0;
1966 btSwap(m_nodes[a],m_nodes[b]);
1967 j=a;a=b;b=j;
1968 }
1969 IndicesToPointers(this,&map[0]);
1970 m_nodes.resize(nnodes);
1971 }
1972#endif
1973 }
1974 m_bUpdateRtCst = true;
1975}
1976
1977//
1978bool btSoftBody::cutLink(const Node* node0, const Node* node1, btScalar position)
1979{
1980 return (cutLink(int(node0 - &m_nodes[0]), int(node1 - &m_nodes[0]), position));
1981}
1982
1983//
1984bool btSoftBody::cutLink(int node0, int node1, btScalar position)
1985{
1986 bool done = false;
1987 int i, ni;
1988 // const btVector3 d=m_nodes[node0].m_x-m_nodes[node1].m_x;
1989 const btVector3 x = Lerp(m_nodes[node0].m_x, m_nodes[node1].m_x, position);
1990 const btVector3 v = Lerp(m_nodes[node0].m_v, m_nodes[node1].m_v, position);
1991 const btScalar m = 1;
1992 appendNode(x, m);
1993 appendNode(x, m);
1994 Node* pa = &m_nodes[node0];
1995 Node* pb = &m_nodes[node1];
1996 Node* pn[2] = {&m_nodes[m_nodes.size() - 2],
1997 &m_nodes[m_nodes.size() - 1]};
1998 pn[0]->m_v = v;
1999 pn[1]->m_v = v;
2000 for (i = 0, ni = m_links.size(); i < ni; ++i)
2001 {
2002 const int mtch = MatchEdge(m_links[i].m_n[0], m_links[i].m_n[1], pa, pb);
2003 if (mtch != -1)
2004 {
2005 appendLink(i);
2006 Link* pft[] = {&m_links[i], &m_links[m_links.size() - 1]};
2007 pft[0]->m_n[1] = pn[mtch];
2008 pft[1]->m_n[0] = pn[1 - mtch];
2009 done = true;
2010 }
2011 }
2012 for (i = 0, ni = m_faces.size(); i < ni; ++i)
2013 {
2014 for (int k = 2, l = 0; l < 3; k = l++)
2015 {
2016 const int mtch = MatchEdge(m_faces[i].m_n[k], m_faces[i].m_n[l], pa, pb);
2017 if (mtch != -1)
2018 {
2019 appendFace(i);
2020 Face* pft[] = {&m_faces[i], &m_faces[m_faces.size() - 1]};
2021 pft[0]->m_n[l] = pn[mtch];
2022 pft[1]->m_n[k] = pn[1 - mtch];
2023 appendLink(pn[0], pft[0]->m_n[(l + 1) % 3], pft[0]->m_material, true);
2024 appendLink(pn[1], pft[0]->m_n[(l + 1) % 3], pft[0]->m_material, true);
2025 }
2026 }
2027 }
2028 if (!done)
2029 {
2030 m_ndbvt.remove(pn[0]->m_leaf);
2031 m_ndbvt.remove(pn[1]->m_leaf);
2032 m_nodes.pop_back();
2033 m_nodes.pop_back();
2034 }
2035 return (done);
2036}
2037
2038//
2039bool btSoftBody::rayTest(const btVector3& rayFrom,
2040 const btVector3& rayTo,
2041 sRayCast& results)
2042{
2043 if (m_faces.size() && m_fdbvt.empty())
2045
2046 results.body = this;
2047 results.fraction = 1.f;
2048 results.feature = eFeature::None;
2049 results.index = -1;
2050
2051 return (rayTest(rayFrom, rayTo, results.fraction, results.feature, results.index, false) != 0);
2052}
2053
2055 const btVector3& rayTo,
2056 sRayCast& results)
2057{
2058 if (m_faces.size() == 0)
2059 return false;
2060 else
2061 {
2062 if (m_fdbvt.empty())
2064 }
2065
2066 results.body = this;
2067 results.fraction = 1.f;
2068 results.index = -1;
2069
2070 return (rayFaceTest(rayFrom, rayTo, results.fraction, results.index) != 0);
2071}
2072
2073//
2098
2100{
2101 int i, ni;
2102
2103 /* Update */
2104 if (m_bUpdateRtCst)
2105 {
2106 m_bUpdateRtCst = false;
2108 m_fdbvt.clear();
2110 {
2112 }
2113 }
2114
2115 /* Prepare */
2116 m_sst.sdt = dt * m_cfg.timescale;
2117 m_sst.isdt = 1 / m_sst.sdt;
2118 m_sst.velmrg = m_sst.sdt * 3;
2120 m_sst.updmrg = m_sst.radmrg * (btScalar)0.25;
2121 /* Forces */
2123 applyForces();
2124 /* Integrate */
2125 for (i = 0, ni = m_nodes.size(); i < ni; ++i)
2126 {
2127 Node& n = m_nodes[i];
2128 n.m_q = n.m_x;
2129 btVector3 deltaV = n.m_f * n.m_im * m_sst.sdt;
2130 {
2131 btScalar maxDisplacement = m_worldInfo->m_maxDisplacement;
2132 btScalar clampDeltaV = maxDisplacement / m_sst.sdt;
2133 for (int c = 0; c < 3; c++)
2134 {
2135 if (deltaV[c] > clampDeltaV)
2136 {
2137 deltaV[c] = clampDeltaV;
2138 }
2139 if (deltaV[c] < -clampDeltaV)
2140 {
2141 deltaV[c] = -clampDeltaV;
2142 }
2143 }
2144 }
2145 n.m_v += deltaV;
2146 n.m_x += n.m_v * m_sst.sdt;
2147 n.m_f = btVector3(0, 0, 0);
2148 }
2149 /* Clusters */
2151 /* Bounds */
2152 updateBounds();
2153 /* Nodes */
2155 vol;
2156 for (i = 0, ni = m_nodes.size(); i < ni; ++i)
2157 {
2158 Node& n = m_nodes[i];
2161 vol,
2162 n.m_v * m_sst.velmrg,
2163 m_sst.updmrg);
2164 }
2165 /* Faces */
2166 if (!m_fdbvt.empty())
2167 {
2168 for (int i = 0; i < m_faces.size(); ++i)
2169 {
2170 Face& f = m_faces[i];
2171 const btVector3 v = (f.m_n[0]->m_v +
2172 f.m_n[1]->m_v +
2173 f.m_n[2]->m_v) /
2174 3;
2175 vol = VolumeOf(f, m_sst.radmrg);
2177 vol,
2178 v * m_sst.velmrg,
2179 m_sst.updmrg);
2180 }
2181 }
2182 /* Pose */
2183 updatePose();
2184 /* Match */
2185 if (m_pose.m_bframe && (m_cfg.kMT > 0))
2186 {
2187 const btMatrix3x3 posetrs = m_pose.m_rot;
2188 for (int i = 0, ni = m_nodes.size(); i < ni; ++i)
2189 {
2190 Node& n = m_nodes[i];
2191 if (n.m_im > 0)
2192 {
2193 const btVector3 x = posetrs * m_pose.m_pos[i] + m_pose.m_com;
2194 n.m_x = Lerp(n.m_x, x, m_cfg.kMT);
2195 }
2196 }
2197 }
2198 /* Clear contacts */
2201 /* Optimize dbvt's */
2205}
2206
2207//
2209{
2210 /* Apply clusters */
2211 applyClusters(false);
2212 /* Prepare links */
2213
2214 int i, ni;
2215
2216 for (i = 0, ni = m_links.size(); i < ni; ++i)
2217 {
2218 Link& l = m_links[i];
2219 l.m_c3 = l.m_n[1]->m_q - l.m_n[0]->m_q;
2220 l.m_c2 = 1 / (l.m_c3.length2() * l.m_c0);
2221 }
2222 /* Prepare anchors */
2223 for (i = 0, ni = m_anchors.size(); i < ni; ++i)
2224 {
2225 Anchor& a = m_anchors[i];
2226 const btVector3 ra = a.m_body->getWorldTransform().getBasis() * a.m_local;
2228 a.m_node->m_im,
2229 a.m_body->getInvMass(),
2231 ra);
2232 a.m_c1 = ra;
2233 a.m_c2 = m_sst.sdt * a.m_node->m_im;
2234 a.m_body->activate();
2235 }
2236 /* Solve velocities */
2237 if (m_cfg.viterations > 0)
2238 {
2239 /* Solve */
2240 for (int isolve = 0; isolve < m_cfg.viterations; ++isolve)
2241 {
2242 for (int iseq = 0; iseq < m_cfg.m_vsequence.size(); ++iseq)
2243 {
2244 getSolver(m_cfg.m_vsequence[iseq])(this, 1);
2245 }
2246 }
2247 /* Update */
2248 for (i = 0, ni = m_nodes.size(); i < ni; ++i)
2249 {
2250 Node& n = m_nodes[i];
2251 n.m_x = n.m_q + n.m_v * m_sst.sdt;
2252 }
2253 }
2254 /* Solve positions */
2255 if (m_cfg.piterations > 0)
2256 {
2257 for (int isolve = 0; isolve < m_cfg.piterations; ++isolve)
2258 {
2259 const btScalar ti = isolve / (btScalar)m_cfg.piterations;
2260 for (int iseq = 0; iseq < m_cfg.m_psequence.size(); ++iseq)
2261 {
2262 getSolver(m_cfg.m_psequence[iseq])(this, 1, ti);
2263 }
2264 }
2265 const btScalar vc = m_sst.isdt * (1 - m_cfg.kDP);
2266 for (i = 0, ni = m_nodes.size(); i < ni; ++i)
2267 {
2268 Node& n = m_nodes[i];
2269 n.m_v = (n.m_x - n.m_q) * vc;
2270 n.m_f = btVector3(0, 0, 0);
2271 }
2272 }
2273 /* Solve drift */
2274 if (m_cfg.diterations > 0)
2275 {
2276 const btScalar vcf = m_cfg.kVCF * m_sst.isdt;
2277 for (i = 0, ni = m_nodes.size(); i < ni; ++i)
2278 {
2279 Node& n = m_nodes[i];
2280 n.m_q = n.m_x;
2281 }
2282 for (int idrift = 0; idrift < m_cfg.diterations; ++idrift)
2283 {
2284 for (int iseq = 0; iseq < m_cfg.m_dsequence.size(); ++iseq)
2285 {
2286 getSolver(m_cfg.m_dsequence[iseq])(this, 1, 0);
2287 }
2288 }
2289 for (int i = 0, ni = m_nodes.size(); i < ni; ++i)
2290 {
2291 Node& n = m_nodes[i];
2292 n.m_v += (n.m_x - n.m_q) * vcf;
2293 }
2294 }
2295 /* Apply clusters */
2296 dampClusters();
2297 applyClusters(true);
2298}
2299
2300//
2301void btSoftBody::staticSolve(int iterations)
2302{
2303 for (int isolve = 0; isolve < iterations; ++isolve)
2304 {
2305 for (int iseq = 0; iseq < m_cfg.m_psequence.size(); ++iseq)
2306 {
2307 getSolver(m_cfg.m_psequence[iseq])(this, 1, 0);
2308 }
2309 }
2310}
2311
2312//
2313void btSoftBody::solveCommonConstraints(btSoftBody** /*bodies*/, int /*count*/, int /*iterations*/)
2314{
2316}
2317
2318//
2320{
2321 const int nb = bodies.size();
2322 int iterations = 0;
2323 int i;
2324
2325 for (i = 0; i < nb; ++i)
2326 {
2327 iterations = btMax(iterations, bodies[i]->m_cfg.citerations);
2328 }
2329 for (i = 0; i < nb; ++i)
2330 {
2331 bodies[i]->prepareClusters(iterations);
2332 }
2333 for (i = 0; i < iterations; ++i)
2334 {
2335 const btScalar sor = 1;
2336 for (int j = 0; j < nb; ++j)
2337 {
2338 bodies[j]->solveClusters(sor);
2339 }
2340 }
2341 for (i = 0; i < nb; ++i)
2342 {
2343 bodies[i]->cleanupClusters();
2344 }
2345}
2346
2347//
2349{
2350 /* Update */
2351 updateNormals();
2352}
2353
2354//
2356{
2357 m_rayFrom = rayFrom;
2358 m_rayNormalizedDirection = (rayTo - rayFrom);
2359 m_rayTo = rayTo;
2360 m_mint = mxt;
2361 m_face = 0;
2362 m_tests = 0;
2363}
2364
2365//
2367{
2369 const btScalar t = rayFromToTriangle(m_rayFrom, m_rayTo, m_rayNormalizedDirection,
2370 f.m_n[0]->m_x,
2371 f.m_n[1]->m_x,
2372 f.m_n[2]->m_x,
2373 m_mint);
2374 if ((t > 0) && (t < m_mint))
2375 {
2376 m_mint = t;
2377 m_face = &f;
2378 }
2379 ++m_tests;
2380}
2381
2382//
2384 const btVector3& rayTo,
2385 const btVector3& rayNormalizedDirection,
2386 const btVector3& a,
2387 const btVector3& b,
2388 const btVector3& c,
2389 btScalar maxt)
2390{
2391 static const btScalar ceps = -SIMD_EPSILON * 10;
2392 static const btScalar teps = SIMD_EPSILON * 10;
2393
2394 const btVector3 n = btCross(b - a, c - a);
2395 const btScalar d = btDot(a, n);
2396 const btScalar den = btDot(rayNormalizedDirection, n);
2397 if (!btFuzzyZero(den))
2398 {
2399 const btScalar num = btDot(rayFrom, n) - d;
2400 const btScalar t = -num / den;
2401 if ((t > teps) && (t < maxt))
2402 {
2403 const btVector3 hit = rayFrom + rayNormalizedDirection * t;
2404 if ((btDot(n, btCross(a - hit, b - hit)) > ceps) &&
2405 (btDot(n, btCross(b - hit, c - hit)) > ceps) &&
2406 (btDot(n, btCross(c - hit, a - hit)) > ceps))
2407 {
2408 return (t);
2409 }
2410 }
2411 }
2412 return (-1);
2413}
2414
2415//
2417{
2418#define PTR2IDX(_p_, _b_) reinterpret_cast<btSoftBody::Node*>((_p_) - (_b_))
2419 btSoftBody::Node* base = m_nodes.size() ? &m_nodes[0] : 0;
2420 int i, ni;
2421
2422 for (i = 0, ni = m_nodes.size(); i < ni; ++i)
2423 {
2424 if (m_nodes[i].m_leaf)
2425 {
2426 m_nodes[i].m_leaf->data = *(void**)&i;
2427 }
2428 }
2429 for (i = 0, ni = m_links.size(); i < ni; ++i)
2430 {
2431 m_links[i].m_n[0] = PTR2IDX(m_links[i].m_n[0], base);
2432 m_links[i].m_n[1] = PTR2IDX(m_links[i].m_n[1], base);
2433 }
2434 for (i = 0, ni = m_faces.size(); i < ni; ++i)
2435 {
2436 m_faces[i].m_n[0] = PTR2IDX(m_faces[i].m_n[0], base);
2437 m_faces[i].m_n[1] = PTR2IDX(m_faces[i].m_n[1], base);
2438 m_faces[i].m_n[2] = PTR2IDX(m_faces[i].m_n[2], base);
2439 if (m_faces[i].m_leaf)
2440 {
2441 m_faces[i].m_leaf->data = *(void**)&i;
2442 }
2443 }
2444 for (i = 0, ni = m_anchors.size(); i < ni; ++i)
2445 {
2446 m_anchors[i].m_node = PTR2IDX(m_anchors[i].m_node, base);
2447 }
2448 for (i = 0, ni = m_notes.size(); i < ni; ++i)
2449 {
2450 for (int j = 0; j < m_notes[i].m_rank; ++j)
2451 {
2452 m_notes[i].m_nodes[j] = PTR2IDX(m_notes[i].m_nodes[j], base);
2453 }
2454 }
2455#undef PTR2IDX
2456}
2457
2458//
2460{
2461#define IDX2PTR(_p_, _b_) map ? (&(_b_)[map[(((char*)_p_) - (char*)0)]]) : (&(_b_)[(((char*)_p_) - (char*)0)])
2462 btSoftBody::Node* base = m_nodes.size() ? &m_nodes[0] : 0;
2463 int i, ni;
2464
2465 for (i = 0, ni = m_nodes.size(); i < ni; ++i)
2466 {
2467 if (m_nodes[i].m_leaf)
2468 {
2469 m_nodes[i].m_leaf->data = &m_nodes[i];
2470 }
2471 }
2472 for (i = 0, ni = m_links.size(); i < ni; ++i)
2473 {
2474 m_links[i].m_n[0] = IDX2PTR(m_links[i].m_n[0], base);
2475 m_links[i].m_n[1] = IDX2PTR(m_links[i].m_n[1], base);
2476 }
2477 for (i = 0, ni = m_faces.size(); i < ni; ++i)
2478 {
2479 m_faces[i].m_n[0] = IDX2PTR(m_faces[i].m_n[0], base);
2480 m_faces[i].m_n[1] = IDX2PTR(m_faces[i].m_n[1], base);
2481 m_faces[i].m_n[2] = IDX2PTR(m_faces[i].m_n[2], base);
2482 if (m_faces[i].m_leaf)
2483 {
2484 m_faces[i].m_leaf->data = &m_faces[i];
2485 }
2486 }
2487 for (i = 0, ni = m_anchors.size(); i < ni; ++i)
2488 {
2489 m_anchors[i].m_node = IDX2PTR(m_anchors[i].m_node, base);
2490 }
2491 for (i = 0, ni = m_notes.size(); i < ni; ++i)
2492 {
2493 for (int j = 0; j < m_notes[i].m_rank; ++j)
2494 {
2495 m_notes[i].m_nodes[j] = IDX2PTR(m_notes[i].m_nodes[j], base);
2496 }
2497 }
2498#undef IDX2PTR
2499}
2500
2501//
2502int btSoftBody::rayTest(const btVector3& rayFrom, const btVector3& rayTo,
2503 btScalar& mint, eFeature::_& feature, int& index, bool bcountonly) const
2504{
2505 int cnt = 0;
2506 btVector3 dir = rayTo - rayFrom;
2507
2508 if (bcountonly || m_fdbvt.empty())
2509 { /* Full search */
2510
2511 for (int i = 0, ni = m_faces.size(); i < ni; ++i)
2512 {
2513 const btSoftBody::Face& f = m_faces[i];
2514
2515 const btScalar t = RayFromToCaster::rayFromToTriangle(rayFrom, rayTo, dir,
2516 f.m_n[0]->m_x,
2517 f.m_n[1]->m_x,
2518 f.m_n[2]->m_x,
2519 mint);
2520 if (t > 0)
2521 {
2522 ++cnt;
2523 if (!bcountonly)
2524 {
2526 index = i;
2527 mint = t;
2528 }
2529 }
2530 }
2531 }
2532 else
2533 { /* Use dbvt */
2534 RayFromToCaster collider(rayFrom, rayTo, mint);
2535
2536 btDbvt::rayTest(m_fdbvt.m_root, rayFrom, rayTo, collider);
2537 if (collider.m_face)
2538 {
2539 mint = collider.m_mint;
2541 index = (int)(collider.m_face - &m_faces[0]);
2542 cnt = 1;
2543 }
2544 }
2545
2546 for (int i = 0; i < m_tetras.size(); i++)
2547 {
2548 const btSoftBody::Tetra& tet = m_tetras[i];
2549 int tetfaces[4][3] = {{0, 1, 2}, {0, 1, 3}, {1, 2, 3}, {0, 2, 3}};
2550 for (int f = 0; f < 4; f++)
2551 {
2552 int index0 = tetfaces[f][0];
2553 int index1 = tetfaces[f][1];
2554 int index2 = tetfaces[f][2];
2555 btVector3 v0 = tet.m_n[index0]->m_x;
2556 btVector3 v1 = tet.m_n[index1]->m_x;
2557 btVector3 v2 = tet.m_n[index2]->m_x;
2558
2559 const btScalar t = RayFromToCaster::rayFromToTriangle(rayFrom, rayTo, dir,
2560 v0, v1, v2,
2561 mint);
2562 if (t > 0)
2563 {
2564 ++cnt;
2565 if (!bcountonly)
2566 {
2568 index = i;
2569 mint = t;
2570 }
2571 }
2572 }
2573 }
2574 return (cnt);
2575}
2576
2577int btSoftBody::rayFaceTest(const btVector3& rayFrom, const btVector3& rayTo,
2578 btScalar& mint, int& index) const
2579{
2580 int cnt = 0;
2581 { /* Use dbvt */
2582 RayFromToCaster collider(rayFrom, rayTo, mint);
2583
2584 btDbvt::rayTest(m_fdbvt.m_root, rayFrom, rayTo, collider);
2585 if (collider.m_face)
2586 {
2587 mint = collider.m_mint;
2588 index = (int)(collider.m_face - &m_faces[0]);
2589 cnt = 1;
2590 }
2591 }
2592 return (cnt);
2593}
2594
2595//
2596static inline btDbvntNode* copyToDbvnt(const btDbvtNode* n)
2597{
2598 if (n == 0)
2599 return 0;
2600 btDbvntNode* root = new btDbvntNode(n);
2601 if (n->isinternal())
2602 {
2603 btDbvntNode* c0 = copyToDbvnt(n->childs[0]);
2604 root->childs[0] = c0;
2605 btDbvntNode* c1 = copyToDbvnt(n->childs[1]);
2606 root->childs[1] = c1;
2607 }
2608 return root;
2609}
2610
2611static inline void calculateNormalCone(btDbvntNode* root)
2612{
2613 if (!root)
2614 return;
2615 if (root->isleaf())
2616 {
2617 const btSoftBody::Face* face = (btSoftBody::Face*)root->data;
2618 root->normal = face->m_normal;
2619 root->angle = 0;
2620 }
2621 else
2622 {
2623 btVector3 n0(0, 0, 0), n1(0, 0, 0);
2624 btScalar a0 = 0, a1 = 0;
2625 if (root->childs[0])
2626 {
2627 calculateNormalCone(root->childs[0]);
2628 n0 = root->childs[0]->normal;
2629 a0 = root->childs[0]->angle;
2630 }
2631 if (root->childs[1])
2632 {
2633 calculateNormalCone(root->childs[1]);
2634 n1 = root->childs[1]->normal;
2635 a1 = root->childs[1]->angle;
2636 }
2637 root->normal = (n0 + n1).safeNormalize();
2638 root->angle = btMax(a0, a1) + btAngle(n0, n1) * 0.5;
2639 }
2640}
2641
2643{
2644 BT_PROFILE("btSoftBody::initializeFaceTree");
2645 m_fdbvt.clear();
2646 // create leaf nodes;
2648 leafNodes.resize(m_faces.size());
2649 for (int i = 0; i < m_faces.size(); ++i)
2650 {
2651 Face& f = m_faces[i];
2653 vol = VolumeOf(f, 0);
2654 btDbvtNode* node = new (btAlignedAlloc(sizeof(btDbvtNode), 16)) btDbvtNode();
2655 node->parent = NULL;
2656 node->data = &f;
2657 node->childs[1] = 0;
2658 node->volume = vol;
2659 leafNodes[i] = node;
2660 f.m_leaf = node;
2661 }
2663 adj.resize(m_faces.size());
2664 // construct the adjacency list for triangles
2665 for (int i = 0; i < adj.size(); ++i)
2666 {
2667 for (int j = i + 1; j < adj.size(); ++j)
2668 {
2669 int dup = 0;
2670 for (int k = 0; k < 3; ++k)
2671 {
2672 for (int l = 0; l < 3; ++l)
2673 {
2674 if (m_faces[i].m_n[k] == m_faces[j].m_n[l])
2675 {
2676 ++dup;
2677 break;
2678 }
2679 }
2680 if (dup == 2)
2681 {
2682 adj[i].push_back(j);
2683 adj[j].push_back(i);
2684 }
2685 }
2686 }
2687 }
2688 m_fdbvt.m_root = buildTreeBottomUp(leafNodes, adj);
2689 if (m_fdbvnt)
2690 delete m_fdbvnt;
2692 updateFaceTree(false, false);
2694}
2695
2696//
2698{
2699 m_ndbvt.clear();
2701 leafNodes.resize(m_nodes.size());
2702 for (int i = 0; i < m_nodes.size(); ++i)
2703 {
2704 Node& n = m_nodes[i];
2706 vol = btDbvtVolume::FromCR(n.m_x, 0);
2707 btDbvtNode* node = new (btAlignedAlloc(sizeof(btDbvtNode), 16)) btDbvtNode();
2708 node->parent = NULL;
2709 node->data = &n;
2710 node->childs[1] = 0;
2711 node->volume = vol;
2712 leafNodes[i] = node;
2713 n.m_leaf = node;
2714 }
2716 adj.resize(m_nodes.size());
2718 old_id.resize(m_nodes.size());
2719 for (int i = 0; i < m_nodes.size(); ++i)
2720 old_id[i] = m_nodes[i].index;
2721 for (int i = 0; i < m_nodes.size(); ++i)
2722 m_nodes[i].index = i;
2723 for (int i = 0; i < m_links.size(); ++i)
2724 {
2725 Link& l = m_links[i];
2726 adj[l.m_n[0]->index].push_back(l.m_n[1]->index);
2727 adj[l.m_n[1]->index].push_back(l.m_n[0]->index);
2728 }
2729 m_ndbvt.m_root = buildTreeBottomUp(leafNodes, adj);
2730 for (int i = 0; i < m_nodes.size(); ++i)
2731 m_nodes[i].index = old_id[i];
2732}
2733
2734//
2736{
2737 btVector3 com(0, 0, 0);
2738 if (m_pose.m_bframe)
2739 {
2740 for (int i = 0, ni = m_nodes.size(); i < ni; ++i)
2741 {
2742 com += m_nodes[i].m_x * m_pose.m_wgh[i];
2743 }
2744 }
2745 return (com);
2746}
2747
2749 const btVector3& x,
2750 btScalar margin,
2751 btSoftBody::sCti& cti) const
2752{
2753 btVector3 nrm;
2754 const btCollisionShape* shp = colObjWrap->getCollisionShape();
2755 // const btRigidBody *tmpRigid = btRigidBody::upcast(colObjWrap->getCollisionObject());
2756 //const btTransform &wtr = tmpRigid ? tmpRigid->getWorldTransform() : colObjWrap->getWorldTransform();
2757 const btTransform& wtr = colObjWrap->getWorldTransform();
2758 //todo: check which transform is needed here
2759
2760 btScalar dst =
2762 wtr.invXform(x),
2763 shp,
2764 nrm,
2765 margin);
2766 if (dst < 0)
2767 {
2768 cti.m_colObj = colObjWrap->getCollisionObject();
2769 cti.m_normal = wtr.getBasis() * nrm;
2770 cti.m_offset = -btDot(cti.m_normal, x - cti.m_normal * dst);
2771 return (true);
2772 }
2773 return (false);
2774}
2775
2776//
2778 const btVector3& x,
2779 btScalar margin,
2780 btSoftBody::sCti& cti, bool predict) const
2781{
2782 btVector3 nrm;
2783 const btCollisionShape* shp = colObjWrap->getCollisionShape();
2784 const btCollisionObject* tmpCollisionObj = colObjWrap->getCollisionObject();
2785 // use the position x_{n+1}^* = x_n + dt * v_{n+1}^* where v_{n+1}^* = v_n + dtg for collision detect
2786 // but resolve contact at x_n
2787 btTransform wtr = (predict) ? (colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform() * (*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform())
2788 : colObjWrap->getWorldTransform();
2789 btScalar dst =
2791 wtr.invXform(x),
2792 shp,
2793 nrm,
2794 margin);
2795
2796 if (!predict)
2797 {
2798 cti.m_colObj = colObjWrap->getCollisionObject();
2799 cti.m_normal = wtr.getBasis() * nrm;
2800 cti.m_offset = dst;
2801 }
2802 if (dst < 0)
2803 return true;
2804 return (false);
2805}
2806
2807//
2808// Compute barycentric coordinates (u, v, w) for
2809// point p with respect to triangle (a, b, c)
2810static void getBarycentric(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, btVector3& bary)
2811{
2812 btVector3 v0 = b - a, v1 = c - a, v2 = p - a;
2813 btScalar d00 = v0.dot(v0);
2814 btScalar d01 = v0.dot(v1);
2815 btScalar d11 = v1.dot(v1);
2816 btScalar d20 = v2.dot(v0);
2817 btScalar d21 = v2.dot(v1);
2818 btScalar denom = d00 * d11 - d01 * d01;
2819 // In the case of a degenerate triangle, pick a vertex.
2820 if (btFabs(denom) < SIMD_EPSILON)
2821 {
2822 bary.setY(btScalar(0.0));
2823 bary.setZ(btScalar(0.0));
2824 }
2825 else
2826 {
2827 bary.setY((d11 * d20 - d01 * d21) / denom);
2828 bary.setZ((d00 * d21 - d01 * d20) / denom);
2829 }
2830 bary.setX(btScalar(1) - bary.getY() - bary.getZ());
2831}
2832
2833//
2835 Face& f,
2836 btVector3& contact_point,
2837 btVector3& bary,
2838 btScalar margin,
2839 btSoftBody::sCti& cti, bool predict) const
2840{
2841 btVector3 nrm;
2842 const btCollisionShape* shp = colObjWrap->getCollisionShape();
2843 const btCollisionObject* tmpCollisionObj = colObjWrap->getCollisionObject();
2844 // use the position x_{n+1}^* = x_n + dt * v_{n+1}^* where v_{n+1}^* = v_n + dtg for collision detect
2845 // but resolve contact at x_n
2846 btTransform wtr = (predict) ? (colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform() * (*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform())
2847 : colObjWrap->getWorldTransform();
2848 btScalar dst;
2850
2851 // #define USE_QUADRATURE 1
2852
2853 // use collision quadrature point
2854#ifdef USE_QUADRATURE
2855 {
2856 dst = SIMD_INFINITY;
2857 btVector3 local_nrm;
2858 for (int q = 0; q < m_quads.size(); ++q)
2859 {
2860 btVector3 p;
2861 if (predict)
2862 p = BaryEval(f.m_n[0]->m_q, f.m_n[1]->m_q, f.m_n[2]->m_q, m_quads[q]);
2863 else
2864 p = BaryEval(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, m_quads[q]);
2866 wtr.invXform(p),
2867 shp,
2868 local_nrm,
2869 margin);
2870 if (local_dst < dst)
2871 {
2872 if (local_dst < 0 && predict)
2873 return true;
2874 dst = local_dst;
2875 contact_point = p;
2876 bary = m_quads[q];
2877 nrm = local_nrm;
2878 }
2879 if (!predict)
2880 {
2881 cti.m_colObj = colObjWrap->getCollisionObject();
2882 cti.m_normal = wtr.getBasis() * nrm;
2883 cti.m_offset = dst;
2884 }
2885 }
2886 return (dst < 0);
2887 }
2888#endif
2889
2890 // collision detection using x*
2891 btTransform triangle_transform;
2892 triangle_transform.setIdentity();
2893 triangle_transform.setOrigin(f.m_n[0]->m_q);
2894 btTriangleShape triangle(btVector3(0, 0, 0), f.m_n[1]->m_q - f.m_n[0]->m_q, f.m_n[2]->m_q - f.m_n[0]->m_q);
2895 btVector3 guess(0, 0, 0);
2896 const btConvexShape* csh = static_cast<const btConvexShape*>(shp);
2897 btGjkEpaSolver2::SignedDistance(&triangle, triangle_transform, csh, wtr, guess, results);
2898 dst = results.distance - 2.0 * csh->getMargin() - margin; // margin padding so that the distance = the actual distance between face and rigid - margin of rigid - margin of deformable
2899 if (dst >= 0)
2900 return false;
2901
2902 // Use consistent barycenter to recalculate distance.
2903 if (this->m_cacheBarycenter)
2904 {
2905 if (f.m_pcontact[3] != 0)
2906 {
2907 for (int i = 0; i < 3; ++i)
2908 bary[i] = f.m_pcontact[i];
2909 contact_point = BaryEval(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary);
2910 const btConvexShape* csh = static_cast<const btConvexShape*>(shp);
2911 btGjkEpaSolver2::SignedDistance(contact_point, margin, csh, wtr, results);
2912 cti.m_colObj = colObjWrap->getCollisionObject();
2913 dst = results.distance;
2914 cti.m_normal = results.normal;
2915 cti.m_offset = dst;
2916
2917 //point-convex CD
2918 wtr = colObjWrap->getWorldTransform();
2919 btTriangleShape triangle2(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);
2920 triangle_transform.setOrigin(f.m_n[0]->m_x);
2921 btGjkEpaSolver2::SignedDistance(&triangle2, triangle_transform, csh, wtr, guess, results);
2922
2923 dst = results.distance - csh->getMargin() - margin;
2924 return true;
2925 }
2926 }
2927
2928 // Use triangle-convex CD.
2929 wtr = colObjWrap->getWorldTransform();
2930 btTriangleShape triangle2(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);
2931 triangle_transform.setOrigin(f.m_n[0]->m_x);
2932 btGjkEpaSolver2::SignedDistance(&triangle2, triangle_transform, csh, wtr, guess, results);
2933 contact_point = results.witnesses[0];
2934 getBarycentric(contact_point, f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary);
2935
2936 for (int i = 0; i < 3; ++i)
2937 f.m_pcontact[i] = bary[i];
2938
2939 dst = results.distance - csh->getMargin() - margin;
2940 cti.m_colObj = colObjWrap->getCollisionObject();
2941 cti.m_normal = results.normal;
2942 cti.m_offset = dst;
2943 return true;
2944}
2945
2947{
2948 const btVector3 zv(0, 0, 0);
2949 int i, ni;
2950
2951 for (i = 0, ni = m_nodes.size(); i < ni; ++i)
2952 {
2953 m_nodes[i].m_n = zv;
2954 }
2955 for (i = 0, ni = m_faces.size(); i < ni; ++i)
2956 {
2957 btSoftBody::Face& f = m_faces[i];
2958 const btVector3 n = btCross(f.m_n[1]->m_x - f.m_n[0]->m_x,
2959 f.m_n[2]->m_x - f.m_n[0]->m_x);
2960 f.m_normal = n;
2962 f.m_n[0]->m_n += n;
2963 f.m_n[1]->m_n += n;
2964 f.m_n[2]->m_n += n;
2965 }
2966 for (i = 0, ni = m_nodes.size(); i < ni; ++i)
2967 {
2968 btScalar len = m_nodes[i].m_n.length();
2969 if (len > SIMD_EPSILON)
2970 m_nodes[i].m_n /= len;
2971 }
2972}
2973
2974//
2976{
2977 /*if( m_acceleratedSoftBody )
2978 {
2979 // If we have an accelerated softbody we need to obtain the bounds correctly
2980 // For now (slightly hackily) just have a very large AABB
2981 // TODO: Write get bounds kernel
2982 // If that is updating in place, atomic collisions might be low (when the cloth isn't perfectly aligned to an axis) and we could
2983 // probably do a test and exchange reasonably efficiently.
2984
2985 m_bounds[0] = btVector3(-1000, -1000, -1000);
2986 m_bounds[1] = btVector3(1000, 1000, 1000);
2987
2988 } else {*/
2989 // if (m_ndbvt.m_root)
2990 // {
2991 // const btVector3& mins = m_ndbvt.m_root->volume.Mins();
2992 // const btVector3& maxs = m_ndbvt.m_root->volume.Maxs();
2993 // const btScalar csm = getCollisionShape()->getMargin();
2994 // const btVector3 mrg = btVector3(csm,
2995 // csm,
2996 // csm) *
2997 // 1; // ??? to investigate...
2998 // m_bounds[0] = mins - mrg;
2999 // m_bounds[1] = maxs + mrg;
3000 // if (0 != getBroadphaseHandle())
3001 // {
3002 // m_worldInfo->m_broadphase->setAabb(getBroadphaseHandle(),
3003 // m_bounds[0],
3004 // m_bounds[1],
3005 // m_worldInfo->m_dispatcher);
3006 // }
3007 // }
3008 // else
3009 // {
3010 // m_bounds[0] =
3011 // m_bounds[1] = btVector3(0, 0, 0);
3012 // }
3013 if (m_nodes.size())
3014 {
3015 btVector3 mins = m_nodes[0].m_x;
3016 btVector3 maxs = m_nodes[0].m_x;
3017 for (int i = 1; i < m_nodes.size(); ++i)
3018 {
3019 for (int d = 0; d < 3; ++d)
3020 {
3021 if (m_nodes[i].m_x[d] > maxs[d])
3022 maxs[d] = m_nodes[i].m_x[d];
3023 if (m_nodes[i].m_x[d] < mins[d])
3024 mins[d] = m_nodes[i].m_x[d];
3025 }
3026 }
3027 const btScalar csm = getCollisionShape()->getMargin();
3028 const btVector3 mrg = btVector3(csm,
3029 csm,
3030 csm);
3031 m_bounds[0] = mins - mrg;
3032 m_bounds[1] = maxs + mrg;
3033 if (0 != getBroadphaseHandle())
3034 {
3036 m_bounds[0],
3037 m_bounds[1],
3039 }
3040 }
3041 else
3042 {
3043 m_bounds[0] =
3044 m_bounds[1] = btVector3(0, 0, 0);
3045 }
3046}
3047
3048//
3050{
3051 if (m_pose.m_bframe)
3052 {
3053 btSoftBody::Pose& pose = m_pose;
3054 const btVector3 com = evaluateCom();
3055 /* Com */
3056 pose.m_com = com;
3057 /* Rotation */
3058 btMatrix3x3 Apq;
3059 const btScalar eps = SIMD_EPSILON;
3060 Apq[0] = Apq[1] = Apq[2] = btVector3(0, 0, 0);
3061 Apq[0].setX(eps);
3062 Apq[1].setY(eps * 2);
3063 Apq[2].setZ(eps * 3);
3064 for (int i = 0, ni = m_nodes.size(); i < ni; ++i)
3065 {
3066 const btVector3 a = pose.m_wgh[i] * (m_nodes[i].m_x - com);
3067 const btVector3& b = pose.m_pos[i];
3068 Apq[0] += a.x() * b;
3069 Apq[1] += a.y() * b;
3070 Apq[2] += a.z() * b;
3071 }
3072 btMatrix3x3 r, s;
3073 PolarDecompose(Apq, r, s);
3074 pose.m_rot = r;
3075 pose.m_scl = pose.m_aqq * r.transpose() * Apq;
3076 if (m_cfg.maxvolume > 1)
3077 {
3078 const btScalar idet = Clamp<btScalar>(1 / pose.m_scl.determinant(),
3079 1, m_cfg.maxvolume);
3080 pose.m_scl = Mul(pose.m_scl, idet);
3081 }
3082 }
3083}
3084
3085//
3086void btSoftBody::updateArea(bool averageArea)
3087{
3088 int i, ni;
3089
3090 /* Face area */
3091 for (i = 0, ni = m_faces.size(); i < ni; ++i)
3092 {
3093 Face& f = m_faces[i];
3094 f.m_ra = AreaOf(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x);
3095 }
3096
3097 /* Node area */
3098
3099 if (averageArea)
3100 {
3102 counts.resize(m_nodes.size(), 0);
3103 for (i = 0, ni = m_nodes.size(); i < ni; ++i)
3104 {
3105 m_nodes[i].m_area = 0;
3106 }
3107 for (i = 0, ni = m_faces.size(); i < ni; ++i)
3108 {
3109 btSoftBody::Face& f = m_faces[i];
3110 for (int j = 0; j < 3; ++j)
3111 {
3112 const int index = (int)(f.m_n[j] - &m_nodes[0]);
3113 counts[index]++;
3114 f.m_n[j]->m_area += btFabs(f.m_ra);
3115 }
3116 }
3117 for (i = 0, ni = m_nodes.size(); i < ni; ++i)
3118 {
3119 if (counts[i] > 0)
3120 m_nodes[i].m_area /= (btScalar)counts[i];
3121 else
3122 m_nodes[i].m_area = 0;
3123 }
3124 }
3125 else
3126 {
3127 // initialize node area as zero
3128 for (i = 0, ni = m_nodes.size(); i < ni; ++i)
3129 {
3130 m_nodes[i].m_area = 0;
3131 }
3132
3133 for (i = 0, ni = m_faces.size(); i < ni; ++i)
3134 {
3135 btSoftBody::Face& f = m_faces[i];
3136
3137 for (int j = 0; j < 3; ++j)
3138 {
3139 f.m_n[j]->m_area += f.m_ra;
3140 }
3141 }
3142
3143 for (i = 0, ni = m_nodes.size(); i < ni; ++i)
3144 {
3145 m_nodes[i].m_area *= 0.3333333f;
3146 }
3147 }
3148}
3149
3151{
3152 int i, ni;
3153
3154 /* Links */
3155 for (i = 0, ni = m_links.size(); i < ni; ++i)
3156 {
3157 Link& l = m_links[i];
3158 Material& m = *l.m_material;
3159 l.m_c0 = (l.m_n[0]->m_im + l.m_n[1]->m_im) / m.m_kLST;
3160 }
3161}
3162
3169
3170//
3172{
3173 int i;
3174
3175 for (i = 0; i < m_clusters.size(); ++i)
3176 {
3177 Cluster& c = *m_clusters[i];
3178 c.m_imass = 0;
3179 c.m_masses.resize(c.m_nodes.size());
3180 for (int j = 0; j < c.m_nodes.size(); ++j)
3181 {
3182 if (c.m_nodes[j]->m_im == 0)
3183 {
3184 c.m_containsAnchor = true;
3185 c.m_masses[j] = BT_LARGE_FLOAT;
3186 }
3187 else
3188 {
3189 c.m_masses[j] = btScalar(1.) / c.m_nodes[j]->m_im;
3190 }
3191 c.m_imass += c.m_masses[j];
3192 }
3193 c.m_imass = btScalar(1.) / c.m_imass;
3195 c.m_lv = btVector3(0, 0, 0);
3196 c.m_av = btVector3(0, 0, 0);
3197 c.m_leaf = 0;
3198 /* Inertia */
3199 btMatrix3x3& ii = c.m_locii;
3200 ii[0] = ii[1] = ii[2] = btVector3(0, 0, 0);
3201 {
3202 int i, ni;
3203
3204 for (i = 0, ni = c.m_nodes.size(); i < ni; ++i)
3205 {
3206 const btVector3 k = c.m_nodes[i]->m_x - c.m_com;
3207 const btVector3 q = k * k;
3208 const btScalar m = c.m_masses[i];
3209 ii[0][0] += m * (q[1] + q[2]);
3210 ii[1][1] += m * (q[0] + q[2]);
3211 ii[2][2] += m * (q[0] + q[1]);
3212 ii[0][1] -= m * k[0] * k[1];
3213 ii[0][2] -= m * k[0] * k[2];
3214 ii[1][2] -= m * k[1] * k[2];
3215 }
3216 }
3217 ii[1][0] = ii[0][1];
3218 ii[2][0] = ii[0][2];
3219 ii[2][1] = ii[1][2];
3220
3221 ii = ii.inverse();
3222
3223 /* Frame */
3226 c.m_framerefs.resize(c.m_nodes.size());
3227 {
3228 int i;
3229 for (i = 0; i < c.m_framerefs.size(); ++i)
3230 {
3231 c.m_framerefs[i] = c.m_nodes[i]->m_x - c.m_com;
3232 }
3233 }
3234 }
3235}
3236
3237//
3239{
3240 BT_PROFILE("UpdateClusters");
3241 int i;
3242
3243 for (i = 0; i < m_clusters.size(); ++i)
3244 {
3246 const int n = c.m_nodes.size();
3247 //const btScalar invn=1/(btScalar)n;
3248 if (n)
3249 {
3250 /* Frame */
3251 const btScalar eps = btScalar(0.0001);
3252 btMatrix3x3 m, r, s;
3253 m[0] = m[1] = m[2] = btVector3(0, 0, 0);
3254 m[0][0] = eps * 1;
3255 m[1][1] = eps * 2;
3256 m[2][2] = eps * 3;
3257 c.m_com = clusterCom(&c);
3258 for (int i = 0; i < c.m_nodes.size(); ++i)
3259 {
3260 const btVector3 a = c.m_nodes[i]->m_x - c.m_com;
3261 const btVector3& b = c.m_framerefs[i];
3262 m[0] += a[0] * b;
3263 m[1] += a[1] * b;
3264 m[2] += a[2] * b;
3265 }
3266 PolarDecompose(m, r, s);
3269 /* Inertia */
3270#if 1 /* Constant */
3272#else
3273#if 0 /* Sphere */
3274 const btScalar rk=(2*c.m_extents.length2())/(5*c.m_imass);
3275 const btVector3 inertia(rk,rk,rk);
3276 const btVector3 iin(btFabs(inertia[0])>SIMD_EPSILON?1/inertia[0]:0,
3277 btFabs(inertia[1])>SIMD_EPSILON?1/inertia[1]:0,
3278 btFabs(inertia[2])>SIMD_EPSILON?1/inertia[2]:0);
3279
3280 c.m_invwi=c.m_xform.getBasis().scaled(iin)*c.m_xform.getBasis().transpose();
3281#else /* Actual */
3282 c.m_invwi[0] = c.m_invwi[1] = c.m_invwi[2] = btVector3(0, 0, 0);
3283 for (int i = 0; i < n; ++i)
3284 {
3285 const btVector3 k = c.m_nodes[i]->m_x - c.m_com;
3286 const btVector3 q = k * k;
3287 const btScalar m = 1 / c.m_nodes[i]->m_im;
3288 c.m_invwi[0][0] += m * (q[1] + q[2]);
3289 c.m_invwi[1][1] += m * (q[0] + q[2]);
3290 c.m_invwi[2][2] += m * (q[0] + q[1]);
3291 c.m_invwi[0][1] -= m * k[0] * k[1];
3292 c.m_invwi[0][2] -= m * k[0] * k[2];
3293 c.m_invwi[1][2] -= m * k[1] * k[2];
3294 }
3295 c.m_invwi[1][0] = c.m_invwi[0][1];
3296 c.m_invwi[2][0] = c.m_invwi[0][2];
3297 c.m_invwi[2][1] = c.m_invwi[1][2];
3298 c.m_invwi = c.m_invwi.inverse();
3299#endif
3300#endif
3301 /* Velocities */
3302 c.m_lv = btVector3(0, 0, 0);
3303 c.m_av = btVector3(0, 0, 0);
3304 {
3305 int i;
3306
3307 for (i = 0; i < n; ++i)
3308 {
3309 const btVector3 v = c.m_nodes[i]->m_v * c.m_masses[i];
3310 c.m_lv += v;
3311 c.m_av += btCross(c.m_nodes[i]->m_x - c.m_com, v);
3312 }
3313 }
3314 c.m_lv = c.m_imass * c.m_lv * (1 - c.m_ldamping);
3315 c.m_av = c.m_invwi * c.m_av * (1 - c.m_adamping);
3316 c.m_vimpulses[0] =
3317 c.m_vimpulses[1] = btVector3(0, 0, 0);
3318 c.m_dimpulses[0] =
3319 c.m_dimpulses[1] = btVector3(0, 0, 0);
3320 c.m_nvimpulses = 0;
3321 c.m_ndimpulses = 0;
3322 /* Matching */
3323 if (c.m_matching > 0)
3324 {
3325 for (int j = 0; j < c.m_nodes.size(); ++j)
3326 {
3327 Node& n = *c.m_nodes[j];
3328 const btVector3 x = c.m_framexform * c.m_framerefs[j];
3329 n.m_x = Lerp(n.m_x, x, c.m_matching);
3330 }
3331 }
3332 /* Dbvt */
3333 if (c.m_collide)
3334 {
3335 btVector3 mi = c.m_nodes[0]->m_x;
3336 btVector3 mx = mi;
3337 for (int j = 1; j < n; ++j)
3338 {
3339 mi.setMin(c.m_nodes[j]->m_x);
3340 mx.setMax(c.m_nodes[j]->m_x);
3341 }
3343 bounds = btDbvtVolume::FromMM(mi, mx);
3344 if (c.m_leaf)
3346 else
3347 c.m_leaf = m_cdbvt.insert(bounds, &c);
3348 }
3349 }
3350 }
3351}
3352
3353//
3355{
3356 for (int i = 0; i < m_joints.size(); ++i)
3357 {
3358 m_joints[i]->Terminate(m_sst.sdt);
3359 if (m_joints[i]->m_delete)
3360 {
3362 m_joints.remove(m_joints[i--]);
3363 }
3364 }
3365}
3366
3367//
3369{
3370 for (int i = 0; i < m_joints.size(); ++i)
3371 {
3372 m_joints[i]->Prepare(m_sst.sdt, iterations);
3373 }
3374}
3375
3376//
3378{
3379 for (int i = 0, ni = m_joints.size(); i < ni; ++i)
3380 {
3381 m_joints[i]->Solve(m_sst.sdt, sor);
3382 }
3383}
3384
3385//
3387{
3388 BT_PROFILE("ApplyClusters");
3389 // const btScalar f0=m_sst.sdt;
3390 //const btScalar f1=f0/2;
3393 deltas.resize(m_nodes.size(), btVector3(0, 0, 0));
3394 weights.resize(m_nodes.size(), 0);
3395 int i;
3396
3397 if (drift)
3398 {
3399 for (i = 0; i < m_clusters.size(); ++i)
3400 {
3401 Cluster& c = *m_clusters[i];
3402 if (c.m_ndimpulses)
3403 {
3406 }
3407 }
3408 }
3409
3410 for (i = 0; i < m_clusters.size(); ++i)
3411 {
3412 Cluster& c = *m_clusters[i];
3413 if (0 < (drift ? c.m_ndimpulses : c.m_nvimpulses))
3414 {
3415 const btVector3 v = (drift ? c.m_dimpulses[0] : c.m_vimpulses[0]) * m_sst.sdt;
3416 const btVector3 w = (drift ? c.m_dimpulses[1] : c.m_vimpulses[1]) * m_sst.sdt;
3417 for (int j = 0; j < c.m_nodes.size(); ++j)
3418 {
3419 const int idx = int(c.m_nodes[j] - &m_nodes[0]);
3420 const btVector3& x = c.m_nodes[j]->m_x;
3421 const btScalar q = c.m_masses[j];
3422 deltas[idx] += (v + btCross(w, x - c.m_com)) * q;
3423 weights[idx] += q;
3424 }
3425 }
3426 }
3427 for (i = 0; i < deltas.size(); ++i)
3428 {
3429 if (weights[i] > 0)
3430 {
3431 m_nodes[i].m_x += deltas[i] / weights[i];
3432 }
3433 }
3434}
3435
3436//
3438{
3439 int i;
3440
3441 for (i = 0; i < m_clusters.size(); ++i)
3442 {
3443 Cluster& c = *m_clusters[i];
3444 if (c.m_ndamping > 0)
3445 {
3446 for (int j = 0; j < c.m_nodes.size(); ++j)
3447 {
3448 Node& n = *c.m_nodes[j];
3449 if (n.m_im > 0)
3450 {
3451 const btVector3 vx = c.m_lv + btCross(c.m_av, c.m_nodes[j]->m_q - c.m_com);
3452 if (vx.length2() <= n.m_v.length2())
3453 {
3454 n.m_v += c.m_ndamping * (vx - n.m_v);
3455 }
3456 }
3457 }
3458 }
3459 }
3460}
3461
3463{
3464 for (int i = 0; i < m_links.size(); ++i)
3465 {
3466 m_links[i].Feature::m_material->m_kLST = k;
3467 }
3469}
3470
3472{
3473 m_gravityFactor = gravFactor;
3474}
3475
3476void btSoftBody::setCacheBarycenter(bool cacheBarycenter)
3477{
3478 m_cacheBarycenter = cacheBarycenter;
3479}
3480
3482{
3483 btScalar unit_simplex_measure = 1. / 6.;
3484
3485 for (int i = 0; i < m_tetras.size(); ++i)
3486 {
3487 Tetra& t = m_tetras[i];
3488 btVector3 c1 = t.m_n[1]->m_x - t.m_n[0]->m_x;
3489 btVector3 c2 = t.m_n[2]->m_x - t.m_n[0]->m_x;
3490 btVector3 c3 = t.m_n[3]->m_x - t.m_n[0]->m_x;
3491 btMatrix3x3 Dm(c1.getX(), c2.getX(), c3.getX(),
3492 c1.getY(), c2.getY(), c3.getY(),
3493 c1.getZ(), c2.getZ(), c3.getZ());
3494 t.m_element_measure = Dm.determinant() * unit_simplex_measure;
3495 t.m_Dm_inverse = Dm.inverse();
3496
3497 // calculate the first three columns of P^{-1}
3498 btVector3 a = t.m_n[0]->m_x;
3499 btVector3 b = t.m_n[1]->m_x;
3500 btVector3 c = t.m_n[2]->m_x;
3501 btVector3 d = t.m_n[3]->m_x;
3502
3503 btScalar det = 1 / (a[0] * b[1] * c[2] - a[0] * b[1] * d[2] - a[0] * b[2] * c[1] + a[0] * b[2] * d[1] + a[0] * c[1] * d[2] - a[0] * c[2] * d[1] + a[1] * (-b[0] * c[2] + b[0] * d[2] + b[2] * c[0] - b[2] * d[0] - c[0] * d[2] + c[2] * d[0]) + a[2] * (b[0] * c[1] - b[0] * d[1] + b[1] * (d[0] - c[0]) + c[0] * d[1] - c[1] * d[0]) - b[0] * c[1] * d[2] + b[0] * c[2] * d[1] + b[1] * c[0] * d[2] - b[1] * c[2] * d[0] - b[2] * c[0] * d[1] + b[2] * c[1] * d[0]);
3504
3505 btScalar P11 = -b[2] * c[1] + d[2] * c[1] + b[1] * c[2] + b[2] * d[1] - c[2] * d[1] - b[1] * d[2];
3506 btScalar P12 = b[2] * c[0] - d[2] * c[0] - b[0] * c[2] - b[2] * d[0] + c[2] * d[0] + b[0] * d[2];
3507 btScalar P13 = -b[1] * c[0] + d[1] * c[0] + b[0] * c[1] + b[1] * d[0] - c[1] * d[0] - b[0] * d[1];
3508 btScalar P21 = a[2] * c[1] - d[2] * c[1] - a[1] * c[2] - a[2] * d[1] + c[2] * d[1] + a[1] * d[2];
3509 btScalar P22 = -a[2] * c[0] + d[2] * c[0] + a[0] * c[2] + a[2] * d[0] - c[2] * d[0] - a[0] * d[2];
3510 btScalar P23 = a[1] * c[0] - d[1] * c[0] - a[0] * c[1] - a[1] * d[0] + c[1] * d[0] + a[0] * d[1];
3511 btScalar P31 = -a[2] * b[1] + d[2] * b[1] + a[1] * b[2] + a[2] * d[1] - b[2] * d[1] - a[1] * d[2];
3512 btScalar P32 = a[2] * b[0] - d[2] * b[0] - a[0] * b[2] - a[2] * d[0] + b[2] * d[0] + a[0] * d[2];
3513 btScalar P33 = -a[1] * b[0] + d[1] * b[0] + a[0] * b[1] + a[1] * d[0] - b[1] * d[0] - a[0] * d[1];
3514 btScalar P41 = a[2] * b[1] - c[2] * b[1] - a[1] * b[2] - a[2] * c[1] + b[2] * c[1] + a[1] * c[2];
3515 btScalar P42 = -a[2] * b[0] + c[2] * b[0] + a[0] * b[2] + a[2] * c[0] - b[2] * c[0] - a[0] * c[2];
3516 btScalar P43 = a[1] * b[0] - c[1] * b[0] - a[0] * b[1] - a[1] * c[0] + b[1] * c[0] + a[0] * c[1];
3517
3518 btVector4 p1(P11 * det, P21 * det, P31 * det, P41 * det);
3519 btVector4 p2(P12 * det, P22 * det, P32 * det, P42 * det);
3520 btVector4 p3(P13 * det, P23 * det, P33 * det, P43 * det);
3521
3522 t.m_P_inv[0] = p1;
3523 t.m_P_inv[1] = p2;
3524 t.m_P_inv[2] = p3;
3525 }
3526}
3527
3528static btScalar Dot4(const btVector4& a, const btVector4& b)
3529{
3530 return a[0] * b[0] + a[1] * b[1] + a[2] * b[2] + a[3] * b[3];
3531}
3532
3534{
3535 btQuaternion q;
3536 for (int i = 0; i < m_tetras.size(); ++i)
3537 {
3539 btVector3 c1 = t.m_n[1]->m_q - t.m_n[0]->m_q;
3540 btVector3 c2 = t.m_n[2]->m_q - t.m_n[0]->m_q;
3541 btVector3 c3 = t.m_n[3]->m_q - t.m_n[0]->m_q;
3542 btMatrix3x3 Ds(c1.getX(), c2.getX(), c3.getX(),
3543 c1.getY(), c2.getY(), c3.getY(),
3544 c1.getZ(), c2.getZ(), c3.getZ());
3545 t.m_F = Ds * t.m_Dm_inverse;
3546
3548 s.m_F = t.m_F;
3549 s.m_J = t.m_F.determinant();
3550 btMatrix3x3 C = t.m_F.transpose() * t.m_F;
3551 s.m_trace = C[0].getX() + C[1].getY() + C[2].getZ();
3552 s.m_cofF = t.m_F.adjoint().transpose();
3553
3554 btVector3 a = t.m_n[0]->m_q;
3555 btVector3 b = t.m_n[1]->m_q;
3556 btVector3 c = t.m_n[2]->m_q;
3557 btVector3 d = t.m_n[3]->m_q;
3558 btVector4 q1(a[0], b[0], c[0], d[0]);
3559 btVector4 q2(a[1], b[1], c[1], d[1]);
3560 btVector4 q3(a[2], b[2], c[2], d[2]);
3561 btMatrix3x3 B(Dot4(q1, t.m_P_inv[0]), Dot4(q1, t.m_P_inv[1]), Dot4(q1, t.m_P_inv[2]),
3562 Dot4(q2, t.m_P_inv[0]), Dot4(q2, t.m_P_inv[1]), Dot4(q2, t.m_P_inv[2]),
3563 Dot4(q3, t.m_P_inv[0]), Dot4(q3, t.m_P_inv[1]), Dot4(q3, t.m_P_inv[2]));
3564 q.setRotation(btVector3(0, 0, 1), 0);
3565 B.extractRotation(q, 0.01); // precision of the rotation is not very important for visual correctness.
3566 btMatrix3x3 Q(q);
3567 s.m_corotation = Q;
3568 }
3569}
3570
3572{
3574 for (int i = 0; i < m_tetras.size(); ++i)
3575 {
3577 }
3578}
3579//
3581{
3582 m_bodies[0].activate();
3583 m_bodies[1].activate();
3584}
3585
3586//
3588{
3589 static const btScalar maxdrift = 4;
3590 Joint::Prepare(dt, iterations);
3591 m_rpos[0] = m_bodies[0].xform() * m_refs[0];
3592 m_rpos[1] = m_bodies[1].xform() * m_refs[1];
3593 m_drift = Clamp(m_rpos[0] - m_rpos[1], maxdrift) * m_erp / dt;
3594 m_rpos[0] -= m_bodies[0].xform().getOrigin();
3595 m_rpos[1] -= m_bodies[1].xform().getOrigin();
3596 m_massmatrix = ImpulseMatrix(m_bodies[0].invMass(), m_bodies[0].invWorldInertia(), m_rpos[0],
3597 m_bodies[1].invMass(), m_bodies[1].invWorldInertia(), m_rpos[1]);
3598 if (m_split > 0)
3599 {
3600 m_sdrift = m_massmatrix * (m_drift * m_split);
3601 m_drift *= 1 - m_split;
3602 }
3603 m_drift /= (btScalar)iterations;
3604}
3605
3606//
3608{
3609 const btVector3 va = m_bodies[0].velocity(m_rpos[0]);
3610 const btVector3 vb = m_bodies[1].velocity(m_rpos[1]);
3611 const btVector3 vr = va - vb;
3612 btSoftBody::Impulse impulse;
3613 impulse.m_asVelocity = 1;
3614 impulse.m_velocity = m_massmatrix * (m_drift + vr * m_cfm) * sor;
3615 m_bodies[0].applyImpulse(-impulse, m_rpos[0]);
3616 m_bodies[1].applyImpulse(impulse, m_rpos[1]);
3617}
3618
3619//
3621{
3622 if (m_split > 0)
3623 {
3624 m_bodies[0].applyDImpulse(-m_sdrift, m_rpos[0]);
3625 m_bodies[1].applyDImpulse(m_sdrift, m_rpos[1]);
3626 }
3627}
3628
3629//
3631{
3632 static const btScalar maxdrift = SIMD_PI / 16;
3633 m_icontrol->Prepare(this);
3634 Joint::Prepare(dt, iterations);
3635 m_axis[0] = m_bodies[0].xform().getBasis() * m_refs[0];
3636 m_axis[1] = m_bodies[1].xform().getBasis() * m_refs[1];
3637 m_drift = NormalizeAny(btCross(m_axis[1], m_axis[0]));
3638 m_drift *= btMin(maxdrift, btAcos(Clamp<btScalar>(btDot(m_axis[0], m_axis[1]), -1, +1)));
3639 m_drift *= m_erp / dt;
3640 m_massmatrix = AngularImpulseMatrix(m_bodies[0].invWorldInertia(), m_bodies[1].invWorldInertia());
3641 if (m_split > 0)
3642 {
3643 m_sdrift = m_massmatrix * (m_drift * m_split);
3644 m_drift *= 1 - m_split;
3645 }
3646 m_drift /= (btScalar)iterations;
3647}
3648
3649//
3651{
3652 const btVector3 va = m_bodies[0].angularVelocity();
3653 const btVector3 vb = m_bodies[1].angularVelocity();
3654 const btVector3 vr = va - vb;
3655 const btScalar sp = btDot(vr, m_axis[0]);
3656 const btVector3 vc = vr - m_axis[0] * m_icontrol->Speed(this, sp);
3657 btSoftBody::Impulse impulse;
3658 impulse.m_asVelocity = 1;
3659 impulse.m_velocity = m_massmatrix * (m_drift + vc * m_cfm) * sor;
3660 m_bodies[0].applyAImpulse(-impulse);
3661 m_bodies[1].applyAImpulse(impulse);
3662}
3663
3664//
3666{
3667 if (m_split > 0)
3668 {
3669 m_bodies[0].applyDAImpulse(-m_sdrift);
3670 m_bodies[1].applyDAImpulse(m_sdrift);
3671 }
3672}
3673
3674//
3676{
3677 Joint::Prepare(dt, iterations);
3678 const bool dodrift = (m_life == 0);
3679 m_delete = (++m_life) > m_maxlife;
3680 if (dodrift)
3681 {
3682 m_drift = m_drift * m_erp / dt;
3683 if (m_split > 0)
3684 {
3685 m_sdrift = m_massmatrix * (m_drift * m_split);
3686 m_drift *= 1 - m_split;
3687 }
3688 m_drift /= (btScalar)iterations;
3689 }
3690 else
3691 {
3692 m_drift = m_sdrift = btVector3(0, 0, 0);
3693 }
3694}
3695
3696//
3698{
3699 const btVector3 va = m_bodies[0].velocity(m_rpos[0]);
3700 const btVector3 vb = m_bodies[1].velocity(m_rpos[1]);
3701 const btVector3 vrel = va - vb;
3702 const btScalar rvac = btDot(vrel, m_normal);
3703 btSoftBody::Impulse impulse;
3704 impulse.m_asVelocity = 1;
3705 impulse.m_velocity = m_drift;
3706 if (rvac < 0)
3707 {
3708 const btVector3 iv = m_normal * rvac;
3709 const btVector3 fv = vrel - iv;
3710 impulse.m_velocity += iv + fv * m_friction;
3711 }
3712 impulse.m_velocity = m_massmatrix * impulse.m_velocity * sor;
3713
3714 if (m_bodies[0].m_soft == m_bodies[1].m_soft)
3715 {
3716 if ((impulse.m_velocity.getX() == impulse.m_velocity.getX()) && (impulse.m_velocity.getY() == impulse.m_velocity.getY()) &&
3717 (impulse.m_velocity.getZ() == impulse.m_velocity.getZ()))
3718 {
3719 if (impulse.m_asVelocity)
3720 {
3721 if (impulse.m_velocity.length() < m_bodies[0].m_soft->m_maxSelfCollisionImpulse)
3722 {
3723 }
3724 else
3725 {
3726 m_bodies[0].applyImpulse(-impulse * m_bodies[0].m_soft->m_selfCollisionImpulseFactor, m_rpos[0]);
3727 m_bodies[1].applyImpulse(impulse * m_bodies[0].m_soft->m_selfCollisionImpulseFactor, m_rpos[1]);
3728 }
3729 }
3730 }
3731 }
3732 else
3733 {
3734 m_bodies[0].applyImpulse(-impulse, m_rpos[0]);
3735 m_bodies[1].applyImpulse(impulse, m_rpos[1]);
3736 }
3737}
3738
3739//
3741{
3742 if (m_split > 0)
3743 {
3744 m_bodies[0].applyDImpulse(-m_sdrift, m_rpos[0]);
3745 m_bodies[1].applyDImpulse(m_sdrift, m_rpos[1]);
3746 }
3747}
3748
3749//
3751{
3752 BT_PROFILE("SoftBody applyForces");
3753 // const btScalar dt = m_sst.sdt;
3754 const btScalar kLF = m_cfg.kLF;
3755 const btScalar kDG = m_cfg.kDG;
3756 const btScalar kPR = m_cfg.kPR;
3757 const btScalar kVC = m_cfg.kVC;
3758 const bool as_lift = kLF > 0;
3759 const bool as_drag = kDG > 0;
3760 const bool as_pressure = kPR != 0;
3761 const bool as_volume = kVC > 0;
3762 const bool as_aero = as_lift ||
3763 as_drag;
3764 //const bool as_vaero = as_aero &&
3765 // (m_cfg.aeromodel < btSoftBody::eAeroModel::F_TwoSided);
3766 //const bool as_faero = as_aero &&
3767 // (m_cfg.aeromodel >= btSoftBody::eAeroModel::F_TwoSided);
3768 const bool use_medium = as_aero;
3769 const bool use_volume = as_pressure ||
3770 as_volume;
3771 btScalar volume = 0;
3772 btScalar ivolumetp = 0;
3773 btScalar dvolumetv = 0;
3774 btSoftBody::sMedium medium;
3775 if (use_volume)
3776 {
3777 volume = getVolume();
3778 ivolumetp = 1 / btFabs(volume) * kPR;
3779 dvolumetv = (m_pose.m_volume - volume) * kVC;
3780 }
3781 /* Per vertex forces */
3782 int i, ni;
3783
3784 for (i = 0, ni = m_nodes.size(); i < ni; ++i)
3785 {
3786 btSoftBody::Node& n = m_nodes[i];
3787 if (n.m_im > 0)
3788 {
3789 if (use_medium)
3790 {
3791 /* Aerodynamics */
3793 }
3794 /* Pressure */
3795 if (as_pressure)
3796 {
3797 n.m_f += n.m_n * (n.m_area * ivolumetp);
3798 }
3799 /* Volume */
3800 if (as_volume)
3801 {
3802 n.m_f += n.m_n * (n.m_area * dvolumetv);
3803 }
3804 }
3805 }
3806
3807 /* Per face forces */
3808 for (i = 0, ni = m_faces.size(); i < ni; ++i)
3809 {
3810 // btSoftBody::Face& f=m_faces[i];
3811
3812 /* Aerodynamics */
3814 }
3815}
3816
3817//
3819{
3820 m_cfg.m_maxStress = maxStress;
3821}
3822
3823//
3825{
3826 if (m_z.size() > 0)
3827 {
3828 for (int i = 0; i < m_renderNodes.size(); ++i)
3829 {
3830 const Node* p0 = m_renderNodesParents[i][0];
3831 const Node* p1 = m_renderNodesParents[i][1];
3832 const Node* p2 = m_renderNodesParents[i][2];
3833 btVector3 normal = btCross(p1->m_x - p0->m_x, p2->m_x - p0->m_x);
3834 btVector3 unit_normal = normal.normalized();
3835 RenderNode& n = m_renderNodes[i];
3836 n.m_x.setZero();
3837 for (int j = 0; j < 3; ++j)
3838 {
3840 }
3841 n.m_x += m_z[i] * unit_normal;
3842 }
3843 }
3844 else
3845 {
3846 for (int i = 0; i < m_renderNodes.size(); ++i)
3847 {
3848 RenderNode& n = m_renderNodes[i];
3849 n.m_x.setZero();
3850 for (int j = 0; j < 4; ++j)
3851 {
3852 if (m_renderNodesParents[i].size())
3853 {
3855 }
3856 }
3857 }
3858 }
3859}
3860
3862{
3863 for (int i = 0; i <= N; ++i)
3864 {
3865 for (int j = 0; i + j <= N; ++j)
3866 {
3867 m_quads.push_back(btVector3(btScalar(i) / btScalar(N), btScalar(j) / btScalar(N), btScalar(N - i - j) / btScalar(N)));
3868 }
3869 }
3870}
3871
3872//
3874{
3875 BT_PROFILE("PSolve_Anchors");
3876 const btScalar kAHR = psb->m_cfg.kAHR * kst;
3877 const btScalar dt = psb->m_sst.sdt;
3878 for (int i = 0, ni = psb->m_anchors.size(); i < ni; ++i)
3879 {
3880 const Anchor& a = psb->m_anchors[i];
3881 const btTransform& t = a.m_body->getWorldTransform();
3882 Node& n = *a.m_node;
3883 const btVector3 wa = t * a.m_local;
3884 const btVector3 va = a.m_body->getVelocityInLocalPoint(a.m_c1) * dt;
3885 const btVector3 vb = n.m_x - n.m_q;
3886 const btVector3 vr = (va - vb) + (wa - n.m_x) * kAHR;
3887 const btVector3 impulse = a.m_c0 * vr * a.m_influence;
3888 n.m_x += impulse * a.m_c2;
3889 a.m_body->applyImpulse(-impulse, a.m_c1);
3890 }
3891}
3892
3893//
3895{
3896 BT_PROFILE("PSolve_RContacts");
3897 const btScalar dt = psb->m_sst.sdt;
3898 const btScalar mrg = psb->getCollisionShape()->getMargin();
3899 btMultiBodyJacobianData jacobianData;
3900 for (int i = 0, ni = psb->m_rcontacts.size(); i < ni; ++i)
3901 {
3902 const RContact& c = psb->m_rcontacts[i];
3903 const sCti& cti = c.m_cti;
3904 if (cti.m_colObj->hasContactResponse())
3905 {
3906 btVector3 va(0, 0, 0);
3907 btRigidBody* rigidCol = 0;
3908 btMultiBodyLinkCollider* multibodyLinkCol = 0;
3909 btScalar* deltaV = NULL;
3910
3912 {
3913 rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
3914 va = rigidCol ? rigidCol->getVelocityInLocalPoint(c.m_c1) * dt : btVector3(0, 0, 0);
3915 }
3917 {
3919 if (multibodyLinkCol)
3920 {
3921 const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
3922 jacobianData.m_jacobians.resize(ndof);
3923 jacobianData.m_deltaVelocitiesUnitImpulse.resize(ndof);
3924 btScalar* jac = &jacobianData.m_jacobians[0];
3925
3926 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);
3927 deltaV = &jacobianData.m_deltaVelocitiesUnitImpulse[0];
3928 multibodyLinkCol->m_multiBody->calcAccelerationDeltasMultiDof(&jacobianData.m_jacobians[0], deltaV, jacobianData.scratch_r, jacobianData.scratch_v);
3929
3930 btScalar vel = 0.0;
3931 for (int j = 0; j < ndof; ++j)
3932 {
3933 vel += multibodyLinkCol->m_multiBody->getVelocityVector()[j] * jac[j];
3934 }
3935 va = cti.m_normal * vel * dt;
3936 }
3937 }
3938
3939 const btVector3 vb = c.m_node->m_x - c.m_node->m_q;
3940 const btVector3 vr = vb - va;
3941 const btScalar dn = btDot(vr, cti.m_normal);
3942 if (dn <= SIMD_EPSILON)
3943 {
3944 const btScalar dp = btMin((btDot(c.m_node->m_x, cti.m_normal) + cti.m_offset), mrg);
3945 const btVector3 fv = vr - (cti.m_normal * dn);
3946 // c0 is the impulse matrix, c3 is 1 - the friction coefficient or 0, c4 is the contact hardness coefficient
3947 const btVector3 impulse = c.m_c0 * ((vr - (fv * c.m_c3) + (cti.m_normal * (dp * c.m_c4))) * kst);
3948 c.m_node->m_x -= impulse * c.m_c2;
3949
3951 {
3952 if (rigidCol)
3953 rigidCol->applyImpulse(impulse, c.m_c1);
3954 }
3956 {
3957 if (multibodyLinkCol)
3958 {
3959 double multiplier = 0.5;
3960 multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV, -impulse.length() * multiplier);
3961 }
3962 }
3963 }
3964 }
3965 }
3966}
3967
3968//
3970{
3971 BT_PROFILE("PSolve_SContacts");
3972
3973 for (int i = 0, ni = psb->m_scontacts.size(); i < ni; ++i)
3974 {
3975 const SContact& c = psb->m_scontacts[i];
3976 const btVector3& nr = c.m_normal;
3977 Node& n = *c.m_node;
3978 Face& f = *c.m_face;
3979 const btVector3 p = BaryEval(f.m_n[0]->m_x,
3980 f.m_n[1]->m_x,
3981 f.m_n[2]->m_x,
3982 c.m_weights);
3983 const btVector3 q = BaryEval(f.m_n[0]->m_q,
3984 f.m_n[1]->m_q,
3985 f.m_n[2]->m_q,
3986 c.m_weights);
3987 const btVector3 vr = (n.m_x - n.m_q) - (p - q);
3988 btVector3 corr(0, 0, 0);
3989 btScalar dot = btDot(vr, nr);
3990 if (dot < 0)
3991 {
3992 const btScalar j = c.m_margin - (btDot(nr, n.m_x) - btDot(nr, p));
3993 corr += c.m_normal * j;
3994 }
3995 corr -= ProjectOnPlane(vr, nr) * c.m_friction;
3996 n.m_x += corr * c.m_cfm[0];
3997 f.m_n[0]->m_x -= corr * (c.m_cfm[1] * c.m_weights.x());
3998 f.m_n[1]->m_x -= corr * (c.m_cfm[1] * c.m_weights.y());
3999 f.m_n[2]->m_x -= corr * (c.m_cfm[1] * c.m_weights.z());
4000 }
4001}
4002
4003//
4005{
4006 BT_PROFILE("PSolve_Links");
4007 for (int i = 0, ni = psb->m_links.size(); i < ni; ++i)
4008 {
4009 Link& l = psb->m_links[i];
4010 if (l.m_c0 > 0)
4011 {
4012 Node& a = *l.m_n[0];
4013 Node& b = *l.m_n[1];
4014 const btVector3 del = b.m_x - a.m_x;
4015 const btScalar len = del.length2();
4016 if (l.m_c1 + len > SIMD_EPSILON)
4017 {
4018 const btScalar k = ((l.m_c1 - len) / (l.m_c0 * (l.m_c1 + len))) * kst;
4019 a.m_x -= del * (k * a.m_im);
4020 b.m_x += del * (k * b.m_im);
4021 }
4022 }
4023 }
4024}
4025
4026//
4028{
4029 BT_PROFILE("VSolve_Links");
4030 for (int i = 0, ni = psb->m_links.size(); i < ni; ++i)
4031 {
4032 Link& l = psb->m_links[i];
4033 Node** n = l.m_n;
4034 const btScalar j = -btDot(l.m_c3, n[0]->m_v - n[1]->m_v) * l.m_c2 * kst;
4035 n[0]->m_v += l.m_c3 * (j * n[0]->m_im);
4036 n[1]->m_v -= l.m_c3 * (j * n[1]->m_im);
4037 }
4038}
4039
4040//
4042{
4043 switch (solver)
4044 {
4045 case ePSolver::Anchors:
4047 case ePSolver::Linear:
4048 return (&btSoftBody::PSolve_Links);
4053 default:
4054 {
4055 }
4056 }
4057 return (0);
4058}
4059
4060//
4062{
4063 switch (solver)
4064 {
4065 case eVSolver::Linear:
4066 return (&btSoftBody::VSolve_Links);
4067 default:
4068 {
4069 }
4070 }
4071 return (0);
4072}
4073
4078
4080{
4081 return m_useSelfCollision;
4082}
4083
4084//
4086{
4088 {
4089 case fCollision::SDF_RS:
4090 {
4093 btTransform wtr = pcoWrap->getWorldTransform();
4094
4095 const btTransform ctr = pcoWrap->getWorldTransform();
4096 const btScalar timemargin = (wtr.getOrigin() - ctr.getOrigin()).length();
4097 const btScalar basemargin = getCollisionShape()->getMargin();
4098 btVector3 mins;
4099 btVector3 maxs;
4101 volume;
4102 pcoWrap->getCollisionShape()->getAabb(pcoWrap->getWorldTransform(),
4103 mins,
4104 maxs);
4105 volume = btDbvtVolume::FromMM(mins, maxs);
4106 volume.Expand(btVector3(basemargin, basemargin, basemargin));
4107 docollide.psb = this;
4108 docollide.m_colObj1Wrap = pcoWrap;
4109 docollide.m_rigidBody = prb1;
4110
4111 docollide.dynmargin = basemargin + timemargin;
4112 docollide.stamargin = basemargin;
4113 m_ndbvt.collideTV(m_ndbvt.m_root, volume, docollide);
4114 }
4115 break;
4116 case fCollision::CL_RS:
4117 {
4119 collider.ProcessColObj(this, pcoWrap);
4120 }
4121 break;
4122 case fCollision::SDF_RD:
4123 {
4125 if (this->isActive())
4126 {
4127 const btTransform wtr = pcoWrap->getWorldTransform();
4128 const btScalar timemargin = 0;
4129 const btScalar basemargin = getCollisionShape()->getMargin();
4130 btVector3 mins;
4131 btVector3 maxs;
4133 volume;
4134 pcoWrap->getCollisionShape()->getAabb(wtr,
4135 mins,
4136 maxs);
4137 volume = btDbvtVolume::FromMM(mins, maxs);
4138 volume.Expand(btVector3(basemargin, basemargin, basemargin));
4140 {
4141 btSoftColliders::CollideSDF_RD docollideNode;
4142 docollideNode.psb = this;
4143 docollideNode.m_colObj1Wrap = pcoWrap;
4144 docollideNode.m_rigidBody = prb1;
4145 docollideNode.dynmargin = basemargin + timemargin;
4146 docollideNode.stamargin = basemargin;
4147 m_ndbvt.collideTV(m_ndbvt.m_root, volume, docollideNode);
4148 }
4149
4151 {
4152 btSoftColliders::CollideSDF_RDF docollideFace;
4153 docollideFace.psb = this;
4154 docollideFace.m_colObj1Wrap = pcoWrap;
4155 docollideFace.m_rigidBody = prb1;
4156 docollideFace.dynmargin = basemargin + timemargin;
4157 docollideFace.stamargin = basemargin;
4158 m_fdbvt.collideTV(m_fdbvt.m_root, volume, docollideFace);
4159 }
4160 }
4161 }
4162 break;
4163 }
4164}
4165
4166//
4168{
4169 BT_PROFILE("Deformable Collision");
4170 const int cf = m_cfg.collisions & psb->m_cfg.collisions;
4171 switch (cf & fCollision::SVSmask)
4172 {
4173 case fCollision::CL_SS:
4174 {
4175 //support self-collision if CL_SELF flag set
4176 if (this != psb || psb->m_cfg.collisions & fCollision::CL_SELF)
4177 {
4179 docollide.ProcessSoftSoft(this, psb);
4180 }
4181 }
4182 break;
4183 case fCollision::VF_SS:
4184 {
4185 //only self-collision for Cluster, not Vertex-Face yet
4186 if (this != psb)
4187 {
4189 /* common */
4190 docollide.mrg = getCollisionShape()->getMargin() +
4191 psb->getCollisionShape()->getMargin();
4192 /* psb0 nodes vs psb1 faces */
4193 docollide.psb[0] = this;
4194 docollide.psb[1] = psb;
4195 docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root,
4196 docollide.psb[1]->m_fdbvt.m_root,
4197 docollide);
4198 /* psb1 nodes vs psb0 faces */
4199 docollide.psb[0] = psb;
4200 docollide.psb[1] = this;
4201 docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root,
4202 docollide.psb[1]->m_fdbvt.m_root,
4203 docollide);
4204 }
4205 }
4206 break;
4207 case fCollision::VF_DD:
4208 {
4209 if (!psb->m_softSoftCollision)
4210 return;
4211 if (psb->isActive() || this->isActive())
4212 {
4213 if (this != psb)
4214 {
4216 /* common */
4217 docollide.mrg = getCollisionShape()->getMargin() +
4218 psb->getCollisionShape()->getMargin();
4219 /* psb0 nodes vs psb1 faces */
4220 if (psb->m_tetras.size() > 0)
4221 docollide.useFaceNormal = true;
4222 else
4223 docollide.useFaceNormal = false;
4224 docollide.psb[0] = this;
4225 docollide.psb[1] = psb;
4226 docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root,
4227 docollide.psb[1]->m_fdbvt.m_root,
4228 docollide);
4229
4230 /* psb1 nodes vs psb0 faces */
4231 if (this->m_tetras.size() > 0)
4232 docollide.useFaceNormal = true;
4233 else
4234 docollide.useFaceNormal = false;
4235 docollide.psb[0] = psb;
4236 docollide.psb[1] = this;
4237 docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root,
4238 docollide.psb[1]->m_fdbvt.m_root,
4239 docollide);
4240 }
4241 else
4242 {
4243 if (psb->useSelfCollision())
4244 {
4246 docollide.mrg = 2 * getCollisionShape()->getMargin();
4247 docollide.psb[0] = this;
4248 docollide.psb[1] = psb;
4249 if (this->m_tetras.size() > 0)
4250 docollide.useFaceNormal = true;
4251 else
4252 docollide.useFaceNormal = false;
4253 /* psb0 faces vs psb0 faces */
4255 this->m_fdbvt.selfCollideT(m_fdbvnt, docollide);
4256 }
4257 }
4258 }
4259 }
4260 break;
4261 default:
4262 {
4263 }
4264 }
4265}
4266
4268{
4269 if (psb->isActive() || this->isActive())
4270 {
4271 if (this != psb)
4272 {
4274 /* common */
4275 docollide.mrg = SAFE_EPSILON; // for rounding error instead of actual margin
4276 docollide.dt = psb->m_sst.sdt;
4277 /* psb0 nodes vs psb1 faces */
4278 if (psb->m_tetras.size() > 0)
4279 docollide.useFaceNormal = true;
4280 else
4281 docollide.useFaceNormal = false;
4282 docollide.psb[0] = this;
4283 docollide.psb[1] = psb;
4284 docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root,
4285 docollide.psb[1]->m_fdbvt.m_root,
4286 docollide);
4287 /* psb1 nodes vs psb0 faces */
4288 if (this->m_tetras.size() > 0)
4289 docollide.useFaceNormal = true;
4290 else
4291 docollide.useFaceNormal = false;
4292 docollide.psb[0] = psb;
4293 docollide.psb[1] = this;
4294 docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root,
4295 docollide.psb[1]->m_fdbvt.m_root,
4296 docollide);
4297 }
4298 else
4299 {
4300 if (psb->useSelfCollision())
4301 {
4303 docollide.mrg = SAFE_EPSILON;
4304 docollide.psb[0] = this;
4305 docollide.psb[1] = psb;
4306 docollide.dt = psb->m_sst.sdt;
4307 if (this->m_tetras.size() > 0)
4308 docollide.useFaceNormal = true;
4309 else
4310 docollide.useFaceNormal = false;
4311 /* psb0 faces vs psb0 faces */
4312 calculateNormalCone(this->m_fdbvnt); // should compute this outside of this scope
4313 this->m_fdbvt.selfCollideT(m_fdbvnt, docollide);
4314 }
4315 }
4316 }
4317}
4318
4320{
4321 m_windVelocity = velocity;
4322}
4323
4325{
4326 return m_windVelocity;
4327}
4328
4330{
4331 int sz = sizeof(btSoftBodyData);
4332 return sz;
4333}
4334
4336const char* btSoftBody::serialize(void* dataBuffer, class btSerializer* serializer) const
4337{
4338 btSoftBodyData* sbd = (btSoftBodyData*)dataBuffer;
4339
4340 btCollisionObject::serialize(&sbd->m_collisionObjectData, serializer);
4341
4342 btHashMap<btHashPtr, int> m_nodeIndexMap;
4343
4344 sbd->m_numMaterials = m_materials.size();
4345 sbd->m_materials = sbd->m_numMaterials ? (SoftBodyMaterialData**)serializer->getUniquePointer((void*)&m_materials) : 0;
4346
4347 if (sbd->m_materials)
4348 {
4349 int sz = sizeof(SoftBodyMaterialData*);
4350 int numElem = sbd->m_numMaterials;
4351 btChunk* chunk = serializer->allocate(sz, numElem);
4352 //SoftBodyMaterialData** memPtr = chunk->m_oldPtr;
4354 for (int i = 0; i < numElem; i++, memPtr++)
4355 {
4357 *memPtr = mat ? (SoftBodyMaterialData*)serializer->getUniquePointer((void*)mat) : 0;
4358 if (!serializer->findPointer(mat))
4359 {
4360 //serialize it here
4361 btChunk* chunk = serializer->allocate(sizeof(SoftBodyMaterialData), 1);
4363 memPtr->m_flags = mat->m_flags;
4364 memPtr->m_angularStiffness = mat->m_kAST;
4365 memPtr->m_linearStiffness = mat->m_kLST;
4366 memPtr->m_volumeStiffness = mat->m_kVST;
4367 serializer->finalizeChunk(chunk, "SoftBodyMaterialData", BT_SBMATERIAL_CODE, mat);
4368 }
4369 }
4370 serializer->finalizeChunk(chunk, "SoftBodyMaterialData", BT_ARRAY_CODE, (void*)&m_materials);
4371 }
4372
4373 sbd->m_numNodes = m_nodes.size();
4374 sbd->m_nodes = sbd->m_numNodes ? (SoftBodyNodeData*)serializer->getUniquePointer((void*)&m_nodes) : 0;
4375 if (sbd->m_nodes)
4376 {
4377 int sz = sizeof(SoftBodyNodeData);
4378 int numElem = sbd->m_numNodes;
4379 btChunk* chunk = serializer->allocate(sz, numElem);
4380 SoftBodyNodeData* memPtr = (SoftBodyNodeData*)chunk->m_oldPtr;
4381 for (int i = 0; i < numElem; i++, memPtr++)
4382 {
4383 m_nodes[i].m_f.serializeFloat(memPtr->m_accumulatedForce);
4384 memPtr->m_area = m_nodes[i].m_area;
4385 memPtr->m_attach = m_nodes[i].m_battach;
4386 memPtr->m_inverseMass = m_nodes[i].m_im;
4387 memPtr->m_material = m_nodes[i].m_material ? (SoftBodyMaterialData*)serializer->getUniquePointer((void*)m_nodes[i].m_material) : 0;
4388 m_nodes[i].m_n.serializeFloat(memPtr->m_normal);
4389 m_nodes[i].m_x.serializeFloat(memPtr->m_position);
4390 m_nodes[i].m_q.serializeFloat(memPtr->m_previousPosition);
4391 m_nodes[i].m_v.serializeFloat(memPtr->m_velocity);
4392 m_nodeIndexMap.insert(&m_nodes[i], i);
4393 }
4394 serializer->finalizeChunk(chunk, "SoftBodyNodeData", BT_SBNODE_CODE, (void*)&m_nodes);
4395 }
4396
4397 sbd->m_numLinks = m_links.size();
4398 sbd->m_links = sbd->m_numLinks ? (SoftBodyLinkData*)serializer->getUniquePointer((void*)&m_links[0]) : 0;
4399 if (sbd->m_links)
4400 {
4401 int sz = sizeof(SoftBodyLinkData);
4402 int numElem = sbd->m_numLinks;
4403 btChunk* chunk = serializer->allocate(sz, numElem);
4404 SoftBodyLinkData* memPtr = (SoftBodyLinkData*)chunk->m_oldPtr;
4405 for (int i = 0; i < numElem; i++, memPtr++)
4406 {
4407 memPtr->m_bbending = m_links[i].m_bbending;
4408 memPtr->m_material = m_links[i].m_material ? (SoftBodyMaterialData*)serializer->getUniquePointer((void*)m_links[i].m_material) : 0;
4409 memPtr->m_nodeIndices[0] = m_links[i].m_n[0] ? m_links[i].m_n[0] - &m_nodes[0] : -1;
4410 memPtr->m_nodeIndices[1] = m_links[i].m_n[1] ? m_links[i].m_n[1] - &m_nodes[0] : -1;
4411 btAssert(memPtr->m_nodeIndices[0] < m_nodes.size());
4412 btAssert(memPtr->m_nodeIndices[1] < m_nodes.size());
4413 memPtr->m_restLength = m_links[i].m_rl;
4414 }
4415 serializer->finalizeChunk(chunk, "SoftBodyLinkData", BT_ARRAY_CODE, (void*)&m_links[0]);
4416 }
4417
4418 sbd->m_numFaces = m_faces.size();
4419 sbd->m_faces = sbd->m_numFaces ? (SoftBodyFaceData*)serializer->getUniquePointer((void*)&m_faces[0]) : 0;
4420 if (sbd->m_faces)
4421 {
4422 int sz = sizeof(SoftBodyFaceData);
4423 int numElem = sbd->m_numFaces;
4424 btChunk* chunk = serializer->allocate(sz, numElem);
4425 SoftBodyFaceData* memPtr = (SoftBodyFaceData*)chunk->m_oldPtr;
4426 for (int i = 0; i < numElem; i++, memPtr++)
4427 {
4428 memPtr->m_material = m_faces[i].m_material ? (SoftBodyMaterialData*)serializer->getUniquePointer((void*)m_faces[i].m_material) : 0;
4429 m_faces[i].m_normal.serializeFloat(memPtr->m_normal);
4430 for (int j = 0; j < 3; j++)
4431 {
4432 memPtr->m_nodeIndices[j] = m_faces[i].m_n[j] ? m_faces[i].m_n[j] - &m_nodes[0] : -1;
4433 }
4434 memPtr->m_restArea = m_faces[i].m_ra;
4435 }
4436 serializer->finalizeChunk(chunk, "SoftBodyFaceData", BT_ARRAY_CODE, (void*)&m_faces[0]);
4437 }
4438
4439 sbd->m_numTetrahedra = m_tetras.size();
4440 sbd->m_tetrahedra = sbd->m_numTetrahedra ? (SoftBodyTetraData*)serializer->getUniquePointer((void*)&m_tetras[0]) : 0;
4441 if (sbd->m_tetrahedra)
4442 {
4443 int sz = sizeof(SoftBodyTetraData);
4444 int numElem = sbd->m_numTetrahedra;
4445 btChunk* chunk = serializer->allocate(sz, numElem);
4446 SoftBodyTetraData* memPtr = (SoftBodyTetraData*)chunk->m_oldPtr;
4447 for (int i = 0; i < numElem; i++, memPtr++)
4448 {
4449 for (int j = 0; j < 4; j++)
4450 {
4451 m_tetras[i].m_c0[j].serializeFloat(memPtr->m_c0[j]);
4452 memPtr->m_nodeIndices[j] = m_tetras[i].m_n[j] ? m_tetras[i].m_n[j] - &m_nodes[0] : -1;
4453 }
4454 memPtr->m_c1 = m_tetras[i].m_c1;
4455 memPtr->m_c2 = m_tetras[i].m_c2;
4456 memPtr->m_material = m_tetras[i].m_material ? (SoftBodyMaterialData*)serializer->getUniquePointer((void*)m_tetras[i].m_material) : 0;
4457 memPtr->m_restVolume = m_tetras[i].m_rv;
4458 }
4459 serializer->finalizeChunk(chunk, "SoftBodyTetraData", BT_ARRAY_CODE, (void*)&m_tetras[0]);
4460 }
4461
4462 sbd->m_numAnchors = m_anchors.size();
4463 sbd->m_anchors = sbd->m_numAnchors ? (SoftRigidAnchorData*)serializer->getUniquePointer((void*)&m_anchors[0]) : 0;
4464 if (sbd->m_anchors)
4465 {
4466 int sz = sizeof(SoftRigidAnchorData);
4467 int numElem = sbd->m_numAnchors;
4468 btChunk* chunk = serializer->allocate(sz, numElem);
4470 for (int i = 0; i < numElem; i++, memPtr++)
4471 {
4472 m_anchors[i].m_c0.serializeFloat(memPtr->m_c0);
4473 m_anchors[i].m_c1.serializeFloat(memPtr->m_c1);
4474 memPtr->m_c2 = m_anchors[i].m_c2;
4475 m_anchors[i].m_local.serializeFloat(memPtr->m_localFrame);
4476 memPtr->m_nodeIndex = m_anchors[i].m_node ? m_anchors[i].m_node - &m_nodes[0] : -1;
4477
4478 memPtr->m_rigidBody = m_anchors[i].m_body ? (btRigidBodyData*)serializer->getUniquePointer((void*)m_anchors[i].m_body) : 0;
4479 btAssert(memPtr->m_nodeIndex < m_nodes.size());
4480 }
4481 serializer->finalizeChunk(chunk, "SoftRigidAnchorData", BT_ARRAY_CODE, (void*)&m_anchors[0]);
4482 }
4483
4484 sbd->m_config.m_dynamicFriction = m_cfg.kDF;
4485 sbd->m_config.m_baumgarte = m_cfg.kVCF;
4486 sbd->m_config.m_pressure = m_cfg.kPR;
4487 sbd->m_config.m_aeroModel = this->m_cfg.aeromodel;
4488 sbd->m_config.m_lift = m_cfg.kLF;
4489 sbd->m_config.m_drag = m_cfg.kDG;
4490 sbd->m_config.m_positionIterations = m_cfg.piterations;
4491 sbd->m_config.m_driftIterations = m_cfg.diterations;
4492 sbd->m_config.m_clusterIterations = m_cfg.citerations;
4493 sbd->m_config.m_velocityIterations = m_cfg.viterations;
4494 sbd->m_config.m_maxVolume = m_cfg.maxvolume;
4495 sbd->m_config.m_damping = m_cfg.kDP;
4496 sbd->m_config.m_poseMatch = m_cfg.kMT;
4497 sbd->m_config.m_collisionFlags = m_cfg.collisions;
4498 sbd->m_config.m_volume = m_cfg.kVC;
4499 sbd->m_config.m_rigidContactHardness = m_cfg.kCHR;
4500 sbd->m_config.m_kineticContactHardness = m_cfg.kKHR;
4501 sbd->m_config.m_softContactHardness = m_cfg.kSHR;
4502 sbd->m_config.m_anchorHardness = m_cfg.kAHR;
4503 sbd->m_config.m_timeScale = m_cfg.timescale;
4504 sbd->m_config.m_maxVolume = m_cfg.maxvolume;
4505 sbd->m_config.m_softRigidClusterHardness = m_cfg.kSRHR_CL;
4506 sbd->m_config.m_softKineticClusterHardness = m_cfg.kSKHR_CL;
4507 sbd->m_config.m_softSoftClusterHardness = m_cfg.kSSHR_CL;
4508 sbd->m_config.m_softRigidClusterImpulseSplit = m_cfg.kSR_SPLT_CL;
4509 sbd->m_config.m_softKineticClusterImpulseSplit = m_cfg.kSK_SPLT_CL;
4510 sbd->m_config.m_softSoftClusterImpulseSplit = m_cfg.kSS_SPLT_CL;
4511
4512 //pose for shape matching
4513 {
4514 sbd->m_pose = (SoftBodyPoseData*)serializer->getUniquePointer((void*)&m_pose);
4515
4516 int sz = sizeof(SoftBodyPoseData);
4517 btChunk* chunk = serializer->allocate(sz, 1);
4518 SoftBodyPoseData* memPtr = (SoftBodyPoseData*)chunk->m_oldPtr;
4519
4521 memPtr->m_bframe = m_pose.m_bframe;
4522 memPtr->m_bvolume = m_pose.m_bvolume;
4524
4525 memPtr->m_numPositions = m_pose.m_pos.size();
4526 memPtr->m_positions = memPtr->m_numPositions ? (btVector3FloatData*)serializer->getUniquePointer((void*)&m_pose.m_pos[0]) : 0;
4527 if (memPtr->m_numPositions)
4528 {
4529 int numElem = memPtr->m_numPositions;
4530 int sz = sizeof(btVector3Data);
4531 btChunk* chunk = serializer->allocate(sz, numElem);
4533 for (int i = 0; i < numElem; i++, memPtr++)
4534 {
4535 m_pose.m_pos[i].serializeFloat(*memPtr);
4536 }
4537 serializer->finalizeChunk(chunk, "btVector3FloatData", BT_ARRAY_CODE, (void*)&m_pose.m_pos[0]);
4538 }
4539 memPtr->m_restVolume = m_pose.m_volume;
4542
4543 memPtr->m_numWeigts = m_pose.m_wgh.size();
4544 memPtr->m_weights = memPtr->m_numWeigts ? (float*)serializer->getUniquePointer((void*)&m_pose.m_wgh[0]) : 0;
4545 if (memPtr->m_numWeigts)
4546 {
4547 int numElem = memPtr->m_numWeigts;
4548 int sz = sizeof(float);
4549 btChunk* chunk = serializer->allocate(sz, numElem);
4550 float* memPtr = (float*)chunk->m_oldPtr;
4551 for (int i = 0; i < numElem; i++, memPtr++)
4552 {
4553 *memPtr = m_pose.m_wgh[i];
4554 }
4555 serializer->finalizeChunk(chunk, "float", BT_ARRAY_CODE, (void*)&m_pose.m_wgh[0]);
4556 }
4557
4558 serializer->finalizeChunk(chunk, "SoftBodyPoseData", BT_ARRAY_CODE, (void*)&m_pose);
4559 }
4560
4561 //clusters for convex-cluster collision detection
4562
4563 sbd->m_numClusters = m_clusters.size();
4564 sbd->m_clusters = sbd->m_numClusters ? (SoftBodyClusterData*)serializer->getUniquePointer((void*)m_clusters[0]) : 0;
4565 if (sbd->m_numClusters)
4566 {
4567 int numElem = sbd->m_numClusters;
4568 int sz = sizeof(SoftBodyClusterData);
4569 btChunk* chunk = serializer->allocate(sz, numElem);
4571 for (int i = 0; i < numElem; i++, memPtr++)
4572 {
4573 memPtr->m_adamping = m_clusters[i]->m_adamping;
4574 m_clusters[i]->m_av.serializeFloat(memPtr->m_av);
4575 memPtr->m_clusterIndex = m_clusters[i]->m_clusterIndex;
4576 memPtr->m_collide = m_clusters[i]->m_collide;
4577 m_clusters[i]->m_com.serializeFloat(memPtr->m_com);
4578 memPtr->m_containsAnchor = m_clusters[i]->m_containsAnchor;
4579 m_clusters[i]->m_dimpulses[0].serializeFloat(memPtr->m_dimpulses[0]);
4580 m_clusters[i]->m_dimpulses[1].serializeFloat(memPtr->m_dimpulses[1]);
4581 m_clusters[i]->m_framexform.serializeFloat(memPtr->m_framexform);
4582 memPtr->m_idmass = m_clusters[i]->m_idmass;
4583 memPtr->m_imass = m_clusters[i]->m_imass;
4584 m_clusters[i]->m_invwi.serializeFloat(memPtr->m_invwi);
4585 memPtr->m_ldamping = m_clusters[i]->m_ldamping;
4586 m_clusters[i]->m_locii.serializeFloat(memPtr->m_locii);
4587 m_clusters[i]->m_lv.serializeFloat(memPtr->m_lv);
4588 memPtr->m_matching = m_clusters[i]->m_matching;
4589 memPtr->m_maxSelfCollisionImpulse = m_clusters[i]->m_maxSelfCollisionImpulse;
4590 memPtr->m_ndamping = m_clusters[i]->m_ndamping;
4591 memPtr->m_ldamping = m_clusters[i]->m_ldamping;
4592 memPtr->m_adamping = m_clusters[i]->m_adamping;
4593 memPtr->m_selfCollisionImpulseFactor = m_clusters[i]->m_selfCollisionImpulseFactor;
4594
4595 memPtr->m_numFrameRefs = m_clusters[i]->m_framerefs.size();
4596 memPtr->m_numMasses = m_clusters[i]->m_masses.size();
4597 memPtr->m_numNodes = m_clusters[i]->m_nodes.size();
4598
4599 memPtr->m_nvimpulses = m_clusters[i]->m_nvimpulses;
4600 m_clusters[i]->m_vimpulses[0].serializeFloat(memPtr->m_vimpulses[0]);
4601 m_clusters[i]->m_vimpulses[1].serializeFloat(memPtr->m_vimpulses[1]);
4602 memPtr->m_ndimpulses = m_clusters[i]->m_ndimpulses;
4603
4604 memPtr->m_framerefs = memPtr->m_numFrameRefs ? (btVector3FloatData*)serializer->getUniquePointer((void*)&m_clusters[i]->m_framerefs[0]) : 0;
4605 if (memPtr->m_framerefs)
4606 {
4607 int numElem = memPtr->m_numFrameRefs;
4608 int sz = sizeof(btVector3FloatData);
4609 btChunk* chunk = serializer->allocate(sz, numElem);
4611 for (int j = 0; j < numElem; j++, memPtr++)
4612 {
4613 m_clusters[i]->m_framerefs[j].serializeFloat(*memPtr);
4614 }
4615 serializer->finalizeChunk(chunk, "btVector3FloatData", BT_ARRAY_CODE, (void*)&m_clusters[i]->m_framerefs[0]);
4616 }
4617
4618 memPtr->m_masses = memPtr->m_numMasses ? (float*)serializer->getUniquePointer((void*)&m_clusters[i]->m_masses[0]) : 0;
4619 if (memPtr->m_masses)
4620 {
4621 int numElem = memPtr->m_numMasses;
4622 int sz = sizeof(float);
4623 btChunk* chunk = serializer->allocate(sz, numElem);
4624 float* memPtr = (float*)chunk->m_oldPtr;
4625 for (int j = 0; j < numElem; j++, memPtr++)
4626 {
4627 *memPtr = m_clusters[i]->m_masses[j];
4628 }
4629 serializer->finalizeChunk(chunk, "float", BT_ARRAY_CODE, (void*)&m_clusters[i]->m_masses[0]);
4630 }
4631
4632 memPtr->m_nodeIndices = memPtr->m_numNodes ? (int*)serializer->getUniquePointer((void*)&m_clusters[i]->m_nodes) : 0;
4633 if (memPtr->m_nodeIndices)
4634 {
4635 int numElem = memPtr->m_numMasses;
4636 int sz = sizeof(int);
4637 btChunk* chunk = serializer->allocate(sz, numElem);
4638 int* memPtr = (int*)chunk->m_oldPtr;
4639 for (int j = 0; j < numElem; j++, memPtr++)
4640 {
4641 int* indexPtr = m_nodeIndexMap.find(m_clusters[i]->m_nodes[j]);
4642 btAssert(indexPtr);
4643 *memPtr = *indexPtr;
4644 }
4645 serializer->finalizeChunk(chunk, "int", BT_ARRAY_CODE, (void*)&m_clusters[i]->m_nodes);
4646 }
4647 }
4648 serializer->finalizeChunk(chunk, "SoftBodyClusterData", BT_ARRAY_CODE, (void*)m_clusters[0]);
4649 }
4650
4651 sbd->m_numJoints = m_joints.size();
4652 sbd->m_joints = m_joints.size() ? (btSoftBodyJointData*)serializer->getUniquePointer((void*)&m_joints[0]) : 0;
4653
4654 if (sbd->m_joints)
4655 {
4656 int sz = sizeof(btSoftBodyJointData);
4657 int numElem = m_joints.size();
4658 btChunk* chunk = serializer->allocate(sz, numElem);
4660
4661 for (int i = 0; i < numElem; i++, memPtr++)
4662 {
4663 memPtr->m_jointType = (int)m_joints[i]->Type();
4664 m_joints[i]->m_refs[0].serializeFloat(memPtr->m_refs[0]);
4665 m_joints[i]->m_refs[1].serializeFloat(memPtr->m_refs[1]);
4666 memPtr->m_cfm = m_joints[i]->m_cfm;
4667 memPtr->m_erp = float(m_joints[i]->m_erp);
4668 memPtr->m_split = float(m_joints[i]->m_split);
4669 memPtr->m_delete = m_joints[i]->m_delete;
4670
4671 for (int j = 0; j < 4; j++)
4672 {
4673 memPtr->m_relPosition[0].m_floats[j] = 0.f;
4674 memPtr->m_relPosition[1].m_floats[j] = 0.f;
4675 }
4676 memPtr->m_bodyA = 0;
4677 memPtr->m_bodyB = 0;
4678 if (m_joints[i]->m_bodies[0].m_soft)
4679 {
4681 memPtr->m_bodyA = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[0].m_soft);
4682 }
4683 if (m_joints[i]->m_bodies[0].m_collisionObject)
4684 {
4686 memPtr->m_bodyA = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[0].m_collisionObject);
4687 }
4688 if (m_joints[i]->m_bodies[0].m_rigid)
4689 {
4691 memPtr->m_bodyA = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[0].m_rigid);
4692 }
4693
4694 if (m_joints[i]->m_bodies[1].m_soft)
4695 {
4697 memPtr->m_bodyB = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[1].m_soft);
4698 }
4699 if (m_joints[i]->m_bodies[1].m_collisionObject)
4700 {
4702 memPtr->m_bodyB = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[1].m_collisionObject);
4703 }
4704 if (m_joints[i]->m_bodies[1].m_rigid)
4705 {
4707 memPtr->m_bodyB = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[1].m_rigid);
4708 }
4709 }
4710 serializer->finalizeChunk(chunk, "btSoftBodyJointData", BT_ARRAY_CODE, (void*)&m_joints[0]);
4711 }
4712
4713 return btSoftBodyDataName;
4714}
4715
4717{
4719 return;
4720
4722 {
4723 m_deactivationTime += timeStep;
4724 }
4725 else
4726 {
4729 }
4730}
4731
4733{
4734 for (int i = 0; i < m_nodes.size(); ++i)
4735 {
4736 m_nodes[i].m_v.setZero();
4737 }
4738}
4739
4741{
4743 return false;
4744
4745 //disable deactivation
4747 return false;
4748
4750 return true;
4751
4753 {
4754 return true;
4755 }
4756 return false;
4757}
#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
void singularValueDecomposition(const btMatrix2x2 &A, GivensRotation &U, const btMatrix2x2 &Sigma, GivensRotation &V, const btScalar tol=64 *std::numeric_limits< btScalar >::epsilon())
2x2 SVD (singular value decomposition) A=USV'
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.
btQuaternion inverse(const btQuaternion &q)
Return the inverse of a quaternion.
btScalar length(const btQuaternion &q)
Return the length of a quaternion.
btScalar btAngle(const btQuaternion &q1, const btQuaternion &q2)
Return the angle between two quaternions.
#define BT_PROFILE(name)
bool gDisableDeactivation
btScalar gDeactivationTime
#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
#define BT_ARRAY_CODE
#define BT_SBNODE_CODE
@ 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 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, const btVector3 &a, const btVector3 &b, const btVector3 &c, btVector3 &bary)
static btScalar Dot4(const btVector4 &a, const btVector4 &b)
static btDbvtNode * buildTreeBottomUp(btAlignedObjectArray< btDbvtNode * > &leafNodes, btAlignedObjectArray< btAlignedObjectArray< int > > &adj)
btSoftBody implementation by Nathanael Presson
static btDbvntNode * copyToDbvnt(const btDbvtNode *n)
static unsigned long seed
Definition btSoftBody.h:39
#define btSoftBodyDataName
Definition btSoftBody.h:37
#define btSoftBodyData
btSoftBody implementation by Nathanael Presson
Definition btSoftBody.h:36
#define NEXTRAND
#define SAFE_EPSILON
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
The btAlignedObjectArray template class uses a subset of the stl::vector interface for its methods It...
int size() const
return the number of elements in the array
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
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...
virtual btScalar getMargin() const =0
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:50
btMatrix3x3 adjoint() const
Return the adjoint of the matrix.
btMatrix3x3 inverse() const
Return the inverse of the matrix.
btMatrix3x3 transpose() const
Return the transpose of the matrix.
void setIdentity()
Set the matrix to the identity.
btScalar determinant() const
Return the determinant of the matrix.
void extractRotation(btQuaternion &q, btScalar tolerance=1.0e-9, int maxIter=100)
extractRotation is from "A robust method to extract the rotational part of deformations" See http://d...
void setZero()
Set the matrix to the identity.
void serializeFloat(struct btMatrix3x3FloatData &dataOut) const
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
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
void applyDeltaVeeMultiDof(const btScalar *delta_vee, btScalar multiplier)
const btScalar * getVelocityVector() const
The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatr...
void setRotation(const btVector3 &axis, const btScalar &_angle)
Set the rotation using axis angle notation.
The btRigidBody is the main class for rigid body objects.
Definition btRigidBody.h:60
btVector3 getVelocityInLocalPoint(const btVector3 &rel_pos) const
btScalar getInvMass() const
const btCollisionShape * getCollisionShape() const
void applyImpulse(const btVector3 &impulse, const btVector3 &rel_pos)
static const btRigidBody * upcast(const btCollisionObject *colObj)
to keep collision detection and dynamics separate we don't store a rigidbody pointer but a rigidbody ...
const btMatrix3x3 & getInvInertiaTensorWorld() const
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:372
btMultiBodyJacobianData jacobianData_normal
Definition btSoftBody.h:371
btMultiBodyJacobianData jacobianData_t2
Definition btSoftBody.h:373
The btSoftBody is an class to simulate cloth and volumetric soft bodies.
Definition btSoftBody.h:75
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
bool m_bUpdateRtCst
Definition btSoftBody.h:834
btScalar m_sleepingThreshold
Definition btSoftBody.h:841
virtual void transformTo(const btTransform &trs)
btVector3 getLinearVelocity()
bool checkFace(int node0, int node1, int node2) const
void advanceDeformation()
void setGravityFactor(btScalar gravFactor)
void updateClusters()
btDbvt m_cdbvt
Definition btSoftBody.h:838
void setPose(bool bvolume, bool bframe)
bool cutLink(int node0, int node1, btScalar position)
void appendFace(int model=-1, Material *mat=0)
void setMass(int node, btScalar mass)
void interpolateRenderMesh()
tJointArray m_joints
Definition btSoftBody.h:830
btScalar m_dampingCoefficient
Definition btSoftBody.h:840
btAlignedObjectArray< TetraScratch > m_tetraScratchesTn
Definition btSoftBody.h:821
void integrateMotion()
void rebuildNodeTree()
bool rayFaceTest(const btVector3 &rayFrom, const btVector3 &rayTo, sRayCast &results)
void appendLinearJoint(const LJoint::Specs &specs, Cluster *body0, Body body1)
virtual void scale(const btVector3 &scl)
btAlignedObjectArray< bool > m_clusterConnectivity
Definition btSoftBody.h:855
void updateFaceTree(bool use_velocity, bool margin)
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:809
void addVelocity(const btVector3 &velocity)
void predictMotion(btScalar dt)
void(*) vsolver_t(btSoftBody *, btScalar)
Definition btSoftBody.h:786
void setSelfCollision(bool useSelfCollision)
void setLinearVelocity(const btVector3 &linVel)
btScalar m_timeacc
Definition btSoftBody.h:832
void appendTetra(int model, Material *mat)
void setRestLengthScale(btScalar restLength)
btDbvntNode * m_fdbvnt
Definition btSoftBody.h:837
virtual void rotate(const btQuaternion &rot)
void applyClusters(bool drift)
void setZeroVelocity()
static void PSolve_Anchors(btSoftBody *psb, btScalar kst, btScalar ti)
btSoftBodyWorldInfo * m_worldInfo
Definition btSoftBody.h:812
void updateArea(bool averageArea=true)
void addForce(const btVector3 &force)
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:819
bool useSelfCollision()
btVector3 evaluateCom() const
void setTotalDensity(btScalar density)
btAlignedObjectArray< const class btCollisionObject * > m_collisionDisabledObjects
Definition btSoftBody.h:77
static void clusterDAImpulse(Cluster *cluster, const btVector3 &impulse)
void appendNode(const btVector3 &x, btScalar m)
void staticSolve(int iterations)
void setVolumeMass(btScalar mass)
btScalar m_restLengthScale
Definition btSoftBody.h:859
bool checkDeformableContact(const btCollisionObjectWrapper *colObjWrap, const btVector3 &x, btScalar margin, btSoftBody::sCti &cti, bool predict=false) const
bool m_reducedModel
Definition btSoftBody.h:861
void cleanupClusters()
Config m_cfg
Definition btSoftBody.h:808
void updateDeactivation(btScalar timeStep)
btAlignedObjectArray< TetraScratch > m_tetraScratches
Definition btSoftBody.h:820
const btVector3 & getWindVelocity()
Return the wind velocity for interaction with the air.
void addAeroForceToFace(const btVector3 &windVelocity, int faceIndex)
btAlignedObjectArray< btVector4 > m_renderNodesInterpolationWeights
Definition btSoftBody.h:849
void appendAngularJoint(const AJoint::Specs &specs, Cluster *body0, Body body1)
tFaceArray m_faces
Definition btSoftBody.h:817
void setAngularVelocity(const btVector3 &angVel)
void setVolumeDensity(btScalar density)
static void clusterDCImpulse(Cluster *cluster, const btVector3 &impulse)
virtual void transform(const btTransform &trs)
bool m_softSoftCollision
Definition btSoftBody.h:853
static void clusterVAImpulse(Cluster *cluster, const btVector3 &impulse)
btScalar getMass(int node) const
tMaterialArray m_materials
Definition btSoftBody.h:831
void setMaxStress(btScalar maxStress)
void dampClusters()
btSoftBody(btSoftBodyWorldInfo *worldInfo, int node_count, const btVector3 *x, const btScalar *m)
void updateDeformation()
btAlignedObjectArray< btScalar > m_z
Definition btSoftBody.h:851
void addAeroForceToNode(const btVector3 &windVelocity, int nodeIndex)
static btVector3 clusterCom(const Cluster *cluster)
tRContactArray m_rcontacts
Definition btSoftBody.h:824
void appendAnchor(int node, btRigidBody *body, bool disableCollisionBetweenLinkedBodies=false, btScalar influence=1)
btVector3 m_bounds[2]
Definition btSoftBody.h:833
btScalar m_maxSpeedSquared
Definition btSoftBody.h:842
void releaseCluster(int index)
btScalar m_repulsionStiffness
Definition btSoftBody.h:844
void setVelocity(const btVector3 &velocity)
tClusterArray m_clusters
Definition btSoftBody.h:839
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 geometricCollisionHandler(btSoftBody *psb)
void releaseClusters()
void refine(ImplicitFn *ifn, btScalar accurary, bool cut)
void setSolver(eSolverPresets::_ preset)
static T BaryEval(const T &a, const T &b, const T &c, const btVector3 &coord)
Material * appendMaterial()
void removeAnchor(int node)
btAlignedObjectArray< DeformableNodeRigidAnchor > m_deformableAnchors
Definition btSoftBody.h:823
void setCacheBarycenter(bool cacheBarycenter)
btScalar m_gravityFactor
Definition btSoftBody.h:845
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)
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:850
tRenderNodeArray m_renderNodes
Definition btSoftBody.h:815
void pointersToIndices()
tNoteArray m_notes
Definition btSoftBody.h:813
void updateNormals()
static void clusterDImpulse(Cluster *cluster, const btVector3 &rpos, const btVector3 &impulse)
btDbvt m_fdbvt
Definition btSoftBody.h:836
tLinkArray m_links
Definition btSoftBody.h:816
void applyForces()
static btVector3 clusterVelocity(const Cluster *cluster, const btVector3 &rpos)
btTransform getRigidTransform()
tSContactArray m_scontacts
Definition btSoftBody.h:829
bool m_cacheBarycenter
Definition btSoftBody.h:846
bool m_useSelfCollision
Definition btSoftBody.h:852
void * m_tag
Definition btSoftBody.h:811
void updatePose()
void initializeClusters()
tAnchorArray m_anchors
Definition btSoftBody.h:822
btScalar getRestLengthScale()
void updateState(const btAlignedObjectArray< btVector3 > &qs, const btAlignedObjectArray< btVector3 > &vs)
void randomizeConstraints()
btVector3 m_windVelocity
Definition btSoftBody.h:857
btScalar getTotalMass() const
tNodeArray m_nodes
Definition btSoftBody.h:814
void appendLink(int model=-1, Material *mat=0)
void setSpringStiffness(btScalar k)
void initializeDmInverse()
void updateConstants()
void setTotalMass(btScalar mass, bool fromfaces=false)
virtual ~btSoftBody()
void appendDeformableAnchor(int node, btRigidBody *body)
void updateLinkConstants()
void initDefaults()
btAlignedObjectArray< btVector3 > m_quads
Definition btSoftBody.h:843
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)
btAlignedObjectArray< btVector3 > m_X
Definition btSoftBody.h:847
virtual void translate(const btVector3 &trs)
btVector3 getCenterOfMass() const
void(*) psolver_t(btSoftBody *, btScalar, btScalar)
Definition btSoftBody.h:785
void initializeFaceTree()
void resetLinkRestLengths()
int clusterCount() const
btDbvt m_ndbvt
Definition btSoftBody.h:835
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.
btVector3 invXform(const btVector3 &inVec) const
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
void setRotation(const btQuaternion &q)
Set the rotational element by btQuaternion.
void setIdentity()
Set this transformation to the identity.
btVector3 & getOrigin()
Return the origin vector translation.
void setOrigin(const btVector3 &origin)
Set the translational element.
void setBasis(const btMatrix3x3 &basis)
Set the rotational element by btMatrix3x3.
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
const btScalar eps
Definition poly34.cpp:11
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
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 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
btDbvtNode * parent
Definition btDbvt.h:183
void * data
Definition btDbvt.h:188
btDbvtVolume volume
Definition btDbvt.h:182
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
static btScalar SignedDistance(const btVector3 &position, btScalar margin, const btConvexShape *shape, const btTransform &wtrs, sResults &results)
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]
btDispatcher * m_dispatcher
Definition btSoftBody.h:55
btSparseSdf< 3 > m_sparsesdf
Definition btSoftBody.h:57
btVector3 m_gravity
Definition btSoftBody.h:56
btScalar m_maxDisplacement
Definition btSoftBody.h:52
btBroadphaseInterface * m_broadphase
Definition btSoftBody.h:54
void Prepare(btScalar dt, int iterations)
void Solve(btScalar dt, btScalar sor)
IControl * m_icontrol
Definition btSoftBody.h:688
void Terminate(btScalar dt)
btRigidBody * m_body
Definition btSoftBody.h:429
btMatrix3x3 m_c0
Definition btSoftBody.h:431
const btTransform & xform() const
Definition btSoftBody.h:546
void Terminate(btScalar dt)
void Prepare(btScalar dt, int iterations)
void Solve(btScalar dt, btScalar sor)
btVector3 m_dimpulses[2]
Definition btSoftBody.h:470
tVector3Array m_framerefs
Definition btSoftBody.h:462
btMatrix3x3 m_invwi
Definition btSoftBody.h:467
btMatrix3x3 m_locii
Definition btSoftBody.h:466
btAlignedObjectArray< Node * > m_nodes
Definition btSoftBody.h:461
btDbvtNode * m_leaf
Definition btSoftBody.h:475
btVector3 m_vimpulses[2]
Definition btSoftBody.h:469
tScalarArray m_masses
Definition btSoftBody.h:460
btTransform m_framexform
Definition btSoftBody.h:463
tPSolverArray m_psequence
Definition btSoftBody.h:737
tPSolverArray m_dsequence
Definition btSoftBody.h:738
eAeroModel::_ aeromodel
Definition btSoftBody.h:710
tVSolverArray m_vsequence
Definition btSoftBody.h:736
btVector4 m_pcontact
Definition btSoftBody.h:311
btVector3 m_normal
Definition btSoftBody.h:308
btDbvtNode * m_leaf
Definition btSoftBody.h:310
Material * m_material
Definition btSoftBody.h:258
virtual btScalar Eval(const btVector3 &x)=0
btVector3 m_refs[2]
Definition btSoftBody.h:639
virtual void Prepare(btScalar dt, int iterations)
void Solve(btScalar dt, btScalar sor)
void Prepare(btScalar dt, int iterations)
void Terminate(btScalar dt)
btDbvtNode * m_leaf
Definition btSoftBody.h:277
btScalar m_coords[4]
Definition btSoftBody.h:442
btVector3 m_offset
Definition btSoftBody.h:439
Node * m_nodes[4]
Definition btSoftBody.h:441
const char * m_text
Definition btSoftBody.h:438
btMatrix3x3 m_scl
Definition btSoftBody.h:454
btScalar m_volume
Definition btSoftBody.h:449
btVector3 m_com
Definition btSoftBody.h:452
tVector3Array m_pos
Definition btSoftBody.h:450
btMatrix3x3 m_aqq
Definition btSoftBody.h:455
btMatrix3x3 m_rot
Definition btSoftBody.h:453
tScalarArray m_wgh
Definition btSoftBody.h:451
RayFromToCaster takes a ray from, ray to (instead of direction!)
Definition btSoftBody.h:762
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)
btScalar m_element_measure
Definition btSoftBody.h:326
btMatrix3x3 m_Dm_inverse
Definition btSoftBody.h:324
btMatrix3x3 m_F
Definition btSoftBody.h:325
btVector4 m_P_inv[3]
Definition btSoftBody.h:327
@ V_TwoSided
Vertex normals are oriented toward velocity.
Definition btSoftBody.h:92
@ V_OneSided
Vertex normals are flipped to match velocity and lift and drag forces are applied.
Definition btSoftBody.h:94
@ V_TwoSidedLiftDrag
Vertex normals are flipped to match velocity.
Definition btSoftBody.h:93
@ F_OneSided
Face normals are flipped to match velocity and lift and drag forces are applied.
Definition btSoftBody.h:97
@ F_TwoSided
Vertex normals are taken as it is.
Definition btSoftBody.h:95
@ F_TwoSidedLiftDrag
Face normals are flipped to match velocity.
Definition btSoftBody.h:96
@ RContacts
Anchor solver.
Definition btSoftBody.h:119
@ SContacts
Rigid contacts solver.
Definition btSoftBody.h:120
@ Anchors
Linear solver.
Definition btSoftBody.h:118
@ SDF_RDN
GJK based Multibody vs. deformable face.
Definition btSoftBody.h:177
@ VF_SS
Rigid versus soft mask.
Definition btSoftBody.h:169
@ Default
SDF based Rigid vs. deformable node.
Definition btSoftBody.h:179
@ VF_DD
Cluster soft body self collision.
Definition btSoftBody.h:172
@ CL_SS
Vertex vs face soft vs soft handling.
Definition btSoftBody.h:170
@ CL_SELF
Cluster vs cluster soft vs soft handling.
Definition btSoftBody.h:171
@ SVSmask
rigid vs deformable
Definition btSoftBody.h:168
@ SDF_RS
Rigid versus soft mask.
Definition btSoftBody.h:164
@ SDF_RD
Cluster vs convex rigid vs soft.
Definition btSoftBody.h:166
@ SDF_RDF
Rigid versus deformable face mask.
Definition btSoftBody.h:175
@ SDF_MDF
GJK based Rigid vs. deformable face.
Definition btSoftBody.h:176
@ CL_RS
SDF based rigid vs soft.
Definition btSoftBody.h:165
@ Default
Enable debug draw.
Definition btSoftBody.h:191
const btCollisionObject * m_colObj
Definition btSoftBody.h:226
btScalar m_offset
Definition btSoftBody.h:228
btVector3 m_normal
Definition btSoftBody.h:227
eFeature::_ feature
soft body
Definition btSoftBody.h:204
btScalar fraction
feature index
Definition btSoftBody.h:206
int index
feature type
Definition btSoftBody.h:205
btSoftBody * body
Definition btSoftBody.h:203
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)